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

Trying Caliko FABRIK Inverse Kinematics in 2D

I downloaded Caliko, the FABRIK inverse kinematic solver in Java.  It has great documentation, along with guidance about how to properly use its jars as libraries in Eclipse (I don't do much Java, glad they mentioned that).  The Javadocs for Caliko is excellent, so I took one of their examples, removed the visualization and added information about the location of bones in the chain and the angles between them:

	package caliko_test;
	import au.edu.federation.caliko.*;
	import au.edu.federation.caliko.FabrikChain2D.BaseboneConstraintType2D;
	import au.edu.federation.caliko.visualisation.*;
	import au.edu.federation.utils.*;
	public class caliko_test {
	public static void main(String[] args)
	{
	// reference unit vectors
	Vec2f UP = new Vec2f(0.0f, 1.0f);
	Vec2f DOWN = new Vec2f(0.0f, -1.0f);
	Vec2f LEFT = new Vec2f(-1.0f, 0.0f);
	Vec2f RIGHT = new Vec2f(1.0f, 0.0f);
	FabrikStructure2D structure = new FabrikStructure2D();
	FabrikChain2D chain = new FabrikChain2D();
	chain.setFixedBaseMode(true);
	float constraintAngle = 90.0f;
	// Create the basebone, set constraints and add it to the chain
	FabrikBone2D base = new FabrikBone2D(new Vec2f(0.0f,0.0f), new Vec2f(0.0f,56) );
	base.setClockwiseConstraintDegs(constraintAngle);
	base.setAnticlockwiseConstraintDegs(constraintAngle);
	chain.addBone(base);
	// Fix the basebone and constrain it to the positive Y-axis
	chain.setBaseboneConstraintType(BaseboneConstraintType2D.GLOBAL_ABSOLUTE);
	chain.setBaseboneConstraintUV(UP);
	// Create and add the second bones
	chain.addConsecutiveConstrainedBone(UP, 146f, constraintAngle, constraintAngle);
	chain.addConsecutiveConstrainedBone(UP, 150f, constraintAngle, constraintAngle);
	// Finally, add the chain to the structure
	structure.addChain(chain);
	Vec2f myTarget = new Vec2f(350f, 40f);
	chain.setMaxIterationAttempts(15);
	System.out.println("MaxIteractionAttempts: "+chain.getMaxIterationAttempts());
	System.out.println("Distance to Target: " + chain.solveForTarget(myTarget));
	for(int i=0;i<chain.getNumBones();++i){
	System.out.println("Bone("+i+") DirectionUV: " + chain.getBone(i).getDirectionUV().toString());
	if(i>0){
	System.out.println("Angle of Bone("+(i-1)+") with Bone("+i+"): "+
	chain.getBone(i-1).getDirectionUV().getSignedAngleDegsTo(chain.getBone(i).getDirectionUV()));
	}
	System.out.println("Bone("+i+") EndLocation: "+ chain.getBone(i).getEndLocation().x +","+chain.getBone(i).getEndLocation().y);
	}
	System.out.println("Target: " + myTarget.toString());
	}
	}

view raw caliko_test.java hosted with ❤ by GitHub

Here is an example solution output:

	MaxIteractionAttempts: 15
	Distance to Target: 2.0320811
	Bone(0) DirectionUV: 0.949, 0.315
	Bone(0) EndLocation: 53.155647,17.620361
	Bone(1) DirectionUV: 0.992, 0.124
	Angle of Bone(0) with Bone(1): -11.213276
	Bone(1) EndLocation: 198.0278,35.73278
	Bone(2) DirectionUV: 1.000, 0.028
	Angle of Bone(1) with Bone(2): -5.5179296
	Bone(2) EndLocation: 347.96872,39.942963
	Target: 350.000, 40.000

view raw caliko_test_output.txt hosted with ❤ by GitHub

Because I don't have a good OpenGL working right now, I just took the data from the chain and plotted it in Excel.  Here are some examples - the red dot is the target:

ikpy - Inverse Inematics, yes. Limits, no!

The Python inverse kinematics library, ikpy, works fine, and it has a cool mechanism to plot chain visualization from Python using matplotlib.pyplot.  EXCEPT ikpy seems to have no concept of collision areas or joint limits.

I found this out by putting in my Lynxmotion AL5D robot arm model as URDF, and loading into ikpy.   Works fine for a target at [17,3,3] (cm):

However if I have a target at [20,3,3], it drives the arm through the table (i.e. Z=0):

If you'd like to see the URDF and IK calculation code, it is here.

So I recommend ikpy for unconstrained 3D modeling, but lacks practicality for me!

So next step, I am going to try Caliko with its FABRIK IK solver in Java.

Inverse Kinematics Take 2

The current inverse kinematics code I have for my Lynxmotion AL5D arm fails to converge towards the edges of the chess board.

One efficient way to solve IK is the FABRIK algorithm.

There is a Python inverse kinematics library, ikpy.  It needs a Unified Robot Description Format (URDF) file to describe the arm though.

Also there is a Java FABRIK:

New Affordable Bus Robotic Servos from LewanSoul

No idea as to the actual quality of these, but LewanSoul now has the LX-16A 17 kg·cm (236 oz·in) @7.4V robotic servo controlled over a bus (perhaps with the same command structure as a Dynamixel) with position/voltage/temperature feedback for only $15.99.

LewanSould also has what appears to be a fairly well-built, if not particularly long reach, 6DOF arm for $119.99 with servos (not the digital bus servos above, though).  This will definitely give the Lynxmotion Arms some serious competition.