Moveit搭建机械臂仿真平台记录

官网tutorials学习过程

找到了官网的教程，教程链接：http://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/getting_started/getting_started.html

下面记录过程

前置工作

安装moveit

sudo apt install ros-melodic-moveit

git clone

先创建一个工作空间，在src目录下

git clone https://github.com/ros-planning/moveit_tutorials.git -b melodic-devel
git clone https://github.com/ros-planning/panda_moveit_config.git -b melodic-devel

编译工作空间

首先安装相关依赖，在src下

rosdep install -y --from-paths . --ignore-src --rosdistro melodic

然后执行

catkin config --extend /opt/ros/${ROS_DISTRO} --cmake-args -DCMAKE_BUILD_TYPE=Release

这时候报了一个错

sudo apt-get install ros-melodic-catkin python-catkin-tools

问题解决，运行结果如下所示

出现了三个【missing】，先忽略

接着catkin build

出现了一个【warning】先忽略

source一下，至此我们完成了前置工作！

---

运行一个demo并且配置参数

roslaunch panda_moveit_config demo.launch rviz_tutorial:=true

在rviz中add一个Motionplanning之后就可以看到机械臂模型了

随后呢，在rviz中进行下面几项配置

1.在 Displays/Global Options/Fixed Frame 选择 panda_link0

2.在 Displays/Motionplanning/Robot Description 选择 robot_description

3.在 Displays/Motionplanning/Planning Scene Topic 选择 /planning_scene

4.在 Displays/Motionplanning/Planning Request/planning Group 选择 panda_arm

5.在 Displays/Motionplanning/Planned Path/Trajectory Topic 选择/move_group/display_planned_path

如果只关心机器人，以及它的起始状态的话，按照下面配置参数

1.勾选 Planned Path 中的 Show Robot Visual

2.取消勾选 Scene Robot 中的 Show Robot Visual

3.勾选 Planning Request 中的 Query Goal State & Query Start State
可以在rivz中保存这些配置

直接在rviz中拖拽机器臂，green 是 初始， orange 是目标

点击plan,就可以看到机械臂的运行。

Move group的C++接口

准备

MoveIt操作一组称为“规划组”的关节，并将它们存储在一个名为JointModelGroup的对象中。在整个MoveIt中，“规划组”和“联合模型组”是可以互换使用的。

static const std::string PLANNING_GROUP = "panda_arm";

使用PLANNING_GROUP这个参数， 初始化一个 MoveGroupinterface 类

moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

使用 PlanningSceneInterface类 在“虚拟世界”场景中添加和删除碰撞对象

moveit::planing_interface::PlanningSceneInterface planning_scene_interface;

原始指针经常用于引用计划组以提高性能。

const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

可视化

MoveItVisualTools包提供了许多用于在RViz中可视化对象、机器人和轨迹的功能，以及调试工具，比如脚本的逐步内省

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();

远程控制是一种自省工具，允许用户通过RViz中的按钮和键盘快捷键逐步执行高级脚本

visual_tools.loadRemoteControl();

RViz提供了多种类型的标记，在本演示中将使用文本、圆柱体和球体

Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); //变换矩阵的初始化
text_pose.translation().z()=1.75;//设置Z值
visual_tools.punlisheText(text_pose,"MoveGroupInterface Demo",rvt::WHITE,rvt::XLARGE);
//在rviz中显示文本

批发布用于减少发送到RViz进行大型可视化的消息数量

visual_tools.trigger();

获取基本的信息

我们可以打印出这个机器人的参考系的名字。

We can also print the name of the end-effector link for this group.

也可以打印机器人末端执行器link的名字

ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

我们可以得到含有机器人所有组的表

ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
          std::ostream_iterator(std::cout, ", ")); //迭代输出

开始demo

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");//在终端中输出提示信息

规划到一个目标位姿

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(target_pose1);

call planner 去计算和可视化这个pose，仅仅是计划，并没有让 move_group 真正的去移动机器人

moveit::planning_interface::MoveGroupInterface::Plan my_plan;//创建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");//是否计算成功，输出相关信息

可视化plans

ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");//输出信息
visual_tools.publishAxisLabeled(target_pose1, "pose1");//在rviz中输出一些标签信息
visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);//显示轨迹

移动到一个目标位姿

移动的操作和以上的步骤相似，但我们需要使用 move() 这个函数，这是个阻塞函数，他需要控制器激活，并且汇报轨迹执行的成功。

规划一个关节空间目标

moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
//RobotState是包含所有当前位置/速度/加速度数据的对象

获得group的当前关节值

std::vector joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group,joint_group_positions);

修改其中一个关节，规划到新的关节空间，并且可视化；

joint_group_position[0] = -1.0;//关节值是-1，需要确定这个是什么单位？
move_group.setJointValueTarget(joint_group_positions);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");

在rviz中可视化

visual_tools.deleteAllMarker();
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");

有路径约束的规划

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.setPathConstraints(test_constraints);

再次使用旧的目标，我们的初始状态也需要满足约束，指定初始状态到一个新的pose

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);//根据pose调用逆解器去逆解关节角
move_group.setStartState(start_state);

设置目标位姿，带有约束的规划是很慢的，因为每个样本都要调用逆解器，因此增加规划时间（default 5s）

move_group.setPoseTarget(target_pose1);
move_group.setPlanningTime(10.0);
success = (move_group.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.clearPathConstraints();//结束的时候，一定要清楚路径约束

笛卡尔路径

通过指定末端执行器要经过的路径点列表，可以直接规划笛卡尔路径，注意，我们是从上面的新开始状态开始的。初始姿势(开始状态)不需要添加到路径点列表，但添加它可以帮助可视化。

std::vector waypoints;
waypoints.push_back(start_pose2);
geometry::Pose target_pose3 = start_pose2;
target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3);
target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3); 
target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3);

笛卡尔运动通常需要在接近和后退的抓取动作等动作中变慢,改变每个关节的最大速度的比例因子

move_group.setMaxVelocityScalingFactor(0.1);

步长1cm插值笛卡尔路径，并且禁用了跳转阈值，在操作真正的硬件时禁用跳跃阈值可能会导致冗余关节出现不可预测的大型运动，并可能是一个安全问题。

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%% achieved)", fraction * 100.0);

rviz中的可视化

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", 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");

添加/移除对象和挂载/卸载对象

定义一个碰撞对象ROS消息

moveit_msgs::CollisionObjection collisition_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
collision_objection.id = "box1";//起个名字
//定义这个盒子，并且添加到世界中
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);//长宽高
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;
//相对 frame_id 为这个盒子指定一个坐标
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4;
box_pose.position.y = -0.2;
box_pose.position.z = 1.0;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

std::vector collision_objects;
collision_objects.push_back(collision_object);

ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
//在rviz中可视化

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 to once the collision object attaches to the "
                    "robot");

设置一个路径，避开这个障碍

move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");

在rviz中可视化

visual_tools.deleteAllMarkers();
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("next step");

加载这个物体到机器人上

ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
move_group.attachObject(collision_object.id);
//在rviz中的可视化
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 to once the collision object attaches to the "
                    "robot");

卸载

ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
move_group.detachObject(collision_object.id);


visual_tools.publishText(text_pose, "Object dettached 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 to once the collision object detaches to the "
                    "robot");

移走物体；

ROS_INFO_NAMED("tutorial", "Remove the object from the world");
std::vector object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);

visual_tools.publishText(text_pose, "Object 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");

ROS系统MoveIt玩转双臂机器人系列（一）--ROS机器人建模 - Shawn0102 - 博客园

gazebo+moveit_实习生雷云斐_20200221_哔哩哔哩_bilibili