From 330099c744c31510a1e267d699b5e9df90e292ce Mon Sep 17 00:00:00 2001
From: Carlos Mastalli <carlos.mastalli@laas.fr>
Date: Wed, 20 Feb 2019 10:37:37 +0100
Subject: [PATCH] [unittest] Added loading test for Talos, Talos arm and HyQ

---
 unittest/test_load.py      | 44 +++++++++++++++++++++++++++
 unittest/unittest_utils.py | 62 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 106 insertions(+)
 create mode 100644 unittest/test_load.py
 create mode 100644 unittest/unittest_utils.py

diff --git a/unittest/test_load.py b/unittest/test_load.py
new file mode 100644
index 0000000..5e2a02d
--- /dev/null
+++ b/unittest/test_load.py
@@ -0,0 +1,44 @@
+from unittest_utils import loadTalosArm, loadTalos, loadHyQ
+import unittest
+
+
+class RobotTestCase(unittest.TestCase):
+    ROBOT = None
+    NQ = None
+    NV = None
+
+    def test_nq(self):
+        model = self.ROBOT.model
+        self.assertEqual(model.nq, self.NQ, "Wrong nq value.")
+
+    def test_nv(self):
+        model = self.ROBOT.model
+        self.assertEqual(model.nv, self.NV, "Wrong nv value.")
+
+    def test_q0(self):
+      self.assertTrue(hasattr(self.ROBOT, "q0"), "It doesn't have q0")
+
+class TalosArmTest(RobotTestCase):
+    RobotTestCase.ROBOT = loadTalosArm()
+    RobotTestCase.NQ = 7
+    RobotTestCase.NV = 7
+
+class TalosArmFloatingTest(RobotTestCase):
+    RobotTestCase.ROBOT = loadTalosArm()
+    RobotTestCase.NQ = 14
+    RobotTestCase.NV = 13
+
+class TalosTest(RobotTestCase):
+    RobotTestCase.ROBOT = loadTalos()
+    RobotTestCase.NQ = 39
+    RobotTestCase.NV = 38
+
+class HyQTest(RobotTestCase):
+    RobotTestCase.ROBOT = loadHyQ()
+    RobotTestCase.NQ = 19
+    RobotTestCase.NV = 18
+
+
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/unittest/unittest_utils.py b/unittest/unittest_utils.py
new file mode 100644
index 0000000..d6ec1c6
--- /dev/null
+++ b/unittest/unittest_utils.py
@@ -0,0 +1,62 @@
+import pinocchio
+from pinocchio.utils import *
+from pinocchio.robot_wrapper import RobotWrapper
+
+
+
+def readParamsFromSrdf(robot, SRDF_PATH, verbose):
+  rmodel = robot.model
+
+  pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
+  rmodel.armature = \
+                    np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
+  try:
+    pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
+    robot.q0.flat[:] = rmodel.referenceConfigurations["half_sitting"].copy()
+  except:
+    print "loadReferenceConfigurations did not work. Please check your Pinocchio Version"
+    try:
+      pinocchio.getNeutralConfiguration(rmodel, SRDF_PATH, verbose)
+      robot.q0.flat[:] = rmodel.neutralConfiguration.copy()
+    except:
+      robot.q0.flat[:] = pinocchio.neutral(rmodel)
+  return
+  
+def loadTalosArm(modelPath='/opt/openrobots/share/example-robot-data'):
+    URDF_FILENAME = "talos_left_arm.urdf"
+    URDF_SUBPATH = "/talos/" + URDF_FILENAME
+    SRDF_FILENAME = "talos.srdf"
+    SRDF_SUBPATH = "/talos/" + SRDF_FILENAME
+    # Load URDF file
+    robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath])
+
+    # Load SRDF file
+    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
+    return robot
+
+def loadTalos(modelPath='/opt/openrobots/share/example-robot-data'):
+    URDF_FILENAME = "talos_reduced.urdf"
+    SRDF_FILENAME = "talos.srdf"
+    SRDF_SUBPATH = "/talos/" + SRDF_FILENAME
+    URDF_SUBPATH = "/talos/" + URDF_FILENAME
+    # Load URDF file
+    robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath],
+                                                               pinocchio.JointModelFreeFlyer())
+    # Load SRDF file
+    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
+    assert((robot.model.armature[:6]==0.).all())
+    return robot
+
+
+def loadHyQ(modelPath='/opt/openrobots/share/example-robot-data'):
+    from pinocchio import JointModelFreeFlyer
+    import os
+    URDF_FILENAME = "hyq_no_sensors.urdf"
+    URDF_SUBPATH = "/hyq/" + URDF_FILENAME
+    robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath],
+                                       pinocchio.JointModelFreeFlyer())
+    # TODO define default position inside srdf
+    robot.q0.flat[7:] = [-0.2, 0.75, -1.5, -0.2, -0.75, 1.5, -0.2, 0.75, -1.5, -0.2, -0.75, 1.5]
+    robot.q0[2] = 0.57750958
+    robot.model.referenceConfigurations["half_sitting"] = robot.q0
+    return robot
-- 
GitLab