The Cartesian motion trajectory of moveit2 MTC has been ignored. How could i solve this problem? #741
Description
Hello. When I attempted to apply some of the routines from moveit2 MTC to a real robotic arm, I encountered the issue where the Cartesian motion trajectory was ignored. As shown in the video: I attempted to move the end of the robotic arm above the target, and then lower it to the grasping position. But the robotic arm reached the target position directly. May I ask how to solve this problem?
My code follow:
`task0.stages()->setName("MoveArm");/* 任务阶段名称 /
task0.loadRobotModel(node_ptr); / 加载节点的机器人模型 */
/* 设置规划组、末端执行器、末端执行器的frame */
const auto& arm_group_name = "arm";
const auto& hand_group_name = "hand";
const auto& hand_frame = "endlink";
task0.setProperty("group", arm_group_name);
task0.setProperty("eef", hand_group_name);
task0.setProperty("ik_frame", hand_frame);
/* 开始准备任务 /
mtc::Stage current_state_ptr = nullptr; //指针用于保存阶段
auto plan_container = Create_SerialContainer(task0,"Pick Target");/* 创建串行容器 */
plan_container->insert( std::move(Memory_Current_Stage(task0,current_state_ptr)) );//保存当前阶段
plan_container->insert( std::move(Gripper_Control(task0,hand_group_name,"open")) );//夹爪打开
plan_container->insert( std::move(Arm_Control(task0,arm_group_name,"display")) );//移动到固定姿势display
//plan_container->insert(std::move( CreateCollisionModifier(task0, "hand", "object01", true, collision_links) ));/* 允许碰撞 */
//========================================================
auto move_to_pick = Create_Connect_Stage(task0, "move to pick", arm_group_name ,sampling_planner, 5.0);
mtc::Stage* pick_connect_stage_ptr = move_to_pick.get(); // 保存指针供监控使用
plan_container->insert(std::move(move_to_pick));//连接器
/* ------------------修改笛卡尔规划器-Begin-2026.3.26 14:11-------------------------- */
plan_container->insert(std::move(Approach_Object_Stage(task0, hand_frame) ));/*接近目标 此处笛卡尔运动存在问题 */
/* ------------------修改笛卡尔规划器-End-2026.3.26 14:11-------------------------- */
/* 设置抓取位姿 /
Eigen::Isometry3d grasp_transform = Eigen::Isometry3d::Identity();
Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
grasp_transform.linear() = q.matrix();
grasp_transform.translation().y() = 0.12; /末端执行器的y轴(夹爪正方向)正方向0.04位置 作为目标位置/
auto grasp_stage = Create_Grasp_Stage(task0, "object01", "open", current_state_ptr, grasp_transform, hand_frame);
mtc::Stage grasp_stage_ptr = grasp_stage.get(); // 保存指针供接近阶段监控
plan_container->insert(std::move(grasp_stage));
//========================================================
task0.add(std::move(plan_container)) ;
RCLCPP_INFO(LOGGER, "任务准备完毕...");
try{
RCLCPP_INFO(LOGGER, "初始化任务...");
task0.init();
}
catch(mtc::InitStageException& e){
RCLCPP_INFO(LOGGER, "初始化失败...");
RCLCPP_ERROR_STREAM(LOGGER, e); return;
}
RCLCPP_INFO(LOGGER, "开始规划...");
if (!task0.plan(2)){RCLCPP_ERROR_STREAM(LOGGER, "没有找到可行方案!"); return; } /* 寻找1个成功的规划方案 */
RCLCPP_INFO(LOGGER, "可视化方案...");
task0.introspection().publishSolution(task0.solutions().front());/将解决方案以可视化的方式发送到rviz2的MotionPlanner列表/
RCLCPP_INFO(LOGGER, "开始执行方案...");
/ 执行方案 */
auto result = task0.execute(task0.solutions().front());
/ 检查执行结果 */
RCLCPP_INFO(LOGGER, "执行完毕,查询执行结果...");
if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS )
{
RCLCPP_ERROR_STREAM(LOGGER, "任务执行失败! 错误码: " << result.val);
return;
}
RCLCPP_INFO(LOGGER, "执行成功.");
`