## Softlimits and scara kinematics

Moderators: TomKerekes, dynomotion

Demondor
Posts: 13
Joined: Fri Mar 22, 2019 8:49 am

### Softlimits and scara kinematics

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
Posts: 13
Joined: Fri Mar 22, 2019 8:49 am

### Re: Softlimits and scara kinematics

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
Posts: 902
Joined: Mon Dec 04, 2017 1:49 am

### Re: Softlimits and scara kinematics

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?
Regards,

Tom Kerekes
Dynomotion, Inc.

Demondor
Posts: 13
Joined: Fri Mar 22, 2019 8:49 am

### Re: Softlimits and scara kinematics

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

Demondor
Posts: 13
Joined: Fri Mar 22, 2019 8:49 am

### Re: Softlimits and scara kinematics

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 = a1 * m_MotionParams.CountsPerInchX;
Acts = a2 * m_MotionParams.CountsPerInchY;
Acts = z * m_MotionParams.CountsPerInchZ;

Acts = a3 * m_MotionParams.CountsPerInchA;
Acts = b * m_MotionParams.CountsPerInchB;
Acts = 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 / m_MotionParams.CountsPerInchX;
double a2 = Acts / m_MotionParams.CountsPerInchY;
double a3 = Acts / 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 / m_MotionParams.CountsPerInchZ;
*ar = a;
*br = Acts / m_MotionParams.CountsPerInchB;
*cr = Acts / 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
Posts: 902
Joined: Mon Dec 04, 2017 1:49 am

### Re: Softlimits and scara kinematics

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?
Regards,

Tom Kerekes
Dynomotion, Inc.

Demondor
Posts: 13
Joined: Fri Mar 22, 2019 8:49 am

### Re: Softlimits and scara kinematics

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
Posts: 3
Joined: Tue Aug 21, 2018 4:14 am

### Re: Softlimits and scara kinematics

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
Posts: 902
Joined: Mon Dec 04, 2017 1:49 am

### Re: Softlimits and scara kinematics

Hi geraldft,

I've attached the files.

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