Kinematics

Forward kinematics

Forward kinematics (FK) is the pose of the end-effector given the joint coordinates. It can be computed for robots of the DHRobot or ERobot class

T = robot.fkine(q)

where T is an SE3 instance.

T = robot.fkine_all(q)

where T is an SE3 instance with multiple values, the pose of each link frame, from the base T[0] to the end-effector T[-1].

Inverse kinematics

Inverse kinematics (IK) is the joint coordinates required to achieve a given end-effector pose. The function is not unique, and there may be no solution.

Method	MATLAB version	Type	Joint limits	Description
ikine_a	ikine6s	analytic	no	For specific DHRobots only
ikine_LM	ikine	numeric	no	Levenberg-Marquadt with global search option
ikine_LMS		numeric	no	Levenberg-Marquadt with Sugihara's tweak
ikine_min	ikunc	numeric	no	Uses SciPy.minimize, user cost function, stiffness
ikine_min(qlim=True)	ikcon	numeric	yes	Uses SciPy.minimize, user cost function, stiffness

All methods take optional method-specific arguments and return a named tuple

sol = ikine_XX(T)

The elements of the tuple sol include at least:

Element	Type	Description
q	ndarray (n)	Joint coordinates for the solution, or None
success	bool	True if a solution found
reason	str	reason for failure
iterations	int	number of iterations
residual	float	final value of cost function

Numerical solutions

These IK solvers minimise a scalar measure of error between the current and the desired end-effector pose. The measure is the squared-norm of a 6-vector comprising:

• translational error (a 3-vector)
• the orientation error as an Euler vector (angle/axis form encoded as a 3-vector)

The SciPy based mimimizers are slow because they use a scalar cost measure and must numerically compute a Jacobian in order to solve.

Performance

6DOF robot

puma = rtb.models.DH.Puma560()
T = puma.fkine(puma.qn)
sol = puma.ikine_XX(T)
print("residual = ", np.linalg.norm(T - puma.fkine(sol.q)))

Operation	Time (ms)	Error
ikine_a	0.35	2.23e-16
ikine_LM	7.8	8.79e-11
ikine_LMS	4.5	2.28e-06
ikine_min(qlim=False)	45	1.29e-07
ikine_min(qlim=True)	1.6e+03	1.56e-07

For ikine_min without joint limits we can request different optimization methods (the default is SLSQP) to be used

Solver	Time (μs)	Error
Nelder-Mead	2.9e+02	0.0988
Powell	6.5e+02	3.53e-12
CG	2.9e+02	1.27e-07
BFGS	1.2e+02	1.28e-07
L-BFGS-B	64	1e-07
TNC	1.7e+02	0.00559
SLSQP	44	1.29e-07
trust-constr	2.3e+02	1.45e-07

The methods Newton-CG, dogleg, trust-ncg, trust-exact, trust-krylov all require a Jacobian matrix to be passed.

For ikine_min with joint limits we can request different optimization methods (the default is trust-constr) to be used

Solver	Time (ms)	Error
Powell	6.5e+02	1.94e-12
L-BFGS-B	67	1.03e-07
TNC	1.7e+02	0.04
SLSQP	48	1.35e-07
trust-constr	1.6e+03	1.56e-07

Interesting attempting constrained optimisation using methods Nelder-Mead, CG, BFGS does not raise an error, they just silently ignore the constraints. Once again, the methods Newton-CG, dogleg, trust-ncg, trust-exact, trust-krylov all require a Jacobian matrix to be passed.

7DOF robot

puma = Panda()
T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
sol = puma.ikine_XX(T)
print("residual = ", np.linalg.norm(T - puma.fkine(sol.q)))

Method	Time (ms)	Residual
ikine_LM	9.1	1.5e-11
ikine_LMS	11	3.6e-6
ikine_min	75	4.5e-8
ikine_min(qlim=True)	1600	1.6e-7

Analytical solutions

Only the DH/Puma560 robot model has an analytic solution method ikine_a(T, config).

Such a method could be added to any other robot class, including any custom class that you might write.

For the class of robots that have 6 joints and a spherical wrist, the Toolbox provides additional support. First define a method in your class

def ikine_a(self, T, config):
    return self.ikine_6s(T, config, func)

where func is a local function func(robot, T, config) that solves for the first three joints, given T which is the pose of the wrist centre with respect to the base. config is a pose configuration string. For the Puma robot this is

Letter	Meaning
l	Choose the left-handed configuration
r	Choose the right-handed configuration
u	Choose the elbow up configuration
d	Choose the elbow down configuration
n	Choose the wrist not-flipped configuration
f	Choose the wrist flipped configuration

but you can choose whatever is appropriate for your robot.

Robots with multiple end effectors

This is just speculation so far...

T = robot.fkine(q, link)

where link is either a reference to a Link subclass object, or the name of the link.

q = robot.ikine_XX(T, link)

where link is either a reference to a Link subclass object, or the name of the link.

Ideally we would be able to express other constraints by passing in a callable

q = robot.ikine_XX(T, lambda q: 0.1 * q[2]**2)

which adds a cost for elbow bend, for example.