diff --git a/src/dynamic_graph/sot/dynamics/hrp2.py.in b/src/dynamic_graph/sot/dynamics/hrp2.py.in index 1a71185624e585728632dda618947c4af1f7662d..93e8ed2c5200fbd062060c343ddb4c597b90efe0 100755 --- a/src/dynamic_graph/sot/dynamics/hrp2.py.in +++ b/src/dynamic_graph/sot/dynamics/hrp2.py.in @@ -22,6 +22,7 @@ from dynamic_graph import enableTrace, plug from dynamic_graph.sot.se3 import R3, SO3, SE3 from dynamic_graph.sot.dynamics.dynamic_hrp2 import DynamicHrp2 +from dynamic_graph.sot.dynamics.dynamic_hrp2_10 import DynamicHrp2_10 from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot @@ -38,7 +39,7 @@ class Hrp2(AbstractHumanoidRobot): res = (config + 10*(0.,)) return res - def __init__(self, name, modelDir, xmlDir, device): + def __init__(self, name, modelDir, xmlDir, device, dynamicType): AbstractHumanoidRobot.__init__ (self, name) self.OperationalPoints.append('waist') @@ -47,10 +48,14 @@ class Hrp2(AbstractHumanoidRobot): specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml' jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml' - self.dynamic = DynamicHrp2(self.name + '_dynamic') - self.dynamic.setFiles(modelDir, modelName, - specificitiesPath, jointRankPath) - self.dynamic.parse() + self.dynamic = self.loadModelFromJrlDynamics( + self.name + '_dynamic', + modelDir, + modelName, + specificitiesPath, + jointRankPath, + dynamicType) + self.dimension = self.dynamic.getDimension() if self.dimension != len(self.halfSitting): raise RuntimeError("invalid half-sitting pose") @@ -80,7 +85,7 @@ class Hrp2Jrl (Hrp2): modelDir = hrp2_10_pkgdatarootdir, xmlDir = hrp2_10_pkgdatarootdir, device = None): - Hrp2.__init__(self, name, modelDir, xmlDir, device) + Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10) class Hrp2Laas (Hrp2): """ @@ -106,4 +111,4 @@ class Hrp2Laas (Hrp2): modelDir = hrp2_14_pkgdatarootdir, xmlDir = hrp2_14_pkgdatarootdir, device = None): - Hrp2.__init__(self, name, modelDir, xmlDir, device) + Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2) diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py index 03c376d26c81d960678809a9a202fb6e7ca926b3..af3add79701bf2217d80bcde8495bcecead5999f 100755 --- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py +++ b/src/dynamic_graph/sot/dynamics/humanoid_robot.py @@ -124,7 +124,8 @@ class AbstractHumanoidRobot (object): return model def loadModelFromJrlDynamics(self, name, modelDir, modelName, - specificitiesPath, jointRankPath): + specificitiesPath, jointRankPath, + dynamicType): """ Load a model using the jrl-dynamics parser. This parser looks for VRML files in which kinematics and dynamics information @@ -134,12 +135,11 @@ class AbstractHumanoidRobot (object): Additional information are located in two different XML files. """ - #FIXME: add support for hrp2-10 here. - model = DynamicHrp2(name) + model = dynamicType(name) model.setFiles(modelDir, modelName, specificitiesPath, jointRankPath) model.parse() - return + return model def initializeOpPoints(self, model): for op in self.OperationalPoints: diff --git a/src/dynamic_graph/sot/dynamics/tools.py b/src/dynamic_graph/sot/dynamics/tools.py index 5b1f80c9aa131296d762357962d2e7ef83f29960..b4e670bec200bf23236b9686d11da43449ae8d99 100755 --- a/src/dynamic_graph/sot/dynamics/tools.py +++ b/src/dynamic_graph/sot/dynamics/tools.py @@ -151,6 +151,9 @@ if 'argv' in sys.__dict__: parser.add_option("-d", "--display", action="store_true", dest="display", default=False, help="enable display using robotviewer") + parser.add_option("-r", "--robot", + action="store", dest="robot", default="Hrp2Laas", + help="Specify which robot model to use") (options, args) = parser.parse_args() if options.display: @@ -162,6 +165,13 @@ if 'argv' in sys.__dict__: # Initialize the stack of tasks. - robot = Hrp2Laas("robot") + robots = { + "Hrp2Laas" : Hrp2Laas, + "Hrp2Jrl" : Hrp2Jrl + } + if not options.robot in robots: + raise RuntimeError ( + "invalid robot name (should by Hrp2Laas or Hrp2Jrl)") + robot = robots[options.robot]("robot") timeStep = .005 solver = Solver(robot)