ros2通过moveit2控制双臂机器人

[zhang_bo7]

已经按照鱼哥教程安装好了moveit2，如图所示设置了规划组。通过程序控制both_arm规划组控制双臂运行出现规划失败情况。

这是我的c++程序

#include <rclcpp/rclcpp.hpp>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/msg/pose.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <iostream>

tf2::Quaternion getQuat(geometry_msgs::msg::Pose target_pose)
{
    tf2::Quaternion targetQ;
    targetQ[0] = target_pose.orientation.x;
    targetQ[1] = target_pose.orientation.y;
    targetQ[2] = target_pose.orientation.z;
    targetQ[3] = target_pose.orientation.w;
    return targetQ;
}


geometry_msgs::msg::Pose pose_specify(float Xx, float Yy, float Zz, float R, float P, float Y)
{
    geometry_msgs::msg::Pose target_pose;
    tf2::Quaternion targetQ;

    target_pose.position.x = Xx;
    target_pose.position.y = Yy;
    target_pose.position.z = Zz;

    targetQ.setRPY(R, P, Y);
    target_pose.orientation.x = targetQ.x();
    target_pose.orientation.y = targetQ.y();
    target_pose.orientation.z = targetQ.z();
    target_pose.orientation.w = targetQ.w();

    return target_pose;
}

bool both_arm_motion(moveit::planning_interface::MoveGroupInterface& move_group_l, moveit::planning_interface::MoveGroupInterface& move_group_r,geometry_msgs::msg::Pose& left_target_pose, geometry_msgs::msg::Pose& right_target_pose)
{
    // Create planning component for both arms
    moveit::planning_interface::MoveGroupInterface::Plan plan_l;
    moveit::planning_interface::MoveGroupInterface::Plan plan_r;
    bool success;
    move_group_l.setStartStateToCurrentState();
    move_group_r.setStartStateToCurrentState();
    RCLCPP_INFO(rclcpp::get_logger("moveit_cpp_tutorial"), "The pose reference frame is: %s", move_group_l.getPlanningFrame().c_str());
    RCLCPP_INFO(rclcpp::get_logger("moveit_cpp_tutorial"), "The pose reference frame is: %s", move_group_r.getPlanningFrame().c_str());
    // Set the target poses
    move_group_l.setPoseTarget(left_target_pose, "er3a_end_frame");
    move_group_r.setPoseTarget(right_target_pose, "linkhou_end_frame");

    // Plan and execute
    do
    {
        success = ((move_group_l.plan(plan_l)&&move_group_r.plan(plan_r))== moveit::planning_interface::MoveItErrorCode::SUCCESS);
    } while (!success);

    move_group_l.execute(plan_l);
    move_group_r.execute(plan_r);
    return true;
}


bool both_arm_motion2(moveit::planning_interface::MoveGroupInterface& move_group,geometry_msgs::msg::Pose& left_target_pose, geometry_msgs::msg::Pose& right_target_pose)
{
    // Create planning component for both arms
    moveit::planning_interface::MoveGroupInterface::Plan dual_plan;

    bool success;
    move_group.setStartStateToCurrentState();

    RCLCPP_INFO(rclcpp::get_logger("moveit_cpp_tutorial"), "The pose reference frame is: %s", move_group.getPlanningFrame().c_str());

    // Set the target poses
    move_group.setPoseTarget(left_target_pose, "er3a_end_frame");
    move_group.setPoseTarget(right_target_pose, "linkhou_end_frame");

    // Plan and execute
    do
    {
        success = (move_group.plan(dual_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    } while (!success);
    RCLCPP_INFO(rclcpp::get_logger("moveit_cpp_tutorial"), "fuck fuck fuck fuck fuck");
    move_group.execute(dual_plan);
    return true;
}

int main(int argc, char* argv[])
{
    // Initialize ROS
    rclcpp::init(argc, argv);

    // Initialize MoveIt
    auto node = std::make_shared<rclcpp::Node>("hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    //const std::string both_arm = "both_arm";
    auto move_group = moveit::planning_interface::MoveGroupInterface(node, "both_arm");
    auto move_group_l = moveit::planning_interface::MoveGroupInterface(node, "left_arm");
    auto move_group_r = moveit::planning_interface::MoveGroupInterface(node, "right_arm");
    
    move_group.allowReplanning(true);
    move_group.setNumPlanningAttempts(10);
    move_group.setMaxVelocityScalingFactor(0.8);
    move_group.setMaxAccelerationScalingFactor(0.8);
    move_group.setPoseReferenceFrame("Link_foot");
    move_group_l.setPlanningTime(5.0);  // Set planning time to 5 seconds
    move_group_r.setPlanningTime(5.0);
    move_group.setPlanningTime(5.0);

    move_group_l.allowReplanning(true);
    move_group_l.setNumPlanningAttempts(10);
    move_group_l.setPoseReferenceFrame("Link_foot");

    move_group_r.allowReplanning(true);
    move_group_r.setNumPlanningAttempts(10);
    move_group_r.setPoseReferenceFrame("Link_foot");
   // moveit::planning_interface::PlanningSceneInterface planning_scene_interface; // 添加此行

    // Call the both_arm_motion function
    geometry_msgs::msg::Pose left_target_pose;
    geometry_msgs::msg::Pose right_target_pose;
    geometry_msgs::msg::PoseStamped current_pose_l = move_group_l.getCurrentPose();
    geometry_msgs::msg::PoseStamped current_pose_r = move_group_r.getCurrentPose();
    RCLCPP_INFO(rclcpp::get_logger("moveit_cpp_tutorial"),"left current Pose : [%f, %f, %f, %f, %f, %f, %f]" ,
    current_pose_l.pose.position.x, current_pose_l.pose.position.y, current_pose_l.pose.position.z,
    current_pose_l.pose.orientation.x, current_pose_l.pose.orientation.y, current_pose_l.pose.orientation.z,
    current_pose_l.pose.orientation.w);

    RCLCPP_INFO(rclcpp::get_logger("moveit_cpp_tutorial"),"right current Pose : [%f, %f, %f, %f, %f, %f, %f]" ,
    current_pose_r.pose.position.x, current_pose_r.pose.position.y, current_pose_r.pose.position.z,
    current_pose_r.pose.orientation.x, current_pose_r.pose.orientation.y, current_pose_r.pose.orientation.z,
    current_pose_r.pose.orientation.w);

                                           
    
    
    tf2::Quaternion left_rotate,right_rotate,q_rot,q_rots45,q_rotn45;
    right_target_pose = pose_specify(0.35,0.5,0.02+0.05,0,M_PI,M_PI/2);
    left_target_pose = pose_specify(0.35,0.7,0.02+0.05,0,0,-M_PI/2);
    left_rotate = getQuat(left_target_pose);
    q_rot.setRPY(-M_PI/4, 0, 0);
    left_rotate = q_rot*left_rotate;  // Calculate the new orientation
    left_rotate.normalize();
    right_rotate = getQuat(right_target_pose);
    q_rot.setRPY(M_PI/4, 0, 0);
    right_rotate = q_rot*right_rotate;  // Calculate the new orientation
    right_rotate.normalize();
    left_target_pose.orientation.x = left_rotate[0];//.70739;
    left_target_pose.orientation.y = left_rotate[1];//.70683;
    left_target_pose.orientation.z = left_rotate[2];
    left_target_pose.orientation.w = left_rotate[3];
    right_target_pose.orientation.x = right_rotate[0];//.70739;
    right_target_pose.orientation.y = right_rotate[1];//.70683;
    right_target_pose.orientation.z = right_rotate[2];
    right_target_pose.orientation.w = right_rotate[3];

    //both_arm_motion(move_group_l, move_group_r, left_target_pose, right_target_pose);
    both_arm_motion2(move_group, left_target_pose, right_target_pose);
    // Shutdown ROS
    rclcpp::shutdown();

    return 0;
}

目标点确定是可行的，因为我通过使用dual_arm_motion测试，即使用move_group_l和move_group_r分别控制两机械臂可以运动到设定点，但使用dual_arm_motion2,即使用move_group就会规划失败c++程序报错：

[INFO] [1702386328.396770338] [move_group_interface]: MoveGroup action client/server ready
[INFO] [1702386328.397073422] [move_group_interface]: Planning request accepted
[INFO] [1702386329.511238255] [move_group_interface]: Planning request aborted
[ERROR] [1702386329.512311632] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached
[INFO] [1702386329.512426744] [move_group_interface]: MoveGroup action client/server ready
[INFO] [1702386329.512875038] [move_group_interface]: Planning request accepted
[INFO] [1702386330.642550132] [move_group_interface]: Planning request aborted
[ERROR] [1702386330.642602329] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached

另外启动的demo.launch终端报错：

[move_group-2] [ERROR] [1702386331.682317993] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:213 - both_arm/both_arm: Insufficient states in sampleable goal region
[move_group-2] [ERROR] [1702386331.682397145] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:213 - both_arm/both_arm: Insufficient states in sampleable goal region
[move_group-2] [WARN] [1702386331.682453413] [ompl]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 0.000423 seconds
[move_group-2] [ERROR] [1702386331.682554129] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:213 - both_arm/both_arm: Insufficient states in sampleable goal region
[move_group-2] [ERROR] [1702386331.682620236] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:213 - both_arm/both_arm: Insufficient states in sampleable goal region
[move_group-2] [WARN] [1702386331.682670208] [ompl]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 0.000185 seconds
[move_group-2] [INFO] [1702386331.754546175] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[move_group-2] [WARN] [1702386331.754646109] [moveit.ros_planning.planning_pipeline]: More than one constraint is set. If your move_group does not have multiple end effectors/arms, this is unusual. Are you using a move_group_interface and forgetting to call clearPoseTargets() or equivalent?
[move_group-2] [INFO] [1702386331.754712714] [moveit_move_group_default_capabilities.move_action_capability]: Catastrophic failure