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 = SOT('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 in the sot
"""
self.sot.push (task.name)
def remove (self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.remove (task.name)
def __str__ (self):
return self.sot.display ()
task_wrist.add (feature_wrist.name)
#Create tracer
tracer = TracerRealTime ('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/','dg_','.dat')
# Make sure signals are recomputed even if not used in the control graph
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace (robot.device, tracer, robot.device.name, "state")
addTrace (robot.device, tracer, feature_wrist._feature.name, "position")
addTrace (robot.device, tracer, feature_wrist._reference.name, "position")
# solver
solver = SOT ('solver')
solver.setSize (dimension)
solver.push (task_waist.name)
solver.push (task_wrist.name)
solver.push (task_elbow.name)
solver.push (task_lift.name)
solver.push (task_wrist1.name)
solver.push (task_wrist2.name)
plug (solver.control, robot.device.control)
#robot.device.increment (0.01)
dt = 0.01
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
#define contactLF and contactRF
for name,joint in [ ['LF',robot.OperationalPointsMap['left-ankle']], ['RF',robot.OperationalPointsMap['right-ankle'] ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact'+name] = contact
target = (0.5,-0.2,1.0)
#addRobotViewer(robot, small=False)
#robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskRH.task.name)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
def runner(n):
for i in xrange(n):
robot.device.increment(dt)
pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value))
#runner(3000)
task_wrist.controlGain.value = 1
task_wrist.add(feature_wrist.name)
#Create tracer
tracer = TracerRealTime('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/', 'dg_', '.dat')
# Make sure signals are recomputed even if not used in the control graph
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace(robot.device, tracer, robot.device.name, "state")
addTrace(robot.device, tracer, feature_wrist._feature.name, "position")
addTrace(robot.device, tracer, feature_wrist._reference.name, "position")
# solver
solver = SOT('solver')
solver.setSize(dimension)
solver.push(task_waist.name)
solver.push(task_wrist.name)
solver.push(task_elbow.name)
solver.push(task_lift.name)
solver.push(task_wrist1.name)
solver.push(task_wrist2.name)
plug(solver.control, robot.device.control)
#robot.device.increment (0.01)
dt = 0.01
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
@loopInThread
def inc():
task_waist_metakine.featureDes.position.value = goal_waist
solver = SolverKine('sot_solver')
solver.setSize (robot.dynamic.getDimension())
robot.device.resize (robot.dynamic.getDimension())
solver.push (task_waist_metakine.task.name)
plug (solver.control,robot.device.control)
'''
# solver
solver = SOT ('solver')
solver.setSize (dimension)
solver.push (task_waist.name)
solver.push (task_wrist.name)
plug (solver.control, robot.device.control)
#robot.device.increment (0.01)
dt = 0.01
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.device.increment(dt)
runner=inc()
runner.once()
[go,stop,next,n]=loopShortcuts(runner)
robot.OperationalPointsMap['left-ankle'])
contactLF.feature.frame('desired')
contactLF.gain.setConstant(10)
contactLF.keep()
locals()['contactLF'] = contactLF
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')
contactRF.gain.setConstant(10)
contactRF.keep()
locals()['contactRF'] = contactRF
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
from dynamic_graph.ros import RosPublish
ros_publish_state = RosPublish("ros_publish_state")
ros_publish_state.add("vector", "state", "/sot_control/state")
from dynamic_graph import plug
plug(robot.device.state, ros_publish_state.state)
robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100)
sot.push(robot.taskUpperBody.name)
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
robot.device.control.recompute(0)
#define contactLF and contactRF
for name, joint in [['LF', robot.OperationalPointsMap['left-ankle']],
['RF', robot.OperationalPointsMap['right-ankle']]]:
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact' + name] = contact
target = (0.5, -0.2, 1.0)
#addRobotViewer(robot, small=False)
#robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskRH.task.name)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
def runner(n):
for i in xrange(n):
robot.device.increment(dt)
pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value))
plug(seqplay.posture, taskPosture.featureDes.errorIN)
getPostureValue = Selec_of_vector("current_posture")
getPostureValue.selec(6, robotDim)
plug(robot.dynamic.position, getPostureValue.sin)
plug(getPostureValue.sout, taskPosture.feature.errorIN)
plug(getPostureValue.sout, seqplay.currentPosture)
setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9))
plug(taskPosture.gain.gain, taskPosture.controlGain)
plug(taskPosture.error, taskPosture.gain.error)
#START SEQUENCE PLAYER
seqplay.start()
taskPosture.featureDes.errorIN.recompute(0)
#PUSH TO SOLVER
sot.push(taskPosture.name)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
def runner(n):
for i in xrange(n):
robot.device.increment(dt)
pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value))
runner(3000)
# Create taks for the base
model.createOpPoint("base", "waist")
# Create task for the wrist
model.createOpPoint("wrist", "wrist_3_joint")
feature_wrist = FeaturePosition('position_wrist', model.wrist, model.Jwrist,
I4)
task_wrist = Task('wrist_task')
task_wrist.controlGain.value = 1.
task_wrist.add(feature_wrist.name)
# Create operational point for the end effector
model.createOpPoint("ee", "ee_fixed_joint")
solver = SOT('solver')
solver.setSize(dimension)
solver.push(task_wrist.name)
plug(solver.control, device.control)
device.increment(0.01)
#Create tracer
tracer = TracerRealTime('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/', 'dg_', '.dat')
# Make sure signals are recomputed even if not used in the control graph
device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace(device, tracer, device.name, "state")
addTrace(device, tracer, feature_wrist._feature.name, "position")
addTrace(device, tracer, feature_wrist._reference.name, "position")
#writeGraph('/tmp/graph')
