FABRIK Inverse Kinematics in 3D with AL5D Arm

FABRIK with Caliko works great for inverse kinematics in 3D as well!  It took me a long time to determine the proper constraint on the first bone (a pan table) to rotate only.  Not sure if I did it the "correct way", but it works anyhow.

I cannot recommend trying the Caliko visualization with GLFW unless you are a GLFW expert.  I spent a long time trying to get it to compile on Eclipse and run without any success.  Lots of combinations of OpenGL and GFLW and Java that can go wrong.   Thus I gave up and used Caliko to output the arm positions in text for input into good old Python with matplotlib.

I'll note that I am modeling my AL5D arm without the wrist & gripper connection.  This is because for my use case of playing chess, I want the wrist to always point down, and I've added some tongs to the gripper to reach between the chess pieces to grab them from above without knocking down other chess pieces.  So I am only modeling the pan base and the first two hinged "bones" and looking at a target that is 140mm above the chess board.

Here are some results of the IK calculations with FABRIK via Caliko.  Note that even in the last case where the arm cannot reach the target, it at least "reaches out towards it", which is better than the algorithm crashing or returning NaN...

Here is the Caliko code:

	package al5d_caliko_3d;
	//
	// 3D simulation of AL5D Robot arm
	// Note that my wrist/actuator is 150mm long and I am ignoring it
	//
	import au.edu.federation.caliko.*;
	import au.edu.federation.utils.*;
	public class al5d_caliko_3d {
	public static void main(String[] args)
	{
	// convenient axes unit vectors
	Vec3f XAXIS = new Vec3f(1f,0f,0f);
	Vec3f YAXIS = new Vec3f(0f,1f,0f);
	Vec3f ZAXIS = new Vec3f(0f,0f,1f);
	Vec3f myTarget;
	if(args.length==3){
	myTarget = new Vec3f(Float.parseFloat(args[0]),
	Float.parseFloat(args[1]),
	Float.parseFloat(args[2]));
	}else{
	myTarget = new Vec3f(100f,100f, 100f); // default target
	}
	// initialize structure & chain
	FabrikStructure3D structure = new FabrikStructure3D();
	FabrikChain3D chain = new FabrikChain3D();
	chain.setFixedBaseMode(true);
	// add base bone
	Vec3f start = new Vec3f(0.0f, 0.0f, 0f);
	Vec3f end = new Vec3f(0.0f, 0.0f, 56f);
	FabrikBone3D basebone = new FabrikBone3D(start, end);
	chain.addBone(basebone);
	chain.setFixedBaseMode(true);
	chain.setGlobalHingedBasebone(XAXIS, 0f, 0f, ZAXIS);
	chain.setBaseboneConstraintUV(ZAXIS);
	// add bones
	chain.addConsecutiveBone(ZAXIS, 146f);
	chain.addConsecutiveBone(ZAXIS, 185f);
	// Finally, add the chain to the structure
	structure.addChain(chain);
	// does not appear to be needed
	//chain.setMaxIterationAttempts(25);
	// solve chain
	Float distance=chain.solveForTarget(myTarget);
	for(int i=0;i<chain.getNumBones();++i){
	System.out.println("Bone("+i+"),"+chain.getBone(i).getStartLocation().x+","+
	chain.getBone(i).getStartLocation().y+","+
	chain.getBone(i).getStartLocation().z
	);
	}
	int i=chain.getNumBones()-1;
	System.out.println("Bone("+(i+1)+"),"+chain.getBone(i).getEndLocation().x+","+
	chain.getBone(i).getEndLocation().y+","+
	chain.getBone(i).getEndLocation().z);
	System.out.println("target,"+myTarget.x+","+
	myTarget.y+","+
	myTarget.z);
	System.out.println("Distance to Target: " + distance);
	System.out.println("Angles:");
	System.out.println("Pan Angle:"+Math.toDegrees(Math.atan2(chain.getBone(1).getEndLocation().y,
	chain.getBone(1).getEndLocation().x)));
	Vec3f humerusUV=chain.getBone(1).getDirectionUV();
	Vec3f ulnaUV=chain.getBone(2).getDirectionUV();
	System.out.println("Shoulder Angle:"+
	Vec3f.getAngleBetweenDegs(ZAXIS, humerusUV));
	System.out.println("Elbow Angle:"+
	Vec3f.getAngleBetweenDegs(humerusUV, ulnaUV));
	}
	}

view raw al5d_caliko_3d.java hosted with ❤ by GitHub

And here is the Python plotting code:

	#
	# usage: java -jar al5d_caliko_3d.jar | python plot_arm.py
	#
	import sys
	import matplotlib as mpl
	from mpl_toolkits.mplot3d import Axes3D
	import numpy as np
	import matplotlib.pyplot as plt
	mpl.rcParams['legend.fontsize'] = 10
	fig = plt.figure()
	ax = fig.gca(projection='3d')
	x=[]
	y=[]
	z=[]
	link,a,b,c=sys.stdin.readline().rstrip().split(',')
	x.append(float(a))
	y.append(float(b))
	z.append(float(c))
	link,a,b,c=sys.stdin.readline().rstrip().split(',')
	x.append(float(a))
	y.append(float(b))
	z.append(float(c))
	link,a,b,c=sys.stdin.readline().rstrip().split(',')
	x.append(float(a))
	y.append(float(b))
	z.append(float(c))
	link,a,b,c=sys.stdin.readline().rstrip().split(',')
	x.append(float(a))
	y.append(float(b))
	z.append(float(c))
	print x,y,z
	ax.plot(x, y, z, label='AL5D arm',marker='o',c='b')
	tx=[]
	ty=[]
	tz=[]
	link,a,b,c=sys.stdin.readline().rstrip().split(',')
	tx.append(float(a))
	ty.append(float(b))
	tz.append(float(c))
	#ax.plot(tx,ty,tz,s=100,c='r',marker='o',label='Target')
	ax.plot(tx,ty,tz,c='r',marker='o',label='Target',linestyle="none")
	ax.set_xlabel('X mm')
	ax.set_ylabel('Y mm')
	ax.set_zlabel('Z mm')
	X=np.array(x)
	Y=np.array(y)
	Z=np.array(z)
	max_range = np.array([X.max()-X.min(), Y.max()-Y.min(), Z.max()-Z.min()]).max() / 2.0
	mid_x = (X.max()+X.min()) * 0.5
	mid_y = (Y.max()+Y.min()) * 0.5
	mid_z = (Z.max()+Z.min()) * 0.5
	ax.set_xlim(mid_x - max_range, mid_x + max_range)
	ax.set_ylim(mid_y - max_range, mid_y + max_range)
	ax.set_zlim(mid_z - max_range, mid_z + max_range)
	#scatter_proxy=ax.plot([0],[0], [0], linestyle="none", c='c', marker = 'o')
	ax.legend()
	plt.legend(numpoints=1)
	plt.title('Target: '+str(tx[0])+', '+str(ty[0])+', '+str(tz[0])+' (mm)')
	plt.show()

view raw plot_arm.py hosted with ❤ by GitHub