增加重力补偿

urdf/robot/openarm_robot.xacro

@@ -32,11 +32,25 @@
             left_arm_base_xyz='0 0 0'
             right_arm_base_rpy='0 0 0'
             left_arm_base_rpy='0 0 0'
-            left_can_interface:='can5'
+            left_can_interface:='can1'
-            right_can_interface:='can4'
+            right_can_interface:='can0'
             left_arm_prefix:='left_'
             right_arm_prefix:='right_'
             can_fd:='false'
+            gravity_compensation_enabled:='false'
+            gravity_compensation_use_kdl:='true'
+            gravity_torque_mode:='false'
+            gravity_scale:='1.0'
+            gravity_tau_limit1:='2.0'
+            gravity_tau_limit2:='2.0'
+            gravity_tau_limit3:='2.0'
+            gravity_tau_limit4:='2.0'
+            gravity_tau_limit5:='2.0'
+            gravity_tau_limit6:='2.0'
+            gravity_tau_limit7:='2.0'
+            kdl_urdf_path:='/tmp/openarm_bimanual_runtime.urdf'
+            kdl_base_link:='openarm_body_link0'
+            kdl_tip_link:=''
             "
             >
 
@@ -110,7 +124,21 @@
             fake_sensor_commands="${fake_sensor_commands}"
             left_arm_prefix="${left_arm_prefix}"
             right_arm_prefix="${right_arm_prefix}"
-            hand="${hand}"/>
+            hand="${hand}"
+            gravity_compensation_enabled="${gravity_compensation_enabled}"
+            gravity_compensation_use_kdl="${gravity_compensation_use_kdl}"
+            gravity_torque_mode="${gravity_torque_mode}"
+            gravity_scale="${gravity_scale}"
+            gravity_tau_limit1="${gravity_tau_limit1}"
+            gravity_tau_limit2="${gravity_tau_limit2}"
+            gravity_tau_limit3="${gravity_tau_limit3}"
+            gravity_tau_limit4="${gravity_tau_limit4}"
+            gravity_tau_limit5="${gravity_tau_limit5}"
+            gravity_tau_limit6="${gravity_tau_limit6}"
+            gravity_tau_limit7="${gravity_tau_limit7}"
+            kdl_urdf_path="${kdl_urdf_path}"
+            kdl_base_link="${kdl_base_link}"
+            kdl_tip_link="${kdl_tip_link}"/>
         </xacro:if>
 
         <xacro:if value="$(eval hand and ee_type == 'openarm_hand')">

urdf/robot/v10.urdf.xacro

@@ -25,11 +25,11 @@
 
   <xacro:arg name="ros2_control" default="false" />
 
-  <xacro:arg name="can_interface" default="can4" />
+  <xacro:arg name="can_interface" default="can0" />
 
   <xacro:arg name="left_can_interface" default="can1" />
 
-  <xacro:arg name="right_can_interface" default="can4" />
+  <xacro:arg name="right_can_interface" default="can0" />
   
   <xacro:arg name="left_arm_prefix" default="left_" />
 
@@ -48,6 +48,20 @@
   <xacro:arg name="left_arm_base_rpy" default="-1.5708 0 0" />
 
   <xacro:arg name="can_fd" default="false" />
+  <xacro:arg name="gravity_compensation_enabled" default="false" />
+  <xacro:arg name="gravity_compensation_use_kdl" default="true" />
+  <xacro:arg name="gravity_torque_mode" default="false" />
+  <xacro:arg name="gravity_scale" default="1.0" />
+  <xacro:arg name="gravity_tau_limit1" default="2.0" />
+  <xacro:arg name="gravity_tau_limit2" default="2.0" />
+  <xacro:arg name="gravity_tau_limit3" default="2.0" />
+  <xacro:arg name="gravity_tau_limit4" default="2.0" />
+  <xacro:arg name="gravity_tau_limit5" default="2.0" />
+  <xacro:arg name="gravity_tau_limit6" default="2.0" />
+  <xacro:arg name="gravity_tau_limit7" default="2.0" />
+  <xacro:arg name="kdl_urdf_path" default="/tmp/openarm_bimanual_runtime.urdf" />
+  <xacro:arg name="kdl_base_link" default="openarm_body_link0" />
+  <xacro:arg name="kdl_tip_link" default="" />
 
   <xacro:openarm_robot
   arm_type="v10"
@@ -83,6 +97,20 @@
   right_arm_base_rpy="$(arg right_arm_base_rpy)"
   left_arm_base_rpy="$(arg left_arm_base_rpy)"
   can_fd="$(arg can_fd)"
+  gravity_compensation_enabled="$(arg gravity_compensation_enabled)"
+  gravity_compensation_use_kdl="$(arg gravity_compensation_use_kdl)"
+  gravity_torque_mode="$(arg gravity_torque_mode)"
+  gravity_scale="$(arg gravity_scale)"
+  gravity_tau_limit1="$(arg gravity_tau_limit1)"
+  gravity_tau_limit2="$(arg gravity_tau_limit2)"
+  gravity_tau_limit3="$(arg gravity_tau_limit3)"
+  gravity_tau_limit4="$(arg gravity_tau_limit4)"
+  gravity_tau_limit5="$(arg gravity_tau_limit5)"
+  gravity_tau_limit6="$(arg gravity_tau_limit6)"
+  gravity_tau_limit7="$(arg gravity_tau_limit7)"
+  kdl_urdf_path="$(arg kdl_urdf_path)"
+  kdl_base_link="$(arg kdl_base_link)"
+  kdl_tip_link="$(arg kdl_tip_link)"
   />
 
 </robot>

urdf/ros2_control/openarm.bimanual.ros2_control.xacro

@@ -4,14 +4,28 @@
 <xacro:macro name="openarm_bimanual_ros2_control"
          params="arm_type
              control_gains
-             left_can_interface:=^|can5
+             left_can_interface:=^|can1
-             right_can_interface:=^|can4
+             right_can_interface:=^|can0
              use_fake_hardware:=^|false
              fake_sensor_commands:=^|false
              hand:=^|false
              left_arm_prefix:=^|left_
              right_arm_prefix:=^|right_
-             can_fd:=^|false">
+             can_fd:=^|false
+             gravity_compensation_enabled:=^|true
+             gravity_compensation_use_kdl:=^|true
+             gravity_torque_mode:=^|true
+             gravity_scale:=^|2.5
+             gravity_tau_limit1:=^|10.0
+             gravity_tau_limit2:=^|10.0
+             gravity_tau_limit3:=^|5.0
+             gravity_tau_limit4:=^|5.0
+             gravity_tau_limit5:=^|5.0
+             gravity_tau_limit6:=^|3.0
+             gravity_tau_limit7:=^|3.0
+             kdl_urdf_path:=^|/tmp/openarm_bimanual_runtime.urdf
+             kdl_base_link:=^|openarm_body_link0
+             kdl_tip_link:=^|">
 
     <!-- Left Arm Hardware Interface -->
     <ros2_control name="openarm_left_hardware_interface" type="system">
@@ -28,6 +42,20 @@
             <param name="arm_prefix">${left_arm_prefix}</param>
             <param name="hand">${hand}</param>
             <param name="can_fd">${can_fd}</param>
+            <param name="gravity_compensation_enabled">${gravity_compensation_enabled}</param>
+            <param name="gravity_compensation_use_kdl">${gravity_compensation_use_kdl}</param>
+            <param name="gravity_torque_mode">${gravity_torque_mode}</param>
+            <param name="gravity_scale">${gravity_scale}</param>
+            <param name="gravity_tau_limit1">${gravity_tau_limit1}</param>
+            <param name="gravity_tau_limit2">${gravity_tau_limit2}</param>
+            <param name="gravity_tau_limit3">${gravity_tau_limit3}</param>
+            <param name="gravity_tau_limit4">${gravity_tau_limit4}</param>
+            <param name="gravity_tau_limit5">${gravity_tau_limit5}</param>
+            <param name="gravity_tau_limit6">${gravity_tau_limit6}</param>
+            <param name="gravity_tau_limit7">${gravity_tau_limit7}</param>
+            <param name="kdl_urdf_path">${kdl_urdf_path}</param>
+            <param name="kdl_base_link">${kdl_base_link}</param>
+            <param name="kdl_tip_link">${kdl_tip_link if kdl_tip_link != '' else 'openarm_' + left_arm_prefix + 'link7'}</param>
             <param name="kp1">${control_gains['joint1']['kp']}</param>
             <param name="kp2">${control_gains['joint2']['kp']}</param>
             <param name="kp3">${control_gains['joint3']['kp']}</param>
@@ -93,6 +121,20 @@
             <param name="arm_prefix">${right_arm_prefix}</param>
             <param name="hand">${hand}</param>
             <param name="can_fd">${can_fd}</param>
+            <param name="gravity_compensation_enabled">${gravity_compensation_enabled}</param>
+            <param name="gravity_compensation_use_kdl">${gravity_compensation_use_kdl}</param>
+            <param name="gravity_torque_mode">${gravity_torque_mode}</param>
+            <param name="gravity_scale">${gravity_scale}</param>
+            <param name="gravity_tau_limit1">${gravity_tau_limit1}</param>
+            <param name="gravity_tau_limit2">${gravity_tau_limit2}</param>
+            <param name="gravity_tau_limit3">${gravity_tau_limit3}</param>
+            <param name="gravity_tau_limit4">${gravity_tau_limit4}</param>
+            <param name="gravity_tau_limit5">${gravity_tau_limit5}</param>
+            <param name="gravity_tau_limit6">${gravity_tau_limit6}</param>
+            <param name="gravity_tau_limit7">${gravity_tau_limit7}</param>
+            <param name="kdl_urdf_path">${kdl_urdf_path}</param>
+            <param name="kdl_base_link">${kdl_base_link}</param>
+            <param name="kdl_tip_link">${kdl_tip_link if kdl_tip_link != '' else 'openarm_' + right_arm_prefix + 'link7'}</param>
             <param name="kp1">${control_gains['joint1']['kp']}</param>
             <param name="kp2">${control_gains['joint2']['kp']}</param>
             <param name="kp3">${control_gains['joint3']['kp']}</param>