Commit 526105e5 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

format

parent 7851a428
import numpy as np import numpy as np
from numpy import cos, sin, sqrt from numpy import cos, sin, sqrt
from .dynamic import DynamicPinocchio from .dynamic import DynamicPinocchio # noqa
def fromSotToPinocchio(q_sot, freeflyer=True): def fromSotToPinocchio(q_sot, freeflyer=True):
if freeflyer: if freeflyer:
......
...@@ -273,9 +273,8 @@ class AbstractRobot(ABC): ...@@ -273,9 +273,8 @@ class AbstractRobot(ABC):
""" """
self.halfSitting = pinocchio.neutral(self.pinocchioModel) self.halfSitting = pinocchio.neutral(self.pinocchioModel)
self.defineHalfSitting(self.halfSitting) self.defineHalfSitting(self.halfSitting)
self.halfSitting = numpy.array(self.halfSitting[:3].tolist() self.halfSitting = numpy.array(self.halfSitting[:3].tolist() + [0., 0., 0.] # Replace quaternion by RPY.
+ [0., 0., 0.] # Replace quaternion by RPY. + self.halfSitting[7:].tolist())
+ self.halfSitting[7:].tolist())
assert self.halfSitting.shape[0] == self.dynamic.getDimension() assert self.halfSitting.shape[0] == self.dynamic.getDimension()
# Set the device limits. # Set the device limits.
...@@ -287,9 +286,9 @@ class AbstractRobot(ABC): ...@@ -287,9 +286,9 @@ class AbstractRobot(ABC):
return [-x for x in v] return [-x for x in v]
self.dynamic.add_signals() self.dynamic.add_signals()
self.device.setPositionBounds( get(self.dynamic.lowerJl), get(self.dynamic.upperJl)) self.device.setPositionBounds(get(self.dynamic.lowerJl), get(self.dynamic.upperJl))
self.device.setVelocityBounds(-get(self.dynamic.upperVl), get(self.dynamic.upperVl)) self.device.setVelocityBounds(-get(self.dynamic.upperVl), get(self.dynamic.upperVl))
self.device.setTorqueBounds (-get(self.dynamic.upperTl), get(self.dynamic.upperTl)) self.device.setTorqueBounds(-get(self.dynamic.upperTl), get(self.dynamic.upperTl))
# 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
...@@ -303,7 +302,9 @@ class AbstractRobot(ABC): ...@@ -303,7 +302,9 @@ class AbstractRobot(ABC):
plug(self.device.state, self.velocityDerivator.sin) plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity) plug(self.velocityDerivator.sout, self.dynamic.velocity)
else: else:
self.dynamic.velocity.value = numpy.zeros([self.dimension,]) self.dynamic.velocity.value = numpy.zeros([
self.dimension,
])
if self.enableAccelerationDerivator: if self.enableAccelerationDerivator:
self.accelerationDerivator = \ self.accelerationDerivator = \
...@@ -312,7 +313,9 @@ class AbstractRobot(ABC): ...@@ -312,7 +313,9 @@ class AbstractRobot(ABC):
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin) plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration) plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else: else:
self.dynamic.acceleration.value = numpy.zeros([self.dimension,]) self.dynamic.acceleration.value = numpy.zeros([
self.dimension,
])
def addTrace(self, entityName, signalName): def addTrace(self, entityName, signalName):
if self.tracer: if self.tracer:
......
...@@ -5,7 +5,6 @@ ...@@ -5,7 +5,6 @@
import unittest import unittest
import numpy as np import numpy as np
import pinocchio
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractHumanoidRobot from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractHumanoidRobot
...@@ -27,11 +26,9 @@ class Robot(AbstractHumanoidRobot): ...@@ -27,11 +26,9 @@ class Robot(AbstractHumanoidRobot):
class HumanoidRobotTest(unittest.TestCase): class HumanoidRobotTest(unittest.TestCase):
def setUp(self): def setUp(self):
import os
dir_path = os.path.dirname(os.path.realpath(__file__))
from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR
self.urdf_file_name=EXAMPLE_ROBOT_DATA_MODEL_DIR+\ self.urdf_file_name = EXAMPLE_ROBOT_DATA_MODEL_DIR + \
'/talos_data/robots/talos_reduced.urdf' '/talos_data/robots/talos_reduced.urdf'
self.name = "talos" self.name = "talos"
def test_non_instanciable_robot(self): def test_non_instanciable_robot(self):
......
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