Commit d73dfffb authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Minor fixes.

parent b789f48c
......@@ -9,7 +9,7 @@ typedef bp::return_value_policy<bp::reference_existing_object> reference_existin
BOOST_PYTHON_MODULE(wrap)
{
bp::import("dynamicgraph");
bp::import("dynamic_graph");
bp::import("pinocchio");
dg::python::exposeEntity<dgs::DynamicPinocchio>()
......
......@@ -271,9 +271,12 @@ class AbstractRobot(ABC):
"""
Half sitting configuration.
"""
self.halfSitting = numpy.asarray(pinocchio.neutral(self.pinocchioModel)).flatten().tolist()
self.halfSitting = pinocchio.neutral(self.pinocchioModel)
self.defineHalfSitting(self.halfSitting)
self.halfSitting[3:7] = [0., 0., 0.] # Replace quaternion by RPY.
self.halfSitting = numpy.array(self.halfSitting[:3].tolist()
+ [0., 0., 0.] # Replace quaternion by RPY.
+ self.halfSitting[7:].tolist())
assert self.halfSitting.shape[0] == self.dynamic.getDimension()
# Set the device limits.
def get(s):
......@@ -283,9 +286,9 @@ class AbstractRobot(ABC):
def opposite(v):
return [-x for x in v]
self.device.setPositionBounds(get(self.dynamic.lowerJl), get(self.dynamic.upperJl))
self.device.setVelocityBounds(opposite(get(self.dynamic.upperVl)), get(self.dynamic.upperVl))
self.device.setTorqueBounds(opposite(get(self.dynamic.upperTl)), get(self.dynamic.upperTl))
self.device.setPositionBounds( get(self.dynamic.lowerJl), get(self.dynamic.upperJl))
self.device.setVelocityBounds(-get(self.dynamic.upperVl), get(self.dynamic.upperVl))
self.device.setTorqueBounds (-get(self.dynamic.upperTl), get(self.dynamic.upperTl))
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
......@@ -299,7 +302,7 @@ class AbstractRobot(ABC):
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension * (0., )
self.dynamic.velocity.value = numpy.zeros([self.dimension,])
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
......@@ -308,7 +311,7 @@ class AbstractRobot(ABC):
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension * (0., )
self.dynamic.acceleration.value = numpy.zeros([self.dimension,])
def addTrace(self, entityName, signalName):
if self.tracer:
......
Markdown is supported
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