Algorithm description

### Space

The homogenous transformation matrix for rotation and prismatic motion are:

R presents the transformation of rotation motion. presents the parallel translation transformation. The 4x4 homogenous matrix is defined as which presents the position and posture of the coordinate.

Kinematics:

D-H(Denavit-Hartenberg) method is a systematic method to present the kinematics relationship of the connected link. This method uses minimum parameters to describe the kinematics characteristics of the linked robot system.

The meaning of the parameters of D-H method

is the length of or the distance between axes and axes

is the distance between and , or the distance along axes between axes and axes

is the angle between the axes of link i and axes with regard to axes , or the angle between axes and axes with regard to axes .

is the angle between axes and with regard to axes , or the angle between axes and axes with regard to axes .

If not specially signed out, all the angles in this document follow the right hand rule.

### Direct kinematics

The direct kinematics of the robot is presented by the equation below:

Where is a 4x4 matrix which describes the position and posture of the nth link with regard to the base frame.

Similarly, the functions below give out the unique relationship between the output link tail end and the links in the motion chain.

Inverse kinematics

The algorithm of inverse kinematics in this toolbox is based on an iterative calculation. This function calculate the joint angles from the given matrix. It takes a set of joint angle which is given by the users as the initial values of the algorithm. Firstly, the transformation matrix T of the given initial joint angles is derived by a direct kinematics calculation. Then, using the error between the derived T and the destination configuration matrix, a iterative calculation is taken. The algorithm will be end until the error bellows a threshold or a calculate step exceeds a certain number.

Dynamics

Direct dynamics

The direct dynamics calculation has 3 steps:

1. calculate the deflection torque

£¨ 1-1 £©

£¨ 1-2 £©

Let , the joint torque is calculated according to and by the inverse dynamics algorithm.

Calculate the inertia tensor H(q) of the robot

Calculate the joint variables

Derived b and H(q), can be obtained by (1-1). Then can be derived by integral calculus.

Inverse dynamics:

The inverse kinematics algorithm in this toolbox is based on an iterative calculation.

The computing of force and torque

# Trajectory planning

Interpolating algorithm used in trajectory planning

Constraints: