def tree_example():
fig = plt.figure()
ax = plt.subplot(111)
# NOTE: shape and lengths match tree constructed below
temp = np.zeros((4,3,3))
temp[:,-1,-1] = 1.
lengths = np.array([1.5,1.,1.,1.])
def bigchart(thetas,out):
c, s = np.cos(thetas), np.sin(thetas)
out[:,0,0] = out[:,1,1] = c
out[:,0,1] = -s
out[:,1,0] = s
out[:,0,-1] = c*lengths
out[:,1,-1] = s*lengths
treeroot = ik.JointTreeNode(E=ik.RotorJoint2D(1.5),
children=[ik.JointTreeNode(E=ik.RotorJoint2D(1.),
children=[
ik.JointTreeNode(E=ik.RotorJoint2D(1.),effectors=[np.zeros(2)]),
ik.JointTreeNode(E=ik.RotorJoint2D(1.),effectors=[np.zeros(2)])
])
])
tree = BigchartJointTreeFKViz(treeroot,bigchart,temp)
solver = ik.construct_solver(tree,dampening_factors=1.,tol=1e-2,maxiter=30,errorlimits=(-2,2))
v = InteractiveIK(fig,ax,tree, np.array([np.pi/4,np.pi/4,np.pi/4,0.]),solver)
ax.set_xlim((-4,4))
ax.set_ylim((-4,4))
return v
def chain_example():
fig = plt.figure()
ax = plt.subplot(111)
treeroot = ik.JointTreeNode(E=ik.RotorJoint2D(1.5),
children=[ik.JointTreeNode(E=ik.RotorJoint2D(1.),effectors=[np.zeros(2)],
children=[ik.JointTreeNode(E=ik.RotorJoint2D(1.),effectors=[np.zeros(2)])])
])
tree = JointTreeFKViz(treeroot)
solver = ik.construct_solver(tree,dampening_factors=1.,tol=1e-2,maxiter=30,errorlimits=(-2,2))
v = InteractiveIK(fig,ax,tree,np.array([np.pi/4,np.pi/4,np.pi/4]),solver)
ax.set_xlim((-4,4))
ax.set_ylim((-4,4))
return v
