目录
调用库
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
启动节点
节点名为move_group_interface,运行一次
ros::init(argc, argv, "move_group_interface");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
定义机械臂
关键在于定义MoveGroupInterface(用于控制机器人运动)、JointModelGroup(获取各关节参数)和MoveitVisualTools(在Rviz上显示动画)。
static const std::string PLANNING_GROUP = "mh12_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
目标为位姿
实际上为设定目标为位姿的关节空间轨迹规划。关键点是定义位姿,其中值得注意的是旋转量使用四元数法表示。设置目标的函数为MoveGroupInterface类中的setPoseTarget。计算路径通过plan函数来进行。控制机器人进行运动通过execute函数进行。
// pose goal
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.6;
target_pose1.position.y = 0.6;
target_pose1.position.z = 1.3;
move_group.setPoseTarget(target_pose1);
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
visual_tools.publishAxisLabeled(target_pose1, "pose1");
visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
move_group.execute(my_plan);
目标关节角
实质上为目标为关节角的关节空间轨迹规划。通过之前定义的joint_model_group和current_state来获取当前关节角(一个数组)。和前面类似,通过setJointValueTarget函数来设置关节角目标。
// joint goal
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
joint_group_positions[0] = 1.0; // radians
move_group.setJointValueTarget(joint_group_positions);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
move_group.execute(my_plan);
连续笛卡尔轨迹
关键在于设置一连串的路径点(一个由位姿组成的矢量),位姿的定义和前面一致。还需要设定轨迹,eef_step设置的是轨迹的插值步长。
// cartesian tarjectory
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose target_pose3 = target_pose1;
waypoints.push_back(target_pose3);
target_pose3.position.z += 0.2;
waypoints.push_back(target_pose3); // down
target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3); // right
target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3); // up and left
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
move_group.execute(trajectory);
结束节点
ros::shutdown();
return 0;