Commit 8f922511 authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

[unittest] Added loading test for Talos, Talos arm and HyQ

parent 8f22bb30
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()
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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment