User Kinematics

Introduction

This document describes implementation of user-defined robot kinematics in the SoftMC system. The supported robot models are limited to 6 axis robots having unique solution of direct kinematics problem.

In order to handle basic kinematics mappings the following terms are defined:

• World-Space: A fixed coordinate system referenced to the base of the robot. usually expressed in Cartesian coordinates for position (X, Y, Z) and another set of coordinates representing orientation, the set of orientation coordinates can differ between many available presentations (Euler angles, quaternion, rotation matrices, ...). Elements of this space we usually call Cartesian points.

• Joint-Space. A coordinate system used to describe the state of the robot in terms of its joint states. Usually a set of joint angles (angles between robot segments) for rotary joints or displacement for linear joints. In many cases they directly represent the motor positions, but in some cases a non-diagonal coupling matrix can be used for translating motor angles into joint angles/displacements. Elements of this space we usually call Joint points.

User Kinematics is a set of user-defined algorithms representing the core kinematics functionality of a robot. Basically it consist of two main functions: Inverse Kinematics (IK) and Direct Kinematics (DK). Inverse Kinematics translate Cartesian or world points into joint coordinates. Usually it contains configuration description of the robot for each given world-space point. Direct Kinematics function translate the given joint point (or joint coordinates) into world point of any chosen coordinate system. For an arbitrary robot two auxiliary functions define its kinematics, first is checking the available world-space working envelope (AF – accessibility function), and the second is the auxiliary function (CF- configuration function) defining the configuration flags of any given joint point.

In cases of simple robot kinematics or extensive analytical models inverse velocity mapping can be defined (IJ - inverse Jacobian function). It is basically the matrix product of robot inverse Jacobian matrix at the given joint coordinates with the actual world velocity vector. As this function can be very complex and not always analytically available, having it is not mandatory. It can be omitted and then the SoftMC system takes the numeric derivative instead (which is slightly less accurate, depending on these elected sample time and robots velocity).

To summarize:

• Direct Kinematics:: DK: JS → WS
• Inverse Kinematics:: IK: WS x CS → JS
• Configuration Function:: CF: JS → CS
• Accessibility Function:: AF:WS → {true, false}
• Inverse Jacobian Function ::: IJ: WS x JS→ JS

Creating a template

First of all a template element has to be created this is done using the special “model” value (5) identifier for user kinematics groups:

Common Shared <robot name> as group {axnm = <axis name>,} model = 5 of <pointtype>

This line defines a robotic group having undefined kinematic model (until it is linked to user functions) spawn on given axes and using the given point type for commands/queries. The <pointtype> is point descriptor and one of the following currently supported point types:

Point Type	Description	# coo
XY	XY table	2
XYZ	XYZ system	3
XYZR	XYZ +roll system	4
XYZRP	XYZ + Roll+Pitch system	5
XYZPR	XYZ + Pitch + Roll system	5
XYZYPR	XYZ + Yaw + Pitch + Roll system	6

User point setting

Additionally user-defined point types can be selected, there are 5 predefined user point types: USER1, USER2, USER3, ..., USER5.

If one of these is selected user need to provide conversion functions between user given Cartesian point coordinates and internal Cartesian point representation (See: Internal presentation of Cartesian points ) This will be done using the function:

int rbt_SetUsrPnt(int usr, int size, utype set, utype get)

where:

• usr is the point index (1,2,3,4,5) denoting which one of the user-defined point types is taken (USER1, ... USER5).
• size is the number of coordinates used for this point type (1 .. 7).
• set is a user-provided function for converting user-given coordinates (#{...}) into internal Cartesian point.
• get is the user provided function converting internal Cartesian point presentation into user-given coordinates (#{...})
• utype is defined as:

typedef int (*utype)(double *vector,double *car);

Where vector is a double floating point array of the user-provided (obtained) coordinates (exact copy of the list between "#{" and "}" and car is the double floating point array of internal Cartesian point representation.

Adding user code

Internal presentation of Cartesian points

In the softMC system all Cartesian points (independently of the selected model or point type) are represented by the following structure:

Component	coordinates	description
Position	X	X in mm
	Y	Y in mm
	Z	Z in mm

Orientation Quaternion
Cos (Phi/2) denoted as "Ro" also
Nx*Sin (Phi/2)
Ny*Sin(Phi/2)
Nz*Sin(Phi/2)

Note that all internal softMC orientations are represented by quaternions of the form: [Cos(phi/2), n*Sin(phi/2)] where b = (Nx,Ny,Nz) and ||n|| =1. Therefore all Cartesian points will be handled as double floating point arrays of 7 elements, for all types of robots

Internal presentation of Joint points

Joint points are internally represented as double floating point arrays. The units are radians or millimeters depending on axis type (rotary, linear). Robot Configuration Flags

Robot configuration flags will be represented as a bit filed with following order:

Flag Name Bits Arm Lefty(1), Righty(2) B0-B1 Elbow Below(1), Above(2) B2-B3 Wrist NoFlip(1), Flip(2) B4-B5

The flags will be transferred as a long integer (32 bit) value. Note that the filed names of the configuration flags are appropriate for general open-kinematics structures. However more than these three configuration flags are not supported in the system (language constrain). Also the filed names could be inappropriate for different kinematics types. If a kinematics that is implemented demands more than three configuration flags they must be implemented via user functions.

Kinematics functions

Once the kinematics equations have been written the following user functions have to be codded.

Configuration Function

int Config(double *jnt)

Where jnt is the joint coordinates array and the returned value is long integer representing the robot configuration flags for the given joint coordinates.

Inverse Kinematics Function

int InverseKinematics(int cfg, double *cpnt, double *jnt)

Where: cfg – desired robot configuration cpnt – artesian point jnt – joint point

Function returns 0 if everything is OK and nonzero number (1,2,3, ...) of axis where a problem is detected.

Direct Kinematics Function

int DirectKinematics(double *cpnt, double *jnt)

Where: cpnt – Cartesian point jnt – joint point

Function returns 0 if everything is OK and nonzero number (1,2,3, ...) of axis where a problem is detected.

Working Envelope Test Function

int Accessible(double *cpnt)

Check if the given point is in the working envelope of the robot. If yes returned 0. If the point is on the outer side of the working envelope 1 is returned. If it is too close to the zero origin point -1 is returned.

Off-Line Computation Function

int Setup()

returns 0 if all is OK. This function may be not needed, it depends on user implementation.

Inverse Jacobian Function

If Inverse Jacobian Matrix can be analytically computed it can be provided together with all user kinematics functions, but if not this function can be omitted.

int InvJacobian (double *JointPoint,double *JointVel,double *CartesianPoint,double *CartesianVel)

Linking the Kinematics Functions to the User template

Once the user kinematics functions are available they can be linked to template group (model=5) using the following function:

int rbt_SetUserKin(int rbt_id, RBT_UFUN &user);

Where rbt_id is the template group id, obtainable querying ElementId of a group. And “user” a structure (class) defined in rbtUkin.typ header file containing pointers to all user functions. Interface user structure (class):

class RBT_UFUN
{
	public:

	int (*setup)();
	int (*ikin)(int, double *, double *);
	int (*ijac)(double *, double *,double *, double *);
	int (*dkin)(double *, double *);	
	int (*acces)(double *);
	int (*cfg)(double *);
	RBT_UFUN()
	{
		ikin = 0;
		ijac = 0;
		dkin = 0;	
		acces = 0;
		cfg = 0;
		setup = 0;
	}	
};

The class should be first filled with appropriate functions and then transferred to rbt_SetUserKin function. Once the function is called there is no further relevance of this structure and it can be deleted.