def addTracer(name,
prefix,
dir="/tmp",
suffix=".txt",
size=10 * 1048576):
# default size: 10Mo
tracer = TracerRealTime(name)
self.tracers[tracer.name] = tracer
tracer.setBufferSize(size)
tracer.open(dir, prefix, suffix)
self.sotrobot.device.after.addSignal(name + ".triger")
return tracer
def dump_sot_sigs(robot, list_of_sigs, duration):
'''dumps several sot signals in /tmp
ex: dump_sot_sig(robot,[entity,signals],1.)'''
tracer = TracerRealTime('tmp_tracer')
tracer.setBufferSize(80 * (2**20))
tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
for sigs in list_of_sigs:
entity = sigs[0]
for sig in sigs[1:]:
full_sig_name = entity.name + '.' + sig
addTrace(tracer, entity, sig)
robot.device.after.addSignal(full_sig_name)
tracer.start()
sleep(duration)
dump_tracer(tracer)
tracer.clear()
def create_tracer(device,
traj_gen=None,
estimator=None,
inv_dyn=None,
torque_ctrl=None):
tracer = TracerRealTime('motor_id_trace')
tracer.setBufferSize(80 * (2**20))
tracer.open('/tmp/', 'dg_', '.dat')
device.after.addSignal('{0}.triger'.format(tracer.name))
addSignalsToTracer(tracer, device, traj_gen, torque_ctrl)
with open('/tmp/dg_info.dat', 'a') as f:
if (estimator != None):
f.write('Estimator encoder delay: {0}\n'.format(
estimator.getDelayEnc()))
f.write('Estimator F/T sensors delay: {0}\n'.format(
estimator.getDelayFTsens()))
f.write('Estimator accelerometer delay: {0}\n'.format(
estimator.getDelayAcc()))
f.write('Estimator gyroscope delay: {0}\n'.format(
estimator.getDelayGyro()))
f.write('Estimator use reference velocities: {0}\n'.format(
estimator.getUseRefJointVel()))
f.write('Estimator use reference accelerations: {0}\n'.format(
estimator.getUseRefJointAcc()))
f.write('Estimator use raw encoders: {0}\n'.format(
estimator.getUseRawEncoders()))
f.write('Estimator use f/t sensors: {0}\n'.format(
estimator.getUseFTsensors()))
f.write('Estimator f/t sensor offsets: {0}\n'.format(
estimator.getFTsensorOffsets()))
if (inv_dyn != None):
f.write('Inv dyn Ks: {0}\n'.format(inv_dyn.Kp.value))
f.write('Inv dyn Kd: {0}\n'.format(inv_dyn.Kd.value))
f.write('Inv dyn Kf: {0}\n'.format(inv_dyn.Kf.value))
f.write('Inv dyn Ki: {0}\n'.format(inv_dyn.Ki.value))
if (torque_ctrl != None):
f.write('Torque ctrl KpTorque: {0}\n'.format(
torque_ctrl.KpTorque.value))
f.write('Torque ctrl KpCurrent: {0}\n'.format(
torque_ctrl.KpCurrent.value))
f.write('Torque ctrl K_tau: {0}\n'.format(torque_ctrl.k_tau.value))
f.write('Torque ctrl K_v: {0}\n'.format(torque_ctrl.k_v.value))
f.close()
return tracer
def create_tracer(device, traj_gen=None, estimator_kin=None,
inv_dyn=None, torque_ctrl=None):
tracer = TracerRealTime('motor_id_trace');
tracer.setBufferSize(80*(2**20));
tracer.open('/tmp/','dg_','.dat');
device.after.addSignal('{0}.triger'.format(tracer.name));
addSignalsToTracer(tracer, device);
with open('/tmp/dg_info.dat', 'a') as f:
if(estimator_kin!=None):
f.write('Estimator encoder delay: {0}\n'.format(robot.filters.estimator_kin.getDelay()));
if(inv_dyn!=None):
f.write('Inv dyn Ks: {0}\n'.format(inv_dyn.Kp.value));
f.write('Inv dyn Kd: {0}\n'.format(inv_dyn.Kd.value));
f.write('Inv dyn Kf: {0}\n'.format(inv_dyn.Kf.value));
f.write('Inv dyn Ki: {0}\n'.format(inv_dyn.Ki.value));
if(torque_ctrl!=None):
f.write('Torque ctrl KpTorque: {0}\n'.format (robot.torque_ctrl.KpTorque.value ));
f.close();
return tracer;
task_wrist2.controlGain.value = 0.
task_wrist2.add (feature_wrist2.name)
# Create task for the wrist3
I4 = ((1.,0,0,0.321),
(0,1.,0,0.109),
(0,0,1.,0.848),
(0,0,0,1.),)
feature_wrist = FeaturePosition ('position_wrist', robot.dynamic.wrist_3_joint, robot.dynamic.Jwrist_3_joint, I4)
task_wrist = Task ('wrist_task')
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)
class AbstractHumanoidRobot (object):
"""
This class instantiates all the entities required to get a consistent
representation of a humanoid robot, mainly:
- device : to integrate velocities into angular control,
- dynamic: to compute forward geometry and kinematics,
- zmpFromForces: to compute ZMP force foot force sensors,
- stabilizer: to stabilize balanced motions
Operational points are stored into 'OperationalPoints' list. Some of them
are also accessible directly as attributes:
- leftWrist,
- rightWrist,
- leftAnkle,
- rightAnkle,
- Gaze.
Operational points are mapped to the actual joints in the robot model
via 'OperationalPointsMap' dictionary.
This attribute *must* be defined in the subclasses
Other attributes require to be defined:
- halfSitting: half-sitting position is the robot initial pose.
This attribute *must* be defined in subclasses.
- dynamic: The robot dynamic model.
- device: The device that integrates the dynamic equation, namely
the real robot or
a simulator
- dimension: The configuration size.
"""
def _initialize (self):
self.OperationalPoints = ['left-wrist', 'right-wrist',
'left-ankle', 'right-ankle',
'gaze']
"""
Operational points are specific interesting points of the robot
used to control it.
When an operational point is defined, signals corresponding to the
point position and jacobian are created.
For instance if creating an operational point for the left-wrist,
the associated signals will be called "left-wrist" and
"Jleft-wrist" for respectively the position and the jacobian.
"""
self.AdditionalFrames = []
"""
Additional frames are frames which are defined w.r.t an operational point
and provides an interesting transformation.
It can be used, for instance, to store the sensor location.
The contained elements must be triplets matching:
- additional frame name,
- transformation w.r.t to the operational point,
- operational point file.
"""
self.frames = dict()
"""
Additional frames defined by using OpPointModifier.
"""
#FIXME: the following options are /not/ independent.
# zmp requires acceleration which requires velocity.
"""
Enable velocity computation.
"""
self.enableVelocityDerivator = False
"""
Enable acceleration computation.
"""
self.enableAccelerationDerivator = False
"""
Enable ZMP computation
"""
self.enableZmpComputation = False
"""
Tracer used to log data.
"""
self.tracer = None
"""
How much data will be logged.
"""
self.tracerSize = 2**20
"""
Automatically recomputed signals through the use
of device.after.
This list is maintained in order to clean the
signal list device.after before exiting.
"""
self.autoRecomputedSignals = []
"""
Which signals should be traced.
"""
self.tracedSignals = {
'dynamic': ["com", "zmp", "angularmomentum",
"position", "velocity", "acceleration"],
'device': ['zmp', 'control', 'state']
}
def help (self):
print (AbstractHumanoidRobot.__doc__)
def loadModelFromKxml(self, name, filename):
"""
Load a model from a kxml file and return the parsed model.
This uses the Python parser class implement in
dynamic_graph.sot.dynamics_pinocchio.parser.
kxml is an extensible file format used by KineoWorks to store
both the robot mesh and its kinematic chain.
The parser also imports inertia matrices which is a
non-standard property.
"""
model = Parser(name, filename).parse()
self.setProperties(model)
return model
def loadModelFromUrdf(self, name, urdfPath,
dynamicType):
"""
Load a model using the pinocchio urdf parser. This parser looks
for urdf files in which kinematics and dynamics information
have been added.
Additional information are located in two different XML files.
"""
model = dynamicType(name)
#TODO: setproperty flags in sot-pinocchio
#self.setProperties(model)
model.setFile(urdfPath)
model.parse()
return model
#TODO: put these flags in sot-pinocchio
#def setProperties(self, model):
# model.setProperty('TimeStep', str(self.timeStep))
#
# model.setProperty('ComputeAcceleration', 'false')
# model.setProperty('ComputeAccelerationCoM', 'false')
# model.setProperty('ComputeBackwardDynamics', 'false')
# model.setProperty('ComputeCoM', 'false')
# model.setProperty('ComputeMomentum', 'false')
# model.setProperty('ComputeSkewCom', 'false')
# model.setProperty('ComputeVelocity', 'false')
# model.setProperty('ComputeZMP', 'false')
# model.setProperty('ComputeAccelerationCoM', 'true')
# model.setProperty('ComputeCoM', 'true')
# model.setProperty('ComputeVelocity', 'true')
# model.setProperty('ComputeZMP', 'true')
#
# if self.enableZmpComputation:
# model.setProperty('ComputeBackwardDynamics', 'true')
# model.setProperty('ComputeAcceleration', 'true')
# model.setProperty('ComputeMomentum', 'true')
def initializeOpPoints(self):
for op in self.OperationalPoints:
self.dynamic.createOpPoint(op, self.OperationalPointsMap[op])
def createFrame(self, frameName, transformation, operationalPoint):
frame = OpPointModifier(frameName)
frame.setTransformation(transformation)
plug(self.dynamic.signal(operationalPoint),
frame.positionIN)
plug(self.dynamic.signal("J{0}".format(operationalPoint)),
frame.jacobianIN)
frame.position.recompute(frame.position.time + 1)
frame.jacobian.recompute(frame.jacobian.time + 1)
return frame
def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
"""
Robot timestep
"""
self.timeStep = self.device.getTimeStep()
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension*(0.,)
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout,
self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension*(0.,)
#self.initializeOpPoints()
#TODO: hand parameters through srdf --- additional frames ---
#self.frames = dict()
#frameName = 'rightHand'
#self.frames [frameName] = self.createFrame (
# "{0}_{1}".format (self.name, frameName),
# self.dynamic.getHandParameter (True), "right-wrist")
# rightGripper is an alias for the rightHand:
#self.frames ['rightGripper'] = self.frames [frameName]
#frameName = 'leftHand'
#self.frames [frameName] = self.createFrame (
# "{0}_{1}".format (self.name, frameName),
# self.dynamic.getHandParameter (False), "left-wrist")
# leftGripper is an alias for the leftHand:
#self.frames ["leftGripper"] = self.frames [frameName]
#for (frameName, transformation, signalName) in self.AdditionalFrames:
# self.frames[frameName] = self.createFrame(
# "{0}_{1}".format(self.name, frameName),
# transformation,
# signalName)
def addTrace(self, entityName, signalName):
if self.tracer:
self.autoRecomputedSignals.append(
'{0}.{1}'.format(entityName, signalName))
addTrace(self, self.tracer, entityName, signalName)
def initializeTracer(self):
if not self.tracer:
self.tracer = TracerRealTime('trace')
self.tracer.setBufferSize(self.tracerSize)
self.tracer.open('/tmp/','dg_','.dat')
# Recompute trace.triger at each iteration to enable tracing.
self.device.after.addSignal('{0}.triger'.format(self.tracer.name))
def traceDefaultSignals (self):
# Geometry / operational points
for s in self.OperationalPoints + self.tracedSignals['dynamic']:
self.addTrace(self.dynamic.name, s)
# Geometry / frames
for (frameName, _, _) in self.AdditionalFrames:
for s in ['position', 'jacobian']:
self.addTrace(self.frames[frameName].name, s)
# Device
for s in self.tracedSignals['device']:
self.addTrace(self.device.name, s)
if type(self.device) != RobotSimu:
self.addTrace(self.device.name, 'robotState')
# Misc
if self.enableVelocityDerivator:
self.addTrace(self.velocityDerivator.name, 'sout')
if self.enableAccelerationDerivator:
self.addTrace(self.accelerationDerivator.name, 'sout')
def __init__(self, name, tracer = None):
self._initialize()
self.name = name
# Initialize tracer if necessary.
if tracer:
self.tracer = tracer
def __del__(self):
if self.tracer:
self.stopTracer()
def startTracer(self):
"""
Start the tracer if it does not already been stopped.
"""
if self.tracer:
self.tracer.start()
def stopTracer(self):
"""
Stop and destroy tracer.
"""
if self.tracer:
self.tracer.dump()
self.tracer.stop()
self.tracer.close()
self.tracer.clear()
for s in self.autoRecomputedSignals:
self.device.after.rmSignal(s)
self.tracer = None
def reset(self, posture = None):
"""
Restart the control from another position.
This method has not been extensively tested and
should be used carefully.
In particular, tasks should be removed from the
solver before attempting a reset.
"""
if not posture:
posture = self.halfSitting
self.device.set(posture)
self.dynamic.com.recompute(self.device.state.time+1)
self.dynamic.Jcom.recompute(self.device.state.time+1)
for op in self.OperationalPoints:
self.dynamic.signal(self.OperationalPointsMap[op]).recompute(self.device.state.time+1)
self.dynamic.signal('J'+self.OperationalPointsMap[op]).recompute(self.device.state.time+1)
class AbstractHumanoidRobot (object):
"""
This class instantiates all the entities required to get a consistent
representation of a humanoid robot, mainly:
- device : to integrate velocities into angular control,
- dynamic: to compute forward geometry and kinematics,
- zmpFromForces: to compute ZMP force foot force sensors,
- stabilizer: to stabilize balanced motions
Operational points are stored into 'OperationalPoints' list. Some of them
are also accessible directly as attributes:
- leftWrist,
- rightWrist,
- leftAnkle,
- rightAnkle,
- Gaze.
Tasks are stored into 'tasks' dictionary.
For portability, some signals are accessible as attributes:
- zmpRef: input (vector),
- comRef: input (vector).
- com: output (vector)
- comSelec input (flag)
- comdot: input (vector) reference velocity of the center of mass
"""
OperationalPoints = []
"""
Operational points are specific interesting points of the robot
used to control it.
When an operational point is defined, signals corresponding to the
point position and jacobian are created.
For instance if creating an operational point for the left-wrist,
the associated signals will be called "left-wrist" and
"Jleft-wrist" for respectively the position and the jacobian.
"""
AdditionalFrames = []
"""
Additional frames are frames which are defined w.r.t an operational point
and provides an interesting transformation.
It can be used, for instance, to store the sensor location.
The contained elements must be triplets matching:
- additional frame name,
- transformation w.r.t to the operational point,
- operational point file.
"""
name = None
"""Entity name (internal use)"""
halfSitting = None
"""
The half-sitting position is the robot initial pose.
This attribute *must* be defined in subclasses.
"""
dynamic = None
"""
The robot dynamic model.
"""
device = None
"""
The device that integrates the dynamic equation, namely
- the real robot or
- a simulator
"""
dimension = None
"""The configuration size."""
featureCom = None
"""
This generic feature takes as input the robot center of mass
and as desired value the featureComDes feature of this class.
"""
featureComDes = None
"""
The feature associated to the robot center of mass desired
position.
"""
comTask = None
features = dict()
"""
Features associated to each operational point. Keys are
corresponding to operational points.
"""
tasks = dict()
"""
Features associated to each operational point. Keys are
corresponding to operational points.
"""
frames = dict()
"""
Additional frames defined by using OpPointModifier.
"""
#FIXME: the following options are /not/ independent.
# zmp requires acceleration which requires velocity.
"""
Enable velocity computation.
"""
enableVelocityDerivator = False
"""
Enable acceleration computation.
"""
enableAccelerationDerivator = False
"""
Enable ZMP computation
"""
enableZmpComputation = False
"""
Tracer used to log data.
"""
tracer = None
"""
How much data will be logged.
"""
tracerSize = 2**20
"""
Automatically recomputed signals through the use
of device.after.
This list is maintained in order to clean the
signal list device.after before exiting.
"""
autoRecomputedSignals = []
"""
Which signals should be traced.
"""
tracedSignals = {
'dynamic': ["com", "zmp", "angularmomentum",
"position", "velocity", "acceleration"],
'device': ['zmp', 'control', 'state']
}
"""
Robot timestep
"""
timeStep = 0.005
def help (self):
print (AbstractHumanoidRobot.__doc__)
def loadModelFromKxml(self, name, filename):
"""
Load a model from a kxml file and return the parsed model.
This uses the Python parser class implement in
dynamic_graph.sot.dynamics.parser.
kxml is an extensible file format used by KineoWorks to store
both the robot mesh and its kinematic chain.
The parser also imports inertia matrices which is a
non-standard property.
"""
model = Parser(name, filename).parse()
self.setProperties(model)
return model
def loadModelFromJrlDynamics(self, name, modelDir, modelName,
specificitiesPath, jointRankPath,
dynamicType):
"""
Load a model using the jrl-dynamics parser. This parser looks
for VRML files in which kinematics and dynamics information
have been added by extending the VRML format.
It is mainly used by OpenHRP.
Additional information are located in two different XML files.
"""
model = dynamicType(name)
self.setProperties(model)
model.setFiles(modelDir, modelName,
specificitiesPath, jointRankPath)
model.parse()
return model
def setProperties(self, model):
model.setProperty('TimeStep', str(self.timeStep))
model.setProperty('ComputeAcceleration', 'false')
model.setProperty('ComputeAccelerationCoM', 'false')
model.setProperty('ComputeBackwardDynamics', 'false')
model.setProperty('ComputeCoM', 'false')
model.setProperty('ComputeMomentum', 'false')
model.setProperty('ComputeSkewCom', 'false')
model.setProperty('ComputeVelocity', 'false')
model.setProperty('ComputeZMP', 'false')
model.setProperty('ComputeAccelerationCoM', 'true')
model.setProperty('ComputeCoM', 'true')
model.setProperty('ComputeVelocity', 'true')
model.setProperty('ComputeZMP', 'true')
if self.enableZmpComputation:
model.setProperty('ComputeBackwardDynamics', 'true')
model.setProperty('ComputeAcceleration', 'true')
model.setProperty('ComputeMomentum', 'true')
def initializeOpPoints(self, model):
for op in self.OperationalPoints:
model.createOpPoint(op, op)
def createCenterOfMassFeatureAndTask(self,
featureName, featureDesName,
taskName,
selec = '011',
gain = 1.):
self.dynamic.com.recompute(0)
self.dynamic.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(self.dynamic.com, featureCom.errorIN)
plug(self.dynamic.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = self.dynamic.com.value
featureCom.setReference(featureComDes.name)
comTask = Task(taskName)
comTask.add(featureName)
comTask.controlGain.value = gain
return (featureCom, featureComDes, comTask)
def createOperationalPointFeatureAndTask(self,
operationalPointName,
featureName,
taskName,
gain = .2):
jacobianName = 'J{0}'.format(operationalPointName)
self.dynamic.signal(operationalPointName).recompute(0)
self.dynamic.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
self.dynamic.signal(operationalPointName),
self.dynamic.signal(jacobianName),
self.dynamic.signal(operationalPointName).value)
task = Task(taskName)
task.add(featureName)
task.controlGain.value = gain
return (feature, task)
def createBalanceTask (self, taskName, gain = 1.):
task = Task (taskName)
task.add (self.featureCom.name)
task.add (self.leftAnkle.name)
task.add (self.rightAnkle.name)
task.controlGain.value = gain
return task
def createFrame(self, frameName, transformation, operationalPoint):
frame = OpPointModifier(frameName)
frame.setTransformation(transformation)
plug(self.dynamic.signal(operationalPoint),
frame.positionIN)
plug(self.dynamic.signal("J{0}".format(operationalPoint)),
frame.jacobianIN)
frame.position.recompute(frame.position.time + 1)
frame.jacobian.recompute(frame.jacobian.time + 1)
return frame
def initializeSignals (self):
"""
For portability, make some signals accessible as attributes.
"""
self.comRef = self.featureComDes.errorIN
self.zmpRef = self.device.zmp
self.com = self.dynamic.com
self.comSelec = self.featureCom.selec
self.comdot = self.featureComDes.errordotIN
def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
self.device.set(self.halfSitting)
self.dynamic.position.value = self.halfSitting
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension*(0.,)
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout,
self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension*(0.,)
"""
self.initializeOpPoints(self.dynamic)
# --- center of mass ------------
(self.featureCom, self.featureComDes, self.comTask) = \
self.createCenterOfMassFeatureAndTask(
'{0}_feature_com'.format(self.name),
'{0}_feature_ref_com'.format(self.name),
'{0}_task_com'.format(self.name))
# --- operational points tasks -----
self.features = dict()
self.tasks = dict()
for op in self.OperationalPoints:
(self.features[op], self.tasks[op]) = \
self.createOperationalPointFeatureAndTask(
op, '{0}_feature_{1}'.format(self.name, op),
'{0}_task_{1}'.format(self.name, op))
# define a member for each operational point
w = op.split('-')
memberName = w[0]
for i in w[1:]:
memberName += i.capitalize()
setattr(self, memberName, self.features[op])
self.tasks ['com'] = self.comTask
# --- balance task --- #
self.tasks ['balance'] =\
self.createBalanceTask ('{0}_task_balance'.format (self.name))
# --- additional frames ---
self.frames = dict()
frameName = 'rightHand'
hp = self.dynamic.getHandParameter (True)
transformation = list (map (list, I4))
for i in range (3): transformation [i][3] = hp [i][3]
transformation = tuple (map (tuple, transformation))
self.frames [frameName] = self.createFrame (
"{0}_{1}".format (self.name, frameName),
transformation, "right-wrist")
frameName = 'leftHand'
hp = self.dynamic.getHandParameter (False)
transformation = list (map (list, I4))
for i in range (3): transformation [i][3] = hp [i][3]
transformation = tuple (map (tuple, transformation))
self.frames [frameName] = self.createFrame (
"{0}_{1}".format (self.name, frameName),
self.dynamic.getHandParameter (False), "left-wrist")
for (frameName, transformation, signalName) in self.AdditionalFrames:
self.frames[frameName] = self.createFrame(
"{0}_{1}".format(self.name, frameName),
transformation,
signalName)
self.initializeSignals ()
"""
def addTrace(self, entityName, signalName):
if self.tracer:
self.autoRecomputedSignals.append(
'{0}.{1}'.format(entityName, signalName))
addTrace(self, self.tracer, entityName, signalName)
def initializeTracer(self):
if not self.tracer:
self.tracer = TracerRealTime('trace')
self.tracer.setBufferSize(self.tracerSize)
self.tracer.open('/tmp/','dg_','.dat')
# Recompute trace.triger at each iteration to enable tracing.
self.device.after.addSignal('{0}.triger'.format(self.tracer.name))
def traceDefaultSignals (self):
# Geometry / operational points
for s in self.OperationalPoints + self.tracedSignals['dynamic']:
self.addTrace(self.dynamic.name, s)
# Geometry / frames
for (frameName, _, _) in self.AdditionalFrames:
for s in ['position', 'jacobian']:
self.addTrace(self.frames[frameName].name, s)
# Robot features
for s in self.OperationalPoints:
self.addTrace(self.features[s]._reference.name, 'position')
self.addTrace(self.tasks[s].name, 'error')
# Com
self.addTrace(self.featureComDes.name, 'errorIN')
self.addTrace(self.comTask.name, 'error')
# Device
for s in self.tracedSignals['device']:
self.addTrace(self.device.name, s)
if type(self.device) != RobotSimu:
self.addTrace(self.device.name, 'robotState')
# Misc
if self.enableVelocityDerivator:
self.addTrace(self.velocityDerivator.name, 'sout')
if self.enableAccelerationDerivator:
self.addTrace(self.accelerationDerivator.name, 'sout')
def __init__(self, name, tracer = None):
self.name = name
# Initialize tracer if necessary.
if tracer:
self.tracer = tracer
def __del__(self):
if self.tracer:
self.stopTracer()
def startTracer(self):
"""
Start the tracer if it does not already been stopped.
"""
if self.tracer:
self.tracer.start()
def stopTracer(self):
"""
Stop and destroy tracer.
"""
if self.tracer:
self.tracer.dump()
self.tracer.stop()
self.tracer.close()
self.tracer.clear()
for s in self.autoRecomputedSignals:
self.device.after.rmSignal(s)
self.tracer = None
def reset(self, posture = None):
"""
Restart the control from another position.
This method has not been extensively tested and
should be used carefully.
In particular, tasks should be removed from the
solver before attempting a reset.
"""
if not posture:
posture = self.halfSitting
self.device.set(posture)
#self.dynamic.com.recompute(self.device.state.time+1)
#self.dynamic.Jcom.recompute(self.device.state.time+1)
#self.featureComDes.errorIN.value = self.dynamic.com.value
for op in self.OperationalPoints:
self.dynamic.signal(op).recompute(self.device.state.time+1)
self.dynamic.signal('J'+op).recompute(self.device.state.time+1)
self.features[op].reference.value = self.dynamic.signal(op).value
class LegsFollowerGraph(object):
legsFollower = None
postureTask = None
postureFeature = None
postureFeatureDes = None
postureError = None
legsTask = None
legsFeature = None
legsFeatureDes = None
legsError = None
waistTask = None
waistFeature = None
waistFeatureDes = None
waistError = None
trace = None
def __init__(self, robot, solver, trace = None, postureTaskDofs = None):
print("Constructor of LegsFollower Graph")
self.robot = robot
self.solver = solver
self.legsFollower = LegsFollower('legs-follower')
self.statelength = len(robot.device.state.value)
# Initialize the posture task.
print("Posture Task")
self.postureTaskDofs = postureTaskDofs
if not self.postureTaskDofs:
self.postureTaskDofs = []
for i in xrange(len(robot.halfSitting) - 6):
# Disable legs dofs.
if i < 12: #FIXME: not generic enough
self.postureTaskDofs.append((i + 6, False))
else:
self.postureTaskDofs.append((i + 6, True))
# This part is taken from feet_follower_graph
self.postureTask = Task(self.robot.name + '_posture')
self.postureFeature = FeaturePosture(self.robot.name + '_postureFeature')
plug(self.robot.device.state, self.postureFeature.state)
posture = list(self.robot.halfSitting)
self.postureFeature.setPosture(tuple(posture))
for (dof, isEnabled) in self.postureTaskDofs:
self.postureFeature.selectDof(dof, isEnabled)
self.postureTask.add(self.postureFeature.name)
self.postureTask.controlGain.value = 2.
# Initialize the waist follower task.
print("Waist Task")
self.robot.features['waist'].selec.value = '111111'
plug(self.legsFollower.waist, self.robot.features['waist'].reference)
self.robot.tasks['waist'].controlGain.value = 1.
# Initialize the legs follower task.
print("Legs Task")
self.legsTask = Task(self.robot.name + '_legs')
self.legsFeature = FeatureGeneric(self.robot.name + '_legsFeature')
legsFeatureDesName = self.robot.name + '_legsFeatureDes'
self.legsFeatureDes = FeatureGeneric(legsFeatureDesName)
self.legsError = LegsError('LegsError')
plug(self.robot.device.state, self.legsError.state)
# self.legsFeatureDes.errorIN.value = self.legsFollower.ldof.value
plug(self.legsFollower.ldof,self.legsFeatureDes.errorIN)
self.legsFeature.jacobianIN.value = self.legsJacobian()
self.legsFeature.setReference(legsFeatureDesName)
plug(self.legsError.error, self.legsFeature.errorIN)
self.legsTask.add(self.legsFeature.name)
self.legsTask.controlGain.value = 5.
#CoM task
print("Com Task")
print (0., 0., self.robot.dynamic.com.value[2])
self.robot.comTask.controlGain.value = 50.
self.robot.featureComDes.errorIN.value = (0., 0., self.robot.dynamic.com.value[2])
self.robot.featureCom.selec.value = '111'
plug(self.legsFollower.com, self.robot.featureComDes.errorIN)
# Plug the legs follower zmp output signals.
plug(self.legsFollower.zmp, self.robot.device.zmp)
solver.sot.remove(self.robot.comTask.name)
print("Push in solver.")
solver.sot.push(self.legsTask.name)
solver.sot.push(self.postureTask.name)
solver.sot.push(self.robot.tasks['waist'].name)
solver.sot.push(self.robot.comTask.name)
solver.sot.remove(self.robot.tasks['left-ankle'].name)
solver.sot.remove(self.robot.tasks['right-ankle'].name)
print solver.sot.display()
print("Tasks added in solver.\n")
print("Command are : \n - f.plug()\n - f.plugViewer()\n - f.plugPlanner()\n"
" - f.plugPlannerWithoutMocap()\n - f.start()\n - f.stop()\n - f.readMocap()\n")
def legsJacobian(self):
j = []
for i in xrange(12):
j.append(oneVector(6+i,self.statelength))
return tuple(j)
def waistJacobian(self):
j = []
for i in xrange(6):
j.append(oneVector(i,self.statelength))
return tuple(j)
def postureJacobian(self):
j = []
for i in xrange(self.statelength):
if i >= 6 + 2 * 6:
j.append(oneVector(i))
if i == 3 or i == 4:
j.append(zeroVector())
return tuple(j)
def computeDesiredValue(self):
e = self.robot.halfSitting
#e = halfSitting
e_ = [e[3], e[4]]
offset = 6 + 2 * 6
for i in xrange(len(e) - offset):
e_.append(e[offset + i])
return tuple(e_)
def canStart(self):
securityThreshold = 1e-3
return (self.postureTask.error.value <=
(securityThreshold,) * len(self.postureTask.error.value))
def setupTrace(self):
self.trace = TracerRealTime('trace')
self.trace.setBufferSize(2**20)
self.trace.open('/tmp/','legs_follower_','.dat')
self.trace.add('legs-follower.com', 'com')
self.trace.add('legs-follower.zmp', 'zmp')
self.trace.add('legs-follower.ldof', 'ldof')
self.trace.add('legs-follower.waist', 'waist')
self.trace.add(self.robot.device.name + '.state', 'state')
self.trace.add(self.legsTask.name + '.error', 'errorLegs')
self.trace.add(self.robot.comTask.name + '.error', 'errorCom')
#self.trace.add('legs-follower.outputStart','start')
#self.trace.add('legs-follower.outputYaw','yaw')
self.trace.add('corba.planner_steps','steps')
self.trace.add('corba.planner_outputGoal','goal')
self.trace.add('corba.waist','waistMocap')
self.trace.add('corba.left-foot','footMocap')
self.trace.add('corba.table','tableMocap')
self.trace.add('corba.bar','barMocap')
self.trace.add('corba.chair','chairMocap')
self.trace.add('corba.helmet','helmetMocap')
self.trace.add('corba.planner_outputObs','obstacles')
self.trace.add(self.robot.dynamic.name + '.left-ankle',
self.robot.dynamic.name + '-left-ankle')
self.trace.add(self.robot.dynamic.name + '.right-ankle',
self.robot.dynamic.name + '-right-ankle')
# Recompute trace.triger at each iteration to enable tracing.
self.robot.device.after.addSignal('legs-follower.zmp')
self.robot.device.after.addSignal('legs-follower.outputStart')
self.robot.device.after.addSignal('legs-follower.outputYaw')
self.robot.device.after.addSignal('corba.planner_steps')
self.robot.device.after.addSignal('corba.planner_outputGoal')
self.robot.device.after.addSignal('corba.waist')
self.robot.device.after.addSignal('corba.left-foot')
self.robot.device.after.addSignal('corba.table')
self.robot.device.after.addSignal('corba.bar')
self.robot.device.after.addSignal('corba.chair')
self.robot.device.after.addSignal('corba.helmet')
self.robot.device.after.addSignal('corba.planner_outputObs')
self.robot.device.after.addSignal(self.robot.dynamic.name + '.left-ankle')
self.robot.device.after.addSignal(self.robot.dynamic.name + '.right-ankle')
self.robot.device.after.addSignal('trace.triger')
return
def plugPlanner(self):
print("Plug planner.")
plug(corba.planner_radQ, self.legsFollower.inputRef)
plug(self.legsFollower.outputStart, corba.planner_inputStart)
plug(corba.waistTimestamp, corba.planner_timestamp)
plug(corba.table, corba.planner_table)
plug(corba.chair, corba.planner_chair)
plug(corba.bar, corba.planner_bar)
plug(corba.waist, corba.planner_waist)
plug(corba.helmet, corba.planner_inputGoal)
plug(corba.torus1, corba.planner_torus1)
plug(corba.torus2, corba.planner_torus2)
plug(corba.signal('left-foot'), corba.planner_foot)
plug(corba.signal('left-footTimestamp'), corba.planner_footTimestamp)
return
def plugPlannerWithoutMocap(self):
print("Plug planner without mocap.")
plug(corba.planner_radQ, self.legsFollower.inputRef)
plug(self.legsFollower.outputStart, corba.planner_inputStart)
return
def plugViewer(self):
print("Plug viewer.")
plug(self.legsFollower.ldof, corba.viewer_Ldof)
plug(self.legsFollower.outputStart, corba.viewer_Start)
plug(self.legsFollower.com, corba.viewer_Com)
plug(self.legsFollower.outputYaw, corba.viewer_Yaw)
plug(corba.planner_steps, corba.viewer_Steps)
plug(corba.planner_outputGoal, corba.viewer_Goal)
plug(corba.table, corba.viewer_Table)
plug(corba.chair, corba.viewer_Chair)
plug(corba.bar, corba.viewer_Bar)
plug(corba.torus1, corba.viewer_Torus1)
plug(corba.torus2, corba.viewer_Torus2)
plug(corba.waist, corba.viewer_Waist)
plug(corba.planner_outputLabyrinth, corba.viewer_Labyrinth)
plug(corba.planner_outputObs, corba.viewer_Obs)
return
def plug(self):
self.plugPlanner()
self.plugViewer()
return
def readMocap(self):
print "Table : "
print corba.table.value
print "Waist : "
if len(corba.waist.value)<3:
corba.waist.value = (0,0,0)
print corba.waist.value
print "Helmet : "
print corba.helmet.value
return;
def start(self):
if not self.canStart():
print("Robot has not yet converged to the initial position,"
" please wait and try again.")
return
print("Start.")
self.postureTask.controlGain.value = 180.
#self.waistTask.controlGain.value = 90.
self.legsTask.controlGain.value = 180.
self.robot.comTask.controlGain.value = 180.
self.robot.tasks['waist'].controlGain.value = 45.
self.setupTrace()
self.trace.start()
self.legsFollower.start()
return
def stop(self):
self.legsFollower.stop()
self.trace.dump()
return
class Factory(GraphFactoryAbstract):
class State:
def __init__(self, tasks, grasps, factory):
self.name = factory._stateName(grasps)
self.grasps = grasps
self.manifold = Manifold()
objectsAlreadyGrasped = {}
for ig, ih in enumerate(grasps):
if ih is not None:
# Add task gripper_close
self.manifold += tasks.g(factory.grippers[ig],
factory.handles[ih],
'gripper_close')
otherGrasp = objectsAlreadyGrasped.get(
factory.objectFromHandle[ih])
self.manifold += tasks.g(factory.grippers[ig],
factory.handles[ih], 'grasp',
otherGrasp)
objectsAlreadyGrasped[
factory.objectFromHandle[ih]] = tasks.g(
factory.grippers[ig], factory.handles[ih], 'grasp',
otherGrasp)
else:
# Add task gripper_open
self.manifold += tasks.g(factory.grippers[ig], None,
'gripper_open')
def __init__(self, supervisor):
super(Factory, self).__init__()
self.tasks = TaskFactory(self)
self.hpTasks = supervisor.hpTasks
self.lpTasks = supervisor.lpTasks
self.affordances = dict()
self.sots = dict()
self.postActions = dict()
self.preActions = dict()
self.timers = {}
self.supervisor = supervisor
self.addTimerToSotControl = False
def _newSoT(self, name):
sot = SOT(name)
sot.setSize(self.sotrobot.dynamic.getDimension())
sot.damping.value = 0.001
if self.addTimerToSotControl:
from .tools import insertTimerOnOutput
self.timers[name] = insertTimerOnOutput(sot.control, "vector")
self.SoTtracer.add(self.timers[name].name + ".timer",
str(len(self.timerIds)) + ".timer")
self.timerIds.append(name)
return sot
def addAffordance(self, aff):
assert isinstance(aff, Affordance)
self.affordances[(aff.gripper, aff.handle)] = aff
def generate(self):
if self.addTimerToSotControl:
# init tracer
from dynamic_graph.tracer_real_time import TracerRealTime
self.SoTtracer = TracerRealTime("tracer_of_timers")
self.timerIds = []
self.SoTtracer.setBufferSize(10 * 1048576) # 10 Mo
self.SoTtracer.open("/tmp", "sot-control-trace", ".txt")
self.sotrobot.device.after.addSignal("tracer_of_timers.triger")
self.supervisor.SoTtracer = self.SoTtracer
self.supervisor.SoTtimerIds = self.timerIds
super(Factory, self).generate()
self.supervisor.sots = self.sots
self.supervisor.grasps = {(gh, w): t
for gh, ts in self.tasks._grasp.items()
for w, t in ts.items()}
self.supervisor.hpTasks = self.hpTasks
self.supervisor.lpTasks = self.lpTasks
self.supervisor.postActions = self.postActions
self.supervisor.preActions = self.preActions
self.supervisor.SoTtimers = self.timers
def setupFrames(self, srdfGrippers, srdfHandles, sotrobot):
self.sotrobot = sotrobot
self.grippersIdx = {g: i for i, g in enumerate(self.grippers)}
self.handlesIdx = {h: i for i, h in enumerate(self.handles)}
self.gripperFrames = {
g: OpFrame(srdfGrippers[g], sotrobot.dynamic.model)
for g in self.grippers
}
self.handleFrames = {h: OpFrame(srdfHandles[h]) for h in self.handles}
def makeState(self, grasps, priority):
# Nothing to do here
return Factory.State(self.tasks, grasps, self)
def makeLoopTransition(self, state):
n = self._loopTransitionName(state.grasps)
sot = self._newSoT('sot_' + n)
self.hpTasks.pushTo(sot)
state.manifold.pushTo(sot)
self.lpTasks.pushTo(sot)
self.sots[n] = sot
def makeTransition(self, stateFrom, stateTo, ig):
sf = stateFrom
st = stateTo
names = self._transitionNames(sf, st, ig)
print "names: {}".format(names)
iobj = self.objectFromHandle[st.grasps[ig]]
obj = self.objects[iobj]
noPlace = self._isObjectGrasped(sf.grasps, iobj)
print "iobj, obj, noPlace: {}, {}, {}".format(iobj, obj, noPlace)
# The different cases:
pregrasp = True
intersec = not noPlace
preplace = not noPlace
# Start here
nWaypoints = pregrasp + intersec + preplace
nTransitions = 1 + nWaypoints
# Link waypoints
transitions = names[:]
assert nWaypoints > 0
M = 1 + pregrasp
sots = []
for i in range(nTransitions):
ns = ("{0}_{1}{2}".format(names[0], i, i + 1),
"{0}_{2}{1}".format(names[1], i, i + 1))
for n in ns:
s = self._newSoT('sot_' + n)
self.hpTasks.pushTo(s)
#if pregrasp and i == 1:
# Add pregrasp task
#self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'pregrasp').pushTo (s)
if i < M: sf.manifold.pushTo(s)
else: st.manifold.pushTo(s)
self.lpTasks.pushTo(s)
self.sots[n] = s
sots.append(n)
key = sots[2 * (M - 1)]
states = (st.name, names[0] + "_intersec")
sot = self._newSoT("postAction_" + key)
self.hpTasks.pushTo(sot)
# self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
st.manifold.pushTo(sot)
self.lpTasks.pushTo(sot)
# print sot
# TODO one post action is missing
if not self.postActions.has_key(key):
self.postActions[key] = dict()
for n in states:
self.postActions[key][n] = sot
key = sots[2 * (M - 1) + 1]
states = (st.name, names[0] + "_intersec")
sot = self._newSoT("preAction_" + key)
self.hpTasks.pushTo(sot)
# self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
sf.manifold.pushTo(sot)
self.lpTasks.pushTo(sot)
# print sot
self.preActions[key] = sot
self.preActions[sots[2 * (M - 1)]] = sot
class AdmittanceControl(object):
"""
The torque controller is then use to maintain a desired force.
It outputs a velocity command to be sent to entity Device.
"""
def __init__(self, name, estimated_theta_closed, desired_torque, period,
nums, denoms):
"""
- estimated_theta_closed: Use for the initial position control. It should correspond to a configuration in collision.
The closer to contact configuration, the least the overshoot.
- desired_torque: The torque to be applied on the object.
- period: The SoT integration time.
- nums, denoms: coefficient of the controller:
\sum_i denoms[i] * d^i theta / dt^i = \sum_j nums[j] d^j torque / dt^j
"""
self.name = name
self.est_theta_closed = estimated_theta_closed
self.desired_torque = desired_torque
self.dt = period
self._makeTorqueControl(nums, denoms)
self._makeIntegrationOfVelocity()
### Feed-forward - contact phase
def _makeTorqueControl(self, nums, denoms):
from agimus_sot.control.controllers import Controller
self.torque_controller = Controller(
self.name + "_torque", nums, denoms, self.dt,
[0. for _ in self.est_theta_closed])
self.torque_controller.addFeedback()
self.torque_controller.reference.value = self.desired_torque
### Internal method
def _makeIntegrationOfVelocity(self):
from dynamic_graph.sot.core import Add_of_vector
self.omega2theta = Add_of_vector(self.name + "_omega2theta")
self.omega2theta.setCoeff2(self.dt)
# self.omega2theta.sin1 <- current position
# self.omega2theta.sin2 <- desired velocity
plug(self.torque_controller.output, self.omega2theta.sin2)
### Setup event to tell when object is grasped and simulate torque feedback.
def setupFeedbackSimulation(self, mass, damping, spring, theta0):
from agimus_sot.control.controllers import Controller
from dynamic_graph.sot.core import Add_of_vector
from agimus_sot.sot import DelayVector
## theta -> phi = theta - theta0
self._sim_theta2phi = Add_of_vector(self.name + "_sim_theta2phi")
self._sim_theta2phi.setCoeff1(1)
self._sim_theta2phi.setCoeff2(-1)
self._sim_theta2phi.sin2.value = theta0
## phi -> torque
from dynamic_graph.sot.core.switch import SwitchVector
from dynamic_graph.sot.core.operator import CompareVector
# reverse = self.theta_open[0] > theta0[0]
reverse = self.desired_torque[0] < 0
self.sim_contact_condition = CompareVector(self.name +
"_sim_contact_condition")
self.sim_contact_condition.setTrueIfAny(False)
self._sim_torque = SwitchVector(self.name + "_sim_torque")
self._sim_torque.setSignalNumber(2)
plug(self.sim_contact_condition.sout, self._sim_torque.boolSelection)
# If phi < 0 (i.e. sim_contact_condition.sout == 1) -> non contact phase
# else contact phase
if reverse:
plug(self._sim_theta2phi.sout, self.sim_contact_condition.sin2)
self.sim_contact_condition.sin1.value = [
0. for _ in self.est_theta_closed
]
else:
plug(self._sim_theta2phi.sout, self.sim_contact_condition.sin1)
self.sim_contact_condition.sin2.value = [
0. for _ in self.est_theta_closed
]
# Non contact phase
# torque = 0, done above
# Contact phase
self.phi2torque = Controller(self.name + "_sim_phi2torque", (
spring,
damping,
mass,
), (1., ), self.dt, [0. for _ in self.est_theta_closed])
#TODO if M != 0: phi2torque.pushNumCoef(((M,),))
plug(self._sim_theta2phi.sout, self.phi2torque.reference)
# Condition
# if phi < 0 -> no contact -> torque = 0
self._sim_torque.sin1.value = [0. for _ in self.est_theta_closed]
# else -> contact -> phi2torque
plug(self.phi2torque.output, self._sim_torque.sin0)
plug(self._sim_torque.sout, self.currentTorqueIn)
def readPositionsFromRobot(self, robot, jointNames):
# TODO Compare current position to self.est_theta_closed and
# so as not to overshoot this position.
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._joint_selec = Selec_of_vector(self.name + "_joint_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
self._joint_selec.addSelec(jmodel.idx_v, jmodel.idx_v + jmodel.nv)
plug(robot.dynamic.position, self._joint_selec.sin)
self.setCurrentPositionIn(self._joint_selec.sout)
## - param torque_constants: Should take into account the motor torque constant and the gear ratio.
## - param first_order_filter: Add a first order filter to the current signal.
def readCurrentsFromRobot(self,
robot,
jointNames,
torque_constants,
first_order_filter=False):
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._current_selec = Selec_of_vector(self.name + "_current_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
# TODO there is no value for the 6 first DoF
assert jmodel.idx_v >= 6
self._current_selec.addSelec(jmodel.idx_v - 6,
jmodel.idx_v - 6 + jmodel.nv)
from dynamic_graph.sot.core.operator import Multiply_of_vector
plug(robot.device.currents, self._current_selec.sin)
self._multiply_by_torque_constants = Multiply_of_vector(
self.name + "_multiply_by_torque_constants")
self._multiply_by_torque_constants.sin0.value = torque_constants
if first_order_filter:
from agimus_sot.control.controllers import Controller
self._first_order_current_filter = Controller(
self.name + "_first_order_current_filter", (5., ), (
5.,
1.,
), self.dt, [0. for _ in self.desired_torque])
plug(self._current_selec.sout,
self._first_order_current_filter.reference)
plug(self._first_order_current_filter.output,
self._multiply_by_torque_constants.sin1)
else:
plug(self._current_selec.sout,
self._multiply_by_torque_constants.sin1)
plug(self._multiply_by_torque_constants.sout, self.currentTorqueIn)
def readTorquesFromRobot(self, robot, jointNames):
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._torque_selec = Selec_of_vector(self.name + "_torque_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
# TODO there is no value for the 6 first DoF
assert jmodel.idx_v >= 6
self._torque_selec.addSelec(jmodel.idx_v - 6,
jmodel.idx_v - 6 + jmodel.nv)
plug(robot.device.ptorques, self._torque_selec.sin)
plug(self._torque_selec.sout, self.currentTorqueIn)
# TODO remove me
def addOutputTo(self, robot, jointNames, mix_of_vector, sot=None):
#TODO assert isinstance(mix_of_vector, Mix_of_vector)
i = mix_of_vector.getSignalNumber()
mix_of_vector.setSignalNumber(i + 1)
plug(self.outputVelocity, mix_of_vector.signal("sin" + str(i)))
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
jmodel = model.joints[jid]
mix_of_vector.addSelec(i, jmodel.idx_v, jmodel.nv)
def addTracerRealTime(self, robot):
from dynamic_graph.tracer_real_time import TracerRealTime
from agimus_sot.tools import filename_escape
self._tracer = TracerRealTime(self.name + "_tracer")
self._tracer.setBufferSize(10 * 1048576) # 10 Mo
self._tracer.open("/tmp", filename_escape(self.name), ".txt")
self._tracer.add(self.omega2theta.name + ".sin1", "_theta_current")
self._tracer.add(self.omega2theta.name + ".sin2", "_omega_desired")
self._tracer.add(self.omega2theta.name + ".sout", "_theta_desired")
self._tracer.add(self.torque_controller.referenceName,
"_reference_torque")
self._tracer.add(self.torque_controller.measurementName,
"_measured_torque")
if hasattr(self, "_current_selec"):
self._tracer.add(self._current_selec.name + ".sout",
"_measured_current")
robot.device.after.addSignal(self._tracer.name + ".triger")
return self._tracer
@property
def outputPosition(self):
return self.omega2theta.sout
@property
def referenceTorqueIn(self):
return self.torque_controller.reference
def setCurrentPositionIn(self, sig):
plug(sig, self.omega2theta.sin1)
if hasattr(self, "_sim_theta2phi"):
plug(sig, self._sim_theta2phi.sin1)
@property
def currentTorqueIn(self):
return self.torque_controller.measurement
@property
def torqueConstants(self):
return self._multiply_by_torque_constants.sin0
class Robot(object):
"""
This class instantiates a robot
"""
init_pos = (0.0)
init_vel = (0.0)
init_acc = (0.0)
"""
Tracer used to log data.
"""
tracer = None
"""
How much data will be logged.
"""
tracerSize = 2**22
"""
Automatically recomputed signals through the use
of device.after.
This list is maintained in order to clean the
signal list device.after before exiting.
"""
autoRecomputedSignals = []
"""
Robot timestep
"""
timeStep = 0.005
def __init__(self, name, device=None, tracer=None):
self.name = name
self.device = device
# Initialize tracer if necessary.
if tracer:
self.tracer = tracer
self.initialize_tracer()
# We trace by default all signals of the device.
self.device_signals_names = []
for signal in self.device.signals():
signal_name = signal.name.split('::')[-1]
self.add_trace(self.device.name, signal_name)
self.device_signals_names.append(signal_name)
# Prepare potential ros import/export
self.ros = Ros(self.device)
# self.export_device_dg_to_ros()
def __del__(self):
if self.tracer:
self.stop_tracer()
def add_trace(self, entityName, signalName):
if self.tracer:
addTrace(self, self.tracer, entityName, signalName)
def _tracer_log_dir(self):
import os
import os.path
import time
log_dir = os.path.join(os.path.expanduser("~"),
"dynamic_graph_manager",
time.strftime("%Y-%m-%d_%H-%M-%S"))
if not os.path.exists(log_dir):
os.makedirs(log_dir)
return log_dir
def initialize_tracer(self):
"""
Initialize the tracer and by default dump the files in
~/dynamic_graph/[date_time]/
"""
if not self.tracer:
self.tracer = TracerRealTime('trace')
self.tracer.setBufferSize(self.tracerSize)
# Recompute trace.triger at each iteration to enable tracing.
self.device.after.addSignal('{0}.triger'.format(self.tracer.name))
def start_tracer(self):
"""
Start the tracer if it has not already been stopped.
"""
if self.tracer:
self.tracer_log_dir = self._tracer_log_dir()
self.tracer.open(self.tracer_log_dir, 'dg_', '.dat')
self.tracer.start()
def stop_tracer(self):
"""
Stop and destroy tracer.
"""
if self.tracer:
self.tracer.dump()
self.tracer.stop()
self.tracer.close()
print("Stored trace in:", self.tracer_log_dir)
# NOTE: Not calling self.tracer.clear() here, as by default the
# tracer should keep it's traced signals attached.
# Null the tracer object, such that initialize_tracer() will reopen it.
self.trace = None
self.initialize_tracer()
def add_to_ros(self,
entityName,
signalName,
topic_name=None,
topic_type=None):
"""
arg: topic_type is a string among:
['double', 'matrix', 'vector', 'vector3', 'vector3Stamped',
'matrixHomo', 'matrixHomoStamped', 'twist', 'twistStamped',
'joint_states'].
Each different strings correspond to a ros message. For rviz
support please use joint_states which correspond to the joint
states including the potential free flyer joint.
"""
# Lookup the entity's signal by name
signal = Entity.entities[entityName].signal(signalName)
if topic_name is None:
topic_name = "/dg__" + entityName + '__' + signalName
new_signal_name = "dg__" + entityName + '__' + signalName
if topic_type is None:
topic_type = "vector"
self.ros.rosPublish.add(topic_type, new_signal_name, topic_name)
plug(signal, self.ros.rosPublish.signal(new_signal_name))
def add_robot_state_to_ros(self, entity_name, signal_name, base_link_name,
joint_names, tf_prefix, joint_state_topic_name):
# Lookup the entity's signal by name
signal = Entity.entities[entity_name].signal(signal_name)
new_signal_name = "dg__" + entity_name + '__' + signal_name
joint_names_string = ""
for s in joint_names:
joint_names_string += s + " "
self.ros.rosRobotStatePublisher.add(
base_link_name,
joint_names_string,
tf_prefix,
new_signal_name,
joint_state_topic_name,
)
plug(signal, self.ros.rosRobotStatePublisher.signal(new_signal_name))
def add_ros_and_trace(self,
entityName,
signalName,
topic_name=None,
topic_type=None):
self.add_trace(entityName, signalName)
self.add_to_ros(entityName,
signalName,
topic_name=topic_name,
topic_type=topic_type)
def export_device_dg_to_ros(self):
"""
Import in ROS the signal from the dynamic graph device.
"""
for sig_name in self.device_signals_names:
self.add_to_ros(self.device.name, sig_name)
# Create task for the wrist3
I4 = (
(1., 0, 0, 0.321),
(0, 1., 0, 0.109),
(0, 0, 1., 0.848),
(0, 0, 0, 1.),
)
feature_wrist = FeaturePosition('position_wrist', robot.dynamic.wrist_3_joint,
robot.dynamic.Jwrist_3_joint, I4)
task_wrist = Task('wrist_task')
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)
class AbstractRobot(ABC):
"""
This class instantiates all the entities required to get a consistent
representation of a robot, mainly:
- device : to integrate velocities into angular control,
- dynamic: to compute forward geometry and kinematics,
- zmpFromForces: to compute ZMP force foot force sensors,
- stabilizer: to stabilize balanced motions
Operational points are stored into 'OperationalPoints' list. Some of them
are also accessible directly as attributes:
- leftWrist,
- rightWrist,
- leftAnkle,
- rightAnkle,
- Gaze.
Operational points are mapped to the actual joints in the robot model
via 'OperationalPointsMap' dictionary.
This attribute *must* be defined in the subclasses
Other attributes require to be defined:
- halfSitting: half-sitting position is the robot initial pose.
This attribute *must* be defined in subclasses.
- dynamic: The robot dynamic model.
- device: The device that integrates the dynamic equation, namely
the real robot or
a simulator
- dimension: The configuration size.
"""
def _initialize(self):
self.OperationalPoints = []
"""
Operational points are specific interesting points of the robot
used to control it.
When an operational point is defined, signals corresponding to the
point position and jacobian are created.
For instance if creating an operational point for the left-wrist,
the associated signals will be called "left-wrist" and
"Jleft-wrist" for respectively the position and the jacobian.
"""
self.AdditionalFrames = []
"""
Additional frames are frames which are defined w.r.t an operational point
and provides an interesting transformation.
It can be used, for instance, to store the sensor location.
The contained elements must be triplets matching:
- additional frame name,
- transformation w.r.t to the operational point,
- operational point file.
"""
self.frames = dict()
"""
Additional frames defined by using OpPointModifier.
"""
# FIXME: the following options are /not/ independent.
# zmp requires acceleration which requires velocity.
"""
Enable velocity computation.
"""
self.enableVelocityDerivator = False
"""
Enable acceleration computation.
"""
self.enableAccelerationDerivator = False
"""
Enable ZMP computation
"""
self.enableZmpComputation = False
"""
Tracer used to log data.
"""
self.tracer = None
"""
How much data will be logged.
"""
self.tracerSize = 2**20
"""
Automatically recomputed signals through the use
of device.after.
This list is maintained in order to clean the
signal list device.after before exiting.
"""
self.autoRecomputedSignals = []
"""
Which signals should be traced.
"""
self.tracedSignals = {
'dynamic': [
"com", "zmp", "angularmomentum", "position", "velocity",
"acceleration"
],
'device': ['zmp', 'control', 'state']
}
def help(self):
print(AbstractHumanoidRobot.__doc__)
def _removeMimicJoints(self, urdfFile=None, urdfString=None):
""" Parse the URDF, extract the mimic joints and call removeJoints. """
# get mimic joints
import xml.etree.ElementTree as ET
if urdfFile is not None:
assert urdfString is None, "One and only one of input argument should be provided"
root = ET.parse(urdfFile)
else:
assert urdfString is not None, "One and only one of input argument should be provided"
root = ET.fromstring(urdfString)
mimicJoints = list()
for e in root.iter('joint'):
if 'name' in e.attrib:
name = e.attrib['name']
for c in e:
if hasattr(c, 'tag') and c.tag == 'mimic':
mimicJoints.append(name)
self.removeJoints(mimicJoints)
def removeJoints(self, joints):
"""
- param joints: a list of joint names to be removed from the self.pinocchioModel
"""
jointIds = list()
for j in joints:
if self.pinocchioModel.existJointName(j):
jointIds.append(self.pinocchioModel.getJointId(j))
if len(jointIds) > 0:
q = pinocchio.neutral(self.pinocchioModel)
self.pinocchioModel = pinocchio.buildReducedModel(
self.pinocchioModel, jointIds, q)
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
def loadModelFromString(self,
urdfString,
rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True):
""" Load a URDF model contained in a string
- param rootJointType: the root joint type. None for no root joint.
- param removeMimicJoints: if True, all the mimic joints found in the model are removed.
"""
if rootJointType is None:
self.pinocchioModel = pinocchio.buildModelFromXML(urdfString)
else:
self.pinocchioModel = pinocchio.buildModelFromXML(
urdfString, rootJointType())
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
if removeMimicJoints:
self._removeMimicJoints(urdfString=urdfString)
def loadModelFromUrdf(self,
urdfPath,
urdfDir=None,
rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True):
"""
Load a model using the pinocchio urdf parser. This parser looks
for urdf files in which kinematics and dynamics information
have been added.
- param urdfPath: a path to the URDF file.
- param urdfDir: A list of directories. If None, will use ROS_PACKAGE_PATH.
"""
if urdfPath.startswith("package://"):
from os import path
n1 = 10 # len("package://")
n2 = urdfPath.index(path.sep, n1)
pkg = urdfPath[n1:n2]
relpath = urdfPath[n2 + 1:]
import rospkg
rospack = rospkg.RosPack()
abspkg = rospack.get_path(pkg)
urdfFile = path.join(abspkg, relpath)
else:
urdfFile = urdfPath
if urdfDir is None:
import os
urdfDir = os.environ["ROS_PACKAGE_PATH"].split(':')
if rootJointType is None:
self.pinocchioModel = pinocchio.buildModelFromUrdf(urdfFile)
else:
self.pinocchioModel = pinocchio.buildModelFromUrdf(
urdfFile, rootJointType())
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
if removeMimicJoints:
self._removeMimicJoints(urdfFile=urdfFile)
def initializeOpPoints(self):
for op in self.OperationalPoints:
self.dynamic.createOpPoint(op, self.OperationalPointsMap[op])
def createFrame(self, frameName, transformation, operationalPoint):
frame = OpPointModifier(frameName)
frame.setTransformation(transformation)
plug(self.dynamic.signal(operationalPoint), frame.positionIN)
plug(self.dynamic.signal("J{0}".format(operationalPoint)),
frame.jacobianIN)
frame.position.recompute(frame.position.time + 1)
frame.jacobian.recompute(frame.jacobian.time + 1)
return frame
def setJointValueInConfig(self, q, jointNames, jointValues):
"""
q: configuration to update
jointNames: list of existing joint names in self.pinocchioModel
jointValues: corresponding joint values.
"""
model = self.pinocchioModel
for jn, jv in zip(jointNames, jointValues):
assert model.existJointName(jn)
joint = model.joints[model.getJointId(jn)]
q[joint.idx_q] = jv
@abstractmethod
def defineHalfSitting(self, q):
"""
Define half sitting configuration using the pinocchio Model (i.e.
with quaternions and not with euler angles).
method setJointValueInConfig may be usefull to implement this function.
"""
pass
def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not hasattr(self, 'dynamic'):
raise RuntimeError("Dynamic robot model must be initialized first")
if not hasattr(self, 'device') or self.device is None:
# raise RuntimeError("A device is already defined.")
self.device = RobotSimu(self.name + '_device')
self.device.resize(self.dynamic.getDimension())
"""
Robot timestep
"""
self.timeStep = self.device.getTimeStep()
# Compute half sitting configuration
import numpy
"""
Half sitting configuration.
"""
self.halfSitting = pinocchio.neutral(self.pinocchioModel)
self.defineHalfSitting(self.halfSitting)
self.halfSitting = numpy.array(
self.halfSitting[:3].tolist() +
[0., 0., 0.] # Replace quaternion by RPY.
+ self.halfSitting[7:].tolist())
assert self.halfSitting.shape[0] == self.dynamic.getDimension()
# Set the device limits.
def get(s):
s.recompute(0)
return s.value
def opposite(v):
return [-x for x in v]
self.dynamic.add_signals()
self.device.setPositionBounds(get(self.dynamic.lowerJl),
get(self.dynamic.upperJl))
self.device.setVelocityBounds(-get(self.dynamic.upperVl),
get(self.dynamic.upperVl))
self.device.setTorqueBounds(-get(self.dynamic.upperTl),
get(self.dynamic.upperTl))
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = numpy.zeros([
self.dimension,
])
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = numpy.zeros([
self.dimension,
])
def addTrace(self, entityName, signalName):
if self.tracer:
self.autoRecomputedSignals.append('{0}.{1}'.format(
entityName, signalName))
addTrace(self, self.tracer, entityName, signalName)
def initializeTracer(self):
if not self.tracer:
self.tracer = TracerRealTime('trace')
self.tracer.setBufferSize(self.tracerSize)
self.tracer.open('/tmp/', 'dg_', '.dat')
# Recompute trace.triger at each iteration to enable tracing.
self.device.after.addSignal('{0}.triger'.format(self.tracer.name))
def traceDefaultSignals(self):
# Geometry / operational points
for s in self.OperationalPoints + self.tracedSignals['dynamic']:
self.addTrace(self.dynamic.name, s)
# Geometry / frames
for (frameName, _, _) in self.AdditionalFrames:
for s in ['position', 'jacobian']:
self.addTrace(self.frames[frameName].name, s)
# Device
for s in self.tracedSignals['device']:
self.addTrace(self.device.name, s)
if type(self.device) != RobotSimu:
self.addTrace(self.device.name, 'robotState')
# Misc
if self.enableVelocityDerivator:
self.addTrace(self.velocityDerivator.name, 'sout')
if self.enableAccelerationDerivator:
self.addTrace(self.accelerationDerivator.name, 'sout')
def __init__(self, name, tracer=None):
self._initialize()
self.name = name
# Initialize tracer if necessary.
if tracer:
self.tracer = tracer
def __del__(self):
if self.tracer:
self.stopTracer()
def startTracer(self):
"""
Start the tracer if it does not already been stopped.
"""
if self.tracer:
self.tracer.start()
def stopTracer(self):
"""
Stop and destroy tracer.
"""
if self.tracer:
self.tracer.dump()
self.tracer.stop()
self.tracer.close()
self.tracer.clear()
for s in self.autoRecomputedSignals:
self.device.after.rmSignal(s)
self.tracer = None
def reset(self, posture=None):
"""
Restart the control from another position.
This method has not been extensively tested and
should be used carefully.
In particular, tasks should be removed from the
solver before attempting a reset.
"""
if not posture:
posture = self.halfSitting
self.device.set(posture)
self.dynamic.com.recompute(self.device.state.time + 1)
self.dynamic.Jcom.recompute(self.device.state.time + 1)
for op in self.OperationalPoints:
self.dynamic.signal(self.OperationalPointsMap[op]).recompute(
self.device.state.time + 1)
self.dynamic.signal('J' + self.OperationalPointsMap[op]).recompute(
self.device.state.time + 1)
class AbstractMobileRobot(object):
OperationalPoints = []
AdditionalFrames = []
name = None
initPosition = None
tracer = None
tracerSize = 2**20
autoRecomputedSignals = []
tracedSignals = {
'dynamic': ["position", "velocity"],
'device': ['control', 'state']
}
# initialize robot
def initializeRobot(self):
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
self.device.set(self.initPosition)
plug(self.device.state, self.dynamic.position)
self.dynamic.velocity.value = self.dimension*(0.,)
self.dynamic.acceleration.value = self.dimension*(0.,)
self.initializeOpPoints(self.dynamic)
# create operational points
def initializeOpPoints(self, model):
for op in self.OperationalPoints:
model.createOpPoint(op, op)
# Tracer methods
def addTrace(self, entityName, signalName):
if self.tracer:
self.autoRecomputedSignals.append(
'{0}.{1}'.format(entityName, signalName))
addTrace(self, self.tracer, entityName, signalName)
def startTracer(self):
"""
Start the tracer if it does not already been stopped.
"""
if self.tracer:
self.tracer.start()
def stopTracer(self):
"""
Stop and destroy tracer.
"""
if self.tracer:
self.tracer.dump()
self.tracer.stop()
self.tracer.close()
self.tracer.clear()
for s in self.autoRecomputedSignals:
self.device.after.rmSignal(s)
self.tracer = None
def initializeTracer(self,robotname):
self.tracer = 0
self.tracer = TracerRealTime('trace')
self.tracer.setBufferSize(self.tracerSize)
self.tracer.open(robotname,'dg_','.dat')
# Recompute trace.triger at each iteration to enable tracing.
self.device.after.addSignal('{0}.triger'.format(self.tracer.name))
def traceDefaultSignals (self):
# Geometry / operational points
for s in self.OperationalPoints + self.tracedSignals['dynamic']:
self.addTrace(self.dynamic.name, s)
# Device
for s in self.tracedSignals['device']:
self.addTrace(self.device.name, s)
if type(self.device) != RobotSimu:
self.addTrace(self.device.name, 'robotState')
# const and deconst
def __init__(self, name, tracer = None):
self.name = name
# Initialize tracer if necessary.
if tracer:
self.tracer = tracer
def __del__(self):
if self.tracer:
self.stopTracer()