Softlimits and scara kinematics

[Demondor]

Hi, Tom.
Using your examples, I wrote a class for scara kinematics. Everything works fine, but there is one thing. I want to limit the axes by the angle of rotation (limits in pulses), the program compares in linear coordinates. Where it is necessary to make changes (to translate linear coordinates into angular) in order to compare the current position in the corners with the limits specified in the corners. The text is typed in google translate, sorry for my English.

[Demondor]

Hi, Tom.
Before editing KMotionCNC, I wrote my program, it translates motion into G code that has been broken into small sections. This is a screen of the working area of the scara manipulator with given angles. So clearly visible problem. The text is typed in google translate, sorry for my English.

[TomKerekes]

Hi Demondor,

I'm not sure I understand. Is the green area the working are? I don't see how that is derived from the links?

There is a virtual function you can add into your Kinematics class to set limits in CAD space. See the example below for our Geppetto Extruder Class where XYZ is limited in CAD space in inches and ABC is limited in degrees.

Code: Select all

int CKinematicsGeppettoExtrude::GetSoftLimits(double *xm, double *xp, double *ym, double *yp, double *zm, double *zp,
	                                   double *am, double *ap, double *bm, double *bp, double *cm, double *cp)
{
	*xm = -5;
	*xp = 5.5;
	*ym = -5;
	*yp = 5;
	*zm = -0.5;
	*zp = 5;
	*am = -25;
	*ap = 25;
	*bm = -25;
	*bp = 25;
	*cm = -25;
	*cp = 25;
	return 0;
}

Can you share your kinematics class?

[Demondor]

Sorry, Tom, I'm a little busy, I will answer at the weekend.

[Demondor]

Hi, Tom.
We write trajectories in CAD programs using such a virtual machine.
Actually the machine looks like this.

A virtual machine can get to a point with coordinates (400,100), then a real machine cannot, because the axis Y will move on the axis X and damage it.
The limitations of the virtual system are not applicable to the real machine. And it is better to use the restrictions as in KFLOP limiting the range of motion of the engine.
And yet, at the beginning of the movement there is a jerk, then the movement becomes smooth. I hope I was able to explain. Sorry, but each language has its own poorly translatable combinations of words.

My Class.

Code: Select all

#include "stdafx.h"
#include "KinematicsScara.h"

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


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

CKinematicsScara::CKinematicsScara()
{
	Length_1 = 7.874; // X axis length
	Length_2 = 7.874; // Y axis length

	SqrLength_1 = sqr(Length_1); //X*X
	SqrLength_2 = sqr(Length_2); //Y*Y

	// copied from gepetto
	m_MotionParams.MaxLinearLength = 0.05;  // limit the segment lengths for nonlinear systems
	m_MotionParams.MaxAngularChange = 0.5;  // limit the segment angle change for nonlinear systems
	m_MotionParams.MaxRapidFRO = 1.0;       // limit the increase in Rapid HW FRO
	m_MotionParams.UseOnlyLinearSegments = true;
}

CKinematicsScara::~CKinematicsScara()
{

}

int CKinematicsScara::TransformCADtoActuators(double x, double y, double z, double a, double b, double c, double *Acts, bool NoGeo)
{
	double _X = x - (Length_1 + Length_2); // shift the coordinates to match the systems
	double _Y = y;

	double distance = sqrt(sqr(_X) + sqr(_Y));  // distance from (x0,y0) to (x2,y2)
	// to simplify calculations
	double distance2 = sqr(_X) + sqr(_Y);

	//the angle between the vector from (0,0) to (x2,y2) and the axis X
	double angle = acos(CorrectAngle(_X / distance));

	// looking for angles in a triangle by the consensus theorem
	double angle1 = acos(CorrectAngle((distance2 + SqrLength_1 - SqrLength_2) / (2 * distance * Length_1))); 
	double angle3 = acos(CorrectAngle((distance2 + SqrLength_2 - SqrLength_1) / (2 * distance * Length_2)));
	double angle2 = PI - angle1 - angle3; 

	double angleMotor1 = angle + angle1; // angle of rotation of the 1st engine
	double a1 = 180 - angleMotor1 * 180 / PI; //angle of rotation of 1 engine in degrees
	double a2 = 180 - angle2 * 180 / PI; //angle of rotation of 2 engine in degrees
	double a3 = -a1 - a2 + a; //we hold the axis A parallel to the X axis

	Acts[0] = a1 * m_MotionParams.CountsPerInchX;
	Acts[1] = a2 * m_MotionParams.CountsPerInchY;
	Acts[2] = z * m_MotionParams.CountsPerInchZ;

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

	return 0;
}

int CKinematicsScara::TransformActuatorstoCAD(double *Acts, double *xr, double *yr, double *zr, double *ar, double *br, double *cr, bool NoGeo)
{
	double a1 = Acts[0] / m_MotionParams.CountsPerInchX;
	double a2 = Acts[1] / m_MotionParams.CountsPerInchY;
	double a3 = Acts[3] / m_MotionParams.CountsPerInchA;

	double angleMotor1 = PI - (a1) * PI / 180;
	double angleMotor2 = PI - (a2) * PI / 180;

	// distance from (0,0) to (x2,y2)
	double distance = sqrt(SqrLength_1 + SqrLength_2 - 2 * Length_1 * Length_2 * cos(angleMotor2));
	// to simplify calculations
	double distance2 = SqrLength_1 + SqrLength_2 - 2 * Length_1 * Length_2 * cos(angleMotor2);

	double t;
	// 1st angle of the triangle
	if (distance != 0)
		t = (SqrLength_1 + distance2 - SqrLength_2) / (2 * Length_1 * distance);
	else
		t = 0;
	double angle1 = acos(CorrectAngle(t));
	
	// angle between the vector from (0,0) to (x2,y2) and the X axis
	double angle = angleMotor1 - angle1;

	double a = a1 + a2 + a3;
	

	// find the coordinates (x2,y2) - the end point
	*xr = distance * cos(angle) + (Length_1 + Length_2);
	*yr = distance * sin(angle);
	
	*zr = Acts[2] / m_MotionParams.CountsPerInchZ;
	*ar = a;
	*br = Acts[4] / m_MotionParams.CountsPerInchB;
	*cr = Acts[5] / m_MotionParams.CountsPerInchC;

	return 0;	
}

// Check for out of range for ACos
double CKinematicsScara::CorrectAngle(double _angle)
{
	if (_angle > 1)
		return 1;
	if (_angle < -1)
		return -1;
	return _angle;
}

[TomKerekes]

Hi Demondor,

Thanks for posting the code. It would have been nice to include the .h file but I created one.

I don't fully understand. But to limit the motion of the Actuators I think you can make a call to TransformCADtoActuators() inside the GetSoftLimits() function. Then check if the Actuator positions are ok and return 0 or 1.

at the beginning of the movement there is a jerk, then the movement becomes smooth

When does the Jerk occur? Maybe the starting position is not synchronized properly to the machine's actuator positions?

[Demondor]

Thanks again Tom, for your support. The header file .h, I think it is not particularly difficult to do, so I did not spread it. The first machine is already working, while executing the trajectory of the code recalculated in the external program. Therefore, the transition to the revised version of KMotionCNC will be when they assemble the second such machine. Jerk, also check is not yet possible. I will try to correct the limits, if I can post the corrections.

[geraldft]

Hi

The .h file would be useful to see. Can you post it?

BTW - is there a way to confirm which kinematics model is being used by KmotionCNC?

Thanks

[TomKerekes]

Hi geraldft,

I've attached the files.

KMotionCNC uses the kinematics class specified in the Kinematics.txt file. See the wiki here.

[Ibzan]

Hello, Tom
I am trying to load a new kinematics, I have followed the steps that you indicate in other comments.
I have the following
1) Direct kinematics equations.
2) Equations inverse kinematics.
3) Class (.h and cpp files).use KinematicsScara as a guide.
attached documents

Hi geraldft,

I've attached the files.

KMotionCNC uses the kinematics class specified in the Kinematics.txt file. See the wiki

Once I have all the files, I click debug but I can't get the robot to work properly.
I add the screenshots of what I did to add the new class.
Could you help me please, I don't know if I'm doing it right or if I'm doing strange things.I'm new to this.