Softlimits and scara kinematics

Moderators: TomKerekes, dynomotion

Post Reply
Demondor
Posts: 16
Joined: Fri Mar 22, 2019 8:49 am

Softlimits and scara kinematics

Post by Demondor » Tue Apr 02, 2019 11:46 am

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

Re: Softlimits and scara kinematics

Post by Demondor » Wed Apr 03, 2019 5:09 am

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.Image

User avatar
TomKerekes
Posts: 1071
Joined: Mon Dec 04, 2017 1:49 am

Re: Softlimits and scara kinematics

Post by TomKerekes » Wed Apr 03, 2019 3:36 pm

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

Re: Softlimits and scara kinematics

Post by Demondor » Fri Apr 05, 2019 6:59 am

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

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

Re: Softlimits and scara kinematics

Post by Demondor » Sun Apr 07, 2019 7:54 am

Hi, Tom.
We write trajectories in CAD programs using such a virtual machine.Image
Actually the machine looks like this.
Image
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;
}

User avatar
TomKerekes
Posts: 1071
Joined: Mon Dec 04, 2017 1:49 am

Re: Softlimits and scara kinematics

Post by TomKerekes » Sun Apr 07, 2019 6:38 pm

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

Re: Softlimits and scara kinematics

Post by Demondor » Mon Apr 08, 2019 6:30 am

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

Re: Softlimits and scara kinematics

Post by geraldft » Sun May 17, 2020 3:56 am

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

User avatar
TomKerekes
Posts: 1071
Joined: Mon Dec 04, 2017 1:49 am

Re: Softlimits and scara kinematics

Post by TomKerekes » Sun May 17, 2020 5:32 pm

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
(1.34 KiB) Downloaded 24 times
KinematicsScara.cpp
(3.74 KiB) Downloaded 26 times
Regards,

Tom Kerekes
Dynomotion, Inc.

Post Reply