IRSO 抹茶冷饮制作

Kinect2与Kinova像素距离转换关系

2019.10.31平面九点法标定

位置 （相对于机械臂）	像素坐标	机械臂距离
右下	（485，64）	（-0.463351628，−0.09442）
中下	（，）	（，）
左下	（1358，66）	（0.45851351628304，−0.11092）
右中	（491，333）	（-0.468351628304，−0.374890134）
中中	（，）	（，）
左中	（1352，334）	（0.451351628304 ，−0.389890134）
右中上	（709，454）	（-0.240351628，−0.507634）
中上	（922，595）	（-0.0121432899982，−0.661076488）
左中上	（1136，454）	（0.220351628，-0.5140134）

2019.10.24平面九点法标定

由于之前标定精度不够，故需要二次标定，这次又9个Mark点

位置 （相对于机械臂）	像素坐标	机械臂距离
右下	（482.5，110.5）	（-0.468351628304，-0.09742）
中下	（，）	（，）
左下	（1332，109.5）	（0.45851351628304，-0.10842）
右中	（489.5，373.5）	（-0.468351628304，-0.377390134096）
中中	（，）	（，）
左中	（1334.5，368）	（0.451351628304 ，-0.391390134096）
右中上	（705，489.5）	（-0.240351628，-0.510134）
中上	（918，628.5）	（-0.0121432899982，-0.663576488495）
左中上	（1127，485.5）	（0.220351628，-0.5140134）

2019.10.20）

1. 标志点数据列表

位置 （相对于机械臂）	坐标	距离格子	推算机械臂距离	实测距离
右下	（415，40）	（-21，-2）	（-0.525，-0.05）	（-0.54，-0.045）
中上	（923， 582）	(0， -25)	（0，-0.625）	（-0.013，-0.635）
左下	（1408， 37 ）	(21，-2)	（0.525， -0.05）	（0.535，-0.06）

△u=993；
△v=543.5；
△x=1.075；
△y=0.5825
du=0.001082578；
dv=0.001071757；
设u₀=415; v₀=38.5;
x₀=-0.54; y₀=-0.045;
区域内一个像素坐标（u,v）；
转换成机械臂下的坐标（x，y）；
x = (u-u₀)*du+x₀;
y= -(v-v₀)*dv+y₀;

进展（2019.10.19）

1. 遇到夹抓开合不稳定情况，当放置饮料杯时，略微放开手抓再抬升，手抓直接开成了最大程度。

关键参数记录

比赛场地

1. 桌子尺寸6ft x 2.5ft x 2.5ft = 1.8288m x 0.762 m x 0.762m Your Robots cannot be on the tables

物件关键位置记录（2019.10.22）

1. TO-GO cup 放置位置:
   header:
   seq: 1130
   stamp:
   secs: 1571717305
   nsecs: 585065260
   frame_id: “j2n6s300_link_base”
   pose:
   position:
   x: -0.443049013615
   y: -0.0938969403505
   z: 0.325840324163
   orientation:
   x: -0.999988794327
   y: 0.00225111353211
   z: -0.00390216475353
   w: 0.00145495776087
2. Bottle 放置位置
   header:
   seq: 6240
   stamp:
   secs: 1571717822
   nsecs: 681883298
   frame_id: “j2n6s300_link_base”
   pose:
   position:
   x: -0.409439235926
   y: 0.0469396002591
   z: 0.139264851809
   orientation:
   x: 0.999981462955
   y: -0.00446957815439
   z: 0.00410881917924
   w: 0.000427188759204
3. Bottle cap 放置位置
   header:
   seq: 15858
   stamp:
   secs: 1571718804
   nsecs: 165162656
   frame_id: “j2n6s300_link_base”
   pose:
   position:
   x: -0.415940225124
   y: -0.269461452961
   z: 0.0239246990532
   orientation:
   x: -0.99996227026
   y: 0.00524091580883
   z: -0.00589505815879
   w: 0.00362820364535

(2019.10.18)

1. 手指最大开合程度
   const int FINGER_MAX1 = 6714;
   const int FINGER_MAX2 = 6924;
   const int FINGER_MAX3 = 7332;

2. Usb Camera照相时机械臂位姿
   position:
   x: -0.00991799216717
   y: -0.319072604179
   z: 0.403790742159
   orientation:
   x: 0.993079960346
   y: -0.0494419597089
   z: 0.10155480355
   w: 0.0321599170566

3. Kinect2照相时，机械臂位姿：
   header:
   seq: 189
   stamp:
   secs: 1571623890
   nsecs: 104486412
   frame_id: “j2n6s300_link_base”
   pose:
   position:
   x: 0.265314340591
   y: -0.166935577989
   z: 0.507371723652
   orientation:
   x: 0.585240662098
   y: 0.417867124081
   z: 0.506171524525
   w: 0.476099491119

新摄像头500W像素手眼关系演算（比赛现场还要更新）

1. 机械臂照相姿态：
   header:
   seq: 23635
   stamp:
   secs: 1571227205
   nsecs: 297072355
   frame_id: “j2n6s300_link_base”
   pose:
   position:
   x: -0.00991799216717
   y: -0.319072604179
   z: 0.403790742159
   orientation:
   x: 0.993079960346
   y: -0.0494419597089
   z: 0.10155480355
   w: 0.0321599170566
2. 采集了四个Mark点
   

位置	坐标	机械臂坐标	距离格子	推算机械臂距离	实测距离
右下	（591, 433）	（0,0）	（-14， -7）	（-0.35，-0.175）	（-0.3575，-0.175）
右上	（590, 33）	(0, 0)	（-14，-25）	（-0.35， -0.625）	（-0.3575，-0.619）
左上	（54, 31）	(0, 0)	（10，,-25）	（0.25， -0.625）	（0.25, -0.619）
左下	（56, 437）	(0, 0)	（10， -7）	（0.25， -0.175）	（0.25, -0.175）

△u=535.5；
△v=403;
△x=0.6075；
△y=0.444；
du=0.00113445；
dv=0.00110174；
设u₀=54; v₀=31;
x₀=0.25; y₀=-0.619;
区域内一个像素坐标（u,v）；
转换成机械臂下的坐标（x，y）；
x = -(u-u₀)*du+x₀;
y= (v-v₀)*dv+y₀;

开瓶盖

1. 手抓预备关闭状态：
   finger1: 3576.0
   finger2: 3582.0
   finger3: 3576.0
2. 下移夹住瓶盖并且旋转
   header:
   seq: 4774
   stamp:
   secs: 1571062609
   nsecs: 703936495
   frame_id: “j2n6s300_link_base”
   pose:
   position:
   x: -0.0573112219572
   y: -0.455660253763
   z: 0.213353410363
   orientation:
   x: 0.735923290253
   y: 0.67551201582
   z: 0.00148253329098
   w: 0.0458080917597

FInger Position
finger1: 5316.0
finger2: 5412.0
finger3: 5412.0
3. 旋转1/4圈后的位姿
header:
seq: 7065
stamp:
secs: 1571150228
nsecs: 645508015
frame_id: “j2n6s300_link_base”
pose:
position:
x: -0.0559672564268
y: -0.453526318073
z: 0.211268812418
orientation:
x: 0.0446137972176
y: 0.998079180717
z: -0.025137944147
w: 0.0348644703627

5. 瓶盖放置位置
   header:
   seq: 7479
   stamp:
   secs: 1571062880
   nsecs: 204447066
   frame_id: “j2n6s300_link_base”
   pose:
   position:
   x: 0.152331009507
   y: -0.457500725985
   z: 0.00700328592211
   orientation:
   x: -0.601398289204
   y: 0.797637403011
   z: -0.0142836142331
   w: 0.0434819534421

倒水倾角测试

1. Tool 绕X轴旋转180度，对应的四元数与rpy
   X: 0.00221690163016
   Y: -0.302506834269
   Z: 0.333475261927
   ThetaX: -3.14104294777
   ThetaY: -0.000418400333729
   ThetaZ: 0.000312260322971
   rostopic echo /j2n6s300_driver/out/cartesian_command
   ljq@li2jq:~$ rostopic echo /j2n6s300_driver/out/tool_pose
   header:
   seq: 19921
   stamp:
   secs: 1571184899
   nsecs: 985040864
   frame_id: “j2n6s300_link_base”
   pose:
   position:
   x: 0.00261627603322
   y: -0.300975441933
   z: 0.330840319395
   orientation:
   x: -0.999991059303
   y: 0.00030416061054
   z: -0.00241345423274
   w: 0.00345951109193

   诡异的位姿状态

   header:
   seq: 33390
   stamp:
   secs: 1571209034
   nsecs: 35809427
   frame_id: “j2n6s300_link_base”
   pose:
   position:
   x: -0.0971814766526
   y: -0.303213894367
   z: 0.301522493362
   orientation:
   x: 0.555734038353
   y: 0.486517608166
   z: 0.449639379978
   w: 0.502279579639
   对应的关节角
   joint1: 321.962799072
   joint2: 220.609481812
   joint3: 81.4867172241
   joint4: -110.819030762
   joint5: 148.863769531
   joint6: 88.2923278809
   joint7: 0.0
   欧拉角
   X: -0.097666233778
   Y: -0.302841901779
   Z: 0.303407788277
   ThetaX: 2.22453641891
   ThetaY: 1.41612005234
   ThetaZ: -0.650523483753