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)
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.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.0,)
self.initializeOpPoints(self.dynamic)
# --- 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
)
class HumanoidRobot(AbstractHumanoidRobot):
def __init__(self,
name,
pinocchio_model,
pinocchio_data,
initialConfig,
OperationalPointsMap=None,
tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = OperationalPointsMap
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(pinocchio_model)
self.dynamic.setData(pinocchio_data)
self.dimension = self.dynamic.getDimension()
self.device = RobotSimu(self.name + "_device")
self.device.resize(self.dynamic.getDimension())
self.halfSitting = initialConfig
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., )
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
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.,)
class HumanoidRobot(AbstractHumanoidRobot):
def __init__(self, name, pinocchio_model, pinocchio_data, initialConfig, OperationalPointsMap = None, tracer = None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = OperationalPointsMap
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(pinocchio_model)
self.dynamic.setData(pinocchio_data)
self.dimension = self.dynamic.getDimension()
self.device = RobotSimu(self.name + "_device")
self.device.resize(self.dynamic.getDimension())
self.halfSitting = initialConfig
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.,)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
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)
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)
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(self.dynamic)
# --- 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)
signal = '{0}.{1}'.format(entityName, signalName)
filename = '{0}-{1}'.format(entityName, signalName)
trace.add(signal, filename)
if autoRecompute:
device.after.addSignal(signal)
I4 = (
(1., 0, 0, 0),
(0, 1., 0, 0),
(0, 0, 1., 0),
(0, 0, 0, 1.),
)
model = RosRobotModel('ur5_dynamic')
device = RobotSimu('ur5_device')
rospy.init_node('fake')
model.loadUrdf(
"file:///local/ngiftsun/devel/ros/catkin_ws/src/ur_description/urdf/ur5_robot.urdf"
)
dimension = model.getDimension()
device.resize(dimension)
plug(device.state, model.position)
# Set velocity and acceleration to 0
model.velocity.value = dimension * (0., )
model.acceleration.value = dimension * (0., )
# Create taks for the base
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
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
# --- additional frames ---
self.frames = dict()
for (frameName, transformation, signalName) in self.AdditionalFrames:
self.frames[frameName] = self.createFrame(
"{0}_{1}".format(self.name, frameName),
transformation,
signalName)
self.initializeSignals ()
class AbstractMobileRobot(object):
OperationalPoints = []
AdditionalFrames = []
name = None
initPosition = None
# 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
# 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()
a = 2
self.initPosition = (0., ) * self.dimension
# initialize ur robot
self.initializeRobot()
__all__ = ["Ur"]
#### demo code ####
# 1. Instanciate a Pr2
# The URDF description of the robot must have
# been loaded in robot_description parameter
# on the Ros Parameter Server
from dynamic_graph.sot.pr2.robot import Pr2
from dynamic_graph.sot.core import RobotSimu
from dynamic_graph import plug
robot = youbot('youbot', device=RobotSimu('youbot'))
plug(robot.device.state, robot.dynamic.position)
# 2. Ros binding
# roscore must be running
from dynamic_graph.ros import Ros
ros = Ros(robot)
# 3. Create a solver
from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver
solver = Solver(robot)
# 4. Define a position task for the right hand
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d
from numpy import eye
from dynamic_graph.sot.core.matrix_util import matrixToTuple
"""
Add a signal to a tracer and recompute it automatically if necessary.
"""
signal = '{0}.{1}'.format(entityName, signalName)
filename = '{0}-{1}'.format(entityName, signalName)
trace.add(signal, filename)
if autoRecompute:
device.after.addSignal(signal)
I4 = ((1.,0,0,0),
(0,1.,0,0),
(0,0,1.,0),
(0,0,0,1.),)
model = RosRobotModel ('ur5_dynamic')
device = RobotSimu ('ur5_device')
rospy.init_node('fake')
model.loadUrdf ("file:///local/ngiftsun/devel/ros/catkin_ws/src/ur_description/urdf/ur5_robot.urdf")
dimension = model.getDimension ()
device.resize (dimension)
plug (device.state, model.position)
# Set velocity and acceleration to 0
model.velocity.value = dimension * (0.,)
model.acceleration.value = dimension * (0.,)
# Create taks for the base
model.createOpPoint ("base", "waist")
# 0. TRICK: import Dynamic as the first command to avoid the crash at the exit
from dynamic_graph.sot.dynamics import Dynamic
# 1. Instanciate a Pr2
# The URDF description of the robot must have
# been loaded in robot_description parameter
# on the Ros Parameter Server
# 1. Init robot, ros binding, solver
from dynamic_graph.sot.pr2.pr2_tasks import *
from dynamic_graph.sot.pr2.robot import *
from dynamic_graph.sot.core import RobotSimu
from dynamic_graph import plug
robot = Pr2('PR2', device=RobotSimu('PR2'))
plug(robot.device.state, robot.dynamic.position)
# 2. Ros binding
# roscore must be running
from dynamic_graph.ros import Ros
ros = Ros(robot)
# Use kine solver (with inequalities)
solver = initialize(robot)
# 4. Define a position task for the right hand
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d
from numpy import eye
from dynamic_graph.sot.core.matrix_util import matrixToTuple
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'right-wrist')
Pr2handMgrip = eye(4)
Pr2handMgrip[0:3, 3] = (0.18, 0, 0)
taskRH.opmodif = matrixToTuple(Pr2handMgrip)
class AbstractMobileRobot(object):
OperationalPoints = []
AdditionalFrames = []
name = None
initPosition = None
# 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
# 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()
# -*- coding: utf-8 -*-
"""
Created on Mon Oct 21 11:34:27 2013
@author: bcoudrin
"""
from dynamic_graph import plug
from dynamic_graph.sot.core import RobotSimu
from dynamic_graph.sot.ur.robot import Ur
from dynamic_graph.sot.dyninv import SolverKine
from dynamic_graph.sot.core.meta_task_6d import toFlags
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.dyninv import TaskJointLimits
#from dynamic_graph.ros.ros_sot_robot_model import Ros
robot = Ur('Ur', device=RobotSimu('ur'))
plug(robot.device.state, robot.dynamic.position)
#ros = Ros(robot)
def toList(sot):
return map(lambda x: x[1:-1], sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)
robot.dynamic.velocity.value = robot.dimension * (0., )
robot.dynamic.acceleration.value = robot.dimension * (0., )
robot.dynamic.ffposition.unplug()
def initializeUrRobot(self):
"""
initialize ur robot
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
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)
def __init__(self, name, device=None, tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.device = device
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
#self.specifySpecialLinks()
self.dynamic.loadFromParameterServer()
self.dimension = self.dynamic.getDimension()
self.initializeUrRobot()
__all__ = ["Ur"]
##########################
####### demo code ########
##########################
robot = Ur('Ur5', device=RobotSimu('Ur5'))
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()