增加调试信息，关闭moveit servo

openarm_bimanual_moveit_config/launch/demo.launch.py

@@ -348,6 +348,6 @@ def generate_launch_description():
             delayed_gripper,
             run_move_group_node,
             rviz_node,
-            delayed_servo,
+            # delayed_servo,
         ]
     )

openarm_bringup/launch/openarm.launch.py

@@ -140,7 +140,7 @@ def generate_launch_description():
         ),
         DeclareLaunchArgument(
             "can_interface",
-            default_value="can4",
+            default_value="can0",
             description="CAN interface to use.",
         ),
         DeclareLaunchArgument(

openarm_hardware/src/v10_simple_hardware.cpp

@@ -274,6 +274,10 @@ hardware_interface::return_type OpenArm_v10HW::write(
   for (size_t i = 0; i < ARM_DOF; ++i) {
     arm_params.push_back(
         {kp_[i], kd_[i], pos_commands_[i], vel_commands_[i], tau_commands_[i]});
+    // if(vel_commands_[i] != 0)
+    //   std::cout << "Arm Joint " << i + 1 << ": pos_cmd=" << pos_commands_[i]
+    //             << ", vel_cmd=" << vel_commands_[i]
+    //             << ", tau_cmd=" << tau_commands_[i] << std::endl;
   }
   openarm_->get_arm().mit_control_all(arm_params);
   // Control gripper if enabled