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 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)
"""
# Plug the solver control into the robot.
plug(self.sot.control, robot.device.control)
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)
robot.control.unplug()
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
return map(lambda x: x[1:-1], sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control, robot.control)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH = MetaTaskKine6d('rh', dyn, 'rh', 'right-wrist')
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.07, -0.02, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH = MetaTaskKine6d('lh', dyn, 'lh', 'left-wrist')
print("Number of dofs (6 for the freeflyer + num of joints):")
print robot.dimension
# FIXME: this must be set so that the graph can be evaluated.
robot.device.zmp.value = (0., 0., 0.)
# Create a solver.
from dynamic_graph.sot.dyninv import SolverKine
def toList(solver):
return map(lambda x: x[1:-1], solver.dispStack().split('|')[1:])
SolverKine.toList = toList
solver = SolverKine('sot')
solver.setSize(robot.dimension)
# set a constant damping to 0.1
solver.damping.value = 0.1
robot.device.control.unplug()
plug(solver.control, robot.device.control)
# Close the control loop
plug(robot.device.state, robot.dynamic.position)
print("...done!")
# Make sure only robot and solver are visible from the outside.
__all__ = ["robot", "solver"]
def SotPr2(robot):
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)
return sot
