def test(self): create_random_mv = lambda: sva.MotionVecd( eigen.Vector3d().Random() * 100, eigen.Vector3d().Random() * 100) # Check empty creation and filling v1 = sva.MotionVecdVector() for i in range(100): v1.append(create_random_mv()) assert (len(v1) == 100) # Check creation from a single MotionVecd object v2 = sva.MotionVecdVector(create_random_mv()) assert (len(v2) == 1) # Check creation from a list v3 = sva.MotionVecdVector([create_random_mv() for i in range(100)]) assert (len(v3) == 100) # Check creation from a lot of MotionVecd v3 = sva.MotionVecdVector(*[create_random_mv() for i in range(100)]) assert (len(v3) == 100) # Check access mv = create_random_mv() v4 = sva.MotionVecdVector([mv] * 100) assert (all([mv == vi for vi in v4]))
def test(self): w = eigen.Vector3d.Random() v = eigen.Vector3d.Random() vec = sva.ImpedanceVecd(w, v) z = vec.vector() self.assertEqual(w, vec.angular()) self.assertEqual(v, vec.linear()) self.assertEqual( z, eigen.Vector6d(w.x(), w.y(), w.z(), v.x(), v.y(), v.z())) self.assertEqual((5. * vec).vector(), 5. * z) self.assertEqual((vec * 5.).vector(), 5. * z) self.assertEqual((vec / 5.).vector(), z / 5.) vec *= 5. self.assertEqual(vec.vector(), 5. * z) vec /= 5. [self.assertAlmostEqual(x, 0, delta=TOL) for x in vec.vector() - z] w2 = eigen.Vector3d.Random() v2 = eigen.Vector3d.Random() vec2 = sva.ImpedanceVecd(w2, v2) z2 = vec2.vector() self.assertAlmostEqual(((vec + vec2).vector() - (z + z2)).norm(), 0, delta=TOL) vec_pluseq = sva.ImpedanceVecd(vec) self.assertEqual(vec_pluseq, vec) vec_pluseq += vec2 self.assertEqual(vec_pluseq, vec + vec2) self.assertEqual(vec, vec) self.assertTrue(not (vec != vec)) w = eigen.Vector3d.Random() v = eigen.Vector3d.Random() mv = sva.MotionVecd(eigen.Vector3d.Random(), eigen.Vector3d.Random()) fv = vec * mv self.assertTrue(isinstance(fv, sva.ForceVecd)) res = eigen.Vector6d( [x * y for x, y in zip(vec.vector(), mv.vector())]) self.assertEqual(res, fv.vector()) fv2 = mv * vec self.assertEqual(fv, fv2) hv = sva.ImpedanceVecd(11., 42.) self.assertEqual(hv.angular(), eigen.Vector3d(11., 11., 11.)) self.assertEqual(hv.linear(), eigen.Vector3d(42., 42., 42.)) z = sva.ImpedanceVecd(0., 0.) self.assertEqual(sva.ImpedanceVecd.Zero(), z)
def test(self): mb1, mbc1Init = arms.makeZXZArm(True, sva.PTransformd(sva.RotZ(-math.pi/4), eigen.Vector3d(-0.5, 0, 0))) rbdyn.forwardKinematics(mb1, mbc1Init) rbdyn.forwardVelocity(mb1, mbc1Init) mb2, mbc2Init = arms.makeZXZArm(False, sva.PTransformd(sva.RotZ(math.pi/2), eigen.Vector3d(0.5, 0, 0))) rbdyn.forwardKinematics(mb2, mbc2Init) rbdyn.forwardVelocity(mb2, mbc2Init) if not LEGACY: mbs = rbdyn.MultiBodyVector([mb1, mb2]) mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init]) else: mbs = [mb1, mb2] mbcs = [rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init)] solver = tasks.qp.QPSolver() if not LEGACY: posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 0.1, 10) posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 0.1, 10) else: posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2, 1) posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2, 1) mrtt = tasks.qp.MultiRobotTransformTask(mbs, 0, 1, "b3", "b3", sva.PTransformd(sva.RotZ(-math.pi/8)), sva.PTransformd.Identity(), 100, 1000) if not LEGACY: mrtt.dimWeight(eigen.VectorXd(0, 0, 1, 1, 1, 0)) else: mrtt.dimWeight(eigen.Vector6d(0, 0, 1, 1, 1, 0)) solver.addTask(posture1Task) solver.addTask(posture2Task) solver.addTask(mrtt) solver.nrVars(mbs, [], []) solver.updateConstrSize() # 3 dof + 9 dof self.assertEqual(solver.nrVars(), 3 + 9) for i in range(2000): 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]) self.assertAlmostEqual(mrtt.eval().norm(), 0, delta = 1e-3) solver.removeTask(posture1Task) solver.removeTask(posture2Task) solver.removeTask(mrtt)
def test(self): from_ = sva.PTransformd(eigen.Matrix3d.Identity(), eigen.Vector3d.Zero()) to = sva.PTransformd(eigen.AngleAxisd(np.pi, eigen.Vector3d.UnitZ()), eigen.Vector3d(1, 2, -3)) res = sva.interpolate(from_, to, 0.5) self.assertAlmostEqual((res.rotation() - eigen.AngleAxisd( np.pi / 2, eigen.Vector3d.UnitZ()).matrix()).norm(), 0, delta=TOL) self.assertAlmostEqual( (res.translation() - eigen.Vector3d(0.5, 1., -1.5)).norm(), 0, delta=TOL)
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): sphere = sch.Sphere(0.1) box = sch.Box(0.1, 0.1, 0.1) box.transform(sva.PTransformd(eigen.Vector3d(0.2, 0, 0))) pair = sch.CD_Pair(sphere, box) print("Distance between sphere and box: {}".format( sqrt(pair.distance())))
def grasp_dataset_to_ptransform(camera_T_base, base_T_endeffector, gripper_z_offset=None): """Convert brainrobotdata features camera_T_base and base_T_endeffector to camera_T_endeffector and ptransforms. This specific function exists because it accepts the raw feature types defined in the google brain robot grasping dataset. # Params camera_T_base: a vector quaternion array base_T_endeffector: a 4x4 homogeneous 3D transformation matrix gripper_z_offset: a float offset in meters in gripper's z axis # Returns PTransformd formatted transforms: camera_T_endeffector_ptrans, base_T_endeffector_ptrans, base_T_camera_ptrans """ base_T_endeffector_ptrans = vector_quaternion_array_to_ptransform( base_T_endeffector) if gripper_z_offset is not None and gripper_z_offset != 0.0: # Add a z axis offset to the gripper frame, measured in meters. q = eigen.Quaterniond.Identity() v = eigen.Vector3d([0, 0, gripper_z_offset]) pt_z_offset = sva.PTransformd(q, v) base_T_endeffector_ptrans = pt_z_offset * base_T_endeffector_ptrans # In this case camera_T_base is a transform that takes a point in the base # frame of reference and transforms it to the camera frame of reference. camera_T_base_ptrans = matrix_to_ptransform(camera_T_base) base_T_camera = camera_T_base_ptrans.inv() # Perform the actual transform calculation camera_T_endeffector_ptrans = base_T_endeffector_ptrans * base_T_camera.inv( ) return camera_T_endeffector_ptrans, base_T_endeffector_ptrans, base_T_camera
def reset_callback(self, data): self.comTask.reset() self.robots().robot(1).posW( sva.PTransformd(sva.RotZ(math.pi), eigen.Vector3d(0.7, 0.5, 0))) self.doorKinematics = mc_solver.KinematicsConstraint( self.robots(), 1, self.qpsolver.timeStep) self.qpsolver.addConstraintSet(self.doorKinematics) self.doorPosture = mc_tasks.PostureTask(self.qpsolver, 1, 5.0, 1000.0) self.qpsolver.addTask(self.doorPosture) self.handTask = mc_tasks.SurfaceTransformTask("RightGripper", self.robots(), 0, 5.0, 1000.0) self.qpsolver.addTask(self.handTask) self.handTask.target( sva.PTransformd(eigen.Vector3d(0, 0, -0.025)) * self.robots().robot(1).surfacePose("Handle"))
def __init__(self, X=sva.PTransformd.Identity(), length=0.1, text=''): """ Create a 3D axis. """ self._X = X self.axesActor = tvtk.AxesActor(total_length=(length, ) * 3, axis_labels=False) self.axesActor.user_transform = tvtk.Transform() textSource = tvtk.TextSource(text=text, backing=False) textPdm = tvtk.PolyDataMapper(input=textSource.output) #self.textActor = tvtk.Actor(mapper=textPdm) self.textActor = tvtk.Follower(mapper=textPdm) # take the maximum component of the bound and use it to scale it m = max(self.textActor.bounds) scale = length / m self.textActor.scale = (scale, ) * 3 # TODO compute the origin well... self.textActor.origin = ( -(self.textActor.bounds[0] + self.textActor.bounds[1]) / 2., -(self.textActor.bounds[2] + self.textActor.bounds[3]) / 2., -(self.textActor.bounds[4] + self.textActor.bounds[5]) / 2., ) ySize = self.textActor.bounds[3] * 1.2 self.X_text = sva.PTransformd(e.Vector3d(0., -ySize, 0.)) self._transform()
def __init__(self, X=sva.PTransformd.Identity(), length=0.1, text=''): """ Create a 3D axis. """ self._X = X self.axesActor = tvtk.AxesActor(total_length=(length, ) * 3, axis_labels=False) self.axesActor.user_transform = tvtk.Transform() textSource = tvtk.TextSource(text=text, backing=False) # textPdm = tvtk.PolyDataMapper(input=textSource.output) textPdm = tvtk.PolyDataMapper() # https://stackoverflow.com/questions/35089379/how-to-fix-traiterror-the-input-trait-of-a-instance-is-read-only # configure_input_data(textPdm, textSource.output_port) # https://github.com/enthought/mayavi/issues/521 textPdm.input_connection = textSource.output_port #self.textActor = tvtk.Actor(mapper=textPdm) self.textActor = tvtk.Follower(mapper=textPdm) # take the maximum component of the bound and use it to scale it m = max(self.textActor.bounds) scale = length / m self.textActor.scale = (scale, ) * 3 # TODO compute the origin well... self.textActor.origin = ( -(self.textActor.bounds[0] + self.textActor.bounds[1]) / 2., -(self.textActor.bounds[2] + self.textActor.bounds[3]) / 2., -(self.textActor.bounds[4] + self.textActor.bounds[5]) / 2., ) ySize = self.textActor.bounds[3] * 1.2 self.X_text = sva.PTransformd(e.Vector3d(0., -ySize, 0.)) self._transform()
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 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 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 makeZXZArm(isFixed=True, X_base=sva.PTransformd.Identity()): mbg = rbdyn.MultiBodyGraph() mass = 1.0 I = eigen.Matrix3d.Identity() h = eigen.Vector3d.Zero() rbi = sva.RBInertiad(mass, h, I) b0 = rbdyn.Body(rbi, "b0") b1 = rbdyn.Body(rbi, "b1") b2 = rbdyn.Body(rbi, "b2") b3 = rbdyn.Body(rbi, "b3") mbg.addBody(b0) mbg.addBody(b1) mbg.addBody(b2) mbg.addBody(b3) j0 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitZ(), True, "j0") j1 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j1") j2 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitZ(), True, "j2") mbg.addJoint(j0) mbg.addJoint(j1) mbg.addJoint(j2) to = sva.PTransformd(eigen.Vector3d(0, 0.5, 0)) _from = sva.PTransformd(eigen.Vector3d(0, 0, 0)) mbg.linkBodies("b0", sva.PTransformd.Identity(), "b1", _from, "j0") mbg.linkBodies("b1", to, "b2", _from, "j1") mbg.linkBodies("b2", to, "b3", _from, "j2") mb = mbg.makeMultiBody("b0", isFixed, X_base) mbc = rbdyn.MultiBodyConfig(mb) mbc.zero(mb) return mb, mbc
def test(self): res = eigen.Vector3d() res = sva.rotationError(eigen.Matrix3d.Identity(), sva.RotX(np.pi / 2)) self.assertAlmostEqual((res - eigen.Vector3d(np.pi / 2, 0, 0)).norm(), 0, delta=TOL) res = sva.rotationError(eigen.Matrix3d.Identity(), sva.RotY(np.pi / 2)) self.assertAlmostEqual((res - eigen.Vector3d(0, np.pi / 2, 0)).norm(), 0, delta=TOL) res = sva.rotationError(eigen.Matrix3d.Identity(), sva.RotZ(np.pi / 2)) self.assertAlmostEqual((res - eigen.Vector3d(0, 0, np.pi / 2)).norm(), 0, delta=TOL) res = sva.rotationError(sva.RotZ(np.pi / 4), sva.RotZ(np.pi / 2)) self.assertAlmostEqual((res - eigen.Vector3d(0, 0, np.pi / 4)).norm(), 0, delta=TOL)
def __init__(self, urdf_path): self.urdf_path = urdf_path self.kine_dyn = from_urdf_file(self.urdf_path) self.id = -1 print("Imported from " + self.urdf_path + ", robot with name " + self.kine_dyn.name.decode("utf-8")) # joints self.dof = self.kine_dyn.mb.nrDof() # set gravity direction (this is the acceleration at base joint for RNEA) self.kine_dyn.mbc.gravity = e.Vector3d(0, 0, 9.81) self.kine_dyn.mbc.zero(self.kine_dyn.mb) # robot limits self.lower_limit = e.VectorXd.Zero(self.dof) self.upper_limit = e.VectorXd.Zero(self.dof) for i, (k, v) in enumerate(self.kine_dyn.limits.lower.items()): self.lower_limit[i] = v for i, (k, v) in enumerate(self.kine_dyn.limits.upper.items()): self.upper_limit[i] = v
def test_vecOriTask(self): vecTask1 = mc_tasks.VectorOrientationTask("r_wrist", e.Vector3d(0., 0., 1.), e.Vector3d(1., 0., 0.), self.robots, 0) vecTask2 = mc_tasks.VectorOrientationTask("r_wrist", e.Vector3d(0., 0., 1.), e.Vector3d(1., 0., 0.), self.robots, 0, 2.0, 500) vecTask3 = mc_tasks.VectorOrientationTask("r_wrist", e.Vector3d(0., 0., 1.), e.Vector3d(1., 0., 0.), self.robots, 0, weight=500.0, stiffness=2.0) assert (vecTask1.stiffness() == vecTask2.stiffness()) assert (vecTask1.stiffness() == vecTask3.stiffness()) assert (vecTask1.weight() == vecTask2.weight()) assert (vecTask1.weight() == vecTask3.weight())
import eigen as e import sva from robots import TutorialTree mbg, mb, mbc = TutorialTree() q = map(list, mbc.q) q[1] = [np.pi / 2.] q[2] = [-np.pi / 4.] q[3] = [-np.pi / 2.] q[4] = [0.5] mbc.q = q rbd.forwardKinematics(mb, mbc) X_s = sva.PTransformd(sva.RotY(-np.pi / 2.), e.Vector3d(0.1, 0., 0.)) mbv = MultiBodyViz(mbg, mb, endEffectorDict={'b4': (X_s, 0.1, (0., 1., 0.))}) # test MultiBodyViz from tvtk.tools import ivtk viewer = ivtk.viewer() viewer.size = (640, 480) mbv.addActors(viewer.scene) mbv.display(mb, mbc) # test axis from axis import Axis a1 = Axis(text='test', length=0.2) a1.addActors(viewer.scene)
if __name__ == "__main__": nrIter = 10000 mb, mbcInit = makeZXZArm() rbdyn.forwardKinematics(mb, mbcInit) rbdyn.forwardVelocity(mb, mbcInit) mbs = rbdyn.MultiBodyVector([mb]) mbcs = rbdyn.MultiBodyConfigVector([mbcInit]) solver = tasks.qp.QPSolver() solver.nrVars(mbs, [], []) solver.updateConstrSize() posD = eigen.Vector3d(0.707106, 0.707106, 0.0) posTask = tasks.qp.PositionTask(mbs, 0, "b3", posD) posTaskSp = tasks.qp.SetPointTask(mbs, 0, posTask, 10, 1) solver.addTask(posTaskSp) for i in range(nrIter): solver.solve(mbs, mbcs) rbdyn.eulerIntegration(mbs[0], mbcs[0], 0.001) rbdyn.forwardKinematics(mbs[0], mbcs[0]) rbdyn.forwardVelocity(mbs[0], mbcs[0]) print("(Python) Final norm of position task: {}".format(posTask.eval().norm()))
def reset_callback(self, reset_data): assert (len(self.observerPipelines()) == 1) assert (self.hasObserverPipeline("FirstPipeline")) assert (not self.hasObserverPipeline("NotAPipeline")) self.addAnchorFrameCallback( "KinematicAnchorFrame::{}".format(self.robot().name().decode()), lambda r: sva.interpolate(r.surfacePose("LeftFoot"), r.surfacePose("RightFoot"), 0.5)) self.positionTask.reset() self.positionTask.position(self.positionTask.position() + eigen.Vector3d(0.1, 0, 0)) self.logger().addLogEntry("PYTHONDOUBLE", lambda: self.d_data) self.logger().addLogEntry("PYTHONDOUBLEV", lambda: self.dv_data) self.logger().addLogEntry("PYTHONQUAT", lambda: self.quat_data) # Demonstrate use of partial self.logger().addLogEntry("PYTHONPT", partial(self.get_pt)) # Alternative syntax for above call # self.logger().addLogEntry("PYTHONPT", partial(SampleController.get_pt, self)) self.logger().addLogEntry( "PYTHONFV", lambda: sva.ForceVecd(eigen.Vector6d(0, 0, 0, 0, 0, 100)) + sva. ForceVecd(eigen.Vector6d(0, 0, 0, 0, 0, 100))) # Not a very efficient way to log a double value self.logger().addLogEntry("PYTHONSTR", lambda: str(self.theta)) # GUI similar to dummyServer self.gui().addElement(["Python"], mc_rtc.gui.Label("theta", lambda: self.theta)) self.gui().addElement( ["Python"], mc_rtc.gui.ArrayLabel( "array", lambda: [self.theta, 2 * self.theta, 4 * self.theta]), mc_rtc.gui.ArrayLabel( "array with labels", lambda: [self.theta, 2 * self.theta, 4 * self.theta], ["x", "y", "z"])) self.gui().addElement( ["Python"], mc_rtc.gui.Button("button", lambda: sys.stdout.write("Button clicked\n"))) self.check_ = True self.gui().addElement(["Python"], mc_rtc.gui.Checkbox("checkbox", lambda: self.check_, self.check)) self.mystring_ = "test" self.myint_ = 0 self.mynumber_ = 0.0 self.gui().addElement(["Python"], mc_rtc.gui.StringInput("string input", self.mystring, self.mystring), mc_rtc.gui.IntegerInput("integer input", self.myint, self.myint), mc_rtc.gui.NumberInput("number input", self.mynumber, self.mynumber)) self.myslider_ = 0.0 self.gui().addElement(["Python"], mc_rtc.gui.NumberSlider("number slider", self.myslider, self.myslider, -100.0, 100.0)) self.myarray_ = [0., 1., 2., 3.] self.gui().addElement(["Python"], mc_rtc.gui.ArrayInput("array input", self.myarray, self.myarray)) self.gui().addElement(["Python"], mc_rtc.gui.ArrayInput("array input with labels", self.myarray, self.myarray, ["w", "x", "y", "z"])) self.mycombo_ = "a" self.gui().addElement(["Python"], mc_rtc.gui.ComboInput("combo input", ["a", "b", "c", "d"], self.mycombo, self.mycombo)) self.mydatacombo_ = "" self.gui().addElement(["Python"], mc_rtc.gui.DataComboInput( "data combo", ["robots"], self.mydatacombo, self.mydatacombo)) self.mypoint3dro_ = [0., 1., 0.] self.gui().addElement(["Python"], mc_rtc.gui.Point3D("Point3D ro", self.mypoint3dro)) self.mypoint3d_ = [0., 0., 1.] self.gui().addElement(["Python"], mc_rtc.gui.Point3D("Point3D", self.mypoint3d, self.mypoint3d)) self.gui().addElement( ["Python", "Form"], mc_rtc.gui.Form( "Submit", lambda c: sys.stdout.write(c.dump(True) + '\n'), mc_rtc.gui.FormCheckbox("Enabled", False, True), mc_rtc.gui.FormIntegerInput("INT", False, 42), mc_rtc.gui.FormNumberInput("NUMBER", False, 0.42), mc_rtc.gui.FormStringInput("STRING", False, "a certain string"), mc_rtc.gui.FormNumberArrayInput("ARRAY_FIXED_SIZE", False, [1, 2, 3]), mc_rtc.gui.FormNumberArrayInput("ARRAY_UNBOUNDED", False), mc_rtc.gui.FormComboInput("CHOOSE WISELY", False, ["A", "B", "C", "D"]), mc_rtc.gui.FormDataComboInput("R0", False, ["robots"]), mc_rtc.gui.FormDataComboInput("R0 surface", False, ["surfaces", "$R0"]), mc_rtc.gui.FormDataComboInput("R1", False, ["robots"]), mc_rtc.gui.FormDataComboInput("R1 surface", False, ["surfaces", "$R1"]))) self._selfDestructId = 0 self.gui().addElement( ["Python", "Experiment"], mc_rtc.gui.Button( "Self-destruct category", lambda: self.gui().removeCategory(["Python", "Experiment"]))) self.gui().addElement(["Python", "Experiment"], mc_rtc.gui.Button("Add self-destruct button", self.addSelfDestructButton))
def test(self): mb1, mbc1Init = arms.makeZXZArm( True, sva.PTransformd(eigen.Vector3d(-0.5, 0, 0))) rbdyn.forwardKinematics(mb1, mbc1Init) rbdyn.forwardVelocity(mb1, mbc1Init) mb2, mbc2Init = arms.makeZXZArm( True, sva.PTransformd(eigen.Vector3d(0.5, 0, 0))) 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) ] nrGen = 3 solver = tasks.qp.QPSolver() contVec = [ tasks.qp.UnilateralContact(0, 1, "b3", "b3", [eigen.Vector3d.Zero()], sva.RotX(math.pi / 2), X_b1_b2, nrGen, 0.7) ] 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) comD = (rbdyn.computeCoM(mb1, mbc1Init) + rbdyn.computeCoM( mb2, mbc2Init)) / 2 + eigen.Vector3d(0, 0, 0.5) multiCoM = tasks.qp.MultiCoMTask(mbs, [0, 1], comD, 10, 500) multiCoM.updateInertialParameters(mbs) contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001) solver.addTask(posture1Task) solver.addTask(posture2Task) solver.nrVars(mbs, contVec, []) solver.addTask(mbs, multiCoM) contCstrSpeed.addToSolver(mbs, solver) solver.updateConstrSize() self.assertEqual(solver.nrVars(), 3 + 3 + 1 * nrGen) for i in range(2000): 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) self.assertAlmostEqual(multiCoM.speed().norm(), 0, delta=1e-3) contCstrSpeed.removeFromSolver(solver) solver.removeTask(posture1Task) solver.removeTask(posture2Task) solver.removeTask(multiCoM)
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 test(self): # regiter custom pickle function rbd.copy_reg_pickle() # create a body with random inertia def makeBody(bName): I = sva.RBInertiad(e3.Vector3d.Random().x(), e3.Vector3d.Random(), e3.Matrix3d.Random()) return rbd.Body(I, bName) body = makeBody('testBody') jR = rbd.Joint(rbd.Joint.Rev, e3.Vector3d.Random().normalized(), True, 'jR') jP = rbd.Joint(rbd.Joint.Prism, e3.Vector3d.Random().normalized(), False, 'jP') jS = rbd.Joint(rbd.Joint.Spherical, False, 'jS') jPla = rbd.Joint(rbd.Joint.Planar, True, 'jPla') jC = rbd.Joint(rbd.Joint.Cylindrical, e3.Vector3d.Random().normalized(), False, 'jC') jFree = rbd.Joint(rbd.Joint.Free, True, 'jFree') jFix = rbd.Joint(rbd.Joint.Fixed, False, 'jFix') mb = rbd.MultiBody( [makeBody('body%s' % i) for i in range(7)], [jR, jP, jS, jPla, jC, jFree, jFix], list(range(-1, 6)), list(range(0, 7)), list(range(-1, 6)), [sva.PTransformd(e3.Vector3d(0., i, 0.)) for i in range(7)]) def test(v, func): pickled = pickle.dumps(v) v2 = pickle.loads(pickled) assert (func(v, v2)) def bodyEq(b1, b2): return b1.inertia() == b2.inertia() and\ b1.name() == b2.name() def jointEq(j1, j2): return j1.type() == j2.type() and\ j1.name() == j2.name() and\ j1.direction() == j2.direction() and\ list(j1.motionSubspace()) == list(j2.motionSubspace()) def multiBodyEq(mb1, mb2): isEq = True for b1, b2 in zip(mb1.bodies(), mb2.bodies()): isEq &= bodyEq(b1, b2) for j1, j2 in zip(mb1.joints(), mb2.joints()): isEq &= jointEq(j1, j2) mb1T = (list(mb1.predecessors()), list(mb1.successors()), list(mb1.parents()), list(mb1.transforms())) mb2T = (list(mb2.predecessors()), list(mb2.successors()), list(mb2.parents()), list(mb2.transforms())) return isEq and mb1T == mb2T # body test(body, bodyEq) # joints test(jR, jointEq) test(jP, jointEq) test(jS, jointEq) test(jPla, jointEq) test(jC, jointEq) test(jFree, jointEq) test(jFix, jointEq) # multiBody test(mb, multiBodyEq)
def createRobot(): mb, mbc, mbg = rbdyn.MultiBody(), rbdyn.MultiBodyConfig( ), rbdyn.MultiBodyGraph() limits = mc_rbdyn_urdf.Limits() visual = {} collision_tf = {} I0 = eigen.Matrix3d([[0.1, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0, 0.001]]) I1 = eigen.Matrix3d([[1.35, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0, 1.251]]) I2 = eigen.Matrix3d([[0.6, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0, 0.501]]) I3 = eigen.Matrix3d([[0.475, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0, 0.376]]) I4 = eigen.Matrix3d([[0.1, 0.0, 0.0], [0.0, 0.3, 0.0], [0.0, 0.0, 0.251]]) T0 = sva.PTransformd(eigen.Vector3d(0.1, 0.2, 0.3)) T1 = sva.PTransformd.Identity() b0 = rbdyn.Body(1., eigen.Vector3d.Zero(), I0, "b0") b1 = rbdyn.Body(5., eigen.Vector3d(0., 0.5, 0.), I1, "b1") b2 = rbdyn.Body(2., eigen.Vector3d(0., 0.5, 0.), I2, "b2") b3 = rbdyn.Body(1.5, eigen.Vector3d(0., 0.5, 0.), I3, "b3") b4 = rbdyn.Body(1., eigen.Vector3d(0.5, 0., 0.), I4, "b4") mbg.addBody(b0) mbg.addBody(b1) mbg.addBody(b2) mbg.addBody(b3) mbg.addBody(b4) j0 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j0") j1 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitY(), True, "j1") j2 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitZ(), True, "j2") j3 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j3") mbg.addJoint(j0) mbg.addJoint(j1) mbg.addJoint(j2) mbg.addJoint(j3) to = sva.PTransformd(eigen.Vector3d(0, 1, 0)) from_ = sva.PTransformd.Identity() mbg.linkBodies("b0", to, "b1", from_, "j0") mbg.linkBodies("b1", to, "b2", from_, "j1") mbg.linkBodies("b2", to, "b3", from_, "j2") mbg.linkBodies("b1", sva.PTransformd(sva.RotX(1.), eigen.Vector3d(1., 0., 0.)), "b4", from_, "j3") mb = mbg.makeMultiBody("b0", True) mbc = rbdyn.MultiBodyConfig(mb) mbc.zero(mb) limits.lower = { "j0": [-1.], "j1": [-1.], "j2": [-1.], "j3": [-float('Inf')] } limits.upper = {"j0": [1.], "j1": [1.], "j2": [1.], "j3": [float('Inf')]} limits.velocity = { "j0": [10.], "j1": [10.], "j2": [10.], "j3": [float('Inf')] } limits.torque = { "j0": [50.], "j1": [50.], "j2": [50.], "j3": [float('Inf')] } v1 = mc_rbdyn_urdf.Visual() v1.origin = T0 geometry = mc_rbdyn_urdf.Geometry() geometry.type = mc_rbdyn_urdf.Geometry.MESH mesh = mc_rbdyn_urdf.GeometryMesh() mesh.filename = "test_mesh1.dae" geometry.data = mesh v1.geometry = geometry v2 = mc_rbdyn_urdf.Visual() v2.origin = T1 mesh.filename = "test_mesh2.dae" geometry.data = mesh v2.geometry = geometry visual = {b"b0": [v1, v2]} return mb, mbc, mbg, limits, visual, collision_tf
def TutorialTree(): """ Return the MultiBodyGraph, MultiBody and the zeroed MultiBodyConfig with the following tree structure: b4 j3 | Spherical Root j0 | j1 j2 j4 ---- b0 ---- b1 ---- b2 ----b3 ----b5 Fixed RevX RevY RevZ PrismZ """ mbg = rbd.MultiBodyGraph() mass = 1. I = e.Matrix3d.Identity() h = e.Vector3d.Zero() rbi = sva.RBInertiad(mass, h, I) b0 = rbd.Body(rbi, "b0") b1 = rbd.Body(rbi, "b1") b2 = rbd.Body(rbi, "b2") b3 = rbd.Body(rbi, "b3") b4 = rbd.Body(rbi, "b4") b5 = rbd.Body(rbi, "b5") mbg.addBody(b0) mbg.addBody(b1) mbg.addBody(b2) mbg.addBody(b3) mbg.addBody(b4) mbg.addBody(b5) j0 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitX(), True, "j0") j1 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitY(), True, "j1") j2 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitZ(), True, "j2") j3 = rbd.Joint(rbd.Joint.Spherical, True, "j3") j4 = rbd.Joint(rbd.Joint.Prism, e.Vector3d.UnitY(), True, "j4") mbg.addJoint(j0) mbg.addJoint(j1) mbg.addJoint(j2) mbg.addJoint(j3) mbg.addJoint(j4) to = sva.PTransformd(e.Vector3d(0., 0.5, 0.)) fro = sva.PTransformd.Identity() mbg.linkBodies("b0", to, "b1", fro, "j0") mbg.linkBodies("b1", to, "b2", fro, "j1") mbg.linkBodies("b2", to, "b3", fro, "j2") mbg.linkBodies("b1", sva.PTransformd(e.Vector3d(0.5, 0., 0.)), "b4", fro, "j3") mbg.linkBodies("b3", to, "b5", fro, "j4") mb = mbg.makeMultiBody("b0", True) mbc = rbd.MultiBodyConfig(mb) mbc.zero(mb) return mbg, mb, mbc
#!/usr/bin/env python # -*- coding: utf-8 -*- import eigen import sva import sch from math import sqrt sphere = sch.Sphere(0.1) box = sch.Box(0.1, 0.1, 0.1) box.transform(sva.PTransformd(eigen.Vector3d(0.2, 0, 0))) pair = sch.CD_Pair(sphere, box) print("Distance between sphere and box: {}".format(sqrt(pair.distance())))