move_group C++
功能描述
move_group是moveit中最简单方便的用户接口,提供大多数操作功能:设置关节、末端目标位置姿态,运动规划,控制机器人运动,环境中添加物体以及机器人上增加删除物体。节点:MoveGroup Node
C++官方代码
move_group_interface_tutorial.cpp
代码框架简介
五种轨迹规划的方法:
- 笛卡尔空间轨迹规划
- 关节空间轨迹规划
- 带有约束的轨迹规划
- 笛卡尔空间路径规划computeCartesianPath
- 增加物体及避障轨迹规划
其他功能:
- 获取机器人当前状态
- 键盘远程控制
- Rviz轨迹可视化
- 机器人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