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.RBInertia(mass, h, I)
b0 = rbd.Body(rbi, 0, "b0")
b1 = rbd.Body(rbi, 1, "b1")
b2 = rbd.Body(rbi, 2, "b2")
b3 = rbd.Body(rbi, 3, "b3")
b4 = rbd.Body(rbi, 4, "b4")
b5 = rbd.Body(rbi, 5, "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, 0, "j0")
j1 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitY(), True, 1, "j1")
j2 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitZ(), True, 2, "j2")
j3 = rbd.Joint(rbd.Joint.Spherical, True, 3, "j3")
j4 = rbd.Joint(rbd.Joint.Prism, e.Vector3d.UnitY(), True, 4, "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(0, to, 1, fro, 0)
mbg.linkBodies(1, to, 2, fro, 1)
mbg.linkBodies(2, to, 3, fro, 2)
mbg.linkBodies(1, sva.PTransformd(e.Vector3d(0.5, 0., 0.)), 4, fro, 3)
mbg.linkBodies(3, to, 5, fro, 4)
mb = mbg.makeMultiBody(0, True)
mbc = rbd.MultiBodyConfig(mb)
mbc.zero(mb)
return mbg, 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)
pdm = tvtk.PolyDataMapper(input=apd.output)
actor = tvtk.Actor(mapper=pdm)
actor.property.color = color
actor.user_transform = tvtk.Transform()
return actor, sva.PTransformd.Identity()
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 J(self, mb, mbc):
X_O_p = self.X_O_p(mbc)
# set transformation in Origin orientation frame
X_O_p_O = sva.PTransformd(X_O_p.rotation()).inv() * X_O_p
jac_mat_dense = self.jac.jacobian(mb, mbc, X_O_p_O)
self.jac.fullJacobian(mb, jac_mat_dense, self.jac_mat_sparse)
return e.toNumpy(self.jac_mat_sparse)
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
import os
sys.path.insert(
0, os.path.join("@CMAKE_CURRENT_BINARY_DIR@", '../binding/python'))
import pickle
import eigen3 as e3
import spacevecalg as sva
if __name__ == '__main__':
# regiter 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(v):
pickled = pickle.dumps(v)
v2 = pickle.loads(pickled)
assert (v == v2)
test(mv)
test(fv)
test(pt)
test(rb)
test(ab)
import spacevecalg as sva
from ik_tasks import BodyTask, PostureTask, CoMTask
from robots import TutorialTree
mbg, mb, mbc = TutorialTree()
quat = e.Quaterniond(np.pi / 3., e.Vector3d(0.1, 0.5, 0.3).normalized())
mbc.q = [[], [3. * np.pi / 4.], [np.pi / 3.], [-3. * np.pi / 4.], [0.],
[quat.w(), quat.x(), quat.y(),
quat.z()]]
rbd.forwardKinematics(mb, mbc)
rbd.forwardVelocity(mb, mbc)
# target frame
X_O_T = sva.PTransformd(sva.RotY(np.pi / 2.), e.Vector3d(1.5, 0.5, 1.))
X_b5_ef = sva.PTransformd(sva.RotX(-np.pi / 2.), e.Vector3d(0., 0.2, 0.))
# create the task
bodyTask = BodyTask(mb, mbg.bodyIdByName("b5"), X_O_T, X_b5_ef)
postureTask = PostureTask(mb, map(list, mbc.q))
comTask = CoMTask(mb, rbd.computeCoM(mb, mbc) + e.Vector3d(0., 0.5, 0.))
tasks = [(100., bodyTask), ((0., 10000., 0.), comTask), (1., postureTask)]
q_res = None
X_O_p_res = None
alphaInfList = []
for iterate, q, alpha, alphaInf in\
multiTaskIk(mb, mbc, tasks, delta=1., maxIter=200, prec=1e-8):
q_res = q
alphaInfList.append(alphaInf)
def anim():
x = 0.2
y = 0.3
z = 0.4
cb1 = sch.Box(x, y, z)
cb2 = sch.Box(x, y, z)
pair = sch.CD_Pair(cb1, cb2)
b1s = tvtk.CubeSource(x_length=x, y_length=y, z_length=z)
b2s = tvtk.CubeSource(x_length=x, y_length=y, z_length=z)
b1m = tvtk.PolyDataMapper(input=b1s.output)
b2m = tvtk.PolyDataMapper(input=b2s.output)
b1a = tvtk.Actor(mapper=b1m)
b2a = tvtk.Actor(mapper=b2m)
line = tvtk.LineSource()
lineM = tvtk.PolyDataMapper(input=line.output)
lineA = tvtk.Actor(mapper=lineM)
b1a.user_transform = tvtk.Transform()
b2a.user_transform = tvtk.Transform()
b1T = sva.PTransformd.Identity()
b2T = sva.PTransformd(Vector3d.UnitZ()*2.)
setTransform(b1a, b1T)
setTransform(b2a, b2T)
viewer = mlab.gcf()
viewer.scene.add_actors([b1a, b2a, lineA])
rx = ry = rz = 0.
t = 0
while True:
b1T = sva.PTransformd(sva.RotZ(rz)*sva.RotY(ry)*sva.RotX(rx))
b2T = sva.PTransformd(Vector3d(0., 0., 2. + np.sin(t)))
setTransform(b1a, b1T)
setTransform(b2a, b2T)
cb1.transform(b1T)
cb2.transform(b2T)
p1 = Vector3d()
p2 = Vector3d()
pair.distance(p1, p2)
line.point1 = list(p1)
line.point2 = list(p2)
rx += 0.01
ry += 0.005
rz += 0.002
t += 0.05
viewer.scene.render()
yield
import eigen3 as e
import spacevecalg as 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()
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)
a1.X = sva.PTransformd(sva.RotX(np.pi / 2.), e.Vector3d.UnitX())
def getAngVelW(self):
X_0_b = list(self.robot.mbc.bodyPosW)[self.bodyIdx]
X_Np_w = sva.PTransformd(X_0_b.rotation().transpose(),
self.X_b_s.translation())
return (X_Np_w *
(list(self.robot.mbc.bodyVelB)[self.bodyIdx])).angular()
body = makeBody(4, 'testBody')
jR = rbd.Joint(rbd.Joint.Rev, e3.Vector3d.Random().normalized(), True, 5, 'jR')
jP = rbd.Joint(rbd.Joint.Prism, e3.Vector3d.Random().normalized(),
False, 10, 'jP')
jS = rbd.Joint(rbd.Joint.Spherical, False, 100, 'jS')
jPla = rbd.Joint(rbd.Joint.Planar, True, 0, 'jPla')
jC = rbd.Joint(rbd.Joint.Cylindrical, e3.Vector3d.Random().normalized(),
False, 50, 'jC')
jFree = rbd.Joint(rbd.Joint.Free, True, 2344, 'jFree')
jFix = rbd.Joint(rbd.Joint.Fixed, False, 3998, 'jFix')
mb = rbd.MultiBody([makeBody(i, 'body%s' % i) for i in xrange(7)],
[jR, jP, jS, jPla, jC, jFree, jFix],
range(-1, 6), range(0, 7), range(-1, 6),
[sva.PTransformd(e3.Vector3d(0.,i,0.)) for i in xrange(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() and\
b1.id() == b2.id()
def jointEq(j1, j2):
return j1.type() == j2.type() and\
j1.name() == j2.name() and\