33
44from __future__ import print_function
55
6- from functools import reduce
6+ import sys
77
8+ import pinocchio
89from dynamic_graph import plug
910from dynamic_graph .sot .core .derivator import Derivator_of_Vector
1011from dynamic_graph .sot .core .op_point_modifier import OpPointModifier
1112from dynamic_graph .sot .core .robot_simu import RobotSimu
12- from dynamic_graph .sot .dynamic_pinocchio import DynamicPinocchio
1313from dynamic_graph .tools import addTrace
1414from dynamic_graph .tracer_real_time import TracerRealTime
1515
16- import pinocchio , sys
17-
1816if sys .version_info .major == 2 :
1917 from abc import ABCMeta , abstractmethod
18+
2019 class ABC :
2120 __metaclass__ = ABCMeta
2221else :
2322 from abc import ABC , abstractmethod
2423
24+
2525class AbstractRobot (ABC ):
2626 """
2727 This class instantiates all the entities required to get a consistent
@@ -147,7 +147,7 @@ def _removeMimicJoints(self, urdfFile=None, urdfString=None):
147147 mimicJoints .append (name )
148148 self .removeJoints (mimicJoints )
149149
150- def removeJoints (self , joints ):
150+ def removeJoints (self , joints ):
151151 """
152152 - param joints: a list of joint names to be removed from the self.pinocchioModel
153153 """
@@ -160,8 +160,7 @@ def removeJoints (self, joints):
160160 self .pinocchioModel = pinocchio .buildReducedModel (self .pinocchioModel , jointIds , q )
161161 self .pinocchioData = pinocchio .Data (self .pinocchioModel )
162162
163- def loadModelFromString (self , urdfString , rootJointType = pinocchio .JointModelFreeFlyer ,
164- removeMimicJoints = True ):
163+ def loadModelFromString (self , urdfString , rootJointType = pinocchio .JointModelFreeFlyer , removeMimicJoints = True ):
165164 """ Load a URDF model contained in a string
166165 - param rootJointType: the root joint type. None for no root joint.
167166 - param removeMimicJoints: if True, all the mimic joints found in the model are removed.
@@ -174,8 +173,11 @@ def loadModelFromString (self, urdfString, rootJointType=pinocchio.JointModelFre
174173 if removeMimicJoints :
175174 self ._removeMimicJoints (urdfString = urdfString )
176175
177- def loadModelFromUrdf (self , urdfPath , urdfDir = None , rootJointType = pinocchio .JointModelFreeFlyer ,
178- removeMimicJoints = True ):
176+ def loadModelFromUrdf (self ,
177+ urdfPath ,
178+ urdfDir = None ,
179+ rootJointType = pinocchio .JointModelFreeFlyer ,
180+ removeMimicJoints = True ):
179181 """
180182 Load a model using the pinocchio urdf parser. This parser looks
181183 for urdf files in which kinematics and dynamics information
@@ -185,10 +187,10 @@ def loadModelFromUrdf(self, urdfPath, urdfDir=None, rootJointType=pinocchio.Join
185187 """
186188 if urdfPath .startswith ("package://" ):
187189 from os import path
188- n1 = 10 # len("package://")
190+ n1 = 10 # len("package://")
189191 n2 = urdfPath .index (path .sep , n1 )
190192 pkg = urdfPath [n1 :n2 ]
191- relpath = urdfPath [n2 + 1 :]
193+ relpath = urdfPath [n2 + 1 :]
192194
193195 import rospkg
194196 rospack = rospkg .RosPack ()
@@ -233,7 +235,7 @@ def setJointValueInConfig(self, q, jointNames, jointValues):
233235 q [joint .idx_q ] = jv
234236
235237 @abstractmethod
236- def defineHalfSitting (self , q ):
238+ def defineHalfSitting (self , q ):
237239 """
238240 Define half sitting configuration using the pinocchio Model (i.e.
239241 with quaternions and not with euler angles).
@@ -256,7 +258,7 @@ def initializeRobot(self):
256258 raise RuntimeError ("Dynamic robot model must be initialized first" )
257259
258260 if not hasattr (self , 'device' ) or self .device is None :
259- #raise RuntimeError("A device is already defined.")
261+ # raise RuntimeError("A device is already defined.")
260262 self .device = RobotSimu (self .name + '_device' )
261263 self .device .resize (self .dynamic .getDimension ())
262264 """
@@ -271,13 +273,16 @@ def initializeRobot(self):
271273 """
272274 self .halfSitting = numpy .asarray (pinocchio .neutral (self .pinocchioModel )).flatten ().tolist ()
273275 self .defineHalfSitting (self .halfSitting )
274- self .halfSitting [3 :7 ] = [0. , 0. , 0. ] # Replace quaternion by RPY.
276+ self .halfSitting [3 :7 ] = [0. , 0. , 0. ] # Replace quaternion by RPY.
275277
276278 # Set the device limits.
277279 def get (s ):
278280 s .recompute (0 )
279281 return s .value
280- def opposite (v ): return [ - x for x in v ]
282+
283+ def opposite (v ):
284+ return [- x for x in v ]
285+
281286 self .device .setPositionBounds (get (self .dynamic .lowerJl ), get (self .dynamic .upperJl ))
282287 self .device .setVelocityBounds (opposite (get (self .dynamic .upperVl )), get (self .dynamic .upperVl ))
283288 self .device .setTorqueBounds (opposite (get (self .dynamic .upperTl )), get (self .dynamic .upperTl ))
@@ -394,6 +399,7 @@ def reset(self, posture=None):
394399 self .dynamic .signal (self .OperationalPointsMap [op ]).recompute (self .device .state .time + 1 )
395400 self .dynamic .signal ('J' + self .OperationalPointsMap [op ]).recompute (self .device .state .time + 1 )
396401
402+
397403class AbstractHumanoidRobot (AbstractRobot ):
398404 def __init__ (self , name , tracer = None ):
399405 AbstractRobot .__init__ (self , name , tracer )
0 commit comments