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

format

parent 7851a428
import numpy as np
from numpy import cos, sin, sqrt
from .dynamic import DynamicPinocchio
from .dynamic import DynamicPinocchio # noqa
def fromSotToPinocchio(q_sot, freeflyer=True):
if freeflyer:
......
......@@ -273,9 +273,8 @@ class AbstractRobot(ABC):
"""
self.halfSitting = pinocchio.neutral(self.pinocchioModel)
self.defineHalfSitting(self.halfSitting)
self.halfSitting = numpy.array(self.halfSitting[:3].tolist()
+ [0., 0., 0.] # Replace quaternion by RPY.
+ self.halfSitting[7:].tolist())
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.
......@@ -287,9 +286,9 @@ class AbstractRobot(ABC):
return [-x for x in v]
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.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
# frame so that operational point positions correspond to
......@@ -303,7 +302,9 @@ class AbstractRobot(ABC):
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = numpy.zeros([self.dimension,])
self.dynamic.velocity.value = numpy.zeros([
self.dimension,
])
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
......@@ -312,7 +313,9 @@ class AbstractRobot(ABC):
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = numpy.zeros([self.dimension,])
self.dynamic.acceleration.value = numpy.zeros([
self.dimension,
])
def addTrace(self, entityName, signalName):
if self.tracer:
......
......@@ -5,7 +5,6 @@
import unittest
import numpy as np
import pinocchio
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractHumanoidRobot
......@@ -27,11 +26,9 @@ class Robot(AbstractHumanoidRobot):
class HumanoidRobotTest(unittest.TestCase):
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
self.urdf_file_name=EXAMPLE_ROBOT_DATA_MODEL_DIR+\
'/talos_data/robots/talos_reduced.urdf'
self.urdf_file_name = EXAMPLE_ROBOT_DATA_MODEL_DIR + \
'/talos_data/robots/talos_reduced.urdf'
self.name = "talos"
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