已经按照鱼哥教程安装好了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