Merge branch 'topic/talos-legs' into 'devel'

Added the loading function for talos legs + removed an path append inside display

See merge request gepetto/example-robot-data!10

python/__init__.py

-from robots_loader import (getModelPath, loadHyQ, loadICub, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand,
-                           readParamsFromSrdf)
+from robots_loader import (getModelPath, loadHyQ, loadICub, loadTalos, loadTalosArm, loadTalosLegs, loadTiago,
+                            loadTiagoNoHand, readParamsFromSrdf)

python/display.py

 import sys
-
-from example_robot_data import loadHyQ, loadICub, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand
-
-sys.path.append('/opt/openrobots/share/example-robot-data/unittest/')
+import example_robot_data
 
 DISPLAY_HYQ = 'hyq' in sys.argv
 DISPLAY_TALOS = 'talos' in sys.argv
 DISPLAY_TALOS_ARM = 'talos_arm' in sys.argv
+DISPLAY_TALOS_LEGS = 'talos_legs' in sys.argv
 DISPLAY_TIAGO = 'tiago' in sys.argv
 DISPLAY_TIAGO_NO_HAND = 'tiago_no_hand' in sys.argv
 DISPLAY_ICUB = 'icub' in sys.argv
 
 if DISPLAY_HYQ:
-    hyq = loadHyQ()
-    hyq.initDisplay(loadModel=True)
+    hyq = example_robot_data.loadHyQ()
+    hyq.initViewer(loadModel=True)
     hyq.display(hyq.q0)
 
 if DISPLAY_TALOS:
-    talos = loadTalos()
-    talos.initDisplay(loadModel=True)
+    talos = example_robot_data.loadTalos()
+    talos.initViewer(loadModel=True)
     talos.display(talos.q0)
 
 if DISPLAY_TALOS_ARM:
-    talos_arm = loadTalosArm()
-    talos_arm.initDisplay(loadModel=True)
+    talos_arm = example_robot_data.loadTalosArm()
+    talos_arm.initViewer(loadModel=True)
     talos_arm.display(talos_arm.q0)
 
+if DISPLAY_TALOS_LEGS:
+    talos_legs = example_robot_data.loadTalosLegs()
+    talos_legs.initViewer(loadModel=True)
+    talos_legs.display(talos_legs.q0)
+
 if DISPLAY_TIAGO:
-    tiago = loadTiago()
-    tiago.initDisplay(loadModel=True)
+    tiago = example_robot_data.loadTiago()
+    tiago.initViewer(loadModel=True)
     tiago.display(tiago.q0)
 
 if DISPLAY_TIAGO_NO_HAND:
-    tiago_no_hand = loadTiagoNoHand()
-    tiago_no_hand.initDisplay(loadModel=True)
+    tiago_no_hand = example_robot_data.loadTiagoNoHand()
+    tiago_no_hand.initViewer(loadModel=True)
     tiago_no_hand.display(tiago_no_hand.q0)
 
 if DISPLAY_ICUB:
-    icub = loadICub()
-    icub.initDisplay(loadModel=True)
+    icub = example_robot_data.loadICub()
+    icub.initViewer(loadModel=True)
     icub.display(icub.q0)

python/robots_loader.py

@@ -66,6 +66,61 @@ def loadTalos():
     addFreeFlyerJointLimits(robot)
     return robot
 
+def loadTalosLegs():
+    robot = loadTalos()
+    URDF_FILENAME = "talos_reduced.urdf"
+    SRDF_FILENAME = "talos.srdf"
+    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
+    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
+    modelPath = getModelPath(URDF_SUBPATH)
+
+    legMaxId = 14
+    m1 = robot.model
+    m2 = pinocchio.Model()
+    for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias):
+        if j.id < legMaxId:
+            jid = m2.addJoint(parent, getattr(pinocchio, j.shortname())(), M, name)
+            up = m2.upperPositionLimit
+            down = m2.lowerPositionLimit
+            up[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
+            down[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
+            m2.upperPositionLimit = up
+            m2.lowerPositionLimit = down
+            assert (jid == j.id)
+            m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
+
+    u = m2.upperPositionLimit
+    u[:7] = 1
+    m2.upperPositionLimit = u
+    limit = m2.lowerPositionLimit
+    limit[:7] = -1
+    m2.lowerPositionLimit = limit
+
+    # q2 = robot.q0[:19]
+    for f in m1.frames:
+        if f.parent < legMaxId:
+            m2.addFrame(f)
+
+    g2 = pinocchio.GeometryModel()
+    for g in robot.visual_model.geometryObjects:
+        if g.parentJoint < 14:
+            g2.addGeometryObject(g)
+
+    robot.model = m2
+    robot.data = m2.createData()
+    robot.visual_model = g2
+    # robot.q0=q2
+    robot.visual_data = pinocchio.GeometryData(g2)
+
+    # Load SRDF file
+    robot.q0 = np.matrix(np.resize(robot.q0, robot.model.nq)).T
+    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
+
+    assert ((m2.armature[:6] == 0.).all())
+    # Add the free-flyer joint limits
+    addFreeFlyerJointLimits(robot)
+    return robot
+
 
 def loadHyQ():
     URDF_FILENAME = "hyq_no_sensors.urdf"

unittest/test_load.py

 #!/usr/bin/env python2
 
 import unittest
-
-from example_robot_data import loadHyQ, loadICub, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand
+import example_robot_data
 
 
 class RobotTestCase(unittest.TestCase):
@@ -23,43 +22,47 @@ class RobotTestCase(unittest.TestCase):
 
 
 class TalosArmTest(RobotTestCase):
-    RobotTestCase.ROBOT = loadTalosArm()
+    RobotTestCase.ROBOT = example_robot_data.loadTalosArm()
     RobotTestCase.NQ = 7
     RobotTestCase.NV = 7
 
 
 class TalosArmFloatingTest(RobotTestCase):
-    RobotTestCase.ROBOT = loadTalosArm()
+    RobotTestCase.ROBOT = example_robot_data.loadTalosArm()
     RobotTestCase.NQ = 14
     RobotTestCase.NV = 13
 
 
 class TalosTest(RobotTestCase):
-    RobotTestCase.ROBOT = loadTalos()
+    RobotTestCase.ROBOT = example_robot_data.loadTalos()
     RobotTestCase.NQ = 39
     RobotTestCase.NV = 38
 
+class TalosLegsTest(RobotTestCase):
+    RobotTestCase.ROBOT = example_robot_data.loadTalosLegs()
+    RobotTestCase.NQ = 19
+    RobotTestCase.NV = 18
 
 class HyQTest(RobotTestCase):
-    RobotTestCase.ROBOT = loadHyQ()
+    RobotTestCase.ROBOT = example_robot_data.loadHyQ()
     RobotTestCase.NQ = 19
     RobotTestCase.NV = 18
 
 
 class TiagoTest(RobotTestCase):
-    RobotTestCase.ROBOT = loadTiago()
+    RobotTestCase.ROBOT = example_robot_data.loadTiago()
     RobotTestCase.NQ = 50
     RobotTestCase.NV = 48
 
 
 class TiagoNoHandTest(RobotTestCase):
-    RobotTestCase.ROBOT = loadTiagoNoHand()
+    RobotTestCase.ROBOT = example_robot_data.loadTiagoNoHand()
     RobotTestCase.NQ = 14
     RobotTestCase.NV = 12
 
 
 class ICubTest(RobotTestCase):
-    RobotTestCase.ROBOT = loadICub(reduced=False)
+    RobotTestCase.ROBOT = example_robot_data.loadICub(reduced=False)
     RobotTestCase.NQ = 39
     RobotTestCase.NV = 38