def test():
pose = [ -30, 0, 0, 0, 0, 0,
-30, 0, 0, 0, 0, 0,
0, 0, 0,
0, 0, 0,
10, -5, 0, -30, 0, 0, 0,
0, 0, 0, 0, 0, 0,
10, 5, 0, -30, 0, 0, 0,
0, 0, 0, 0, 0, 0 ]
seq_svc.setJointAngles(bodyinfo.makeCommandPose(pose), 5.0)
def goHalfSitting():
tm = bodyinfo.timeToHalfsitPose
seq_svc.setJointAngles(bodyinfo.makeCommandPose(bodyinfo.halfsitPose), tm)
#seq_svc.setBasePos(bodyinfo.halfsitBasePos, tm)
#seq_svc.setZmp([0.023, 0, -bodyinfo.halfsitBasePos[2] ], tm)
seq_svc.waitInterpolation()
def goInital():
tm = bodyinfo.timeToInitialPose
seq_svc.setJointAngles(bodyinfo.makeCommandPose(bodyinfo.initialPose), tm)
#seq_svc.setBasePos([0.0, 0.0, bodyinfo.initialWaistHeight], tm)
#seq_svc.setZmp([0.0, 0.0, -bodyinfo.initialWaistHeight], tm)
seq_svc.waitInterpolation()
def setJointAnglesDeg(pose, tm, wait=True):
seq_svc.setJointAngles(bodyinfo.makeCommandPose(pose), tm)
if wait:
seq_svc.waitInterpolation()
def goHalfSitting():
tm = bodyinfo.timeToHalfsitPose
seq_svc.setJointAngles(bodyinfo.makeCommandPose(bodyinfo.halfsitPose), tm)
seq_svc.setBasePos([0.0274, 0.0, 0.7879], tm)
seq_svc.setZmp([0.0274, 0.0, -0.7879], tm)
seq_svc.waitInterpolation()
def setJointAnglesDeg(pose, tm, wait=True):
seq_svc.setJointAngles(bodyinfo.makeCommandPose(pose), tm)
if wait:
seq_svc.waitInterpolation()
