move_group C++ 轨迹规划

move_group C++

功能描述

move_group是moveit中最简单方便的用户接口，提供大多数操作功能：设置关节、末端目标位置姿态，运动规划，控制机器人运动，环境中添加物体以及机器人上增加删除物体。节点：MoveGroup Node

---

C++官方代码

move_group_interface_tutorial.cpp

代码框架简介

五种轨迹规划的方法：

1. 笛卡尔空间轨迹规划
2. 关节空间轨迹规划
3. 带有约束的轨迹规划
4. 笛卡尔空间路径规划computeCartesianPath
5. 增加物体及避障轨迹规划

其他功能：

1. 获取机器人当前状态
2. 键盘远程控制
3. Rviz轨迹可视化
4. 机器人attached/detached物体

---

代码详细笔记

设置规划组获取基本状态

//moveit的操作对象的move_group的一组关节，所以首先设置planing_group(JointModelGroup)
static const std::string PLANNING_GROUP = "panda_arm";
static const std::string PLANNING_GROUP = "manioulator";//UR的规划组
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
//设置一个指向规划组的全局指针joint_model_group，后面会经常用到。
const robot_state::JointModelGroup* joint_model_group =
    move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
//获取基本状态
ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

笛卡尔目标点规划

//Pose格式的末端目标位置姿态。
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
//move_group.setPoseTarget
move_group.setPoseTarget(target_pose1);

//所有规划方法设置完target之后的plan和move都相同
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
//move_group.plan
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group.move();

关节空间目标点规划

//创建一个包含所有机器人状态的指针（位置、速度、加速度）
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
//获取机器人当前关节位置
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
//第一个关节运动-1rad
joint_group_positions[0] = -1.0;  // radians
//move_group.setJointValueTarget
move_group.setJointValueTarget(joint_group_positions);

move_group.getCurrentState()
move_group.getCurrentState() 包含所有机器人状态的指针，例程中调用的三个成员函数分别是：move_group.getCurrentState()->getJointModelGroup(“manipulator”); 用于返回当前规划组；move_group.getCurrentState()->copyJointGroupPositions(joint_model_group, joint_group_positions); 用于返回当前关节位置。
start_state.setFromIK(joint_model_group, start_pose2);用于逆运动学解算。
其他的成员函数还包括正运动学getGlobalLinkTransform，逆运动学setFromIK，获取雅各比矩阵getJacobian，设定关节位置setJointGroupPositions，关节限制enforceBounds等等。

加路径约束的笛卡尔目标点规划

//添加姿态约束，也可以添加位置约束，姿态约束的意思就是保持规划过程中保持姿态不变
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "panda_link7";
ocm.header.frame_id = "panda_link0";
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);
//路径规划的起始点也需要满足约束，所以在设置完约束之后，任意设置一个新点作为起始点并通过逆运动学使其满足约束
robot_state::RobotState start_state(*move_group.getCurrentState());
geometry_msgs::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
start_state.setFromIK(joint_model_group, start_pose2);
move_group.setStartState(start_state);//之前都没有设过，默认是当前状态作为起始点
move_group.setPoseTarget(target_pose1);
//约束规划因为每次需要逆运动学求解，所以将默认解算时间5s延长到10s
move_group.setPlanningTime(10.0);
//规划完成之后清除约束和起始点（把当前状态作为起始点）
move_group.clearPathConstraints();
move_group.setStartStateToCurrentState();

笛卡尔多点路径规划

//建立一个waypoints列表，这里push_back添加，规划出平滑轨迹，将当前位置作为第一个点
geometry_msgs::Pose target_pose3 = move_group.getCurrentPose().pose;

std::vector<geometry_msgs::Pose> waypoints;
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

//每个关节最大速度比例因子限制关节速度
move_group.setMaxVelocityScalingFactor(0.1);

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);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

笛卡尔多点路径规划的相关参数
eef_step：步长，分辨率；
jump_threshold机器人配置空间距离阈值，防止逆运动学求解的跳跃，设为0以disable，实际操作中可能会有安全问题。
fraction：得分即航点规划完成比例。返回介于0.0到1.0之间的值，该值表示按航路点描述所达到的路径比例。 出现错误时返回-1.0。

设置轨迹Target的API：

• move_group.setPoseTarget
• move_group.setJointValueTarget
• move_group.computeCartesianPath