F#src 4537 test compliance

sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro

@@ -23,9 +23,35 @@
     <xacro:shadowhand muscletrans="false" muscle="false" bio="false" bt_sp="false" ubi="false" eli="false"
                       reflect="1.0" prefix="rh_" lf="true" cable_mesh="true"/>
 
+    <!-- We use wrist_3_link instead of ee_link due to a bug in preserveFixedJoint
+         where it currently won't work with links that don't have an inertial component
+         as is the case with ee_link. These links get removed when converting to SDF.
+         See more on this subject in this issue https://github.com/osrf/sdformat/issues/199 -->
     <joint name="ra_arm_to_hand" type="fixed">
-        <parent link="ra_ee_link"/>
+        <parent link="ra_wrist_3_link"/>
         <child link="rh_forearm"/>
-        <origin xyz="0.012 0.0 0.0" rpy="${pi/2.0} ${0.0} ${pi/2.0}"/>
+        <origin xyz="0.0 0.0 0.012" rpy="${0.0} ${0.0} ${pi}"/>
     </joint>
+
+    <gazebo reference="ra_arm_to_hand">
+      <preserveFixedJoint>true</preserveFixedJoint>
+      <sensor name="ra_force_torque" type="force_torque">
+        <always_on>true</always_on>
+        <visualize>true</visualize>
+        <force_torque>
+          <frame>sensor</frame>
+          <measure_direction>child_to_parent</measure_direction>
+        </force_torque>
+      </sensor>
+
+      <provideFeedback>true</provideFeedback>
+    </gazebo>
+
+    <gazebo>
+      <plugin name="ft_sensor" filename="libgazebo_ros_ft_sensor.so">
+        <updateRate>125.0</updateRate>
+        <topicName>ra_wrench</topicName>
+        <jointName>ra_arm_to_hand</jointName>
+      </plugin>
+    </gazebo>
 </robot>

sr_multi_moveit/sr_multi_moveit_config/config/robot_configs/right_sh_ur10.yaml

@@ -6,7 +6,7 @@ robot:
       arm:
         name: ur10
         main_group: manipulator
-        moveit_path: 
+        moveit_path:
           package: sr_multi_moveit_config
           relative_path: /config/ur
         extra_groups_config_path: /config

sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml

@@ -0,0 +1,50 @@
+# gazebo_ros_control:
+#     pid_gains:
+#       ra_shoulder_pan_joint:  {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_shoulder_lift_joint: {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_elbow_joint:         {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_wrist_1_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_wrist_2_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_wrist_3_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+
+
+ra_trajectory_controller:
+  type: "velocity_controllers/ComplianceController"
+  joints:
+    - ra_shoulder_pan_joint
+    - ra_shoulder_lift_joint
+    - ra_elbow_joint
+    - ra_wrist_1_joint
+    - ra_wrist_2_joint
+    - ra_wrist_3_joint
+  compliance_command_timeout: 1.5
+  constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 1.0
+      ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
+      ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
+      ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_1_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_2_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
+  stop_trajectory_duration: 0.5
+  state_publish_rate:  25
+  action_monitor_rate: 10
+  allow_partial_joints_goal: true
+  #!!These values have not been optimized!!
+  gains:
+    ra_shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+
+  # Use a feedforward term to reduce the size of PID gains
+  velocity_ff:
+    ra_shoulder_pan_joint: 1.0
+    ra_shoulder_lift_joint: 1.0
+    ra_elbow_joint: 1.0
+    ra_wrist_1_joint: 1.0
+    ra_wrist_2_joint: 1.0
+    ra_wrist_3_joint: 1.0

sr_robot_launch/config/ra_compliant_trajectory_controller.yaml

@@ -0,0 +1,49 @@
+
+joints: &robot_joints
+  - ra_shoulder_pan_joint
+  - ra_shoulder_lift_joint
+  - ra_elbow_joint
+  - ra_wrist_1_joint
+  - ra_wrist_2_joint
+  - ra_wrist_3_joint
+
+ra_trajectory_controller:
+  type: "velocity_controllers/ComplianceController"
+  joints: *robot_joints
+  compliance_command_timeout: 1.5
+  constraints:
+      goal_time: 6.0
+      stopped_velocity_tolerance: 0.05
+      ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
+      ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
+      ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_1_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_2_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
+  stop_trajectory_duration: 0.5
+  state_publish_rate:  250
+  action_monitor_rate: 10
+  allow_partial_joints_goal: true
+  #!!These values have not been optimized!!
+  gains:
+    p: &p_gain 20.0
+    i: &i_gain 0.0
+    d: &d_gain 0.0
+    clamp: &i_clamp 1.0
+    ra_shoulder_pan_joint:  {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_shoulder_lift_joint: {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_elbow_joint:         {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_1_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_2_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_3_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+
+  # Use a feedforward term to reduce the size of PID gains
+  velocity_ff:
+    var: &v_ff 0.0
+    ra_shoulder_pan_joint: *v_ff
+    ra_shoulder_lift_joint: *v_ff
+    ra_elbow_joint: *v_ff
+    ra_wrist_1_joint: *v_ff
+    ra_wrist_2_joint: *v_ff
+    ra_wrist_3_joint: *v_ff
+

sr_robot_launch/config/ra_trajectory_controller.yaml

@@ -1,13 +1,16 @@
+
+joints: &robot_joints
+  - ra_shoulder_pan_joint
+  - ra_shoulder_lift_joint
+  - ra_elbow_joint
+  - ra_wrist_1_joint
+  - ra_wrist_2_joint
+  - ra_wrist_3_joint
+
 ra_trajectory_controller:
   type: "position_controllers/JointTrajectoryController"
   hardware_interface:
-    joints: &robot_joints
-      - ra_shoulder_pan_joint
-      - ra_shoulder_lift_joint
-      - ra_elbow_joint
-      - ra_wrist_1_joint
-      - ra_wrist_2_joint
-      - ra_wrist_3_joint
+    joints: *robot_joints
   joints: *robot_joints
   constraints:
       goal_time: 0.6

sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch

@@ -44,6 +44,9 @@
   <arg name="arm_robot_hw_1" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/right_ur10e_arm_robot_hw.yaml"/>
   <arg name="arm_robot_hw_2" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/left_bimanual_ur_arm_robot_hw.yaml"/>
   <arg name="arm_robot_hw_2" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/left_bimanual_ur10e_arm_robot_hw.yaml"/>
+  <arg name="arm_compliance_control" default="false"/>
+  <arg if="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/VelocityJointInterface"/>
+  <arg unless="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/PositionJointInterface"/>
   <arg name="urcap_program_name" default="external_ctrl.urp"/>
 
   <arg name="arm_trajectory" default="true"/>
@@ -56,8 +59,8 @@
   <arg unless="$(eval hands == 'none')" name="arm_payload_cog" default="[0.0, 0.0, 0.12]"/>
   <arg if="$(eval hands == 'none')" name="arm_payload_mass" default="0.0"/>
   <arg if="$(eval hands == 'none')" name="arm_payload_cog" default="[0.0, 0.0, 0.0]"/>
+  <arg name="load_robot_description_command" default="xacro $(arg robot_description) arm_1_z:=$(arg arm_1_z) arm_2_z:=$(arg arm_2_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) transmission_hw_interface:=$(arg arm_transmission_interface)"/>
 
-  <arg name="load_robot_description_command" default="xacro $(arg robot_description) arm_1_z:=$(arg arm_1_z) arm_2_z:=$(arg arm_2_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation)"/>
 
   <node pkg="rosservice" type="rosservice" name="set_right_speed_scale" args="call --wait /ra_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg right_arm_speed_scale)'"/>
   <node pkg="rosservice" type="rosservice" name="set_left_speed_scale" args="call --wait /la_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg left_arm_speed_scale)'"/>

sr_robot_launch/launch/sr_hardware_control_loop.launch

@@ -46,6 +46,7 @@
   <arg name="hand_trajectory" default="true"/>
 
   <!-- ARMS CONFIG-->
+  <arg name="arm_compliance_control" default="true"/>
   <arg name="initial_z" if="$(arg arm)" default="0.7551"/>
   <arg name="initial_z" unless="$(arg arm)" default="0.0"/>
   <arg name="arm_x_separation" default="0.0"/>
@@ -61,10 +62,14 @@
   <arg if="$(arg hand)" name="arm_payload_cog" default="[0.0, 0.0, 0.12]"/>
   <arg unless="$(arg hand)" name="arm_payload_mass" default="0.0"/>
   <arg unless="$(arg hand)" name="arm_payload_cog" default="[0.0, 0.0, 0.0]"/>
+  <arg if="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/VelocityJointInterface"/>
+  <arg unless="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/PositionJointInterface"/>
   <arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="urcap_program_name" default="external_ctrl.urp"/>
-  <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config:=$(arg kinematics_config)"/>
+  <arg if="$(arg arm_compliance_control)" name="trajectory_controller_config" default="$(arg arm_prefix)_compliant_trajectory_controller.yaml"/>
+  <arg unless="$(arg arm_compliance_control)" name="trajectory_controller_config" default="$(arg arm_prefix)_trajectory_controller.yaml"/>
+  <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config:=$(arg kinematics_config) transmission_hw_interface:=$(arg arm_transmission_interface)"/>
 
   <node pkg="rosservice" type="rosservice" name="set_speed_scale" args="call --wait /$(arg arm_prefix)_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg arm_speed_scale)'"/>
   <node pkg="rosservice" type="rosservice" name="set_payload" args="call --wait /$(arg arm_prefix)_sr_ur_robot_hw/set_payload '{payload: $(arg arm_payload_mass), center_of_gravity: $(arg arm_payload_cog)}'"/>
@@ -156,23 +161,28 @@
       - unique_robot_hw
   </rosparam>
 
+  <node if="$(arg arm_compliance_control)" name="conditional_delayed_rostool_wrench_to_joint_vel_pub" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
+    <param name="package_name" value="wrench_to_joint_vel_pub" />
+    <param name="executable_name" value="ur_compliance.launch" />
+    <rosparam param="params_list">[/robot_description, /robot_description_semantic]</rosparam>
+    <param name="timeout" value="30.0" />
+  </node>
+
   <!-- These will be loaded if hand is false so UR10 with box will load instead. -->
   <group unless="$(arg hand)">
     <include file="$(find ros_ethercat_model)/launch/joint_state_publisher.launch">
       <arg name="publish_rate" value="$(arg joint_state_pub_frequency)"/>
     </include>
     <node if="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="gdb -ex run -args"/>
     <node unless="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="ethercat_grant"/>
+
   </group>
   <!-- These will be loaded if arm and hand are enabled -->
   <!-- Trajectory mode -->
   <group if="$(arg arm_ctrl)">
     <group if="$(arg arm_trajectory)">
-      <group if="$(arg hand_ctrl)">
-        <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
-      </group>
-      <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
-      <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+      <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_config)"/>
+      <node unless="$(arg hand_ctrl)" name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
             args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
     </group>
     <!-- Position mode -->