环境:Ubuntu20.04 + ros-noetic + moveit1
安装教程:https://blog.csdn.net/qq_43557907/article/details/125081735
上篇:https://blog.csdn.net/qq_43557907/article/details/125124633
本文为我的个人笔记,是翻译的官方手册。因为我个人插入了图片和注释加以补充,所以不提倡转载。
我只是一个小白,文中肯定会有一些错误,欢迎指正。
本篇文章为 Move Group C++ Interface 的下半部分。
9.路径约束的规划 Planning with Path Constraints
在机器人关节上,路径约束可以很容易地指定。让我们为 group 指定一个路径约束和一个目标位姿。第一步,定义路径约束:
// 创建一个方向约束信息
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;
然后,设定其作为 group 的路径约束。
// 创建一个约束信息
moveit_msgs::Constraints test_constraints;
// 将上述方向约束信息添加到此约束信息中
test_constraints.orientation_constraints.push_back(ocm);
// 将此约束信息添加到环境约束中
move_group_interface.setPathConstraints(test_constraints);
10.在关节空间中执行规划
Moveit 可以选择在关节空间或笛卡尔空间来描述规划问题。
在 ompl_planning.yaml 文件中设定 group 的参数enforce_joint_model_state_space:true
,这样就可以在关节空间中执行所有规划。
默认情况下,通过方向路径约束的规划需要在笛卡尔空间中进行采样,以便调用 IK 作为生成式采样器。
使用关节空间的话,规划进程将使用拒绝采样去寻找有效请求,这很有可能增加规划时间。
我们将重复使用之前规划好的目标,工作前提是当前状态已经满足路径约束,所以我们需要将机械臂的起始状态设置到一个新的位姿(见上图)。
// 获得机械臂当前的状态
moveit::core::RobotState start_state(*move_group_interface.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_interface.setStartState(start_state);
现在,我们要从新的起始状态规划到之前创建的目标点
// 设置目标位姿
move_group_interface.setPoseTarget(target_pose1);
带有约束的规划可能会很慢,因为每一个样本都必须请求一个运动学逆解。让我们增加规划时间,从默认的5s到确保有足够的时间去成功规划(这里设置成10s)。
// 设置规划时间
move_group_interface.setPlanningTime(10.0);
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
在 rviz 中可视化规划
visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(start_pose2, "start");
visual_tools.publishAxisLabeled(target_pose1, "goal");
visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");
当路径约束完成后,确保清除它
// 清除路径约束
move_group_interface.clearPathConstraints();
11.笛卡尔路径
通过指定一个末端执行器的路径点列表,你可以直接规划一个笛卡尔路径。我们从上文的新起始状态开始,最初的位姿(起始状态)不需要去添加到路径点列表中,但是添加的话有助于可视化。
// 创建一个 vector 容器,用来储存路径点
std::vector<geometry_msgs::Pose> waypoints;
waypoints.push_back(start_pose2);
geometry_msgs::Pose target_pose3 = start_pose2;
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
我们想将笛卡尔路径设置为 1cm 的分辨率,所以指定0.01作为笛卡尔变换中的最大步幅。我们设置跳跃阈值 jump threshold 为 0.0,代表不允许跳跃。
关于jump threshold:https://blog.csdn.net/Bobby95/article/details/118663350
规划的关节连续运动之间的距离<=Jump_threshold*关节位置的平均距离/绝对距离
用于在笛卡尔空间运动时, 避免在经过奇异点时产生关节的抖动 。
警告:当操作真实的硬件时禁用 jump threshold 会引起较大不可预测的冗余关节的动作,成为一个安全问题。
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0; // 跳跃阈值
const double eef_step = 0.01; // 终端步进值
// 规划路径 ,fraction返回1代表规划成功
double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);
在 rviz 中可视化规划
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");
笛卡尔动作通常是缓慢的。例如当接近对象时,笛卡尔规划的速度不能顺利地通过 maxVelocityScalingFactor 设置,但是需要你手动计算轨迹的时间,正如 这里 所描述的。
你可以像这样实现一个轨迹:
move_group_interface.execute(trajectory);
12.向环境中添加一个物体
首先,让我们在无物体的情况下规划到另一个简单的目标。
move_group_interface.setStartState(*move_group_interface.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.x = 1.0;
another_pose.position.x = 0.7;
another_pose.position.y = 0.0;
another_pose.position.z = 0.59;
move_group_interface.setPoseTarget(another_pose);
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");
现在,让我们定义一个让机器人去避开的碰撞对象信息。
moveit_msgs::CollisionObject collision_object;
// 这个对象参考机器人的参考坐标系,即世界坐标系
collision_object.header.frame_id = move_group_interface.getPlanningFrame();
设置碰撞对象的 id
collision_object.id = "box1";
定义一个形状 box,并将其添加到 world 中
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.1;
primitive.dimensions[primitive.BOX_Y] = 1.5;
primitive.dimensions[primitive.BOX_Z] = 0.5;
定义 box 的位姿(相对于 frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.5;
box_pose.position.y = 0.0;
box_pose.position.z = 0.25;
// 将形状 box 及其位姿添加到碰撞对象中
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
// 创建一个碰撞对象集,并添加上述碰撞对象
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
现在,让我们添加这个碰撞对象集到 world 中
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
在 rviz 中展示状态文本,等待 MoveGroup 接受和处理碰撞对象信息。
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
现在,当我们规划一个轨迹时,它将会避开障碍物
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED");
visual_tools.publishText(text_pose, "Obstacle 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 once the plan is complete");
13.为机器人链接对象
你可以为机械臂链接一个对象,使其随着机械臂几何学而移动。这个仿真拾起了待操作的对象,其动作规划应该避免两个对象间的碰撞。
moveit_msgs::CollisionObject object_to_attach;
object_to_attach.id = "cylinder1";
shape_msgs::SolidPrimitive cylinder_primitive;
cylinder_primitive.type = primitive.CYLINDER;
cylinder_primitive.dimensions.resize(2);
cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20;
cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;
我们为这个圆柱定义坐标系/位姿,使其出现在夹具中。
object_to_attach.header.frame_id = move_group_interface.getEndEffectorLink();
geometry_msgs::Pose grab_pose;
grab_pose.orientation.w = 1.0;
grab_pose.position.z = 0.2;
首先,我们添加对象到 world(不用vector)
object_to_attach.primitives.push_back(cylinder_primitive);
object_to_attach.primitive_poses.push_back(grab_pose);
object_to_attach.operation = object_to_attach.ADD;
// 注意,添加单个对象时,这里是 apply 而不是 add
planning_scene_interface.applyCollisionObject(object_to_attach);
然后,我们链接物体到机械臂上,它使用 frame_id 去决定哪个关节是要链接的。你可以使用 applyAttachedCollisionObject 去直接链接一个物体到机械臂上。
ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
move_group_interface.attachObject(object_to_attach.id, "panda_hand");
visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot");
重新规划轨迹,现在机械爪中有物体了。
move_group_interface.setStartStateToCurrentState();
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");
14.分离并移除物体
现在,让我们使圆柱从机械臂夹具上分开。
ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
move_group_interface.detachObject(object_to_attach.id);
在 rviz 中展示状态信息
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot");
现在,让我们从 world 中移除物体
ROS_INFO_NAMED("tutorial", "Remove the objects from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
object_ids.push_back(object_to_attach.id);
planning_scene_interface.removeCollisionObjects(object_ids);
在 rviz 中展示状态信息
visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disappears");
15.launch 文件
完整的 launch 文件在 here 。教程中的所有代码都可以通过 moveit_tutorials 包来运行,前提是你已经安装好 Moveit。
<launch>
<node name="move_group_interface_tutorial" pkg="moveit_tutorials" type="move_group_interface_tutorial" respawn="false" output="screen">
</node>
</launch>