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 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 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 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']
}
"""
Robot timestep
"""
self.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_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')
# 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 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)
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)
task_wrist.controlGain.value = 1.
task_wrist.add(feature_wrist.name)
# Create operational point for the end effector
model.createOpPoint("ee", "ee_fixed_joint")
solver = SOT('solver')
solver.setSize(dimension)
solver.push(task_wrist.name)
plug(solver.control, device.control)
device.increment(0.01)
#Create tracer
tracer = TracerRealTime('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/', 'dg_', '.dat')
# Make sure signals are recomputed even if not used in the control graph
device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace(device, tracer, device.name, "state")
addTrace(device, tracer, feature_wrist._feature.name, "position")
addTrace(device, tracer, feature_wrist._reference.name, "position")
#writeGraph('/tmp/graph')
dt = .01
tracer.start()
for i in range(1000):
device.increment(dt)
tracer.stop()
tracer.dump()
task_wrist.add (feature_wrist.name)
# Create operational point for the end effector
model.createOpPoint ("ee", "ee_fixed_joint")
solver = SOT ('solver')
solver.setSize (dimension)
solver.push (task_wrist.name)
plug (solver.control, device.control)
device.increment (0.01)
#Create tracer
tracer = TracerRealTime ('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/','dg_','.dat')
# Make sure signals are recomputed even if not used in the control graph
device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace (device, tracer, device.name, "state")
addTrace (device, tracer, feature_wrist._feature.name, "position")
addTrace (device, tracer, feature_wrist._reference.name, "position")
#writeGraph('/tmp/graph')
dt = .01
tracer.start ()
for i in range (1000):
device.increment (dt)
tracer.stop ()
tracer.dump ()