diff --git a/python/__init__.py b/python/__init__.py index 3d3f63d33f71163662a3fb00de55525d7bccf6d0..3d3d9da9d4cbf659620cacbe4e6f721499efb622 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -1,2 +1,2 @@ -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) diff --git a/python/display.py b/python/display.py index 58f30a12d3098d6ff47fa30e0ef46d61a6a2ba07..313e19876b304a87b917ad8882f9c5ad2991e6c1 100644 --- a/python/display.py +++ b/python/display.py @@ -1,42 +1,45 @@ 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) diff --git a/python/robots_loader.py b/python/robots_loader.py index de1cf2d26b0c13171c3aa6b2a0c3fe0d600ff829..ec1c91de8502df35a0e7de4936162294afb22d6d 100644 --- a/python/robots_loader.py +++ b/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" diff --git a/unittest/test_load.py b/unittest/test_load.py index 8e1f4e09a4454818e3c264ee5f8fe0ac21d618dc..f5a12606d472b212afa22861d2cf388bcdc0d0e2 100755 --- a/unittest/test_load.py +++ b/unittest/test_load.py @@ -1,8 +1,7 @@ #!/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