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')