V-rep Hexapod tutorial（2）
1.添加用户参数User Parameters
(1)给hexa_body添加Non-threaded script
(2)hexa_body的脚本代码中插如如下代码并运行
function sysCall_init()
Base=sim.getObjectHandle('hexa_body')
sim.setUserParameter(Base,'@enable','')
end
（3）点击运行
运行后在hexa_body获得如下图标，双击可打开用户参数对话框
（4）添加用户参数
| Name | Value | Unit |
|---|---|---|
| stepVelocity | 0.5 | 1/s |
| stepAmplitude | 0.16 | m |
| stepHeight | 0.04 | m |
| movementDirection | 0 | degrees |
| rotationMode | 0 | -1: CCW, 1: CW, 0: no rotation |
| movementStrength | 0 | 0-1 |
2.打开动力学
3.设置参数
（1）设置六条腿的第二个和第三个关节的位置为0，让六个脚能搭在地面上
4.代码理解
（1）hexapod的脚本
线程子脚本，只运行一次，运动前机身的“热身动作”
//这个线程脚本（threaded）是六足动物的主控制脚本。它产生身体运动并进行控制
//另一个非线程脚本（non-threaded），负责生成行走运动
//另一个脚本是通过仿真参数控制的：这个脚本写入参数（sim.setScriptSimulationParamete）
//并且另一个脚本读取参数(sim.getScriptSimulationParameter)
//----------------------------函数setStepMode：设置用户参数里的六个值-----------------------//
setStepMode=function(stepVelocity,stepAmplitude,stepHeight,movementDirection,rotationMode,movementStrength)
sim.setScriptSimulationParameter(sim.handle_tree,'stepVelocity',stepVelocity)
sim.setScriptSimulationParameter(sim.handle_tree,'stepAmplitude',stepAmplitude)
sim.setScriptSimulationParameter(sim.handle_tree,'stepHeight',stepHeight)
sim.setScriptSimulationParameter(sim.handle_tree,'movementDirection',movementDirection)
sim.setScriptSimulationParameter(sim.handle_tree,'rotationMode',rotationMode)
sim.setScriptSimulationParameter(sim.handle_tree,'movementStrength',movementStrength)
end
//-------------------------函数moveBody：让机身在原地运动（各个方向旋转）----------------------//
moveBody=function(index)
//获得当前的目标位置和角度（3个方向）
local p={initialP[1],initialP[2],initialP[3]}
local o={initialO[1],initialO[2],initialO[3]}
//把腿部和机身移动到给定的位置和方向（实际在第一次运行时该行代码不动）
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
//输入的索引为0：腿部不动，机身抬起h后下落h
if (index==0) then
//机身抬起，速度加倍
p[3]=p[3]-0.03*sizeFactor
sim.moveToPosition(legBase,antBase,p,o,vel*2,accel)
//机身下落，速度加倍
p[3]=p[3]+0.03*sizeFactor
sim.moveToPosition(legBase,antBase,p,o,vel*2,accel)
end
//输入的索引为1：4次弯曲【twisting】
if (index==1) then
o[1]=o[1]+5*math.pi/180
o[2]=o[2]-05*math.pi/180
o[3]=o[3]-15*math.pi/180
p[1]=p[1]-0.03*sizeFactor
p[2]=p[2]+0.015*sizeFactor
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
o[1]=o[1]-10*math.pi/180
o[3]=o[3]+30*math.pi/180
p[2]=p[2]-0.04*sizeFactor
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
o[1]=o[1]+10*math.pi/180
o[2]=o[2]+10*math.pi/180
p[2]=p[2]+0.03*sizeFactor
p[1]=p[1]+0.06*sizeFactor
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
o[1]=o[1]-10*math.pi/180
o[3]=o[3]-30*math.pi/180
p[2]=p[2]-0.03*sizeFactor
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
end
//输入的索引为2：绕着x轴旋转三次：17，-34，17【rolling】
if (index==2) then
p[3]=p[3]-0.0*sizeFactor
o[1]=o[1]+17*math.pi/180
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
o[1]=o[1]-34*math.pi/180
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
o[1]=o[1]+17*math.pi/180
p[3]=p[3]+0.0*sizeFactor
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
end
//输入的索引为3：绕着y轴旋转三次：15，-30，15【pitching】
if (index==3) then
-- pitching
p[3]=p[3]-0.0*sizeFactor
o[2]=o[2]+15*math.pi/180
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
o[2]=o[2]-30*math.pi/180
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
o[2]=o[2]+15*math.pi/180
p[3]=p[3]+0.0*sizeFactor
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
end
//输入的索引为4：绕着z轴旋转三次：30，-60，30【yawing】
if (index==4) then
p[3]=p[3]+0.0*sizeFactor
o[3]=o[3]+30*math.pi/180
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
o[3]=o[3]-60*math.pi/180
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
o[3]=o[3]+30*math.pi/180
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
p[3]=p[3]-0.0*sizeFactor
sim.moveToPosition(legBase,antBase,p,o,vel,accel)
end
end
//---------------------------------------线程子脚本的主要函数----------------------------------//
function sysCall_threadmain()
//获得机身的目标Dummy
antBase=sim.getObjectHandle('hexa_base')
//获得足部的目标Dummy，hexa_legBase下包含六条腿的目标Dummy
legBase=sim.getObjectHandle('hexa_legBase')
//检索场景对象的大小因子。尺寸因子与真实物体的大小不同。使用此功能可以对伸缩操作做出反应。
sizeFactor=sim.getObjectSizeFactor(antBase)
//物体移动的速度
vel=0.05
//物体移动的角速度
accel=0.05
//物体移动的目标位置targetPosition
initialP={0,0,0}
//物体移动的目标角度targetOrientation
initialO={0,0,0}
//设置物体初始先腿不动，机身向上移动，因此只改变targetPosition的z分量（注意，此时sizeFactor为1，initialP[3]为负，机身向上移动，[1]和[2]都是正负和x、y轴的正方形对应）
initialP[3]=initialP[3]-0.03*sizeFactor
//将对象移动到给定的位置和方向
sim.moveToPosition(legBase,antBase,initialP,initialO,vel,accel)
stepHeight=0.02*sizeFactor
maxWalkingStepSize=0.11*sizeFactor
walkingVel=0.9
//原地移动旋转
//设置六个参数：步速(1/s)、步幅(m)、步距(m)、运动方向(degrees)、转动模式(-1: CCW, 1: CW, 0: no rotation)、运动强度(0-1)
setStepMode(walkingVel,maxWalkingStepSize,stepHeight,0,0,0)
moveBody(0)
moveBody(1)
moveBody(2)
moveBody(3)
moveBody(4)
//保持身体固定的姿势向前走
setStepMode(walkingVel,maxWalkingStepSize,stepHeight,0,0,1)
//线程等待
sim.wait(12)
for i=1,27,1 do
setStepMode(walkingVel,maxWalkingStepSize,stepHeight,10*i,0,1)
sim.wait(0.5)
end
//停止
setStepMode(walkingVel,maxWalkingStepSize,stepHeight,270,0,0)
sim.wait(2)
//向前走，同时改变身体姿势
setStepMode(walkingVel,maxWalkingStepSize*0.5,stepHeight,0,0,1)
moveBody(0)
moveBody(1)
moveBody(2)
moveBody(3)
moveBody(4)
//停止
setStepMode(walkingVel,maxWalkingStepSize*0.5,stepHeight,0,0,0)
sim.wait(2)
//原地旋转
setStepMode(walkingVel,maxWalkingStepSize*0.5,stepHeight,0,1,1)
sim.wait(24)
-- Stop:
setStepMode(walkingVel,maxWalkingStepSize*0.5,stepHeight,0,0,0)
end
（2）hexa_body的脚本
非线程子脚本，这个脚本负责移动腿来行走
本质上，是通过每次调用线程时，赋予p不同的位置，再赋予给不同的腿来实现
//----------------------------------非线程子脚本的初始化函数----------------------------------//
function sysCall_init()
//获得六条腿的目标Dummy的中心
antBase=sim.getObjectHandle('hexa_legBase')
//获得六条腿的尖端和目标Dummy
legTips={-1,-1,-1,-1,-1,-1}
legTargets={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
legTips[i]=sim.getObjectHandle('hexa_footTip'..i-1)
legTargets[i]=sim.getObjectHandle('hexa_footTarget'..i-1)
end
//获取六条腿的尖端的位置（相对于平面中心）
//sim.getObjectPosition(objectHandle,relativeToObjectHandle)
//参数objectHandle：对象的句柄
//参数relativeToObjectHandle：表示相对于我们想要的参考框架的位置。
//指定-1来获取绝对位置，指定sim.handle_parent来获取相对于对象父体的位置，或者指定相对于我们想要的参考框架的对象句柄。
//如果这个句柄是一个关节的句柄，那么将返回相对于该关节移动框架的位置（除非objectHandle与sim.handleflag_reljointbaseframe相结合，在这种情况下，将返回相对于该关节基本框架的位置）。
initialPos={nil,nil,nil,nil,nil,nil}
for i=1,6,1 do
initialPos[i]=sim.getObjectPosition(legTips[i],antBase)
end
//腿部移动顺序
legMovementIndex={1,4,2,6,3,5}
//六个用户参数
stepVelocity=0.5
stepAmplitude=0.16
stepHeight=0.04
movementDirection=0*math.pi/180
rotation=0
movementStrength=1
stepProgression=0
realMovementStrength=0
end
//-----------------------------------非线程子脚本的恢复函数-----------------------------------//
function sysCall_cleanup()
end
//-----------------------------非线程子脚本的主要函数：驱动函数-----------------------------//
function sysCall_actuation()
//检索仿真时间步长（在每个主脚本仿真过程中所经过的仿真时间（即不是实时的））
//这个值对于一个特定的模拟来说可能不是恒定的。
dt=sim.getSimulationTimeStep()
//获得六个用户参数
stepVelocity=sim.getScriptSimulationParameter(sim.handle_self,'stepVelocity')
stepAmplitude=sim.getScriptSimulationParameter(sim.handle_self,'stepAmplitude')
stepHeight=sim.getScriptSimulationParameter(sim.handle_self,'stepHeight')
movementDirection=math.pi*sim.getScriptSimulationParameter(sim.handle_self,'movementDirection')/180
rotation=sim.getScriptSimulationParameter(sim.handle_self,'rotationMode')
movementStrength=sim.getScriptSimulationParameter(sim.handle_self,'movementStrength')
//dt为0.05，dx为目标movementStrength和实际realMovementStrength的差
dx=movementStrength-realMovementStrength
//如果差值大于0.05，dx=dt*0.5=0.025
if (math.abs(dx)>dt*0.1) then
dx=math.abs(dx)*dt*0.5/dx
end
//更新实际realMovementStrength
realMovementStrength=realMovementStrength+dx
//对每条腿执行如下操作
for leg=1,6,1 do
sp=(stepProgression+(legMovementIndex[leg]-1)/6) % 1
offset={0,0,0}
if (sp<(1/3)) then
offset[1]=sp*3*stepAmplitude/2
else
if (sp<(1/3+1/6)) then
s=sp-1/3
offset[1]=stepAmplitude/2-stepAmplitude*s*6/2
offset[3]=s*6*stepHeight
else
if (sp<(2/3)) then
s=sp-1/3-1/6
offset[1]=-stepAmplitude*s*6/2
offset[3]=(1-s*6)*stepHeight
else
s=sp-2/3
offset[1]=-stepAmplitude*(1-s*3)/2
end
end
end
md=movementDirection+math.abs(rotation)*math.atan2(initialPos[leg][1]*rotation,-initialPos[leg][2]*rotation)
offset2={offset[1]*math.cos(md)*realMovementStrength,offset[1]*math.sin(md)*realMovementStrength,offset[3]*realMovementStrength}
p={initialPos[leg][1]+offset2[1],initialPos[leg][2]+offset2[2],initialPos[leg][3]+offset2[3]}
//我们只是设定了想要的脚的位置。IK在那之后被隐式地处理(在默认的主脚本中)。你也可以用sim.handleIkGroup()来显式处理IK。
sim.setObjectPosition(legTargets[leg],antBase,p)
end
stepProgression=stepProgression+dt*stepVelocity
end