Commit 6aaa92f3 authored by Thomas Moulard's avatar Thomas Moulard
Browse files

Add robot related Python files.

parent f70b4ec5
......@@ -65,6 +65,8 @@ EXEC_PROGRAM(${PYTHON_EXECUTABLE} ARGS "-c \"from distutils import sysconfig; pr
# Install empty __init__.py files in intermediate directories.
INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/hrp2.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/humanoid_robot.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/parser.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dynamics
)
......
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
from dynamic_graph.sot import SE3, R3, SO3
from dynamic_graph.sot.core import RobotSimu, FeaturePoint6dRelative, \
FeatureGeneric, FeatureJointLimits, Task, Constraint, GainAdaptive, SOT
from dynamic_graph.sot.dynamics import AngleEstimator
from dynamic_graph import enableTrace, plug
from dynamic_graph.sot.se3 import R3, SO3, SE3
from dynamic_graph.sot.dynamics.dynamic_hrp2 import DynamicHrp2
from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
class Hrp2(AbstractHumanoidRobot):
"""
This class instanciates a simple humanoid robot with
"""
halfSitting = (
# Free flyer
0., 0., 0., 0., 0. , 0.,
# Legs
# 0., 0., -1.04720, 2.09440, -1.04720, 0.,
# 0., 0., -1.04720, 2.09440, -1.04720, 0.,
0., 0., -0.453786, 0.872665, -0.418879, 0.,
0., 0., -0.453786, 0.872665, -0.418879, 0.,
# Chest and head
0., 0., 0., 0.,
# Arms
# 0.17453, -0.17453, -0.17453, -0.87266, 0., 0., 0.1,
# 0.17453, 0.17453, 0.17453, -0.87266, 0., 0., 0.1
0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1,
0.261799, 0.17453, 0., -0.523599, 0., 0., 0.1,
)
def __init__(self, name, modelDir, xmlDir, simu):
AbstractHumanoidRobot.__init__ (self, name, simu)
modelName = 'HRP2JRLmainSmall.wrl'
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
self.dynamicRobot = DynamicHrp2(self.name + '.dynamics')
self.dynamicRobot.setFiles(modelDir, modelName,
specificitiesPath, jointRankPath)
self.dynamicRobot.parse()
self.dimension = self.dynamicRobot.getDimension()
if self.dimension != len(self.halfSitting):
raise "invalid half-sitting pose"
self.forwardKinematics = DynamicHrp2(self.name + '.forwardKinematics')
self.forwardKinematics.setFiles(modelDir, modelName,
specificitiesPath, jointRankPath)
self.forwardKinematics.parse()
self.initializeRobot()
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
from dynamic_graph.sot import SE3, R3, SO3
from dynamic_graph.sot.core.feature_position import FeaturePosition
from dynamic_graph.sot.core import RobotSimu, FeaturePoint6dRelative, \
FeatureGeneric, FeatureJointLimits, Task, Constraint, GainAdaptive, SOT
from dynamic_graph.sot.dynamics.parser import Parser
from dynamic_graph.sot.dynamics import AngleEstimator
from dynamic_graph import plug
I3 = reduce(lambda m, i: m + (i*(0.,)+(1.,)+ (2-i)*(0.,),), range(3), ())
I4 = reduce(lambda m, i: m + (i*(0.,)+(1.,)+ (2-i)*(0.,),), range(4), ())
class AbstractHumanoidRobot (object):
"""
This class instantiates all the entities required to get a consistent
representation of a humanoid robot:
- robot models (dynamic model and the separate forward kinematics
only model used to compute the freeflyer position)
- angleEstimator used to link the two robot models
- usual features and tasks for a robot:
- center of mass
- one task per operational point
"""
OperationalPoints = ['left-wrist', 'right-wrist',
'left-ankle', 'right-ankle']
"""
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.
"""
name = None
"""Entity name (internal use)"""
#FIXME: it should be some kind of global flag instead.
simu = False
"""Are we in simulation or not?"""
halfSitting = None
"""
The half-sitting position is the robot initial pose.
This attribute *must* be defined in subclasses.
"""
dynamicRobot = None
"""
The robot dynamic model.
"""
forwardKinematics = None
"""
The forward kinematics model.
This alternative representation of the robot is required to compute
the freeflyer (i.e. waist) position in the global frame.
In the dynamic robot model, the freeflyer is not actuated. Hence, the
freeflyer value remains zero all the time.
FIXME: describe the algorithm used to deduce the freeflyer position
and the role of the angle estimator (ankle flexibility?).
"""
dimension = None
"""The configuration size."""
angleEstimator = None
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.
"""
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()
model.setProperty('ComputeVelocity', 'true')
model.setProperty('ComputeCoM', 'true')
model.setProperty('ComputeAccelerationCoM', 'false')
model.setProperty('ComputeMomentum', 'false')
model.setProperty('ComputeZMP', 'true')
model.setProperty('ComputeBackwardDynamics', 'false')
return model
def loadModelFromJrlDynamics(self, name, modelDir, modelName,
specificitiesPath, jointRankPath):
"""
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.
"""
#FIXME: add support for hrp2-10 here.
model = DynamicHrp2(name)
model.setFiles(modelDir, modelName,
specificitiesPath, jointRankPath)
model.parse()
return
def initializeOpPoints(self, model, prefix):
for op in self.OperationalPoints:
model.createOpPoint(op, op)
def initializeRobot(self):
"""
If the two robots models are correctly loaded (respectively in
attributes robotDynamics and forwardKinematics), this method
will then initialize the operational points, set the position
to half-sitting with null velocity/acceleration.
It also initializes the angle estimator which makes the link
between the dynamics model and the forward kinematics one.
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.dynamicRobot or not self.forwardKinematics:
raise "robots models have to be initialized first"
if self.simu:
self.robotSimu = RobotSimu(self.name + '.robotSimu')
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
self.robotSimu.set(self.halfSitting)
self.dynamicRobot.signal('position').value = self.halfSitting
self.dynamicRobot.signal('velocity').value = self.dimension*(0.,)
self.dynamicRobot.signal('acceleration').value = self.dimension*(0.,)
self.forwardKinematics.signal('position').value = self.halfSitting
self.forwardKinematics.signal('velocity').value = self.dimension*(0.,)
self.forwardKinematics.signal('acceleration').value = \
self.dimension*(0.,)
self.initializeOpPoints(self.dynamicRobot,
self.name + '.dynamics')
self.initializeOpPoints(self.forwardKinematics,
self.name + '.forwardKinematics')
# --- freeflyer pose ------------
self.angleEstimator = AngleEstimator('angleEstimator')
self.angleEstimator.setFromSensor(False)
self.angleEstimator.signal('sensorWorldRotation').value = I3
self.angleEstimator.signal('sensorEmbeddedPosition').value = I4
# --- center of mass ------------
self.featureCom = FeatureGeneric(self.name + '.feature.com')
plug(self.dynamicRobot.signal('com'), self.featureCom.signal('errorIN'))
plug(self.dynamicRobot.signal('Jcom'),
self.featureCom.signal('jacobianIN'))
self.featureCom.signal('selec').value = '011'
self.featureComDes = FeatureGeneric(self.name + '.feature.ref.com')
self.featureComDes.signal('errorIN').value = (.0, .0, 0.)
self.featureCom.signal('sdes').value = self.featureComDes
self.comTask = Task(self.name + '.task.com')
self.comTask.add(self.name + '.feature.com')
self.comTask.signal('controlGain').value = 1.
# --- operational points tasks -----
self.features = dict()
self.tasks = dict()
for op in self.OperationalPoints:
self.features[op] = \
FeaturePosition(self.name + '.feature.' + op,
self.dynamicRobot.signal(op),
self.dynamicRobot.signal('J' + op),
SE3())
self.tasks[op] = Task(self.name + '.task.' + op)
self.tasks[op].add(self.name + '.feature.' + op)
self.tasks[op].signal('controlGain').value = .2
def __init__(self, name, simu):
self.name = name
if simu:
self.simu = True
else:
self.simu = False
class HumanoidRobot(AbstractHumanoidRobot):
halfSitting = [] #FIXME
def __init__(self, name, filename, simu):
AbstractHumanoidRobot.__init__(self, name, simu)
self.filename = filename
self.dynamicRobot = \
self.loadModelFromKxml (self.name + '.dynamics', self.filename)
self.dimension = self.dynamicRobot.getDimension()
self.halfSitting = self.dimension*(0.,)
# Create entity to compute position of contact foot in root
# joint frame.
self.forwardKinematics = \
self.loadModelFromKxml (self.name + '.forwardKinematics',
self.filename)
self.initializeRobot()
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
import unittest
from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot
from dynamic_graph.sot.dynamics.hrp2 import Hrp2
class Hrp2Test(unittest.TestCase):
def test_simple(self):
pass
def test_model_not_exist(self):
#FIXME: search what is the real returned type.
self.assertRaises(Exception, Hrp2, "robot",
"IDONOTEXIST", "IDONOTEXIST", True)
if __name__ == '__main__':
unittest.main()
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
import unittest
from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot
class HumanoidRobotTest(unittest.TestCase):
def test_simple(self):
pass
def test_model_not_exist(self):
self.assertRaises(IOError, HumanoidRobot, "robot", "IDONOTEXIST", True)
if __name__ == '__main__':
unittest.main()
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment