Update controllers and configs

c300/c300_bringup/config/c300_isaac_controllers.yaml

@@ -33,7 +33,7 @@ diff_drive_base_controller:
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
+    use_stamped_vel: true
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits
@@ -43,7 +43,7 @@ diff_drive_base_controller:
     linear.x.has_jerk_limits: false
     linear.x.max_velocity: 2.0
     linear.x.min_velocity: -2.0
-    linear.x.max_acceleration: 0.5
+    linear.x.max_acceleration: 2.5
     linear.x.max_jerk: 0.0
     linear.x.min_jerk: 0.0
 
@@ -52,7 +52,7 @@ diff_drive_base_controller:
     angular.z.has_jerk_limits: false
     angular.z.max_velocity: 2.0
     angular.z.min_velocity: -2.0
-    angular.z.max_acceleration: 1.0
-    angular.z.min_acceleration: -1.0
+    angular.z.max_acceleration: 3.2
+    angular.z.min_acceleration: -3.2
     angular.z.max_jerk: 0.0
     angular.z.min_jerk: 0.0

c300/c300_bringup/launch/isaac_c300.launch.py

@@ -58,7 +58,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_file",
-            default_value="c300_base.urdf",
+            default_value="c300.urdf.xacro",
             description="URDF/XACRO description file with the robot.",
         )
     )
@@ -145,7 +145,7 @@ def generate_launch_description():
             robot_controllers,
         ],
         remappings=[
-            ("/diff_drive_base_controller/cmd_vel_unstamped", "/cmd_vel"),
+            ("/diff_drive_base_controller/cmd_vel", "/cmd_vel_nav"),
             ("/diff_drive_base_controller/odom", "/odom"),
         ],
         output="both",

c300/c300_bringup/package.xml

@@ -18,6 +18,7 @@
   <exec_depend>ros_gz_sim</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>
+  <exec_depend>joint_state_topic_hardware_interface</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

c300/c300_bringup/worlds/depot.sdf

@@ -20,6 +20,10 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+       filename='gz-sim-contact-system'
+       name='gz::sim::systems::Contact'>
+    </plugin>
 
     <include>
       <uri>

c300/c300_description/urdf/c300.ros2_control.xacro

@@ -15,15 +15,17 @@
           <plugin>gz_ros2_control/GazeboSimSystem</plugin>
         </xacro:if>
         <xacro:if value="${sim_isaac}">
-          <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
+          <plugin>joint_state_topic_hardware_interface/JointStateTopicSystem</plugin>
           <param name="joint_commands_topic">${isaac_joint_commands}</param>
           <param name="joint_states_topic">${isaac_joint_states}</param>
           <param name="sum_wrapped_joint_states">true</param>
+          <param name="trigger_joint_command_threshold">-1</param> <!-- always publish commands on the topic -->
         </xacro:if>
         <xacro:if value="${mock_hardware}">
           <plugin>mock_components/GenericSystem</plugin>
           <param name="fake_sensor_commands">false</param>
           <param name="state_following_offset">0.0</param>
+          <param name="calculate_dynamics">true</param>
         </xacro:if>
       </hardware>
       <joint name="drivewhl_l_joint">

c300/c300_description/urdf/c300_macro.xacro

@@ -5,7 +5,7 @@
     prefix
     sim_isaac:=false
     sim_gazebo:=false
-    isaac_joint_commands:=/isaac_joint_commands
+    isaac_joint_commands:=/base_joint_command
     isaac_joint_states:=/isaac_joint_states
     mock_hardware:=false">
 

c300/c300_navigation/params/nav2_params_mppi.yaml

@@ -354,13 +354,13 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "CLOSED_LOOP"
-    max_velocity: [1.0, 0.0, 1.0] # [x, y, theta]
-    min_velocity: [-1.0, 0.0, -1.0] # [x, y, theta]
+    max_velocity: [2.0, 0.0, 2.0] # [x, y, theta]
+    min_velocity: [-2.0, 0.0, -2.0] # [x, y, theta]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
     odom_topic: "odom"
     odom_duration: 0.1
-    deadband_velocity: [0.0, 0.0, 0.0]
+    deadband_velocity: [0.05, 0.0, 0.15]
     velocity_timeout: 1.0
     enable_stamped_cmd_vel: True
 

c3pzero/c3pzero_bringup/config/c3pzero_gz_controllers.yaml

@@ -114,7 +114,7 @@ left_arm_jtc:
 right_arm_gripper_controller:
   ros__parameters:
     action_monitor_rate: 20.0
-    allow_stalling: false
+    allow_stalling: true
     goal_tolerance: 0.01
     joint: right_arm_gripper_joint
     stall_timeout: 1.0
@@ -123,7 +123,7 @@ right_arm_gripper_controller:
 left_arm_gripper_controller:
   ros__parameters:
     action_monitor_rate: 20.0
-    allow_stalling: false
+    allow_stalling: true
     goal_tolerance: 0.01
     joint: left_arm_gripper_joint
     stall_timeout: 1.0

c3pzero/c3pzero_bringup/config/c3pzero_hardware_controllers.yaml

@@ -45,7 +45,7 @@ diff_drive_base_controller:
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
+    use_stamped_vel: true
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits
@@ -86,6 +86,7 @@ right_arm_jtc:
     state_publish_rate: 100.0
     action_monitor_rate: 20.0
     allow_partial_joints_goal: false
+    allow_nonzero_velocity_at_trajectory_end: true
     constraints:
       stopped_velocity_tolerance: 0.0
       goal_time: 0.0
@@ -107,6 +108,7 @@ left_arm_jtc:
     state_publish_rate: 100.0
     action_monitor_rate: 20.0
     allow_partial_joints_goal: false
+    allow_nonzero_velocity_at_trajectory_end: true
     constraints:
       stopped_velocity_tolerance: 0.0
       goal_time: 0.0

c3pzero/c3pzero_bringup/config/c3pzero_isaac_controllers.yaml

@@ -45,7 +45,7 @@ diff_drive_base_controller:
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
+    use_stamped_vel: true
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits
@@ -55,7 +55,7 @@ diff_drive_base_controller:
     linear.x.has_jerk_limits: false
     linear.x.max_velocity: 2.0
     linear.x.min_velocity: -2.0
-    linear.x.max_acceleration: 0.5
+    linear.x.max_acceleration: 2.5
     linear.x.max_jerk: 0.0
     linear.x.min_jerk: 0.0
 

c3pzero/c3pzero_bringup/launch/isaac_c3pzero.launch.py

@@ -78,11 +78,12 @@ def generate_launch_description():
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[
-            {"use_sim_time": True, "robot_description": robot_description_content},
+            {"use_sim_time": True},
             robot_controllers,
         ],
         remappings=[
-            ("/diff_drive_base_controller/cmd_vel_unstamped", "/cmd_vel"),
+            ("~/robot_description", "/robot_description"),
+            ("/diff_drive_base_controller/cmd_vel", "/cmd_vel_nav"),
             ("/diff_drive_base_controller/odom", "/odom"),
         ],
         output="both",
@@ -102,10 +103,6 @@ def generate_launch_description():
             "--controller-manager",
             "/controller_manager",
         ],
-        remappings=[
-            ("/diff_drive_base_controller/cmd_vel_unstamped", "/cmd_vel"),
-            ("/diff_drive_base_controller/odom", "/odom"),
-        ],
         output="both",
     )
 

c3pzero/c3pzero_bringup/rviz/c3pzero.rviz

@@ -5,8 +5,8 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /LaserScan1/Topic1
-      Splitter Ratio: 0.36942070722579956
-    Tree Height: 182
+      Splitter Ratio: 0.6189376711845398
+    Tree Height: 1022
 Visualization Manager:
   Class: ""
   Displays:
@@ -147,6 +147,7 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+          Value: true
         left_arm_wrist_camera_color_frame:
           Alpha: 1
           Show Axes: false
@@ -263,6 +264,7 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+          Value: true
         right_arm_wrist_camera_color_frame:
           Alpha: 1
           Show Axes: false
@@ -366,20 +368,20 @@ Visualization Manager:
       Axis: Z
       Channel Name: intensity
       Class: rviz_default_plugins/LaserScan
-      Color: 255; 255; 255
-      Color Transformer: Intensity
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 0
+      Max Intensity: 17
       Min Color: 0; 0; 0
-      Min Intensity: 0
+      Min Intensity: 1
       Name: LaserScan
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.009999999776482582
+      Size (m): 0.004999999888241291
       Style: Flat Squares
       Topic:
         Depth: 5
@@ -392,7 +394,7 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Class: rviz_default_plugins/Camera
-      Enabled: true
+      Enabled: false
       Far Plane Distance: 100
       Image Rendering: background and overlay
       Name: RightWristCamera
@@ -403,7 +405,7 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /right_arm_wrist_camera/color/image_raw
-      Value: true
+      Value: false
       Visibility:
         Grid: true
         LaserScan: true
@@ -414,7 +416,7 @@ Visualization Manager:
         Value: true
       Zoom Factor: 1
     - Class: rviz_default_plugins/Camera
-      Enabled: true
+      Enabled: false
       Far Plane Distance: 100
       Image Rendering: background and overlay
       Name: LeftWristCamera
@@ -425,7 +427,7 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /left_arm_wrist_camera/color/image_raw
-      Value: true
+      Value: false
       Visibility:
         Grid: true
         LaserScan: true
@@ -456,25 +458,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 2.973403215408325
+      Distance: 3.0082173347473145
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.18479067087173462
-        Y: -0.09187475591897964
-        Z: 0.46009013056755066
+        X: 0.2740049660205841
+        Y: -0.2658533453941345
+        Z: 0.6486567258834839
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5347971320152283
+      Pitch: 0.5897971391677856
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.175384283065796
+      Yaw: 3.3403968811035156
     Saved: ~
 Window Geometry:
   Displays:
@@ -484,9 +486,9 @@ Window Geometry:
   Hide Right Dock: true
   LeftWristCamera:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000003330000042dfc0200000003fb000000100044006900730070006c0061007900730100000044000000f7000000eb00fffffffb00000020005200690067006800740057007200690073007400430061006d0065007200610100000142000001940000001600fffffffb0000001e004c0065006600740057007200690073007400430061006d00650072006101000002dd000001940000001600ffffff000004460000042d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001b300000439fc0200000003fb000000100044006900730070006c006100790073010000003b00000439000000c700fffffffb00000020005200690067006800740057007200690073007400430061006d006500720061000000013b0000019a0000001600fffffffb0000001e004c0065006600740057007200690073007400430061006d00650072006100000001d90000029b0000001600ffffff000005c70000043900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RightWristCamera:
     collapsed: false
   Width: 1920
-  X: 588
-  Y: 141
+  X: 3200
+  Y: 166

c3pzero/c3pzero_description/urdf/c3pzero.urdf.xacro

@@ -31,7 +31,7 @@
       mock_hardware="$(arg mock_hardware)"
       sim_gazebo="$(arg sim_gazebo)"
       sim_isaac="$(arg sim_isaac)"
-      isaac_joint_commands="/mobile_base_joint_commands"
+      isaac_joint_commands="/base_joint_command"
       isaac_joint_states="/isaac_joint_states"/>
 
     <!-- Right Arm -->
@@ -53,6 +53,13 @@
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:load_piper>
 
+    <link name="$(arg prefix)right_arm_grasp_link"/>
+    <joint name="$(arg prefix)right_arm_grasp_joint" type="fixed">
+      <parent link="$(arg prefix)right_arm_gripper_base_link"/>
+      <child link="$(arg prefix)right_arm_grasp_link"/>
+      <origin xyz="0.0 0.0 0.1" rpy="0 0 0"/>
+    </joint>
+
     <xacro:wrist_camera
         parent="$(arg prefix)right_arm_gripper_aux_mount"
         prefix="$(arg prefix)right_arm_"
@@ -78,6 +85,13 @@
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:load_piper>
 
+    <link name="$(arg prefix)left_arm_grasp_link"/>
+    <joint name="$(arg prefix)left_arm_grasp_joint" type="fixed">
+      <parent link="$(arg prefix)left_arm_gripper_base_link"/>
+      <child link="$(arg prefix)left_arm_grasp_link"/>
+      <origin xyz="0.0 0.0 0.1" rpy="0 0 0"/>
+    </joint>
+
     <xacro:wrist_camera
         parent="$(arg prefix)left_arm_gripper_aux_mount"
         prefix="$(arg prefix)left_arm_"

c3pzero/c3pzero_moveit_config/config/c3pzero.srdf.xacro

@@ -80,6 +80,12 @@
     <group_state name="closed" group="right_gripper">
         <joint name="right_arm_gripper_joint" value="0"/>
     </group_state>
+    <group_state name="open" group="left_gripper">
+        <joint name="left_arm_gripper_joint" value="0.038"/>
+    </group_state>
+    <group_state name="closed" group="left_gripper">
+        <joint name="left_arm_gripper_joint" value="0"/>
+    </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <end_effector name="right_eff" parent_link="right_arm_tool_0" group="right_gripper" parent_group="right_manipulator"/>
     <end_effector name="left_eff" parent_link="left_arm_tool_0" group="left_gripper" parent_group="left_manipulator"/>