Skip to content
This repository was archived by the owner on Nov 28, 2025. It is now read-only.

Commit 526105e

Browse files
committed
format
1 parent 7851a42 commit 526105e

File tree

3 files changed

+14
-13
lines changed

3 files changed

+14
-13
lines changed

src/dynamic_graph/sot/dynamic_pinocchio/__init__.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
import numpy as np
22
from numpy import cos, sin, sqrt
33

4-
from .dynamic import DynamicPinocchio
4+
from .dynamic import DynamicPinocchio # noqa
5+
56

67
def fromSotToPinocchio(q_sot, freeflyer=True):
78
if freeflyer:

src/dynamic_graph/sot/dynamic_pinocchio/humanoid_robot.py

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -273,9 +273,8 @@ def initializeRobot(self):
273273
"""
274274
self.halfSitting = pinocchio.neutral(self.pinocchioModel)
275275
self.defineHalfSitting(self.halfSitting)
276-
self.halfSitting = numpy.array(self.halfSitting[:3].tolist()
277-
+ [0., 0., 0.] # Replace quaternion by RPY.
278-
+ self.halfSitting[7:].tolist())
276+
self.halfSitting = numpy.array(self.halfSitting[:3].tolist() + [0., 0., 0.] # Replace quaternion by RPY.
277+
+ self.halfSitting[7:].tolist())
279278
assert self.halfSitting.shape[0] == self.dynamic.getDimension()
280279

281280
# Set the device limits.
@@ -287,9 +286,9 @@ def opposite(v):
287286
return [-x for x in v]
288287

289288
self.dynamic.add_signals()
290-
self.device.setPositionBounds( get(self.dynamic.lowerJl), get(self.dynamic.upperJl))
289+
self.device.setPositionBounds(get(self.dynamic.lowerJl), get(self.dynamic.upperJl))
291290
self.device.setVelocityBounds(-get(self.dynamic.upperVl), get(self.dynamic.upperVl))
292-
self.device.setTorqueBounds (-get(self.dynamic.upperTl), get(self.dynamic.upperTl))
291+
self.device.setTorqueBounds(-get(self.dynamic.upperTl), get(self.dynamic.upperTl))
293292

294293
# Freeflyer reference frame should be the same as global
295294
# frame so that operational point positions correspond to
@@ -303,7 +302,9 @@ def opposite(v):
303302
plug(self.device.state, self.velocityDerivator.sin)
304303
plug(self.velocityDerivator.sout, self.dynamic.velocity)
305304
else:
306-
self.dynamic.velocity.value = numpy.zeros([self.dimension,])
305+
self.dynamic.velocity.value = numpy.zeros([
306+
self.dimension,
307+
])
307308

308309
if self.enableAccelerationDerivator:
309310
self.accelerationDerivator = \
@@ -312,7 +313,9 @@ def opposite(v):
312313
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
313314
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
314315
else:
315-
self.dynamic.acceleration.value = numpy.zeros([self.dimension,])
316+
self.dynamic.acceleration.value = numpy.zeros([
317+
self.dimension,
318+
])
316319

317320
def addTrace(self, entityName, signalName):
318321
if self.tracer:

tests/python/humanoid_robot.py

Lines changed: 2 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
import unittest
66

77
import numpy as np
8-
import pinocchio
98
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
109
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractHumanoidRobot
1110

@@ -27,11 +26,9 @@ def defineHalfSitting(self, q):
2726

2827
class HumanoidRobotTest(unittest.TestCase):
2928
def setUp(self):
30-
import os
31-
dir_path = os.path.dirname(os.path.realpath(__file__))
3229
from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR
33-
self.urdf_file_name=EXAMPLE_ROBOT_DATA_MODEL_DIR+\
34-
'/talos_data/robots/talos_reduced.urdf'
30+
self.urdf_file_name = EXAMPLE_ROBOT_DATA_MODEL_DIR + \
31+
'/talos_data/robots/talos_reduced.urdf'
3532
self.name = "talos"
3633

3734
def test_non_instanciable_robot(self):

0 commit comments

Comments
 (0)