Gen3 Lite gripper control issue

[PrwTsrt]

I can't control the gripper with MoveIt when I execute the gripper plan; it always fails. However, the arm operates without any issues.

These are my configs and launch file.

robot.launch.py

import os

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import (
    DeclareLaunchArgument,
    OpaqueFunction,
    RegisterEventHandler,
)
from launch.event_handlers import OnProcessExit
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder


def launch_setup(context, *args, **kwargs):
    # Initialize Arguments
    robot_ip = LaunchConfiguration("robot_ip")
    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
    gripper_max_velocity = LaunchConfiguration("gripper_max_velocity")
    gripper_max_force = LaunchConfiguration("gripper_max_force")
    launch_rviz = LaunchConfiguration("launch_rviz")
    use_sim_time = LaunchConfiguration("use_sim_time")
    use_internal_bus_gripper_comm = LaunchConfiguration("use_internal_bus_gripper_comm")

    launch_arguments = {
        "robot_ip": robot_ip,
        "use_fake_hardware": use_fake_hardware,
        "gripper": "gen3_lite_2f",
        "gripper_joint_name": "right_finger_bottom_joint",
        "dof": "6",
        "gripper_max_velocity": gripper_max_velocity,
        "gripper_max_force": gripper_max_force,
        "use_internal_bus_gripper_comm": use_internal_bus_gripper_comm,
    }

    moveit_config = (
        MoveItConfigsBuilder("gen3_lite_gen3_lite_2f", package_name="kinova_gen3_lite_moveit_config")
        .robot_description(mappings=launch_arguments)
        .trajectory_execution(file_path="config/moveit_controllers.yaml")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
        .to_moveit_configs()
    )

    moveit_config.moveit_cpp.update({"use_sim_time": use_sim_time.perform(context) == "true"})

    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            moveit_config.to_dict(),
        ],
    )

    # Static TF
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["--frame-id", "world", "--child-frame-id", "base_link"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[
            moveit_config.robot_description,
        ],
    )

    # ros2_control using FakeSystem as hardware
    ros2_controllers_path = os.path.join(
        get_package_share_directory("kinova_gen3_lite_moveit_config"),
        "config",
        "ros2_controllers.yaml",
    )
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[ros2_controllers_path],
        remappings=[("/controller_manager/robot_description", "/robot_description")],
        output="both",
    )

    robot_traj_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["gen3_lite_arm_controller", "-c", "/controller_manager"],
    )

    robot_pos_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["twist_controller", "--inactive", "-c", "/controller_manager"],
    )

    robot_hand_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["gen3_lite_hand_controller", "-c", "/controller_manager"],
    )

    fault_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["fault_controller", "-c", "/controller_manager"],
        condition=UnlessCondition(use_fake_hardware),
    )

    # rviz with moveit configuration
    rviz_config_file = (
        get_package_share_directory("kinova_gen3_lite_moveit_config")
        + "/config/moveit.rviz"
    )
    rviz_node = Node(
        package="rviz2",
        condition=IfCondition(launch_rviz),
        executable="rviz2",
        name="rviz2_moveit",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
        ],
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    # Delay rviz start after `joint_state_broadcaster`
    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=joint_state_broadcaster_spawner,
            on_exit=[rviz_node],
        ),
        condition=IfCondition(launch_rviz),
    )

    nodes_to_start = [
        ros2_control_node,
        robot_state_publisher,
        joint_state_broadcaster_spawner,
        delay_rviz_after_joint_state_broadcaster_spawner,
        robot_traj_controller_spawner,
        robot_pos_controller_spawner,
        robot_hand_controller_spawner,
        fault_controller_spawner,
        move_group_node,
        static_tf,
    ]

    return nodes_to_start


def generate_launch_description():
    # Declare arguments
    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument(
            "robot_ip",
            description="IP address by which the robot can be reached.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "use_fake_hardware",
            default_value="false",
            description="Start robot with fake hardware mirroring command to its states.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "gripper_max_velocity",
            default_value="100.0",
            description="Max velocity for gripper commands",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "gripper_max_force",
            default_value="100.0",
            description="Max force for gripper commands",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "use_internal_bus_gripper_comm",
            default_value="true",
            description="Use arm's internal gripper connection",
        )
    )

    declared_arguments.append(
        DeclareLaunchArgument(
            "use_sim_time",
            default_value="false",
            description="Use simulated clock",
        )
    )

    declared_arguments.append(
        DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
    )

    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

moveit_controllers.yaml

moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

moveit_simple_controller_manager:
  controller_names:
    - gen3_lite_arm_controller
    - gen3_lite_hand_controller

  gen3_lite_arm_controller:
    type: FollowJointTrajectory
    action_ns: follow_joint_trajectory
    default: true
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
    action_ns: follow_joint_trajectory
    default: true
  gen3_lite_hand_controller:
    type: GripperCommand
    joints:
      - right_finger_bottom_joint
    action_ns: gripper_cmd
    default: true

ros2_controllers.yaml

controller_manager:
  ros__parameters:
    update_rate: 1000  # Hz

    gen3_lite_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController


    gen3_lite_hand_controller:
      type: position_controllers/GripperActionController


    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    twist_controller:
      type: picknik_twist_controller/PicknikTwistController

    fault_controller:
      type: picknik_reset_fault_controller/PicknikResetFaultController

gen3_lite_arm_controller:
  ros__parameters:
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.0
      goal_time: 0.0
      
gen3_lite_hand_controller:
  ros__parameters:
    joint: right_finger_bottom_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

twist_controller:
  ros__parameters:
    joint: tcp
    interface_names:
      - twist.linear.x
      - twist.linear.y
      - twist.linear.z
      - twist.angular.x
      - twist.angular.y
      - twist.angular.z

[martinleroux]

Hi @PrwTsrt ,
I believe we are already working on a fix for this. The person who is handling it is currently travelling to a tradeshow. I will forward him this issue to ensure he sees it upon his return at the end of the week.

Cheers

[aalmrad]

Hello @PrwTsrt ,
Thank you for reaching out. After checking your files, I can see that a separate group was created for the right_finger_bottom_joint with a separate controller inside Moveit2. However in the case of the Gen3-Lite, the gripper is part of the robotic arm and should be treated as such, therefore the right_finger_bottom_joint should be added to the group containing the rest of the robotic arm's joints. I have created a moveit2 package for the Gen3-Lite that addresses this issue and you can find it in the following pull request: #231
If you face any additional problems or have further questions, please do not hesitate to reach out.

[PrwTsrt]

Hello @rahulpatel,

Thank you for your prompt response and for addressing the issue. I integrated the moveit2 package from pull request #231, and everything is working perfectly now.

I appreciate your support and the quick resolution. If I encounter any further issues or have additional questions, I'll be sure to reach out.

Thanks again!

[PrwTsrt]

Hi @rahulpatel,

After rebuilding my workspace and cloning the Git repository, I discovered that I am unable to control the arm without the gripper. Is it possible to separate the control of the arm and the gripper using MoveIt?

[aalmrad]

Hello @PrwTsrt ,

Using the setup assistant, you can customize the moveit package that i have created for the Gen3 Lite to your liking. For instance, you can put the gripper's joint under a separate control group to plan and execute its motion separately from the arm. Just a quick note, my name tag is @aalmrad . I'm here to help with any other questions or solutions you might need.

[PrwTsrt]

Hello @aalmrad, I apologize for the misunderstanding earlier. I attempted to separate the gripper into its own control group. However, when I set the controller type to FollowJointTrajectory, I encountered an error

[ros2_control_node-1] During ros2_control interface configuration, degrees of freedom is not valid; it should be positive. Actual DOF is 0

I also changed the controller type to GripperCommand, which allowed me to control the arm without issues. Unfortunately, when I try to execute the gripper plan, it consistently fails.

.srdf file

<group name="arm">
        <joint name="base_joint"/>
        <joint name="joint_1"/>
        <joint name="joint_2"/>
        <joint name="joint_3"/>
        <joint name="joint_4"/>
        <joint name="joint_5"/>
        <joint name="joint_6"/>
    </group>
    <group name="hand">
        <joint name="right_finger_tip_joint"/>
        <joint name="right_finger_bottom_joint"/>
        <joint name="left_finger_tip_joint"/>
        <joint name="left_finger_bottom_joint"/>
        <joint name="gripper_base_joint"/>
    </group>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="hand" parent_link="end_effector_link" group="hand"/>
    <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
    <passive_joint name="left_finger_bottom_joint"/>
    <passive_joint name="left_finger_tip_joint"/>
    <passive_joint name="right_finger_tip_joint"/>

ros2_controllers.yaml

controller_manager:
  ros__parameters:
    update_rate: 1000  # Hz

    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController


    hand_controller:
      type: joint_trajectory_controller/JointTrajectoryController


    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

arm_controller:
  ros__parameters:
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
hand_controller:
  ros__parameters:
    joint: right_finger_bottom_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

moveit_controllers.yaml

moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

moveit_simple_controller_manager:
  controller_names:
    - arm_controller
    - hand_controller

  arm_controller:
    type: FollowJointTrajectory
    action_ns: follow_joint_trajectory
    default: true
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
    action_ns: follow_joint_trajectory
    default: true
  hand_controller:
    type: FollowJointTrajectory
    action_ns: follow_joint_trajectory
    default: true
    joints:
      - right_finger_bottom_joint
    action_ns: follow_joint_trajectory
    default: true

[aalmrad]

Hello @PrwTsrt,

Please refer to the recently merged pull request to solve this problem.

Best,
Abed