run MoveIt Task Constructor demo using gen3

[patrickwasp]

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]

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)

ros2_kortex/kortex_moveit_config/kinova_gen3_6dof_robotiq_2f_85_moveit_config/launch/robot.launch.py

Lines 41 to 57 in a2f018a

	launch_arguments = {
	"robot_ip": robot_ip,
	"use_fake_hardware": use_fake_hardware,
	"gripper": "robotiq_2f_85",
	"gripper_joint_name": "robotiq_85_left_knuckle_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", package_name="kinova_gen3_6dof_robotiq_2f_85_moveit_config")
	.robot_description(mappings=launch_arguments)
	.planning_pipelines(pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"])
	.to_moveit_configs()
	)

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]

@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]

@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";