From 8d37abd977cf1cea1087621c0f8c47c2accbc1ac Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Wed, 28 Aug 2019 13:59:00 +0200 Subject: [PATCH 1/7] [cleanup] Removed usless system path append + import the entire example_robot_data module --- python/display.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/python/display.py b/python/display.py index 58f30a1..2a2b095 100644 --- a/python/display.py +++ b/python/display.py @@ -1,8 +1,6 @@ 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 @@ -12,31 +10,31 @@ DISPLAY_TIAGO_NO_HAND = 'tiago_no_hand' in sys.argv DISPLAY_ICUB = 'icub' in sys.argv if DISPLAY_HYQ: - hyq = loadHyQ() + hyq = example_robot_data.loadHyQ() hyq.initDisplay(loadModel=True) hyq.display(hyq.q0) if DISPLAY_TALOS: - talos = loadTalos() + talos = example_robot_data.loadTalos() talos.initDisplay(loadModel=True) talos.display(talos.q0) if DISPLAY_TALOS_ARM: - talos_arm = loadTalosArm() + talos_arm = example_robot_data.loadTalosArm() talos_arm.initDisplay(loadModel=True) talos_arm.display(talos_arm.q0) if DISPLAY_TIAGO: - tiago = loadTiago() + tiago = example_robot_data.loadTiago() tiago.initDisplay(loadModel=True) tiago.display(tiago.q0) if DISPLAY_TIAGO_NO_HAND: - tiago_no_hand = loadTiagoNoHand() + tiago_no_hand = example_robot_data.loadTiagoNoHand() tiago_no_hand.initDisplay(loadModel=True) tiago_no_hand.display(tiago_no_hand.q0) if DISPLAY_ICUB: - icub = loadICub() + icub = example_robot_data.loadICub() icub.initDisplay(loadModel=True) icub.display(icub.q0) -- GitLab From f5883f2cc76fcc3c0a852ddf9723b9151889027f Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Wed, 28 Aug 2019 14:10:42 +0200 Subject: [PATCH 2/7] [talos] Added the talos legs --- python/robots_loader.py | 55 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/python/robots_loader.py b/python/robots_loader.py index de1cf2d..ec1c91d 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" -- GitLab From 1f101bf224eceb31967ff744bf417ab79c2edd57 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Wed, 28 Aug 2019 14:10:59 +0200 Subject: [PATCH 3/7] [talos] Included in the module --- python/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/__init__.py b/python/__init__.py index 3d3f63d..3d3d9da 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) -- GitLab From 842a943a430a2321e5e7432b19feb63f27798817 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Wed, 28 Aug 2019 14:11:31 +0200 Subject: [PATCH 4/7] [display] Updated a deprecated pinocchio function --- python/display.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python/display.py b/python/display.py index 2a2b095..683edc9 100644 --- a/python/display.py +++ b/python/display.py @@ -11,30 +11,30 @@ DISPLAY_ICUB = 'icub' in sys.argv if DISPLAY_HYQ: hyq = example_robot_data.loadHyQ() - hyq.initDisplay(loadModel=True) + hyq.initViewer(loadModel=True) hyq.display(hyq.q0) if DISPLAY_TALOS: talos = example_robot_data.loadTalos() - talos.initDisplay(loadModel=True) + talos.initViewer(loadModel=True) talos.display(talos.q0) if DISPLAY_TALOS_ARM: talos_arm = example_robot_data.loadTalosArm() - talos_arm.initDisplay(loadModel=True) + talos_arm.initViewer(loadModel=True) talos_arm.display(talos_arm.q0) if DISPLAY_TIAGO: tiago = example_robot_data.loadTiago() - tiago.initDisplay(loadModel=True) + tiago.initViewer(loadModel=True) tiago.display(tiago.q0) if DISPLAY_TIAGO_NO_HAND: tiago_no_hand = example_robot_data.loadTiagoNoHand() - tiago_no_hand.initDisplay(loadModel=True) + tiago_no_hand.initViewer(loadModel=True) tiago_no_hand.display(tiago_no_hand.q0) if DISPLAY_ICUB: icub = example_robot_data.loadICub() - icub.initDisplay(loadModel=True) + icub.initViewer(loadModel=True) icub.display(icub.q0) -- GitLab From b7a633cc971c4593e79d28774dff45d662c847ad Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Wed, 28 Aug 2019 14:11:45 +0200 Subject: [PATCH 5/7] [display] Added the talos legs in the display option --- python/display.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/python/display.py b/python/display.py index 683edc9..313e198 100644 --- a/python/display.py +++ b/python/display.py @@ -1,10 +1,10 @@ import sys - 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 @@ -24,6 +24,11 @@ if DISPLAY_TALOS_ARM: 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 = example_robot_data.loadTiago() tiago.initViewer(loadModel=True) -- GitLab From 2a45f195485bd37cac79ab0fec167cddc06041d6 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Wed, 28 Aug 2019 14:14:53 +0200 Subject: [PATCH 6/7] [unittest] Import the entire example_robot_data module --- unittest/test_load.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/unittest/test_load.py b/unittest/test_load.py index 8e1f4e0..6b92f9b 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,43 @@ 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 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 -- GitLab From 0629d81ee967264e13ee7689e22591e7f8c79c15 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Wed, 28 Aug 2019 14:16:11 +0200 Subject: [PATCH 7/7] [unittest] Added the talos legs unit-test --- unittest/test_load.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/unittest/test_load.py b/unittest/test_load.py index 6b92f9b..f5a1260 100755 --- a/unittest/test_load.py +++ b/unittest/test_load.py @@ -38,6 +38,10 @@ class TalosTest(RobotTestCase): 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 = example_robot_data.loadHyQ() -- GitLab