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 __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 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 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_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 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 endEffectorBody(X_s, size, color):
"""
Return a end effector reprsented by a plane
and the appropriate static transform.
"""
apd = tvtk.AppendPolyData()
ls = tvtk.LineSource(point1=(0., 0., 0.),
point2=tuple(X_s.translation()))
p1 = (sva.PTransformd(e.Vector3d.UnitX()*size)*X_s).translation()
p2 = (sva.PTransformd(e.Vector3d.UnitY()*size)*X_s).translation()
ps = tvtk.PlaneSource(origin=tuple(X_s.translation()),
point1=tuple(p1),
point2=tuple(p2),
center=tuple(X_s.translation()))
# apd.add_input(ls.output)
# apd.add_input(ps.output)
# https://github.com/enthought/mayavi/blob/ac5c8e316335078c25461a0bce4a724ae86f1836/tvtk/tests/test_tvtk.py#L586
apd.add_input_data(ls.output)
apd.add_input_data(ps.output)
# pdm = tvtk.PolyDataMapper(input=apd.output)
# arcPdm = tvtk.PolyDataMapper(input=arcSource.output)
pdm = 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)# https://github.com/enthought/mayavi/issues/521
pdm.input_connection = apd.output_port
prop = tvtk.Property(color=color)
# https://github.com/enthought/mayavi/issues/521
# arcPdm.input_connection = arcSource.output_port
actor = tvtk.Actor(mapper=pdm, property=prop)
actor.property.color = color
actor.user_transform = tvtk.Transform()
return actor, sva.PTransformd.Identity()
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):
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 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 _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 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):
# 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)
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.UnitX(), True, "j0")
j1 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j1")
j2 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j2")
mbg.addJoint(j0)
mbg.addJoint(j1)
mbg.addJoint(j2)
to = sva.PTransformd(eigen.Vector3d.UnitY())
from_ = sva.PTransformd(eigen.Vector3d.Zero())
mbg.linkBodies("b0", from_, "b1", from_, "j0")
mbg.linkBodies("b1", to, "b2", from_, "j1")
mbg.linkBodies("b2", to, "b3", from_, "j2")
mb = mbg.makeMultiBody("b0", True)
mbc = rbdyn.MultiBodyConfig(mb)
mbc.zero(mb)
rbdyn.forwardKinematics(mb, mbc)
print(mbc.bodyPosW[-1])
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)
def test(self):
Eq = eigen.AngleAxisd(np.pi / 2, eigen.Vector3d.UnitX())
r = eigen.Vector3d.Random() * 100
pt = sva.PTransformd(Eq, r)
ptInv = pt.inv()
pt6d = pt.matrix()
ptInv6d = ptInv.matrix()
ptDual6d = pt.dualMatrix()
M = eigen.Matrix3d([[1, 2, 3], [2, 1, 4], [3, 4, 1]])
H = eigen.Matrix3d.Random() * 100
I = eigen.Matrix3d([[1, 2, 3], [2, 1, 4], [3, 4, 1]])
ab = sva.ABInertiad(M, H, I)
ab6d = ab.matrix()
mass = 1.
h = eigen.Vector3d.Random() * 100
rb = sva.RBInertiad(mass, h, I)
rb6d = rb.matrix()
w = eigen.Vector3d.Random() * 100
v = eigen.Vector3d.Random() * 100
n = eigen.Vector3d.Random() * 100
f = eigen.Vector3d.Random() * 100
mVec = sva.MotionVecd(w, v)
mVec6d = mVec.vector()
fVec = sva.ForceVecd(n, f)
fVec6d = fVec.vector()
mvRes1 = pt * mVec
mvRes16d = pt6d * mVec6d
self.assertAlmostEqual((mvRes1.vector() - mvRes16d).norm(),
0,
delta=TOL)
self.assertAlmostEqual((mvRes1.angular() - pt.angularMul(mVec)).norm(),
0,
delta=TOL)
self.assertAlmostEqual((mvRes1.linear() - pt.linearMul(mVec)).norm(),
0,
delta=TOL)
# Note: the C++ version would test the vectorized version here but this is not supported by the Python bindings
mvRes2 = pt.invMul(mVec)
mvRes26d = ptInv6d * mVec6d
self.assertAlmostEqual((mvRes2.vector() - mvRes26d).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(mvRes2.angular() - pt.angularInvMul(mVec)).norm(), 0, delta=TOL)
self.assertAlmostEqual(
(mvRes2.linear() - pt.linearInvMul(mVec)).norm(), 0, delta=TOL)
# Same note about the vectorized version
fvRes1 = pt.dualMul(fVec)
fvRes16d = ptDual6d * fVec6d
self.assertAlmostEqual((fvRes1.vector() - fvRes16d).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(fvRes1.couple() - pt.coupleDualMul(fVec)).norm(), 0, delta=TOL)
self.assertAlmostEqual((fvRes1.force() - pt.forceDualMul(fVec)).norm(),
0,
delta=TOL)
# Same note about the vectorized version
fvRes2 = pt.transMul(fVec)
fvRes26d = pt6d.transpose() * fVec6d
self.assertAlmostEqual((fvRes2.vector() - fvRes26d).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(fvRes2.couple() - pt.coupleTransMul(fVec)).norm(), 0, delta=TOL)
self.assertAlmostEqual(
(fvRes2.force() - pt.forceTransMul(fVec)).norm(), 0, delta=TOL)
# Same note about the vectorized version
rbRes1 = pt.dualMul(rb)
rbRes16d = ptDual6d * rb6d * ptInv6d
self.assertAlmostEqual((rbRes1.matrix() - rbRes16d).norm(),
0,
delta=TOL)
rbRes2 = pt.transMul(rb)
rbRes26d = pt6d.transpose() * rb6d * pt6d
self.assertAlmostEqual((rbRes2.matrix() - rbRes26d).norm(),
0,
delta=TOL)
abRes1 = pt.dualMul(ab)
abRes16d = ptDual6d * ab6d * ptInv6d
self.assertAlmostEqual((abRes1.matrix() - abRes16d).norm(),
0,
delta=TOL)
abRes2 = pt.transMul(ab)
abRes26d = pt6d.transpose() * ab6d * pt6d
self.assertAlmostEqual((abRes2.matrix() - abRes26d).norm(),
0,
delta=TOL)
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 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()
mb2, mbc2Init = arms.makeZXZArm()
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
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 contraint and test PositionTask on the second robot
solver = tasks.qp.QPSolver()
contVec = [
tasks.qp.UnilateralContact(0, 1, "b3", "b3",
[eigen.Vector3d.Zero()],
sva.RotX(math.pi / 2), X_b1_b2, 3,
math.tan(math.pi / 4))
]
oriD = sva.RotZ(math.pi / 4)
if not LEGACY:
posD = oriD * mbc2Init.bodyPosW[-1].translation()
else:
posD = oriD * list(mbc2Init.bodyPosW)[-1].translation()
posTask = tasks.qp.PositionTask(mbs, 1, "b3", posD)
posTaskSp = tasks.qp.SetPointTask(mbs, 1, posTask, 1000, 1)
contCstrAcc = tasks.qp.ContactAccConstr()
contCstrAcc.addToSolver(solver)
solver.addTask(posTaskSp)
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 3 + 3)
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)
self.assertAlmostEqual(posTask.eval().norm(), 0, delta=1e-5)
contCstrAcc.removeFromSolver(solver)
solver.removeTask(posTaskSp)
# Test ContactSpeedConstr constraint and OrientationTask on the second robot
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
oriTask = tasks.qp.OrientationTask(mbs, 1, "b3", oriD)
oriTaskSp = tasks.qp.SetPointTask(mbs, 1, oriTask, 1000, 1)
contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001)
contCstrSpeed.addToSolver(solver)
solver.addTask(oriTaskSp)
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)
self.assertAlmostEqual(oriTask.eval().norm(), 0, delta=1e-5)
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
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)
#!/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())))
