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:

• ikine_6s is a method of the DHRobot class
• 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 example, the Puma robot defines this as

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.

Base and tool transforms

All robot classes support a base transform. This defines the pose of the robot's base with respect to the world frame and is by default null transform (identity matrix).

DHRobot objects support a tool transform. This defines the tip of the robot's end-effector with respect to the final link frame, which is typically inside the spherical wrist. By default this is a null transform (identity matrix). Note that some robot models describe the tool transform using DH parameters, see Section 5 of this tutorial.

Joint offsets

DHRobot support a joint offset which is important since often the required zero-angle configuration is not what the user or robot controller considers the zero-angle configuration, due to the constraints imposed by DH notation.

For forward kinematics the joint offsets are added to yield the "kinematic" joint angles, and then the forward kinematics are computed. This occurs within the A method of the DHLink class. This means that the offsets are the kinematic joints angles corresponding to the user's zero-angle configuration.

Jacobian calculation use the A method so joint offsets are taken into consideration.

Numerical inverse kinematics use the fkine method so joint offsets are taken into consideration. Analytical inverse kinematics explicitly subtract the offset in the ikine_6s method.

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.