diff --git a/cisstRobot/code/robManipulator.cpp b/cisstRobot/code/robManipulator.cpp index f33e5c9fd..f6048f799 100644 --- a/cisstRobot/code/robManipulator.cpp +++ b/cisstRobot/code/robManipulator.cpp @@ -1061,7 +1061,7 @@ robManipulator::RNE_MDH( const vctDynamicVector& 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] ); diff --git a/cisstRobot/code/robMass.cpp b/cisstRobot/code/robMass.cpp index f535dcd8d..bece1c325 100644 --- a/cisstRobot/code/robMass.cpp +++ b/cisstRobot/code/robMass.cpp @@ -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(); diff --git a/cisstRobot/tests/robManipulatorTest.cpp b/cisstRobot/tests/robManipulatorTest.cpp index 5d1964c8f..5e99dc77e 100644 --- a/cisstRobot/tests/robManipulatorTest.cpp +++ b/cisstRobot/tests/robManipulatorTest.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "robManipulatorTest.h" #include "robRobotsKinematics.h" @@ -23,6 +24,22 @@ vctDynamicVector 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; @@ -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 qd(2, 0.0); + vctDynamicVector qdd(2, 0.0); + vctFixedSizeVector 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 q(2, 0.0, 0.0); + vctDynamicVector 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(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(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(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(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 q(2, 0.0); + vctDynamicVector qdd(2, 0.0); + vctFixedSizeVector ft(0.0); + vct3 gravity(0.0, 0.0, g); + + // arm spinning in plane perpendicular to gravity + vctDynamicVector qd(2, 1.75, 0.0); + vctDynamicVector 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(2, 0.0, 1.5); + qd = vctDynamicVector(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(2, cmnPI, 1.5); + qd = vctDynamicVector(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(2, cmnPI_2, 1.5); + qd = vctDynamicVector(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(2, 0.0, 2.0); + qd = vctDynamicVector(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 q(2, 0.0, 0.0); + vctDynamicVector qdd(2, 0.0); + vctFixedSizeVector ft(0.0); + vct3 gravity(0.0, 0.0, g); + + // arm spinning in plane perpendicular to gravity, + // while the prismatic joint is still + vctDynamicVector qd(2, 2.2, 0.0); + vctDynamicVector 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(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(2, 0.0, 1.5); + qd = vctDynamicVector(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 q(1, 0.0); + vctDynamicVector qdd(1, 0.0); + vctFixedSizeVector ft(0.0); + vct3 gravity(0.0, 0.0, g); + + // // arm spinning in plane perpendicular to gravity at constant speed + vctDynamicVector qd(1, 1.0); + vctDynamicVector 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(1, 0.0); + qdd = vctDynamicVector(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 q(2, 0.0, 0.0); + vctDynamicVector qdd(2, 0.0); + vctFixedSizeVector ft(0.0); + vct3 gravity(0.0, 0.0, g); + + // arm spinning in plane perpendicular to gravity at constant speed + vctDynamicVector qd(2, 1.0, 0.0); + vctDynamicVector 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(2, 0.0, 0.0); + qdd = vctDynamicVector(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 ); diff --git a/cisstRobot/tests/robManipulatorTest.h b/cisstRobot/tests/robManipulatorTest.h index ff731d078..c13b13ce9 100644 --- a/cisstRobot/tests/robManipulatorTest.h +++ b/cisstRobot/tests/robManipulatorTest.h @@ -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 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(); };