路点规划

路径规划

C++ 实现

string groupName = "manipulator";
moveit::planning_interface::MoveGroupInterface group(groupName);

geometry_msgs::Pose pose;
//位置
pose.position.x = 0.28;
pose.position.y = 0.2;
pose.position.z = 0.5;
//姿态
pose.orientation.x = 0;
pose.orientation.y = 0;
pose.orientation.z = 0;
pose.orientation.w = 1;
group.setPoseTarget(pose);

moveit::planning_interface::MoveGroupInterface::Plan plan;
group.plan(plan);

Python实现

groupName = "manipulator"
commander = moveit_commander.MoveGroupCommander(groupName)

pose = Pose()
# 位置
pose.position.x = 0.3
pose.position.y = 0.2
pose.position.z = 0.3
# 姿态
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1

commander.set_pose_target(pose)
commander.plan()

规划结果判断

C++ 实现

const moveit::planning_interface::MoveItErrorCode &code = group.plan(plan);
if (code == code.SUCCESS) {
    // success
} else if(code == code.PLANNING_FAILED) {
    // failed
}

Python实现

plan = move_group.plan()
if len(plan.joint_trajectory.points) != 0:
    print "success"
else:
    print "failed"

可视化路点

C++ 实现

#include "moveit_visual_tools/moveit_visual_tools.h"
...
//绘制移动轨迹
string frame = group.getPlanningFrame();
moveit_visual_tools::MoveItVisualTools tools(frame);
tools.deleteAllMarkers();

tools.publishAxisLabeled(pose, "target");

const moveit::core::JointModelGroup *jointModelGroup = group.getCurrentState()->getJointModelGroup(groupName);
tools.publishTrajectoryLine(plan.trajectory_, jointModelGroup);
tools.trigger();