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)