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