D初识-机器人建模

Robocup3D概况,参赛者的任务,用一段代码，来模拟机器人的大脑,Agent实现概况,从server获取消息机器人模型决策向server返回驱动信息,Server的消息,1.消息格式为字符串2.每0.2s为一个周期3.每个周期获得机器人身体信息每3个周期获得一次视觉信息,消息内容,(time(now128.57)(GS(t35.18)(pmPlayOn)(GYR(ntorso)(rt0.00-0.000.00)(ACC(ntorso)(a0.00-0.000.00)(HJ(nhj1)(ax-0.00)(HJ(nhj2)(ax0.00)(See(F1L(pol11.5252.09-8.24)(F1R(pol11.52-52.19-8.24)(B(pol1.89-0.20-58.27)(P(teamFantasia)(id6)(head(pol4.7610.20-19.48)(rlowerarm(pol4.638.00-20.16)(llowerarm(pol4.7913.39-19.25)(rfoot(pol4.3011.87-18.68)(lfoot(pol4.3413.18-18.88)(HJ(nraj1)(ax-0.00)(HJ(nraj2)(ax0.00)(HJ(nraj3)(ax0.00)(HJ(nraj4)(ax-0.00)(HJ(nlaj1)(ax0.00)(HJ(nlaj2)(ax-0.00)(HJ(nlaj3)(ax0.00)(HJ(nlaj4)(ax-0.00)(HJ(nrlj1)(ax-0.00)(HJ(nrlj2)(ax-0.00)(HJ(nrlj3)(ax-0.00)(HJ(nrlj4)(ax-0.00)(HJ(nrlj5)(ax0.00)(HJ(nrlj6)(ax0.00)(HJ(nllj1)(ax0.00)(HJ(nllj2)(ax0.00)(HJ(nllj3)(ax-0.00)(HJ(nllj4)(ax0.00)(HJ(nllj5)(ax0.00)(HJ(nllj6)(ax0.00),消息内容,(time(now128.57)(GS(t35.18)(pmPlayOn）time：server运行时间128.57sGS：比赛开始了35.15spm：比赛模式，PlayOn代表正在比赛,消息内容,(GYR(ntorso)(rt0.50-0.300.10)(ACC(ntorso)(a0.80-3.309.25)GYR：陀螺仪（测机器人饶x、y、z三轴的选择）torso：表示陀螺仪测的是躯干的选择ACC：躯干加速度（加速度不考虑重力，所以静止时，机器人会在z轴上受到9-10左右的加速度）,身体关节信息,身体关节信息,(HJ表示关节信息hj1、hj2头部raj1raj4右臂laj1laj4左臂rlj1rlj6右腿llj1llj6左腿,视觉消息,视觉三个信息：1.与目标连线在水平面投影与正方向夹角2.与目标连线在竖直面投影与水平面夹角3.到目标的距离,在机器人的视觉中，没有坐标的概念,X,Z,Y,水平面,竖直面,视觉消息,(F1R标志物：4个角旗、4个门柱,视觉消息,(P(teamFantasia)(id6)(head(pol4.7610.20-19.48)(rlowerarm(pol4.638.00-20.16)(llowerarm(pol4.7913.39-19.25)(rfoot(pol4.3011.87-18.68)(lfoot(pol4.3413.18-18.88)其他队员信息：队伍名称、队员号码、头和四肢位置,脚部受力,(FRP(nrf)(c0.000.08-0.01)(f0.00-8.931.71)脚部受力：受力点、受力大小（3维力，缺少转动信息）,机器人模型,1.模型建立将机器人等效成N个连杆，每个连杆有一个关节每一个关节是一个坐标系,机器人模型,1.模型建立建立起机器人模型之后，根据server提供的个关节转动角度，就可以递推出身体各个部位相对躯干的坐标（正运动学：坐标系转换等）,机器人模型,2.求解姿态逆运动学根据某些连杆的期望位移，