增加夹爪的重力补偿

rviz/bimanual.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
       Splitter Ratio: 0.5
-    Tree Height: 139
+    Tree Height: 409
   - Class: rviz_common/Views
     Expanded:
       - /Current View1
@@ -51,6 +51,11 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        openarm_body_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         openarm_left_hand:
           Alpha: 1
           Show Axes: false
@@ -105,10 +110,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        openarm_left_link8:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
         openarm_left_right_finger:
           Alpha: 1
           Show Axes: false
@@ -168,20 +169,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        openarm_right_link8:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
         openarm_right_right_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        v10_body_link0:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         world:
           Alpha: 1
           Show Axes: false
@@ -223,35 +215,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 5
+      Distance: 2.3220205307006836
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0
+        X: -0.07885967940092087
-        Y: 0
+        Y: 0.03267698734998703
-        Z: 0
+        Z: 0.2304663509130478
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.33000001311302185
+      Pitch: 0.2900000512599945
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.5
+      Yaw: 0.7618141174316406
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 826
+  Height: 1174
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001da000002a8fc0200000002fb000000100044006900730070006c0061007900730100000069000001790000017800fffffffb0000000a0056006900650077007301000001ee000001230000012300ffffff000002ca000002a800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001da0000043cfc0200000002fb000000100044006900730070006c006100790073010000003d00000260000000c900fffffffb0000000a0056006900650077007301000002a3000001d6000000a400ffffff000007d80000043c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1200
+  Width: 2488
-  X: 42
+  X: 70
-  Y: 64
+  Y: 27

urdf/robot/openarm_robot.xacro

@@ -48,6 +48,8 @@
             gravity_tau_limit5:='2.0'
             gravity_tau_limit6:='2.0'
             gravity_tau_limit7:='2.0'
+            gripper_gravity_tau:='0.0'
+            gripper_gravity_tau_limit:='0.5'
             kdl_urdf_path:='/tmp/openarm_bimanual_runtime.urdf'
             kdl_base_link:='openarm_body_link0'
             kdl_tip_link:=''
@@ -136,6 +138,8 @@
             gravity_tau_limit5="${gravity_tau_limit5}"
             gravity_tau_limit6="${gravity_tau_limit6}"
             gravity_tau_limit7="${gravity_tau_limit7}"
+            gripper_gravity_tau="${gripper_gravity_tau}"
+            gripper_gravity_tau_limit="${gripper_gravity_tau_limit}"
             kdl_urdf_path="${kdl_urdf_path}"
             kdl_base_link="${kdl_base_link}"
             kdl_tip_link="${kdl_tip_link}"/>

urdf/robot/v10.urdf.xacro

@@ -59,6 +59,8 @@
   <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="gripper_gravity_tau" default="0.0" />
+  <xacro:arg name="gripper_gravity_tau_limit" default="0.5" />
   <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="" />
@@ -108,6 +110,8 @@
   gravity_tau_limit5="$(arg gravity_tau_limit5)"
   gravity_tau_limit6="$(arg gravity_tau_limit6)"
   gravity_tau_limit7="$(arg gravity_tau_limit7)"
+  gripper_gravity_tau="$(arg gripper_gravity_tau)"
+  gripper_gravity_tau_limit="$(arg gripper_gravity_tau_limit)"
   kdl_urdf_path="$(arg kdl_urdf_path)"
   kdl_base_link="$(arg kdl_base_link)"
   kdl_tip_link="$(arg kdl_tip_link)"

urdf/ros2_control/openarm.bimanual.ros2_control.xacro

@@ -23,6 +23,8 @@
              gravity_tau_limit5:=^|5.0
              gravity_tau_limit6:=^|3.0
              gravity_tau_limit7:=^|3.0
+             gripper_gravity_tau:=^|0.0
+             gripper_gravity_tau_limit:=^|0.5
              kdl_urdf_path:=^|/tmp/openarm_bimanual_runtime.urdf
              kdl_base_link:=^|openarm_body_link0
              kdl_tip_link:=^|">
@@ -53,6 +55,8 @@
             <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="gripper_gravity_tau">${gripper_gravity_tau}</param>
+            <param name="gripper_gravity_tau_limit">${gripper_gravity_tau_limit}</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>
@@ -132,6 +136,8 @@
             <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="gripper_gravity_tau">${gripper_gravity_tau}</param>
+            <param name="gripper_gravity_tau_limit">${gripper_gravity_tau_limit}</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>