def test_ros(robot):
# SCRIPT PARAMETERS
createRosTopics = 1; # 1=true, 0=false
nj = 30; # number of joints
app = Application(robot);
dq_des = nj*(0.0,);
app.robot.device.control.value = dq_des;
if(createRosTopics==1):
ros = RosImport('rosImport');
ros.add('vector', 'robotStateRos', 'robotState');
plug(robot.device.robotState, ros.robotStateRos);
robot.device.after.addSignal('rosImport.trigger');
return ros
def test_ros(robot):
# SCRIPT PARAMETERS
createRosTopics = 1
# 1=true, 0=false
nj = 30
# number of joints
app = Application(robot)
dq_des = nj * (0.0, )
app.robot.device.control.value = dq_des
if (createRosTopics == 1):
ros = RosImport('rosImport')
ros.add('vector', 'robotStateRos', 'robotState')
plug(robot.device.robotState, ros.robotStateRos)
robot.device.after.addSignal('rosImport.trigger')
return ros
def setupEvents (self):
from dynamic_graph_hpp.sot import Event, CompareDouble
from dynamic_graph.sot.core.operator import Norm_of_vector
from dynamic_graph.ros import RosImport
self.norm = Norm_of_vector ("control_norm")
plug (self.sotrobot.device.control, self.norm.sin)
self.norm_comparision = CompareDouble ("control_norm_comparison")
plug (self.norm.sout, self.norm_comparision.sin1)
self.norm_comparision.sin2.value = 1e-2
self.norm_event = Event ("control_norm_event")
plug (self.norm_comparision.sout, self.norm_event.condition)
# self.sotrobot.device.after.addSignal (self.norm_event.check.name)
self.sotrobot.device.after.addSignal ("control_norm_event.check")
self.norm_ri = RosImport ('ros_import_control_norm')
self.norm_ri.add ('double', 'event_control_norm', '/sot_hpp/control_norm_changed')
plug (self.norm.sout, self.norm_ri.event_control_norm)
# plug (self.norm_event.trigger, self.norm_ri.trigger)
self.norm_event.addSignal ("ros_import_control_norm.trigger")
#!/usr/bin/env python
from dynamic_graph.ros import RosImport
ri = RosImport('rosimport')
ri.add('double', 'doubleS', 'doubleT')
ri.add('vector', 'vectorS', 'vectorT')
ri.add('matrix', 'matrixS', 'matrixT')
ri.doubleS.value = 42.
ri.vectorS.value = (
42.,
42.,
)
ri.matrixS.value = (
(
42.,
42.,
),
(
42.,
42.,
),
)
ri.trigger.recompute(ri.trigger.time + 1)
class Supervisor(object):
"""
Steps: P = placement, G = grasp, p = pre-P, g = pre-G
0. P <-> GP
1. P <-> gP
2. gP <-> GP
3. GP <-> G
4. GP <-> Gp
5. Gp <-> G
"""
def __init__(self, sotrobot, lpTasks=None, hpTasks=None):
self.sotrobot = sotrobot
self.hpTasks = hpTasks if hpTasks is not None else _hpTasks(sotrobot)
self.lpTasks = lpTasks if lpTasks is not None else _lpTasks(sotrobot)
self.currentSot = None
def setupEvents(self):
from dynamic_graph_hpp.sot import Event, CompareDouble
from dynamic_graph.sot.core.operator import Norm_of_vector
from dynamic_graph.ros import RosImport
self.norm = Norm_of_vector("control_norm")
plug(self.sotrobot.device.control, self.norm.sin)
self.norm_comparision = CompareDouble("control_norm_comparison")
plug(self.norm.sout, self.norm_comparision.sin1)
self.norm_comparision.sin2.value = 1e-2
self.norm_event = Event("control_norm_event")
plug(self.norm_comparision.sout, self.norm_event.condition)
# self.sotrobot.device.after.addSignal (self.norm_event.check.name)
self.sotrobot.device.after.addSignal("control_norm_event.check")
self.norm_ri = RosImport('ros_import_control_norm')
self.norm_ri.add('double', 'event_control_norm',
'/sot_hpp/control_norm_changed')
plug(self.norm.sout, self.norm_ri.event_control_norm)
# plug (self.norm_event.trigger, self.norm_ri.trigger)
self.norm_event.addSignal("ros_import_control_norm.trigger")
def makeInitialSot(self):
# Create the initial sot (keep)
sot = SOT('sot_keep')
sot.setSize(self.sotrobot.dynamic.getDimension())
self.keep_posture = Posture("posture_keep", self.sotrobot)
self.keep_posture.tp.setWithDerivative(False)
# TODO : I do agree that this is a dirty hack.
# The COM of the robot in the HPP frame is at those coordinates (approx.).
# But the keep_posture task is « internally » (there is no actuator able to directly move the COM,
# but the controller in the task is computing controls anyway, and integrating them)
# moving the computed COM to its reference value which is (0, 0, 0).
# So, when we send the goal coordinates of the feet from HPP to the SoT, there is an offset of 0,74m
# between the goal and the current position of the feet. This was the cause of the strange feet
# movements at the beginning of the demo.
# Maybe we can get the COM position and orientation from HPP at the beginning of the trajectory
# to initialize self.sotrobot.dynamic.position.value
self.keep_posture._signalPositionRef().value = tuple(
[-0.74, 0.0, 1.0, 0.0, 0.0, 0.0] +
list(self.sotrobot.dynamic.position.value)[6:])
self.keep_posture.pushTo(sot)
self.sots[-1] = sot
def topics(self):
c = self.hpTasks + self.lpTasks
for g in self.grasps.values():
c += g
return c.topics
def plugTopics(self, rosexport):
self.rosexport = rosexport
topics = self.topics()
for n, t in topics.items():
if t.has_key('handler'):
topic = _handlers[t['handler']](n, t)
else:
topic = t["topic"]
rosexport.add(t["type"], n, topic)
for s in t['signalGetters']:
plug(rosexport.signal(n), s())
print topic, "plugged to", n, ', ', len(
t['signalGetters']), 'times'
def isSotConsistentWithCurrent(self, id, thr=1e-3):
if self.currentSot is None or id == self.currentSot:
return True
csot = self.sots[self.currentSot]
nsot = self.sots[id]
t = self.sotrobot.device.control.time
csot.control.recompute(t)
nsot.control.recompute(t)
from numpy import array, linalg
error = array(nsot.control.value) - array(csot.control.value)
n = linalg.norm(error)
if n > thr:
print "Control not consistent:", linalg.norm(error)
print error
return False
return True
def clearQueues(self):
exec("tmp = " + self.rosexport.list())
for s in tmp:
self.rosexport.clearQueue(s)
def readQueue(self, read):
if read < 0:
print "ReadQueue argument should be >= 0"
return
t = self.sotrobot.device.control.time
self.rosexport.readQueue(t + read)
def stopReadingQueue(self):
self.rosexport.readQueue(-1)
def plugSot(self, id, check=False):
if check and not self.isSotConsistentWithCurrent(id):
# raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id))
print "Sot %d not consistent with sot %d" % (self.currentSot, id)
if id == -1:
# TODO : Explanation and linked TODO in the function makeInitialSot
if self.sotrobot.dynamic.position.value[0] > -0.5:
self.keep_posture._signalPositionRef().value = tuple(
[-0.74, 0.0, 1.0, 0.0, 0.0, 0.0] +
list(self.sotrobot.dynamic.position.value)[6:])
else:
self.keep_posture._signalPositionRef(
).value = self.sotrobot.dynamic.position.value
sot = self.sots[id]
# Start reading queues
self.readQueue(10)
plug(sot.control, self.sotrobot.device.control)
print "Current sot:", id
print sot.display()
self.currentSot = id
def runPreAction(self, idTransition):
if self.preActions.has_key(idTransition):
sot = self.preActions[idTransition]
print "Running pre action", idTransition
print sot.display()
t = self.sotrobot.device.control.time
sot.control.recompute(t - 1)
plug(sot.control, self.sotrobot.device.control)
return
print "No pre action", idTransition
def runPostAction(self, idStateTarget):
if self.postActions.has_key(self.currentSot):
d = self.postActions[self.currentSot]
if d.has_key(idStateTarget):
sot = d[idStateTarget]
print "Running post action", self.currentSot, idStateTarget
print sot.display()
t = self.sotrobot.device.control.time
sot.control.recompute(t - 1)
plug(sot.control, self.sotrobot.device.control)
return
print "No post action", self.currentSot, idStateTarget
def getJointList(self, prefix=""):
return [prefix + n for n in self.sotrobot.dynamic.model.names[1:]]
#!/usr/bin/python
from dynamic_graph.ros import RosImport
ri = RosImport('rosimport')
ri.add('double', 'doubleS', 'doubleT')
ri.add('vector', 'vectorS', 'vectorT')
ri.add('matrix', 'matrixS', 'matrixT')
ri.doubleS.value = 42.
ri.vectorS.value = (42., 42.,)
ri.matrixS.value = ((42., 42.,),(42., 42.,),)
ri.trigger.recompute(ri.trigger.time + 1)
def create_ros_topics(robot=None,
estimator=None,
torque_ctrl=None,
traj_gen=None,
ctrl_manager=None,
inv_dyn=None,
adm_ctrl=None,
ff_locator=None,
floatingBase=None):
from dynamic_graph.ros import RosImport
ros = RosImport('rosImport')
if (robot != None):
ros.add('vector', 'robotState_ros', 'robotState')
ros.add('vector', 'gyrometer_ros', 'gyrometer')
ros.add('vector', 'accelerometer_ros', 'accelerometer')
ros.add('vector', 'forceRLEG_ros', 'forceRLEG')
ros.add('vector', 'forceLLEG_ros', 'forceLLEG')
ros.add('vector', 'currents_ros', 'currents')
ros.add('vector', 'forceRARM_ros', 'forceRARM')
ros.add('vector', 'forceLARM_ros', 'forceLARM')
plug(robot.device.robotState, ros.robotState_ros)
plug(robot.device.gyrometer, ros.gyrometer_ros)
plug(robot.device.accelerometer, ros.accelerometer_ros)
plug(robot.device.forceRLEG, ros.forceRLEG_ros)
plug(robot.device.forceLLEG, ros.forceLLEG_ros)
plug(robot.device.currents, ros.currents_ros)
plug(robot.device.forceRARM, ros.forceRARM_ros)
plug(robot.device.forceLARM, ros.forceLARM_ros)
# robot.device.after.addSignal('rosImport.trigger');
robot.device.after.addDownsampledSignal('rosImport.trigger', 1)
if (estimator != None):
# ros.add('vector', 'estimator_jointsPositions_ros', 'estimator_jointsPositions');
ros.add('vector', 'estimator_jointsVelocities_ros',
'estimator_jointsVelocities')
ros.add('vector', 'estimator_jointsAccelerations_ros',
'estimator_jointsAccelerations')
# ros.add('vector', 'estimator_torsoAcceleration_ros', 'estimator_torsoAcceleration');
# ros.add('vector', 'estimator_torsoAngularVelocity_ros', 'estimator_torsoAngularVelocity');
ros.add('vector', 'estimator_contactWrenchLeftSole_ros',
'estimator_contactWrenchLeftSole')
ros.add('vector', 'estimator_contactWrenchRightSole_ros',
'estimator_contactWrenchRightSole')
ros.add('vector', 'estimator_ftSensRightFootPrediction_ros',
'estimator_ftSensRightFootPrediction')
# ros.add('vector', 'estimator_contactWrenchLeftHand_ros', 'estimator_contactWrenchLeftHand');
# ros.add('vector', 'estimator_contactWrenchRightHand_ros', 'estimator_contactWrenchRightHand');
# ros.add('vector', 'estimator_contactWrenchBody_ros', 'estimator_contactWrenchBody');
ros.add('vector', 'estimator_jointsTorques_ros',
'estimator_jointsTorques')
ros.add('vector', 'estimator_jointsTorquesFromInertiaModel_ros',
'estimator_jointsTorquesFromInertiaModel')
ros.add('vector', 'estimator_jointsTorquesFromMotorModel_ros',
'estimator_jointsTorquesFromMotorModel')
ros.add('vector', 'estimator_currentFiltered_ros',
'estimator_currentFiltered')
ros.add('vector', 'estimator_dynamicsError_ros',
'estimator_dynamicsError')
# plug(estimator.jointsPositions, ros.estimator_jointsPositions_ros);
plug(estimator.jointsVelocities, ros.estimator_jointsVelocities_ros)
plug(estimator.jointsAccelerations,
ros.estimator_jointsAccelerations_ros)
# plug(estimator.torsoAcceleration, ros.estimator_torsoAcceleration_ros);
# plug(estimator.torsoAngularVelocity, ros.estimator_torsoAngularVelocity_ros);
plug(estimator.contactWrenchLeftSole,
ros.estimator_contactWrenchLeftSole_ros)
plug(estimator.contactWrenchRightSole,
ros.estimator_contactWrenchRightSole_ros)
plug(estimator.ftSensRightFootPrediction,
ros.estimator_ftSensRightFootPrediction_ros)
# plug(estimator.contactWrenchLeftHand, ros.estimator_contactWrenchLeftHand_ros);
# plug(estimator.contactWrenchRightHand, ros.estimator_contactWrenchRightHand_ros);
# plug(estimator.contactWrenchBody, ros.estimator_contactWrenchBody_ros);
plug(estimator.jointsTorques, ros.estimator_jointsTorques_ros)
plug(estimator.jointsTorquesFromInertiaModel,
ros.estimator_jointsTorquesFromInertiaModel_ros)
plug(estimator.jointsTorquesFromMotorModel,
ros.estimator_jointsTorquesFromMotorModel_ros)
plug(estimator.currentFiltered, ros.estimator_currentFiltered_ros)
plug(estimator.dynamicsError, ros.estimator_dynamicsError_ros)
robot.device.after.addSignal('estimator.contactWrenchRightFoot')
if (torque_ctrl != None):
# ros.add('vector', 'torque_ctrl_predictedPwm_ros', 'torque_ctrl_predictedPwm');
# ros.add('vector', 'torque_ctrl_predictedPwm_tau_ros', 'torque_ctrl_predictedPwm_tau');
# ros.add('vector', 'torque_ctrl_pwm_ff_ros', 'torque_ctrl_pwm_ff');
# ros.add('vector', 'torque_ctrl_pwm_fb_ros', 'torque_ctrl_pwm_fb');
# ros.add('vector', 'torque_ctrl_pwm_friction_ros', 'torque_ctrl_pwm_friction');
# ros.add('vector', 'torque_ctrl_smoothSignDq_ros', 'torque_ctrl_smoothSignDq');
# ros.add('vector', 'torque_ctrl_predictedJointsTorques_ros', 'torque_ctrl_predictedJointsTorques');
ros.add('vector', 'torque_ctrl_controlCurrent_ros',
'torque_ctrl_controlCurrent')
ros.add('vector', 'torque_ctrl_desiredCurrent_ros',
'torque_ctrl_desiredCurrent')
# plug(torque_ctrl.predictedPwm, ros.torque_ctrl_predictedPwm_ros);
# plug(torque_ctrl.predictedPwm_tau, ros.torque_ctrl_predictedPwm_tau_ros);
# plug(torque_ctrl.pwm_ff, ros.torque_ctrl_pwm_ff_ros);
# plug(torque_ctrl.pwm_fb, ros.torque_ctrl_pwm_fb_ros);
# plug(torque_ctrl.pwm_friction, ros.torque_ctrl_pwm_friction_ros);
# plug(torque_ctrl.smoothSignDq, ros.torque_ctrl_smoothSignDq_ros);
# plug(torque_ctrl.predictedJointsTorques, ros.torque_ctrl_predictedJointsTorques_ros);
plug(torque_ctrl.controlCurrent, ros.torque_ctrl_controlCurrent_ros)
plug(torque_ctrl.desiredCurrent, ros.torque_ctrl_desiredCurrent_ros)
if (traj_gen != None):
ros.add('vector', 'traj_gen_q_ros', 'traj_gen_q')
ros.add('vector', 'traj_gen_dq_ros', 'traj_gen_dq')
ros.add('vector', 'traj_gen_ddq_ros', 'traj_gen_ddq')
ros.add('vector', 'traj_gen_fRightFoot_ros', 'traj_gen_fRightFoot')
plug(traj_gen.q, ros.traj_gen_q_ros)
plug(traj_gen.dq, ros.traj_gen_dq_ros)
plug(traj_gen.ddq, ros.traj_gen_ddq_ros)
plug(traj_gen.fRightFoot, ros.traj_gen_fRightFoot_ros)
if (ctrl_manager != None):
ros.add('vector', 'ctrl_manager_pwmDes_ros', 'ctrl_manager_pwmDes')
ros.add('vector', 'ctrl_manager_pwmDesSafe_ros',
'ctrl_manager_pwmDesSafe')
ros.add('vector', 'ctrl_manager_signOfControlFiltered_ros',
'ctrl_manager_signOfControlFiltered')
ros.add('vector', 'ctrl_manager_signOfControl_ros',
'ctrl_manager_signOfControl')
plug(ctrl_manager.pwmDes, ros.ctrl_manager_pwmDes_ros)
plug(ctrl_manager.pwmDesSafe, ros.ctrl_manager_pwmDesSafe_ros)
plug(ctrl_manager.signOfControlFiltered,
ros.ctrl_manager_signOfControlFiltered_ros)
plug(ctrl_manager.signOfControl, ros.ctrl_manager_signOfControl_ros)
if (inv_dyn != None):
# ros.add('vector', 'inv_dyn_tauDes_ros', 'inv_dyn_tauDes');
# ros.add('vector', 'inv_dyn_tauFF_ros', 'inv_dyn_tauFF');
# ros.add('vector', 'inv_dyn_tauFB_ros', 'inv_dyn_tauFB');
# ros.add('vector', 'inv_dyn_ddqDes_ros', 'inv_dyn_ddqDes');
# ros.add('vector', 'inv_dyn_qError_ros', 'inv_dyn_qError');
# plug(inv_dyn.tauDes, ros.inv_dyn_tauDes_ros);
# plug(inv_dyn.tauFF, ros.inv_dyn_tauFF_ros);
# plug(inv_dyn.tauFB, ros.inv_dyn_tauFB_ros);
# plug(inv_dyn.ddqDes, ros.inv_dyn_ddqDes_ros);
# plug(inv_dyn.qError, ros.inv_dyn_qError_ros);
ros.add('vector', 'inv_dyn_tauDes_ros', 'inv_dyn_tauDes')
plug(inv_dyn.tau_des, ros.inv_dyn_tauDes_ros)
if (adm_ctrl != None):
ros.add('vector', 'adm_ctrl_qDes_ros', 'adm_ctrl_qDes')
ros.add('vector', 'adm_ctrl_dqDes_ros', 'adm_ctrl_dqDes')
ros.add('vector', 'adm_ctrl_fRightFootError_ros',
'adm_ctrl_fRightFootError')
plug(adm_ctrl.qDes, ros.adm_ctrl_qDes_ros)
plug(adm_ctrl.dqDes, ros.adm_ctrl_dqDes_ros)
plug(adm_ctrl.fRightFootError, ros.adm_ctrl_fRightFootError_ros)
if (ff_locator != None):
plug(ff_locator.base6dFromFoot_encoders, ros.robotState_ros)
if (floatingBase != None):
ros.add('vector', 'floatingBase_pos_ros', 'floatingBase_pos')
plug(floatingBase.soutPos, ros.floatingBase_pos_ros)
return ros
class Supervisor(object):
"""
Steps: P = placement, G = grasp, p = pre-P, g = pre-G
0. P <-> GP
1. P <-> gP
2. gP <-> GP
3. GP <-> G
4. GP <-> Gp
5. Gp <-> G
"""
def __init__ (self, sotrobot, lpTasks = None, hpTasks = None):
self.sotrobot = sotrobot
self.hpTasks = hpTasks if hpTasks is not None else _hpTasks(sotrobot)
self.lpTasks = lpTasks if lpTasks is not None else _lpTasks(sotrobot)
self.currentSot = None
def initForGrasps (self, hppclient, grippers, objects, handlesPerObjects):
handles = [ h for i in idx(objects) for h in handlesPerObjects[i] ]
self.grippers = [ OpFrame(hppclient) for _ in idx(grippers)]
self.handles = [ OpFrame(hppclient) for _ in idx(handles )]
self.grippersIdx = { grippers[i] : i for i in idx(grippers) }
self.handlesIdx = { handles[i] : i for i in idx(handles) }
for ig, g in idx_zip(grippers): self.grippers[ig].setHppGripper (g)
for ih, h in idx_zip(handles ): self.handles [ih].setHppHandle (h)
for g in self.grippers: g.setSotFrameFromHpp (self.sotrobot.dynamic.model)
for h in self.handles : h.setSotFrameFromHpp (self.sotrobot.dynamic.model)
def setupEvents (self):
from dynamic_graph_hpp.sot import Event, CompareDouble
from dynamic_graph.sot.core.operator import Norm_of_vector
from dynamic_graph.ros import RosImport
self.norm = Norm_of_vector ("control_norm")
plug (self.sotrobot.device.control, self.norm.sin)
self.norm_comparision = CompareDouble ("control_norm_comparison")
plug (self.norm.sout, self.norm_comparision.sin1)
self.norm_comparision.sin2.value = 1e-2
self.norm_event = Event ("control_norm_event")
plug (self.norm_comparision.sout, self.norm_event.condition)
# self.sotrobot.device.after.addSignal (self.norm_event.check.name)
self.sotrobot.device.after.addSignal ("control_norm_event.check")
self.norm_ri = RosImport ('ros_import_control_norm')
self.norm_ri.add ('double', 'event_control_norm', '/sot_hpp/control_norm_changed')
plug (self.norm.sout, self.norm_ri.event_control_norm)
# plug (self.norm_event.trigger, self.norm_ri.trigger)
self.norm_event.addSignal ("ros_import_control_norm.trigger")
def makeGrasps (self, transitions):
"""
@param transition: a list of dictionaries containing the following keys:
- "id", "name": the id and name of the transition in hpp.
- "manifold": a tuple of (gripper_index, handle_index) defining the submanifold into which the transition takes place.
- "grasp" (optional) : (gripper_index, handle_index) corresponding to the added/removed grasp.
- "forward" (required if "grasp") : True if added, False if removed.
- "step" (required if "grasp") : integer [ 0, 5 ].
"""
self.grasps = dict()
self.sots = dict()
self.postActions = dict()
self.preActions = dict()
self.transitions = transitions
print "Transitions:\n"
print transitions
print "\n"
for t in transitions:
# Create SOT solver
# sot = SOT ('sot_' + str(t['id']) + '-' + t['name'])
sot = SOT ('sot_' + str(t['id']))
sot.setSize(self.sotrobot.dynamic.getDimension())
# Push high priority tasks (Equilibrium for instance)
self.hpTasks.pushTo(sot)
# Create (or get) the tasks
M = self._manifold(t["manifold"])
if t.has_key("grasp"):
grasp = self._manifold(t["manifold"] + (t["grasp"],))
forward = bool(t["forward"])
step = int(t["step"])
assert step >= 0 and step <= 5, "'step' must be an integer within [0, 5]"
# TODO Events should be set up here for each step.
# For instance, adding some events based on force feedback to say
# when the object is grasped.
if forward:
if step == 1:
M.pushTo(sot)
elif step == 0 or step == 2:
grasp.pushTo(sot)
else:
grasp.pushTo(sot)
else:
if step == 1:
M.pushTo(sot)
elif step == 0 or step == 2:
grasp.pushTo(sot)
else:
grasp.pushTo(sot)
else:
M.pushTo(sot)
# Put low priority tasks
self.lpTasks.pushTo(sot)
self.sots[t['id']] = sot
def makeInitialSot (self):
# Create the initial sot (keep)
sot = SOT ('sot_keep')
sot.setSize(self.sotrobot.dynamic.getDimension())
self.keep_posture = Posture ("posture_keep", self.sotrobot)
self.keep_posture.tp.setWithDerivative (False)
# TODO : I do agree that this is a dirty hack.
# The COM of the robot in the HPP frame is at those coordinates (approx.).
# But the keep_posture task is « internally » (there is no actuator able to directly move the COM,
# but the controller in the task is computing controls anyway, and integrating them)
# moving the computed COM to its reference value which is (0, 0, 0).
# So, when we send the goal coordinates of the feet from HPP to the SoT, there is an offset of 0,74m
# between the goal and the current position of the feet. This was the cause of the strange feet
# movements at the beginning of the demo.
# Maybe we can get the COM position and orientation from HPP at the beginning of the trajectory
# to initialize self.sotrobot.dynamic.position.value
self.keep_posture._signalPositionRef().value = tuple([-0.74, 0.0, 1.0, 0.0, 0.0, 0.0] + list(self.sotrobot.dynamic.position.value)[6:])
self.keep_posture.pushTo(sot)
self.sots[-1] = sot
def topics (self):
c = self.hpTasks + self.lpTasks
for g in self.grasps.values():
c += g
return c.topics
def plugTopics (self, rosexport):
self.rosexport = rosexport
topics = self.topics()
for n, t in topics.items():
if t.has_key('handler'):
topic = _handlers[t['handler']] (n, t)
else:
topic = t["topic"]
rosexport.add (t["type"], n, topic)
for s in t['signalGetters']:
plug (rosexport.signal(n), s())
print topic, "plugged to", n, ', ', len(t['signalGetters']), 'times'
def setupReferences (self, gui, rosexport):
ret = dict()
gui.createGroup("references")
topics = self.topics()
for n, t in topics.items():
types = []
if t.has_key('handler'):
if t['handler'] == 'hppjoint':
if t['velocity']:
types = ['linvel', 'angvel']
else:
types = ['pose',]
elif t['handler'] == 'hppcom':
if t['velocity']:
types = ['linvel',]
else:
types = ['point',]
name = "ref_" + n
ret[n] = list()
for i,type in idx_zip(types):
nameref = name + str(i)
if type == 'pose':
gui.addXYZaxis(nameref, (1,0,0,1), 0.005, 0.05)
gui.setVisibility(nameref, 'ALWAYS_ON_TOP')
gui.addToGroup(nameref, "references")
ret[n].append(type)
# elif type == '':
return ret
def setReferenceValue (self, gui, rosexport, refs):
from dynamic_graph.sot.tools.quaternion import Quaternion
from dynamic_graph.sot.tools.se3 import SE3
idx = 0
for n, types in refs.items():
for i,type in idx_zip(types):
nameref = "ref_" + n + str(i)
sig = rosexport.signal(n)
# if sig.time != self.sotrobot.device.control.time or len(sig.value) == 0:
if len(sig.value) == 0:
if gui.getIntProperty(nameref, "visibility") != 2:
print "Hiding", nameref
gui.setVisibility(nameref, 'OFF')
continue
else:
if gui.getIntProperty(nameref, "visibility") == 2:
print "Showing", nameref
# gui.setVisibility(nameref, 'ON')
gui.setVisibility(nameref, 'ALWAYS_ON_TOP')
if type == 'pose':
H = SE3(sig.value)
q = Quaternion(sig.value)
gui.applyConfiguration(nameref, list(H.translation.value) + list(q.array[1:]) + list(q.array[:1]) )
gui.refresh()
def isSotConsistentWithCurrent(self, id, thr = 1e-3):
if self.currentSot is None or id == self.currentSot:
return True
csot = self.sots[self.currentSot]
nsot = self.sots[id]
t = self.sotrobot.device.control.time
csot.control.recompute(t)
nsot.control.recompute(t)
from numpy import array, linalg
error = array(nsot.control.value) - array(csot.control.value)
n = linalg.norm(error)
if n > thr:
print "Control not consistent:", linalg.norm(error)
print error
return False
return True
def clearQueues(self):
exec ("tmp = " + self.rosexport.list())
for s in tmp:
self.rosexport.clearQueue(s)
def readQueue(self, read):
if read < 0:
print "ReadQueue argument should be >= 0"
return
t = self.sotrobot.device.control.time
self.rosexport.readQueue (t + read)
def stopReadingQueue(self):
self.rosexport.readQueue (-1)
def plugSot(self, id, check = False):
if check and not self.isSotConsistentWithCurrent (id):
# raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id))
print "Sot %d not consistent with sot %d" % (self.currentSot, id)
if id == -1:
# TODO : Explanation and linked TODO in the function makeInitialSot
if self.sotrobot.dynamic.position.value[0] > -0.5:
self.keep_posture._signalPositionRef().value = tuple([-0.74, 0.0, 1.0, 0.0, 0.0, 0.0] + list(self.sotrobot.dynamic.position.value)[6:])
else:
self.keep_posture._signalPositionRef().value = self.sotrobot.dynamic.position.value
sot = self.sots[id]
# Start reading queues
self.readQueue(10)
plug(sot.control, self.sotrobot.device.control)
print "Current sot:", id
print sot.display()
self.currentSot = id
def runPreAction(self, idTransition):
if self.preActions.has_key(idTransition):
sot = self.preActions[idTransition]
print "Running pre action", idTransition
print sot.display()
t = self.sotrobot.device.control.time
sot.control.recompute(t-1)
plug(sot.control, self.sotrobot.device.control)
return
print "No pre action", idTransition
def runPostAction(self, idStateTarget):
if self.postActions.has_key(self.currentSot):
d = self.postActions[self.currentSot]
if d.has_key(idStateTarget):
sot = d[idStateTarget]
print "Running post action", self.currentSot, idStateTarget
print sot.display()
t = self.sotrobot.device.control.time
sot.control.recompute(t-1)
plug(sot.control, self.sotrobot.device.control)
return
print "No post action", self.currentSot, idStateTarget
def getJointList (self, prefix = ""):
return [ prefix + n for n in self.sotrobot.dynamic.model.names[1:] ]
def _manifold (self, idxs):
if self.grasps.has_key(idxs):
return self.grasps[idxs]
if len(idxs) == 1:
m = Grasp(self.grippers[idxs[0][0]], self.handles[idxs[0][1]])
m.makeTasks(self.sotrobot)
elif len(idxs) == 0:
m = Manifold()
else:
subm = self._manifold(idxs[:-1])
# TODO Find relative
m = Grasp(self.grippers[idxs[-1][0]], self.handles[idxs[-1][1]])
m.makeTasks(self.sotrobot)
m += subm
self.grasps[idxs] = m
return m