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