class Solver:
def __init__(self, robot):
self.robot = robot
# Make sure control does not exceed joint limits.
self.jointLimitator = JointLimitator('joint_limitator')
plug(self.robot.dynamic.position, self.jointLimitator.joint)
plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
# Create the solver.
self.sot = SolverKine('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setSize(self.robot.dimension)
# Plug the solver control into the filter.
plug(self.sot.control, self.jointLimitator.controlIN)
# Important: always use 'jointLimitator.control'
# and NOT 'sot.control'!
if robot.device:
plug(self.jointLimitator.control, robot.device.control)
def push(self, task):
"""
Proxy method to push a task (not a MetaTask) in the sot
"""
self.sot.push(task.name)
if task.name != "taskposture" and "taskposture" in self.toList():
self.sot.down("taskposture")
def rm(self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.rm(task.name)
def pop(self):
"""
Proxy method to remove the last (usually posture) task from the sot
"""
self.sot.pop()
def __str__(self):
return self.sot.display()
def toList(self):
"""
Creates the list of the tasks in the sot
"""
return map(lambda x: x[1:-1], self.sot.dispStack().split('|')[1:])
def clear(self):
"""
Proxy method to remove all tasks from the sot
"""
self.sot.clear()
class Solver:
def __init__(self, robot):
self.robot = robot
# Make sure control does not exceed joint limits.
self.jointLimitator = JointLimitator("joint_limitator")
plug(self.robot.dynamic.position, self.jointLimitator.joint)
plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
# Create the solver.
self.sot = SolverKine("solver")
self.sot.signal("damping").value = 1e-6
self.sot.setSize(self.robot.dimension)
# Plug the solver control into the filter.
plug(self.sot.control, self.jointLimitator.controlIN)
# Important: always use 'jointLimitator.control'
# and NOT 'sot.control'!
if robot.device:
plug(self.jointLimitator.control, robot.device.control)
def push(self, task):
"""
Proxy method to push a task (not a MetaTask) in the sot
"""
self.sot.push(task.name)
if task.name != "taskposture" and "taskposture" in self.toList():
self.sot.down("taskposture")
def rm(self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.rm(task.name)
def pop(self):
"""
Proxy method to remove the last (usually posture) task from the sot
"""
self.sot.pop()
def __str__(self):
return self.sot.display()
def toList(self):
"""
Creates the list of the tasks in the sot
"""
return map(lambda x: x[1:-1], self.sot.dispStack().split("|")[1:])
def clear(self):
"""
Proxy method to remove all tasks from the sot
"""
self.sot.clear()
class Solver:
def __init__(self, robot):
self.robot = robot
"""
# Make sure control does not exceed joint limits.
self.jointLimitator = JointLimitator('joint_limitator')
plug(self.robot.dynamic.position, self.jointLimitator.joint)
plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
"""
# Create the solver.
self.sot = SolverKine('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setSize(self.robot.dimension)
# Set second order computation
self.robot.device.setSecondOrderIntegration()
self.sot.setSecondOrderKinematics()
plug(self.robot.device.velocity,self.robot.dynamic.velocity)
plug(self.robot.dynamic.velocity,self.sot.velocity)
# Plug the solver control into the robot.
plug(self.sot.control, robot.device.control)
def push (self, task):
"""
Proxy method to push a task (not a MetaTask) in the sot
"""
self.sot.push (task.name)
if task.name!="taskposture" and "taskposture" in self.toList():
self.sot.down("taskposture")
def rm (self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.rm (task.name)
def pop (self):
"""
Proxy method to remove the last (usually posture) task from the sot
"""
self.sot.pop ()
def __str__ (self):
return self.sot.display ()
def toList(self):
"""
Creates the list of the tasks in the sot
"""
return map(lambda x: x[1:-1],self.sot.dispStack().split('|')[1:])
def clear(self):
"""
Proxy method to remove all tasks from the sot
"""
self.sot.clear ()
tr.add(taskRF.featureDes.name + '.position', 'refr')
tr.add(taskLF.featureDes.name + '.position', 'refl')
tr.add('dyn.rf', 'r')
tr.add('dyn.lf', 'l')
tr.add('featureComDes.errorIN', 'comref')
tr.add('dyn.com', 'com')
tr.add(taskWaist.gain.name + '.gain', 'gainwaist')
history = History(dyn, 1)
# --- RUN -----------------------------------------------------------------
featurePosture.selec.value = toFlags(range(6, 36))
sot.clear()
for task in [taskWaist, taskRF, taskLF]:
task.feature.position.recompute(0)
task.feature.keep()
task.feature.selec.value = '111111'
sot.push(task.task.name)
taskWaist.ref = matrixToTuple(eye(4))
taskWaist.feature.selec.value = '111011'
taskWaist.gain.setByPoint(18, 0.1, 0.005, 0.8)
attime(200, (lambda: sot.rm(taskWaist.task.name), 'Rm waist'),
(lambda: pg.initState(), 'Init PG'),
(lambda: pg.parseCmd(":stepseq " + seqpart), 'Push step list'),
(lambda: sot.push(taskCom.name), 'Push COM'),
(lambda: plug(pg.rightfootref, taskRF.featureDes.position), 'Plug RF'),
tr.add(taskRF.featureDes.name+'.position','refr')
tr.add(taskLF.featureDes.name+'.position','refl')
tr.add('dyn.rf','r')
tr.add('dyn.lf','l')
tr.add('featureComDes.errorIN','comref')
tr.add('dyn.com','com')
tr.add(taskWaist.gain.name+'.gain','gainwaist')
history = History(dyn,1)
# --- RUN -----------------------------------------------------------------
featurePosture.selec.value = toFlags( range(6,36) )
sot.clear()
for task in [taskWaist, taskRF, taskLF]:
task.feature.position.recompute(0)
task.feature.keep()
task.feature.selec.value = '111111'
sot.push(task.task.name)
taskWaist.ref = matrixToTuple(eye(4))
taskWaist.feature.selec.value = '111011'
taskWaist.gain.setByPoint(18,0.1,0.005,0.8)
attime(200
,(lambda : sot.rm(taskWaist.task.name),'Rm waist')
,(lambda : pg.initState(),'Init PG')
,(lambda : pg.parseCmd(":stepseq " + seqpart),'Push step list')
,(lambda : sot.push(taskCom.name),'Push COM')
