def __init__(self, robot, solver):
self.solver = solver
self.robot = robot
# Push tasks
# Waist
self.robot.comSelec.value = "111"
self.robot.waist.selec.value = "111000"
self.robot.tasks["waist"].controlGain.value = 180.0
self.solver.push(self.robot.tasks["waist"])
# Get height of center of mass
self.robot.com.recompute(0)
com = self.robot.com.value
self.zCom = com[2]
omega = sqrt(gravity / self.zCom)
# Get positions of foot centers
#
ankleWrtFoot = self.robot.dynamic.getAnklePositionInFootFrame()
footCenterWrtAnkle = R3(tuple(map(lambda x: -x, ankleWrtFoot)))
lf = SE3(self.robot.dynamic.signal("left-ankle").value) * footCenterWrtAnkle
# right foot by symmetry
ankleWrtFoot = (ankleWrtFoot[0], -ankleWrtFoot[1], ankleWrtFoot[2])
footCenterWrtAnkle = R3(tuple(map(lambda x: -x, ankleWrtFoot)))
rf = SE3(self.robot.dynamic.signal("right-ankle").value) * footCenterWrtAnkle
# Create and plug stepper entity
#
self.stepper = Stepper("stepper")
self.stepper.setLeftFootCenter(lf.toTuple())
self.stepper.setRightFootCenter(rf.toTuple())
self.stepper.setLeftAnklePosition(self.robot.leftAnkle.position.value)
self.stepper.setRightAnklePosition(self.robot.rightAnkle.position.value)
self.stepper.setCenterOfMass(com)
self.stepper.setFootWidth(0.5 * self.robot.dynamic.getSoleWidth())
self.stepper.setStepHeight(0.08 * self.zCom)
self.stepper.setMaxComGain(200.0)
plug(self.stepper.comGain, self.robot.comTask.controlGain)
plug(self.stepper.comReference, self.robot.comRef)
plug(self.stepper.zmpReference, self.robot.zmpRef)
plug(self.stepper.leftAnkleReference, self.robot.leftAnkle.reference)
plug(self.stepper.rightAnkleReference, self.robot.rightAnkle.reference)
# Trace signals
self.robot.initializeTracer()
self.robot.addTrace("stepper", "leftAnkleReference")
self.robot.addTrace("stepper", "rightAnkleReference")
self.robot.startTracer()
class Motion(object):
signalList = []
def __init__(self, robot, solver):
self.solver = solver
self.robot = robot
# Push tasks
# Waist
self.robot.comSelec.value = "111"
self.robot.waist.selec.value = "111000"
self.robot.tasks["waist"].controlGain.value = 180.0
self.solver.push(self.robot.tasks["waist"])
# Get height of center of mass
self.robot.com.recompute(0)
com = self.robot.com.value
self.zCom = com[2]
omega = sqrt(gravity / self.zCom)
# Get positions of foot centers
#
ankleWrtFoot = self.robot.dynamic.getAnklePositionInFootFrame()
footCenterWrtAnkle = R3(tuple(map(lambda x: -x, ankleWrtFoot)))
lf = SE3(self.robot.dynamic.signal("left-ankle").value) * footCenterWrtAnkle
# right foot by symmetry
ankleWrtFoot = (ankleWrtFoot[0], -ankleWrtFoot[1], ankleWrtFoot[2])
footCenterWrtAnkle = R3(tuple(map(lambda x: -x, ankleWrtFoot)))
rf = SE3(self.robot.dynamic.signal("right-ankle").value) * footCenterWrtAnkle
# Create and plug stepper entity
#
self.stepper = Stepper("stepper")
self.stepper.setLeftFootCenter(lf.toTuple())
self.stepper.setRightFootCenter(rf.toTuple())
self.stepper.setLeftAnklePosition(self.robot.leftAnkle.position.value)
self.stepper.setRightAnklePosition(self.robot.rightAnkle.position.value)
self.stepper.setCenterOfMass(com)
self.stepper.setFootWidth(0.5 * self.robot.dynamic.getSoleWidth())
self.stepper.setStepHeight(0.08 * self.zCom)
self.stepper.setMaxComGain(200.0)
plug(self.stepper.comGain, self.robot.comTask.controlGain)
plug(self.stepper.comReference, self.robot.comRef)
plug(self.stepper.zmpReference, self.robot.zmpRef)
plug(self.stepper.leftAnkleReference, self.robot.leftAnkle.reference)
plug(self.stepper.rightAnkleReference, self.robot.rightAnkle.reference)
# Trace signals
self.robot.initializeTracer()
self.robot.addTrace("stepper", "leftAnkleReference")
self.robot.addTrace("stepper", "rightAnkleReference")
self.robot.startTracer()
def play(self):
totalTime = 25.0
nbSteps = int(totalTime / timeStep) + 1
path = []
n = len(self.signalList)
# Initialize empty list for each task
plot = n * [[]]
for i in xrange(nbSteps):
print (i)
if i == 10:
self.stepper.start()
t = timeStep * i
self.robot.device.increment(timeStep)
config = self.robot.device.state.value
path.append(config)
if hasRobotViewer:
clt.updateElementConfig(rvName, toViewerConfig(config))
for signal, j in zip(self.signalList, range(n)):
plot[j] = plot[j] + [signal(self.robot)]
return (tuple(path), plot)
def draw(self, plot):
import matplotlib.pyplot as pl
fig = []
ax = []
for p, signal in zip(plot, self.signalList):
fig.append(pl.figure())
ax.append(fig[-1].add_subplot(111))
ax[-1].plot(map(lambda x: timeStep * x, xrange(len(p))), p)
leg = ax[-1].legend((signal.name,))
pl.show()
