ROS+Kinova Jaco+KinectV2学习总结（持续更新）_void kinova::kinovaposeactionserver::actioncallbac-CSDN博客

本文链接：https://blog.csdn.net/qq_34897331/article/details/100534597

ROS+Kinova Jaco+KinectV2学习总结（持续更新）

ROS
Kinova Jaco2
KinectV2

ROS

launch文件具体怎么配置的，如下情况怎么写进launch文件中。后面的publish_tf：=true 怎么加入到launch文件中。
解决办法：直接找到对应的launch 文件将其参数修改

roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true

手眼标定如何验证？
设想方案，将之前用于标定的Aruco Marker放置在桌上，摄像机识别出其位姿，利用转换四元数将其转到kinova_base坐标下，然后移动机械臂到该位置，目测差距
需要进行的准备有
（1）Aruco Marker在相机坐标系下读取的值如何调取：
可以通过单独启动Kinect2与Aruc_ros包通过调用主题 rostopic echo /aruco_single/pose来得到目标二维码当前的位姿。
（2）如何利用坐标系变换将上述二维码的位姿转化至机械臂坐标系下：
通过求得的位姿转换矩阵进行，详见后面手眼标定结果测定。
（3）如何将四元数输入给机械臂让其运动
参考了冯总给的代码，给定位姿共七个变量的值，让机械臂移动到当前二维码的位姿，判断移动效果

Kinova Jaco2

手动记录一组手眼数据验证：
相机识别的二维码位置姿态：

hand->world translation:
x: -0.191744078725
y: -0.475852502269
z: -0.586699783713
rotation:
x: -0.13003760705
y: -0.721003427285
z: -0.679178408107
w: 0.0442828246318
camera->marker translation:
x: 0.0990315854722
y: -0.0710397396415
z: 0.238137921395
rotation:
x: -0.298145197233
y: -0.540952322232
z: 0.502256112907
w: 0.605160163498
translation:
x: 0.148883197411
y: 0.94183035325
z: 0.44762276537
rotation:
x: 0.0134658202658
y: -0.807156185901
z: 0.583534463855
w: 0.0883464359154
Base-RGB camera的旋转轴为（0.0135 -0.8103 0.5858），旋转角度为169.8638
四元数理解：q=(x,y,z,w)将其转换为一个旋转轴与旋转角则u=（m, n, l）与旋转角a的关系（注意：q^2 = 1.）
四元数的基本数学方程为 : q = cos (a/2) + i(m * sin(a/2)) + j(n * sin(a/2)) + k(l * sin(a/2))
记录下来的样本点是hand->world translation（与base一样），以及camera->marker translation。
From hand---->world tranlation在hand frame 坐标系中沿着x, y, z方向移动，然后在当前坐标系下做四元数旋转？？？？(需要查找资料验证)

坐标系解读

Kinova 与kinect 的坐标系参考：来自论文《基于视觉的工件定位与抓取》，通过知网可以查到。
Kinect2从垂直镜头出去为Z+， IR–>rgb镜头方向为x+
world，root与Kinova_base重合
End effector 与base frame—其中红为x，绿为y
4. 相机，Marker，base，end effector四个坐标系，很明显的可以看到相机与marker坐标系差距很大（其实是没有问题的，以下给出说明）
更新说明//(2019-9-9)
由于marker是抓在end effector上，按理说应该在rgb_camera_frame与end effector frame之间，可是由图中的tf显示marker frame在rgb frame的另一侧了，与常识违背了常识。为此博主郁闷了很长一段时间，以为是什么配置文件没有设好。后来转念一想，手眼标定的目的就是为了求出机械臂基座到rgb摄像头的坐标系变换，而且对于这两个坐标系在tf中预先没有办法设置，所以给出的效果图与实际情况不一定对应，但这对于最终要求出的矩阵来说并没有什么影响。
5. 在仅有marker与kinect相机坐标系下，可以发现rgb与ir只在x轴方向有位移偏差，x的正方向是从ir—>rgb的。~~相机与机械臂底座的相对坐标系设置有误~~
6. ~~二维码坐标系方向，与相机的相对位置相反~~
7. End-effector坐标系,沿着手抓伸出去为Z+，指向单手指的方向为X+
8. Kinectv2内部结构图

Kinova-坐标系转换

从topic 读取的pose值
header:
seq: 21216
stamp:
secs: 1567753705
nsecs: 668286743
frame_id: “j2n6s300_link_base”
pose:
position:
x: -0.219809994102
y: -0.22127571702
z: 0.33824878931
orientation:
x: 0.551666080952
y: 0.098624587059
z: 0.0631555914879
w: 0.825802087784
程序设定的值
x = -0.25;
y = -0.21;
z = 0.37;
qx = 0.47;
qy = 0.087;
qz = 0.039;
qw = 0.877;
x跟z相差有好几厘米
冯总的例子
read from topic
header:
seq: 585
stamp:
secs: 1567754239
nsecs: 72034709
frame_id: “j2n6s300_link_base”
pose:
position:
x: 0.248747527599
y: -0.158704578876
z: 0.530243873596
orientation:
x: 0.636672258377
y: 0.292063385248
z: 0.383489638567
w: 0.601899504662
程序设定
x=0.25;
y=0.41;
z=0.67;
qx=0.47;
qy=0.087;
qz=0.039;
qw=0.877;
y值相差尤其大
[ WARN] [1567754581.081251960]: void kinova::KinovaPoseActionServer::actionCallback(const ArmPoseGoalConstPtr&): LINE 201, Trajectory command failed
rosrun kinova_demo pose_action_client.py -v j2n6s300 mq – 0.25 0.41 0.67 0.47 0.087 0.039 0.877
直接利用上述command 报错

开始认真学习Kinova-Ros Package

Package Link：https://github.com/Kinovarobotics/kinova-ros.
等待去探索的link：https://www.kinovarobotics.com/en/knowledge-hub/all-kinova-products.
[file:///home/ljq/kinova_ws/src/kinova-ros/kinova_docs/kinova_comm/html/index.html](file:///home/ljq/kinova_ws/src/kinova-ros/kinova_docs/kinova_comm/html/index.html).
What Euler convention is used to represents the orientation?
Convention XYZ
绕定轴X-Y-Z旋转（RPY角）等价于绕动轴Z-Y-X旋转（Euler角）
orientation is Euler-ZYX = Rz(ThetaZ)*Ry(ThetaY)*Rx(ThetaX)
What is the reference frame of the robotical arm?
The Cartesian position and orientation returned by the API’s functions are relative to the base reference frame. The origin is at the center and bottom of the base. The Z axis points upward, the X axis points to the left when facing the connectors and the Y axis is toward the connectors (See the DH Parameters document).
Executing multiple Cartesian waypoints without stopping
The action client executes one goal at a time. In case the user wants to give multiple waypoints to the robot without stopping at every waypoint, the service AddPoseToCartesianTrajectories can be used. This service adds the commanded poses to a buffer that that maintained by the robot. The robot executes the poses in this buffer in the order that they are added, without stopping between poses.
The service ClearTrajectories can be used to clear the trajectory buffer in the base.
如何查看ros主题对应的数据类型
rostopic info /j2n6s300_driver/pose_action/tool_pose/goalType: kinova_msgs/ArmPoseActionGoal
ljq@li2jq:~/catkin_ws$ rosmsg show kinova_msgs/ArmPoseActionGoal
std_msgs/Header header
uint32 seq
time stamp
string frame_id
actionlib_msgs/GoalID goal_id
time stamp
string id
kinova_msgs/ArmPoseGoal goal
geometry_msgs/PoseStamped pose
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w

验证手眼标定

easy-hand 启动程序rqt截图
hand->world translation:
x: 0.0358302294152
y: -0.530977618377
z: -0.287131334214
rotation:
x: 0.642899371644
y: 0.317631312816
z: 0.424695250058
w: -0.552652414847
camera->marker translation:
x: -0.255517060315
y: -0.00112922074519
z: 0.617180912075
rotation:
x: -0.326337535394
y: 0.429796512242
z: -0.476412510395
w: 0.694125270392
通过kinect2的topic： rostopic echo /aruco_single/pose
header:
seq: 1247
stamp:
secs: 1567755641
nsecs: 38812030
frame_id: “kinect2_rgb_optical_frame”
pose:
position:
x: -0.253196358681
y: -0.00122881529387
z: 0.610595107079
orientation:
x: -0.32823198181
y: 0.429094700888
z: -0.479805337884
w: 0.691323615628
kinova的当前tool pose
header:
seq: 268
stamp:
secs: 1567755786
nsecs: 109028538
frame_id: “j2n6s300_link_base”
pose:
position:
x: 0.209570646286
y: -0.257534563541
z: 0.505395889282
orientation:
x: 0.642875790596
y: 0.317655742168
z: 0.424677908421
w: 0.552679359913
之前手眼标定转换关系由kinova_base------>camera_frame
translation:
x: 0.148883197411
y: 0.94183035325
z: 0.44762276537
rotation:
x: 0.0134658202658
y: -0.807156185901
z: 0.583534463855
w: 0.0883464359154
由Camera-Marker的旋转矩阵
结果：
代码输入的post:
x=0.4979;
y=0.3934;
z=0.2938;
qx=-0.1531;
qy=-0.3373;
qz=0.6221;
qw=0.6898;
实际运行topic的反馈值
header:
seq: 3720
stamp:
secs: 1567762979
nsecs: 883286480
frame_id: “j2n6s300_link_base”
pose:
position:
x: 0.527681827545
y: 0.348959267139
z: 0.354591667652
orientation:
x: 0.467053562403
y: 0.123147562146
z: 0.607851207256
w: 0.630248010159
手眼标定结果不准，机械臂原因，标定误差，位置误差。

亟须解决问题

程序给予kinova的位姿值，与运动完成后由topic获取的值有偏差，是否需要对机械臂进行校准？
验证机械臂手部校准：
(1) 通过冯总的demo
程序指令位姿
x=0.2979;
y=0.1934;
z=0.3938;
qx=0.1531;
qy=0.3373;
qz=0.6221;
qw=0.6898;
topic读取出来的位姿：
header:
seq: 3484
stamp:
secs: 1568019267
nsecs: 183693428
frame_id: “j2n6s300_link_base”
pose:
position:
x: 0.296284109354
y: -0.0423925444484
z: 0.525291800499
orientation:
x: 0.476095199585
y: 0.343332052231
z: 0.517202734947
w: 0.622862696648
推测可能情况为机械臂并没有到达指定位置，但程序正常运行了，需要添加一个判断，决定机械臂是否运动到指定位置了。
kinect2读取Aruco Marker的位姿是否准确？
大致吻合

二次手眼标定

数据采集样本更多，更稳定。在校准内参时，每一组采了100张照片。在进行手眼标定时采了50个样本。
更新了环境
3. 通过让end effector运动到结算出来的像极坐标系下方，粗略的观察标定结果
4. 手眼标定结果
Robot_base---->camera frame
translation:
x: -0.00276831555534
y: -0.528081440518
z: 1.09819347189
rotation:
x: 0.999739147086
y: -0.00729168962601
z: 0.00791491117064
w: 0.0201450546611
在实际运行中，发现y值可以补偿4cm，即可与相机坐标系更加吻合。实际运行中translation中的y值设定为----- （-0.568081440518）
通过matlab求出旋转矩阵，加上translation得到最终变换矩阵

0.9998	-0.0143	0.0161	-0.002768
-0.0149	-0.9991	0.0402	-0.56808
0.0155	-0.0404	-0.9991	1.09819
0	0	0	1

手眼标定结果验证
只通过topic aruco-pose读取了x,y,z的值，pc = [-0.1621, -0.0631, 0.6406, 1.0]
通过上述转换矩阵（其中的y值加了偏差）可得：pf=[-0.1536, -0.4769, 0.4582, 1.00]实际完成效果图
观察可以发现y值仍有些许偏差，可以通过在旋转矩阵中继续补偿y+

Marker识别的坐标系验证

在这里插入图片描述

结果验证

在这里插入图片描述其中x基本无偏差，z的偏差为+1cm（Zk-Zm），y有+1cm（Yk-Ym）

Cartesian Control

The arm has a maximum reach of about 0.9m (90cm), so the “position” range is about +/-0.9 for all three dimensions. The three wrist joints are capable of continuous rotation, and therefore capable of being commanded up to +/-174.5 rad. You may want to limit to +/-6.28 rad in software to prevent the joints from rotating excessively.