Verified Commit dcc3b9cc authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

Merge pull request #550 from gabrielebndn/topic/pythontest

[python] Fixed python tests and moved them into unit test, fix #533
parents bd0f2d77 aad0c5ae
...@@ -42,9 +42,9 @@ def log(x): ...@@ -42,9 +42,9 @@ def log(x):
if np.isscalar(x): if np.isscalar(x):
return math.log(x) return math.log(x)
if isinstance(x, np.ndarray): if isinstance(x, np.ndarray):
if len(x) == 4: if x.shape == (4, 4):
return se3.log6FromMatrix(x) return se3.log6FromMatrix(x)
if len(x) == 3: if x.shape == (3, 3):
return se3.log3(x) return se3.log3(x)
raise ValueError('Error only 3 and 4 matrices are allowed.') raise ValueError('Error only 3 and 4 matrices are allowed.')
raise ValueError('Error log is only defined for real, matrix3, matrix4 and se3.SE3 objects.') raise ValueError('Error log is only defined for real, matrix3, matrix4 and se3.SE3 objects.')
......
# NOTE: this example needs gepetto-gui to be installed
# usage: launch gepetto-gui and then run this test
import unittest import unittest
import pinocchio as se3 import pinocchio as se3
import numpy as np import numpy as np
...@@ -5,15 +8,14 @@ import os ...@@ -5,15 +8,14 @@ import os
from pinocchio.robot_wrapper import RobotWrapper from pinocchio.robot_wrapper import RobotWrapper
# Warning : the paths are here hard-coded. This file is only here as an example current_file = os.path.dirname(os.path.abspath(__file__))
romeo_model_path = os.path.abspath(os.path.join(current_file, '../models/romeo/romeo_description')) romeo_model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo'))
romeo_model_file = romeo_model_path + "/urdf/romeo.urdf" romeo_model_path = os.path.abspath(os.path.join(romeo_model_dir, 'romeo_description/urdf/romeo.urdf'))
hint_list = [romeo_model_dir, "wrong/hint"] # hint list
list_hints = [romeo_model_path,"titi"] robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
robot = RobotWrapper(romeo_model_file,list_hints, se3.JointModelFreeFlyer())
robot.initDisplay() robot.initDisplay()
robot.loadDisplayModel("world/pinocchio") robot.loadDisplayModel("pinocchio")
q0 = np.matrix([ q0 = np.matrix([
0, 0, 0.840252, 0, 0, 0, 1, # Free flyer 0, 0, 0.840252, 0, 0, 0, 1, # Free flyer
...@@ -26,4 +28,3 @@ q0 = np.matrix([ ...@@ -26,4 +28,3 @@ q0 = np.matrix([
]).T ]).T
robot.display(q0) robot.display(q0)
import pinocchio as pin import pinocchio as pin
from pinocchio.romeo_wrapper import RomeoWrapper from pinocchio.romeo_wrapper import RomeoWrapper
DISPLAY = False
## Load Romeo with RomeoWrapper ## Load Romeo with RomeoWrapper
import os import os
current_path = os.getcwd() current_path = os.getcwd()
...@@ -24,17 +22,3 @@ q0 = robot.q0 ...@@ -24,17 +22,3 @@ q0 = robot.q0
com = robot.com(q0) com = robot.com(q0)
# This last command is similar to # This last command is similar to
com2 = pin.centerOfMass(model,data,q0) com2 = pin.centerOfMass(model,data,q0)
## load model into gepetto-gui
if DISPLAY:
import gepetto.corbaserver
cl = gepetto.corbaserver.Client()
gui = cl.gui
if gui.nodeExists("world"):
gui.deleteNode("world","ON")
robot.initDisplay(loadModel=False)
robot.loadDisplayModel("pinocchio")
robot.display(robot.q0)
gui = robot.viewer.gui
...@@ -3,6 +3,8 @@ from pinocchio.utils import np, npl, rand, skew, zero ...@@ -3,6 +3,8 @@ from pinocchio.utils import np, npl, rand, skew, zero
from test_case import TestCase from test_case import TestCase
# This whole file seems to be outdated and superseded by more recent tests
# Probably it should be removed and its contents moved or split somewhere else
class TestSE3(TestCase): class TestSE3(TestCase):
def setUp(self): def setUp(self):
...@@ -15,7 +17,7 @@ class TestSE3(TestCase): ...@@ -15,7 +17,7 @@ class TestSE3(TestCase):
R, p, m = self.R, self.p, self.m R, p, m = self.R, self.p, self.m
X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])]) X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X) self.assertApprox(m.action, X)
M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)]) M = np.vstack([np.hstack([R, p]), np.matrix([0., 0., 0., 1.], np.double)])
self.assertApprox(m.homogeneous, M) self.assertApprox(m.homogeneous, M)
m2 = se3.SE3.Random() m2 = se3.SE3.Random()
self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous) self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
...@@ -25,20 +27,25 @@ class TestSE3(TestCase): ...@@ -25,20 +27,25 @@ class TestSE3(TestCase):
self.assertApprox(m * p, m.rotation * p + m.translation) self.assertApprox(m * p, m.rotation * p + m.translation)
self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation) self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
p = np.vstack([p, 1]) ## not supported
self.assertApprox(m * p, m.homogeneous * p) # p = np.vstack([p, 1])
self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p) # self.assertApprox(m * p, m.homogeneous * p)
# self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
p = rand(6) ## not supported
self.assertApprox(m * p, m.action * p) # p = rand(6)
self.assertApprox(m.actInv(p), npl.inv(m.action) * p) # self.assertApprox(m * p, m.action * p)
# self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
# Currently, the different cases do not throw the same exception type.
# To have a more robust test, only Exception is checked.
# In the comments, the most specific actual exception class at the time of writing
p = rand(5) p = rand(5)
with self.assertRaises(ValueError): with self.assertRaises(Exception): # RuntimeError
m * p m * p
with self.assertRaises(ValueError): with self.assertRaises(Exception): # RuntimeError
m.actInv(p) m.actInv(p)
with self.assertRaises(ValueError): with self.assertRaises(Exception): # Boost.Python.ArgumentError (subclass of TypeError)
m.actInv('42') m.actInv('42')
def test_motion(self): def test_motion(self):
...@@ -50,7 +57,7 @@ class TestSE3(TestCase): ...@@ -50,7 +57,7 @@ class TestSE3(TestCase):
vv = v.linear vv = v.linear
vw = v.angular vw = v.angular
self.assertApprox(v.vector, np.vstack([vv, vw])) self.assertApprox(v.vector, np.vstack([vv, vw]))
self.assertApprox((v ** v).vector, zero(6)) self.assertApprox((v ^ v).vector, zero(6))
def test_force(self): def test_force(self):
m = self.m m = self.m
...@@ -64,7 +71,7 @@ class TestSE3(TestCase): ...@@ -64,7 +71,7 @@ class TestSE3(TestCase):
self.assertApprox((m.actInv(f)).vector, m.action.T * f.vector) self.assertApprox((m.actInv(f)).vector, m.action.T * f.vector)
v = se3.Motion.Random() v = se3.Motion.Random()
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]])) f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
self.assertApprox((v ** f).vector, zero(6)) self.assertApprox((v ^ f).vector, zero(6))
def test_inertia(self): def test_inertia(self):
m = self.m m = self.m
...@@ -80,11 +87,14 @@ class TestSE3(TestCase): ...@@ -80,11 +87,14 @@ class TestSE3(TestCase):
def test_cross(self): def test_cross(self):
m = se3.Motion.Random() m = se3.Motion.Random()
f = se3.Force.Random() f = se3.Force.Random()
self.assertApprox(m ** m, m.cross_motion(m)) self.assertApprox(m ^ m, m.cross(m))
self.assertApprox(m ** f, m.cross_force(f)) self.assertApprox(m ^ f, m.cross(f))
with self.assertRaises(ValueError): with self.assertRaises(TypeError):
m ** 2 m ^ 2
def test_exp(self): def test_exp(self):
m = se3.Motion.Random() m = se3.Motion.Random()
self.assertApprox(se3.exp(m), se3.exp6FromMotion(m)) self.assertApprox(se3.exp(m), se3.exp6FromMotion(m))
if __name__ == '__main__':
unittest.main()
\ No newline at end of file
import unittest import unittest
import pinocchio as se3 import pinocchio as se3
import numpy as np import numpy as np
from pinocchio.utils import eye,zero,rand from pinocchio.utils import eye,zero,rand,skew
ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double) ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
...@@ -45,8 +45,18 @@ class TestSE3Bindings(unittest.TestCase): ...@@ -45,8 +45,18 @@ class TestSE3Bindings(unittest.TestCase):
def test_homogeneous(self): def test_homogeneous(self):
amb = se3.SE3.Random() amb = se3.SE3.Random()
aMb = amb.homogeneous aMb = amb.homogeneous
self.assertTrue(np.allclose(aMb[0:3,0:3], amb.rotation)) # bloc 33 = rotation de amb self.assertTrue(np.allclose(aMb[0:3,0:3], amb.rotation)) # top left 33 corner = rotation of amb
self.assertTrue(np.allclose(aMb[0:3,3], amb.translation)) # derniere colonne = translation de amb self.assertTrue(np.allclose(aMb[0:3,3], amb.translation)) # top 3 of last column = translation of amb
self.assertTrue(np.allclose(aMb[3,:], [0.,0.,0.,1.])) # last row = 0 0 0 1
def test_action_matrix(self):
amb = se3.SE3.Random()
aXb = amb.action
self.assertTrue(np.allclose(aXb[:3,:3], amb.rotation)) # top left 33 corner = rotation of amb
self.assertTrue(np.allclose(aXb[3:,3:], amb.rotation)) # bottom right 33 corner = rotation of amb
tblock = skew(amb.translation)*amb.rotation
self.assertTrue(np.allclose(aXb[:3,3:], tblock)) # top right 33 corner = translation + rotation
self.assertTrue(np.allclose(aXb[3:,:3], self.m3zero)) # bottom left 33 corner = 0
def test_inverse(self): def test_inverse(self):
amb = se3.SE3.Random() amb = se3.SE3.Random()
...@@ -54,17 +64,33 @@ class TestSE3Bindings(unittest.TestCase): ...@@ -54,17 +64,33 @@ class TestSE3Bindings(unittest.TestCase):
bma = amb.inverse() bma = amb.inverse()
self.assertTrue(np.allclose(np.linalg.inv(aMb), bma.homogeneous)) self.assertTrue(np.allclose(np.linalg.inv(aMb), bma.homogeneous))
def test_internal_product(self): def test_internal_product_vs_homogeneous(self):
amb = se3.SE3.Random() amb = se3.SE3.Random()
aMb = amb.homogeneous
bmc = se3.SE3.Random() bmc = se3.SE3.Random()
bMc = bmc.homogeneous
amc = amb*bmc amc = amb*bmc
cma = amc.inverse() cma = amc.inverse()
aMb = amb.homogeneous
bMc = bmc.homogeneous
aMc = amc.homogeneous aMc = amc.homogeneous
cMa = np.linalg.inv(aMc)
self.assertTrue(np.allclose(aMb*bMc, aMc)) self.assertTrue(np.allclose(aMb*bMc, aMc))
self.assertTrue(np.allclose(cma.homogeneous,np.linalg.inv(aMc))) self.assertTrue(np.allclose(cma.homogeneous,cMa))
def test_internal_product_vs_action(self):
amb = se3.SE3.Random()
bmc = se3.SE3.Random()
amc = amb * bmc
cma = amc.inverse()
aXb = amb.action
bXc = bmc.action
aXc = amc.action
cXa = np.linalg.inv(aXc)
self.assertTrue(np.allclose(aXb*bXc, aXc))
self.assertTrue(np.allclose(cma.action,cXa))
def test_point_action(self): def test_point_action(self):
amb = se3.SE3.Random() amb = se3.SE3.Random()
...@@ -74,20 +100,12 @@ class TestSE3Bindings(unittest.TestCase): ...@@ -74,20 +100,12 @@ class TestSE3Bindings(unittest.TestCase):
p = p_homogeneous[0:3].copy() p = p_homogeneous[0:3].copy()
# act # act
self.assertTrue(np.allclose(amb.act_point(p),(aMb*p_homogeneous)[0:3])) self.assertTrue(np.allclose(amb.act(p),(aMb*p_homogeneous)[0:3]))
# actinv # actinv
bMa = np.linalg.inv(aMb) bMa = np.linalg.inv(aMb)
bma = amb.inverse() bma = amb.inverse()
self.assertTrue(np.allclose(bma.act_point(p), (bMa * p_homogeneous)[0:3])) self.assertTrue(np.allclose(bma.act(p), (bMa * p_homogeneous)[0:3]))
def test_action_matrix(self):
amb = se3.SE3.Random()
bmc = se3.SE3.Random()
amc = amb * bmc
aXb = amb.action
bXc = bmc.action
aXc = aXb * bXc
self.assertTrue(np.allclose(amc.action,aXc))
if __name__ == '__main__':
unittest.main()
...@@ -20,6 +20,7 @@ class TestForceBindings(unittest.TestCase): ...@@ -20,6 +20,7 @@ class TestForceBindings(unittest.TestCase):
self.assertTrue(np.allclose(self.v3zero, f.angular)) self.assertTrue(np.allclose(self.v3zero, f.angular))
self.assertTrue(np.allclose(self.v6zero, f.vector)) self.assertTrue(np.allclose(self.v6zero, f.vector))
# TODO: this is not nice, since a random vector can *in theory* be zero
def test_setRandom(self): def test_setRandom(self):
f = se3.Force.Zero() f = se3.Force.Zero()
f.setRandom() f.setRandom()
...@@ -37,7 +38,7 @@ class TestForceBindings(unittest.TestCase): ...@@ -37,7 +38,7 @@ class TestForceBindings(unittest.TestCase):
def test_set_linear(self): def test_set_linear(self):
f = se3.Force.Zero() f = se3.Force.Zero()
lin = rand(3) # TODO np.matrix([1,2,3],np.double) OR np.matrix( np.array([1,2,3], np.double), np.double) lin = rand(3)
f.linear = lin f.linear = lin
self.assertTrue(np.allclose(f.linear, lin)) self.assertTrue(np.allclose(f.linear, lin))
...@@ -62,11 +63,11 @@ class TestForceBindings(unittest.TestCase): ...@@ -62,11 +63,11 @@ class TestForceBindings(unittest.TestCase):
def test_se3_action(self): def test_se3_action(self):
f = se3.Force.Random() f = se3.Force.Random()
m = se3.SE3.Random() m = se3.SE3.Random()
self.assertTrue(np.allclose((m * f).vector, np.linalg.inv(m.action.T) * f.vector)) self.assertTrue(np.allclose((m * f).vector, np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(np.allclose(m.act(f).vector, np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(np.allclose((m.actInv(f)).vector, m.action.T * f.vector)) self.assertTrue(np.allclose((m.actInv(f)).vector, m.action.T * f.vector))
v = se3.Motion.Random() v = se3.Motion(np.vstack([f.vector[3:], f.vector[:3]]))
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]])) self.assertTrue(np.allclose((v ^ f).vector, zero(6)))
self.assertTrue(np.allclose((v ** f).vector, zero(6)))
if __name__ == '__main__':
unittest.main()
...@@ -2,7 +2,6 @@ import unittest ...@@ -2,7 +2,6 @@ import unittest
import pinocchio as se3 import pinocchio as se3
import numpy as np import numpy as np
from pinocchio.utils import eye,zero,rand from pinocchio.utils import eye,zero,rand
from pinocchio.robot_wrapper import RobotWrapper
import os import os
ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double) ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
...@@ -17,36 +16,43 @@ class TestFrameBindings(unittest.TestCase): ...@@ -17,36 +16,43 @@ class TestFrameBindings(unittest.TestCase):
m3ones = eye(3) m3ones = eye(3)
m4ones = eye(4) m4ones = eye(4)
current_file = os.path.dirname(os.path.abspath(__file__)) def setUp(self):
pinocchio_models_dir = os.path.abspath(os.path.join(current_file, '../models/romeo/romeo_description')) self.model = se3.Model.BuildHumanoidSimple()
romeo_model_path = os.path.abspath(os.path.join(pinocchio_mdoels_dir, '/urdf/romeo.urdf')) self.parent_idx = self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") else (self.model.njoints-1)
hint_list = [pinocchio_models_dir, "wrong/hint"] # hint list self.frame_name = self.model.names[self.parent_idx] + "_frame"
robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer()) self.frame_placement = se3.SE3.Random()
self.frame_type = se3.FrameType.OP_FRAME
self.model.addFrame(se3.Frame(self.frame_name, self.parent_idx, 0, self.frame_placement, self.frame_type))
self.frame_idx = self.model.getFrameId(self.frame_name)
def tearDown(self):
del self.model
def test_type_get_set(self): def test_type_get_set(self):
f = self.robot.model.frames[5] f = self.model.frames[self.frame_idx]
self.assertTrue(f.type == se3.FrameType.JOINT) self.assertTrue(f.type == self.frame_type)
f.type = se3.FrameType.BODY f.type = se3.FrameType.BODY
self.assertTrue(f.type == se3.FrameType.BODY) self.assertTrue(f.type == se3.FrameType.BODY)
def test_name_get_set(self): def test_name_get_set(self):
f = self.robot.model.frames[5] f = self.model.frames[self.frame_idx]
self.assertTrue(f.name == 'LHipYaw') self.assertTrue(f.name == self.frame_name)
f.name = 'new_hip_frame' f.name = 'new_hip_frame'
self.assertTrue(f.name == 'new_hip_frame') self.assertTrue(f.name == 'new_hip_frame')
def test_parent_get_set(self): def test_parent_get_set(self):
f = self.robot.model.frames[5] f = self.model.frames[self.frame_idx]
self.assertTrue(f.parent == 2) self.assertTrue(f.parent == self.parent_idx)
f.parent = 5 newparent = self.parent_idx-1
self.assertTrue(f.parent == 5) f.parent = newparent
self.assertTrue(f.parent == newparent)
def test_placement_get_set(self): def test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0,0,0],np.double)) f = self.model.frames[self.frame_idx]
new_m = se3.SE3(rand([3,3]), rand(3)) self.assertTrue(np.allclose(f.placement.homogeneous, self.frame_placement.homogeneous))
f = self.robot.model.frames[2] new_placement = se3.SE3.Random()
self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous)) f.placement = new_placement
f.placement = new_m self.assertTrue(np.allclose(f.placement.homogeneous, new_placement.homogeneous))
self.assertTrue(np.allclose(f.placement.homogeneous, new_m.homogeneous))
if __name__ == '__main__':
unittest.main()
...@@ -2,11 +2,19 @@ import unittest ...@@ -2,11 +2,19 @@ import unittest
import pinocchio as se3 import pinocchio as se3
import numpy as np import numpy as np
from pinocchio.utils import eye,zero,rand from pinocchio.utils import eye,zero,rand
from pinocchio.robot_wrapper import RobotWrapper
import os import os
ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double) ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
# TODO: remove these lines. Do not use RobotWrapper. Do not use URDF
from pinocchio.robot_wrapper import RobotWrapper
current_file = os.path.dirname(os.path.abspath(__file__))
romeo_model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo'))
romeo_model_path = os.path.abspath(os.path.join(romeo_model_dir, 'romeo_description/urdf/romeo.urdf'))
hint_list = [romeo_model_dir, "wrong/hint"] # hint list
robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
expected_mesh_path = os.path.join(romeo_model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae')
class TestGeometryObjectBindings(unittest.TestCase): class TestGeometryObjectBindings(unittest.TestCase):
v3zero = zero(3) v3zero = zero(3)
...@@ -17,21 +25,24 @@ class TestGeometryObjectBindings(unittest.TestCase): ...@@ -17,21 +25,24 @@ class TestGeometryObjectBindings(unittest.TestCase):
m3ones = eye(3) m3ones = eye(3)
m4ones = eye(4) m4ones = eye(4)
# WARNING: the collision model is the same object is the same for all the tests.
current_file = os.path.dirname(os.path.abspath(__file__)) # This can cause problems if a test expects the collision model to be in a certain way but a previous test has changed it
pinocchio_models_dir = os.path.abspath(os.path.join(current_file, '../models/romeo/romeo_description')) # Still, at the moment this is not the case.
romeo_model_path = os.path.abspath(pinocchio_models_dir, '/urdf/romeo.urdf') # Loading the URDF and related meshes before each test would make the whole unit test run too slowly
hint_list = [pinocchio_models_dir, "wrong/hint"] # hint list #
robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer()) # TODO: do not use URDF. Then, build self.collision_model from scratch for each test
def setUp(self):
self.collision_model = robot.collision_model
self.expected_mesh_path = expected_mesh_path
def test_name_get_set(self): def test_name_get_set(self):
col = self.robot.collision_model.geometryObjects[1] col = self.collision_model.geometryObjects[1]
self.assertTrue(col.name == 'LHipPitchLink_0') self.assertTrue(col.name == 'LHipPitchLink_0')
col.name = 'new_collision_name' col.name = 'new_collision_name'
self.assertTrue(col.name == 'new_collision_name') self.assertTrue(col.name == 'new_collision_name')
def test_parent_get_set(self): def test_parent_get_set(self):
col = self.robot.collision_model.geometryObjects[1] col = self.collision_model.geometryObjects[1]
self.assertTrue(col.parentJoint == 4) self.assertTrue(col.parentJoint == 4)
col.parentJoint = 5 col.parentJoint = 5
self.assertTrue(col.parentJoint == 5) self.assertTrue(col.parentJoint == 5)
...@@ -39,15 +50,14 @@ class TestGeometryObjectBindings(unittest.TestCase): ...@@ -39,15 +50,14 @@ class TestGeometryObjectBindings(unittest.TestCase):
def test_placement_get_set(self): def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero) m = se3.SE3(self.m3ones, self.v3zero)
new_m = se3.SE3(rand([3,3]), rand(3)) new_m = se3.SE3(rand([3,3]), rand(3))
col = self.robot.collision_model.geometryObjects[1] col = self.collision_model.geometryObjects[1]
self.assertTrue(np.allclose(col.placement.homogeneous,m.homogeneous)) self.assertTrue(np.allclose(col.placement.homogeneous,m.homogeneous))
col.placement = new_m col.placement = new_m
self.assertTrue(np.allclose(col.placement.homogeneous , new_m.homogeneous)) self.assertTrue(np.allclose(col.placement.homogeneous , new_m.homogeneous))
def test_meshpath_get(self): def test_meshpath_get(self):
expected_mesh_path = os.path.join(self.pinocchio_models_dir,'meshes/romeo/collision/LHipPitch.dae') col = self.collision_model.geometryObjects[1]
col = self.robot.collision_model.geometryObjects[1] self.assertTrue(col.meshPath == self.expected_mesh_path)
self.assertTrue(col.meshPath == expected_mesh_path)
if __name__ == '__main__': if __name__ == '__main__':
unittest.main() unittest.main()
...@@ -27,7 +27,7 @@ class TestInertiaBindings(unittest.TestCase): ...@@ -27,7 +27,7 @@ class TestInertiaBindings(unittest.TestCase):
self.assertTrue(np.allclose(self.v3zero, Y.lever)) self.assertTrue(np.allclose(self.v3zero, Y.lever))
self.assertTrue(np.allclose(self.m3ones, Y.inertia)) self.assertTrue(np.allclose(self.m3ones, Y.inertia))
# TODO: this is not nice, since a random matrix can *in theory* be unity
def test_setRandom(self): def test_setRandom(self):