dobot moveit 包_DOBOT magician魔术师在ROS下使用moveit编写代码控制（笛卡尔空间控制走直线）...

C++ 版

#include #include #include int main(int argc, char **argv)

{

ros::init(argc, argv, "magician_moveit_cartesian_demo");

ros::AsyncSpinner spinner(1);

spinner.start();

moveit::planning_interface::MoveGroupInterface arm("magician_arm");

//获取终端link的名称

std::string end_effector_link = arm.getEndEffectorLink();

//设置目标位置所使用的参考坐标系

std::string reference_frame = "magician_origin";

arm.setPoseReferenceFrame(reference_frame);

//当运动规划失败后，允许重新规划

arm.allowReplanning(true);

//设置位置(单位：米)和姿态(单位：弧度)的允许误差

arm.setGoalPositionTolerance(0.001);

arm.setGoalOrientationTolerance(0.01);

//设置允许的最大速度和加速度

arm.setMaxAccelerationScalingFactor(0.2);

arm.setMaxVelocityScalingFactor(0.2);

// 控制机械臂先回到初始化位置

arm.setNamedTarget("home");

arm.move();

sleep(1);

// 获取当前位姿数据最为机械臂运动的起始位姿

geometry_msgs::Pose start_pose = arm.getCurrentPose(end_effector_link).pose;

std::vectorwaypoints;

//将初始位姿加入路点列表

waypoints.push_back(start_pose);

start_pose.position.z -= 0.093;

waypoints.push_back(start_pose);

start_pose.position.x += 0.04;

waypoints.push_back(start_pose);

start_pose.position.y += 0.04;

waypoints.push_back(start_pose);

start_pose.position.x -= 0.04;

start_pose.position.y -= 0.04;

waypoints.push_back(start_pose);

// 笛卡尔空间下的路径规划

moveit_msgs::RobotTrajectory trajectory;

const double jump_threshold = 0.0;

const double eef_step = 0.005;

double fraction = 0.0;

int maxtries = 100; //最大尝试规划次数

int attempts = 0; //已经尝试规划次数

while(fraction < 1.0 && attempts < maxtries)

{

fraction = arm.computeCartesianPath(waypoints,