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))