Kinovarobotics / ros2_kortex

ROS2 driver for the Gen3 Kinova robot arm
Other
46 stars 43 forks source link

run MoveIt Task Constructor demo using gen3 #177

Closed patrickwasp closed 3 weeks ago

patrickwasp commented 1 year ago

I'm trying to run modify the tutorial for task constructor to run with a gen3 arm. It seems that the following pieces of config would have to change, but I'm not sure what to.

pick_place_demo.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_dict()

    # MTC Demo node
    pick_place_demo = Node(
        package="mtc_tutorial",
        executable="mtc_tutorial",
        output="screen",
        parameters=[
            moveit_config,
        ],
    )

    return LaunchDescription([pick_place_demo])

specifically this line: moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_dict()

and

mtc_tutorial.cpp

mtc::Task MTCTaskNode::createTask()
{
  moveit::task_constructor::Task task;
  task.stages()->setName("demo task");
  task.loadRobotModel(node_);

  const auto& arm_group_name = "panda_arm";
  const auto& hand_group_name = "hand";
  const auto& hand_frame = "panda_hand";

  // Set task properties
  task.setProperty("group", arm_group_name);
  task.setProperty("eef", hand_group_name);
  task.setProperty("ik_frame", hand_frame);

I think this might work once the moveit_config is setup correctly

  const auto &arm_group_name = "manipulator";
  const auto &hand_group_name = "gripper";
  const auto &hand_frame = "end_effector_link";
felixmaisonneuve commented 1 year ago

Hi @patrickwasp,

I don't know much about the MTC tutorial, but I can help you with the portion of code you linked. First, for the pick_and_place launch file, you want a moveit config for our Gen3 arm. You can take the one from our moveit example (I took the 6dof example, but a 7dof example is available as well) https://github.com/Kinovarobotics/ros2_kortex/blob/a2f018ab8c39e97fe91f57c27b8e59852b029da4/kortex_moveit_config/kinova_gen3_6dof_robotiq_2f_85_moveit_config/launch/robot.launch.py#L41-L57

make sure to pass moveit_config.to_dict() in your parameters or add .to_dict() when creating your moveit_config object https://github.com/Kinovarobotics/ros2_kortex/blob/main/kortex_moveit_config/kinova_gen3_6dof_robotiq_2f_85_moveit_config/launch/robot.launch.py#L66

For the CreateTask function, what you are saying makes sense, but you will need to use the correct node_ with the gen3 model

Best, Felix

patrickwasp commented 1 year ago

@felixmaisonneuve I prepared a minimal environment to troubleshoot working with a kinova 6 dof and robotiq 85 gripper along with task constructor.

https://github.com/ecoation-labs/ros2-kortex-pick-and-place-with-moveit-task-constructor/

I'm looking to replicate the tutorial example with the FRANKA EMIKA Panda arm shown in the moveit2 tutorial documentation.

Can you please see if you can find the errors I made?

ssapsu commented 5 months ago

@patrickwasp Hi, patrick Did you figured out how to solve that problem?? I found moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_dict() should be changed to moveit_config = MoveItConfigsBuilder("gen3", package_name="kinova_gen3_6dof_robotiq_2f_85_moveit_config").to_dict() and maybe I think

  const auto& arm_group_name = "panda_arm";
  const auto& hand_group_name = "hand";
  const auto& hand_frame = "panda_hand";

should be replace to

  const auto& arm_group_name = "manipulator";
  const auto& hand_group_name = "gripper";
  const auto& hand_frame = "end_effector_link";
aalmrad commented 3 weeks ago

Closing due to inactivity - reply to the thread if you need the issue to be reopened