From 872e8c3892bbb2009c79928b5743de9ac048e35a Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Thu, 8 Sep 2011 16:34:41 +0200
Subject: [PATCH] Fix ZMP computation subgraph.
---
.../sot/dynamics/humanoid_robot.py | 26 +++++++++++++------
1 file changed, 18 insertions(+), 8 deletions(-)
diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
index 527e296..2aa8054 100755
--- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py
+++ b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
@@ -154,17 +154,27 @@ class AbstractHumanoidRobot (object):
return model
def setProperties(self, model):
- model.setProperty('ComputeVelocity', 'true')
- model.setProperty('ComputeCoM', 'true')
+ model.setProperty('TimeStep', '0.005')
+
+ 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')
- model.setProperty('ComputeBackwardDynamics', 'false')
if self.enableZmpComputation:
- model.setProperty('ComputeAcceleration', 'true')
model.setProperty('ComputeBackwardDynamics', 'true')
- model.setProperty('ComputeZMP', 'true')
+ model.setProperty('ComputeAcceleration', 'true')
+ model.setProperty('ComputeMomentum', 'true')
+
def initializeOpPoints(self, model):
for op in self.OperationalPoints:
@@ -194,8 +204,8 @@ class AbstractHumanoidRobot (object):
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
- self.velocityDerivator.dt.value = 5e-3
- plug(self.dynamic.position, self.velocityDerivator.sin)
+ self.velocityDerivator.dt.value = 0.005
+ plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension*(0.,)
@@ -203,7 +213,7 @@ class AbstractHumanoidRobot (object):
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
- self.accelerationDerivator.dt.value = 5e-3
+ self.accelerationDerivator.dt.value = 0.005
plug(self.velocityDerivator.sout,
self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
--
GitLab