Difference between revisions of "Kinematics"

Revision as of 13:41, 6 October 2018

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

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)