def test_explog(self):
self.assertApprox(exp(42), math.exp(42))
self.assertApprox(log(42), math.log(42))
self.assertApprox(exp(log(42)), 42)
self.assertApprox(log(exp(42)), 42)
m = rand(3)
self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test
self.assertApprox(log(exp(m)), m)
m = pin.SE3.Random()
self.assertApprox(exp(log(m)), m)
m = rand(6)
self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test (actually, only angular part)
self.assertApprox(log(exp(m)), m)
m = eye(4)
self.assertApprox(exp(log(m)).homogeneous, m)
with self.assertRaises(ValueError):
exp(eye(4))
with self.assertRaises(ValueError):
exp(list(range(3)))
with self.assertRaises(ValueError):
log(list(range(3)))
with self.assertRaises(ValueError):
log(zero(5))
with self.assertRaises(ValueError):
log(zero((3,1)))
def test_explog(self):
self.assertApprox(exp(42), math.exp(42))
self.assertApprox(log(42), math.log(42))
self.assertApprox(exp(log(42)), 42)
self.assertApprox(log(exp(42)), 42)
m = rand(3)
self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test
self.assertApprox(log(exp(m)), m)
m = pin.SE3.Random()
self.assertApprox(exp(log(m)), m)
m = rand(6)
self.assertTrue(
np.linalg.norm(m) <
np.pi) # necessary for next test (actually, only angular part)
self.assertApprox(log(exp(m)), m)
m = eye(4)
self.assertApprox(exp(log(m)).homogeneous, m)
with self.assertRaises(ValueError):
exp(eye(4))
with self.assertRaises(ValueError):
exp(list(range(3)))
with self.assertRaises(ValueError):
log(list(range(3)))
with self.assertRaises(ValueError):
log(zero(5))
with self.assertRaises(ValueError):
log(zero((3, 1)))
def test_identity(self):
transform = se3.SE3.Identity()
self.assertTrue(np.allclose(zero(3), transform.translation))
self.assertTrue(np.allclose(eye(3), transform.rotation))
self.assertTrue(np.allclose(eye(4), transform.homogeneous))
self.assertTrue(np.allclose(eye(6), transform.action))
transform.setRandom()
transform.setIdentity()
self.assertTrue(np.allclose(eye(4), transform.homogeneous))
def test_identity(self):
transform = pin.SE3.Identity()
self.assertTrue(np.allclose(zero(3),transform.translation))
self.assertTrue(np.allclose(eye(3), transform.rotation))
self.assertTrue(np.allclose(eye(4), transform.homogeneous))
self.assertTrue(np.allclose(eye(6), transform.action))
transform.setRandom()
transform.setIdentity()
self.assertTrue(np.allclose(eye(4), transform.homogeneous))
def PlaySequence(self, vis_forces=False, pause=False):
'''
Visualize the human joints extracted from c3d.
'''
self.viewer.ShowXYZAxis(False)
for i in range(self.num_frames_in_video):
joint_3d_positions_reshaped = np.matrix(
self.joint_3d_positions[i].reshape(-1)).T
self.joint_visuals.Display(joint_3d_positions_reshaped)
if vis_forces:
for n in range(self.num_contact_forces):
joint_id = self.contact_force_joint_map[n]
oMc = se3.SE3(
eye(3),
np.matrix(self.joint_3d_positions[i, joint_id, :]).T)
cPhic = np.matrix(self.contact_forces_local[i, n, :]).T
if self.mask_uncaptured_forces[i, n] == 1:
color_linear_force = self.force_colors[2]
else:
color_linear_force = self.force_colors[0]
self.PlaceForceArrow(self.contact_force_names[n],
oMc,
cPhic,
color_linear_force=color_linear_force)
self.viewer.gui.refresh()
time.sleep(1 / self.fps_video)
if pause:
raw_input('#{}. Press any key to continue...'.format(i))
self.viewer.ShowXYZAxis(True)
def test_Jexp6(self):
v = pin.Motion.Zero()
J = pin.Jexp6(v)
self.assertApprox(J, eye(6))
J2 = pin.Jexp6(np.array(v))
self.assertApprox(J, J2)
class TestFrameBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3, 3])
m6zero = zero([6, 6])
m3ones = eye(3)
m4ones = eye(4)
current_file = os.path.dirname(os.path.abspath(__file__))
pinocchio_models_dir = os.path.abspath(
os.path.join(current_file, '../models/romeo'))
romeo_model_path = os.path.abspath(
os.path.join(pinocchio_mdoels_dir, '/urdf/romeo.urdf'))
hint_list = [pinocchio_models_dir, "wrong/hint"] # hint list
robot = RobotWrapper(romeo_model_path, hint_list,
se3.JointModelFreeFlyer())
def test_type_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.type == se3.FrameType.JOINT)
f.type = se3.FrameType.BODY
self.assertTrue(f.type == se3.FrameType.BODY)
def test_name_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.name == 'LHipYaw')
f.name = 'new_hip_frame'
self.assertTrue(f.name == 'new_hip_frame')
def test_parent_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.parent == 2)
f.parent = 5
self.assertTrue(f.parent == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0, 0, 0], np.double))
new_m = se3.SE3(rand([3, 3]), rand(3))
f = self.robot.model.frames[2]
self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous))
f.placement = new_m
self.assertTrue(np.allclose(f.placement.homogeneous,
new_m.homogeneous))
class TestGeometryObjectBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3, 3])
m6zero = zero([6, 6])
m3ones = eye(3)
m4ones = eye(4)
current_file = os.path.dirname(os.path.abspath(__file__))
pinocchio_models_dir = os.path.abspath(
os.path.join(current_file, '../models/romeo'))
romeo_model_path = os.path.abspath(pinocchio_models_dir,
'/urdf/romeo.urdf')
hint_list = [pinocchio_models_dir, "wrong/hint"] # hint list
robot = RobotWrapper(romeo_model_path, hint_list,
se3.JointModelFreeFlyer())
def test_name_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.name == 'LHipPitchLink_0')
col.name = 'new_collision_name'
self.assertTrue(col.name == 'new_collision_name')
def test_parent_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.parentJoint == 4)
col.parentJoint = 5
self.assertTrue(col.parentJoint == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
new_m = se3.SE3(rand([3, 3]), rand(3))
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(np.allclose(col.placement.homogeneous, m.homogeneous))
col.placement = new_m
self.assertTrue(
np.allclose(col.placement.homogeneous, new_m.homogeneous))
def test_meshpath_get(self):
expected_mesh_path = os.path.join(
self.pinocchio_models_dir, 'meshes/romeo/collision/LHipPitch.dae')
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.meshPath == expected_mesh_path)
def solve(self, eps=1e-8):
'''
Implement a LCQP solver, with numerical threshold eps.
'''
Cp = npl.pinv(self.C, eps)
xopt = Cp * self.d
P = eye(self.nx * self.N) - Cp * self.C
xopt += npl.pinv(self.A * P, eps) * (self.b - self.A * xopt)
return xopt
def solve(self, eps=1e-8):
'''
Implement a LCQP solver, with numerical threshold eps.
'''
Cp = npl.pinv(self.C, eps)
xopt = Cp * self.d
P = eye(self.nx * self.N) - Cp * self.C
xopt += npl.pinv(self.A * P, eps) * (self.b - self.A * xopt)
return xopt
def __init__(self, urdf, pkgs):
super(MobileRobotWrapper, self).__init__(self, urdf, pkgs, pin.JointModelPlanar())
M0 = pin.SE3(eye(3), np.matrix([.0, .0, .6]).T)
self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2]
self.visual_model.geometryObjects[0].placement = M0 * self.visual_model.geometryObjects[0].placement
self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0]
# Placement of the "mobile" frame wrt basis center.
basisMop = pin.SE3(eye(3), np.matrix([.3, .0, .1]).T)
self.model.addFrame(pin.Frame('mobile', 1, 1, basisMop, pin.FrameType.OP_FRAME))
# Placement of the tool frame wrt end effector frame (located at the center of the wrist)
effMop = pin.SE3(eye(3), np.matrix([.0, .08, .095]).T)
self.model.addFrame(pin.Frame('tool', 6, 6, effMop, pin.FrameType.OP_FRAME))
# Create data again after setting frames
self.data = self.model.createData()
def __init__(self, urdf, pkgs):
self.initFromURDF(urdf, pkgs, pin.JointModelPlanar())
M0 = pin.SE3(eye(3), np.matrix([.0, .0, .6]).T)
self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2]
self.visual_model.geometryObjects[
0].placement = M0 * self.visual_model.geometryObjects[0].placement
self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0]
# Placement of the "mobile" frame wrt basis center.
basisMop = pin.SE3(eye(3), np.matrix([.3, .0, .1]).T)
self.model.addFrame(
pin.Frame('mobile', 1, 1, basisMop, pin.FrameType.OP_FRAME))
# Placement of the tool frame wrt end effector frame (located at the center of the wrist)
effMop = pin.SE3(eye(3), np.matrix([.0, .08, .095]).T)
self.model.addFrame(
pin.Frame('tool', 6, 6, effMop, pin.FrameType.OP_FRAME))
# Create data again after setting frames
self.data = self.model.createData()
class TestFrameBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3, 3])
m6zero = zero([6, 6])
m3ones = eye(3)
m4ones = eye(4)
PKG = '/opt/openrobots/share'
URDF = join(PKG, 'romeo_description/urdf/romeo.urdf')
robot = RobotWrapper(URDF, [PKG])
def test_type_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.type == se3.FrameType.JOINT)
f.type = se3.FrameType.BODY
self.assertTrue(f.type == se3.FrameType.BODY)
def test_name_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.name == 'LHipYaw')
f.name = 'new_hip_frame'
self.assertTrue(f.name == 'new_hip_frame')
def test_parent_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.parent == 2)
f.parent = 5
self.assertTrue(f.parent == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0, 0, 0], np.double))
new_m = se3.SE3(rand([3, 3]), rand(3))
f = self.robot.model.frames[2]
self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous))
f.placement = new_m
self.assertTrue(np.allclose(f.placement.homogeneous,
new_m.homogeneous))
class TestFrameBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3, 3])
m6zero = zero([6, 6])
m3ones = eye(3)
m4ones = eye(4)
current_file = os.path.dirname(os.path.abspath(__file__))
pinocchio_models_dir = os.path.abspath(
os.path.join(current_file, '../models'))
romeo_model_path = os.path.abspath(
os.path.join(current_file, '../models/romeo.urdf'))
hint_list = [pinocchio_models_dir, "wrong/hint"] # hint list
robot = RobotWrapper(romeo_model_path, hint_list,
se3.JointModelFreeFlyer())
def test_name_get_set(self):
f = self.robot.model.operational_frames[1]
self.assertTrue(f.name == 'r_sole_joint')
f.name = 'new_sole_joint'
self.assertTrue(f.name == 'new_sole_joint')
def test_parent_get_set(self):
f = self.robot.model.operational_frames[1]
self.assertTrue(f.parent == 13)
f.parent = 5
self.assertTrue(f.parent == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0, 0, -0.0684], np.double))
new_m = se3.SE3(rand([3, 3]), rand(3))
f = self.robot.model.operational_frames[1]
self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous))
f.placement = new_m
self.assertTrue(np.allclose(f.placement.homogeneous,
new_m.homogeneous))
class TestGeometryObjectBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3,3])
m6zero = zero([6,6])
m3ones = eye(3)
m4ones = eye(4)
PKG = '/opt/openrobots/share'
URDF = join(PKG, 'romeo_description/urdf/romeo.urdf')
robot = RobotWrapper(URDF, [PKG])
def test_name_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.name == 'LHipPitchLink_0')
col.name = 'new_collision_name'
self.assertTrue(col.name == 'new_collision_name')
def test_parent_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.parentJoint == 4)
col.parentJoint = 5
self.assertTrue(col.parentJoint == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
new_m = se3.SE3(rand([3,3]), rand(3))
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(np.allclose(col.placement.homogeneous,m.homogeneous))
col.placement = new_m
self.assertTrue(np.allclose(col.placement.homogeneous , new_m.homogeneous))
def test_meshpath_get(self):
expected_mesh_path = os.path.join(self.pinocchio_models_dir,'meshes/romeo/collision/LHipPitch.dae')
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.meshPath == expected_mesh_path)
def runningModel(contactIds, effectors, com=None, integrationStep=1e-2):
'''
Creating the action model for floating-base systems. A walker system
is by default a floating-base system.
contactIds is a list of frame Ids of points that should be in contact.
effectors is a dict of key frame ids and SE3 values of effector references.
'''
actModel = ActuationModelFreeFloating(rmodel)
State = StatePinocchio(rmodel)
# Creating a 6D multi-contact model, and then including the supporting foot
contactModel = ContactModelMultiple(rmodel)
for cid in contactIds:
contactModel.addContact('contact%d' % cid,
ContactModel6D(rmodel, cid, ref=None))
# Creating the cost model for a contact phase
costModel = CostModelSum(rmodel, actModel.nu)
wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv)
costModel.addCost('xreg',
weight=1e-1,
cost=CostModelState(
rmodel,
State,
ref=rmodel.defaultState,
nu=actModel.nu,
activation=ActivationModelWeightedQuad(wx)))
costModel.addCost('ureg',
weight=1e-4,
cost=CostModelControl(rmodel, nu=actModel.nu))
for fid, ref in effectors.items():
if not isinstance(ref, SE3):
ref = SE3(eye(3), a2m(ref))
costModel.addCost("track%d" % fid,
weight=100.,
cost=CostModelFramePlacement(rmodel, fid, ref,
actModel.nu))
if com is not None:
costModel.addCost("com",
weight=100.,
cost=CostModelCoM(rmodel, ref=com, nu=actModel.nu))
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = DifferentialActionModelFloatingInContact(rmodel, actModel,
contactModel, costModel)
model = IntegratedActionModelEuler(dmodel)
model.timeStep = integrationStep
return model
def test_Jlog3(self):
m = eye(3)
J = pin.Jlog3(m)
self.assertApprox(J, eye(3))
def test_setRandom(self):
Y = pin.Inertia.Identity()
Y.setRandom()
self.assertFalse(Y.mass == 1)
self.assertFalse(np.allclose(zero(3), Y.lever))
self.assertFalse(np.allclose(eye(3), Y.inertia))
def test_log3(self):
m = eye(3)
v = pin.log3(m)
self.assertApprox(v, zero(3))
def trans(x, y, z):
return pin.SE3(eye(3), np.matrix([x, y, z]).T)
def test_Jlog3(self):
m = eye(3)
J = pin.Jlog3(m)
self.assertApprox(J, eye(3))
def test_Jlog6(self):
m = pin.SE3.Identity()
J = pin.Jlog6(m)
self.assertApprox(J, eye(6))
print('===CBK=== {0:4d} {1: 3.2f} {2: 3.2f}'.format(
self.nfeval, q[0], q[1]))
q = np.matrix(q).T
robot.display(q)
# place('world/blue', robot.placement(q, robot.rh))
self.nfeval += 1
time.sleep(self.dt)
# Question 1-2-3
rh = np.matrix([1.5, 0.1, 1.5]).T # x,y,z
rf = np.matrix([0., -0.1, 0.0]).T # x,y,z
lf = np.matrix([0., 0.1, 0.0]).T # x,y,z
com = np.matrix([0.0, 0.0, 0.5]).T # x,y,z
place('world/red', SE3(eye(3), rh))
place('world/blue', SE3(eye(3), rf))
place('world/green', SE3(eye(3), lf))
place('world/yellow', SE3(eye(3), com))
cost = Cost(rh, rf, lf, com)
# If the quaternion is not unitary, it does not correspond a valid rotation. The
# behavior is then not mathematically valid and is implementation dependant. In
# our implementation, quaternions with norm larger than one are interpreted as
# rotation+scaling.
print('Optimize with any (non unitary) quaternion')
cost.weightQuat = 0.0
qopt = fmin_bfgs(cost, robot.q0, callback=CallbackLogger(.1))
# We implement the constraint as a cost with high weight. The quaternion is now unitary.
def test_setRandom(self):
Y = pin.Inertia.Identity()
Y.setRandom()
self.assertFalse(Y.mass == 1)
self.assertFalse(np.allclose(zero(3), Y.lever))
self.assertFalse(np.allclose(eye(3), Y.inertia))
def test_set_rotation(self):
transform = se3.SE3.Identity()
transform.rotation = zero([3, 3])
self.assertFalse(np.allclose(transform.rotation, eye(3)))
self.assertTrue(np.allclose(transform.rotation, zero([3, 3])))
def test_Jexp3(self):
v = zero(3)
m = pin.Jexp3(v)
self.assertApprox(m, eye(3))
def test_identity_getters(self):
Y = pin.Inertia.Identity()
self.assertTrue(Y.mass == 1)
self.assertTrue(np.allclose(zero(3), Y.lever))
self.assertTrue(np.allclose(eye(3), Y.inertia))
def test_Jlog6(self):
m = pin.SE3.Identity()
J = pin.Jlog6(m)
self.assertApprox(J, eye(6))
def test_log6_homogeneous(self):
m = eye(4)
v = pin.log6(m)
self.assertApprox(v.vector, zero(6))
def test_Jexp6(self):
v = pin.Motion.Zero()
J = pin.Jexp6(v)
self.assertApprox(J,eye(6))
def test_Jexp6(self):
v = pin.Motion.Zero()
J = pin.Jexp6(v)
self.assertApprox(J, eye(6))
def createHand(self, root_id=0, prefix='', joint_placement=None):
def trans(x, y, z):
return pin.SE3(eye(3), np.matrix([x, y, z]).T)
def inertia(m, c):
return pin.Inertia(m, np.matrix(c, np.double).T, eye(3) * m ** 2)
def joint_name(body):
return prefix + body + '_joint'
def body_name(body):
return 'world/' + prefix + body
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
joint_id = root_id
cm = 1e-2
joint_placement = joint_placement if joint_placement is not None else pin.SE3.Identity()
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('wrist'))
self.model.appendBodyToJoint(joint_id, inertia(3, [0, 0, 0]), pin.SE3.Identity())
L, W, H = 3 * cm, 5 * cm, 1 * cm
self.viewer.viewer.gui.addSphere(body_name('wrist'), .02, color)
self.viewer.viewer.gui.addBox(body_name('wpalm'), L / 2, W / 2, H, color)
self.visuals.append(Visual(body_name('wpalm'), joint_id, trans(L / 2, 0, 0)))
self.viewer.viewer.gui.addCapsule(body_name('wpalmb'), H, W, color)
self.visuals.append(Visual(body_name('wpalmb'), joint_id, pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
self.viewer.viewer.gui.addCapsule(body_name('wpalmt'), H, W, color)
pos = pin.SE3(rotate('x', pi / 2), np.matrix([L, 0, 0]).T)
self.visuals.append(Visual(body_name('wpalmt'), joint_id, pos, H, W))
self.viewer.viewer.gui.addCapsule(body_name('wpalml'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, -W / 2, 0]).T)
self.visuals.append(Visual(body_name('wpalml'), joint_id, pos, H, L))
self.viewer.viewer.gui.addCapsule(body_name('wpalmr'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, +W / 2, 0]).T)
self.visuals.append(Visual(body_name('wpalmr'), joint_id, pos, H, L))
joint_placement = pin.SE3(eye(3), np.matrix([5 * cm, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('palm'))
self.model.appendBodyToJoint(joint_id, inertia(2, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('palm2'), 1 * cm, W, color)
self.visuals.append(Visual(body_name('palm2'), joint_id, pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
FL = 4 * cm
palmIdx = joint_id
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, W / 2, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger11'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger11'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger11'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger12'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger12'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger12'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger13'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger13'), H, color)
self.visuals.append(Visual(body_name('finger13'), joint_id, trans(2 * H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, 0, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger21'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger21'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger21'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger22'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger22'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger22'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger23'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger23'), H, color)
self.visuals.append(Visual(body_name('finger23'), joint_id, trans(H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, -W / 2, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger31'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger31'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger31'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger32'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger32'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger32'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger33'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger33'), H, color)
self.visuals.append(Visual(body_name('finger33'), joint_id, trans(2 * H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([1 * cm, -W / 2 - H * 1.5, 0]).T)
joint_id = self.model.addJoint(1, pin.JointModelRY(), joint_placement, joint_name('thumb1'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb1'), H, 2 * cm, color)
pos = pin.SE3(rotate('z', pi / 3) * rotate('x', pi / 2), np.matrix([1 * cm, -1 * cm, 0]).T)
self.visuals.append(Visual(body_name('thumb1'), joint_id, pos, 2 * cm))
joint_placement = pin.SE3(rotate('z', pi / 3) * rotate('x', pi), np.matrix([3 * cm, -1.8 * cm, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRZ(), joint_placement, joint_name('thumb2'))
self.model.appendBodyToJoint(joint_id, inertia(.4, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb2'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('x', pi / 3), np.matrix([-0.7 * cm, .8 * cm, -0.5 * cm]).T)
self.visuals.append(Visual(body_name('thumb2'), joint_id, pos, H, FL - 2 * H))
# Prepare some patches to represent collision points. Yet unvisible.
for i in range(10):
self.viewer.viewer.gui.addCylinder('world/wa%i' % i, .01, .003, [1.0, 0, 0, 1])
self.viewer.viewer.gui.addCylinder('world/wb%i' % i, .01, .003, [1.0, 0, 0, 1])
self.viewer.viewer.gui.setVisibility('world/wa%i' % i, 'OFF')
self.viewer.viewer.gui.setVisibility('world/wb%i' % i, 'OFF')
def test_log6_homogeneous(self):
m = eye(4)
v = pin.log6(m)
self.assertApprox(v.vector, zero(6))
def inertia(m, c):
return pin.Inertia(m, np.matrix(c, np.double).T, eye(3) * m ** 2)
def test_identity_getters(self):
Y = pin.Inertia.Identity()
self.assertTrue(Y.mass == 1)
self.assertTrue(np.allclose(zero(3), Y.lever))
self.assertTrue(np.allclose(eye(3), Y.inertia))
def test_get_rotation(self):
transform = se3.SE3.Identity()
self.assertTrue(np.allclose(transform.rotation, eye(3)))
def createHand(self, root_id=0, prefix='', joint_placement=None):
def trans(x, y, z):
return pin.SE3(eye(3), np.matrix([x, y, z]).T)
def inertia(m, c):
return pin.Inertia(m, np.matrix(c, np.double).T, eye(3) * m**2)
def joint_name(body):
return prefix + body + '_joint'
def body_name(body):
return 'world/' + prefix + body
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
joint_id = root_id
cm = 1e-2
joint_placement = joint_placement if joint_placement is not None else pin.SE3.Identity(
)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('wrist'))
self.model.appendBodyToJoint(joint_id, inertia(3, [0, 0, 0]),
pin.SE3.Identity())
L, W, H = 3 * cm, 5 * cm, 1 * cm
self.viewer.viewer.gui.addSphere(body_name('wrist'), .02, color)
self.viewer.viewer.gui.addBox(body_name('wpalm'), L / 2, W / 2, H,
color)
self.visuals.append(
Visual(body_name('wpalm'), joint_id, trans(L / 2, 0, 0)))
self.viewer.viewer.gui.addCapsule(body_name('wpalmb'), H, W, color)
self.visuals.append(
Visual(body_name('wpalmb'), joint_id,
pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
self.viewer.viewer.gui.addCapsule(body_name('wpalmt'), H, W, color)
pos = pin.SE3(rotate('x', pi / 2), np.matrix([L, 0, 0]).T)
self.visuals.append(Visual(body_name('wpalmt'), joint_id, pos, H, W))
self.viewer.viewer.gui.addCapsule(body_name('wpalml'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, -W / 2, 0]).T)
self.visuals.append(Visual(body_name('wpalml'), joint_id, pos, H, L))
self.viewer.viewer.gui.addCapsule(body_name('wpalmr'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, +W / 2, 0]).T)
self.visuals.append(Visual(body_name('wpalmr'), joint_id, pos, H, L))
joint_placement = pin.SE3(eye(3), np.matrix([5 * cm, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('palm'))
self.model.appendBodyToJoint(joint_id, inertia(2, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('palm2'), 1 * cm, W, color)
self.visuals.append(
Visual(body_name('palm2'), joint_id,
pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
FL = 4 * cm
palmIdx = joint_id
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, W / 2, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(),
joint_placement, joint_name('finger11'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger11'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger11'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger12'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger12'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger12'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger13'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger13'), H, color)
self.visuals.append(
Visual(body_name('finger13'), joint_id, trans(2 * H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, 0, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(),
joint_placement, joint_name('finger21'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger21'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger21'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger22'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger22'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger22'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger23'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger23'), H, color)
self.visuals.append(
Visual(body_name('finger23'), joint_id, trans(H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, -W / 2, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(),
joint_placement, joint_name('finger31'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger31'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger31'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger32'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger32'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger32'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger33'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger33'), H, color)
self.visuals.append(
Visual(body_name('finger33'), joint_id, trans(2 * H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3),
np.matrix([1 * cm, -W / 2 - H * 1.5, 0]).T)
joint_id = self.model.addJoint(1, pin.JointModelRY(), joint_placement,
joint_name('thumb1'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb1'), H, 2 * cm,
color)
pos = pin.SE3(
rotate('z', pi / 3) * rotate('x', pi / 2),
np.matrix([1 * cm, -1 * cm, 0]).T)
self.visuals.append(Visual(body_name('thumb1'), joint_id, pos, 2 * cm))
joint_placement = pin.SE3(
rotate('z', pi / 3) * rotate('x', pi),
np.matrix([3 * cm, -1.8 * cm, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRZ(),
joint_placement, joint_name('thumb2'))
self.model.appendBodyToJoint(joint_id, inertia(.4, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb2'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('x', pi / 3),
np.matrix([-0.7 * cm, .8 * cm, -0.5 * cm]).T)
self.visuals.append(
Visual(body_name('thumb2'), joint_id, pos, H, FL - 2 * H))
# Prepare some patches to represent collision points. Yet unvisible.
for i in range(10):
self.viewer.viewer.gui.addCylinder('world/wa%i' % i, .01, .003,
[1.0, 0, 0, 1])
self.viewer.viewer.gui.addCylinder('world/wb%i' % i, .01, .003,
[1.0, 0, 0, 1])
self.viewer.viewer.gui.setVisibility('world/wa%i' % i, 'OFF')
self.viewer.viewer.gui.setVisibility('world/wb%i' % i, 'OFF')
def test_set_rotation(self):
transform = pin.SE3.Identity()
transform.rotation = zero([3,3])
self.assertFalse(np.allclose(transform.rotation, eye(3)))
self.assertTrue(np.allclose(transform.rotation, zero([3,3])))
def inertia(m, c):
return pin.Inertia(m, np.matrix(c, np.double).T, eye(3) * m**2)
def test_log3(self):
m = eye(3)
v = pin.log3(m)
self.assertApprox(v, zero(3))
def trans(x, y, z):
return pin.SE3(eye(3), np.matrix([x, y, z]).T)
class TestMotionBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3, 3])
m3ones = eye(3)
m4ones = eye(4)
def test_zero_getters(self):
v = se3.Motion.Zero()
self.assertTrue(np.allclose(self.v3zero, v.linear))
self.assertTrue(np.allclose(self.v3zero, v.angular))
self.assertTrue(np.allclose(self.v6zero, v.vector))
def test_setRandom(self):
v = se3.Motion.Zero()
v.setRandom()
self.assertFalse(np.allclose(self.v3zero, v.linear))
self.assertFalse(np.allclose(self.v3zero, v.angular))
self.assertFalse(np.allclose(self.v6zero, v.vector))
def test_setZero(self):
v = se3.Motion.Zero()
v.setRandom()
v.setZero()
self.assertTrue(np.allclose(self.v3zero, v.linear))
self.assertTrue(np.allclose(self.v3zero, v.angular))
self.assertTrue(np.allclose(self.v6zero, v.vector))
def test_set_linear(self):
v = se3.Motion.Zero()
lin = rand(
3
) # TODO np.matrix([1,2,3],np.double) OR np.matrix( np.array([1,2,3], np.double), np.double)
v.linear = lin
self.assertTrue(np.allclose(v.linear, lin))
def test_set_angular(self):
v = se3.Motion.Zero()
ang = rand(3)
v.angular = ang
self.assertTrue(np.allclose(v.angular, ang))
def test_set_vector(self):
v = se3.Motion.Zero()
vec = rand(6)
v.vector = vec
self.assertTrue(np.allclose(v.vector, vec))
def test_internal_sums(self):
v1 = se3.Motion.Random()
v2 = se3.Motion.Random()
self.assertTrue(np.allclose((v1 + v2).vector, v1.vector + v2.vector))
self.assertTrue(np.allclose((v1 - v2).vector, v1.vector - v2.vector))
def test_se3_action(self):
m = se3.SE3.Random()
v = se3.Motion.Random()
self.assertTrue(np.allclose((m * v).vector, m.action * v.vector))
self.assertTrue(
np.allclose((m.actInv(v)).vector,
np.linalg.inv(m.action) * v.vector))
self.assertTrue(np.allclose((v**v).vector, zero(6)))
'''
This function load a UR5 robot n a new model, move the basis to placement <M0>
and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
It returns the robot wrapper (model,data).
'''
robot = RobotWrapper(urdf, [PKG])
robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1]
robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement
robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0]
robot.initDisplay(loadModel=True, viewerRootNodeName="world/" + name)
return robot
robots = []
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
Mt = SE3(eye(3), np.matrix([.3, 0, 0]).T) # First robot is simply translated
for i in range(4):
robots.append(loadRobot(SE3(rotate('z', np.pi / 2 * i), zero(3)) * Mt, "robot%d" % i))
# Set up the robots configuration with end effector pointed upward.
q0 = np.matrix([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0]).T
for i in range(4):
robots[i].display(q0)
# Add a new object featuring the parallel robot tool plate.
gepettoViewer = robots[0].viewer.gui
w, h, d = 0.25, 0.25, 0.005
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
gepettoViewer.addBox('world/toolplate', w, h, d, color)
Mtool = SE3(rotate('z', 1.268), np.matrix([0, 0, .77]).T)
gepettoViewer.applyConfiguration('world/toolplate', se3ToXYZQUATtuple(Mtool))
class TestSE3Bindings(unittest.TestCase):
v3zero = zero(3)
v3ones = ones(3)
m3zero = zero([3,3])
m3ones = eye(3)
m4ones = eye(4)
def test_identity(self):
transform = se3.SE3.Identity()
self.assertTrue(np.allclose(self.v3zero,transform.translation))
self.assertTrue(np.allclose(self.m3ones, transform.rotation))
self.assertTrue(np.allclose(self.m4ones, transform.homogeneous))
transform.setRandom()
transform.setIdentity()
self.assertTrue(np.allclose(self.m4ones, transform.homogeneous))
def test_get_translation(self):
transform = se3.SE3.Identity()
self.assertTrue(np.allclose(transform.translation, self.v3zero))
def test_get_rotation(self):
transform = se3.SE3.Identity()
self.assertTrue(np.allclose(transform.rotation, self.m3ones))
def test_set_translation(self):
transform = se3.SE3.Identity()
transform.translation = self.v3ones
self.assertFalse(np.allclose(transform.translation, self.v3zero))
self.assertTrue(np.allclose(transform.translation, self.v3ones))
def test_set_rotation(self):
transform = se3.SE3.Identity()
transform.rotation = self.m3zero
self.assertFalse(np.allclose(transform.rotation, self.m3ones))
self.assertTrue(np.allclose(transform.rotation, self.m3zero))
def test_homogeneous(self):
amb = se3.SE3.Random()
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,3], amb.translation)) # derniere colonne = translation de amb
def test_inverse(self):
amb = se3.SE3.Random()
aMb = amb.homogeneous
bma = amb.inverse()
self.assertTrue(np.allclose(np.linalg.inv(aMb), bma.homogeneous))
def test_internal_product(self):
amb = se3.SE3.Random()
aMb = amb.homogeneous
bmc = se3.SE3.Random()
bMc = bmc.homogeneous
amc = amb*bmc
cma = amc.inverse()
aMc = amc.homogeneous
self.assertTrue(np.allclose(aMb*bMc, aMc))
self.assertTrue(np.allclose(cma.homogeneous,np.linalg.inv(aMc)))
def test_point_action(self):
amb = se3.SE3.Random()
aMb = amb.homogeneous
p_homogeneous = rand(4)
p_homogeneous[3] = 1
p = p_homogeneous[0:3].copy()
# act
self.assertTrue(np.allclose(amb.act_point(p),(aMb*p_homogeneous)[0:3]))
# actinv
bMa = np.linalg.inv(aMb)
bma = amb.inverse()
self.assertTrue(np.allclose(bma.act_point(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))
def test_get_rotation(self):
transform = pin.SE3.Identity()
self.assertTrue(np.allclose(transform.rotation, eye(3)))
def test_Jexp3(self):
v = zero(3)
m = pin.Jexp3(v)
self.assertApprox(m, eye(3))
def test_setIdentity(self):
Y = pin.Inertia.Zero()
Y.setIdentity()
self.assertTrue(Y.mass == 1)
self.assertTrue(np.allclose(zero(3), Y.lever))
self.assertTrue(np.allclose(eye(3), Y.inertia))
def test_setIdentity(self):
Y = pin.Inertia.Zero()
Y.setIdentity()
self.assertTrue(Y.mass == 1)
self.assertTrue(np.allclose(zero(3), Y.lever))
self.assertTrue(np.allclose(eye(3), Y.inertia))
