Backport: Allows setting initial joint positions for Generic system (#219)

Co-authored-by: Jorge Nicho <[email protected]>
Co-authored-by: Denis Štogl <[email protected]>

Author: 3 people

.github/workflows/ci-format.yml

@@ -17,6 +17,8 @@ jobs:
     steps:
     - uses: actions/checkout@v2
     - uses: actions/setup-python@v2
+      with:
+        python-version: 3.9.7
     - name: Install system hooks
       run: sudo apt-get install clang-format-10 cppcheck
     - uses: pre-commit/[email protected]

README.md

@@ -111,6 +111,20 @@ The most relevant arguments are the following:
   - `robot_ip` (*mandatory*) - IP address by which the root can be reached.
   - `use_fake_hardware` (default: *false*) - use simple hardware emulator from ros2_control.
     Useful for testing launch files, descriptions, etc. See explanation below.
+  - `initial_positions` (default: dictionary with all joint values set to 0) - Allows passing a dictionary to set the initial joint values for the fake hardware from [ros2_control](http://control.ros.org/).  It can also be set from a yaml file with the `load_yaml` commands as follows:
+      ```
+      <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)}"/>
+      ```
+      In this example, the **initial_positions_file** is a xacro argument that contains the absolute path to a yaml file. An example of the initial positions yaml file is as follows:
+      ```
+      elbow_joint: 1.158
+      shoulder_lift_joint: -0.953
+      shoulder_pan_joint: 1.906
+      wrist_1_joint: -1.912
+      wrist_2_joint: -1.765
+      wrist_3_joint: 0.0
+      ```
+
   - `fake_sensor_commands` (default: *false*) - enables setting sensor values for the hardware emulators.
     Useful for offline testing of controllers.
   - `robot_controller` (default: *joint_trajectory_controller*) - controller for robot joints to be started.

ur_description/config/initial_positions.yaml

@@ -0,0 +1,6 @@
+shoulder_pan_joint: 0.0
+shoulder_lift_joint: -1.57
+elbow_joint: 0.0
+wrist_1_joint: -1.57
+wrist_2_joint: 0.0
+wrist_3_joint: 0.0

ur_description/urdf/ur.ros2_control.xacro

@@ -3,6 +3,7 @@
 
   <xacro:macro name="ur_ros2_control" params="name prefix
     use_fake_hardware:=false fake_sensor_commands:=false
+    initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}
     script_filename output_recipe_filename
     input_recipe_filename tf_prefix
     hash_kinematics robot_ip">
@@ -51,7 +52,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 -->
+        <param name="initial_position">${initial_positions['shoulder_pan_joint']}</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <joint name="${prefix}shoulder_lift_joint">
         <command_interface name="position">
@@ -65,7 +66,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 -->
+        <param name="initial_position">${initial_positions['shoulder_lift_joint']}</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <joint name="${prefix}elbow_joint">
         <command_interface name="position">
@@ -79,7 +80,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 -->
+        <param name="initial_position">${initial_positions['elbow_joint']}</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <joint name="${prefix}wrist_1_joint">
         <command_interface name="position">
@@ -93,7 +94,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 -->
+        <param name="initial_position">${initial_positions['wrist_1_joint']}</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <joint name="${prefix}wrist_2_joint">
         <command_interface name="position">
@@ -107,7 +108,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 -->
+        <param name="initial_position">${initial_positions['wrist_2_joint']}</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <joint name="${prefix}wrist_3_joint">
         <command_interface name="position">
@@ -121,7 +122,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 -->
+        <param name="initial_position">${initial_positions['wrist_3_joint']}</param>  <!-- initial position for the FakeSystem -->
       </joint>
       <sensor name="tcp_fts_sensor">
         <state_interface name="force.x"/>

ur_description/urdf/ur.urdf.xacro

@@ -19,6 +19,10 @@
    <xacro:arg name="use_fake_hardware" default="false" />
    <xacro:arg name="fake_sensor_commands" default="false" />
 
+   <!-- initial position -->
+   <xacro:arg name="initial_positions_file" default="$(find ur_description)/config/initial_positions.yaml"/>
+   <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
+
    <!-- arm -->
    <xacro:ur_robot
      prefix="$(arg prefix)"
@@ -31,5 +35,6 @@
      safety_pos_margin="$(arg safety_pos_margin)"
      safety_k_position="$(arg safety_k_position)"
      use_fake_hardware="$(arg use_fake_hardware)"
-     fake_sensor_commands="$(arg fake_sensor_commands)" />
+     fake_sensor_commands="$(arg fake_sensor_commands)"
+     initial_positions="${load_yaml(initial_positions_file)}" />
 </robot>

ur_description/urdf/ur_macro.xacro

@@ -64,8 +64,8 @@
     safety_pos_margin:=0.15
     safety_k_position:=20
     use_fake_hardware:=false
-    fake_sensor_commands:=false"
-  >
+    fake_sensor_commands:=false
+    initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}">
     <!-- Load configuration data from the provided .yaml files -->
     <xacro:read_model_data
       joint_limits_parameters_file="${joint_limits_parameters_file}"
@@ -78,12 +78,15 @@
     <xacro:arg name="output_recipe_filename" default="$(find ur_robot_driver)/resources/rtde_output_recipe.txt"/>
     <xacro:arg name="input_recipe_filename" default="$(find ur_robot_driver)/resources/rtde_input_recipe.txt"/>
     <xacro:arg name="robot_ip" default="10.0.1.186"/>
+
+
     <!-- ros2 control include -->
     <xacro:include filename="$(find ur_description)/urdf/ur.ros2_control.xacro" />
     <!-- ros2 control instance -->
     <xacro:ur_ros2_control
       name="URPositionHardwareInterface" prefix="${prefix}"
       use_fake_hardware="$(arg use_fake_hardware)"
+      initial_positions="${initial_positions}"
       fake_sensor_commands="$(arg fake_sensor_commands)"
       script_filename="$(arg script_filename)"
       output_recipe_filename="$(arg output_recipe_filename)"