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())
# test vector6d
from vector6d import ForceVecViz, MotionVecViz
M = sva.MotionVecd(e.Vector3d(0.2, 0.1, 0.), e.Vector3d(0.1, 0., 0.2))
F = sva.ForceVecd(e.Vector3d(-0.2, -0.1, 0.), e.Vector3d(-0.1, 0., -0.2))
MV = MotionVecViz(M, a1.X)
FV = ForceVecViz(F, sva.PTransformd(sva.RotX(np.pi/2.), e.Vector3d.UnitX()*1.4))
MV.addActors(viewer.scene)
FV.addActors(viewer.scene)
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)
a1.X = sva.PTransformd(sva.RotX(np.pi / 2.), e.Vector3d.UnitX())
# test vector6d
from vector6d import ForceVecViz, MotionVecViz
M = sva.MotionVecd(e.Vector3d(0.2, 0.1, 0.), e.Vector3d(0.1, 0., 0.2))
F = sva.ForceVecd(e.Vector3d(-0.2, -0.1, 0.), e.Vector3d(-0.1, 0., -0.2))
MV = MotionVecViz(M, a1.X)
FV = ForceVecViz(
F, sva.PTransformd(sva.RotX(np.pi / 2.),
e.Vector3d.UnitX() * 1.4))
MV.addActors(viewer.scene)
FV.addActors(viewer.scene)
