Elite机械臂Moveit程序

/ 2025-08-12

// moveit_best_path_selector.cpp
// 完整 MoveIt C++ 节点示例：
// - 连续生成多条路径（多次调用 plan()）
// - 评估路径长度（关节空间总距离）
// - 选出最佳路径并执行
//
// 使用环境：ROS1 + MoveIt1
// 编译方法：把此文件放到你的 catkin 包的 src/ 下，修改 CMakeLists.txt 添加可执行文件并链接 moveit 和 roscpp
// 在文件末尾有示例 CMakeLists.txt / package.xml 片段







#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit_msgs/RobotTrajectory.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>








#include
#include
#include

#include <geometry_msgs/Pose.h>
#include <boost/asio.hpp>
#include
using boost::asio::ip::tcp;


// 评估函数：按关节空间总绝对增量来衡量路径长度（越小越好）
double evaluatePathLength(const moveit_msgs::RobotTrajectory &traj)
{
const trajectory_msgs::JointTrajectory &jt = traj.joint_trajectory;
if (jt.points.size() < 2)
return std::numeric_limits::infinity();




double total = 0.0;
for (size_t i = 1; i < jt.points.size(); ++i)
{
for (size_t j = 0; j < jt.joint_names.size(); ++j)
{
double a = jt.points[i - 1].positions[j];
double b = jt.points[i].positions[j];
total += fabs(b - a);
}
}
return total;
}










// 给 trajectory 重新做时间参数化（可选，但常用）
bool timeParameterizeTrajectory(robot_trajectory::RobotTrajectory &robot_traj)
{
trajectory_processing::IterativeParabolicTimeParameterization iptp;
bool success = iptp.computeTimeStamps(robot_traj);
return success;
}





int main(int argc, char **argv)
{
ros::init(argc, argv, “test_v2”);
ros::NodeHandle nh(“~”);
ros::AsyncSpinner spinner(1);
spinner.start();




// 参数
std::string move_group_name;
int attempts;
double planning_time;
bool execute_best;



//失败次数统计
int a = 0;
//总次数
int sum = 0;


double total_score = 0;

nh.paramstd::string(“move_group”, move_group_name, “arm”);
nh.param(“attempts”, attempts, 10);
nh.param(“planning_time”, planning_time, 10.0);
nh.param(“execute_best”, execute_best, true);


ROS_INFO(“[best_path_selector] group=%s attempts=%d planning_time=%.2f execute=%s”,
move_group_name.c_str(), attempts, planning_time, execute_best ? “true” : “false”);

moveit::planning_interface::MoveGroupInterface move_group(move_group_name);
moveit::planning_interface::MoveGroupInterface gripper_group(“hand”);
move_group.setPlanningTime(planning_time);

// 可选：你可以设置允许重连或规划器等
// move_group.setPlannerId(“RRTConnectkConfigDefault”);

// 设置起点为当前状态
move_group.setStartStateToCurrentState();

  1. // 设置运动速度 (0-1之间)

move_group.setMaxVelocityScalingFactor(0.9);
move_group.setMaxAccelerationScalingFactor(0.9);
gripper_group.setMaxVelocityScalingFactor(0.9);
gripper_group.setMaxAccelerationScalingFactor(0.9);


  1.   // Socket 初始化
  2. boost::asio::io_service io_service;
  3. tcp::acceptor acceptor(io_service, tcp::endpoint(tcp::v4(), 5000)); // 监听 5000 端口
  4. ROS_INFO_STREAM("PyCharm-socket");
  7.     // 等待连接
  8.     tcp::socket socket(io_service);
  9.     acceptor.accept(socket);
  10.     ROS_INFO("PyCharm-connect");
  14. // 主循环控制

while (ros::ok()) {
/* 第一步：移动到home位置并打开夹爪 */
move_group.setNamedTarget(“home”);
moveit::planning_interface::MoveGroupInterface::Plan arm_plan;
if (move_group.plan(arm_plan)) {
move_group.execute(arm_plan);
ROS_INFO(“Moved to HOME position”);
} else {ROS_INFO(“Moved to HOME position ERROR”);}






  1. // 打开夹爪
  2. gripper_group.setNamedTarget("open");
  3. gripper_group.move();
  4. ROS_INFO("Gripper OPENED");
  5. ros::Duration(0.5).sleep();  // 等待动作完成
  9.     // 等待连接
  10.     //tcp::socket socket(io_service);
  11.     //acceptor.accept(socket);
  12.     //ROS_INFO("PyCharm-connect");
  13.     // 读取数据
  14.     boost::asio::streambuf buf;
  15.     boost::asio::read_until(socket, buf, "\n");
  16.     std::istream is(&buf);
  17.     std::string data;
  18.     std::getline(is, data);
  19.     // 解析 X,Y,Z
  20.     std::stringstream ss(data);
  21.     double X, Y, Z;
  22.     char comma;
  23.     ss >> X >> comma >> Y >> comma >> Z;
  24.     ROS_INFO("socketdata: X=%.3f, Y=%.3f, Z=%.3f", X, Y, Z);

// 目标示例：这里演示用 named target 或者外部设置的目标
// 如果用户在参数 server 提供名为 “target_pose” 的目标 pose，可替换下面的逻辑
// 这里我们先检查是否有 named target “home”，否则示例为目标位姿从参数获取（可根据需要修改）

// 下面示例使用当前 pose 加一个小位移作为目标（仅做示范）
//geometry_msgs::PoseStamped current_pose = move_group.getCurrentPose();
//geometry_msgs::Pose target_pose = current_pose.pose;
geometry_msgs::Pose target_pose;
//target_pose.position.z += 0.05; // 向上 5cm（示例）
target_pose.position.x = X;
target_pose.position.y = Y;
target_pose.position.z = Z; 






  1. tf2::Quaternion q;
  2. q.setRPY(M_PI, 0, 0);  // Roll=180°, Pitch=0°, Yaw=0°
  3. target_pose.orientation.x = q.x();
  4. target_pose.orientation.y = q.y();
  5. target_pose.orientation.z = q.z();
  6. target_pose.orientation.w = q.w();

move_group.setPoseTarget(target_pose);

// 记录最佳 plan
moveit::planning_interface::MoveGroupInterface::Plan best_plan;
double best_score = std::numeric_limits::infinity();
bool found_any = false;


for (int i = 0; i < attempts; ++i)
{
moveit::planning_interface::MoveGroupInterface::Plan plan;
move_group.setStartStateToCurrentState();


  1. // 每次尝试你可以随机化 seed 或改变 planner id 来得到不同路径（这里使用默认）
  2. move_group.setPlanningTime(planning_time);
  4. //moveit::planning_interface::MoveItErrorCode success = move_group.plan(plan);
  5. moveit::core::MoveItErrorCode success = move_group.plan(plan);
  7. if (success == moveit::planning_interface::MoveItErrorCode::SUCCESS)
  8. {
  9.   double score = evaluatePathLength(plan.trajectory_);
  10.   ROS_INFO("[attempt %d] path score = %.6f", i + 1, score);
  12.   if (score < best_score)
  13.   {
  14.     best_score = score;
  15.     best_plan = plan;
  16.     found_any = true;
  17.   }
  18. }
  19. else
  20. {
  21.   ROS_WARN("[attempt %d] planning failed", i + 1);
  22. }

}

if (!found_any)
{
ROS_ERROR(“No valid plan found after %d attempts”, attempts);
continue;
//ros::shutdown();
//return 1;
}





ROS_INFO(“Best path score = %.6f”, best_score);

sum++;
total_score+=best_score;
ROS_INFO(“average_score = %.6f”, total_score/sum);

if (best_score > 6)
{
a++;
continue;
}
ROS_INFO(“success: sum = %d and failed: a = %d”, sum , a);




// 可选：对 best_plan 进行时间参数化，得到 RobotTrajectory
// 把 moveit_msgs::RobotTrajectory -> robot_trajectory::RobotTrajectory
robot_model::RobotModelConstPtr kmodel = move_group.getRobotModel();
robot_trajectory::RobotTrajectory rt(kmodel, move_group.getName());
rt.setRobotTrajectoryMsg(*move_group.getCurrentState(), best_plan.trajectory_);



bool tps_ok = timeParameterizeTrajectory(rt);
if (!tps_ok)
ROS_WARN(“Time parameterization failed, proceeding with original trajectory message”);

moveit_msgs::RobotTrajectory out_traj;
rt.getRobotTrajectoryMsg(out_traj);
best_plan.trajectory_ = out_traj; // update

if (execute_best)
{
ROS_INFO(“Executing best plan…”);
move_group.execute(best_plan);


  1. gripper_group.setNamedTarget("close");
  2. //gripper_group.setJointValueTarget(0.0, 0.0);
  3. gripper_group.move();
  4. ROS_INFO("Gripper CLOSED");
  5. ros::Duration(0.5).sleep();  // 等待动作完成
  7. ROS_INFO("Execution done");
  12. move_group.setNamedTarget("workplace");
  13. if (move_group.plan(arm_plan)) {
  14.   move_group.execute(arm_plan);
  15.   ROS_INFO("Moved to WORKPLACE position");
  16. } else {ROS_INFO("Moved to WORKPLACE position ERROR");}
  19. gripper_group.setNamedTarget("open");
  20. gripper_group.move();
  21. ROS_INFO("Gripper OPENED");
  22. ros::Duration(0.5).sleep();
  25. /*** 第三步：返回home位置并打开夹爪 ***/
  26. move_group.setNamedTarget("home");
  27. if (move_group.plan(arm_plan)) {
  28.   move_group.execute(arm_plan);
  29.   ROS_INFO("Returned to HOME position");
  30. } else {ROS_INFO("Returned to HOME position ERROR");}
  32. // 再次打开夹爪
  33. gripper_group.setNamedTarget("open");
  34. gripper_group.move();
  35. ROS_INFO("Gripper OPENED again");
  36. ros::Duration(0.5).sleep();  // 等待动作完成
  38. ROS_INFO("Completed one full cycle");
  39. // 可根据需要添加循环间隔时间
  40. // ros::Duration(2.0).sleep();

}
else
{
ROS_INFO(“execute_best=false, not executing. Best plan available in variable ‘best_plan’.”);
}
}




  1. //===== 关闭 Socket =====
  2. if (acceptor.is_open()) {
  3.     acceptor.close();
  4.     ROS_INFO("Socket 已关闭");
  5. }

ros::shutdown();
return 0;
}

/*
CMakeLists.txt 示例片段（添加到你的 catkin package 的 CMakeLists.txt）：

find_package(catkin REQUIRED COMPONENTS
roscpp
moveit_core
moveit_ros_planning
moveit_ros_planning_interface
moveit_ros_perception
geometry_msgs
)






catkin_package(
CATKIN_DEPENDS roscpp moveit_core moveit_ros_planning moveit_ros_planning_interface geometry_msgs
)

include_directories(${catkin_INCLUDE_DIRS})
add_executable(moveit_best_path_selector src/moveit_best_path_selector.cpp)
target_link_libraries(moveit_best_path_selector ${catkin_LIBRARIES})

package.xml 依赖示例（只列关键部分）：
roscpp
moveit_core
moveit_ros_planning
moveit_ros_planning_interface
geometry_msgs




用法：
rosrun moveit_best_path_selector _move_group:=manipulator _attempts:=8 _planning_time:=3.0 _execute_best:=true

说明：

  • 改动点：
    • 你通常会把目标从参数或 topic 读入（这里为了示例直接在代码里构造了一个小位移目标）
    • 可以在每次 attempt 前调用 move_group.setPlannerId(…) 用不同规划器来获得更多多样化的候选路径
    • 评估函数可以替换成笛卡尔路径长度、总耗时、避障裕度等

*/

转载请注明作者和出处，并添加本页链接。
原文链接: //pongpongkai.top/139

下一篇：没有下一篇了

上一篇：java和gradle版本
皖ICP备2025092356号-1 | 沪公网安备31011202021249号