Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Kinematics functions for the meArm robot arm
/*
RobotArmKinematics.cpp
Contains the kinematics functions for the meArm robot arm
See also: https://www.instructables.com/id/4-DOF-Mechanical-Arm-Robot-Controlled-by-Arduino
Servo trims are chosen, so that 0°, 0°, 0° (left/right, back/front, up/down) results in the neutral position of the robot arm.
Neutral means: pivot direction forward, and both arms rectangular up and forward
Neutral position: X=0
Y=(LIFT_ARM_LENGTH_MILLIMETER + CLAW_LENGTH_MILLIMETER) - Here claw length is from wrist to hand PLUS base center to shoulder
Z=HORIZONTAL_ARM_LENGTH_MILLIMETER
Copyright (C) 2019-2022 Armin Joachimsmeyer
armin.joachimsmeyer@gmail.com
This file is part of ServoEasing https://github.com/ArminJo/ServoEasing.
ServoEasing is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/gpl.html>.
*/
#include <Arduino.h> // for PI etc.
#include "RobotArmKinematics.h"
#if __has_include("RobotArmServoConfiguration.h")
#include "RobotArmServoConfiguration.h"
#else
#define HORIZONTAL_ARM_LENGTH_MILLIMETER 80
#define LIFT_ARM_LENGTH_MILLIMETER 80
#define CLAW_LENGTH_MILLIMETER 68 // Length from wrist to hand PLUS base center to shoulder
#endif
//#define LOCAL_TRACE
/*
Inverse kinematics: X,Y,Z -> servo angle
*/
/**
Get angle from a triangle using the cosine rule
All sides are given
*/
bool getAngleOfTriangle(float aOppositeSide, float aAdjacentSide1, float aAdjacentSide2, float &aComputedAngle) {
/*
Cosine rule:
C*C = A*A + B*B - 2*A*B*cos(angle_AB)
C is opposite side
A, B are adjacent sides
*/
float tDenominator = 2 * aAdjacentSide1 * aAdjacentSide2; // (2*A*B)
if (tDenominator == 0) { // @suppress("Direct float comparison")
return false;
}
float tCosinusAB = (aAdjacentSide1 * aAdjacentSide1 + aAdjacentSide2 * aAdjacentSide2 - aOppositeSide * aOppositeSide)
/ tDenominator;
if (tCosinusAB > 1 || tCosinusAB < -1) {
return false;
}
aComputedAngle = acos(tCosinusAB);
return true;
}
/*
Convert Cartesian to polar coordinates
returns 0 to PI (0 to 180 degree)
returns 0 to -PI (o to -180 degree) if aYValue < 0
*/
void cartesianToPolar(float aXValue, float aYValue, float &aRadius, float &aAngleRadiant) {
// Determine magnitude of Cartesian coordinates
aRadius = sqrt(aXValue * aXValue + aYValue * aYValue);
// Don't try to calculate zero-magnitude vectors' angles
if (aRadius == 0) { // @suppress("Direct float comparison")
aAngleRadiant = 0;
return;
}
float tNormalizedXValue = aXValue / aRadius;
// Calculate angle in 0..PI
// use cos since for negative y values it needs only inverting the sign of radiant
aAngleRadiant = acos(tNormalizedXValue);
// Convert to full range
if (aYValue < 0) {
aAngleRadiant *= -1;
}
#if defined(LOCAL_TRACE)
Serial.print(F("XValue="));
Serial.print(aXValue);
Serial.print(F(" YValue="));
Serial.print(aYValue);
Serial.print(F(" Radius="));
Serial.print(aRadius);
Serial.print(F(" AngleDegree="));
Serial.println(aAngleRadiant * RAD_TO_DEG);
#endif
}
/**
Inverse kinematics: X,Y,Z -> servo angle
Reads the Cartesian positions from aPositionStruct and fills the angles of aPositionStruct with the computed value
@param aPositionStruct structure holding input position and output angles
returns true if solving was successful, false if solving is not possible
*/
bool doInverseKinematics(struct ArmPosition *aPositionStruct) {
bool tReturnValue = true;
// Get horizontal degree for servo (0-180 degree) and get radius for next computations
float tRadiusHorizontalMillimeter, tHorizontalAngleRadiant;
/*
First doInverseKinematics the horizontal (X/Y) plane. Get LeftRightDegree and radius from X and Y.
*/
cartesianToPolar(aPositionStruct->LeftRight, aPositionStruct->BackFront, tRadiusHorizontalMillimeter, tHorizontalAngleRadiant);
aPositionStruct->LeftRightDegree = (tHorizontalAngleRadiant * RAD_TO_DEG) - 90; // tLeftRightDegree: -90 to 90 degree, -90 is right, 0 degree is neutral, 90 is left
/*
Now we have the base rotation for horizontal (X/Y) plane.
Because we cannot move claw behind the base axis, we must check if the radius in horizontal plane is smaller than the virtual claw length!
*/
if (tRadiusHorizontalMillimeter < CLAW_LENGTH_MILLIMETER) {
Serial.print(F("For "));
printPositionCartesian(aPositionStruct);
Serial.print(F(" horizontal radius "));
Serial.print(tRadiusHorizontalMillimeter);
Serial.println(F("mm is < claw length. -> Take claw length now."));
tRadiusHorizontalMillimeter = 0; // fallback
tReturnValue = false;
} else {
tRadiusHorizontalMillimeter -= CLAW_LENGTH_MILLIMETER;
}
/*
Now doInverseKinematics the vertical plane
Get vertical angle and radius/distance to claw - angle is negative for claw below horizontal zero plane
*/
float tVerticalAngleToClawRadiant, tRadiusVerticalMillimeter;
cartesianToPolar(tRadiusHorizontalMillimeter, aPositionStruct->DownUp, tRadiusVerticalMillimeter, tVerticalAngleToClawRadiant);
#if defined(LOCAL_TRACE)
Serial.print(F("RadiusHorizontal="));
Serial.print(tRadiusHorizontalMillimeter);
Serial.print(F(" VerticalAngleToClawDegee="));
Serial.print(tVerticalAngleToClawRadiant * RAD_TO_DEG);
Serial.print(F(" RadiusVertical="));
Serial.print(tRadiusVerticalMillimeter);
#endif
// Solve arm inner angles
float tAngleClawHorizontalRadiant; // angle between vertical angle to claw and horizontal arm
if (!getAngleOfTriangle(LIFT_ARM_LENGTH_MILLIMETER, HORIZONTAL_ARM_LENGTH_MILLIMETER, tRadiusVerticalMillimeter,
tAngleClawHorizontalRadiant)) {
Serial.print(F("Cannot solve angle between claw base and horizontal arm : "));
printPositionCartesianWithLinefeed(aPositionStruct);
return false;
}
#if defined(LOCAL_TRACE)
Serial.print(F(" AngleClawHorizontalDegree="));
Serial.print(tAngleClawHorizontalRadiant * RAD_TO_DEG);
#endif
float tAngleHorizontalLiftRadiant; // angle between horizontal and lift arm
if (!getAngleOfTriangle(tRadiusVerticalMillimeter, HORIZONTAL_ARM_LENGTH_MILLIMETER, LIFT_ARM_LENGTH_MILLIMETER,
tAngleHorizontalLiftRadiant)) {
Serial.print(F("Cannot solve angle between horizontal and lift arm: "));
printPositionCartesianWithLinefeed(aPositionStruct);
return false;
}
/*
Vertical plane schematic of inverse kinematic values
LIFT_ARM
__________/______________________________
| <-AngleHorizontalLift /
| /
|<-HORIZONTAL_ARM /
| /
| /
| /
| /
| /
| RadiusVertical-> /
| /
| /
| /
| /
| /
| AngleClawHorizontal
| | /
| /
|/ <-VerticalAngleToClaw
_______________________________
*/
aPositionStruct->BackFrontDegree = (HALF_PI - (tVerticalAngleToClawRadiant + tAngleClawHorizontalRadiant)) * RAD_TO_DEG;
// Now BackFrontDegree == 0 is vertical, front > 0, back < 0
#if defined(LOCAL_TRACE)
Serial.print(F(" BackFrontDegree="));
Serial.print(aPositionStruct->BackFrontDegree);
#endif
aPositionStruct->DownUpDegree = (tVerticalAngleToClawRadiant + tAngleClawHorizontalRadiant + tAngleHorizontalLiftRadiant - PI)
* RAD_TO_DEG;
// Now DownUpDegree == 0 is horizontal, up > 0, down < 0
#if defined(LOCAL_TRACE)
Serial.print(F(" DownUpDegree="));
Serial.println(aPositionStruct->DownUpDegree);
#endif
return tReturnValue;
}
/*
Forward kinematics: servo angle -> X,Y,Z
*/
void polarToCartesian(float aRadius, float aAngleRadiant, float &aXValue, float &aYValue) {
aXValue = aRadius * cos(aAngleRadiant);
aYValue = aRadius * sin(aAngleRadiant);
}
/**
Fill X,Y,Z in aPositionStruct according to angles of aPositionStruct.
always solvable :-)
*/
void doForwardKinematics(struct ArmPosition *aPositionStruct) {
float tInputAngle;
float tHeightOfHorizontalArm, tHorizontalArmHorizontalShift, tLiftHorizontal, tLiftHeight, tInputAngleRad;
// input angle: horizontal = 0 vertical = 90
tInputAngle = aPositionStruct->BackFrontDegree;
// here we have: 0 is vertical, front > 0, back < 0
tInputAngle *= -1;
// here we have: 0 is vertical, front < 0, back > 0
tInputAngleRad = (tInputAngle + 90) * DEG_TO_RAD;
polarToCartesian(HORIZONTAL_ARM_LENGTH_MILLIMETER, tInputAngleRad, tHorizontalArmHorizontalShift, tHeightOfHorizontalArm);
// input angle: horizontal = 0, up > 0, down < 0
tInputAngle = aPositionStruct->DownUpDegree;
tInputAngleRad = tInputAngle * DEG_TO_RAD;
polarToCartesian(LIFT_ARM_LENGTH_MILLIMETER, tInputAngleRad, tLiftHorizontal, tLiftHeight);
aPositionStruct->DownUp = tHeightOfHorizontalArm + tLiftHeight;
// Add horizontal length
float tHorizontalArmAndClawShift = tHorizontalArmHorizontalShift + tLiftHorizontal + CLAW_LENGTH_MILLIMETER;
// input is horizontal plane angle: forward = 0 right > 0 left < 0
tInputAngle = aPositionStruct->LeftRightDegree;
// here we have horizontal plane angle: forward = 0 left > 0 right < 0
tInputAngle *= -1;
tInputAngleRad = tInputAngle * DEG_TO_RAD;
polarToCartesian(tHorizontalArmAndClawShift, tInputAngleRad, aPositionStruct->BackFront, aPositionStruct->LeftRight);
}
/*
No trailing linefeed!
*/
void printPositionCartesian(struct ArmPosition *aPositionStruct) {
Serial.print(F("X="));
Serial.print(aPositionStruct->LeftRight);
Serial.print(F("mm, Y="));
Serial.print(aPositionStruct->BackFront);
Serial.print(F("mm, Z="));
Serial.print(aPositionStruct->DownUp);
Serial.print(F("mm"));
}
void printPositionCartesianWithLinefeed(struct ArmPosition *aPositionStruct) {
printPositionCartesian(aPositionStruct);
Serial.println();
}
void printPosition(struct ArmPosition *aPositionStruct) {
Serial.print(F("LeftRight="));
Serial.print(aPositionStruct->LeftRight);
Serial.print("|");
Serial.print(aPositionStruct->LeftRightDegree);
Serial.print(F(" BackFront="));
Serial.print(aPositionStruct->BackFront);
Serial.print("|");
Serial.print(aPositionStruct->BackFrontDegree);
Serial.print(F(" DownUp="));
Serial.print(aPositionStruct->DownUp);
Serial.print("|");
Serial.println(aPositionStruct->DownUpDegree);
}
void printPositionShort(struct ArmPosition *aPositionStruct) {
Serial.print(aPositionStruct->LeftRight);
Serial.print(" ");
Serial.print(aPositionStruct->BackFront);
Serial.print(" ");
Serial.print(aPositionStruct->DownUp);
Serial.print(F(" <-> "));
Serial.print(aPositionStruct->LeftRightDegree);
Serial.print(" ");
Serial.print(aPositionStruct->BackFrontDegree);
Serial.print(" ");
Serial.println(aPositionStruct->DownUpDegree);
}
void printPositionShortWithUnits(struct ArmPosition *aPositionStruct) {
printPositionCartesian(aPositionStruct);
Serial.print(F(" <-> "));
Serial.print(aPositionStruct->LeftRightDegree);
Serial.print(F(", "));
Serial.print(aPositionStruct->BackFrontDegree);
Serial.print(F(", "));
Serial.print(aPositionStruct->DownUpDegree);
Serial.println(F(" degree"));
}
#if defined(LOCAL_TRACE)
#undef LOCAL_TRACE
#endif
void setup() {
pinMode(LED_BUILTIN, OUTPUT);
Serial.begin(115200);
// Just to know which program is running on my Arduino
Serial.println(F("START " __FILE__ " from " __DATE__));
struct ArmPosition sEndPosition; // Target position
sEndPosition.LeftRightDegree = 12;
sEndPosition.BackFrontDegree = 42;
sEndPosition.DownUpDegree = 100;
doForwardKinematics(&sEndPosition);
printPositionShortWithUnits(&sEndPosition);
doInverseKinematics(&sEndPosition);
printPositionShortWithUnits(&sEndPosition);
}
void loop() {
// put your main code here, to run repeatedly:
delay(1000);
}
/*
* RobotArmKinematics.h
*
* Copyright (C) 2022 Armin Joachimsmeyer
* armin.joachimsmeyer@gmail.com
*
* This file is part of ServoEasing https://github.com/ArminJo/ServoEasing.
*
* ServoEasing is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/gpl.html>.
*
*/
#ifndef ROBOT_ARM_KINEMATICS_H_
#define ROBOT_ARM_KINEMATICS_H_
#include <stdint.h>
/*
* First part of the name describes negative values. LeftRight value negative means left side.
*/
struct ArmPosition {
float LeftRight;
float BackFront;
float DownUp;
int LeftRightDegree; // horizontal plane angle: forward = 0 left > 0 right < 0
int BackFrontDegree; // 0 is vertical, front > 0, back < 0
int DownUpDegree; // 0 is horizontal, up > 0, down < 0
};
/*
* Inverse kinematics: X,Y,Z -> servo angle
*/
void cartesianToPolar(float a, float b, float& r, float& theta);
bool getAngleOfTriangle(float opp, float adj1, float adj2, float& theta);
bool doInverseKinematics(struct ArmPosition * aPositionStruct);
/*
* Forward kinematics: servo angle -> X,Y,Z
*/
void polarToCartesian(float r, float theta, float& a, float& b);
void doForwardKinematics(struct ArmPosition * aPositionStruct);
/*
* Helper functions
*/
void printPosition(struct ArmPosition * aPositionStruct);
void printPositionCartesianWithLinefeed(struct ArmPosition * aPositionStruct);
void printPositionCartesian(struct ArmPosition * aPositionStruct);
void printPositionShort(struct ArmPosition * aPositionStruct);
void printPositionShortWithUnits(struct ArmPosition * aPositionStruct);
#endif // ROBOT_ARM_KINEMATICS_H_
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment