Kinematics

Advantages of Kinematics and Rotation Tool Center Point (RTCP)

RTCP takes into consideration the Kinematics of the machine to allow angular changes to the tool while keeping the tool center point at the same location relative to the machine table. Note this is true regardless of whether the Tool angle changes or the Table angle changes.  The diagram below shows an example where the Tool angle is changed.

To perform RTCP an XYZ adjustment is applied to correct for XYZ shifts that occur with ABC angular changes.  

For Gimbal Head type systems the Tool Tip location relative to the head pivot points must be known to compute the appropriate adjustments.  This means that changes of Tool Lengths and Tool Offsets will affect the required adjustments.

For Trunnion Table table type systems the Tool Tip location relative to the Table pivot points must be known to compute the appropriate adjustments.  This means that changes to GCode Offsets (Fixture/work or Global) will shift all the Tool Tip locations relative to the Table and will affect the required adjustments.

The data flow diagram below shows the difference between having a CAD system perform the Kinematics vs having the Controller perform the Kinematics.  Having the Controller perform the Kinematics allows the same GCode to be used even though Tool Lengths, Work Offsets, or even Machine Geometries have been changed.

An additional advantage to having Kinematics performed in the Controller is that the Kinematics/RTCP is performed continuously throughout angular motions and not only on GCode Coordinates/Blocks/Lines.  So for example in the case where only the B Axis is desired to rotate:

G0 B0  (start at angle 0)

G1 B90 (rotate B 90 degrees).

With RTCP in the Controller, only one line of GCode is required to rotate B 90degrees and throughout the motion the tool tip will remain in the same place.  With RTCP in CAD, many lines of GCode would be required that include all the intermediate XYZ positions to keep the tool tip in the same place.

Overview

Kinematics are part of the Coordinated Motion Library which is used by the GCode Interpreter.  It provides a transformation from the desired CAD positions to the System's required Actuator positions to move there.

To add new Kinematics a new C++ Kinematics Class will be required to be added to the GCodeInterpreter Project.  See here for information on modifying and building the KMotion Libraries and PC Examples.

Normally only equations for inverse kinematics (determines Actuator positions from given CAD position) are required to be defined.  The CKinematics class will automatically solve forward kinematics numerically.

A text file (Kinematics.txt) in the in the <>\KMotion\Data directory contains a string that is used by the CCoordMotion class to determine what Kinematics Class is to be used.  If the text file doesn't exist standard, orthogonal, linear Kinematics are used.

After adding your new kinematics class a test for your new class should be added to CoordMotion.cpp into the code shown below.  Also the appropriate string should be placed into the Kinematics.txt file.  Additional parameters might be included in this file to be read by your Kinematics Class to make it more general.

	FILE *f = fopen((CString)MainPath + "\\Data\\Kinematics.txt","rt");

	if (f)
	{
		char s[81];
		fgets(s, 80, f);
		// one exists, check if it is calling for Geppetto otherwise assume it is the 3Rod

		if (strstr(s, "5AxisTableAC") != NULL)
			Kinematics = new CKinematics5AxisTableAC;
		else if (strstr(s, "5AxisTableBC") != NULL)
			Kinematics = new CKinematics5AxisTableBC;
		else if (strstr(s, "5AxisGimbalAB") != NULL)
			Kinematics = new CKinematics5AxisGimbalAB;
		else if (strstr(s, "5AxisGimbalCB") != NULL)
			Kinematics = new CKinematics5AxisGimbalCB;
		else if (strstr(s, "GeppettoExtruder") != NULL)
			Kinematics = new CKinematicsGeppettoExtrude;
		else if (strstr(s, "Geppetto") != NULL)
			Kinematics = new CKinematicsGeppetto;
		else
			Kinematics = new CKinematics3Rod;
		
		fclose(f);
	}

Note: In order for your new class to be defined its header file should be added into the standard header include file StdAfx.h in the GCodeInterpreter Directory as are the other Kinematic class headers as shown below:

#include "Kinematics.h"
#include "Kinematics3Rod.h"
#include "KinematicsGeppetto.h"
#include "KinematicsGeppettoExtrude.h"
#include "Kinematics5AxisGimbalAB.h"
#include "Kinematics5AxisGimbalCB.h"
#include "Kinematics5AxisTableAC.h"
#include "Kinematics5AxisTableBC.h"

Kinematics3Rod

The first non-linear Kinematics that was added to the KMotion Libraries was the simple Kinematics3Rod class shown below.  Given the XYZ CAD position it computes the necessary lengths of 3 "rod" type linear actuators to place the end effector at the desired CAD location.  Also below is a old video of it in operation.

	// Kinematics3Rod.cpp: implementation of the CKinematics3Rod class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Kinematics3Rod.h"

#define sqr(x) ((x)*(x))


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CKinematics3Rod::CKinematics3Rod()
{
	Act0Center.x = 12;
	Act0Center.y = 0;
	Act0Center.z = 0;

	Act1Center.x = 0;
	Act1Center.y = 12;
	Act1Center.z = 0;

	Act2Center.x = 11;
	Act2Center.y = 3;
	Act2Center.z = 12;

	// when actuator position is zero this is how long the rod is

	Act0Off = sqrt(sqr(Act0Center.x) + sqr(Act0Center.y) + sqr(Act0Center.z));  
	Act1Off = sqrt(sqr(Act1Center.x) + sqr(Act1Center.y) + sqr(Act1Center.z));
	Act2Off = sqrt(sqr(Act2Center.x) + sqr(Act2Center.y) + sqr(Act2Center.z));

	m_MotionParams.MaxLinearLength = 0.25;  // limit the segment lengs for nonlinear systems
	m_MotionParams.MaxRapidFRO = 1.0;       // limit the increase in Rapid HW FRO
	m_MotionParams.UseOnlyLinearSegments=true;
}

CKinematics3Rod::~CKinematics3Rod()
{

}

int CKinematics3Rod::TransformCADtoActuators(double x, double y, double z, double a, double b, double c, double *Acts, bool NoGeo)
{
	// find lengths of each actuator

	GeoCorrect(x,y,z,&x,&y,&z);

	double r0 = sqrt(sqr(x-Act0Center.x) + sqr(y-Act0Center.y) + sqr(z-Act0Center.z)) - Act0Off;
	double r1 = sqrt(sqr(x-Act1Center.x) + sqr(y-Act1Center.y) + sqr(z-Act1Center.z)) - Act1Off;
	double r2 = sqrt(sqr(x-Act2Center.x) + sqr(y-Act2Center.y) + sqr(z-Act2Center.z)) - Act2Off;

	Acts[0] = r0*m_MotionParams.CountsPerInchX;
	Acts[1] = r1*m_MotionParams.CountsPerInchY;
	Acts[2] = r2*m_MotionParams.CountsPerInchZ;

	Acts[3] = a*m_MotionParams.CountsPerInchA;
	Acts[4] = b*m_MotionParams.CountsPerInchB;
	Acts[5] = c*m_MotionParams.CountsPerInchC;

	return 0;
}


// perform Inversion to go the other way

int CKinematics3Rod::TransformActuatorstoCAD(double *Acts, double *xr, double *yr, double *zr, double *ar, double *br, double *cr, bool NoGeo)
{
	return InvertTransformCADtoActuators(Acts, xr, yr, zr, ar, br, cr);
}

Alex's system with a Table A axis and Gimbal B axis (Kinematics5AxisTableAGimbalB.cpp)

Here is the math. First the definitions:

Origin Definition

The Solution

The formula in C++ code:

// Kienematics for Table with A Axis and Tool Gimble head with B Axis 
//
// A axis always rotates about X axis
// B axis always rotates about Y axis

int CKinematics5AxisTableAGimbalB::TransformCADtoActuators(double x, double y, double z, double a, double b, double c, double *Acts, bool NoGeo)
{
	double Xt, Yt, Zt, Xa, Ya, Za, ToolXR, ToolZR, ChuckXR, ChuckZR, Dummy;


	if (m_MotionParams.TCP_Active)
	{
		//Find CAD Tool Tip Location from coordinate passed in (Xt, Yt, Zt)
		Xt = x - m_MotionParams.TCP_X;  // remove tool offset
		Yt = y - m_MotionParams.TCP_Y;
		Zt = z - m_MotionParams.TCP_Z;

		//Perform A rotation to obtain (Xa, Ya, Za)
		Rotate3(0, 0, 0, Xt, Yt, Zt, a, 0, 0, &Xa, &Ya, &Za);	

		//Determine ChuckXR ChuckZR by rotating PivotChuckLength by Angle B 
		Rotate3(0, 0, 0, 0, 0, PivotToChuckLength, 0, -b, 0, &ChuckXR, &Dummy, &ChuckZR);

		//Determine Rotated Tool offset ToolXR ToolZR by rotating ToolX ToolZ by Angle B
		Rotate3(0, 0, 0, m_MotionParams.TCP_X, m_MotionParams.TCP_Y, m_MotionParams.TCP_Z, 0, -b, 0, &ToolXR, &Dummy, &ToolZR);

		x = Xa + ToolXR + ChuckXR;
		y = Ya;
		z = Za + ToolZR + ChuckZR - PivotToChuckLength;
	}

	if (!NoGeo) GeoCorrect(x,y,z,&x,&y, &z);

	Acts[0] = x*m_MotionParams.CountsPerInchX;
	Acts[1] = y*m_MotionParams.CountsPerInchY;
	Acts[2] = z*m_MotionParams.CountsPerInchZ;
	Acts[3] = a*m_MotionParams.CountsPerInchA;
	Acts[4] = b*m_MotionParams.CountsPerInchB;
	Acts[5] = c*m_MotionParams.CountsPerInchC;

	return 0;
}

Scara Robot Kinematics by Demondor

See Forum Thread with code here.

Geometry below

Class name CKinematicsScara

2 Axis Spray Robot by Scott Little

Simulation

Forum Thread here.

Class Name CKinematics2AxisRobot

3 Axis Cable Delta Robot by Александр Украинец

Class Name CKinematics2AxisRobot

Project Details

Shared Project Files