Add parameters for checking start state (#143)

* Add parameters for checking start state.

* Sync with ros2_control_demos updates.

* Adapt initial positions to default ur home position.

* Fill all start states for fake system use.

Author: livanov93

Universal_Robots_ROS2_Driver.repos

@@ -10,7 +10,7 @@ repositories:
   ros2_control_demos:
     type: git
     url: https://github.com/ros-controls/ros2_control_demos.git
-    version: add_demo_nodes
+    version: master
   ros2_controllers:
     type: git
     url: https://github.com/ros-controls/ros2_controllers.git

ur_bringup/config/test_goal_publishers_config.yaml

@@ -18,6 +18,14 @@ publisher_scaled_joint_trajectory_controller:
       - wrist_2_joint
       - wrist_3_joint
 
+    check_starting_point: true
+    starting_point_limits:
+      shoulder_pan_joint: [-0.1,0.1]
+      shoulder_lift_joint: [-1.6,-1.5]
+      elbow_joint: [-0.1,0.1]
+      wrist_1_joint: [-1.6,-1.5]
+      wrist_2_joint: [-0.1,0.1]
+      wrist_3_joint: [-0.1,0.1]
 
 publisher_joint_trajectory_controller:
   ros__parameters:
@@ -38,3 +46,12 @@ publisher_joint_trajectory_controller:
       - wrist_1_joint
       - wrist_2_joint
       - wrist_3_joint
+
+    check_starting_point: true
+    starting_point_limits:
+      shoulder_pan_joint: [-0.1,0.1]
+      shoulder_lift_joint: [-1.6,-1.5]
+      elbow_joint: [-0.1,0.1]
+      wrist_1_joint: [-1.6,-1.5]
+      wrist_2_joint: [-0.1,0.1]
+      wrist_3_joint: [-0.1,0.1]

ur_description/urdf/ur.ros2_control.xacro

@@ -51,6 +51,7 @@
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
+        <param name="initial_position">0.0</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <joint name="${prefix}shoulder_lift_joint">
         <command_interface name="position">
@@ -78,6 +79,7 @@
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
+        <param name="initial_position">0.0</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <joint name="${prefix}wrist_1_joint">
         <command_interface name="position">
@@ -91,6 +93,7 @@
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
+        <param name="initial_position">-1.57</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <joint name="${prefix}wrist_2_joint">
         <command_interface name="position">
@@ -104,6 +107,7 @@
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
+        <param name="initial_position">0.0</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <joint name="${prefix}wrist_3_joint">
         <command_interface name="position">
@@ -117,6 +121,7 @@
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
+        <param name="initial_position">0.0</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <sensor name="tcp_fts_sensor">
         <state_interface name="force.x"/>