Slow and shaky gripper when using twist_controller

[julien-audet]

Versions

• OS: Ubuntu 22.04
• ROS 2: Humble
• Using humble branch
• Gen3 6 dof with robotiq 2f 85 gripper

Steps to reproduce

Launch robot:

ros2 launch kinova_gen3_6dof_robotiq_2f_85_moveit_config robot.launch.py \
  robot_ip:=yyy.yyy.yyy.yyy \
  use_fake_hardware:=true

Change controller to twist_controller:

ros2 service call /controller_manager/switch_controller controller_manager_msgs/srv/SwitchController "{
  activate_controllers: [twist_controller],
  deactivate_controllers: [joint_trajectory_controller],
  strictness: 1,
  activate_asap: true,

Use action gripper command to close or open gripper

ros2 action send_goal /robotiq_gripper_controller/gripper_cmd control_msgs/action/GripperCommand "{command:{position: 0.0, max_effort: 100.0}}"
}"

Gripper is shaky and slow when using the twist_controller, and it is fast and smooth when using joint_trajectory controller.

Problem

I think the problem is that the twist_controller uses servoing mode SINGLE_LEVEL_SERVOING which I think (according to the doc) is running at 40 Hz, versus LOW_LEVEL_SERVOING, which is running at 1000 Hz. From ros2_kortex/kortex_driver/src/hardware_interface.cpp:

/hardware_interface.cpp

      if (arm_mode == k_api::Base::ServoingMode::SINGLE_LEVEL_SERVOING)
      {
        k_api::Base::GripperCommand gripper_command;
        gripper_command.set_mode(k_api::Base::GRIPPER_POSITION);
        auto finger = gripper_command.mutable_gripper()->add_finger();
        finger->set_finger_identifier(1);
        finger->set_value(
          static_cast<float>(position / 0.81));  // This values needs to be between 0 and 1
        base_.SendGripperCommand(gripper_command);
      }
      else if (arm_mode == k_api::Base::ServoingMode::LOW_LEVEL_SERVOING)
      {
        // % open/closed, this values needs to be between 0 and 100
        gripper_motor_command_->set_position(static_cast<float>(position / 0.81 * 100.0));
        // % gripper speed between 0 and 100 percent
        gripper_motor_command_->set_velocity(static_cast<float>(velocity));
        // % max force threshold, between 0 and 100
        gripper_motor_command_->set_force(static_cast<float>(force));
      }

Also, if both controllers are down, the gripper won't work.

Is it possible to use the gripper without shaking, or without the twist_controller or joint_trajectory_controller active?

[aalmrad]

@julien-audet the twist controller is supposed to be used with real-life hardware. Are you sure you were able to switch from joint_trajectory_controller to twist_controller?

[julien-audet]

Yes, it successfully switched to the twist_controller (I can move the robot using /twist_controller/commands), but the gripper becomes slow and shaky. It is not shaky at all when switching back to the joint_trajectory_controller. Have you tried to reproduce the issue?