ROS双臂定点抓取问题记录

平台：ROS-Melodic+MoveIt！
编程语言：C++
机械臂模型：KINOVA-j2s6s200

双臂抓取基本思路分主臂从臂，先后规划并到达各自抓取点，然后对从臂末端添加相对于主臂末端的运动学约束，通过求解逆运动学并规划添加路径点形成轨迹实现双臂同时运动。

问题一

主臂抓取点位姿问题

最开始的时候没有添加运动学约束，只是通过添加两个机械臂的路径点和C++的API生成轨迹，然后取出轨迹中规划好的路径点给非链式的双臂整体movegroup循环赋关节值，实现双臂同时运动。主臂抓取点设置的语段如下：

 std::vector<moveit_msgs::Grasp> grasps;grasps.resize(1);grasps[0].grasp_pose.header.frame_id = "root";tf2::Quaternion orientation;orientation.setRPY(0, -M_PI / 2, M_PI / 2);grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);grasps[0].grasp_pose.pose.position.x = 0.6;grasps[0].grasp_pose.pose.position.y = -0.3;grasps[0].grasp_pose.pose.position.z = 0.35;

在我直接给路径点赋值的时候这段运行的很顺利，但是在我尝试加入运动学以后这个抓取点在终端一直提示没有规划出来的解决方案，无法执行。如图：

以为是目标物体挡住了主臂的规划路线，但是去掉以后仍然无法规划。图中从臂规划顺利，主从臂姿态对称，从臂姿态：

orientation1.setRPY(0, M_PI / 2, M_PI / 2);

我把不能运行的工作空间删了又把之前可以运行的工作空间拷回来重新catkin，但是仍然规划错误，主臂没有动作，奇怪的是今天之前这个抓取点一直是可达的，抓取也没有问题，同样的程序同样的配置今天却跑不通了…

————————————————————————————————————
解决：今天打开代码的时候灵光乍现，改掉了抓取函数中的参数：

void pick(moveit::planning_interface::MoveGroupInterface& move_group)
{std::vector<moveit_msgs::Grasp> grasps;grasps.resize(1);grasps[0].grasp_pose.header.frame_id = "root";tf2::Quaternion orientation;orientation.setRPY(0, M_PI / 2, -M_PI / 2);grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);grasps[0].grasp_pose.pose.position.x = 0.6;grasps[0].grasp_pose.pose.position.y = -0.3;grasps[0].grasp_pose.pose.position.z = 0.35;grasps[0].pre_grasp_approach.direction.header.frame_id = "root";grasps[0].pre_grasp_approach.direction.vector.y = 0.001;//这里本来是y = 0;grasps[0].pre_grasp_approach.min_distance = 0.005;grasps[0].pre_grasp_approach.desired_distance = 0.015;grasps[0].post_grasp_retreat.direction.header.frame_id = "root";grasps[0].post_grasp_retreat.direction.vector.y = 0.001;//同样y = 0;grasps[0].post_grasp_retreat.min_distance = 0.005;grasps[0].post_grasp_retreat.desired_distance = 0.015;openGripper1(grasps[0].pre_grasp_posture);openGripper1(grasps[0].grasp_posture);move_group.pick("cylinder", grasps);
}

两个vector决定了末端的抓取和退出方向，当y = 0的时候相当于没有设定抓取和退出方向，导致规划器求解一直失败。

这段函数因为一开始不会用pick函数，所以从大佬那里kiao来的，实现的功能应该是机械臂末端设置抓取姿态，通过末端的微小移动抓取到物体再撤回到抓取姿态。改一下参数就可以实现末端没有进退的直接夹爪闭合，也就是末端直接运动到夹爪闭合的姿态，然后将物体attach到主臂末端，等待从臂规划。实现效果图：

问题二

双臂抓取物体后同时抬起

刚接触moveit的时候思路是给从臂末端添加一个相对于主臂的动态约束，但是后来发现这玩意基本上是行不通的，只能另找办法…可能有一些更加涉及底层的东西可以实现这个思路，但是现阶段能想到的办法只有同时控制两个机械臂的各个关节，实现两臂的同时运动（之前师姐给我建议用ros的message filter通过回调函数同时调用两个topic，然后保持接收时间戳同步，奈何搞了好几天也没搞明白回调函数的两个传参在进行机械臂控制的时候到底咋用，就放弃了）。

在moveit配置的时候除去夹爪的规划组定义了三个，group1主臂，group2从臂，body是不加基座的所有joint（非链式结构，只是添加了joint，也就是说body这个组规划的时候不能通过逆运动学求解器来计算状态）：

moveit::planning_interface::MoveGroupInterface group1("group1");
moveit::planning_interface::MoveGroupInterface group2("group2");
moveit::planning_interface::MoveGroupInterface body("body");

在主从臂分别到达抓取点以后，以抓取位姿为起点，到目标位姿为终点，先利用moveit的正运动学计算两条单臂规划组group1和group2末端轨迹：

moveit_msgs::RobotTrajectory trajectory1;
std::string end_effector_link1 = group1.getEndEffectorLink();
group1.setPoseReferenceFrame("root");
geometry_msgs::Pose start_pose1 = group1.getCurrentPose(end_effector_link1).pose;
std::vector<geometry_msgs::Pose> waypoints1;waypoints1.push_back(start_pose1);
start_pose1.position.z = 0.6;waypoints1.push_back(start_pose1);
start_pose1.position.x = 0.5;waypoints1.push_back(start_pose1);
start_pose1.position.y = -0.15;waypoints1.push_back(start_pose1);
group1.computeCartesianPath(waypoints1, eef_step, jump_threshold, trajectory1);//这句计算出到达目标点的路径，储存到trajectory1中；

这样用cout可以输出trajectory1中的数据：各个关节角的角度、速度和加速度。其中关节角的值就是重点，通过循环语句将每个规划点的所有关节值提取出来给规划组body中的关节角赋值，并且让body再循环中持续运动到达目标点：

std::vector<double> target_pose_state(12);//这个数组里面一共存12个关节值，顺序为主臂的1-6再从臂的1-6；
int i, j, k;
for(i = 1; i <= 3; i++)//i的最终大小取决于规划出来的路径点数量；
{for(j = 0, k = 6; j < 6, k < 12; j++, k++){target_pose_state[j] = trajectory1.joint_trajectory.points[i].positions[j];target_pose_state[k] = trajectory2.joint_trajectory.points[i].positions[j];}body.setJointValueTarget(target_pose_state);body.move();
}

moveit中非链式结构的机械臂没法执行轨迹，所以只能用这种循环让机械臂连续移动，而且在rviz里面这个循环里的所有轨迹点都是通过一条规划轨迹运动的，可以说等价于轨迹运动。

trajectory中的轨迹点数量不固定，决定于规划路径的长度和每步步长限制，也就是computeCartesianPath()语句里中间那两个变量。如果双臂目标位置和起始位置距离过远导致机械臂有夸张的运动轨迹从而脱离抓取物，这时就要考虑在轨迹点中插入中间点，让两条臂规划的路线尽可能保持一致。

从臂轨迹规划点原理上是主臂轨迹点末端的位姿通过平移转换得到的（抓取物为刚性物体，所以两个臂的末端夹爪不会有相对位姿的改变，可以只考虑两个臂的位姿），可以通过对主臂的运动学建模得到从臂末端位姿，然后再求解逆运动学得到从臂关节值，在相应的关节空间控制步骤时赋值给body规划组的后六个关节，就能实现从臂规划点根据主臂规划点的位姿进行规划。

抬起时的路径较为简单时，比如仅仅在xyz轴上移动，通过路径点添加实现关节空间的运动就足够了，原因还是上面说的抓取刚性物体两臂末端相对位姿不变，所以仅在插入路径点时保持两臂末端固定方向的距离即可。

ROS双臂定点抓取问题记录相关推荐

基于机器视觉的ROS机械臂抓取实验
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨曾三来源丨混沌无形点击进入->3D视觉工坊学习交流群摘要:为了减少机械臂在产品分类 ...
机械臂论文笔记（一）【基于卷积神经网络的二指机械手抓取姿态生成研究】
基于卷积神经网络的二指机械手抓取姿态生成研究论文下载摘要第1章绪论 1.1 抓取生成国内外研究现状 1.1.1已知物体抓取生成 1.1.2相似物体抓取生成 1.1.3 未知物体抓取生成 1. ...
Linux 日志抓取
抓取日志,记录下工作中用到过的命令 cat 命令抓取日志开头的前五个 cat error.log | head -n 5 抓取日志末尾的后五个 cat error.log | tail -n 5 h ...
机器人抓取（二）—— ROS 控制 onrobot RG2 / RG6 机械手（第二代）
机械臂为UR5 CB3 .机械手onrobot第二代rg6夹抓.上位机为Jetson AGX Xavier ubuntu 18.04. 接上篇:机器人抓取(一)-- ROS MoveIt! 程序控制真 ...
通过ROS控制真实机械臂(15) --- 视觉抓取之手眼标定
通过视觉传感器赋予机械臂"眼睛"的功能,配合ATI力和力矩传感器,就可以完成机械臂"手眼"结合的能力,完成视觉抓取过程.目前测试的视觉传感器为 ZED mini ...
ROS下使用dobot越疆科技的M1-B1机器人进行定点抓取代码
ROS下使用越疆科技的M1-B1机器人进行定点抓取代码 #include <ros/ros.h> #include "ros/console.h" #include & ...
用Python抓取某东购买记录并统计MM的bra大小
来源 | 大数据前沿作者 | 二胖并不胖二胖最近在逛京东的时候偶然发现:MM们购买bra的记录上竟然留下了尺寸和颜色等信息,我当时就想,要不要抓点数据下来看看啊? 然后就有了这篇文章~~~ Let ...
ROS笔记（37）抓取和放置
ROS笔记(37) 抓取和放置 1. pick and place 2. 启动机械臂 3. 创建抓取的目标物体 4. 设置目标物体的放置位置 4. 设置抓取姿态 5. pick 6. place 7. ...
python爬虫之scrapy初试与抓取链家成交房产记录
接上一篇文章,本机安装好python之后和scrapy之后,我们开始学习使用scrapy创建爬虫程序. 今天先来点简单的,不那么复杂,先看看抓取链家网里面的房价信息. 首先使用CMD命令行进入F盘创建 ...

ROS双臂定点抓取问题记录

问题一

问题二

ROS双臂定点抓取问题记录相关推荐

最新文章

热门文章