def test_eigen_and_sva(): """ Test/demo of 3D geometry libraries eigen and sva eigen # https://github.com/jrl-umi3218/Eigen3ToPython sva # https://github.com/jrl-umi3218/SpaceVecAlg """ print('eigen and sva test') if eigen is not None and sva is not None: qa = np.array([0, 0, 0, 1]) v = eigen.Vector3d([1, 1, 1]) qa4 = eigen.Vector4d(qa) q = eigen.Quaterniond(qa4) pt = sva.PTransformd(q, v) print(str(np.array(pt.rotation()))) rot = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]] assert np.allclose(np.array(rot), np.array(pt.rotation())) pt.translation() trans = pt.translation() assert trans.x() == 1.0 assert trans.y() == 1.0 assert trans.z() == 1.0 trans_np = np.array(trans) assert np.allclose(trans_np, v)
def test(self): create_random_pt = lambda: sva.PTransformd( eigen.Quaterniond(eigen.Vector4d.Random().normalized()), eigen.Vector3d().Random() * 100) # Check empty creation and filling v1 = sva.PTransformdVector() for i in range(100): v1.append(create_random_pt()) assert (len(v1) == 100) # Check creation from a single PTransformd object v2 = sva.PTransformdVector(create_random_pt()) assert (len(v2) == 1) # Check creation from a list v3 = sva.PTransformdVector([create_random_pt() for i in range(100)]) assert (len(v3) == 100) # Check creation from a lot of PTransformd v3 = sva.PTransformdVector(*[create_random_pt() for i in range(100)]) assert (len(v3) == 100) # Check access pt = create_random_pt() v4 = sva.PTransformdVector([pt] * 100) assert (all([pt == vi for vi in v4]))
def matrix_to_vector_quaternion_array(matrix, inverse=False, verbose=0): """Convert a 4x4 Rt transformation matrix into a vector quaternion array containing 3 vector entries (x, y, z) and 4 quaternion entries (x, y, z, w) # Params matrix: The 4x4 Rt rigid body transformation matrix to convert into a vector quaternion array. inverse: Inverts the full matrix before loading into the array. Useful when the transformation to be reversed and for testing/debugging purposes. # Returns np.array([x, y, z, qx, qy, qz, qw]) """ rot = eigen.Matrix3d(matrix[:3, :3]) quaternion = eigen.Quaterniond(rot) translation = matrix[:3, 3].transpose() if inverse: quaternion = quaternion.inverse() translation *= -1 # coeffs are in xyzw order q_floats_array = np.array(quaternion.coeffs()) # q_floats_array = np.array([quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w()]).astype(np.float32) vec_quat_7 = np.append(translation, q_floats_array) if verbose > 0: print(vec_quat_7) return vec_quat_7
def ptransform_to_vector_quaternion_array(ptransform, q_inverse=True, dtype=np.float32): """Convert a PTransformD into a vector quaternion array containing 3 vector entries (x, y, z) and 4 quaternion entries (x, y, z, w) # Params ptransform: The plucker transform from sva.PTransformd to be converted q_inverse: Invert the quaternion after it is extracted, defaults to True. With a PTransform the rotation component must be inverted before loading and after extraction. See https://github.com/ahundt/grl/blob/master/include/grl/vrep/SpaceVecAlg.hpp#L22 for a well tested example. See https://github.com/jrl-umi3218/Tasks/issues/10 for a detailed discussion leading to this conclusion. The default for q_inverse should be True, do not switch the q_inverse default to False without careful consideration and testing, though such a change may be appropriate when loading into another transform representation in which the rotation component is expected to be inverted. TODO(ahundt) in process of debugging, correct docstring when all issues are resolved. """ rot = ptransform.rotation() quaternion = eigen.Quaterniond(rot) if q_inverse: quaternion = quaternion.inverse() translation = np.array(ptransform.translation()).reshape(3) # coeffs are in xyzw order q_floats_array = np.array(quaternion.coeffs()) vec_quat_7 = np.append(translation, q_floats_array) return vec_quat_7.astype(dtype)
def test_configuration_writing(): tmpF = tempfile.NamedTemporaryFile(delete=False).name config_ref = mc_rtc.Configuration() ref_bool = False config_ref.add("bool", ref_bool) ref_uint = 42 config_ref.add("uint", ref_uint) ref_int = -42 config_ref.add("int", ref_int) ref_double = 42.5 config_ref.add("double", ref_double) ref_string = "sometext" config_ref.add("string", ref_string) ref_v3d = eigen.Vector3d(1.2, 3.4, 5.6) config_ref.add("v3d", ref_v3d) ref_v6d = eigen.Vector6d(0.1, 1.2, 2.3, 3.4, 4.5, 5.6) config_ref.add("v6d", ref_v6d) ref_vxd = eigen.VectorXd(0.1, 3.2, 4.2, 4.5, 5.4) config_ref.add("vxd", ref_vxd) ref_quat = eigen.Quaterniond(0.71, 0, 0.71, 0) ref_quat.normalize() config_ref.add("quat", ref_quat) ref_int_v = [0, 1, 2, 3, 4, 5] config_ref.add("int_v", ref_int_v) ref_double_v = [0.1, 1.0, 0.2, 2.0, 0.3] config_ref.add("double_v", ref_double_v) ref_double_v_v = [ref_double_v, ref_double_v, [0], [], [5.0, 4.0, 3.5]] config_ref.add("double_v_v", ref_double_v_v) ref_v3d_v = [eigen.Vector3d.Random() for i in range(10)] config_ref.add("v3d_v", ref_v3d_v) config_ref.add("dict") config_ref("dict").add("int", ref_int) config_ref.add("dict2").add("double_v", ref_double_v) config_ref.save(tmpF) config_test = mc_rtc.Configuration(tmpF) assert (config_test("bool", bool) == ref_bool) assert (config_test("uint", int) == ref_uint) assert (config_test("int", int) == ref_int) assert (config_test("double", float) == ref_double) assert (config_test("string", str) == ref_string) assert (config_test("v3d", eigen.Vector3d) == ref_v3d) assert (config_test("v6d", eigen.Vector6d) == ref_v6d) assert (config_test("vxd", eigen.VectorXd) == ref_vxd) assert (config_test("quat", eigen.Quaterniond).isApprox(ref_quat)) assert (config_test("int_v", [int]) == ref_int_v) assert (config_test("double_v", [float]) == ref_double_v) assert (config_test("double_v_v", [[0.]]) == ref_double_v_v) assert (all([ (v - ref_v).norm() < 1e-9 for v, ref_v in zip(config_test("v3d_v", [eigen.Vector3d]), ref_v3d_v) ])) assert (config_test("dict")("int", int) == ref_int) assert (config_test("dict2")("double_v", [float]) == ref_double_v) config_test("dict2")("double_v").save(tmpF) assert (mc_rtc.Configuration(tmpF).to([float]) == ref_double_v)
def run_callback(self): hj1_q = self.robot().mbc.q[self.hj1_index][0] self.v3d_data = eigen.Vector3d.Random() self.d_data += 1.0 self.dv_data = [self.d_data] * 3 self.theta += 0.005 self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta)) return True
def prismaticJoint(joint): """ Return a prism and the appropriate static transform. """ axis = e.Vector3d(joint.motionSubspace()[3:]) s = tvtk.CubeSource(x_length=0.02, y_length=0.1, z_length=0.02) quat = e.Quaterniond() quat.setFromTwoVectors(axis, e.Vector3d.UnitY()) return makeActor(s, tuple(axis)), sva.PTransformd(quat)
def revoluteJoint(joint): """ Return a cylinder and the appropriate static transform. """ axis = e.Vector3d(joint.motionSubspace()[:3]) s = tvtk.CylinderSource(height=0.1, radius=0.02) quat = e.Quaterniond() # Cylinder is around the Y axis quat.setFromTwoVectors(axis, e.Vector3d.UnitY()) return makeActor(s, tuple(axis)), sva.PTransformd(quat)
def vector_quaternion_array_to_ptransform(vector_quaternion_array, q_inverse=True, t_inverse=False, pt_inverse=False): """Convert a vector and quaternion pose array to a plucker transform. # Params vector_quaternion_array: A numpy array containing a pose. A pose is a 6 degree of freedom rigid transform represented with 7 values: [x, y, z, qx, qy, qz, qw] consisting of a vector (x, y, z) for cartesian motion and quaternion (qx, qy, qz, qw) for rotation. This is the data format used by the google brain robot data grasping dataset's tfrecord poses. eigen Quaterniond is also ordered xyzw. q_inverse: Invert the quaternion before it is loaded into the PTransformd, defaults to True. With a PTransform the rotation component must be inverted before loading and after extraction. See https://github.com/ahundt/grl/blob/master/include/grl/vrep/SpaceVecAlg.hpp#L22 for a well tested example. See https://github.com/jrl-umi3218/Tasks/issues/10 for a detailed discussion leading to this conclusion. The default for q_inverse should be True, do not switch the q_inverse default to False without careful consideration and testing, though such a change may be appropriate when loading into another transform representation in which the rotation component is expected to be inverted. # Returns A plucker transform PTransformd object as defined by the spatial vector algebra library. https://github.com/jrl-umi3218/SpaceVecAlg https://en.wikipedia.org/wiki/Pl%C3%BCcker_coordinates """ v = eigen.Vector3d(vector_quaternion_array[:3]) if t_inverse is True: v *= -1 # TODO(ahundt) use following lines after https://github.com/jrl-umi3218/Eigen3ToPython/pull/15 is fixed # qa4 = eigen.Vector4d() # q = eigen.Quaterniond(qa4) # Quaterniond important coefficient ordering details: # scalar constructor is Quaterniond(w,x,y,z) # vector constructor is Quaterniond(np.array([x,y,z,w])) # Quaterniond.coeffs() is [x,y,z,w] # https://eigen.tuxfamily.org/dox/classEigen_1_1Quaternion.html xyzw = eigen.Vector4d(vector_quaternion_array[3:]) q = eigen.Quaterniond(xyzw) # The ptransform needs the rotation component to inverted before construction. # see https://github.com/ahundt/grl/blob/master/include/grl/vrep/SpaceVecAlg.hpp#L22 for a well tested example # see https://github.com/jrl-umi3218/Tasks/issues/10 for a detailed discussion leading to this conclusion if q_inverse is True: q = q.inverse() pt = sva.PTransformd(q, v) if pt_inverse is True: pt = pt.inv() return pt
def vector_to_ptransform(XYZ): """Convert (x,y,z) translation to sva.Ptransformd. Convert an xyz 3 element translation vector to an sva.PTransformd plucker ptransform object with no rotation applied. In other words, the rotation component will be the identity rotation. """ q = eigen.Quaterniond() q.setIdentity() v = eigen.Vector3d(XYZ) ptransform = sva.PTransformd(q, v) return ptransform
def test_angle_axis(): aa = e.AngleAxisd() # quaternion xyzw coefficient order q = e.Quaterniond(e.Vector4d(0., 0., 0., 1.)) aa = e.AngleAxisd(q) assert(aa.angle() == 0) v = e.Vector3d.UnitX() aa = e.AngleAxisd(0.1, v) assert(aa.axis() == v) assert(aa.angle() == 0.1) aa2 = aa.inverse() assert(aa2.axis() == v) assert(aa2.angle() == -0.1)
def _createVector(self, vector, frame, color): source = tvtk.ArrowSource() pdm = tvtk.PolyDataMapper(input=source.output) actor = tvtk.Actor(mapper=pdm) actor.user_transform = tvtk.Transform() actor.property.color = color norm = vector.norm() actor.scale = (norm,)*3 quat = e.Quaterniond() # arrow are define on X axis quat.setFromTwoVectors(vector, e.Vector3d.UnitX()) X = sva.PTransformd(quat)*frame setActorTransform(actor, X) return actor, X
def test(self): Em = eigen.AngleAxisd(np.pi / 2, eigen.Vector3d.UnitX()).inverse().matrix() Eq = eigen.Quaterniond( eigen.AngleAxisd(np.pi / 2, eigen.Vector3d.UnitX()).inverse()) r = eigen.Vector3d.Random() * 100 pt1 = sva.PTransformd.Identity() self.assertEqual(pt1.rotation(), eigen.Matrix3d.Identity()) self.assertEqual(pt1.translation(), eigen.Vector3d.Zero()) pt2 = sva.PTransformd(Em, r) self.assertEqual(pt2.rotation(), Em) self.assertEqual(pt2.translation(), r) pt3 = sva.PTransformd(Eq, r) self.assertEqual(pt3.rotation(), Eq.toRotationMatrix()) self.assertEqual(pt3.translation(), r) pt4 = sva.PTransformd(Eq) self.assertEqual(pt4.rotation(), Eq.toRotationMatrix()) self.assertEqual(pt4.translation(), eigen.Vector3d.Zero()) pt5 = sva.PTransformd(Em) self.assertEqual(pt5.rotation(), Em) self.assertEqual(pt5.translation(), eigen.Vector3d.Zero()) pt6 = sva.PTransformd(r) self.assertEqual(pt6.rotation(), eigen.Matrix3d.Identity()) self.assertEqual(pt6.translation(), r) pttmp = sva.PTransformd( eigen.AngleAxisd(np.pi / 4, eigen.Vector3d.UnitY()).matrix(), eigen.Vector3d.Random() * 100.) pt7 = pt2 * pttmp ptm = pt2.matrix() * pttmp.matrix() self.assertAlmostEqual((pt7.matrix() - ptm).norm(), 0, delta=TOL) pt8 = pt2.inv() self.assertAlmostEqual((pt8.matrix() - pt2.matrix().inverse()).norm(), 0, delta=TOL) self.assertEqual(pt2, pt2) self.assertNotEqual(pt2, pt8) self.assertTrue(pt2 != pt8) self.assertTrue(not (pt2 != pt2))
def run_callback(self): hj1_q = self.robot().mbc.q[self.hj1_index][0] self.v3d_data = eigen.Vector3d.Random() self.d_data += 1.0 self.dv_data = [self.d_data] * 3 self.theta += 0.005 self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta)) assert (self.observerPipeline("FirstPipeline").success()) if abs(self.d_data - 2.0) < 1e-6: self.removeAnchorFrameCallback("KinematicAnchorFrame::{}".format( self.robot().name())) self.addAnchorFrameCallback( "KinematicAnchorFrame::{}".format(self.robot().name()), self.anchorFrameCallback) return True
def test(self): X_a_b = sva.PTransformd( eigen.Quaterniond(eigen.Vector4d.Random()).normalized(), eigen.Vector3d.Random()) X_a_c = sva.PTransformd( eigen.Quaterniond(eigen.Vector4d.Random()).normalized(), eigen.Vector3d.Random()) V_a_b = sva.transformVelocity(X_a_b) V_a_c = sva.transformVelocity(X_a_c) self.assertAlmostEqual( (V_a_b.angular() - sva.rotationVelocity(X_a_b.rotation())).norm(), 0, delta=TOL) self.assertAlmostEqual((V_a_b.linear() - X_a_b.translation()).norm(), 0, delta=TOL) self.assertAlmostEqual( (V_a_c.angular() - sva.rotationVelocity(X_a_c.rotation())).norm(), 0, delta=TOL) self.assertAlmostEqual((V_a_c.linear() - X_a_c.translation()).norm(), 0, delta=TOL) V_b_c_a = sva.transformError(X_a_b, X_a_c) w_b_c_a = sva.rotationError(X_a_b.rotation(), X_a_c.rotation()) v_b_c_a = X_a_c.translation() - X_a_b.translation() self.assertAlmostEqual((V_b_c_a.angular() - w_b_c_a).norm(), 0, delta=TOL) self.assertAlmostEqual((V_b_c_a.linear() - v_b_c_a).norm(), 0, delta=TOL)
def _createVector(self, vector, frame, color): source = tvtk.ArrowSource() # pdm = tvtk.PolyDataMapper(input=source.output) pdm = tvtk.PolyDataMapper() # https://github.com/enthought/mayavi/issues/521 pdm.input_connection = source.output_port # configure_input_data(pdm, source) prop = tvtk.Property(color=color) actor = tvtk.Actor(mapper=pdm, property=prop) actor.user_transform = tvtk.Transform() # commented due to api change, new lines are above Actor creation # actor.property.color = color norm = vector.norm() actor.scale = (norm, ) * 3 quat = e.Quaterniond() # arrow are define on X axis quat.setFromTwoVectors(vector, e.Vector3d.UnitX()) X = sva.PTransformd(quat) * frame transform.setActorTransform(actor, X) return actor, X
def __init__(self, rm, dt): self.qpsolver.addConstraintSet(self.dynamicsConstraint) self.qpsolver.addConstraintSet(self.contactConstraint) self.qpsolver.addTask(self.postureTask) self.positionTask = mc_tasks.PositionTask("R_WRIST_Y_S", self.robots(), 0) self.qpsolver.addTask(self.positionTask) self.qpsolver.setContacts([ mc_rbdyn.Contact(self.robots(), "LeftFoot", "AllGround"), mc_rbdyn.Contact(self.robots(), "RightFoot", "AllGround") ]) self.hj1_name = "NECK_P" self.hj1_index = self.robot().jointIndexByName(self.hj1_name) self.hj1_q = 0.5 # Stuff to log self.v3d_data = eigen.Vector3d.Random() self.logger().addLogEntry("PYTHONV3D", lambda: self.v3d_data) self.d_data = 0.0 self.dv_data = [self.d_data] * 3 self.theta = 0 self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta))
def test(self): # register custom pickle function sva.copy_reg_pickle() mv = sva.MotionVecd(e3.Vector6d.Random()) fv = sva.ForceVecd(e3.Vector6d.Random()) pt = sva.PTransformd(e3.Quaterniond(e3.Vector4d.Random().normalized()), e3.Vector3d.Random()) rb = sva.RBInertiad(3., e3.Vector3d.Random(), e3.Matrix3d.Random()) ab = sva.ABInertiad(e3.Matrix3d.Random(), e3.Matrix3d.Random(), e3.Matrix3d.Random()) def test_pickle(v): pickled = pickle.dumps(v) v2 = pickle.loads(pickled) self.assertEqual(v, v2) test_pickle(mv) test_pickle(fv) test_pickle(pt) test_pickle(rb) test_pickle(ab)
def test(self): mb1, mbc1Init = arms.makeZXZArm() rbdyn.forwardKinematics(mb1, mbc1Init) rbdyn.forwardVelocity(mb1, mbc1Init) mb2, mbc2Init = arms.makeZXZArm(False) if not LEGACY: mb2InitPos = mbc1Init.bodyPosW[-1].translation() else: mb2InitPos = list(mbc1Init.bodyPosW)[-1].translation() mb2InitOri = eigen.Quaterniond(sva.RotY(math.pi / 2)) if not LEGACY: mbc2Init.q[0] = [ mb2InitOri.w(), mb2InitOri.x(), mb2InitOri.y(), mb2InitOri.z(), mb2InitPos.x(), mb2InitPos.y() + 1, mb2InitPos.z() ] mbc2Init.q[0] = [ mb2InitOri.w(), mb2InitOri.x(), mb2InitOri.y(), mb2InitOri.z(), mb2InitPos.x(), mb2InitPos.y() + 1, mb2InitPos.z() ] rbdyn.forwardKinematics(mb2, mbc2Init) rbdyn.forwardVelocity(mb2, mbc2Init) if not LEGACY: X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1]) X_0_b2 = sva.PTransformd(mbc2Init.bodyPosW[-1]) else: X_0_b1 = sva.PTransformd(list(mbc1Init.bodyPosW)[-1]) X_0_b2 = sva.PTransformd(list(mbc2Init.bodyPosW)[-1]) X_b1_b2 = X_0_b2 * X_0_b1.inv() if not LEGACY: mbs = rbdyn.MultiBodyVector([mb1, mb2]) mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init]) else: mbs = [mb1, mb2] mbcs = [ rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init) ] # Test ContactAccConstr constraint and PositionTask on the second robot solver = tasks.qp.QPSolver() points = [ eigen.Vector3d(0.1, 0, 0.1), eigen.Vector3d(0.1, 0, -0.1), eigen.Vector3d(-0.1, 0, -0.1), eigen.Vector3d(-0.1, 0, 0.1), ] biPoints = [ eigen.Vector3d.Zero(), eigen.Vector3d.Zero(), eigen.Vector3d.Zero(), eigen.Vector3d.Zero(), ] nrGen = 4 biFrames = [ sva.RotX(math.pi / 4), sva.RotX(3 * math.pi / 4), sva.RotX(math.pi / 4) * sva.RotY(math.pi / 2), sva.RotX(3 * math.pi / 4) * sva.RotY(math.pi / 2), ] # The fixed robot can pull the other contVecFail = [ tasks.qp.UnilateralContact(0, 1, "b3", "b0", points, sva.RotX(-math.pi / 2), X_b1_b2, nrGen, 0.7) ] # The fixed robot can push the other contVec = [ tasks.qp.UnilateralContact(0, 1, "b3", "b0", points, sva.RotX(math.pi / 2), X_b1_b2, nrGen, 0.7) ] # The fixed robot has non coplanar force apply on the other contVecBi = [ tasks.qp.BilateralContact(tasks.qp.ContactId(0, 1, "b3", "b0"), biPoints, biFrames, X_b1_b2, nrGen, 1) ] if not LEGACY: posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 2, 1) posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 2, 1) else: posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2, 1) posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2, 1) contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001) Inf = float("inf") torqueMin1 = [[], [-Inf], [-Inf], [-Inf]] torqueMax1 = [[], [Inf], [Inf], [Inf]] torqueMin2 = [[0, 0, 0, 0, 0, 0], [-Inf], [-Inf], [-Inf]] torqueMax2 = [[0, 0, 0, 0, 0, 0], [Inf], [Inf], [Inf]] motion1 = tasks.qp.MotionConstr( mbs, 0, tasks.TorqueBound(torqueMin1, torqueMax1)) motion2 = tasks.qp.MotionConstr( mbs, 1, tasks.TorqueBound(torqueMin2, torqueMax2)) plCstr = tasks.qp.PositiveLambda() motion1.addToSolver(solver) motion2.addToSolver(solver) plCstr.addToSolver(solver) contCstrSpeed.addToSolver(solver) solver.addTask(posture1Task) solver.addTask(posture2Task) # Check the impossible motion solver.nrVars(mbs, contVecFail, []) solver.updateConstrSize() self.assertEqual(solver.nrVars(), 3 + 9 + 4 * nrGen) self.assertFalse(solver.solve(mbs, mbcs)) # Check the unilateral motion if not LEGACY: mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init]) else: mbcs = [ rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init) ] solver.nrVars(mbs, contVec, []) solver.updateConstrSize() for i in range(1000): if not LEGACY: self.assertTrue(solver.solve(mbs, mbcs)) else: self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs)) solver.updateMbc(mbcs[0], 0) solver.updateMbc(mbcs[1], 1) for i in range(2): rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001) rbdyn.forwardKinematics(mbs[i], mbcs[i]) rbdyn.forwardVelocity(mbs[i], mbcs[i]) # Check that the link hold if not LEGACY: X_0_b1_post = mbcs[0].bodyPosW[-1] X_0_b2_post = mbcs[1].bodyPosW[-1] else: X_0_b1_post = list(mbcs[0].bodyPosW)[-1] X_0_b2_post = list(mbcs[1].bodyPosW)[-1] X_b1_b2_post = X_0_b2 * X_0_b1.inv() self.assertAlmostEqual( (X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(), 0, delta=1e-5) # Force in the world frame must be the same f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone) f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone) self.assertAlmostEqual((f1 + f2).norm(), 0, delta=1e-5) # Check the bilateral motion if not LEGACY: mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init]) else: mbcs = [ rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init) ] solver.nrVars(mbs, contVec, []) solver.updateConstrSize() self.assertEqual(solver.nrVars(), 3 + 9 + 4 * nrGen) for i in range(1000): if not LEGACY: self.assertTrue(solver.solve(mbs, mbcs)) else: self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs)) solver.updateMbc(mbcs[0], 0) solver.updateMbc(mbcs[1], 1) for i in range(2): rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001) rbdyn.forwardKinematics(mbs[i], mbcs[i]) rbdyn.forwardVelocity(mbs[i], mbcs[i]) # Check that the link hold if not LEGACY: X_0_b1_post = mbcs[0].bodyPosW[-1] X_0_b2_post = mbcs[1].bodyPosW[-1] else: X_0_b1_post = list(mbcs[0].bodyPosW)[-1] X_0_b2_post = list(mbcs[1].bodyPosW)[-1] X_b1_b2_post = X_0_b2 * X_0_b1.inv() self.assertAlmostEqual( (X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(), 0, delta=1e-5) # Force in the world frame must be the same f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone) f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone) self.assertAlmostEqual((f1 + f2).norm(), 0, delta=1e-5) plCstr.removeFromSolver(solver) motion2.removeFromSolver(solver) motion1.removeFromSolver(solver) contCstrSpeed.removeFromSolver(solver) solver.removeTask(posture1Task) solver.removeTask(posture2Task)
def test_configuration_reading(config, fromDisk2): @raises(RuntimeError) def test_throw(): config("NONE") test_throw() assert (config("int", int) == 42) assert (config("int", 0) == 42) assert (config("NONE", 100) == 100) assert (config("dict")("int", int) == 42) assert (config("double", float) == 42.5) assert (config("double", 0.) == 42.5) assert (config("NONE", 42.42) == 42.42) assert (config("dict")("double", float) == 42.5) assert (config("string", str) == "sometext") try: assert (config("string", unicode) == "sometext") except NameError: pass assert (config("string", "") == "sometext") assert (config("string", u"") == "sometext") assert (config("NONE", "another") == "another") assert (config("dict")("string", "") == "sometext") ref = eigen.Vector3d(1.0, 2.3, -100) zero = eigen.Vector3d.Zero() assert (config("v3d", eigen.Vector3d) == ref) assert (config("v3d", zero) == ref) assert (config("v6d", zero) == zero) @raises(RuntimeError) def test_v6d_to_v3d_throw(): config("v6d", eigen.Vector3d) test_v6d_to_v3d_throw() assert (config("dict")("v3d", eigen.Vector3d) == ref) assert (config("dict")("v3d", zero) == ref) ref = eigen.Vector6d(1.0, -1.5, 2.0, -2.5, 3.0, -3.5) zero = eigen.Vector6d.Zero() assert (config("v6d", eigen.Vector6d) == ref) assert (config("v6d", zero) == ref) assert (config("v3d", zero) == zero) @raises(RuntimeError) def test_v3d_to_v6d_throw(): config("v3d", eigen.Vector6d) test_v3d_to_v6d_throw() assert (config("dict")("v6d", eigen.Vector6d) == ref) assert (config("dict")("v6d", zero) == ref) ref3 = eigen.VectorXd(1, 2.3, -100) ref6 = eigen.VectorXd(ref) ref = eigen.VectorXd(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10) assert (config("v6d", eigen.VectorXd) == ref6) assert (config("v3d", eigen.VectorXd) == ref3) assert (config("vXd", eigen.VectorXd) == ref) @raises(RuntimeError) def test_int_tovxd_throw(): config("int", eigen.VectorXd) test_int_tovxd_throw() assert (config("dict")("v6d", eigen.VectorXd) == ref6) assert (config("dict")("v3d", eigen.VectorXd) == ref3) assert (config("emptyArray", eigen.VectorXd.Zero(100)) == eigen.VectorXd.Zero(0)) ref = eigen.Quaterniond(0.71, 0, 0.71, 0) ref.normalize() assert (config("quat", eigen.Quaterniond).isApprox(ref)) assert (config("dict")("quat", eigen.Quaterniond).isApprox(ref)) assert (config("boolTrue", bool)) assert (config("boolTrue", False)) assert (not config("boolFalse", bool)) assert (not config("boolFalse", True)) assert (config("bool1", bool)) assert (not config("bool0", bool)) assert (config("dict")("boolTrue", bool)) assert (not config("dict")("bool0", bool)) ref = [0, 1, 2, 3, 4, 5] assert ([c.to(int) for c in config("intV")] == ref) assert ([c.to(int) for c in config("dict")("intV")] == ref) assert ([c.to(float) for c in config("intV")] == ref) assert ([c.to(float) for c in config("dict")("intV")] == ref) assert (config("intV", [int]) == ref) assert (config("intV", [0]) == ref) ref = ["a", "b", "c", "foo", "bar"] assert ([c.to(str) for c in config("stringV")] == ref) assert ([c.to(str) for c in config("dict")("stringV")] == ref) ref = [1.1, 2.2, 3.3] assert ([c.to(float) for c in config("doubleA3")] == ref) ref = [42.5, -42.5] assert ([c.to(float) for c in config("doubleDoublePair")] == ref) ref = [42.5, "sometext"] c = config("doubleStringPair") assert ([c[0].to(float), c[1].to(str)] == ref) if fromDisk2: config.load(sampleConfig2(True)) else: config.loadData(sampleConfig2(False)) assert (config("int", int) == 12) assert (config("sint", int) == -42) ref = ["a2", "b2", "c2"] assert ([c.to(str) for c in config("stringV")] == ref)
def rpyFromQuat(quat): """Same as mc_rbdyn::rpyFromQuat.""" import eigen return rpyFromMat(list(eigen.Quaterniond(*quat).toRotationMatrix()))
def test_quaternion(): q = e.Quaterniond() # Identity, xyzw coefficient order. id_v = e.Vector4d(0., 0., 0., 1.) # Identity, wxyz scalar constructor order. q = e.Quaterniond(1., 0., 0., 0.) q2 = e.Quaterniond(id_v) # Both identity quaternions must be equal. assert(q.angularDistance(q2) == 0) q3 = q2*q assert(q.angularDistance(q3) == 0) v4 = e.Vector4d.Random() while v4 == id_v: v4 = e.Vector4d.Random() v4.normalize() q = e.Quaterniond(v4) # Vector4d ctor assert(q.coeffs() == v4) q = e.Quaterniond(v4[3], v4[0], v4[1], v4[2]) # 4 doubles ctor assert(q.coeffs() == v4) q_copy = e.Quaterniond(q) # Copy ctor assert(q_copy.coeffs() == q.coeffs()) q_copy.setIdentity() assert(q_copy.coeffs() != q.coeffs()) # Check the two objects are actually different q = e.Quaterniond(np.pi, e.Vector3d.UnitZ()) # Angle-Axis assert((q.coeffs() - e.Vector4d(0., 0., 1., 0.)).norm() < 1e-6) q = e.Quaterniond(e.AngleAxisd(np.pi, e.Vector3d.UnitZ())) assert((q.coeffs() - e.Vector4d(0., 0., 1., 0.)).norm() < 1e-6) q = e.Quaterniond(e.Matrix3d.Identity()) assert(q.coeffs() == id_v) q = e.Quaterniond.Identity() assert(q.coeffs() == id_v) # Check getters assert(q.x() == 0) assert(q.y() == 0) assert(q.z() == 0) assert(q.w() == 1) assert(q.vec() == e.Vector3d.Zero()) assert(q.coeffs() == id_v) # Check setters q = e.Quaterniond(v4) # Rebuild from something other than Identity q.setIdentity() assert(q.coeffs() == id_v) q.setFromTwoVectors(e.Vector3d.UnitX(), e.Vector3d.UnitY()) assert(q.isApprox(e.Quaterniond(np.pi/2, e.Vector3d.UnitZ()))) q.setIdentity() # Operations assert(e.Quaterniond(id_v).angularDistance(e.Quaterniond(np.pi, e.Vector3d.UnitZ())) == np.pi) assert(e.Quaterniond(v4).conjugate().coeffs() == e.Vector4d(-v4.x(), -v4.y(), -v4.z(), v4.w())) assert(e.Quaterniond(id_v).dot(e.Quaterniond(np.pi, e.Vector3d.UnitZ())) == np.cos(np.pi/2)) check_quaternion_almost_equals(e.Quaterniond(v4).inverse(), e.Quaterniond(v4).conjugate()) assert(q.isApprox(q)) assert(q.isApprox(q, 1e-8)) assert(q.matrix() == e.Matrix3d.Identity()) assert(q.toRotationMatrix() == e.Matrix3d.Identity()) v4_2 = e.Vector4d.Random() v4_2 = 2 * v4_2 / v4_2.norm() q = e.Quaterniond(v4_2) assert(q.norm() != 1.0) q_n = q.normalized() assert(q.norm() != 1.0 and q_n.norm() == 1.0) q.normalize() assert(q.norm() == 1.0) check_quaternion_almost_equals(e.Quaterniond.Identity().slerp(1.0, q), q) assert(q.squaredNorm() == 1.0) # Static method q = e.Quaterniond.UnitRandom() assert(q.norm() == 1.0)