Skip to content

Commit

Permalink
Merge pull request #106 from brendanburkhart/bug-manipulator-dynamics
Browse files Browse the repository at this point in the history 
Bug fix for robManipulator inverse dynamics
  • Loading branch information
@adeguet1
adeguet1 authored Jan 29, 2025
2 parents 237faf0 + 38be352 commit 6a01f7f
Show file tree
Hide file tree
Showing 4 changed files with 274 additions and 6 deletions.
2 changes: 1 addition & 1 deletion cisstRobot/code/robManipulator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1061,7 +1061,7 @@ robManipulator::RNE_MDH( const vctDynamicVector<double>& q,
const robMass & massData = links[i].MassData();
m = massData.Mass();
s = massData.CenterOfMass();
I = massData.MomentOfInertia();
I = massData.MomentOfInertiaAtCOM();

if( i==0 ){
A = R*links[i].Orientation( q[i] );
Expand Down
6 changes: 3 additions & 3 deletions cisstRobot/code/robMass.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,9 +105,9 @@ robMass::Errno robMass::ReadMass(const Json::Value &config)
com[0] = config.get("cx", 0.0).asDouble();
com[1] = config.get("cy", 0.0).asDouble();
com[2] = config.get("cz", 0.0).asDouble();
D[0][0] = config.get("Ixx", 0.0001).asDouble();
D[1][1] = config.get("Iyy", 0.0001).asDouble();
D[2][2] = config.get("Izz", 0.0001).asDouble();
D[0][0] = config.get("Ixx", 0.0).asDouble();
D[1][1] = config.get("Iyy", 0.0).asDouble();
D[2][2] = config.get("Izz", 0.0).asDouble();
x1 = config.get("x1", 1.0).asDouble();
x2 = config.get("x2", 0.0).asDouble();
x3 = config.get("x3", 0.0).asDouble();
Expand Down
246 changes: 246 additions & 0 deletions cisstRobot/tests/robManipulatorTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include <cisstCommon/cmnPath.h>
#include <cisstRobot/robManipulator.h>
#include <cisstRobot/robModifiedDH.h>
#include "robManipulatorTest.h"

#include "robRobotsKinematics.h"
Expand All @@ -23,6 +24,22 @@ vctDynamicVector<double> robManipulatorTest::RandomWAMVector() const {
return q;
}

double robManipulatorTest::EulerForce(double mass, double angular_acceleration, double rotation_distance) {
// Euler force = -m * angular_acceleration x rotation_distance
return -mass * angular_acceleration * rotation_distance;
}

double robManipulatorTest::CoriolisForce(double mass, double angular_velocity, double radial_velocity) {
// Coriolis force = -2*m*(angular_velocity x radial_velocity)
return -2.0 * mass * angular_velocity * radial_velocity;
}

double robManipulatorTest::CentrifugalForce(double mass, double angular_velocity, double rotation_distance) {
// Centrifugal force is -m*w x (w x r). Assuming w and r are orthogonal, w x w x r = -||w||^2 * r,
// so scalar force is +m*w*w*r
return mass * angular_velocity * angular_velocity * rotation_distance;
}

void robManipulatorTest::TestForwardKinematics(){

cmnPath path;
Expand Down Expand Up @@ -79,4 +96,233 @@ void robManipulatorTest::TestInverseDynamics(){

}

robManipulator RPTestManipulator() {
robManipulator RP;

robJoint R_joint = robJoint(CMN_JOINT_REVOLUTE, robJoint::ACTIVE, 0.0, -cmnPI, cmnPI, 0.0);
robKinematics* R_kinematics = new robModifiedDH(0.0, 0.0, 0.0, 0.0, R_joint);
robMass R_mass = robMass(1.5, vct3(0.0, 0.0, 0.0), vct3x3(0.0), vct3x3::Eye());

robJoint P_joint = robJoint(CMN_JOINT_PRISMATIC, robJoint::ACTIVE, 0.0, -10.0, 10.0, 0.0);
robKinematics* P_kinematics = new robModifiedDH(-cmnPI_2, 0.0, 0.0, 1.5, P_joint);
robMass P_mass = robMass(2.0, vct3(0.0, 0.0, 0.0), vct3x3(0.0), vct3x3::Eye());

RP.links.push_back(robLink(R_kinematics, R_mass));
RP.links.push_back(robLink(P_kinematics, P_mass));

return RP;
}

robManipulator RTestManipulator() {
robManipulator R;

robJoint joint = robJoint(CMN_JOINT_REVOLUTE, robJoint::ACTIVE, 0.0, -cmnPI, cmnPI, 0.0);
robKinematics* kinematics = new robModifiedDH(0.0, 0.0, 0.0, 0.0, joint);
robMass mass = robMass(1.5, vct3(3.0, 0.0, 0.0), vct3x3(0.0), vct3x3::Eye());

R.links.push_back(robLink(kinematics, mass));

return R;
}

robManipulator RRTestManipulator() {
robManipulator RR;

robJoint joint_1 = robJoint(CMN_JOINT_REVOLUTE, robJoint::ACTIVE, 0.0, -cmnPI, cmnPI, 0.0);
robKinematics* kinematics_1 = new robModifiedDH(0.0, 0.0, 0.0, 0.0, joint_1);
robMass mass_1 = robMass(0.0, vct3(0.0, 0.0, 0.0), vct3x3(0.0), vct3x3::Eye());

robJoint joint_2 = robJoint(CMN_JOINT_REVOLUTE, robJoint::ACTIVE, 0.0, -cmnPI, cmnPI, 0.0);
robKinematics* kinematics_2 = new robModifiedDH(0.0, 2.5, 0.0, 0.0, joint_2);
robMass mass_2 = robMass(2.5, vct3(0.5, 0.0, 0.0), vct3x3(0.0), vct3x3::Eye());

RR.links.push_back(robLink(kinematics_1, mass_1));
RR.links.push_back(robLink(kinematics_2, mass_2));

return RR;
}

void robManipulatorTest::TestInverseDynamicsGravity(){
robManipulator RP = RPTestManipulator();

const double g = 9.81;
const double mass = 2.0;
const double length = 1.5;
const double epsilon = 1e-10; // numerical tolerance

vctDynamicVector<double> qd(2, 0.0);
vctDynamicVector<double> qdd(2, 0.0);
vctFixedSizeVector<double, 6> ft(0.0);
vct3 gravity(0.0, g, 0.0); // point gravity along -y axis

// arm vertical (relative to gravity), force along prismatic but no torque
vctDynamicVector<double> q(2, 0.0, 0.0);
vctDynamicVector<double> tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(mass * g, tau[1], epsilon);

// arm vertical and extended, no change
q = vctDynamicVector<double>(2, 0.0, 1.0);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(mass * g, tau[1], epsilon);

// arm horizontal (relative to gravity), torque but no force
q = vctDynamicVector<double>(2, cmnPI_2, 0.0);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(-length * mass * g, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[1], epsilon);

// arm horizontal and extended, increased torque
q = vctDynamicVector<double>(2, cmnPI_2, 1.0);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(-(length + 1) * mass * g, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[1], epsilon);

// gravity perpendicular to both joints, no efforts
gravity = vct3(0.0, 0.0, 9.81);
q = vctDynamicVector<double>(2, cmnPI_4, 1.0);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[1], epsilon);
}

void robManipulatorTest::TestInverseDynamicsCentrifugal(){
robManipulator RP = RPTestManipulator();

const double g = 9.81;
const double mass = 2.0;
const double length = 1.5;
const double epsilon = 1e-10; // numerical tolerance

vctDynamicVector<double> q(2, 0.0);
vctDynamicVector<double> qdd(2, 0.0);
vctFixedSizeVector<double, 6> ft(0.0);
vct3 gravity(0.0, 0.0, g);

// arm spinning in plane perpendicular to gravity
vctDynamicVector<double> qd(2, 1.75, 0.0);
vctDynamicVector<double> tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CentrifugalForce(mass, 1.75, length), tau[1], epsilon);

// same but arm is extended
q = vctDynamicVector<double>(2, 0.0, 1.5);
qd = vctDynamicVector<double>(2, 1.75, 0.0);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CentrifugalForce(mass, 1.75, length + 1.5), tau[1], epsilon);

// arm is spinning in same plane as gravity, pointing down
gravity = vct3(0.0, g, 0.0);
q = vctDynamicVector<double>(2, cmnPI, 1.5);
qd = vctDynamicVector<double>(2, 2.0, 0.0);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CentrifugalForce(mass, 2.0, length + 1.5) - mass * g, tau[1], epsilon);

// arm is spinning in same plane as gravity, pointing sidewise
q = vctDynamicVector<double>(2, cmnPI_2, 1.5);
qd = vctDynamicVector<double>(2, 1.25, 0.0);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(-(length + 1.5) * mass * g, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CentrifugalForce(mass, 1.25, length + 1.5), tau[1], epsilon);

// arm is spinning in same plane as gravity, pointing up
q = vctDynamicVector<double>(2, 0.0, 2.0);
qd = vctDynamicVector<double>(2, 1.2, 0.0);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CentrifugalForce(mass, 1.2, length + 2.0) + mass * g, tau[1], epsilon);
}

void robManipulatorTest::TestInverseDynamicsCoriolis(){
robManipulator RP = RPTestManipulator();

const double g = 9.81;
const double mass = 2.0;
const double length = 1.5;
const double epsilon = 1e-10; // numerical tolerance

vctDynamicVector<double> q(2, 0.0, 0.0);
vctDynamicVector<double> qdd(2, 0.0);
vctFixedSizeVector<double, 6> ft(0.0);
vct3 gravity(0.0, 0.0, g);

// arm spinning in plane perpendicular to gravity,
// while the prismatic joint is still
vctDynamicVector<double> qd(2, 2.2, 0.0);
vctDynamicVector<double> tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CentrifugalForce(mass, 2.2, length), tau[1], epsilon);

// arm spinning in plane perpendicular to gravity,
// while the prismatic joint is retracting slowly
qd = vctDynamicVector<double>(2, 1.6, -0.5);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CoriolisForce(mass, 1.6, -0.5) * length, tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CentrifugalForce(mass, 1.6, length), tau[1], epsilon);

// arm spinning in plane perpendicular to gravity,
// while the prismatic joint is extended + extending quickly
q = vctDynamicVector<double>(2, 0.0, 1.5);
qd = vctDynamicVector<double>(2, -1.0, 2.0);
tau = RP.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CoriolisForce(mass, -1.0, 2.0) * (length + 1.5), tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-CentrifugalForce(mass, -1.0, length + 1.5), tau[1], epsilon);
}

void robManipulatorTest::TestInverseDynamicsSimpleEuler(){
robManipulator R = RTestManipulator();

const double g = 9.81;
const double mass = 1.5;
const double length = 3.0;
const double epsilon = 1e-10; // numerical tolerance

vctDynamicVector<double> q(1, 0.0);
vctDynamicVector<double> qdd(1, 0.0);
vctFixedSizeVector<double, 6> ft(0.0);
vct3 gravity(0.0, 0.0, g);

// // arm spinning in plane perpendicular to gravity at constant speed
vctDynamicVector<double> qd(1, 1.0);
vctDynamicVector<double> tau = R.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(-EulerForce(mass, 0.0, length) * length, tau[0], epsilon);

// arm (not) spinning in plane perpendicular to gravity, speeding up
qd = vctDynamicVector<double>(1, 0.0);
qdd = vctDynamicVector<double>(1, 0.5);
tau = R.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(-EulerForce(mass, 0.5, length) * length, tau[0], epsilon);
}

void robManipulatorTest::TestInverseDynamicsEuler(){
robManipulator RR = RRTestManipulator();

const double g = 9.81;
const double mass = 2.5;
const double link_length = 2.5;
const double com_offset = 0.5;
const double epsilon = 1e-10; // numerical tolerance

vctDynamicVector<double> q(2, 0.0, 0.0);
vctDynamicVector<double> qdd(2, 0.0);
vctFixedSizeVector<double, 6> ft(0.0);
vct3 gravity(0.0, 0.0, g);

// arm spinning in plane perpendicular to gravity at constant speed
vctDynamicVector<double> qd(2, 1.0, 0.0);
vctDynamicVector<double> tau = RR.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(-EulerForce(mass, 0.0, link_length + com_offset) * (link_length + com_offset), tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-EulerForce(mass, 0.0, link_length + com_offset) * com_offset, tau[1], epsilon);

// arm (not) spinning in plane perpendicular to gravity, speeding up
qd = vctDynamicVector<double>(2, 0.0, 0.0);
qdd = vctDynamicVector<double>(2, 0.75, 0.0);
tau = RR.RNE_MDH( q, qd, qdd, ft, gravity );
CPPUNIT_ASSERT_DOUBLES_EQUAL(-EulerForce(mass, 0.75, link_length + com_offset) * (link_length + com_offset), tau[0], epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-EulerForce(mass, 0.75, link_length + com_offset) * com_offset, tau[1], epsilon);
}

CPPUNIT_TEST_SUITE_REGISTRATION( robManipulatorTest );
26 changes: 24 additions & 2 deletions cisstRobot/tests/robManipulatorTest.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,18 +13,40 @@ class robManipulatorTest : public CppUnit::TestFixture {
CPPUNIT_TEST(TestForwardKinematics);
CPPUNIT_TEST(TestInverseKinematics);

CPPUNIT_TEST(TestInverseDynamicsGravity);
CPPUNIT_TEST(TestInverseDynamicsCentrifugal);
CPPUNIT_TEST(TestInverseDynamicsCoriolis);
CPPUNIT_TEST(TestInverseDynamicsSimpleEuler);
CPPUNIT_TEST(TestInverseDynamicsEuler);

//CPPUNIT_TEST(TestInverseDynamics);

CPPUNIT_TEST_SUITE_END();

vctDynamicVector<double> RandomWAMVector() const;

public:

// Compute Euler force, given mass, angular acceleration, the distance from the com
// to the center of rotation, and from com to the joint axis
static double EulerForce(double mass, double angular_acceleration, double rotation_distance);

// Compute Coriolis force, given mass, angular velocity,
// radial velocity of mass, and distance from com to the joint axis
static double CoriolisForce(double mass, double angular_velocity, double radial_velocity);

// Compute centrifugal force, given mass, angular velocity,
// and the distance from center of mass to the center of rotation
static double CentrifugalForce(double mass, double angular_velocity, double rotation_distance);

public:
void TestForwardKinematics();
void TestInverseKinematics();

void TestInverseDynamics();

void TestInverseDynamicsGravity();
void TestInverseDynamicsCentrifugal();
void TestInverseDynamicsCoriolis();
void TestInverseDynamicsSimpleEuler();
void TestInverseDynamicsEuler();
};

0 comments on commit 6a01f7f

Please sign in to comment.