def plotktree(ax, model, fontsize = 8.0, joint_color = 'r'):
coox = sv.Xtrans([1.0, 0.0, 0.0])
cooy = sv.Xtrans([0.0, 1.0, 0.0])
cooz = sv.Xtrans([0.0, 0.0, 1.0])
rootbody = model.bodies[0]
def linear_shift(x,y,z): return (x - rootbody.CoM[0],
y - rootbody.CoM[1],
z - rootbody.CoM[2])
if rootbody.figure != None:
rootbody.figure.draw(ax, linear_shift)
vn = sv.vector3d(rootbody.Xup)
ax.plot([vn[0]],[vn[1]], [vn[2]],'gs', markersize=10)
ax.text(vn[0],vn[1],vn[2], rootbody.description(),fontsize=fontsize, rotation=0.0, color = joint_color)
for ind in range(1,len(model.parent)+1):
body = model.bodies[ind]
pbody = model.bodies[model.parent[ind - 1]]
joint = pbody.joints[body]
v = sv.vector3d(body.Xup)
vn = sv.vector3d(pbody.Xup)
# drawing joint
ax.plot([v[0],vn[0]],[v[1],vn[1]],[v[2],vn[2]], color=joint_color)
ax.text(v[0]/2.0,
v[1]/2.0,
v[2]/2.0,
joint.description(),fontsize=fontsize, rotation=0.0, color = joint_color)
ax.plot([v[0]],[v[1]], [v[2]],'gs', markersize=3)
e = sv.vector3d(coox*body.Xup)
ax.plot([v[0],e[0]],[v[1],e[1]],[v[2],e[2]], "b")
e = sv.vector3d(cooy*body.Xup)
ax.plot([v[0],e[0]],[v[1],e[1]],[v[2],e[2]], "r")
e = sv.vector3d(cooz*body.Xup)
ax.plot([v[0],e[0]],[v[1],e[1]],[v[2],e[2]], "g")
body_transform = sv.Xtrans(body.CoM)*body.Xup
vcom = sv.vector3d(body_transform)
ax.plot([v[0],vcom[0]],[v[1],vcom[1]],[v[2],vcom[2]], color='r')
v = vcom
ax.plot([v[0]],[v[1]],[v[2]],'ys', markersize=10)
ax.text(v[0],v[1],v[2], body.description(),fontsize=fontsize, rotation=0.0, color = joint_color)
e = sv.vector3d(coox*body_transform)
ax.plot([v[0],e[0]],[v[1],e[1]],[v[2],e[2]], "b")
e = sv.vector3d(cooy*body_transform)
ax.plot([v[0],e[0]],[v[1],e[1]],[v[2],e[2]], "r")
e = sv.vector3d(cooz*body_transform)
ax.plot([v[0],e[0]],[v[1],e[1]],[v[2],e[2]], "g")
# Next visualize figures
def transform(x,y,z):
v = sv.vector3d(sv.Xtrans([x + body.CoM[0],
y + body.CoM[1],
z + body.CoM[2]])*body.Xup)
return (v[0],v[1],v[2])
if body.figure != None: body.figure.draw(ax,transform)
def transform(x,y,z):
v = sv.vector3d(sv.Xtrans([x + body.CoM[0],
y + body.CoM[1],
z + body.CoM[2]])*body.Xup)
return (v[0],v[1],v[2])
def position(self):
return sv.vector3d(self.Xup) + np.matrix(self.CoM)
