From 54a9a9c6c3b6c771c97d3caaed173dfc88fb86a7 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Thu, 8 Sep 2011 10:59:43 +0200
Subject: [PATCH] Add support for velocity/acceleration/zmp computation in
jrl-dynamics.
---
.../sot/dynamics/humanoid_robot.py | 59 ++++++++++++++++---
1 file changed, 50 insertions(+), 9 deletions(-)
diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
index 8fcab3b..527e296 100755
--- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py
+++ b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
@@ -15,6 +15,7 @@
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
from dynamic_graph.sot import SE3, R3, SO3
+from dynamic_graph.sot.core.derivator import Derivator_of_Vector
from dynamic_graph.sot.core.feature_position import FeaturePosition
from dynamic_graph.sot.core import RobotSimu, FeaturePoint6dRelative, \
FeatureGeneric, FeatureJointLimits, TaskPD, Constraint, GainAdaptive, SOT
@@ -101,6 +102,22 @@ class AbstractHumanoidRobot (object):
corresponding to operational points.
"""
+ #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
+
+
def loadModelFromKxml(self, name, filename):
"""
Load a model from a kxml file and return the parsed model.
@@ -113,14 +130,8 @@ class AbstractHumanoidRobot (object):
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')
+ self.setProperties(model)
return model
def loadModelFromJrlDynamics(self, name, modelDir, modelName,
@@ -139,8 +150,22 @@ class AbstractHumanoidRobot (object):
model.setFiles(modelDir, modelName,
specificitiesPath, jointRankPath)
model.parse()
+ self.setProperties(model)
return model
+ def setProperties(self, model):
+ model.setProperty('ComputeVelocity', 'true')
+ model.setProperty('ComputeCoM', 'true')
+ model.setProperty('ComputeAccelerationCoM', 'false')
+ model.setProperty('ComputeMomentum', 'false')
+ model.setProperty('ComputeZMP', 'true')
+ model.setProperty('ComputeBackwardDynamics', 'false')
+
+ if self.enableZmpComputation:
+ model.setProperty('ComputeAcceleration', 'true')
+ model.setProperty('ComputeBackwardDynamics', 'true')
+ model.setProperty('ComputeZMP', 'true')
+
def initializeOpPoints(self, model):
for op in self.OperationalPoints:
model.createOpPoint(op, op)
@@ -166,8 +191,24 @@ class AbstractHumanoidRobot (object):
# position in freeflyer frame.
self.device.set(self.halfSitting)
self.dynamic.position.value = self.halfSitting
- self.dynamic.velocity.value = self.dimension*(0.,)
- self.dynamic.acceleration.value = self.dimension*(0.,)
+
+ if self.enableVelocityDerivator:
+ self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
+ self.velocityDerivator.dt.value = 5e-3
+ plug(self.dynamic.position, 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 = 5e-3
+ 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)
--
GitLab