Skip to content
Snippets Groups Projects
Commit 439ad4c4 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Make python scripts more compact,

    - by calling signals through Entity instance members instead of by
      signal('name'),
    - by replacing dynamicRobot member by dynamic,
    - by replacing simu flag by simulation,
    - by replacing robotSimu member by simu
    - by defining members for each operational point:
        AbstractHumanoidRobot.features['right-wrist'] can be accessed by
	AbstractHumanoidRobot.rightWrist for instance.
parent 0e2ad7d0
No related branches found
No related tags found
1 merge request!1[major][cpp] starting point to integrate pinocchio
...@@ -62,20 +62,20 @@ class Hrp2(AbstractHumanoidRobot): ...@@ -62,20 +62,20 @@ class Hrp2(AbstractHumanoidRobot):
return res return res
def __init__(self, name, def __init__(self, name,
simu, simulation,
modelDir = hrp2_14_pkgdatarootdir, modelDir = hrp2_14_pkgdatarootdir,
xmlDir = hrp2_14_pkgdatarootdir): xmlDir = hrp2_14_pkgdatarootdir):
AbstractHumanoidRobot.__init__ (self, name, simu) AbstractHumanoidRobot.__init__ (self, name, simulation)
modelName = 'HRP2JRLmainsmall.wrl' modelName = 'HRP2JRLmainsmall.wrl'
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml' specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml' jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
self.dynamicRobot = DynamicHrp2(self.name + '.dynamics') self.dynamic = DynamicHrp2(self.name + '.dynamics')
self.dynamicRobot.setFiles(modelDir, modelName, self.dynamic.setFiles(modelDir, modelName,
specificitiesPath, jointRankPath) specificitiesPath, jointRankPath)
self.dynamicRobot.parse() self.dynamic.parse()
self.dimension = self.dynamicRobot.getDimension() self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting): if self.dimension != len(self.halfSitting):
raise "invalid half-sitting pose" raise "invalid half-sitting pose"
self.initializeRobot() self.initializeRobot()
...@@ -59,7 +59,7 @@ class AbstractHumanoidRobot (object): ...@@ -59,7 +59,7 @@ class AbstractHumanoidRobot (object):
"""Entity name (internal use)""" """Entity name (internal use)"""
#FIXME: it should be some kind of global flag instead. #FIXME: it should be some kind of global flag instead.
simu = False simulation = False
"""Are we in simulation or not?""" """Are we in simulation or not?"""
halfSitting = None halfSitting = None
...@@ -68,7 +68,7 @@ class AbstractHumanoidRobot (object): ...@@ -68,7 +68,7 @@ class AbstractHumanoidRobot (object):
This attribute *must* be defined in subclasses. This attribute *must* be defined in subclasses.
""" """
dynamicRobot = None dynamic = None
""" """
The robot dynamic model. The robot dynamic model.
""" """
...@@ -152,66 +152,72 @@ class AbstractHumanoidRobot (object): ...@@ -152,66 +152,72 @@ class AbstractHumanoidRobot (object):
- the center of mass task used to keep the robot stability - the center of mass task used to keep the robot stability
- one task per operational point to ease robot control - one task per operational point to ease robot control
""" """
if not self.dynamicRobot: if not self.dynamic:
raise "robots models have to be initialized first" raise "robots models have to be initialized first"
if self.simu: if self.simulation:
self.robotSimu = RobotSimu(self.name + '.robotSimu') self.simu = RobotSimu(self.name + '.simu')
# Freeflyer reference frame should be the same as global # Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to # frame so that operational point positions correspond to
# position in freeflyer frame. # position in freeflyer frame.
self.robotSimu.set(self.halfSitting) self.simu.set(self.halfSitting)
self.dynamicRobot.signal('position').value = self.halfSitting self.dynamic.position.value = self.halfSitting
self.dynamicRobot.signal('velocity').value = self.dimension*(0.,) self.dynamic.velocity.value = self.dimension*(0.,)
self.dynamicRobot.signal('acceleration').value = self.dimension*(0.,) self.dynamic.acceleration.value = self.dimension*(0.,)
self.initializeOpPoints(self.dynamicRobot, self.initializeOpPoints(self.dynamic,
self.name + '.dynamics') self.name + '.dynamics')
# --- center of mass ------------ # --- center of mass ------------
self.featureCom = FeatureGeneric(self.name + '.feature.com') self.featureCom = FeatureGeneric(self.name + '.feature.com')
plug(self.dynamicRobot.signal('com'), self.featureCom.signal('errorIN')) plug(self.dynamic.com, self.featureCom.errorIN)
plug(self.dynamicRobot.signal('Jcom'), plug(self.dynamic.Jcom,
self.featureCom.signal('jacobianIN')) self.featureCom.jacobianIN)
self.featureCom.signal('selec').value = '011' self.featureCom.selec.value = '011'
self.featureComDes = FeatureGeneric(self.name + '.feature.ref.com') self.featureComDes = FeatureGeneric(self.name + '.feature.ref.com')
self.featureComDes.signal('errorIN').value = (.0, .0, 0.) self.featureComDes.errorIN.value = (.0, .0, 0.)
self.featureCom.signal('sdes').value = self.featureComDes self.featureCom.sdes.value = self.featureComDes
self.comTask = Task(self.name + '.task.com') self.comTask = Task(self.name + '.task.com')
self.comTask.add(self.name + '.feature.com') self.comTask.add(self.name + '.feature.com')
self.comTask.signal('controlGain').value = 1. self.comTask.controlGain.value = 1.
# --- operational points tasks ----- # --- operational points tasks -----
self.features = dict() self.features = dict()
self.tasks = dict() self.tasks = dict()
for op in self.OperationalPoints: for op in self.OperationalPoints:
self.dynamicRobot.signal(op).recompute(0) self.dynamic.signal(op).recompute(0)
self.features[op] = \ self.features[op] = \
FeaturePosition(self.name + '.feature.' + op, FeaturePosition(self.name + '.feature.' + op,
self.dynamicRobot.signal(op), self.dynamic.signal(op),
self.dynamicRobot.signal('J' + op), self.dynamic.signal('J' + op),
self.dynamicRobot.signal(op).value) self.dynamic.signal(op).value)
self.tasks[op] = Task(self.name + '.task.' + op) self.tasks[op] = Task(self.name + '.task.' + op)
self.tasks[op].add(self.name + '.feature.' + op) self.tasks[op].add(self.name + '.feature.' + op)
self.tasks[op].signal('controlGain').value = .2 self.tasks[op].controlGain.value = .2
# 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])
def __init__(self, name, simu): def __init__(self, name, simulation):
self.name = name self.name = name
if simu: if simulation:
self.simu = True self.simulation = True
else: else:
self.simu = False self.simulation = False
def restart(self): def restart(self):
if self.robotSimu: if self.simu:
self.robotSimu.set(self.halfSitting) self.simu.set(self.halfSitting)
self.dynamicRobot.signal('position').value = self.halfSitting self.dynamic.position.value = self.halfSitting
self.dynamicRobot.signal('velocity').value = self.dimension * (0.,) self.dynamic.velocity.value = self.dimension * (0.,)
self.dynamicRobot.signal('acceleration').value = self.dimension * (0.,) self.dynamic.acceleration.value = self.dimension * (0.,)
if self.robotSimu: if self.simu:
self.robotSimu.increment(.001) self.simu.increment(.001)
...@@ -221,14 +227,14 @@ class HumanoidRobot(AbstractHumanoidRobot): ...@@ -221,14 +227,14 @@ class HumanoidRobot(AbstractHumanoidRobot):
halfSitting = [] #FIXME halfSitting = [] #FIXME
name = None name = None
simu = None simulation = None
filename = None filename = None
def __init__(self, name, simu, filename): def __init__(self, name, simulation, filename):
AbstractHumanoidRobot.__init__(self, name, simu) AbstractHumanoidRobot.__init__(self, name, simulation)
self.filename = filename self.filename = filename
self.dynamicRobot = \ self.dynamic = \
self.loadModelFromKxml (self.name + '.dynamics', self.filename) self.loadModelFromKxml (self.name + '.dynamics', self.filename)
self.dimension = self.dynamicRobot.getDimension() self.dimension = self.dynamic.getDimension()
self.halfSitting = self.dimension*(0.,) self.halfSitting = self.dimension*(0.,)
self.initializeRobot() self.initializeRobot()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment