一.Asti机器人
1.下肢结构
猜测：
足底按照两条轨迹线运动，继而实现向前走。
2.读代码
function sysCall_init()
//句柄
asti=sim.getObjectHandle("Asti")//整个机器人asti
lFoot=sim.getObjectHandle("leftFootTarget")
rFoot=sim.getObjectHandle("rightFootTarget")//target dummy假体
lPath=sim.getObjectHandle("leftFootPath")
rPath=sim.getObjectHandle("rightFootPath")//导入的路径
lPathLength=sim.getPathLength(lPath)
rPathLength=sim.getPathLength(rPath)//路径长度 length of the path given in meters
ui=simGetUIHandle("astiUserInterface")
simSetUIButtonLabel(ui,0,sim.getObjectName(asti).." user interface")
dist=0
correction=0.0305
minVal={0, -- Step size
0, -- Walking speed
-math.pi/2, -- Neck 1
-math.pi/8, -- Neck 2
-math.pi/2, -- Left shoulder 1
0, -- Left shoulder 2
-math.pi/2, -- Left forearm
-math.pi/2, -- Right shoulder 1
0, -- Right shoulder 2
-math.pi/2} -- Right forearm
rangeVal={ 2, -- Step size
0.8, -- Walking speed
math.pi, -- Neck 1
math.pi/4, -- Neck 2
math.pi/2, -- Left shoulder 1
math.pi/2, -- Left shoulder 2
math.pi/2, -- Left forearm
math.pi/2, -- Right shoulder 1
math.pi/2, -- Right shoulder 2
math.pi/2} -- Right forearm
uiSliderIDs={3,4,5,6,7,8,9,10,11,12} // 见图1
relativeStepSize=1
nominalVelocity=0.4
neckJoints={sim.getObjectHandle("neckJoint0"),sim.getObjectHandle("neckJoint1")}//头部关节
leftArmJoints={sim.getObjectHandle("leftArmJoint0"),sim.getObjectHandle("leftArmJoint1"),sim.getObjectHandle("leftArmJoint2")}
rightArmJoints={sim.getObjectHandle("rightArmJoint0"),sim.getObjectHandle("rightArmJoint1"),sim.getObjectHandle("rightArmJoint2")}
//左右肢关节
-- Now apply current values to the user interface:
simSetUISlider(ui,uiSliderIDs[1],(relativeStepSize-minVal[1])*1000/rangeVal[1])
simSetUISlider(ui,uiSliderIDs[2],(nominalVelocity-minVal[2])*1000/rangeVal[2])
simSetUISlider(ui,uiSliderIDs[3],(sim.getJointPosition(neckJoints[1])-minVal[3])*1000/rangeVal[3])
simSetUISlider(ui,uiSliderIDs[4],(sim.getJointPosition(neckJoints[2])-minVal[4])*1000/rangeVal[4])
simSetUISlider(ui,uiSliderIDs[5],(sim.getJointPosition(leftArmJoints[1])-minVal[5])*1000/rangeVal[5])
simSetUISlider(ui,uiSliderIDs[6],(sim.getJointPosition(leftArmJoints[2])-minVal[6])*1000/rangeVal[6])
simSetUISlider(ui,uiSliderIDs[7],(sim.getJointPosition(leftArmJoints[3])-minVal[7])*1000/rangeVal[7])
simSetUISlider(ui,uiSliderIDs[8],(sim.getJointPosition(rightArmJoints[1])-minVal[8])*1000/rangeVal[8])
simSetUISlider(ui,uiSliderIDs[9],(sim.getJointPosition(rightArmJoints[2])-minVal[9])*1000/rangeVal[9])
simSetUISlider(ui,uiSliderIDs[10],(sim.getJointPosition(rightArmJoints[3])-minVal[10])*1000/rangeVal[10])
//第三项都是步长，是滑动UI时的最小变化量
end
function sysCall_cleanup()
end
function sysCall_actuation()
-- Read desired values from the user interface:
relativeStepSize=minVal[1]+simGetUISlider(ui,uiSliderIDs[1])*rangeVal[1]/1000
nominalVelocity=minVal[2]+simGetUISlider(ui,uiSliderIDs[2])*rangeVal[2]/1000
sim.setJointTargetPosition(neckJoints[1],minVal[3]+simGetUISlider(ui,uiSliderIDs[3])*rangeVal[3]/1000)
sim.setJointTargetPosition(neckJoints[2],minVal[4]+simGetUISlider(ui,uiSliderIDs[4])*rangeVal[4]/1000)
sim.setJointTargetPosition(leftArmJoints[1],minVal[5]+simGetUISlider(ui,uiSliderIDs[5])*rangeVal[5]/1000)
sim.setJointTargetPosition(leftArmJoints[2],minVal[6]+simGetUISlider(ui,uiSliderIDs[6])*rangeVal[6]/1000)
sim.setJointTargetPosition(leftArmJoints[3],minVal[7]+simGetUISlider(ui,uiSliderIDs[7])*rangeVal[7]/1000)
sim.setJointTargetPosition(rightArmJoints[1],minVal[8]+simGetUISlider(ui,uiSliderIDs[8])*rangeVal[8]/1000)
sim.setJointTargetPosition(rightArmJoints[2],minVal[9]+simGetUISlider(ui,uiSliderIDs[9])*rangeVal[9]/1000)
sim.setJointTargetPosition(rightArmJoints[3],minVal[10]+simGetUISlider(ui,uiSliderIDs[10])*rangeVal[10]/1000)
//根据UI的滑动转动关节
//重点
-- Get the desired position and orientation of each foot from the paths (you can also use a table of values for that):
//从路径中获取每只脚所需的位置和方向
t=sim.getSimulationTimeStep()*nominalVelocity //仿真时间*速度0.4=前进的距离
dist=dist+t//移动的距离累加
lPos=sim.getPositionOnPath(lPath,dist/lPathLength)//函数功能：检索沿路径对象的点的绝对插值位置 第二个参数是轨迹上的相对距离
lOr=sim.getOrientationOnPath(lPath,dist/lPathLength)//函数功能：检索沿路径对象的点的绝对插值方向
p=sim.getPathPosition(rPath)
rPos=sim.getPositionOnPath(rPath,(dist+correction)/rPathLength)
rOr=sim.getOrientationOnPath(rPath,(dist+correction)/rPathLength) //右边的轨迹
-- Now we have the desired absolute position and orientation for each foot.
//通过sim.getPositionOnPath和sim.getOrientationOnPath有了每个脚想要到达的绝对位姿
-- Now transform the absolute position/orientation to position/orientation relative to asimo
//把绝对位姿转换成相对于asimo的位姿
-- Then modulate the movement forward/backward with the desired "step size"
//按理想的步长向前向后调整
-- Then transform back into absolute position/orientation:
//然后转换回绝对位置/方向 ？？？
astiM=sim.getObjectMatrix(asti,-1)//检索asti相对于世界坐标系的转换矩阵，见下图2
astiMInverse=simGetInvertedMatrix(astiM) //求逆矩阵
m=sim.multiplyMatrices(astiMInverse,sim.buildMatrix(lPos,lOr))//第一个函数是矩阵相乘，第二个函数根据坐标和欧拉角生成转移矩阵
m[8]=m[8]*relativeStepSize
m=sim.multiplyMatrices(astiM,m)
lPos={m[4],m[8],m[12]}
lOr=sim.getEulerAnglesFromMatrix(m)
m=sim.multiplyMatrices(astiMInverse,sim.buildMatrix(rPos,rOr))
m[8]=m[8]*relativeStepSize //按理想的步长调整
m=sim.multiplyMatrices(astiM,m)//转换回来
rPos={m[4],m[8],m[12]}
rOr=sim.getEulerAnglesFromMatrix(m)//从矩阵中得到欧拉角
-- Finally apply the desired positions/orientations to each foot
-- We simply apply them to two dummy objects that are then handled
//改变假体位姿
-- by the IK module to automatically calculate all leg joint desired values
-- Since the leg joints operate in hybrid mode, the IK calculation results
-- are then automatically applied as the desired values during dynamics calculation
//自动调用IK模块求解
sim.setObjectPosition(lFoot,-1,lPos)
sim.setObjectOrientation(lFoot,-1,lOr)
//按求出的位姿设置target假体的位姿
sim.setObjectPosition(rFoot,-1,rPos)
sim.setObjectOrientation(rFoot,-1,rOr)
end
图1
通过UI可以调节机器人移动速度和上半身的关节转动
图2
3.具体思路
UI控制上肢部分不阐述，具体说说下肢的行走部分。