Kinematics

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"

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);
}

==A more complex 6 axis Kinematics (CKinematicsGeppetto)==

Here is the math. First the definitions:

Origin Definition

The Solution

The 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;
}