From 696e144f11caad8f12092f2f0fede06989be4f73 Mon Sep 17 00:00:00 2001 From: Antoine Hoarau Date: Sat, 7 Apr 2018 22:27:19 +0200 Subject: [PATCH 01/79] clamp the value sent to acos to be in bounds due to rounding, it can happen that abs( 1 - (dot(ab, bc) / abdist / bcdist) ) < eps --- orocos_kdl/src/path_roundedcomposite.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/src/path_roundedcomposite.cpp b/orocos_kdl/src/path_roundedcomposite.cpp index b79e1f9fd..dbc67b05f 100644 --- a/orocos_kdl/src/path_roundedcomposite.cpp +++ b/orocos_kdl/src/path_roundedcomposite.cpp @@ -85,7 +85,8 @@ void Path_RoundedComposite::Add(const Frame& F_base_point) { if (bcdist < eps) { throw Error_MotionPlanning_Not_Feasible(3); } - double alpha = acos(dot(ab, bc) / abdist / bcdist); + // Clamp to avoid rounding errors (acos is defineed between [-1 ; 1]) + double alpha = acos(std::max(-1., std::min(dot(ab, bc) / abdist / bcdist, 1.))); if ((PI - alpha) < eps) { throw Error_MotionPlanning_Not_Feasible(4); } From 26e270c85628dd565eecf9b65010d4e7e4770a59 Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Tue, 10 Apr 2018 11:49:21 -0400 Subject: [PATCH 02/79] KDL frames: removed unnecessary epsilon checks in GetRotAngle singular case MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The set of conditionals where xx, yy, and zz are checked for being < epsilon seems to be searching for the case where the matrix is the negative identity matrix. For example, if xx > yy and xx > zz and xx < epsilon, then the diagonal elements data[0], data[4], and data[8] are all approximately equal -1. Yet this will return an angle-axis of (0, 1/sqrt2, 1/sqrt2) which corresponds to a rotation matrix of [-1 0 0; 0 0 1; 0 1 0] which is not the negative identity matrix. The same is true for yy or zz being the largest value but still ~0. But the negative identity matrix is an improper rotation matrix so the checks should not be there. This code was drawn from the java code at http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm. In the final entry under “Correspondence on this page”, Gene Gorokhovsky basically gives protocode for the 180 deg singularity case, which does not include the ‘ < epsilon ‘ checks. So it seems this was added afterwards as a way to prevent a divide by zero for the case of the negative identity matrix which is not a legal input. --- orocos_kdl/src/frames.cpp | 45 ++++++++------------------------------- 1 file changed, 9 insertions(+), 36 deletions(-) diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index cb8cd0531..95fa1c9c5 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -397,50 +397,23 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { if ((xx > yy) && (xx > zz)) { // data[0] is the largest diagonal term - if (xx < epsilon) - { - x = 0; - y = half_sqrt_2; - z = half_sqrt_2; - } - else - { - x = sqrt(xx); - y = xy/x; - z = xz/x; - } + x = sqrt(xx); + y = xy/x; + z = xz/x; } else if (yy > zz) { // data[4] is the largest diagonal term - if (yy < epsilon) - { - x = half_sqrt_2; - y = 0; - z = half_sqrt_2; - } - else - { - y = sqrt(yy); - x = xy/y; - z = yz/y; - } + y = sqrt(yy); + x = xy/y; + z = yz/y; } else { // data[8] is the largest diagonal term so base result on this - if (zz < epsilon) - { - x = half_sqrt_2; - y = half_sqrt_2; - z = 0; - } - else - { - z = sqrt(zz); - x = xz/z; - y = yz/z; - } + z = sqrt(zz); + x = xz/z; + y = yz/z; } axis = KDL::Vector(x, y, z); return angle; // return 180 deg rotation From affad33e43360381570403d01bdb12a814da9e36 Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Wed, 28 Mar 2018 11:37:34 -0400 Subject: [PATCH 03/79] KDL Frames: Added Improper Rotation test for GetRotAngle KDL assumes that rotation matrices are proper, i.e. orthonormal with an eigenvalue of +1. Improper rotation matrices with an eigenvalue of -1, such as the negative identity matrix, involve a mirroring of axes such as going from a right-hand to left-hand coordinate system or vice versa. However, rotation matrices are always proper if operating within a right hand or left hand system. This means also that GetRotAngle will not return the correct angle-axis if the rotation matrix is improper. Two test cases were added to verify this, one for a nonsingular case and the other for a singular case (180 deg rotation) to demonstrate this. --- orocos_kdl/tests/framestest.cpp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index 51ef5511e..bb8c0efb0 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -438,6 +438,30 @@ void FramesTest::TestGetRotAngle() { double angle = KDL::Rotation(-1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, -1 - 1e-6).GetRotAngle(axis); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", M_PI, angle, epsilon); } + + // Tests to show that GetRotAngle does not work for an improper rotation matrix which has a determinant of -1; + // an improper rotation matrix corresponds to a rotation between a right-hand and left-hand coordinate system + { + Vector axis; + double angle; + Rotation R, Rout; + double det; + // Improper Rotation Matrix for 120 deg rotation + R = KDL::Rotation( 0, -1, 0, 0, 0, -1, -1, 0, 0); + det = +R(0,0)*(R(1,1)*R(2,2)-R(2,1)*R(1,2))-R(0,1)*(R(1,0)*R(2,2)-R(2,0)*R(1,2))+R(0,2)*(R(1,0)*R(2,1)-R(2,0)*R(1,1)); + CPPUNIT_ASSERT_EQUAL(det,-1.0); + angle = R.GetRotAngle(axis); + Rout = KDL::Rotation::Rot(axis, angle); + CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout)); + // Improper Rotation matrix for 180 deg rotation (singular) + R = KDL::Rotation( -1, 0, 0, 0, -1, 0, 0, 0, -1); + det = +R(0,0)*(R(1,1)*R(2,2)-R(2,1)*R(1,2))-R(0,1)*(R(1,0)*R(2,2)-R(2,0)*R(1,2))+R(0,2)*(R(1,0)*R(2,1)-R(2,0)*R(1,1)); + CPPUNIT_ASSERT_EQUAL(det,-1.0); + angle = R.GetRotAngle(axis); + Rout = KDL::Rotation::Rot(axis, angle); + CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout)); + } + } void FramesTest::TestQuaternion() { From 05a1d0330da2e76f17f1a70a52b3085e53be3dfd Mon Sep 17 00:00:00 2001 From: "luz.paz" Date: Wed, 25 Apr 2018 15:36:46 -0400 Subject: [PATCH 04/79] Misc. trivial typos Found via `codespell` --- orocos_kdl/examples/geometry.cpp | 2 +- orocos_kdl/src/chainjnttojacsolver.cpp | 2 +- orocos_kdl/src/path_composite.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/examples/geometry.cpp b/orocos_kdl/examples/geometry.cpp index b90599f4d..a4696d51a 100644 --- a/orocos_kdl/examples/geometry.cpp +++ b/orocos_kdl/examples/geometry.cpp @@ -81,7 +81,7 @@ int main() KDL::Vector(0,-1,0), KDL::Vector(-1,0,0)); //Creating a rotation matrix out of 9 values, Be careful, these - //values can result in an inconsisten rotation matrix if the + //values can result in an inconsistent rotation matrix if the //resulting rows/columns are not orthogonal/normalized KDL::Rotation r3(0,0,-1,1,0,0,0,-1,0); //Creating an Identity rotation matrix diff --git a/orocos_kdl/src/chainjnttojacsolver.cpp b/orocos_kdl/src/chainjnttojacsolver.cpp index 098559c0b..54dd8915d 100644 --- a/orocos_kdl/src/chainjnttojacsolver.cpp +++ b/orocos_kdl/src/chainjnttojacsolver.cpp @@ -55,7 +55,7 @@ namespace KDL else segmentNr = seg_nr; - //Initialize Jacobian to zero since only segmentNr colunns are computed + //Initialize Jacobian to zero since only segmentNr columns are computed SetToZero(jac) ; if( q_in.rows()!=chain.getNrOfJoints() || jac.columns() != chain.getNrOfJoints()) diff --git a/orocos_kdl/src/path_composite.cpp b/orocos_kdl/src/path_composite.cpp index 62e099d51..4df5f3929 100644 --- a/orocos_kdl/src/path_composite.cpp +++ b/orocos_kdl/src/path_composite.cpp @@ -50,7 +50,7 @@ namespace KDL { // simple linear search : TODO : make it binary search // uses cached_... variables // returns the relative path length within the segment -// you propably want to use the cached_index variable +// you probably want to use the cached_index variable double Path_Composite::Lookup(double s) const { assert(s>=-1e-12); From b9e3ede8028aa69fca2bcf45133dcba1e14550c8 Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Sat, 28 Apr 2018 12:59:45 -0700 Subject: [PATCH 05/79] Minor: replace tab with spaces in chainiksolvervel_wdls --- orocos_kdl/src/chainiksolvervel_wdls.cpp | 38 ++++++++++++------------ 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/orocos_kdl/src/chainiksolvervel_wdls.cpp b/orocos_kdl/src/chainiksolvervel_wdls.cpp index 33ae9ff13..f46eb1c12 100644 --- a/orocos_kdl/src/chainiksolvervel_wdls.cpp +++ b/orocos_kdl/src/chainiksolvervel_wdls.cpp @@ -122,10 +122,10 @@ namespace KDL return (error = E_SIZE_MISMATCH); error = jnt2jac.JntToJac(q_in,jac); if ( error < E_NOERROR) return error; - + double sum; unsigned int i,j; - + // Initialize (internal) return values nrZeroSigmas = 0 ; sigmaMin = 0.; @@ -137,11 +137,11 @@ namespace KDL tmp_jac(i,j) = jac(i,j); } */ - + // Create the Weighted jacobian tmp_jac_weight1 = jac.data.lazyProduct(weight_js); tmp_jac_weight2 = weight_ts.lazyProduct(tmp_jac_weight1); - + // Compute the SVD of the weighted jacobian svdResult = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter); if (0 != svdResult) @@ -176,22 +176,22 @@ namespace KDL // Note: singular values are always positive so sigmaMin >=0 if ( sigmaMin < eps ) { - lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ; + lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ; + } + if(fabs(S(i))=6 due to cols>rows + } + // Count number of singular values near zero + ++nrZeroSigmas ; } - if(fabs(S(i))=6 due to cols>rows - } - // Count number of singular values near zero - ++nrZeroSigmas ; - } - else { + else { tmp(i) = sum/S(i); - } + } } /* @@ -208,7 +208,7 @@ namespace KDL // If number of near zero singular values is greater than the full rank // of jac, then wdls is active - if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) { + if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) { return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular } else { return (error = E_NOERROR); // have converged From ee996a4442339010571cadd976fc6ed17c98dfab Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Mon, 14 May 2018 11:54:04 -0700 Subject: [PATCH 06/79] Fixed python3 std string conversion issue --- python_orocos_kdl/PyKDL/std_string.sip | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/python_orocos_kdl/PyKDL/std_string.sip b/python_orocos_kdl/PyKDL/std_string.sip index e31324add..a399c9b2e 100644 --- a/python_orocos_kdl/PyKDL/std_string.sip +++ b/python_orocos_kdl/PyKDL/std_string.sip @@ -47,17 +47,16 @@ *sipCppPtr = new std::string; return 1; } - if (PyUnicode_Check(sipPy)) { - PyObject* s = PyUnicode_AsEncodedString(sipPy, "UTF-8", ""); - *sipCppPtr = new std::string(PyUnicode_AS_DATA(s)); - Py_DECREF(s); - return 1; - } #if PY_MAJOR_VERSION < 3 if (PyString_Check(sipPy)) { *sipCppPtr = new std::string(PyString_AS_STRING(sipPy)); return 1; } +#else + if (PyUnicode_Check(sipPy)) { + *sipCppPtr = new std::string(PyUnicode_AsUTF8(sipPy)); + return 1; + } #endif return 0; From 00bf1444d565b0c62a76011a69643a3070c3395b Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Mon, 14 May 2018 12:31:07 -0700 Subject: [PATCH 07/79] Added back Python2 unicode support --- python_orocos_kdl/PyKDL/std_string.sip | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/std_string.sip b/python_orocos_kdl/PyKDL/std_string.sip index a399c9b2e..d91bda972 100644 --- a/python_orocos_kdl/PyKDL/std_string.sip +++ b/python_orocos_kdl/PyKDL/std_string.sip @@ -48,7 +48,13 @@ return 1; } #if PY_MAJOR_VERSION < 3 - if (PyString_Check(sipPy)) { + if (PyUnicode_Check(sipPy)) { + PyObject* s = PyUnicode_AsUTF8String(sipPy); + *sipCppPtr = new std::string(PyString_AS_STRING(s)); + Py_DECREF(s); + return 1; + } + else if (PyString_Check(sipPy)) { *sipCppPtr = new std::string(PyString_AS_STRING(sipPy)); return 1; } From a6cf9d06bf71535a9e82bd0c40daebf5f47844f4 Mon Sep 17 00:00:00 2001 From: Till Hofmann Date: Wed, 30 May 2018 16:44:35 +0200 Subject: [PATCH 08/79] Increase threshold tolerance in jacobiandottest This fixes test failures on i686, where the error would be just slightly higher than the allowed value. --- orocos_kdl/tests/jacobiandottest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/tests/jacobiandottest.cpp b/orocos_kdl/tests/jacobiandottest.cpp index 31950184a..f453d7383 100644 --- a/orocos_kdl/tests/jacobiandottest.cpp +++ b/orocos_kdl/tests/jacobiandottest.cpp @@ -251,7 +251,7 @@ bool runTest(const Chain& chain,const int& representation) for(double dt=1e-6;dt<0.1;dt*=10) { - double eps_diff_vs_solver = 3.0*dt; // Apparently :) + double eps_diff_vs_solver = 4.0*dt; // Apparently :) for(int i=0;i<100;i++) { From 99121451aa4861ad8e6f8bc8b92bf0b683c1714c Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Sat, 9 Jun 2018 15:22:41 -0700 Subject: [PATCH 09/79] Fixed velocityprofile_dirac does not stop bug and replaced tab with 4 spaces --- orocos_kdl/src/velocityprofile_dirac.cpp | 11 +++-- orocos_kdl/src/velocityprofile_dirac.hpp | 60 ++++++++++++------------ 2 files changed, 38 insertions(+), 33 deletions(-) diff --git a/orocos_kdl/src/velocityprofile_dirac.cpp b/orocos_kdl/src/velocityprofile_dirac.cpp index 9f4bc7639..de5051612 100644 --- a/orocos_kdl/src/velocityprofile_dirac.cpp +++ b/orocos_kdl/src/velocityprofile_dirac.cpp @@ -42,7 +42,7 @@ namespace KDL { } void VelocityProfile_Dirac:: - SetProfileDuration(double pos1,double pos2,double duration) + SetProfileDuration(double pos1,double pos2,double duration) { SetProfile(pos1,pos2); t = duration; @@ -53,10 +53,15 @@ namespace KDL { } double VelocityProfile_Dirac::Pos(double time) const { - if ( t == 0 ) + if ( t == 0 ) { return time == 0 ? p1 : p2; - else + } else if (time < 0) { + return p1; + } else if (time <= t) { return p1 + (( p2 - p1)/t)*time; + } else { + return p2; + } } double VelocityProfile_Dirac::Vel(double time) const { diff --git a/orocos_kdl/src/velocityprofile_dirac.hpp b/orocos_kdl/src/velocityprofile_dirac.hpp index ce06b6d44..07aa62b5f 100644 --- a/orocos_kdl/src/velocityprofile_dirac.hpp +++ b/orocos_kdl/src/velocityprofile_dirac.hpp @@ -33,37 +33,37 @@ namespace KDL { - /** - * A Dirac VelocityProfile generates an infinite velocity - * so that the position jumps from A to B in in infinite short time. - * In practice, this means that the maximum values are ignored and - * for any t : Vel(t) == 0 and Acc(t) == 0. - * Further Pos( -0 ) = pos1 and Pos( +0 ) = pos2. - * - * However, if a duration is given, it will create an unbound - * rectangular velocity profile for that duration, otherwise, - * Duration() == 0; - * @ingroup Motion - */ - class VelocityProfile_Dirac : public VelocityProfile - { - double p1,p2,t; - public: - void SetProfile(double pos1,double pos2); - virtual void SetProfileDuration(double pos1,double pos2,double duration); - virtual double Duration() const; - virtual double Pos(double time) const; - virtual double Vel(double time) const; - virtual double Acc(double time) const; - virtual void Write(std::ostream& os) const; - virtual VelocityProfile* Clone() const { - VelocityProfile_Dirac* res = new VelocityProfile_Dirac(); - res->SetProfileDuration( p1, p2, t ); - return res; - } + /** + * A Dirac VelocityProfile generates an infinite velocity + * so that the position jumps from A to B in in infinite short time. + * In practice, this means that the maximum values are ignored and + * for any t : Vel(t) == 0 and Acc(t) == 0. + * Further Pos( -0 ) = pos1 and Pos( +0 ) = pos2. + * + * However, if a duration is given, it will create an unbound + * rectangular velocity profile for that duration, otherwise, + * Duration() == 0; + * @ingroup Motion + */ + class VelocityProfile_Dirac : public VelocityProfile + { + double p1,p2,t; + public: + void SetProfile(double pos1,double pos2); + virtual void SetProfileDuration(double pos1,double pos2,double duration); + virtual double Duration() const; + virtual double Pos(double time) const; + virtual double Vel(double time) const; + virtual double Acc(double time) const; + virtual void Write(std::ostream& os) const; + virtual VelocityProfile* Clone() const { + VelocityProfile_Dirac* res = new VelocityProfile_Dirac(); + res->SetProfileDuration( p1, p2, t ); + return res; + } - virtual ~VelocityProfile_Dirac() {} - }; + virtual ~VelocityProfile_Dirac() {} + }; } From e1ca7faf1efa816f2bc12b741d0fda307d60c760 Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Thu, 14 Jun 2018 21:59:32 -0700 Subject: [PATCH 10/79] Added ChainIdSolver python wrapper --- python_orocos_kdl/PyKDL/kinfam.sip | 109 +++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) diff --git a/python_orocos_kdl/PyKDL/kinfam.sip b/python_orocos_kdl/PyKDL/kinfam.sip index d87fd00d6..993596310 100644 --- a/python_orocos_kdl/PyKDL/kinfam.sip +++ b/python_orocos_kdl/PyKDL/kinfam.sip @@ -19,6 +19,92 @@ //License along with this library; if not, write to the Free Software //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +template +%MappedType std::vector +{ +%TypeHeaderCode +#include +%End + +%ConvertFromTypeCode + PyObject *l = PyList_New(sipCpp -> size()); + + // Create the Python list of the correct length. + if (!l) + return NULL; + + // Go through each element in the C++ instance and convert it to a + // wrapped P2d. + for (int i = 0; i < (int)sipCpp->size(); ++i) { + TYPE *cpp = new TYPE(sipCpp->at(i)); + PyObject *pobj = sipConvertFromInstance(cpp, sipClass_TYPE, sipTransferObj); + + // Get the Python wrapper for the Type instance, creating a new + // one if necessary, and handle any ownership transfer. + if (!pobj) { + // There was an error so garbage collect the Python list. + Py_DECREF(l); + return NULL; + } + + // Add the wrapper to the list. + PyList_SET_ITEM(l, i, pobj); + } + + // Return the Python list. + return l; +%End + +%ConvertToTypeCode + // Check if type is compatible + if (!sipIsErr) { + // Must be any iterable + PyObject *i = PyObject_GetIter(sipPy); + bool iterable = (i != NULL); + Py_XDECREF(i); + return iterable; + } + + // Iterate over the object + PyObject *iterator = PyObject_GetIter(sipPy); + PyObject *item; + + std::vector *V = new std::vector(); + + while ((item = PyIter_Next(iterator))) + { + if (!sipCanConvertToInstance(item, sipClass_TYPE, SIP_NOT_NONE)) { + PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to TYPE"); + *sipIsErr = 1; + break; + } + + int state; + TYPE* p = reinterpret_cast( + sipConvertToInstance(item, sipClass_TYPE, 0, SIP_NOT_NONE, &state, sipIsErr)); + + if (!*sipIsErr) + V->push_back(*p); + + sipReleaseInstance(p, sipClass_TYPE, state); + Py_DECREF(item); + } + + Py_DECREF(iterator); + + if (*sipIsErr) { + delete V; + return 0; + } + + *sipCppPtr = V; + return sipGetState(sipTransferObj); +%End +}; + + + class Joint{ %TypeHeaderCode @@ -572,3 +658,26 @@ public: int JntToJac(const JntArray& q_in,Jacobian& jac); virtual void updateInternalDataStructures(); }; + + +class ChainIdSolver : SolverI +{ +%TypeHeaderCode +#include +using namespace KDL; +%End + virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector& f_ext,JntArray &torques)=0; +}; + +class ChainIdSolver_RNE : ChainIdSolver +{ +%TypeHeaderCode +#include +using namespace KDL; +%End + +public: + ChainIdSolver_RNE(const Chain& chain,Vector grav); + int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector& f_ext,JntArray &torques); + virtual void updateInternalDataStructures(); +}; From c36600aaf122db4433b19e8456e31ca03341a3a3 Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Tue, 26 Jun 2018 21:26:21 -0400 Subject: [PATCH 11/79] Adding setters and getters for RotationalInertia This also moves the definition for Inertia-Vector multiplication out of the sip class body, which was causing a code generation problem. --- python_orocos_kdl/PyKDL/kinfam.sip | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/kinfam.sip b/python_orocos_kdl/PyKDL/kinfam.sip index d87fd00d6..436e77edf 100644 --- a/python_orocos_kdl/PyKDL/kinfam.sip +++ b/python_orocos_kdl/PyKDL/kinfam.sip @@ -69,8 +69,27 @@ public: RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); static RotationalInertia Zero()/Factory/; - Vector operator*(Vector omega) const /Factory/; + + double __getitem__(int index); +%MethodCode + if (a0 < 0 || a0 >= 9) { + PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range"); + return 0; + } + sipRes=(*sipCpp).data[a0]; +%End + + void __setitem__(int i, double value); +%MethodCode + if (a0 < 0 || a0 >= 9) { + PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range"); + return 0; + } + (*sipCpp).data[a0]=a1; +%End + }; +Vector operator*(RotationalInertia& Ia, Vector omega) const /Factory/; RotationalInertia operator*(double a, const RotationalInertia& I)/Factory/; RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib)/Factory/; From fd5e2bbe0050c2dfcf0f008e357f6019c1de50c5 Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Sun, 11 Mar 2018 22:20:42 -0400 Subject: [PATCH 12/79] Adding catkin env-hooks to support venv --- python_orocos_kdl/CMakeLists.txt | 7 ++++++- .../env-hooks/python_orocos_kdl_site_packages.bash.in | 1 + .../env-hooks/python_orocos_kdl_site_packages.sh.in | 1 + .../env-hooks/python_orocos_kdl_site_packages.tcsh.in | 1 + .../env-hooks/python_orocos_kdl_site_packages.zsh.in | 1 + 5 files changed, 10 insertions(+), 1 deletion(-) create mode 120000 python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in create mode 100644 python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in create mode 120000 python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in create mode 120000 python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 35c1c3e29..449f8662b 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -22,4 +22,9 @@ set(SIP_EXTRA_OPTIONS "-o") set(PYTHON_SITE_PACKAGES_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES}) add_sip_python_module(PyKDL PyKDL/PyKDL.sip ${orocos_kdl_LIBRARIES}) -install(FILES package.xml DESTINATION share/python_orocos_kdl) \ No newline at end of file +install(FILES package.xml DESTINATION share/python_orocos_kdl) + +find_package(catkin) +if(catkin_FOUND) + catkin_add_env_hooks(python_orocos_kdl_site_packages SHELLS bash tcsh zsh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) +endif() diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in new file mode 120000 index 000000000..47a63df22 --- /dev/null +++ b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in @@ -0,0 +1 @@ +python_orocos_kdl_site_packages.sh.in \ No newline at end of file diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in new file mode 100644 index 000000000..13d938d3c --- /dev/null +++ b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in @@ -0,0 +1 @@ +export PYTHONPATH=@PYTHON_SITE_PACKAGES_INSTALL_DIR@:$PYTHONPATH diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in new file mode 120000 index 000000000..47a63df22 --- /dev/null +++ b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in @@ -0,0 +1 @@ +python_orocos_kdl_site_packages.sh.in \ No newline at end of file diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in new file mode 120000 index 000000000..47a63df22 --- /dev/null +++ b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in @@ -0,0 +1 @@ +python_orocos_kdl_site_packages.sh.in \ No newline at end of file From 692463cfa05e1154fc3f12281e38c1bb9012de6d Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Fri, 29 Jun 2018 14:58:40 -0700 Subject: [PATCH 13/79] Added velocityprofile_dirac unittest --- orocos_kdl/tests/velocityprofiletest.cpp | 42 ++++++++++++++++++++++++ orocos_kdl/tests/velocityprofiletest.hpp | 7 ++++ 2 files changed, 49 insertions(+) diff --git a/orocos_kdl/tests/velocityprofiletest.cpp b/orocos_kdl/tests/velocityprofiletest.cpp index 149de6b72..d4044b4a5 100644 --- a/orocos_kdl/tests/velocityprofiletest.cpp +++ b/orocos_kdl/tests/velocityprofiletest.cpp @@ -334,3 +334,45 @@ void VelocityProfileTest::TestTrapHalf_SetDuration_End() CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon); } + +void VelocityProfileTest::TestDirac_SetProfile() +{ + double time; + double pos1 = 10.0; + double pos2 = -1.0; + + VelocityProfile_Dirac v; + v.SetProfile(pos1, pos2); + + time = 0; + CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon); + + time = 1.0; + CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon); + + time = -1.0; + CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon); +} + +void VelocityProfileTest::TestDirac_SetProfileDuration() +{ + double time; + double pos1 = 10.0; + double pos2 = -1.0; + double duration = 5.0; + VelocityProfile_Dirac v; + v.SetProfileDuration(pos1, pos2, duration); + + time = -1.0; + CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon); + + time = duration/2; + CPPUNIT_ASSERT_DOUBLES_EQUAL((pos1 + pos2)/2, v.Pos(time), epsilon); + CPPUNIT_ASSERT_DOUBLES_EQUAL((pos2-pos1)/duration, v.Vel(time), epsilon); + + time = duration; + CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon); + + time = duration + 1.0; + CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon); +} diff --git a/orocos_kdl/tests/velocityprofiletest.hpp b/orocos_kdl/tests/velocityprofiletest.hpp index fb6e2ce24..27b5494b9 100644 --- a/orocos_kdl/tests/velocityprofiletest.hpp +++ b/orocos_kdl/tests/velocityprofiletest.hpp @@ -4,6 +4,7 @@ #include #include #include +#include class VelocityProfileTest : public CppUnit::TestFixture { @@ -18,6 +19,9 @@ class VelocityProfileTest : public CppUnit::TestFixture CPPUNIT_TEST(TestTrapHalf_SetDuration_Start); CPPUNIT_TEST(TestTrapHalf_SetDuration_End); + CPPUNIT_TEST(TestDirac_SetProfile); + CPPUNIT_TEST(TestDirac_SetProfileDuration); + CPPUNIT_TEST_SUITE_END(); public: @@ -33,6 +37,9 @@ class VelocityProfileTest : public CppUnit::TestFixture void TestTrapHalf_SetProfile_End(); void TestTrapHalf_SetDuration_Start(); void TestTrapHalf_SetDuration_End(); + + void TestDirac_SetProfile(); + void TestDirac_SetProfileDuration(); }; #endif From 06efdca935540b539004b6aa84147f3121ec8834 Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Wed, 11 Jul 2018 10:00:49 -0700 Subject: [PATCH 14/79] Added QUIET to find catkin to suppress message --- python_orocos_kdl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 449f8662b..f60f20102 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -24,7 +24,7 @@ add_sip_python_module(PyKDL PyKDL/PyKDL.sip ${orocos_kdl_LIBRARIES}) install(FILES package.xml DESTINATION share/python_orocos_kdl) -find_package(catkin) +find_package(catkin QUIET) if(catkin_FOUND) catkin_add_env_hooks(python_orocos_kdl_site_packages SHELLS bash tcsh zsh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) endif() From 4ac5950fac65935ae53bc9279829111a94d1e355 Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Thu, 19 Jul 2018 21:50:49 -0700 Subject: [PATCH 15/79] Updated VelocityProfile_Dirac to return p1 when t=0 and time <= 0 --- orocos_kdl/src/velocityprofile_dirac.cpp | 2 +- orocos_kdl/tests/velocityprofiletest.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/velocityprofile_dirac.cpp b/orocos_kdl/src/velocityprofile_dirac.cpp index de5051612..23c915b59 100644 --- a/orocos_kdl/src/velocityprofile_dirac.cpp +++ b/orocos_kdl/src/velocityprofile_dirac.cpp @@ -54,7 +54,7 @@ namespace KDL { double VelocityProfile_Dirac::Pos(double time) const { if ( t == 0 ) { - return time == 0 ? p1 : p2; + return time <= 0 ? p1 : p2; } else if (time < 0) { return p1; } else if (time <= t) { diff --git a/orocos_kdl/tests/velocityprofiletest.cpp b/orocos_kdl/tests/velocityprofiletest.cpp index d4044b4a5..fb52ed9e1 100644 --- a/orocos_kdl/tests/velocityprofiletest.cpp +++ b/orocos_kdl/tests/velocityprofiletest.cpp @@ -351,7 +351,7 @@ void VelocityProfileTest::TestDirac_SetProfile() CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon); time = -1.0; - CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon); + CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon); } void VelocityProfileTest::TestDirac_SetProfileDuration() From 3ecba6781ee96f3d334a3b1da7bac52de3a724d6 Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Fri, 20 Jul 2018 12:40:06 -0700 Subject: [PATCH 16/79] Minor fix: fixed error message in PyKDL Wrench --- python_orocos_kdl/PyKDL/frames.sip | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/frames.sip b/python_orocos_kdl/PyKDL/frames.sip index 07a9d24c4..59f27ceef 100644 --- a/python_orocos_kdl/PyKDL/frames.sip +++ b/python_orocos_kdl/PyKDL/frames.sip @@ -369,7 +369,7 @@ public: double __getitem__ (int i) const; %MethodCode if (a0 < 0 || a0 > 5) { - PyErr_SetString(PyExc_IndexError, "Twist index out of range"); + PyErr_SetString(PyExc_IndexError, "Wrench index out of range"); return 0; } sipRes=(*sipCpp)(a0); @@ -378,7 +378,7 @@ public: void __setitem__(int i, double value); %MethodCode if (a0 < 0 || a0 > 5) { - PyErr_SetString(PyExc_IndexError, "Twist index out of range"); + PyErr_SetString(PyExc_IndexError, "Wrench index out of range"); return 0; } (*sipCpp)(a0)=a1; From ed2b5ad0eda949d5785e6810a209a6852df1fc7d Mon Sep 17 00:00:00 2001 From: "luz.paz" Date: Mon, 30 Jul 2018 14:30:08 -0400 Subject: [PATCH 17/79] trivial typo fixes --- orocos_kdl/src/path_line.hpp | 2 +- orocos_kdl/src/utilities/error_stack.cxx | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/path_line.hpp b/orocos_kdl/src/path_line.hpp index 548aaf4aa..7f27ba906 100644 --- a/orocos_kdl/src/path_line.hpp +++ b/orocos_kdl/src/path_line.hpp @@ -98,7 +98,7 @@ class Path_Line : public Path * of the frame in which you express your path. * Other implementations for RotationalInterpolations COULD be * (not implemented) (yet) : - * 1) quaternion interpolation : but this is more difficult for the human to interprete + * 1) quaternion interpolation : but this is more difficult for the human to interpret * 2) 3-axis interpolation : express the orientation of the frame in e.g. * euler zyx angles alfa,beta, gamma and interpolate these parameters. * But this is dependent of the frame you choose as a reference and diff --git a/orocos_kdl/src/utilities/error_stack.cxx b/orocos_kdl/src/utilities/error_stack.cxx index 864f333c0..78c5027a9 100644 --- a/orocos_kdl/src/utilities/error_stack.cxx +++ b/orocos_kdl/src/utilities/error_stack.cxx @@ -23,7 +23,7 @@ namespace KDL { // Trace of the call stack of the I/O routines to help user -// interprete error messages from I/O +// interpret error messages from I/O typedef std::stack ErrorStack; // should be in Thread Local Storage if this gets multithreaded one day... From e7713a5d300bbdbd8724503cc96060deac4f8d25 Mon Sep 17 00:00:00 2001 From: Antoine Hoarau Date: Tue, 7 Aug 2018 17:54:12 +0200 Subject: [PATCH 18/79] fix typos --- orocos_kdl/src/chainjnttojacdotsolver.cpp | 6 +++--- orocos_kdl/src/chainjnttojacdotsolver.hpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/orocos_kdl/src/chainjnttojacdotsolver.cpp b/orocos_kdl/src/chainjnttojacdotsolver.cpp index cb47d750d..c171087c1 100644 --- a/orocos_kdl/src/chainjnttojacdotsolver.cpp +++ b/orocos_kdl/src/chainjnttojacdotsolver.cpp @@ -85,7 +85,7 @@ int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, if (representation_ == BODYFIXED) { // Ref Frame {ee}, Ref Point {ee} jac_.changeBase(F_bs_ee_.M.Inverse()); - } else if (representation_ == INTERTIAL) { + } else if (representation_ == INERTIAL) { // Ref Frame {bs}, Ref Point {bs} jac_.changeRefPoint(-F_bs_ee_.p); } else { @@ -125,7 +125,7 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J return getPartialDerivativeHybrid(J,joint_idx,column_idx); case BODYFIXED: return getPartialDerivativeBodyFixed(J,joint_idx,column_idx); - case INTERTIAL: + case INERTIAL: return getPartialDerivativeInertial(J,joint_idx,column_idx); default: SetToZero(this->t_djdq_); @@ -211,7 +211,7 @@ void ChainJntToJacDotSolver::setRepresentation(const int& representation) { if(representation == HYBRID || representation == BODYFIXED || - representation == INTERTIAL) + representation == INERTIAL) this->representation_ = representation; } diff --git a/orocos_kdl/src/chainjnttojacdotsolver.hpp b/orocos_kdl/src/chainjnttojacdotsolver.hpp index ecffa1355..675a36070 100644 --- a/orocos_kdl/src/chainjnttojacdotsolver.hpp +++ b/orocos_kdl/src/chainjnttojacdotsolver.hpp @@ -57,7 +57,7 @@ class ChainJntToJacDotSolver : public SolverI // Body-fixed representation ref Frame: end-effector, ref Point: end-effector static const int BODYFIXED = 1; // Intertial representation ref Frame: base, ref Point: base - static const int INTERTIAL = 2; + static const int INERTIAL = 2; explicit ChainJntToJacDotSolver(const Chain& chain); virtual ~ChainJntToJacDotSolver(); @@ -100,11 +100,11 @@ class ChainJntToJacDotSolver : public SolverI * * @return void */ - void setInternialRepresentation(){setRepresentation(INTERTIAL);} + void setInertialRepresentation(){setRepresentation(INERTIAL);} /** * @brief Sets the internal variable for the representation (with a check on the value) * - * @param representation The representation for Jdot : HYBRID,BODYFIXED or INTERTIAL + * @param representation The representation for Jdot : HYBRID,BODYFIXED or INERTIAL * @return void */ void setRepresentation(const int& representation); From 66e94469da485aaf8604a79a80b952f058338b7f Mon Sep 17 00:00:00 2001 From: Antoine Hoarau Date: Thu, 9 Aug 2018 14:16:28 +0200 Subject: [PATCH 19/79] fix test typo --- orocos_kdl/tests/jacobiandottest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/tests/jacobiandottest.cpp b/orocos_kdl/tests/jacobiandottest.cpp index f453d7383..fd86d896e 100644 --- a/orocos_kdl/tests/jacobiandottest.cpp +++ b/orocos_kdl/tests/jacobiandottest.cpp @@ -100,7 +100,7 @@ void changeRepresentation(Jacobian& J,const Frame& F_bs_ee,const int& representa // Ref Frame {ee}, Ref Point {ee} J.changeBase(F_bs_ee.M.Inverse()); break; - case ChainJntToJacDotSolver::INTERTIAL: + case ChainJntToJacDotSolver::INERTIAL: // Ref Frame {bs}, Ref Point {bs} J.changeRefPoint(-F_bs_ee.p); break; From d5908df8c7ab099f944eafc6f35c3d8d316bd367 Mon Sep 17 00:00:00 2001 From: "luz.paz" Date: Thu, 6 Sep 2018 09:22:57 -0400 Subject: [PATCH 20/79] few more typos --- orocos_kdl/src/velocityprofile_spline.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/orocos_kdl/src/velocityprofile_spline.hpp b/orocos_kdl/src/velocityprofile_spline.hpp index 0ce9ea896..3d17abdba 100644 --- a/orocos_kdl/src/velocityprofile_spline.hpp +++ b/orocos_kdl/src/velocityprofile_spline.hpp @@ -16,10 +16,10 @@ class VelocityProfile_Spline : public VelocityProfile VelocityProfile_Spline(const VelocityProfile_Spline &p); virtual ~VelocityProfile_Spline(); - + virtual void SetProfile(double pos1, double pos2); /** - * Generate linear interpolation coeffcients. + * Generate linear interpolation coefficients. * * @param pos1 begin position. * @param pos2 end position. @@ -29,7 +29,7 @@ class VelocityProfile_Spline : public VelocityProfile double pos1, double pos2, double duration); /** - * Generate cubic spline interpolation coeffcients. + * Generate cubic spline interpolation coefficients. * * @param pos1 begin position. * @param vel1 begin velocity. @@ -41,7 +41,7 @@ class VelocityProfile_Spline : public VelocityProfile double pos1, double vel1, double pos2, double vel2, double duration); /** - * Generate quintic spline interpolation coeffcients. + * Generate quintic spline interpolation coefficients. * * @param pos1 begin position. * @param vel1 begin velocity. From c59cc0cfe4f04c27404cdfed55e8493b8c4ffb7b Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Thu, 17 May 2018 12:05:20 -0400 Subject: [PATCH 21/79] Joint: add function to Joint Class to get joint inertia --- orocos_kdl/src/joint.hpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/orocos_kdl/src/joint.hpp b/orocos_kdl/src/joint.hpp index a188aff95..65f1ee410 100644 --- a/orocos_kdl/src/joint.hpp +++ b/orocos_kdl/src/joint.hpp @@ -192,6 +192,16 @@ namespace KDL { } }; + /** + * Request the inertia of the joint. + * + * @return const reference to the inertia of the joint + */ + const double& getInertia() const + { + return inertia; + }; + virtual ~Joint(); private: From 00f8bc816fe75bf6e412f66eacb94f6bf9655787 Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Thu, 17 May 2018 12:07:03 -0400 Subject: [PATCH 22/79] ChainDynParam: add joint inertia to diagonal elements of joint space inertia matrix The joint inertia consists primarily of the effective motor inertia at the link output. This can be significant in comparison with the link inertia. --- orocos_kdl/src/chaindynparam.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/src/chaindynparam.cpp b/orocos_kdl/src/chaindynparam.cpp index 3773c3b0f..61ed64227 100644 --- a/orocos_kdl/src/chaindynparam.cpp +++ b/orocos_kdl/src/chaindynparam.cpp @@ -98,7 +98,8 @@ namespace KDL { F=Ic[i]*S[i]; if(chain.getSegment(i).getJoint().getType()!=Joint::None) { - H(k,k)=dot(S[i],F); + H(k,k)=dot(S[i],F); + H(k,k)+=chain.getSegment(i).getJoint().getInertia(); // add joint inertia j=k; //countervariable for the joints l=i; //countervariable for the segments while(l!=0) //go from leaf to root starting at i From b8c2b5da0842ab73d0b656888b24d41bff779bd3 Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Thu, 17 May 2018 12:08:21 -0400 Subject: [PATCH 23/79] ChainIdSolver_RNE: add joint inertial torque The effective motor inertia multiplied by the joint acceleration needs to be added to the joint torque. --- orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp index 9a63fb5a1..2e4980f61 100644 --- a/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp +++ b/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp @@ -87,8 +87,11 @@ namespace KDL{ //Sweep from leaf to root j=nj-1; for(int i=ns-1;i>=0;i--){ - if(chain.getSegment(i).getJoint().getType()!=Joint::None) - torques(j--)=dot(S[i],f[i]); + if(chain.getSegment(i).getJoint().getType()!=Joint::None){ + torques(j)=dot(S[i],f[i]); + torques(j)+=chain.getSegment(i).getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia + --j; + } if(i!=0) f[i-1]=f[i-1]+X[i]*f[i]; } From 933502eb28e0c1fdd1a1ba2a131d648cc59286cc Mon Sep 17 00:00:00 2001 From: "luz.paz" Date: Tue, 2 Oct 2018 08:30:23 -0400 Subject: [PATCH 24/79] Followup typo --- orocos_kdl/src/chainjnttojacsolver.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/src/chainjnttojacsolver.hpp b/orocos_kdl/src/chainjnttojacsolver.hpp index eef3816fd..25c3f234c 100644 --- a/orocos_kdl/src/chainjnttojacsolver.hpp +++ b/orocos_kdl/src/chainjnttojacsolver.hpp @@ -47,7 +47,7 @@ namespace KDL /** * Calculate the jacobian expressed in the base frame of the * chain, with reference point at the end effector of the - * *chain. The alghoritm is similar to the one used in + * *chain. The algorithm is similar to the one used in * KDL::ChainFkSolverVel_recursive * * @param q_in input joint positions @@ -56,7 +56,7 @@ namespace KDL * @return success/error code */ virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNR=-1); - + /** * * @param locked_joints new values for locked joints @@ -75,4 +75,3 @@ namespace KDL }; } #endif - From 273de3a1c4b1f6bb33f2467ae33ba58938fff068 Mon Sep 17 00:00:00 2001 From: mlutter Date: Wed, 7 Nov 2018 21:10:06 +0100 Subject: [PATCH 25/79] exposing the Jacobian Dot Solver and the optional Segment index to Jacobian and Jacobian Dot solver --- python_orocos_kdl/PyKDL/kinfam.sip | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/kinfam.sip b/python_orocos_kdl/PyKDL/kinfam.sip index f1507e41f..4c66237ef 100644 --- a/python_orocos_kdl/PyKDL/kinfam.sip +++ b/python_orocos_kdl/PyKDL/kinfam.sip @@ -674,10 +674,21 @@ using namespace KDL; %End public: ChainJntToJacSolver(const Chain& chain); - int JntToJac(const JntArray& q_in,Jacobian& jac); + int JntToJac(const JntArray& q_in,Jacobian& jac, int seg_nr=-1); virtual void updateInternalDataStructures(); }; +class ChainJntToJacDotSolver : SolverI +{ +%TypeHeaderCode +#include +using namespace KDL; +%End +public: + ChainJntToJacDotSolver(const Chain& chain); + int JntToJacDot(const JntArrayVel& q_in,Jacobian& jac, int seg_nr=-1); + virtual void updateInternalDataStructures(); +}; class ChainIdSolver : SolverI { From 69ef26bbd88b495aa586d56b4fdd4444ceb013ca Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Tue, 8 Jan 2019 21:46:16 +0100 Subject: [PATCH 26/79] Recursive Newton-Euler inverse dynamics for trees --- TEMP.cpp | 53 ++++++ orocos_kdl/src/treeidsolver.hpp | 62 ++++++ .../treeidsolver_recursive_newton_euler.cpp | 138 ++++++++++++++ .../treeidsolver_recursive_newton_euler.hpp | 84 +++++++++ orocos_kdl/tests/CMakeLists.txt | 22 ++- orocos_kdl/tests/treeinvdyntest.cpp | 178 ++++++++++++++++++ orocos_kdl/tests/treeinvdyntest.hpp | 33 ++++ 7 files changed, 562 insertions(+), 8 deletions(-) create mode 100644 TEMP.cpp create mode 100644 orocos_kdl/src/treeidsolver.hpp create mode 100644 orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp create mode 100644 orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp create mode 100644 orocos_kdl/tests/treeinvdyntest.cpp create mode 100644 orocos_kdl/tests/treeinvdyntest.hpp diff --git a/TEMP.cpp b/TEMP.cpp new file mode 100644 index 000000000..4f3ae5f17 --- /dev/null +++ b/TEMP.cpp @@ -0,0 +1,53 @@ +if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments()) + return (error = E_NOT_UP_TO_DATE); + +//Check sizes when in debug mode +if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns) + return (error = E_SIZE_MISMATCH); +unsigned int j=0; + +//Sweep from root to leaf +for(unsigned int i=0;i=0;i--){ + if(chain.getSegment(i).getJoint().getType()!=Joint::None){ + torques(j)=dot(S[i],f[i]); + torques(j)+=chain.getSegment(i).getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia + --j; + } + if(i!=0) + f[i-1]=f[i-1]+X[i]*f[i]; +} +return (error = E_NOERROR); diff --git a/orocos_kdl/src/treeidsolver.hpp b/orocos_kdl/src/treeidsolver.hpp new file mode 100644 index 000000000..1eef27547 --- /dev/null +++ b/orocos_kdl/src/treeidsolver.hpp @@ -0,0 +1,62 @@ +// Copyright (C) 2009 Ruben Smits + +// Version: 1.0 +// Author: Franco Fusco +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_TREE_IDSOLVER_HPP +#define KDL_TREE_IDSOLVER_HPP + +#include "tree.hpp" +#include "frames.hpp" +#include "jntarray.hpp" +#include "solveri.hpp" + +namespace KDL +{ + + typedef std::map WrenchMap; + + /** + * \brief This abstract class encapsulates the inverse + * dynamics solver for a KDL::Tree. + * + */ + class TreeIdSolver : public KDL::SolverI + { + public: + /** + * Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces + * to joint torques/forces. + * + * @param q input joint positions + * @param q_dot input joint velocities + * @param q_dotdot input joint accelerations + * @param f_ext the external forces (no gravity) on the segments + * @param torque output joint torques + * + * @return if < 0 something went wrong + */ + virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext,JntArray &torques)=0; + + // Need functions to return the manipulator mass, coriolis and gravity matrices - Lagrangian Formulation. + }; + +} + +#endif diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp new file mode 100644 index 000000000..7103d9978 --- /dev/null +++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp @@ -0,0 +1,138 @@ +// Copyright (C) 2009 Ruben Smits + +// Version: 1.0 +// Author: Franco Fusco +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "treeidsolver_recursive_newton_euler.hpp" +#include "frames_io.hpp" + +namespace KDL{ + + TreeIdSolver_RNE::TreeIdSolver_RNE(const Tree& tree_, Vector grav): + tree(tree_), nj(tree.getNrOfJoints()), ns(tree.getNrOfSegments()) + { + ag=-Twist(grav,Vector::Zero()); + initAuxVariables(); + } + + void TreeIdSolver_RNE::updateInternalDataStructures() { + nj = tree.getNrOfJoints(); + ns = tree.getNrOfSegments(); + initAuxVariables(); + } + + void TreeIdSolver_RNE::initAuxVariables() { + const SegmentMap& segments = tree.getSegments(); + for(SegmentMap::const_iterator seg = segments.begin(); seg != segments.end(); seg++) { + X[seg->first] = Frame(); + S[seg->first] = Twist(); + v[seg->first] = Twist(); + a[seg->first] = Twist(); + f[seg->first] = Wrench(); + } + } + + int TreeIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques) + { + //Check that the tree was not modified externally + if(nj != tree.getNrOfJoints() || ns != tree.getNrOfSegments()) + return (error = E_NOT_UP_TO_DATE); + + //Check sizes of joint vectors + if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj) + return (error = E_SIZE_MISMATCH); + + try { + //Do the recursion here + rne_step(tree.getRootSegment(), q, q_dot, q_dotdot, f_ext, torques); + } + catch(const std::out_of_range&) { + //If in rne_step we get an out_of_range exception it means that some call + //to map::at failed. This can happen only if updateInternalDataStructures + //was not called after changing something in the tree. Note that it + //should be impossible to reach this point as consistency of the tree is + //checked above. + return (error = E_NOT_UP_TO_DATE); + } + return (error = E_NOERROR); + } + + + void TreeIdSolver_RNE::rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques) { + const Segment& seg = GetTreeElementSegment(segment->second); + const std::string& segname = segment->first; + const std::string& parname = GetTreeElementParent(segment->second)->first; + + //Do forward calculations involving velocity & acceleration of this segment + double q_, qdot_, qdotdot_; + unsigned int j = GetTreeElementQNr(segment->second); + if(seg.getJoint().getType()!=Joint::None){ + q_ = q(j); + qdot_ = q_dot(j); + qdotdot_ = q_dotdot(j); + } + else + q_ = qdot_ = qdotdot_ = 0.0; + + //Calculate segment properties: X,S,vj,cj + + //Remark this is the inverse of the frame for transformations from the parent to the current coord frame + X.at(segname) = seg.pose(q_); + + //Transform velocity and unit velocity to segment frame + Twist vj = X.at(segname).M.Inverse( seg.twist(q_,qdot_) ); + S.at(segname) = X.at(segname).M.Inverse( seg.twist(q_,1.0) ); + + //calculate velocity and acceleration of the segment (in segment coordinates) + if(segment == tree.getRootSegment()) { + v.at(segname) = vj; + a.at(segname) = X.at(segname).Inverse(ag) + S.at(segname)*qdotdot_+ v.at(segname)*vj; + } + else { + v.at(segname) = X.at(segname).Inverse(v.at(parname)) + vj; + a.at(segname) = X.at(segname).Inverse(a.at(parname)) + S.at(segname)*qdotdot_ + v.at(segname)*vj; + } + + //Calculate the force for the joint + //Collect RigidBodyInertia and external forces + const RigidBodyInertia& I = seg.getInertia(); + f.at(segname) = I*a.at(segname) + v.at(segname)*(I*v.at(segname)); + if(f_ext.find(segname) != f_ext.end()) + f.at(segname) = f.at(segname) - f_ext.at(segname); + + //propagate calculations over each child segment + SegmentMap::const_iterator child; + for (unsigned int i = 0; i < GetTreeElementChildren(segment->second).size(); i++) { + child = GetTreeElementChildren(segment->second)[i]; + rne_step(child, q, q_dot, q_dotdot, f_ext, torques); + } + + //do backward calculations involving wrenches and joint efforts + + //If there is a moving joint, evaluate its effort + if(seg.getJoint().getType()!=Joint::None){ + torques(j) = dot(S.at(segname), f.at(segname)); + torques(j) += seg.getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia + } + + //add reaction forces to parent segment + if(segment != tree.getRootSegment()) + f.at(parname) = f.at(parname) + X.at(segname)*f.at(segname); + } +}//namespace diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp new file mode 100644 index 000000000..95af77a01 --- /dev/null +++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp @@ -0,0 +1,84 @@ +// Copyright (C) 2009 Ruben Smits + +// Version: 1.0 +// Author: Franco Fusco +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP +#define KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP + +#include "treeidsolver.hpp" + +namespace KDL{ + /** + * \brief Recursive newton euler inverse dynamics solver for kinematic trees. + * + * It calculates the torques for the joints, given the motion of + * the joints (q,qdot,qdotdot), external forces on the segments + * (expressed in the segments reference frame) and the dynamical + * parameters of the segments. + * + * This is an extension of the inverse dynamic solver for kinematic chains, + * \see ChainIdSolver_RNE. The main difference is the use of STL maps + * instead of vectors to represent external wrenches (as well as internal + * variables exploited during the recursion). + */ + class TreeIdSolver_RNE : public TreeIdSolver { + public: + /** + * Constructor for the solver, it will allocate all the necessary memory + * \param tree The kinematic tree to calculate the inverse dynamics for, an internal reference will be stored. + * \param grav The gravity vector to use during the calculation. + */ + TreeIdSolver_RNE(const Tree& tree, Vector grav); + + /** + * Function to calculate from Cartesian forces to joint torques. + * Input parameters; + * \param q The current joint positions + * \param q_dot The current joint velocities + * \param q_dotdot The current joint accelerations + * \param f_ext The external forces (no gravity) on the segments + * Output parameters: + * \param torques the resulting torques for the joints + */ + int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques); + + /// @copydoc KDL::SolverI::updateInternalDataStructures + virtual void updateInternalDataStructures(); + + private: + ///Helper function to initialize private members X, S, v, a, f + void initAuxVariables(); + + ///One recursion step + void rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques); + + const Tree& tree; + unsigned int nj; + unsigned int ns; + std::map X; + std::map S; + std::map v; + std::map a; + std::map f; + Twist ag; + }; +} + +#endif diff --git a/orocos_kdl/tests/CMakeLists.txt b/orocos_kdl/tests/CMakeLists.txt index 06382cdb4..6f91b036f 100644 --- a/orocos_kdl/tests/CMakeLists.txt +++ b/orocos_kdl/tests/CMakeLists.txt @@ -9,7 +9,7 @@ IF(ENABLE_TESTS) SET_TARGET_PROPERTIES( framestest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") ADD_TEST(framestest framestest) - + ADD_EXECUTABLE(kinfamtest kinfamtest.cpp test-runner.cpp) TARGET_LINK_LIBRARIES(kinfamtest orocos-kdl ${CPPUNIT}) SET(TESTNAME "kinfamtest") @@ -44,7 +44,7 @@ IF(ENABLE_TESTS) SET_TARGET_PROPERTIES( jacobiandottest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") ADD_TEST(jacobiandottest jacobiandottest) - + ADD_EXECUTABLE(velocityprofiletest velocityprofiletest.cpp test-runner.cpp) SET(TESTNAME "velocityprofiletest") TARGET_LINK_LIBRARIES(velocityprofiletest orocos-kdl ${CPPUNIT}) @@ -52,6 +52,13 @@ IF(ENABLE_TESTS) COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") ADD_TEST(velocityprofiletest velocityprofiletest) + ADD_EXECUTABLE(treeinvdyntest treeinvdyntest.cpp test-runner.cpp) + SET(TESTNAME "treeinvdyntest") + TARGET_LINK_LIBRARIES(treeinvdyntest orocos-kdl ${CPPUNIT}) + SET_TARGET_PROPERTIES( treeinvdyntest PROPERTIES + COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") + ADD_TEST(treeinvdyntest treeinvdyntest) + # ADD_EXECUTABLE(rframestest rframestest.cpp) # TARGET_LINK_LIBRARIES(rframestest orocos-kdl) # ADD_TEST(rframestest rframestest) @@ -60,7 +67,7 @@ IF(ENABLE_TESTS) # TARGET_LINK_LIBRARIES(rallnumbertest orocos-kdl) # ADD_TEST(rallnumbertest rallnumbertest) # -# +# # IF(OROCOS_PLUGIN) # ADD_EXECUTABLE(toolkittest toolkittest.cpp) # LINK_DIRECTORIES(${OROCOS_RTT_LINK_DIRS}) @@ -78,10 +85,9 @@ IF(ENABLE_TESTS) # CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/PyKDLtest.py # ${CMAKE_CURRENT_BINARY_DIR}/PyKDLtest.py COPY_ONLY) # ADD_TEST(PyKDLtest PyKDLtest.py) -# -# +# +# # ENDIF(PYTHON_BINDINGS) - - ADD_CUSTOM_TARGET(check ctest -V WORKING_DIRECTORY ${PROJ_BINARY_DIR}/tests) -ENDIF(ENABLE_TESTS) + ADD_CUSTOM_TARGET(check ctest -V WORKING_DIRECTORY ${PROJ_BINARY_DIR}/tests) +ENDIF(ENABLE_TESTS) diff --git a/orocos_kdl/tests/treeinvdyntest.cpp b/orocos_kdl/tests/treeinvdyntest.cpp new file mode 100644 index 000000000..07ccebfbe --- /dev/null +++ b/orocos_kdl/tests/treeinvdyntest.cpp @@ -0,0 +1,178 @@ +#include "treeinvdyntest.hpp" +#include +#include +#include +#include +#include +#include + + +CPPUNIT_TEST_SUITE_REGISTRATION( TreeInvDynTest ); + +using namespace KDL; +using std::cout; +using std::endl; + +void TreeInvDynTest::setUp() +{ + srand( (unsigned)time( NULL )); + + //spatial inertia (just to test dynamics) + RigidBodyInertia inertia(0.3, Vector(0.0, 0.1, 0.0), RotationalInertia(0.005, 0.001, 0.001)); + + //create chain #1 + chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ), + Frame(Vector(0.0,0.0,0.0)), inertia)); + chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX), + Frame(Vector(0.0,0.0,0.9)), inertia)); + chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::None), + Frame(Vector(-0.4,0.0,0.0)))); + chain1.addSegment(Segment("Segment 14", Joint("Joint 14", Joint::RotX), + Frame(Vector(0.0,0.0,1.2)), inertia)); + chain1.addSegment(Segment("Segment 15", Joint("Joint 15", Joint::None), + Frame(Vector(0.4,0.0,0.0)), inertia)); + chain1.addSegment(Segment("Segment 16", Joint("Joint 16", Joint::RotZ), + Frame(Vector(0.0,0.0,1.4)), inertia)); + chain1.addSegment(Segment("Segment 17", Joint("Joint 17", Joint::RotX), + Frame(Vector(0.0,0.0,0.0)), inertia)); + chain1.addSegment(Segment("Segment 18", Joint("Joint 18", Joint::RotZ), + Frame(Vector(0.0,0.0,0.4)), inertia)); + chain1.addSegment(Segment("Segment 19", Joint("Joint 19", Joint::None), + Frame(Vector(0.0,0.0,0.0)), inertia)); + + //create chain #2 + chain2.addSegment(Segment("Segment 21", Joint("Joint 21", Joint::RotZ), + Frame(Vector(0.0,0.0,0.5)), inertia)); + chain2.addSegment(Segment("Segment 22", Joint("Joint 22", Joint::RotX), + Frame(Vector(0.0,0.0,0.4)), inertia)); + chain2.addSegment(Segment("Segment 23", Joint("Joint 23", Joint::RotX), + Frame(Vector(0.0,0.0,0.3)), inertia)); + chain2.addSegment(Segment("Segment 24", Joint("Joint 24", Joint::RotX), + Frame(Vector(0.0,0.0,0.2)), inertia)); + chain2.addSegment(Segment("Segment 25", Joint("Joint 25", Joint::RotZ), + Frame(Vector(0.0,0.0,0.1)), inertia)); + + //create tree as two chains attached to the root + tree = Tree(); + tree.addChain(chain1, tree.getRootSegment()->first); + tree.addChain(chain2, tree.getRootSegment()->first); + + //create a simple "y-shaped" tree + m=1.0, Iz=0.05, L=0.5; //geometric and dynamic parameters + Segment s1("S1", Joint("q1", Joint::RotZ), Frame(), RigidBodyInertia(m, Vector(L,0,0), RotationalInertia(0,0,Iz))); + Segment s2("S2", Joint("q2", Vector(L,0,0), Vector(0,0,1), Joint::RotAxis), Frame(Vector(L,0,0)), RigidBodyInertia(m, Vector(L/2,0,0), RotationalInertia(0,0,Iz))); + Segment s3("S3", Joint("q3", Vector(2*L,0,0), Vector(0,0,1), Joint::RotAxis), Frame(Vector(2*L,0,0)), RigidBodyInertia(m, Vector(L/2,0,0), RotationalInertia(0,0,Iz))); + ytree = Tree("root"); + ytree.addSegment(s1, "root"); + ytree.addSegment(s2, "S1"); + ytree.addSegment(s3, "S1"); +} + + +void TreeInvDynTest::tearDown() { } + + +void TreeInvDynTest::UpdateTreeTest() { + // std::cout << "Tree: " << endl << tree2str(tree) << endl; //NOTE: tree2str is not available as I implemented it in another "parallel" commit + std::cout << "\nTree: " << endl << tree << endl; + + Tree temp_tree(tree); + TreeIdSolver_RNE solver(temp_tree, Vector::Zero()); + JntArray q, qd, qdd, tau; + + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + q.resize(temp_tree.getNrOfJoints()); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + qd.resize(temp_tree.getNrOfJoints()); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + qdd.resize(temp_tree.getNrOfJoints()); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + tau.resize(temp_tree.getNrOfJoints()); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + + temp_tree.addSegment(Segment("extra"), temp_tree.getRootSegment()->first); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); + solver.updateInternalDataStructures(); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau)); +} + + +void TreeInvDynTest::TwoChainsTest() { + Vector gravity(0,0,-9.8); + TreeIdSolver_RNE solver(tree, gravity); + ChainIdSolver_RNE solver1(chain1, gravity); + ChainIdSolver_RNE solver2(chain2, gravity); + + unsigned int nt = tree.getNrOfJoints(); + unsigned int n1 = chain1.getNrOfJoints(); + unsigned int n2 = chain2.getNrOfJoints(); + + //Check that sizes are consistent -- otherwise following code does not make sense! + CPPUNIT_ASSERT_EQUAL(nt, n1+n2); + + JntArray q(nt), qd(nt), qdd(nt), tau(nt); //data related to tree + JntArray q1(n1), qd1(n1), qdd1(n1), tau1(n1); //data related to chain1 + JntArray q2(n2), qd2(n2), qdd2(n2), tau2(n2); //data related to chain2 + + unsigned int iterations = 100; + while(iterations-- > 0) { + //Randomize joint vectors + for(unsigned int i=0; i 0) { + //Randomize joint vectors + for(unsigned int i=0; i + +#include +#include + + +using namespace KDL; + +class TreeInvDynTest : public CppUnit::TestFixture +{ + CPPUNIT_TEST_SUITE(TreeInvDynTest); + CPPUNIT_TEST(UpdateTreeTest); + CPPUNIT_TEST(TwoChainsTest); + CPPUNIT_TEST(YTreeTest); + CPPUNIT_TEST_SUITE_END(); + +public: + void setUp(); + void tearDown(); + + void UpdateTreeTest(); + void TwoChainsTest(); + void YTreeTest(); + +private: + Chain chain1,chain2; + Tree tree, ytree; + double m, Iz, L; +}; +#endif From d600d489080e8c4fdaa48d0097fa19d9dd8288f6 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Tue, 8 Jan 2019 21:47:14 +0100 Subject: [PATCH 27/79] Recursive Newton-Euler inverse dynamics for trees --- TEMP.cpp | 53 ----------------------------------------------------- 1 file changed, 53 deletions(-) delete mode 100644 TEMP.cpp diff --git a/TEMP.cpp b/TEMP.cpp deleted file mode 100644 index 4f3ae5f17..000000000 --- a/TEMP.cpp +++ /dev/null @@ -1,53 +0,0 @@ -if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments()) - return (error = E_NOT_UP_TO_DATE); - -//Check sizes when in debug mode -if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns) - return (error = E_SIZE_MISMATCH); -unsigned int j=0; - -//Sweep from root to leaf -for(unsigned int i=0;i=0;i--){ - if(chain.getSegment(i).getJoint().getType()!=Joint::None){ - torques(j)=dot(S[i],f[i]); - torques(j)+=chain.getSegment(i).getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia - --j; - } - if(i!=0) - f[i-1]=f[i-1]+X[i]*f[i]; -} -return (error = E_NOERROR); From 847460ad5f6d767419d4cd52935c1b0878eacea1 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Wed, 9 Jan 2019 09:56:51 +0100 Subject: [PATCH 28/79] Newton-Euler inverse dynamics for trees (fixed missing gravity in test) --- orocos_kdl/tests/treeinvdyntest.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/tests/treeinvdyntest.cpp b/orocos_kdl/tests/treeinvdyntest.cpp index 07ccebfbe..5127bc20d 100644 --- a/orocos_kdl/tests/treeinvdyntest.cpp +++ b/orocos_kdl/tests/treeinvdyntest.cpp @@ -138,7 +138,8 @@ void TreeInvDynTest::TwoChainsTest() { void TreeInvDynTest::YTreeTest() { std::cout << "\nY-shaped tree: " << endl << ytree << endl; - TreeIdSolver_RNE solver(ytree, Vector(0,-9.8,0)); + double g = 9.8; + TreeIdSolver_RNE solver(ytree, Vector(0,-g,0)); //Following is just a check in case the model is modified. In this case the //analytic model derived using Euler-Lagrange equations would not be valid. @@ -175,4 +176,5 @@ void TreeInvDynTest::YTreeTest() { //compare efforts CPPUNIT_ASSERT_EQUAL(tau, eff); } + } From bf385afcfe62e563ddf74582f3d34566f0a55ce6 Mon Sep 17 00:00:00 2001 From: "Sean Yen [MSFT]" Date: Mon, 20 Aug 2018 15:17:26 -0700 Subject: [PATCH 29/79] Fix SIPMacros.cmake for Windows build. SIP takes .sip to generate C++ code to build native Python binding module, and SIP_CONCAT_PARTS decides how many C++ files to generate for parallel build. And since it doesn't know how many parts SIP actually created in Cmake configuration time, it emits command to generate the stub C++ files in case they are not there. The "touch" implementation seems to be broken on Windows. It is generating a file called "filename" under the current directory, instead of touching and creating the stub files. Instead of special handling the touch commands for Windows, replace the logic with a more generic "COMMAND ${CMAKE_COMMAND} -E touch ${_sip_output_files}" to be cross platform. --- python_orocos_kdl/cmake/SIPMacros.cmake | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/python_orocos_kdl/cmake/SIPMacros.cmake b/python_orocos_kdl/cmake/SIPMacros.cmake index 4464a9267..f54e704b0 100644 --- a/python_orocos_kdl/cmake/SIPMacros.cmake +++ b/python_orocos_kdl/cmake/SIPMacros.cmake @@ -92,20 +92,10 @@ MACRO(ADD_SIP_PYTHON_MODULE MODULE_NAME MODULE_SIP) ENDIF( ${CONCAT_NUM} LESS ${SIP_CONCAT_PARTS} ) ENDFOREACH(CONCAT_NUM RANGE 0 ${SIP_CONCAT_PARTS} ) - IF(NOT WIN32) - SET(TOUCH_COMMAND touch) - ELSE(NOT WIN32) - SET(TOUCH_COMMAND echo) - # instead of a touch command, give out the name and append to the files - # this is basically what the touch command does. - FOREACH(filename ${_sip_output_files}) - FILE(APPEND filename "") - ENDFOREACH(filename ${_sip_output_files}) - ENDIF(NOT WIN32) ADD_CUSTOM_COMMAND( OUTPUT ${_sip_output_files} COMMAND ${CMAKE_COMMAND} -E echo ${message} - COMMAND ${TOUCH_COMMAND} ${_sip_output_files} + COMMAND ${CMAKE_COMMAND} -E touch ${_sip_output_files} COMMAND ${SIP_EXECUTABLE} ${_sip_tags} ${_sip_x} ${SIP_EXTRA_OPTIONS} -j ${SIP_CONCAT_PARTS} -c ${CMAKE_CURRENT_SIP_OUTPUT_DIR} ${_sip_includes} ${_abs_module_sip} DEPENDS ${_abs_module_sip} ${SIP_EXTRA_FILES_DEPEND} ) From 4b2e3e6480745c82da4896f7b66e9fc8194c0903 Mon Sep 17 00:00:00 2001 From: imartini Date: Thu, 28 Feb 2019 14:33:37 +0100 Subject: [PATCH 30/79] Add private constructor for delegating initialization. Fixes #177. --- orocos_kdl/src/path_circle.cpp | 12 +++++++++--- orocos_kdl/src/path_circle.hpp | 10 ++++++++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/src/path_circle.cpp b/orocos_kdl/src/path_circle.cpp index 5efd22bc8..ad7e20c88 100644 --- a/orocos_kdl/src/path_circle.cpp +++ b/orocos_kdl/src/path_circle.cpp @@ -55,9 +55,7 @@ Path_Circle::Path_Circle(const Frame& F_base_start, RotationalInterpolation* _orient, double _eqradius, bool _aggregate) : - orient(_orient) , - eqradius(_eqradius), - aggregate(_aggregate) + Path_Circle(_orient, _eqradius, _aggregate) { F_base_center.p = _V_base_center; orient->SetStartEnd(F_base_start.M,R_base_end); @@ -91,6 +89,14 @@ Path_Circle::Path_Circle(const Frame& F_base_start, } +Path_Circle::Path_Circle(RotationalInterpolation* _orient, + double _eqradius, + bool _aggregate) : + orient(_orient) , + eqradius(_eqradius), + aggregate(_aggregate) + {} + double Path_Circle::LengthToS(double length) { return length/scalelin; diff --git a/orocos_kdl/src/path_circle.hpp b/orocos_kdl/src/path_circle.hpp index 36166cf99..f3123fe40 100644 --- a/orocos_kdl/src/path_circle.hpp +++ b/orocos_kdl/src/path_circle.hpp @@ -92,6 +92,16 @@ class Path_Circle : public Path double eqradius, bool _aggregate=true); + /** + * object initialization is delegated to this constructor such that destructor is called always + */ + private: + Path_Circle(RotationalInterpolation* otraj, + double eqradius, + bool _aggregate); + + public: + double LengthToS(double length); virtual double PathLength(); From e489875a9ca98baa5ed985549c1ff0d5475e18ad Mon Sep 17 00:00:00 2001 From: Ruben Smits Date: Mon, 4 Mar 2019 09:45:36 +0100 Subject: [PATCH 31/79] Revert "Add private constructor for delegating initialization. Fixes #177." --- orocos_kdl/src/path_circle.cpp | 12 +++--------- orocos_kdl/src/path_circle.hpp | 10 ---------- 2 files changed, 3 insertions(+), 19 deletions(-) diff --git a/orocos_kdl/src/path_circle.cpp b/orocos_kdl/src/path_circle.cpp index ad7e20c88..5efd22bc8 100644 --- a/orocos_kdl/src/path_circle.cpp +++ b/orocos_kdl/src/path_circle.cpp @@ -55,7 +55,9 @@ Path_Circle::Path_Circle(const Frame& F_base_start, RotationalInterpolation* _orient, double _eqradius, bool _aggregate) : - Path_Circle(_orient, _eqradius, _aggregate) + orient(_orient) , + eqradius(_eqradius), + aggregate(_aggregate) { F_base_center.p = _V_base_center; orient->SetStartEnd(F_base_start.M,R_base_end); @@ -89,14 +91,6 @@ Path_Circle::Path_Circle(const Frame& F_base_start, } -Path_Circle::Path_Circle(RotationalInterpolation* _orient, - double _eqradius, - bool _aggregate) : - orient(_orient) , - eqradius(_eqradius), - aggregate(_aggregate) - {} - double Path_Circle::LengthToS(double length) { return length/scalelin; diff --git a/orocos_kdl/src/path_circle.hpp b/orocos_kdl/src/path_circle.hpp index f3123fe40..36166cf99 100644 --- a/orocos_kdl/src/path_circle.hpp +++ b/orocos_kdl/src/path_circle.hpp @@ -92,16 +92,6 @@ class Path_Circle : public Path double eqradius, bool _aggregate=true); - /** - * object initialization is delegated to this constructor such that destructor is called always - */ - private: - Path_Circle(RotationalInterpolation* otraj, - double eqradius, - bool _aggregate); - - public: - double LengthToS(double length); virtual double PathLength(); From 69fd0972503d9d3b5f5b2b5a2f82c22aab2c6d6a Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Wed, 27 Mar 2019 15:42:48 +0100 Subject: [PATCH 32/79] Added #include --- orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp index 7103d9978..b4757f6dd 100644 --- a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp +++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp @@ -21,6 +21,7 @@ #include "treeidsolver_recursive_newton_euler.hpp" #include "frames_io.hpp" +#include namespace KDL{ From 6c29de10681cdf728abf4f2ca5e2d59e64977859 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Wed, 27 Mar 2019 16:35:30 +0100 Subject: [PATCH 33/79] Edit test --- orocos_kdl/tests/treeinvdyntest.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/orocos_kdl/tests/treeinvdyntest.cpp b/orocos_kdl/tests/treeinvdyntest.cpp index 5127bc20d..5001af8ca 100644 --- a/orocos_kdl/tests/treeinvdyntest.cpp +++ b/orocos_kdl/tests/treeinvdyntest.cpp @@ -144,10 +144,7 @@ void TreeInvDynTest::YTreeTest() { //Following is just a check in case the model is modified. In this case the //analytic model derived using Euler-Lagrange equations would not be valid. unsigned int dof = ytree.getNrOfJoints(); - if(dof != 3) - throw std::runtime_error("The analytic euler-lagrange model was derived for" - " a specific 3-dof model. The kinematic tree has" - " however " + std::to_string(dof) + " dof."); + CPPUNIT_ASSERT_EQUAL(dof, 3); // The analytic euler-lagrange model was derived for a specific 3-dof model JntArray q(dof), qd(dof), qdd(dof), tau(dof), eff(dof); From 59142178e63b3543a6d56078123cbd25afce30ad Mon Sep 17 00:00:00 2001 From: "luz.paz" Date: Thu, 2 May 2019 13:28:05 -0400 Subject: [PATCH 34/79] Misc doxy and source comment typo fixes Found via `codespell -q 3` --- orocos_kdl/doc/tex/UserManual.tex | 2 +- orocos_kdl/src/chainfksolver.hpp | 4 ++-- orocos_kdl/src/chainiksolvervel_pinv_nso.hpp | 2 +- orocos_kdl/src/path_roundedcomposite.cpp | 2 +- orocos_kdl/src/treefksolver.hpp | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/orocos_kdl/doc/tex/UserManual.tex b/orocos_kdl/doc/tex/UserManual.tex index 784b0ad1a..4accacc2a 100644 --- a/orocos_kdl/doc/tex/UserManual.tex +++ b/orocos_kdl/doc/tex/UserManual.tex @@ -120,7 +120,7 @@ \subsection{Vector} \paragraph{Add and subtract vectors} \label{sec:add-subtract-vectors} -You can add or substract a vector with another vector: +You can add or subtract a vector with another vector: \begin{lstlisting} v2+=v1; v3-=v1; diff --git a/orocos_kdl/src/chainfksolver.hpp b/orocos_kdl/src/chainfksolver.hpp index f82c20d13..5b1c40624 100644 --- a/orocos_kdl/src/chainfksolver.hpp +++ b/orocos_kdl/src/chainfksolver.hpp @@ -108,7 +108,7 @@ namespace KDL { class ChainFkSolverAcc : public KDL::SolverI { public: /** - * Calculate forward position, velocity and accelaration + * Calculate forward position, velocity and acceleration * kinematics, from joint coordinates to cartesian coordinates * * @param q_in input joint coordinates (position, velocity and @@ -120,7 +120,7 @@ namespace KDL { */ virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0; /** - * Calculate forward position, velocity and accelaration + * Calculate forward position, velocity and acceleration * kinematics, from joint coordinates to cartesian coordinates * * @param q_in input joint coordinates (position, velocity and diff --git a/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp b/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp index 0690bd4a2..be2c891f9 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp +++ b/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp @@ -121,7 +121,7 @@ namespace KDL virtual int setOptPos(const JntArray &opt_pos); /** - *Set null psace velocity gain + *Set null space velocity gain * *@param alpha NUllspace velocity cgain * diff --git a/orocos_kdl/src/path_roundedcomposite.cpp b/orocos_kdl/src/path_roundedcomposite.cpp index dbc67b05f..8d40491e7 100644 --- a/orocos_kdl/src/path_roundedcomposite.cpp +++ b/orocos_kdl/src/path_roundedcomposite.cpp @@ -85,7 +85,7 @@ void Path_RoundedComposite::Add(const Frame& F_base_point) { if (bcdist < eps) { throw Error_MotionPlanning_Not_Feasible(3); } - // Clamp to avoid rounding errors (acos is defineed between [-1 ; 1]) + // Clamp to avoid rounding errors (acos is defined between [-1 ; 1]) double alpha = acos(std::max(-1., std::min(dot(ab, bc) / abdist / bcdist, 1.))); if ((PI - alpha) < eps) { throw Error_MotionPlanning_Not_Feasible(4); diff --git a/orocos_kdl/src/treefksolver.hpp b/orocos_kdl/src/treefksolver.hpp index 3d521d340..80ca1fc70 100644 --- a/orocos_kdl/src/treefksolver.hpp +++ b/orocos_kdl/src/treefksolver.hpp @@ -89,7 +89,7 @@ namespace KDL { // class TreeFkSolverAcc { // public: /** - * Calculate forward position, velocity and accelaration + * Calculate forward position, velocity and acceleration * kinematics, from joint coordinates to cartesian coordinates * * @param q_in input joint coordinates (position, velocity and From ed3d25d3cd8212067d189e1329ca7b6f2c2cfab6 Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Tue, 1 May 2018 11:39:39 -0400 Subject: [PATCH 35/79] Generate forward dynamics solver for chain based on recursive Newton-Euler Method Created forward dynamics solver class based on inverse dynamics solver and inertia matrix from chaindynparam solver. In addition, a LDL solver function was added under the utilities to calculate the joint acceleration from the inertia matrix inverse times the joint torques. A 4th-order Runga-Kutta integrator was implemented for integrating the joint acceleration. --- orocos_kdl/src/chainfdsolver.hpp | 61 ++++++++ .../chainfdsolver_recursive_newton_euler.cpp | 138 ++++++++++++++++++ .../chainfdsolver_recursive_newton_euler.hpp | 111 ++++++++++++++ 3 files changed, 310 insertions(+) create mode 100644 orocos_kdl/src/chainfdsolver.hpp create mode 100644 orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp create mode 100644 orocos_kdl/src/chainfdsolver_recursive_newton_euler.hpp diff --git a/orocos_kdl/src/chainfdsolver.hpp b/orocos_kdl/src/chainfdsolver.hpp new file mode 100644 index 000000000..b542bb794 --- /dev/null +++ b/orocos_kdl/src/chainfdsolver.hpp @@ -0,0 +1,61 @@ +// Copyright (C) 2018 Ruben Smits + +// Version: 1.0 +// Author: Ruben Smits, Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_FDSOLVER_HPP +#define KDL_CHAIN_FDSOLVER_HPP + +#include "chain.hpp" +#include "frames.hpp" +#include "jntarray.hpp" +#include "solveri.hpp" + +namespace KDL +{ + + typedef std::vector Wrenches; + + /** + * \brief This abstract class encapsulates the inverse + * dynamics solver for a KDL::Chain. + * + */ + class ChainFdSolver : public KDL::SolverI + { + public: + /** + * Calculate forward dynamics from joint positions, joint velocities, joint torques/forces, + * and externally applied forces/torques to joint accelerations. + * + * @param q input joint positions + * @param q_dot input joint velocities + * @param torque input joint torques + * @param f_ext external forces + * + * @param q_dotdot output joint accelerations + * + * @return if < 0 something went wrong + */ + virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext,JntArray &q_dotdot)=0; + }; + +} + +#endif diff --git a/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp new file mode 100644 index 000000000..5da6c50bf --- /dev/null +++ b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp @@ -0,0 +1,138 @@ +// Copyright (C) 2018 Craig Carignan + +// Version: 1.0 +// Author: Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "chainfdsolver_recursive_newton_euler.hpp" +#include "utilities/ldl_solver_eigen.hpp" +#include "frames_io.hpp" +#include "kinfam_io.hpp" + +namespace KDL{ + + ChainFdSolver_RNE::ChainFdSolver_RNE(const Chain& _chain, Vector _grav): + chain(_chain), + DynSolver(chain, _grav), + IdSolver(chain, _grav), + nj(chain.getNrOfJoints()), + ns(chain.getNrOfSegments()), + H(nj), + Tzeroacc(nj), + H_eig(nj,nj), + Tzeroacc_eig(nj), + L_eig(nj,nj), + D_eig(nj), + r_eig(nj), + acc_eig(nj) + { + } + + void ChainFdSolver_RNE::updateInternalDataStructures() { + nj = chain.getNrOfJoints(); + ns = chain.getNrOfSegments(); + } + + int ChainFdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot) + { + if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments()) + return (error = E_NOT_UP_TO_DATE); + + //Check sizes of function parameters + if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns) + return (error = E_SIZE_MISMATCH); + + // Inverse Dynamics: + // T = H * qdd + Tcor + Tgrav - J^T * Fext + // Forward Dynamics: + // 1. Call ChainDynParam->JntToMass to get H + // 2. Call ChainIdSolver_RNE->CartToJnt with qdd=0 to get Tcor+Tgrav-J^T*Fext + // 3. Calculate qdd = H^-1 * T where T are applied joint torques minus non-inertial internal torques + + // Calculate Joint Space Inertia Matrix + error = DynSolver.JntToMass(q, H); + if (error < 0) + return (error); + + // Calculate non-inertial internal torques by inputting zero joint acceleration to ID + for(int i=0;i + +// Version: 1.0 +// Author: Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP +#define KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP + +#include "chainfdsolver.hpp" +#include "chainidsolver_recursive_newton_euler.hpp" +#include "chaindynparam.hpp" + +namespace KDL{ + /** + * \brief Recursive newton euler forward dynamics solver + * + * The algorithm implementation is based on the book "Rigid Body + * Dynamics Algorithms" of Roy Featherstone, 2008 + * (ISBN:978-0-387-74314-1) See Chapter 6 for basic algorithm. + * + * It calculates the accelerations for the joints (qdotdot), given the + * position and velocity of the joints (q,qdot,qdotdot), external forces + * on the segments (expressed in the segments reference frame), + * and the dynamical parameters of the segments. + */ + class ChainFdSolver_RNE : public ChainFdSolver{ + public: + /** + * Constructor for the solver, it will allocate all the necessary memory + * \param chain The kinematic chain to calculate the forward dynamics for, an internal copy will be made. + * \param grav The gravity vector to use during the calculation. + */ + ChainFdSolver_RNE(const Chain& chain, Vector grav); + ~ChainFdSolver_RNE(){}; + + /** + * Function to calculate from Cartesian forces to joint torques. + * Input parameters; + * \param q The current joint positions + * \param q_dot The current joint velocities + * \param torques The current joint torques (applied by controller) + * \param f_ext The external forces (no gravity) on the segments + * Output parameters: + * \param q_dotdot The resulting joint accelerations + */ + int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot); + + /// @copydoc KDL::SolverI::updateInternalDataStructures + virtual void updateInternalDataStructures(); + + /** + * Function to integrate the joint accelerations resulting from the forward dynamics solver. + * Input parameters; + * \param nj The number of joints + * \param t The current time + * \param dt The integration period + * \param q The current joint positions + * \param q_dot The current joint velocities + * \param torques The current joint torques (applied by controller) + * \param f_ext The external forces (no gravity) on the segments + * \param fdsolver The forward dynamics solver + * Output parameters: + * \param t The updated time + * \param q The updated joint positions + * \param q_dot The updated joint velocities + * \param q_dotdot The current joint accelerations + * \param dq The joint position increment + * \param dq_dot The joint velocity increment + * Temporary parameters: + * \param qtemp Intermediate joint positions + * \param qdtemp Intermediate joint velocities + */ + void RK4Integrator(int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot, + KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver, + KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot, + KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp); + + private: + const Chain& chain; + ChainDynParam DynSolver; + ChainIdSolver_RNE IdSolver; + unsigned int nj; + unsigned int ns; + JntArray Tzeroacc; + JntSpaceInertiaMatrix H; + Eigen::MatrixXd H_eig; + Eigen::VectorXd Tzeroacc_eig; + Eigen::MatrixXd L_eig; + Eigen::VectorXd D_eig; + Eigen::VectorXd r_eig; + Eigen::VectorXd acc_eig; + }; +} + +#endif From 9df416a9e665364b8f14f8c005e014f263fc836c Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Tue, 1 May 2018 12:02:45 -0400 Subject: [PATCH 36/79] Implement LDL solver for finding joint acceleration The joint acceleration is found by multiplying all of the joint torques by the inverse of the joint space mass matrix. Rather than explicitly calculating the inverse of the mass matrix, this can be calculated more efficiently by doing a LDL^T decomposition of the mass matrix and multiplying by the torques in one operation. This is similar to a Cholesky Decomposition but avoids the need for calculating a square root. Size checks were added for all of the matrices and vectors based on the row size of input square matrix A. Note that the upper triangular elements of L are set to 0 and the diagonal elements are set to 1 so that the output value of L is correct. The algorithm does not do this automatically because it is not required to compute the output vector. --- orocos_kdl/src/utilities/ldl_solver_eigen.cpp | 82 +++++++++++++++++++ orocos_kdl/src/utilities/ldl_solver_eigen.hpp | 59 +++++++++++++ 2 files changed, 141 insertions(+) create mode 100644 orocos_kdl/src/utilities/ldl_solver_eigen.cpp create mode 100644 orocos_kdl/src/utilities/ldl_solver_eigen.hpp diff --git a/orocos_kdl/src/utilities/ldl_solver_eigen.cpp b/orocos_kdl/src/utilities/ldl_solver_eigen.cpp new file mode 100644 index 000000000..8466c5ffa --- /dev/null +++ b/orocos_kdl/src/utilities/ldl_solver_eigen.cpp @@ -0,0 +1,82 @@ +// Copyright (C) 2018 Craig Carignan + +// Version: 1.0 +// Author: Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "ldl_solver_eigen.hpp" + +namespace KDL{ + + int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q) + { + const int n = A.rows(); + int error=SolverI::E_NOERROR; + + //Check sizes + if(A.cols()!=n || v.rows()!=n || L.rows()!=n || L.cols()!=n || D.rows()!=n || vtmp.rows()!=n || q.rows()!=n) + return (error = SolverI::E_SIZE_MISMATCH); + + for(int i=0;i0) { + for(int j=0;j<=i-1;++j) + D(i) -= D(j)*L(i,j)*L(i,j); + } + for(int j=1;ji) { + L(j,i)=A(i,j)/D(i); + if(i>0) { + for(int k=0;k<=i-1;++k) + L(j,i) -= L(j,k)*L(i,k)*D(k)/D(i); + } + } + } + } + for(int i=0;i0) { + for(int j=0;j<=i-1;++j) + vtmp(i) -= L(i,j)*vtmp(j); + } + } + for(int i=n-1;i>=0;--i) { + q(i)=vtmp(i)/D(i); + if(i 1 ) + { + for(int i=0;i + +// Version: 1.0 +// Author: Craig Carignan +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +// Inverse of a positive definite symmetric matrix times a vector +// based on LDL^T Decomposition +#ifndef LDL_SOLVER_EIGEN_HPP +#define LDL_SOLVER_EIGEN_HPP + + +#include +#include "../solveri.hpp" + +using namespace Eigen; + +namespace KDL +{ + /** + * \brief Solves the system of equations Aq = v for q via LDL decomposition, + * where A is a square positive definite matrix + * + * The algorithm factor A into the product of three matrices LDL^T, where L + * is a lower triangular matrix and D is a diagonal matrix. This allows q + * to be computed without explicity inverting A. Note that the LDL decomposition + * is a variant of the classical Cholesky Decomposition that does not require + * the computation of square roots. + * Input parameters: + * @param A matrix(nxn) + * @param v vector n + * @param vtmp vector n [temp variable] + * Output parameters: + * @param L matrix(nxn) + * @param D vector n + * @param q vector n + * @return 0 if successful, E_SIZE_MISMATCH if dimensions do not match + * References: + * https://en.wikipedia.org/wiki/Cholesky_decomposition + */ + int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q); +} +#endif From 52908c89365f00ccd82d52682d4d662f22ccd971 Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Wed, 2 Jan 2019 16:52:26 -0500 Subject: [PATCH 37/79] Add FD solver tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Added a “development” test for the FD solver that outlines how the new class is structured and does checks at each step. Also added a “consistency” test for the FD solver that takes the acceleration output of the FD solver and inputs it to the ID solver and checks whether the output torque agrees with the input of the FD solver. --- orocos_kdl/tests/solvertest.cpp | 300 ++++++++++++++++++++++++++++++++ orocos_kdl/tests/solvertest.hpp | 9 +- 2 files changed, 308 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 76b5eb43e..1702cbb37 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -119,6 +119,57 @@ void SolverTest::setUp() Frame::DH_Craig1989(0.0, -M_PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), Frame(Rotation::Identity(),Vector(0.0,0.0,0.155)))); + + // Motoman SIA10 Chain with Mass Parameters (for forward dynamics tests) + + // effective motor inertia is included as joint inertia + static const double scale=1; + static const double offset=0; + static const double inertiamotorA=5.0; // effective motor inertia kg-m^2 + static const double inertiamotorB=3.0; // effective motor inertia kg-m^2 + static const double inertiamotorC=1.0; // effective motor inertia kg-m^2 + static const double damping=0; + static const double stiffness=0; + + // Segment Inertias + KDL::RigidBodyInertia inert1(15.0, KDL::Vector(0.0, -0.02, 0.0), // mass, CM + KDL::RotationalInertia(0.1, 0.05, 0.1, 0.0, 0.0, 0.0)); // inertia + KDL::RigidBodyInertia inert2(5.0, KDL::Vector(0.0, -0.02, -0.1), + KDL::RotationalInertia(0.01, 0.1, 0.1, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert3(5.0, KDL::Vector(0.0, -0.05, 0.02), + KDL::RotationalInertia(0.05, 0.01, 0.05, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert4(3.0, KDL::Vector(0.0, 0.02, -0.15), + KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert5(3.0, KDL::Vector(0.0, -0.05, 0.01), + KDL::RotationalInertia(0.02, 0.01, 0.02, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert6(3.0, KDL::Vector(0.0, -0.01, -0.1), + KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0)); + KDL::RigidBodyInertia inert7(1.0, KDL::Vector(0.0, 0.0, 0.05), + KDL::RotationalInertia(0.01, 0.01, 0.1, 0.0, 0.0, 0.0)); + + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), + Frame::DH(0.0, M_PI_2, 0.36, 0.0), + inert1)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), + Frame::DH(0.0, -M_PI_2, 0.0, 0.0), + inert2)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), + Frame::DH(0.0, M_PI_2, 0.36, 0.0), + inert3)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), + Frame::DH(0.0, -M_PI_2, 0.0, 0.0), + inert4)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), + Frame::DH(0.0, M_PI_2, 0.36, 0.0), + inert5)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), + Frame::DH(0.0, -M_PI_2, 0.0, 0.0), + inert6)); + motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), + Frame::DH(0.0, 0.0, 0.0, 0.0), + inert7)); + motomansia10dyn.addSegment(Segment(Joint(Joint::None), + Frame(Rotation::Identity(),Vector(0.0,0.0,0.155)))); } void SolverTest::tearDown() @@ -847,3 +898,252 @@ void SolverTest::FkVelVectTest() CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5)); } + +void SolverTest::FdSolverDevelopmentTest() +{ + int ret; + double eps=1.e-3; + + std::cout<<"KDL FD Solver Development Test for Motoman SIA10"< #include #include +#include +#include +#include using namespace KDL; @@ -34,6 +37,8 @@ class SolverTest : public CppUnit::TestFixture CPPUNIT_TEST(IkVelSolverWDLSTest ); CPPUNIT_TEST(FkPosVectTest ); CPPUNIT_TEST(FkVelVectTest ); + CPPUNIT_TEST(FdSolverDevelopmentTest ); + CPPUNIT_TEST(FdSolverConsistencyTest ); CPPUNIT_TEST(UpdateChainTest ); CPPUNIT_TEST_SUITE_END(); @@ -50,11 +55,13 @@ class SolverTest : public CppUnit::TestFixture void IkVelSolverWDLSTest(); void FkPosVectTest(); void FkVelVectTest(); + void FdSolverDevelopmentTest(); + void FdSolverConsistencyTest(); void UpdateChainTest(); private: - Chain chain1,chain2,chain3,chain4, chaindyn,motomansia10; + Chain chain1, chain2, chain3, chain4, chaindyn, motomansia10, motomansia10dyn; void FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver); void FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver); From 99ecf37c108966d5cfe8406b7226dafdac1ea89a Mon Sep 17 00:00:00 2001 From: ccarigna Date: Tue, 16 Apr 2019 14:49:43 -0400 Subject: [PATCH 38/79] Add LDL solver test --- orocos_kdl/tests/solvertest.cpp | 56 +++++++++++++++++++++++++++++++++ orocos_kdl/tests/solvertest.hpp | 2 ++ 2 files changed, 58 insertions(+) diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 1702cbb37..de393241b 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -1147,3 +1147,59 @@ void SolverTest::FdSolverConsistencyTest() return; } + +void SolverTest::LDLdecompTest() +{ + std::cout<<"LDL Solver Test"< Date: Fri, 24 May 2019 17:05:04 +0200 Subject: [PATCH 39/79] fix LDL solver test (copy-paste error) --- orocos_kdl/tests/solvertest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index de393241b..e0b3049af 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -1190,7 +1190,7 @@ void SolverTest::LDLdecompTest() // Verify solution for x for(int i=0;i<3;i++){ - xout(i,i) = x(i); + CPPUNIT_ASSERT(Equal(xout(i), x(i), eps)); } // Test reconstruction of A from LDL^T decomposition From d00599973997d21fbd0793b358bcb12b0bf6fc90 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Mon, 27 May 2019 16:23:17 +0200 Subject: [PATCH 40/79] fix build for missing isnan definition in newer compilers --- orocos_kdl/src/utilities/utility.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/src/utilities/utility.h b/orocos_kdl/src/utilities/utility.h index e2febdaf0..83266ed48 100644 --- a/orocos_kdl/src/utilities/utility.h +++ b/orocos_kdl/src/utilities/utility.h @@ -127,9 +127,9 @@ namespace KDL { } #endif - - - +#if (__cplusplus > 199711L) +using std::isnan; +#endif /** * Auxiliary class for argument types (Trait-template class ) From f89f89b05b1aac59a6e632269c80a9e55cc2d6ba Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Tue, 28 May 2019 14:17:36 +0200 Subject: [PATCH 41/79] utilities: replace deprecated auto_ptr with a basic smart pointer scoped_ptr (supports C++98) --- orocos_kdl/src/path.cpp | 13 ++-- orocos_kdl/src/path_composite.cpp | 3 +- orocos_kdl/src/path_roundedcomposite.cpp | 5 +- orocos_kdl/src/rotational_interpolation.cpp | 1 - orocos_kdl/src/trajectory.cpp | 6 +- orocos_kdl/src/utilities/scoped_ptr.hpp | 78 +++++++++++++++++++++ 6 files changed, 93 insertions(+), 13 deletions(-) create mode 100644 orocos_kdl/src/utilities/scoped_ptr.hpp diff --git a/orocos_kdl/src/path.cpp b/orocos_kdl/src/path.cpp index 9b14b0813..b22f2ab93 100644 --- a/orocos_kdl/src/path.cpp +++ b/orocos_kdl/src/path.cpp @@ -43,6 +43,7 @@ #include "utilities/error.h" #include "utilities/error_stack.h" +#include "utilities/scoped_ptr.hpp" #include "path.hpp" #include "path_line.hpp" #include "path_point.hpp" @@ -78,7 +79,7 @@ Path* Path::Read(istream& is) { Frame endpos; is >> startpos; is >> endpos; - auto_ptr orient( RotationalInterpolation::Read(is) ); + scoped_ptr orient( RotationalInterpolation::Read(is) ); double eqradius; is >> eqradius; EatEnd(is,']'); @@ -99,7 +100,7 @@ Path* Path::Read(istream& is) { is >> R_base_end; is >> alpha; alpha *= deg2rad; - auto_ptr orient( RotationalInterpolation::Read(is) ); + scoped_ptr orient( RotationalInterpolation::Read(is) ); is >> eqradius; EatEnd(is,']'); IOTracePop(); @@ -119,8 +120,8 @@ Path* Path::Read(istream& is) { is >> radius; double eqradius; is >> eqradius; - auto_ptr orient( RotationalInterpolation::Read(is) ); - auto_ptr tr( + scoped_ptr orient( RotationalInterpolation::Read(is) ); + scoped_ptr tr( new Path_RoundedComposite(radius,eqradius,orient.release()) ); int size; @@ -139,7 +140,7 @@ Path* Path::Read(istream& is) { } else if (strcmp(storage,"COMPOSITE")==0) { IOTrace("COMPOSITE"); int size; - auto_ptr tr( new Path_Composite() ); + scoped_ptr tr( new Path_Composite() ); is >> size; int i; for (i=0;i tr( Path::Read(is) ); + scoped_ptr tr( Path::Read(is) ); is >> times; EatEnd(is,']'); IOTracePop(); diff --git a/orocos_kdl/src/path_composite.cpp b/orocos_kdl/src/path_composite.cpp index 4df5f3929..1901c871b 100644 --- a/orocos_kdl/src/path_composite.cpp +++ b/orocos_kdl/src/path_composite.cpp @@ -42,6 +42,7 @@ #include "path_composite.hpp" #include "utilities/error.h" +#include "utilities/scoped_ptr.hpp" #include namespace KDL { @@ -110,7 +111,7 @@ Twist Path_Composite::Acc(double s,double sd,double sdd) const { } Path* Path_Composite::Clone() { - std::auto_ptr comp( new Path_Composite() ); + scoped_ptr comp( new Path_Composite() ); for (unsigned int i = 0; i < dv.size(); ++i) { comp->Add(gv[i].first->Clone(), gv[i].second); } diff --git a/orocos_kdl/src/path_roundedcomposite.cpp b/orocos_kdl/src/path_roundedcomposite.cpp index dbc67b05f..4bb366311 100644 --- a/orocos_kdl/src/path_roundedcomposite.cpp +++ b/orocos_kdl/src/path_roundedcomposite.cpp @@ -44,6 +44,7 @@ #include "path_line.hpp" #include "path_circle.hpp" #include "utilities/error.h" +#include "utilities/scoped_ptr.hpp" #include @@ -105,11 +106,11 @@ void Path_RoundedComposite::Add(const Frame& F_base_point) { if (d >= bcdist) throw Error_MotionPlanning_Not_Feasible(6); - std::auto_ptr < Path + scoped_ptr < Path > line1( new Path_Line(F_base_start, F_base_via, orient->Clone(), eqradius)); - std::auto_ptr < Path + scoped_ptr < Path > line2( new Path_Line(F_base_via, F_base_point, orient->Clone(), eqradius)); diff --git a/orocos_kdl/src/rotational_interpolation.cpp b/orocos_kdl/src/rotational_interpolation.cpp index f8186e67a..bb388c53d 100644 --- a/orocos_kdl/src/rotational_interpolation.cpp +++ b/orocos_kdl/src/rotational_interpolation.cpp @@ -51,7 +51,6 @@ namespace KDL { using namespace std; RotationalInterpolation* RotationalInterpolation::Read(istream& is) { - // auto_ptr because exception can be thrown ! IOTrace("RotationalInterpolation::Read"); char storage[64]; EatWord(is,"[",storage,sizeof(storage)); diff --git a/orocos_kdl/src/trajectory.cpp b/orocos_kdl/src/trajectory.cpp index 6a3e78639..d7e66ca0e 100644 --- a/orocos_kdl/src/trajectory.cpp +++ b/orocos_kdl/src/trajectory.cpp @@ -42,6 +42,7 @@ #include "utilities/error.h" #include "utilities/error_stack.h" +#include "utilities/scoped_ptr.hpp" #include "trajectory.hpp" #include "path.hpp" #include "trajectory_segment.hpp" @@ -55,15 +56,14 @@ namespace KDL { using namespace std; Trajectory* Trajectory::Read(std::istream& is) { - // auto_ptr because exception can be thrown ! IOTrace("Trajectory::Read"); char storage[64]; EatWord(is,"[",storage,sizeof(storage)); Eat(is,'['); if (strcmp(storage,"SEGMENT")==0) { IOTrace("SEGMENT"); - auto_ptr geom( Path::Read(is) ); - auto_ptr motprof( VelocityProfile::Read(is) ); + scoped_ptr geom( Path::Read(is) ); + scoped_ptr motprof( VelocityProfile::Read(is) ); EatEnd(is,']'); IOTracePop(); IOTracePop(); diff --git a/orocos_kdl/src/utilities/scoped_ptr.hpp b/orocos_kdl/src/utilities/scoped_ptr.hpp new file mode 100644 index 000000000..b35b1e532 --- /dev/null +++ b/orocos_kdl/src/utilities/scoped_ptr.hpp @@ -0,0 +1,78 @@ +/*************************************************************************** + scoped_ptr - a not too smart pointer for exception safe heap objects + ------------------------- + begin : May 2019 + copyright : (C) 2019 Intermodalics + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library 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 * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + +#if (__cplusplus > 199711L) +#include +#else +#include +#endif + +namespace KDL { + +template class scoped_ptr; + +template void swap(scoped_ptr&, scoped_ptr&); + +template +class scoped_ptr { + public: + scoped_ptr() : ptr_(0) { } + explicit scoped_ptr(T* p) : ptr_(p) { } + + ~scoped_ptr() { delete ptr_; } + + T* operator->() { return ptr_; } + const T* operator->() const { return ptr_; } + + T* get() const { return ptr_; } + + void reset(T* p = 0) { + T* old = ptr_; + ptr_ = p; + delete old; + } + + T* release() { + T* old = ptr_; + ptr_ = 0; + return old; + } + + friend void swap<>(scoped_ptr& a, scoped_ptr& b); + + private: + scoped_ptr(const scoped_ptr&); // not-copyable + scoped_ptr& operator=(const scoped_ptr&); // not-copyable + + T* ptr_; +}; + +template +void swap(scoped_ptr& a, scoped_ptr& b) { + using std::swap; + swap(a.ptr_, b.ptr_); +} + + +} // namespace KDL \ No newline at end of file From 6c07d3775645416afb41e95899ffdb7c953a2118 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Tue, 28 May 2019 17:27:13 +0200 Subject: [PATCH 42/79] miscellaneous warning fixes --- orocos_kdl/src/chainiksolvervel_pinv.cpp | 2 +- orocos_kdl/src/chainiksolvervel_pinv_givens.cpp | 3 +-- orocos_kdl/src/frames.cpp | 2 -- orocos_kdl/src/path_composite.cpp | 6 +++--- orocos_kdl/tests/jacobiandottest.cpp | 8 ++++---- orocos_kdl/tests/solvertest.cpp | 1 - 6 files changed, 9 insertions(+), 13 deletions(-) diff --git a/orocos_kdl/src/chainiksolvervel_pinv.cpp b/orocos_kdl/src/chainiksolvervel_pinv.cpp index 8b5d70ec6..4ecc0cc49 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.cpp @@ -25,8 +25,8 @@ namespace KDL { ChainIkSolverVel_pinv::ChainIkSolverVel_pinv(const Chain& _chain,double _eps,int _maxiter): chain(_chain), - nj(chain.getNrOfJoints()), jnt2jac(chain), + nj(chain.getNrOfJoints()), jac(nj), svd(jac), U(6,JntArray(nj)), diff --git a/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp b/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp index e5ab28408..dd2d9ab8f 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp @@ -93,8 +93,7 @@ namespace KDL else jac_eigen(i,j)=jac(i,j); } - int ret = svd_eigen_Macie(jac_eigen,U,S,V,B,tempi,1e-15,toggle); - //std::cout<<"# sweeps: "< yy) && (xx > zz)) { // data[0] is the largest diagonal term diff --git a/orocos_kdl/src/path_composite.cpp b/orocos_kdl/src/path_composite.cpp index 1901c871b..c93f5a293 100644 --- a/orocos_kdl/src/path_composite.cpp +++ b/orocos_kdl/src/path_composite.cpp @@ -128,18 +128,18 @@ void Path_Composite::Write(std::ostream& os) { } int Path_Composite::GetNrOfSegments() { - return dv.size(); + return static_cast(dv.size()); } Path* Path_Composite::GetSegment(int i) { assert(i>=0); - assert(i(dv.size())); return gv[i].first; } double Path_Composite::GetLengthToEndOfSegment(int i) { assert(i>=0); - assert(i(dv.size())); return dv[i]; } diff --git a/orocos_kdl/tests/jacobiandottest.cpp b/orocos_kdl/tests/jacobiandottest.cpp index fd86d896e..97de6d9ec 100644 --- a/orocos_kdl/tests/jacobiandottest.cpp +++ b/orocos_kdl/tests/jacobiandottest.cpp @@ -113,8 +113,8 @@ void Jdot_diff(const Jacobian& J_q, { assert(J_q.columns() == J_qdt.columns()); assert(J_q.columns() == Jdot.columns()); - for(int l=0;l<6;l++) - for(int c=0;c Date: Wed, 29 May 2019 14:26:26 -0400 Subject: [PATCH 43/79] Misc. source comment typo and whitespace fixes --- orocos_kdl/src/chainjnttojacdotsolver.hpp | 42 +++++++++++------------ 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/orocos_kdl/src/chainjnttojacdotsolver.hpp b/orocos_kdl/src/chainjnttojacdotsolver.hpp index 675a36070..b2d74d2fc 100644 --- a/orocos_kdl/src/chainjnttojacdotsolver.hpp +++ b/orocos_kdl/src/chainjnttojacdotsolver.hpp @@ -56,23 +56,23 @@ class ChainJntToJacDotSolver : public SolverI static const int HYBRID = 0; // Body-fixed representation ref Frame: end-effector, ref Point: end-effector static const int BODYFIXED = 1; - // Intertial representation ref Frame: base, ref Point: base + // Inertial representation ref Frame: base, ref Point: base static const int INERTIAL = 2; explicit ChainJntToJacDotSolver(const Chain& chain); virtual ~ChainJntToJacDotSolver(); /** * @brief Computes \f$ {}_{bs}\dot{J}^{ee}.\dot{q} \f$ - * + * * @param q_in Current joint positions and velocities * @param jac_dot_q_dot The twist representing Jdot*qdot * @param seg_nr The final segment to compute - * @return int 0 if no errors happened + * @return int 0 if no errors happened */ virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1); /** - * @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$ - * + * @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$ + * * @param q_in Current joint positions and velocities * @param jdot The jacobian time derivative in the configured representation * (HYBRID, BODYFIXED or INERTIAL) @@ -81,34 +81,34 @@ class ChainJntToJacDotSolver : public SolverI */ virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1); int setLockedJoints(const std::vector& locked_joints); - + /** * @brief JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector) - * - * + * + * * @return void */ void setHybridRepresentation(){setRepresentation(HYBRID);} /** * @brief JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector) - * + * * @return void */ void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);} /** * @brief JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base) - * + * * @return void */ void setInertialRepresentation(){setRepresentation(INERTIAL);} /** * @brief Sets the internal variable for the representation (with a check on the value) - * + * * @param representation The representation for Jdot : HYBRID,BODYFIXED or INERTIAL * @return void */ void setRepresentation(const int& representation); - + /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); @@ -119,7 +119,7 @@ class ChainJntToJacDotSolver : public SolverI protected: /** * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$ - * + * * @param bs_J_ee The Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver) * @param joint_idx The index of the current joint (j in the formula) * @param column_idx The index of the current column (i in the formula) @@ -130,41 +130,41 @@ class ChainJntToJacDotSolver : public SolverI const unsigned int& column_idx); /** * @brief Computes \f$ \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$ - * + * * @param bs_J_ee The Jacobian expressed in the end effector frame with the end effector as the reference point * @param joint_idx The indice of the current joint (j in the formula) * @param column_idx The indice of the current column (i in the formula) * @return Twist The twist representing dJi/dqj .qdotj - */ + */ const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee, const unsigned int& joint_idx, const unsigned int& column_idx); /** * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \f$ - * + * * @param ee_J_ee The Jacobian expressed in the base frame with the base as the reference point * @param joint_idx The indice of the current joint (j in the formula) * @param column_idx The indice of the current column (i in the formula) * @return Twist The twist representing dJi/dqj .qdotj - */ + */ const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs, const unsigned int& joint_idx, const unsigned int& column_idx); /** * @brief Computes \f$ \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$ - * + * * @param bs_J_bs The Jacobian expressed in the base frame with the end effector as the reference point * @param joint_idx The indice of the current joint (j in the formula) * @param column_idx The indice of the current column (i in the formula) - * @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj + * @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj * @return Twist The twist representing dJi/dqj .qdotj - */ + */ const Twist& getPartialDerivative(const Jacobian& J, const unsigned int& joint_idx, const unsigned int& column_idx, const int& representation); private: - + const Chain& chain; std::vector locked_joints_; unsigned int nr_of_unlocked_joints_; From 6e8e85af2e8691e77a850cb825f53d43798629d2 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Fri, 31 May 2019 10:36:32 +0200 Subject: [PATCH 44/79] scoped_ptr: add missing newline at end of file --- orocos_kdl/src/utilities/scoped_ptr.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/utilities/scoped_ptr.hpp b/orocos_kdl/src/utilities/scoped_ptr.hpp index b35b1e532..a7f6f73d4 100644 --- a/orocos_kdl/src/utilities/scoped_ptr.hpp +++ b/orocos_kdl/src/utilities/scoped_ptr.hpp @@ -75,4 +75,4 @@ void swap(scoped_ptr& a, scoped_ptr& b) { } -} // namespace KDL \ No newline at end of file +} // namespace KDL From 54d12f67062c510d998bff481f59b239b0a32190 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Thu, 6 Jun 2019 16:27:11 +0200 Subject: [PATCH 45/79] Cast to uint in comparison test --- orocos_kdl/tests/treeinvdyntest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/tests/treeinvdyntest.cpp b/orocos_kdl/tests/treeinvdyntest.cpp index 5001af8ca..c6f5b9cf8 100644 --- a/orocos_kdl/tests/treeinvdyntest.cpp +++ b/orocos_kdl/tests/treeinvdyntest.cpp @@ -144,7 +144,7 @@ void TreeInvDynTest::YTreeTest() { //Following is just a check in case the model is modified. In this case the //analytic model derived using Euler-Lagrange equations would not be valid. unsigned int dof = ytree.getNrOfJoints(); - CPPUNIT_ASSERT_EQUAL(dof, 3); // The analytic euler-lagrange model was derived for a specific 3-dof model + CPPUNIT_ASSERT_EQUAL(dof, (unsigned int)3); // The analytic euler-lagrange model was derived for a specific 3-dof model JntArray q(dof), qd(dof), qdd(dof), tau(dof), eff(dof); From 64d6d9c705484c6b0359f39b4182e672c44c8cd7 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 6 May 2020 22:55:44 +0200 Subject: [PATCH 46/79] Merge pull request #229 from MatthijsBurgh/pybind11 PyKDL: PyBind11 based --- .gitmodules | 4 + .travis.yml | 64 +++ orocos_kdl/CMakeLists.txt | 12 +- .../CheckSTLContainers.cmake | 0 orocos_kdl/{config => cmake}/FindEigen3.cmake | 0 .../{config => cmake}/FindPkgConfig.cmake | 0 orocos_kdl/config/DependentOption.cmake | 37 -- orocos_kdl/package.xml | 14 +- orocos_kdl/src/CMakeLists.txt | 46 -- orocos_kdl/src/chain.cpp | 8 +- orocos_kdl/src/chaindynparam.cpp | 33 +- orocos_kdl/src/chainfksolverpos_recursive.cpp | 16 +- orocos_kdl/src/chainfksolvervel_recursive.cpp | 16 +- .../chainidsolver_recursive_newton_euler.cpp | 23 +- orocos_kdl/src/chainidsolver_vereshchagin.cpp | 14 +- orocos_kdl/src/chainiksolverpos_lma.cpp | 8 +- orocos_kdl/src/chainjnttojacdotsolver.cpp | 24 +- orocos_kdl/src/chainjnttojacsolver.cpp | 10 +- orocos_kdl/src/frameacc.hpp | 2 +- orocos_kdl/src/frameacc.inl | 4 +- orocos_kdl/src/frames.cpp | 16 +- orocos_kdl/src/frames.hpp | 14 +- orocos_kdl/src/framevel.hpp | 31 +- orocos_kdl/src/framevel.inl | 157 ++++- orocos_kdl/src/framevel_io.hpp | 1 - orocos_kdl/src/jacobian.cpp | 25 +- orocos_kdl/src/joint.cpp | 16 +- orocos_kdl/src/joint.hpp | 42 +- orocos_kdl/src/segment.hpp | 16 +- orocos_kdl/src/tree.cpp | 10 +- .../treeidsolver_recursive_newton_euler.cpp | 139 +++++ orocos_kdl/src/treejnttojacsolver.cpp | 2 +- orocos_kdl/src/utilities/rall1d.h | 18 +- orocos_kdl/src/utilities/rall1d_io.h | 3 +- orocos_kdl/src/utilities/rall2d.h | 20 +- orocos_kdl/src/utilities/rall2d_io.h | 4 +- orocos_kdl/tests/CMakeLists.txt | 23 - python_orocos_kdl/CMakeLists.txt | 55 +- python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp | 39 ++ python_orocos_kdl/PyKDL/pybind11/PyKDL.h | 34 ++ python_orocos_kdl/PyKDL/pybind11/dynamics.cpp | 92 +++ python_orocos_kdl/PyKDL/pybind11/frames.cpp | 451 +++++++++++++++ python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 380 ++++++++++++ python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 486 ++++++++++++++++ python_orocos_kdl/PyKDL/{ => sip}/PyKDL.sip | 22 +- .../PyKDL/{ => sip}/dynamics.sip | 43 +- python_orocos_kdl/PyKDL/{ => sip}/frames.sip | 117 ++-- .../PyKDL/{ => sip}/framevel.sip | 127 +++- python_orocos_kdl/PyKDL/{ => sip}/kinfam.sip | 330 +++++++---- .../PyKDL/{ => sip}/std_string.sip | 18 +- python_orocos_kdl/PyKDL/sip/std_vector.sip | 353 ++++++++++++ python_orocos_kdl/cmake/SIPMacros.cmake | 4 +- python_orocos_kdl/mainpage.dox | 26 - python_orocos_kdl/manifest.xml | 22 - python_orocos_kdl/package.xml | 20 +- python_orocos_kdl/pybind11 | 1 + python_orocos_kdl/rosdoc.yaml | 3 +- python_orocos_kdl/tests/PyKDLtest.py | 23 +- python_orocos_kdl/tests/dynamicstest.py | 75 +++ python_orocos_kdl/tests/framestest.py | 541 +++++++++++++----- python_orocos_kdl/tests/frameveltest.py | 342 ++++++++--- python_orocos_kdl/tests/jointtypetest.py | 48 ++ python_orocos_kdl/tests/kinfamtest.py | 287 +++++++--- 63 files changed, 3934 insertions(+), 877 deletions(-) create mode 100644 .gitmodules create mode 100644 .travis.yml rename orocos_kdl/{config => cmake}/CheckSTLContainers.cmake (100%) rename orocos_kdl/{config => cmake}/FindEigen3.cmake (100%) rename orocos_kdl/{config => cmake}/FindPkgConfig.cmake (100%) delete mode 100644 orocos_kdl/config/DependentOption.cmake create mode 100644 orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp create mode 100644 python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp create mode 100644 python_orocos_kdl/PyKDL/pybind11/PyKDL.h create mode 100644 python_orocos_kdl/PyKDL/pybind11/dynamics.cpp create mode 100644 python_orocos_kdl/PyKDL/pybind11/frames.cpp create mode 100644 python_orocos_kdl/PyKDL/pybind11/framevel.cpp create mode 100644 python_orocos_kdl/PyKDL/pybind11/kinfam.cpp rename python_orocos_kdl/PyKDL/{ => sip}/PyKDL.sip (63%) rename python_orocos_kdl/PyKDL/{ => sip}/dynamics.sip (72%) rename python_orocos_kdl/PyKDL/{ => sip}/frames.sip (89%) rename python_orocos_kdl/PyKDL/{ => sip}/framevel.sip (73%) rename python_orocos_kdl/PyKDL/{ => sip}/kinfam.sip (66%) rename python_orocos_kdl/PyKDL/{ => sip}/std_string.sip (78%) create mode 100644 python_orocos_kdl/PyKDL/sip/std_vector.sip delete mode 100644 python_orocos_kdl/mainpage.dox delete mode 100644 python_orocos_kdl/manifest.xml create mode 160000 python_orocos_kdl/pybind11 create mode 100644 python_orocos_kdl/tests/dynamicstest.py create mode 100644 python_orocos_kdl/tests/jointtypetest.py diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..36b541a9c --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "python_orocos_kdl/pybind11"] + path = python_orocos_kdl/pybind11 + url = https://github.com/pybind/pybind11.git + branch = v2.4.3 diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 000000000..dad4a432c --- /dev/null +++ b/.travis.yml @@ -0,0 +1,64 @@ +language: cpp +os: linux +dist: xenial + +compiler: + - gcc + - clang + +env: + - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=OFF + - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=ON + - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=OFF + - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=ON + +before_script: + - sudo apt-get -qq update + - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python3-sip-dev python-psutil python3-psutil python-future python3-future + # build orocos_kdl + - cd orocos_kdl + - mkdir build + - cd build + - cmake -DENABLE_TESTS:BOOL=ON -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. + # compile and install orocos_kdl + - make + - sudo make install + - cd ../.. + + # build python bindings python 2 + - cd python_orocos_kdl + - mkdir build2 + - cd build2 + - export ROS_PYTHON_VERSION=2 + - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} -DBUILD_PYKDL_PYBIND11=${BUILD_PYKDL_PYBIND11} ./.. + # compile and install python bindings python 2 + - make + - sudo make install + - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib + - sudo ldconfig + - cd ../.. + + # build python bindings python 3 + - cd python_orocos_kdl + - mkdir build3 + - cd build3 + - export ROS_PYTHON_VERSION=3 + - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} -DBUILD_PYKDL_PYBIND11=${BUILD_PYKDL_PYBIND11} ./.. + # compile and install python bindings python 3 + - make + - sudo make install + - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib + - sudo ldconfig + - cd ../.. + +script: + # execute orocos_kdl tests + - cd orocos_kdl/build + - make check + - cd ../.. + # execute python bindings tests + - cd python_orocos_kdl + # python 2 + - python2 tests/PyKDLtest.py + # python 3 + - python3 tests/PyKDLtest.py diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index c323e145a..86f3574b5 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -44,14 +44,14 @@ SET( KDL_CFLAGS "") find_package(Eigen 3 QUIET) if(NOT Eigen_FOUND) - include(${PROJ_SOURCE_DIR}/config/FindEigen3.cmake) + include(${PROJ_SOURCE_DIR}/cmake/FindEigen3.cmake) set(Eigen_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}") endif() include_directories(${Eigen_INCLUDE_DIR}) SET(KDL_CFLAGS "${KDL_CFLAGS} -I${Eigen_INCLUDE_DIR}") # Check the platform STL containers capabilities -include(config/CheckSTLContainers.cmake) +include(cmake/CheckSTLContainers.cmake) CHECK_STL_CONTAINERS() # Set the default option appropriately @@ -64,6 +64,12 @@ endif(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) # Allow the user to select the Tree API version to use set(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface") +#Sanity check, inform the user +if(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE) + message(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of " + "incomplete types, this configuration is likely invalid") +endif() + # The new interface requires the use of shared pointers if(KDL_USE_NEW_TREE_INTERFACE) # We need shared_ptr from boost since not all compilers are c++11 capable @@ -72,8 +78,6 @@ if(KDL_USE_NEW_TREE_INTERFACE) set(KDL_INCLUDE_DIRS ${Boost_INCLUDE_DIRS}) endif(KDL_USE_NEW_TREE_INTERFACE) -INCLUDE (${PROJ_SOURCE_DIR}/config/DependentOption.cmake) - OPTION(ENABLE_TESTS OFF "Enable building of tests") IF( ENABLE_TESTS ) # If not in standard paths, set CMAKE_xxx_PATH's in environment, eg. diff --git a/orocos_kdl/config/CheckSTLContainers.cmake b/orocos_kdl/cmake/CheckSTLContainers.cmake similarity index 100% rename from orocos_kdl/config/CheckSTLContainers.cmake rename to orocos_kdl/cmake/CheckSTLContainers.cmake diff --git a/orocos_kdl/config/FindEigen3.cmake b/orocos_kdl/cmake/FindEigen3.cmake similarity index 100% rename from orocos_kdl/config/FindEigen3.cmake rename to orocos_kdl/cmake/FindEigen3.cmake diff --git a/orocos_kdl/config/FindPkgConfig.cmake b/orocos_kdl/cmake/FindPkgConfig.cmake similarity index 100% rename from orocos_kdl/config/FindPkgConfig.cmake rename to orocos_kdl/cmake/FindPkgConfig.cmake diff --git a/orocos_kdl/config/DependentOption.cmake b/orocos_kdl/config/DependentOption.cmake deleted file mode 100644 index 4ec4a6e1b..000000000 --- a/orocos_kdl/config/DependentOption.cmake +++ /dev/null @@ -1,37 +0,0 @@ -# - Macro to provide an option dependent on other options. -# This macro presents an option to the user only if a set of other -# conditions are true. When the option is not presented a default -# value is used, but any value set by the user is preserved for when -# the option is presented again. -# Example invocation: -# DEPENDENT_OPTION(USE_FOO "Use Foo" ON -# "USE_BAR;NOT USE_ZOT" OFF) -# If USE_BAR is true and USE_ZOT is false, this provides an option called -# USE_FOO that defaults to ON. Otherwise, it sets USE_FOO to OFF. If -# the status of USE_BAR or USE_ZOT ever changes, any value for the -# USE_FOO option is saved so that when the option is re-enabled it -# retains its old value. -MACRO(DEPENDENT_OPTION option doc default depends force) - IF(${option}_ISSET MATCHES "^${option}_ISSET$") - SET(${option}_AVAILABLE 1) - FOREACH(d ${depends}) - STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") - IF(${CMAKE_DEPENDENT_OPTION_DEP}) - ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) - SET(${option}_AVAILABLE 0) - ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) - ENDFOREACH(d) - IF(${option}_AVAILABLE) - OPTION(${option} "${doc}" "${default}") - SET(${option} "${${option}}" CACHE BOOL "${doc}" FORCE) - ELSE(${option}_AVAILABLE) - IF(${option} MATCHES "^${option}$") - ELSE(${option} MATCHES "^${option}$") - SET(${option} "${${option}}" CACHE INTERNAL "${doc}") - ENDIF(${option} MATCHES "^${option}$") - SET(${option} ${force}) - ENDIF(${option}_AVAILABLE) - ELSE(${option}_ISSET MATCHES "^${option}_ISSET$") - SET(${option} "${${option}_ISSET}") - ENDIF(${option}_ISSET MATCHES "^${option}_ISSET$") -ENDMACRO(DEPENDENT_OPTION) diff --git a/orocos_kdl/package.xml b/orocos_kdl/package.xml index cc8cfbf0b..f858bfd21 100644 --- a/orocos_kdl/package.xml +++ b/orocos_kdl/package.xml @@ -1,9 +1,10 @@ - + + orocos_kdl 1.4.0 This package contains a recent version of the Kinematics and Dynamics - Library (KDL), distributed by the Orocos Project. + Library (KDL), distributed by the Orocos Project. Ruben Smits http://wiki.ros.org/orocos_kdl @@ -12,11 +13,14 @@ cmake eigen - catkin - eigen - pkg-config + catkin + eigen + pkg-config cppunit + + doxygen + cmake diff --git a/orocos_kdl/src/CMakeLists.txt b/orocos_kdl/src/CMakeLists.txt index 079ca8ac1..7557069bb 100644 --- a/orocos_kdl/src/CMakeLists.txt +++ b/orocos_kdl/src/CMakeLists.txt @@ -3,52 +3,6 @@ FILE( GLOB KDL_HPPS [^.]*.hpp [^.]*.inl) FILE( GLOB UTIL_HPPS utilities/[^.]*.h utilities/[^.]*.hpp) -INCLUDE(CheckCXXSourceCompiles) -SET(CMAKE_REQUIRED_FLAGS) -CHECK_CXX_SOURCE_COMPILES(" - #include - #include - #include - - class TreeElement; - typedef std::map SegmentMap; - - class TreeElement - { - TreeElement(const std::string& name): number(0) {} - - public: - int number; - SegmentMap::const_iterator parent; - std::vector children; - - static TreeElement Root(std::string& name) - { - return TreeElement(name); - } - }; - - int main() - { - return 0; - } - " - HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - -if(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT Off) -ELSE(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT On) -ENDIF(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - -SET(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface") - -#Sanity check, inform the user -IF(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE) - MESSAGE(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of " - "incomplete types, this configuration is likely invalid") -ENDIF() - #In Windows (Visual Studio) it is necessary to specify the postfix #of the debug library name and no symbols are exported by kdl, #so it is necessary to compile it as a static library diff --git a/orocos_kdl/src/chain.cpp b/orocos_kdl/src/chain.cpp index 2b6320b27..016dc4b90 100644 --- a/orocos_kdl/src/chain.cpp +++ b/orocos_kdl/src/chain.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -55,7 +55,7 @@ namespace KDL { { segments.push_back(segment); nrOfSegments++; - if(segment.getJoint().getType()!=Joint::None) + if(segment.getJoint().getType()!=Joint::Fixed) nrOfJoints++; } diff --git a/orocos_kdl/src/chaindynparam.cpp b/orocos_kdl/src/chaindynparam.cpp index 3773c3b0f..ccf6b478b 100644 --- a/orocos_kdl/src/chaindynparam.cpp +++ b/orocos_kdl/src/chaindynparam.cpp @@ -1,8 +1,9 @@ -// Copyright (C) 2009 Dominick Vanthienen +// Copyright (C) 2020 Ruben Smits // Version: 1.0 // Author: Dominick Vanthienen -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -65,13 +66,13 @@ namespace KDL { return (error = E_SIZE_MISMATCH); unsigned int k=0; double q_; - + //Sweep from root to leaf for(unsigned int i=0;i=0;i--) { - + if(i!=0) { //assumption that previous segment is parent Ic[i-1]=Ic[i-1]+X[i]*Ic[i]; - } + } F=Ic[i]*S[i]; - if(chain.getSegment(i).getJoint().getType()!=Joint::None) + if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { H(k,k)=dot(S[i],F); j=k; //countervariable for the joints @@ -106,14 +107,14 @@ namespace KDL { //assumption that previous segment is parent F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l] l--; //go down a segment - - if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint - { + + if(chain.getSegment(l).getJoint().getType()!=Joint::Fixed) //if the joint connected to segment is not a fixed joint + { j--; - H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment + H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment H(j,k)=H(k,j); } - } + } k--; //this if-loop should be repeated nj times (k=nj-1 to k=0) } @@ -127,10 +128,10 @@ namespace KDL { //make a null matrix with the size of q_dotdot and a null wrench SetToZero(jntarraynull); - + //the calculation of coriolis matrix C return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis); - + } //calculate gravity matrix G @@ -138,7 +139,7 @@ namespace KDL { { //make a null matrix with the size of q_dotdot and a null wrench - + SetToZero(jntarraynull); //the calculation of coriolis matrix C return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity); diff --git a/orocos_kdl/src/chainfksolverpos_recursive.cpp b/orocos_kdl/src/chainfksolverpos_recursive.cpp index 99d7d4793..c7bc8bbd3 100644 --- a/orocos_kdl/src/chainfksolverpos_recursive.cpp +++ b/orocos_kdl/src/chainfksolverpos_recursive.cpp @@ -1,9 +1,9 @@ -// Copyright (C) 2007 Francois Cauwe -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Francois Cauwe +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -46,7 +46,7 @@ namespace KDL { else{ int j=0; for(unsigned int i=0;i -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Francois Cauwe +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -51,7 +51,7 @@ namespace KDL int j=0; for (unsigned int i=0;i +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -23,7 +23,7 @@ #include "frames_io.hpp" namespace KDL{ - + ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav): chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()), X(ns),S(ns),v(ns),a(ns),f(ns) @@ -54,17 +54,17 @@ namespace KDL{ //Sweep from root to leaf for(unsigned int i=0;i=0;i--){ - if(chain.getSegment(i).getJoint().getType()!=Joint::None) - torques(j--)=dot(S[i],f[i]); + if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { + torques(j)=dot(S[i],f[i]); + torques(j)+=chain.getSegment(i).getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia + --j; + } if(i!=0) f[i-1]=f[i-1]+X[i]*f[i]; } diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.cpp b/orocos_kdl/src/chainidsolver_vereshchagin.cpp index 880503a53..5b5e95a49 100644 --- a/orocos_kdl/src/chainidsolver_vereshchagin.cpp +++ b/orocos_kdl/src/chainidsolver_vereshchagin.cpp @@ -1,8 +1,10 @@ -// Copyright (C) 2009, 2011 +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov -// Maintainer: Ruben Smits, Azamat Shakhimardanov +// Author: Ruben Smits +// Author: Herman Bruyninckx +// Author: Azamat Shakhimardanov +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -124,7 +126,7 @@ void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const //external forces are taken into account through s.U. Wrench FextLocal = F_total.M.Inverse() * f_ext[i]; s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i]; - if (segment.getJoint().getType() != Joint::None) + if (segment.getJoint().getType() != Joint::Fixed) j++; } @@ -240,7 +242,7 @@ void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const Jnt vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data); s.EZ.noalias() = s.E.transpose() * vZ; - if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) + if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed) j--; } } @@ -325,7 +327,7 @@ void ChainIdSolver_Vereshchagin::final_upwards_sweep(JntArray &q_dotdot, JntArra // nullspace forces. q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp); s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed - if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) + if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed) j++; } } diff --git a/orocos_kdl/src/chainiksolverpos_lma.cpp b/orocos_kdl/src/chainiksolverpos_lma.cpp index 8b11ae573..72d2112c3 100644 --- a/orocos_kdl/src/chainiksolverpos_lma.cpp +++ b/orocos_kdl/src/chainiksolverpos_lma.cpp @@ -5,7 +5,7 @@ /************************************************************************** begin : May 2012 - copyright : (C) 2012 Erwin Aertbelien + copyright : (C) 2020 Erwin Aertbelien email : firstname.lastname@mech.kuleuven.ac.be History (only major changes)( AUTHOR-Description ) : @@ -144,7 +144,7 @@ void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) { T_base_head = Frame::Identity(); // frame w.r.t. base of head for (unsigned int i=0;i + Copyright (C) 2020 Antoine Hoarau This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public @@ -98,7 +98,7 @@ int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, for(unsigned int i=0;irepresentation_ = representation; } diff --git a/orocos_kdl/src/chainjnttojacsolver.cpp b/orocos_kdl/src/chainjnttojacsolver.cpp index 098559c0b..0f98a9919 100644 --- a/orocos_kdl/src/chainjnttojacsolver.cpp +++ b/orocos_kdl/src/chainjnttojacsolver.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -70,7 +70,7 @@ namespace KDL Frame total; for (unsigned int i=0;i tmp2) { return tmp1*sqrt(1+sqr(data[1]/data[0])); @@ -100,13 +100,13 @@ namespace KDL { } } // makes v a unitvector and returns the norm of v. - // if v is smaller than eps, Vector(1,0,0) is returned with norm 0. + // if v is smaller than eps, Vector(1,0) is returned with norm 0. // if this is not good, check the return value of this method. double Vector2::Normalize(double eps) { double v = this->Norm(); if (v < eps) { *this = Vector2(1,0); - return v; + return 0; } else { *this = (*this)/v; return v; @@ -115,7 +115,7 @@ namespace KDL { // do some effort not to lose precision - double Vector::Norm() const + double Vector::Norm(double eps) const { double tmp1; double tmp2; @@ -124,7 +124,7 @@ namespace KDL { if (tmp1 >= tmp2) { tmp2=fabs(data[2]); if (tmp1 >= tmp2) { - if (tmp1 == 0) { + if (tmp1 < eps) { // only to everything exactly zero case, all other are handled correctly return 0; } @@ -149,7 +149,7 @@ namespace KDL { double v = this->Norm(); if (v < eps) { *this = Vector(1,0,0); - return v; + return 0; } else { *this = (*this)/v; return v; diff --git a/orocos_kdl/src/frames.hpp b/orocos_kdl/src/frames.hpp index 1764c1487..0710d75b5 100644 --- a/orocos_kdl/src/frames.hpp +++ b/orocos_kdl/src/frames.hpp @@ -237,7 +237,7 @@ class Vector double Normalize(double eps=epsilon); //! @return the norm of the vector - double Norm() const; + double Norm(double eps=epsilon) const; @@ -1015,7 +1015,7 @@ class Vector2 double Normalize(double eps=epsilon); //! @return the norm of the vector - double Norm() const; + double Norm(double eps=epsilon) const; //! projects v in its XY plane, and sets *this to these values inline void Set3DXY(const Vector& v); @@ -1250,19 +1250,9 @@ IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1); IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); #ifdef KDL_INLINE -// #include "vector.inl" -// #include "wrench.inl" - //#include "rotation.inl" - //#include "frame.inl" - //#include "twist.inl" - //#include "vector2.inl" - //#include "rotation2.inl" - //#include "frame2.inl" #include "frames.inl" #endif - - } diff --git a/orocos_kdl/src/framevel.hpp b/orocos_kdl/src/framevel.hpp index 243898a60..650bd3c83 100644 --- a/orocos_kdl/src/framevel.hpp +++ b/orocos_kdl/src/framevel.hpp @@ -104,7 +104,7 @@ class VectorVel IMETHOD VectorVel& operator -= (const VectorVel& arg); IMETHOD static VectorVel Zero(); IMETHOD void ReverseSign(); - IMETHOD doubleVel Norm() const; + IMETHOD doubleVel Norm(double eps=epsilon) const; IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2); @@ -128,6 +128,14 @@ class VectorVel IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps); IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps); IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps); + + IMETHOD friend bool operator==(const VectorVel& r1,const VectorVel& r2); + IMETHOD friend bool operator!=(const VectorVel& r1,const VectorVel& r2); + IMETHOD friend bool operator==(const Vector& r1,const VectorVel& r2); + IMETHOD friend bool operator!=(const Vector& r1,const VectorVel& r2); + IMETHOD friend bool operator==(const VectorVel& r1,const Vector& r2); + IMETHOD friend bool operator!=(const VectorVel& r1,const Vector& r2); + IMETHOD friend VectorVel operator - (const VectorVel& r); IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs); IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs); @@ -185,6 +193,13 @@ class RotationVel IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps); IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps); + IMETHOD friend bool operator==(const RotationVel& r1,const RotationVel& r2); + IMETHOD friend bool operator!=(const RotationVel& r1,const RotationVel& r2); + IMETHOD friend bool operator==(const Rotation& r1,const RotationVel& r2); + IMETHOD friend bool operator!=(const Rotation& r1,const RotationVel& r2); + IMETHOD friend bool operator==(const RotationVel& r1,const Rotation& r2); + IMETHOD friend bool operator!=(const RotationVel& r1,const Rotation& r2); + IMETHOD TwistVel Inverse(const TwistVel& arg) const; IMETHOD TwistVel Inverse(const Twist& arg) const; IMETHOD TwistVel operator * (const TwistVel& arg) const; @@ -239,6 +254,13 @@ class FrameVel IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps); IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps); + IMETHOD friend bool operator==(const FrameVel& a,const FrameVel& b); + IMETHOD friend bool operator!=(const FrameVel& a,const FrameVel& b); + IMETHOD friend bool operator==(const Frame& a,const FrameVel& b); + IMETHOD friend bool operator!=(const Frame& a,const FrameVel& b); + IMETHOD friend bool operator==(const FrameVel& a,const Frame& b); + IMETHOD friend bool operator!=(const FrameVel& a,const Frame& b); + IMETHOD TwistVel Inverse(const TwistVel& arg) const; IMETHOD TwistVel Inverse(const Twist& arg) const; IMETHOD TwistVel operator * (const TwistVel& arg) const; @@ -311,6 +333,13 @@ class TwistVel IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps); IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps); + IMETHOD friend bool operator==(const TwistVel& a,const TwistVel& b); + IMETHOD friend bool operator!=(const TwistVel& a,const TwistVel& b); + IMETHOD friend bool operator==(const Twist& a,const TwistVel& b); + IMETHOD friend bool operator!=(const Twist& a,const TwistVel& b); + IMETHOD friend bool operator==(const TwistVel& a,const Twist& b); + IMETHOD friend bool operator!=(const TwistVel& a,const Twist& b); + // = Conversion to other entities IMETHOD Twist GetTwist() const; IMETHOD Twist GetTwistDot() const; diff --git a/orocos_kdl/src/framevel.inl b/orocos_kdl/src/framevel.inl index 61a43fdc2..05aceb0af 100644 --- a/orocos_kdl/src/framevel.inl +++ b/orocos_kdl/src/framevel.inl @@ -81,6 +81,41 @@ bool Equal(const Frame& r1,const FrameVel& r2,double eps) { bool Equal(const FrameVel& r1,const Frame& r2,double eps) { return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); } +bool operator==(const FrameVel& r1,const FrameVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.M == r2.M ); +#endif +} +bool operator!=(const FrameVel& r1,const FrameVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const Frame& r1,const FrameVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.M == r2.M ); +#endif +} +bool operator!=(const Frame& r1,const FrameVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const FrameVel& r1,const Frame& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.M == r2.M ); +#endif +} +bool operator!=(const FrameVel& r1,const Frame& r2) { + return !operator==(r1,r2); +} + + Frame FrameVel::GetFrame() const { return Frame(M.R,p.p); @@ -315,9 +350,11 @@ void VectorVel::ReverseSign() { p.ReverseSign(); v.ReverseSign(); } -doubleVel VectorVel::Norm() const { - double n = p.Norm(); - return doubleVel(n,dot(p,v)/n); +doubleVel VectorVel::Norm(double eps) const { + double n = p.Norm(eps); + if (n < eps) // Setting norm of p and v to 0 in case norm of p is smaller than eps + return doubleVel(0, 0); + return doubleVel(n, dot(p,v)/n); } bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) { @@ -329,6 +366,40 @@ bool Equal(const Vector& r1,const VectorVel& r2,double eps) { bool Equal(const VectorVel& r1,const Vector& r2,double eps) { return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps)); } +bool operator==(const VectorVel& r1,const VectorVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.v == r2.v ); +#endif +} +bool operator!=(const VectorVel& r1,const VectorVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const Vector& r1,const VectorVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1 == r2.p && + Vector::Zero() == r2.v); +#endif +} +bool operator!=(const Vector& r1,const VectorVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const VectorVel& r1,const Vector& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2 && + r1.v == Vector::Zero() ); +#endif +} +bool operator!=(const VectorVel& r1,const Vector& r2) { + return !operator==(r1,r2); +} + bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) { return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps)); @@ -339,6 +410,41 @@ bool Equal(const Rotation& r1,const RotationVel& r2,double eps) { bool Equal(const RotationVel& r1,const Rotation& r2,double eps) { return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps)); } +bool operator==(const RotationVel& r1,const RotationVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.w == r2.w && + r1.R == r2.R ); +#endif +} +bool operator!=(const RotationVel& r1,const RotationVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const Rotation& r1,const RotationVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (Vector::Zero() == r2.w && + r1 == r2.R); +#endif +} +bool operator!=(const Rotation& r1,const RotationVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const RotationVel& r1,const Rotation& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.w == Vector::Zero() && + r1.R == r2); +#endif +} +bool operator!=(const RotationVel& r1,const Rotation& r2) { + return !operator==(r1,r2); +} + + bool Equal(const TwistVel& a,const TwistVel& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); @@ -351,7 +457,39 @@ bool Equal(const TwistVel& a,const Twist& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); } - +bool operator==(const TwistVel& a,const TwistVel& b) { +#ifdef KDL_USE_EQUAL + return Equal(a, b); +#else + return (a.rot == b.rot && + a.vel == b.vel ); +#endif +} +bool operator!=(const TwistVel& a,const TwistVel& b) { + return !operator==(a,b); +} +bool operator==(const Twist& a,const TwistVel& b) { +#ifdef KDL_USE_EQUAL + return Equal(a, b); +#else + return (a.rot == b.rot && + a.vel == b.vel ); +#endif +} +bool operator!=(const Twist& r1,const TwistVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const TwistVel& r1,const Twist& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (a.rot == b.rot && + a.vel == b.vel ); +#endif +} +bool operator!=(const TwistVel& r1,const Twist& r2) { + return !operator==(r1,r2); +} IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) { @@ -364,17 +502,6 @@ IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) { return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v)); } - - - - - - - - - - - TwistVel TwistVel::Zero() { return TwistVel(VectorVel::Zero(),VectorVel::Zero()); diff --git a/orocos_kdl/src/framevel_io.hpp b/orocos_kdl/src/framevel_io.hpp index 2124c13c4..a3dcc0743 100644 --- a/orocos_kdl/src/framevel_io.hpp +++ b/orocos_kdl/src/framevel_io.hpp @@ -21,7 +21,6 @@ #include "utilities/utility_io.h" #include "utilities/rall1d_io.h" -#include "framevel_io.hpp" #include "frames_io.hpp" namespace KDL { diff --git a/orocos_kdl/src/jacobian.cpp b/orocos_kdl/src/jacobian.cpp index 839ad0011..7b99f23a7 100644 --- a/orocos_kdl/src/jacobian.cpp +++ b/orocos_kdl/src/jacobian.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -33,15 +33,16 @@ namespace KDL Jacobian::Jacobian(unsigned int nr_of_columns): data(6,nr_of_columns) { + data.setZero(); } - + Jacobian::Jacobian(const Jacobian& arg): data(arg.data) { } Jacobian& Jacobian::operator = (const Jacobian& arg) - { + { this->data=arg.data; return *this; } @@ -49,7 +50,7 @@ namespace KDL Jacobian::~Jacobian() { - + } void Jacobian::resize(unsigned int new_nr_of_columns) @@ -95,7 +96,7 @@ namespace KDL dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB)); return true; } - + void Jacobian::changeBase(const Rotation& rot){ for(unsigned int i=0;isetColumn(i,rot*this->getColumn(i));; @@ -114,7 +115,7 @@ namespace KDL for(unsigned int i=0;isetColumn(i,frame*this->getColumn(i)); } - + bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest) { if(src1.columns()!=dest.columns()) @@ -128,12 +129,12 @@ namespace KDL { return Equal((*this),arg); } - + bool Jacobian::operator!=(const Jacobian& arg)const { return !Equal((*this),arg); } - + bool Equal(const Jacobian& a,const Jacobian& b,double eps) { if(a.rows()==b.rows()&&a.columns()==b.columns()){ @@ -141,11 +142,11 @@ namespace KDL }else return false; } - + Twist Jacobian::getColumn(unsigned int i) const{ return Twist(Vector(data(0,i),data(1,i),data(2,i)),Vector(data(3,i),data(4,i),data(5,i))); } - + void Jacobian::setColumn(unsigned int i,const Twist& t){ data.col(i).head<3>()=Eigen::Map(t.vel.data); data.col(i).tail<3>()=Eigen::Map(t.rot.data); diff --git a/orocos_kdl/src/joint.cpp b/orocos_kdl/src/joint.cpp index e487334b8..b2e7c4a3b 100644 --- a/orocos_kdl/src/joint.cpp +++ b/orocos_kdl/src/joint.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -42,7 +42,7 @@ namespace KDL { } // constructor for joint along arbitrary axis, at arbitrary origin - Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, + Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) , axis(_axis / _axis.Norm()), origin(_origin) @@ -56,7 +56,7 @@ namespace KDL { } // constructor for joint along arbitrary axis, at arbitrary origin - Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, + Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()),origin(_origin) @@ -97,7 +97,7 @@ namespace KDL { return Frame(Vector(0.0,scale*q+offset,0.0)); case TransZ: return Frame(Vector(0.0,0.0,scale*q+offset)); - case None: + case Fixed: return Frame::Identity(); } return Frame::Identity(); @@ -122,7 +122,7 @@ namespace KDL { return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0)); case TransZ: return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0)); - case None: + case Fixed: return Twist::Zero(); } return Twist::Zero(); @@ -148,7 +148,7 @@ namespace KDL { return Vector(0.,1.,0.); case TransZ: return Vector(0.,0.,1.); - case None: + case Fixed: return Vector::Zero(); } return Vector::Zero(); diff --git a/orocos_kdl/src/joint.hpp b/orocos_kdl/src/joint.hpp index a188aff95..ad1cc0cae 100644 --- a/orocos_kdl/src/joint.hpp +++ b/orocos_kdl/src/joint.hpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -44,12 +44,12 @@ namespace KDL { */ class Joint { public: - typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None} JointType; + typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed,None=Fixed} JointType; /** * Constructor of a joint. * * @param name of the joint - * @param type type of the joint, default: Joint::None + * @param type type of the joint, default: Joint::Fixed * @param scale scale between joint input and actual geometric * movement, default: 1 * @param offset offset between joint input and actual @@ -59,12 +59,12 @@ namespace KDL { * @param stiffness 1D stiffness along the joint axis, * default: 0 */ - explicit Joint(const std::string& name, const JointType& type=None,const double& scale=1,const double& offset=0, + explicit Joint(const std::string& name, const JointType& type=Fixed,const double& scale=1,const double& offset=0, const double& inertia=0,const double& damping=0,const double& stiffness=0); /** * Constructor of a joint. * - * @param type type of the joint, default: Joint::None + * @param type type of the joint, default: Joint::Fixed * @param scale scale between joint input and actual geometric * movement, default: 1 * @param offset offset between joint input and actual @@ -74,7 +74,7 @@ namespace KDL { * @param stiffness 1D stiffness along the joint axis, * default: 0 */ - explicit Joint(const JointType& type=None,const double& scale=1,const double& offset=0, + explicit Joint(const JointType& type=Fixed,const double& scale=1,const double& offset=0, const double& inertia=0,const double& damping=0,const double& stiffness=0); /** * Constructor of a joint. @@ -128,16 +128,16 @@ namespace KDL { */ Twist twist(const double& qdot)const; - /** - * Request the Vector corresponding to the axis of a revolute joint. - * - * @return Vector. e.g (1,0,0) for RotX etc. + /** + * Request the Vector corresponding to the axis of a revolute joint. + * + * @return Vector. e.g (1,0,0) for RotX etc. */ Vector JointAxis() const; - /** - * Request the Vector corresponding to the origin of a revolute joint. - * + /** + * Request the Vector corresponding to the origin of a revolute joint. + * * @return Vector */ Vector JointOrigin() const; @@ -160,8 +160,8 @@ namespace KDL { { return type; }; - - /** + + /** * Request the stringified type of the joint. * * @return const string @@ -185,10 +185,10 @@ namespace KDL { return "TransY"; case TransZ: return "TransZ"; - case None: - return "None"; + case Fixed: + return "Fixed"; default: - return "None"; + return "Fixed"; } }; @@ -209,7 +209,7 @@ namespace KDL { mutable double q_previous; - + class joint_type_exception: public std::exception{ virtual const char* what() const throw(){ return "Joint Type excption";} diff --git a/orocos_kdl/src/segment.hpp b/orocos_kdl/src/segment.hpp index 934b17a07..fff85297e 100644 --- a/orocos_kdl/src/segment.hpp +++ b/orocos_kdl/src/segment.hpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -57,22 +57,22 @@ namespace KDL { * * @param name name of the segment * @param joint joint of the segment, default: - * Joint(Joint::None) + * Joint(Joint::Fixed) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() * @param M rigid body inertia of the segment, default: Inertia::Zero() */ - explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); /** * Constructor of the segment * * @param joint joint of the segment, default: - * Joint(Joint::None) + * Joint(Joint::Fixed) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() * @param M rigid body inertia of the segment, default: Inertia::Zero() */ - explicit Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + explicit Segment(const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); Segment(const Segment& in); Segment& operator=(const Segment& arg); @@ -148,7 +148,7 @@ namespace KDL { */ Frame getFrameToTip()const { - + return joint.pose(0)*f_tip; } diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index 1ca751489..37446052a 100644 --- a/orocos_kdl/src/tree.cpp +++ b/orocos_kdl/src/tree.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -59,7 +59,7 @@ bool Tree::addSegment(const Segment& segment, const std::string& hook_name) { return false; pair retval; //insert new element - unsigned int q_nr = segment.getJoint().getType() != Joint::None ? nrOfJoints : 0; + unsigned int q_nr = segment.getJoint().getType() != Joint::Fixed ? nrOfJoints : 0; #ifdef KDL_USE_NEW_TREE_INTERFACE retval = segments.insert(make_pair(segment.getName(), TreeElementType( new TreeElement(segment, parent, q_nr)))); @@ -75,7 +75,7 @@ bool Tree::addSegment(const Segment& segment, const std::string& hook_name) { //increase number of segments nrOfSegments++; //increase number of joints - if (segment.getJoint().getType() != Joint::None) + if (segment.getJoint().getType() != Joint::Fixed) nrOfJoints++; return true; } diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp new file mode 100644 index 000000000..91a5246b3 --- /dev/null +++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp @@ -0,0 +1,139 @@ +// Copyright (C) 2020 Ruben Smits + +// Version: 1.0 +// Author: Franco Fusco +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "treeidsolver_recursive_newton_euler.hpp" +#include "frames_io.hpp" +#include + +namespace KDL{ + + TreeIdSolver_RNE::TreeIdSolver_RNE(const Tree& tree_, Vector grav): + tree(tree_), nj(tree.getNrOfJoints()), ns(tree.getNrOfSegments()) + { + ag=-Twist(grav,Vector::Zero()); + initAuxVariables(); + } + + void TreeIdSolver_RNE::updateInternalDataStructures() { + nj = tree.getNrOfJoints(); + ns = tree.getNrOfSegments(); + initAuxVariables(); + } + + void TreeIdSolver_RNE::initAuxVariables() { + const SegmentMap& segments = tree.getSegments(); + for(SegmentMap::const_iterator seg = segments.begin(); seg != segments.end(); seg++) { + X[seg->first] = Frame(); + S[seg->first] = Twist(); + v[seg->first] = Twist(); + a[seg->first] = Twist(); + f[seg->first] = Wrench(); + } + } + + int TreeIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques) + { + //Check that the tree was not modified externally + if(nj != tree.getNrOfJoints() || ns != tree.getNrOfSegments()) + return (error = E_NOT_UP_TO_DATE); + + //Check sizes of joint vectors + if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj) + return (error = E_SIZE_MISMATCH); + + try { + //Do the recursion here + rne_step(tree.getRootSegment(), q, q_dot, q_dotdot, f_ext, torques); + } + catch(const std::out_of_range&) { + //If in rne_step we get an out_of_range exception it means that some call + //to map::at failed. This can happen only if updateInternalDataStructures + //was not called after changing something in the tree. Note that it + //should be impossible to reach this point as consistency of the tree is + //checked above. + return (error = E_NOT_UP_TO_DATE); + } + return (error = E_NOERROR); + } + + + void TreeIdSolver_RNE::rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques) { + const Segment& seg = GetTreeElementSegment(segment->second); + const std::string& segname = segment->first; + const std::string& parname = GetTreeElementParent(segment->second)->first; + + //Do forward calculations involving velocity & acceleration of this segment + double q_, qdot_, qdotdot_; + unsigned int j = GetTreeElementQNr(segment->second); + if(seg.getJoint().getType()!=Joint::Fixed) { + q_ = q(j); + qdot_ = q_dot(j); + qdotdot_ = q_dotdot(j); + } + else + q_ = qdot_ = qdotdot_ = 0.0; + + //Calculate segment properties: X,S,vj,cj + + //Remark this is the inverse of the frame for transformations from the parent to the current coord frame + X.at(segname) = seg.pose(q_); + + //Transform velocity and unit velocity to segment frame + Twist vj = X.at(segname).M.Inverse( seg.twist(q_,qdot_) ); + S.at(segname) = X.at(segname).M.Inverse( seg.twist(q_,1.0) ); + + //calculate velocity and acceleration of the segment (in segment coordinates) + if(segment == tree.getRootSegment()) { + v.at(segname) = vj; + a.at(segname) = X.at(segname).Inverse(ag) + S.at(segname)*qdotdot_+ v.at(segname)*vj; + } + else { + v.at(segname) = X.at(segname).Inverse(v.at(parname)) + vj; + a.at(segname) = X.at(segname).Inverse(a.at(parname)) + S.at(segname)*qdotdot_ + v.at(segname)*vj; + } + + //Calculate the force for the joint + //Collect RigidBodyInertia and external forces + const RigidBodyInertia& I = seg.getInertia(); + f.at(segname) = I*a.at(segname) + v.at(segname)*(I*v.at(segname)); + if(f_ext.find(segname) != f_ext.end()) + f.at(segname) = f.at(segname) - f_ext.at(segname); + + //propagate calculations over each child segment + SegmentMap::const_iterator child; + for (unsigned int i = 0; i < GetTreeElementChildren(segment->second).size(); i++) { + child = GetTreeElementChildren(segment->second)[i]; + rne_step(child, q, q_dot, q_dotdot, f_ext, torques); + } + + //do backward calculations involving wrenches and joint efforts + + //If there is a moving joint, evaluate its effort + if(seg.getJoint().getType()!=Joint::Fixed) { + torques(j) = dot(S.at(segname), f.at(segname)); + torques(j) += seg.getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia + } + + //add reaction forces to parent segment + if(segment != tree.getRootSegment()) + f.at(parname) = f.at(parname) + X.at(segname)*f.at(segname); + } +}//namespace diff --git a/orocos_kdl/src/treejnttojacsolver.cpp b/orocos_kdl/src/treejnttojacsolver.cpp index 9b5ff856a..0afb4d1c5 100644 --- a/orocos_kdl/src/treejnttojacsolver.cpp +++ b/orocos_kdl/src/treejnttojacsolver.cpp @@ -47,7 +47,7 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std: T_total = T_local * T_total; //get the twist of the segment: - if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::None) { + if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::Fixed) { Twist t_local = GetTreeElementSegment(it->second).twist(q_in(q_nr), 1.0); //transform the endpoint of the local twist to the global endpoint: t_local = t_local.RefPoint(T_total.p - T_local.p); diff --git a/orocos_kdl/src/utilities/rall1d.h b/orocos_kdl/src/utilities/rall1d.h index 4b3ba1ac1..812c69fe3 100644 --- a/orocos_kdl/src/utilities/rall1d.h +++ b/orocos_kdl/src/utilities/rall1d.h @@ -56,7 +56,7 @@ class Rall1d T t; //!< value V grad; //!< gradient public : - INLINE Rall1d() {} + INLINE Rall1d():t(),grad() {}; T value() const { return t; @@ -471,6 +471,22 @@ INLINE bool Equal(const Rall1d& y,const Rall1d& x,double eps=epsi return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps)); } +template +INLINE bool operator==(const Rall1d& y,const Rall1d& x) +{ +#ifdef KDL_USE_EQUAL + return Equal(y, x); +#else + return (x.t == y.t && x.grad == y.grad); +#endif +} + +template +INLINE bool operator!=(const Rall1d& y,const Rall1d& x) +{ + return !operator==(y, x); +} + } diff --git a/orocos_kdl/src/utilities/rall1d_io.h b/orocos_kdl/src/utilities/rall1d_io.h index d69b26177..ac2115904 100644 --- a/orocos_kdl/src/utilities/rall1d_io.h +++ b/orocos_kdl/src/utilities/rall1d_io.h @@ -18,6 +18,7 @@ #ifndef Rall_IO_H #define Rall_IO_H +#include #include "utility_io.h" #include "rall1d.h" @@ -26,7 +27,7 @@ namespace KDL { template inline std::ostream& operator << (std::ostream& os,const Rall1d& r) { - os << "Rall1d(" << r.t <<"," << r.grad <<")"; + os << "Rall1d<" << typeid(T).name() << ", "<< typeid(V).name() << ", " << typeid(S).name() << ">(" << r.t <<"," << r.grad <<")"; return os; } diff --git a/orocos_kdl/src/utilities/rall2d.h b/orocos_kdl/src/utilities/rall2d.h index 811bad553..fd6f6f965 100644 --- a/orocos_kdl/src/utilities/rall2d.h +++ b/orocos_kdl/src/utilities/rall2d.h @@ -59,7 +59,7 @@ class Rall2d V dd; //!< 2nd derivative public : // = Constructors - INLINE Rall2d() {} + INLINE Rall2d():t(),d(),dd() {}; explicit INLINE Rall2d(typename TI::Arg c) {t=c;SetToZero(d);SetToZero(dd);} @@ -531,6 +531,24 @@ INLINE bool Equal(const Rall2d& y,const Rall2d& x,double eps=epsi ); } +template +INLINE bool operator==(const Rall2d& y,const Rall2d& x) +{ +#ifdef KDL_USE_EQUAL + return Equal(y, x); +#else + return (x.t == y.t && + x.d == y.d && + x.dd == y.dd); +#endif + +} + +template +INLINE bool operator!=(const Rall2d& y,const Rall2d& x) +{ + return !operator==(y, x); +} } diff --git a/orocos_kdl/src/utilities/rall2d_io.h b/orocos_kdl/src/utilities/rall2d_io.h index 8870243a5..5ea145cd2 100644 --- a/orocos_kdl/src/utilities/rall2d_io.h +++ b/orocos_kdl/src/utilities/rall2d_io.h @@ -19,7 +19,7 @@ #define Rall2d_IO_H - +#include #include "utility_io.h" #include "rall2d.h" @@ -28,7 +28,7 @@ namespace KDL { template std::ostream& operator << (std::ostream& os,const Rall2d& r) { - os << "Rall2d(" << r.t <<"," << r.d <<","<(" << r.t <<"," << r.d <<","< +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include "PyKDL.h" + +namespace py = pybind11; + + +PYBIND11_MODULE(PyKDL, m) +{ + m.doc() = "Orocos KDL Python wrapper"; // optional module docstring + m.attr("__version__") = KDL_VERSION_STRING; + init_frames(m); + init_framevel(m); + init_kinfam(m); + init_dynamics(m); +} diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h new file mode 100644 index 000000000..17256935d --- /dev/null +++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h @@ -0,0 +1,34 @@ +//Copyright (C) 2020 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include +#include + + +void init_frames(pybind11::module &m); +void init_framevel(pybind11::module &m); +void init_kinfam(pybind11::module &m); +void init_dynamics(pybind11::module &m); diff --git a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp new file mode 100644 index 000000000..b63b08763 --- /dev/null +++ b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp @@ -0,0 +1,92 @@ +//Copyright (C) 2020 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_dynamics(pybind11::module &m) +{ + // -------------------- + // JntSpaceInertiaMatrix + // -------------------- + py::class_ jnt_space_inertia_matrix(m, "JntSpaceInertiaMatrix"); + jnt_space_inertia_matrix.def(py::init<>()); + jnt_space_inertia_matrix.def(py::init()); + jnt_space_inertia_matrix.def(py::init()); + jnt_space_inertia_matrix.def("resize", &JntSpaceInertiaMatrix::resize); + jnt_space_inertia_matrix.def("rows", &JntSpaceInertiaMatrix::rows); + jnt_space_inertia_matrix.def("columns", &JntSpaceInertiaMatrix::columns); + jnt_space_inertia_matrix.def("__getitem__", [](const JntSpaceInertiaMatrix &jm, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i >= jm.rows() || j < 0 || j >= jm.columns()) + throw py::index_error("Inertia index out of range"); + + return jm((unsigned int)i, (unsigned int)j); + }); + jnt_space_inertia_matrix.def("__setitem__", [](JntSpaceInertiaMatrix &jm, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i >= jm.rows() || j < 0 || j >= jm.columns()) + throw py::index_error("Inertia index out of range"); + + jm((unsigned int)i, (unsigned int)j) = value; + }); + jnt_space_inertia_matrix.def("__repr__", [](const JntSpaceInertiaMatrix &jm) + { + std::ostringstream oss; + oss << jm; + return oss.str(); + }); + jnt_space_inertia_matrix.def(py::self == py::self); + + m.def("Add", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, JntSpaceInertiaMatrix&)) &KDL::Add); + m.def("Subtract", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&,JntSpaceInertiaMatrix&)) &KDL::Subtract); + m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Multiply); + m.def("Divide", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Divide); + m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const JntArray&, JntArray&)) &KDL::Multiply); + m.def("SetToZero", (void (*)(JntSpaceInertiaMatrix&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, double)) &KDL::Equal, + py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); + + + // -------------------- + // ChainDynParam + // -------------------- + py::class_ chain_dyn_param(m, "ChainDynParam"); + chain_dyn_param.def(py::init()); + chain_dyn_param.def("JntToCoriolis", &ChainDynParam::JntToCoriolis); + chain_dyn_param.def("JntToMass", &ChainDynParam::JntToMass); + chain_dyn_param.def("JntToGravity", &ChainDynParam::JntToGravity); +} diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp new file mode 100644 index 000000000..65afa0f10 --- /dev/null +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -0,0 +1,451 @@ +//Copyright (C) 2020 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_frames(py::module &m) +{ + // -------------------- + // Vector + // -------------------- + py::class_ vector(m, "Vector"); + vector.def(py::init<>()); + vector.def(py::init()); + vector.def(py::init()); + vector.def("x", (void (Vector::*)(double)) &Vector::x); + vector.def("y", (void (Vector::*)(double)) &Vector::y); + vector.def("z", (void (Vector::*)(double)) &Vector::z); + vector.def("x", (double (Vector::*)(void) const) &Vector::x); + vector.def("y", (double (Vector::*)(void) const) &Vector::y); + vector.def("z", (double (Vector::*)(void) const) &Vector::z); + vector.def("__getitem__", [](const Vector &v, int i) + { + if (i < 0 || i > 2) + throw py::index_error("Vector index out of range"); + + return v(i); + }); + vector.def("__setitem__", [](Vector &v, int i, double value) + { + if (i < 0 || i > 2) + throw py::index_error("Vector index out of range"); + + v(i) = value; + }); + vector.def("__repr__", [](const Vector &v) + { + std::ostringstream oss; + oss << v; + return oss.str(); + }); + vector.def("ReverseSign", &Vector::ReverseSign); + vector.def(py::self -= py::self); + vector.def(py::self += py::self); + vector.def(py::self + py::self); + vector.def(py::self - py::self); + vector.def(py::self * double()); + vector.def(double() * py::self); + vector.def(py::self / double()); + vector.def(py::self * py::self); + vector.def(py::self == py::self); + vector.def(py::self != py::self); + vector.def("__neg__", [](const Vector &a) + { + return operator-(a); + }, py::is_operator()); + vector.def_static("Zero", &Vector::Zero); + vector.def("Norm", &Vector::Norm, py::arg("eps")=epsilon); + vector.def("Normalize", &Vector::Normalize, py::arg("eps")=epsilon); + vector.def(py::pickle( + [](const Vector &v) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(v.x(), v.y(), v.z()); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 3) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Vector v(t[0].cast(), t[1].cast(), t[2].cast()); + return v; + })); + + m.def("SetToZero", (void (*)(Vector&)) &KDL::SetToZero); + m.def("dot", (double (*)(const Vector&, const Vector&)) &KDL::dot); + m.def("Equal", (bool (*)(const Vector&, const Vector&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Wrench + // -------------------- + py::class_ wrench(m, "Wrench"); + wrench.def(py::init<>()); + wrench.def(py::init()); + wrench.def(py::init()); + wrench.def_readwrite("force", &Wrench::force); + wrench.def_readwrite("torque", &Wrench::torque); + wrench.def("__getitem__", [](const Wrench &t, int i) + { + if (i < 0 || i > 5) + throw py::index_error("Wrench index out of range"); + + return t(i); + }); + wrench.def("__setitem__", [](Wrench &t, int i, double value) + { + if (i < 0 || i > 5) + throw py::index_error("Wrench index out of range"); + + t(i) = value; + }); + wrench.def("__repr__", [](const Wrench &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + wrench.def_static("Zero", &Wrench::Zero); + wrench.def("ReverseSign", &Wrench::ReverseSign); + wrench.def("RefPoint", &Wrench::RefPoint); + wrench.def(py::self -= py::self); + wrench.def(py::self += py::self); + wrench.def(py::self * double()); + wrench.def(double() * py::self); + wrench.def(py::self / double()); + wrench.def(py::self + py::self); + wrench.def(py::self - py::self); + wrench.def(py::self == py::self); + wrench.def(py::self != py::self); + wrench.def("__neg__", [](const Wrench &w) + { + return operator-(w); + }, py::is_operator()); + wrench.def(py::pickle( + [](const Wrench &wr) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(wr.force, wr.torque); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Wrench wr(t[0].cast(), t[1].cast()); + return wr; + })); + + m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const Wrench&, const Wrench&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Twist + // -------------------- + py::class_ twist(m, "Twist"); + twist.def(py::init<>()); + twist.def(py::init()); + twist.def(py::init()); + twist.def_readwrite("vel", &Twist::vel); + twist.def_readwrite("rot", &Twist::rot); + twist.def("__getitem__", [](const Twist &t, int i) + { + if (i < 0 || i > 5) + throw py::index_error("Twist index out of range"); + + return t(i); + }); + twist.def("__setitem__", [](Twist &t, int i, double value) + { + if (i < 0 || i > 5) + throw py::index_error("Twist index out of range"); + + t(i) = value; + }); + twist.def("__repr__", [](const Twist &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + twist.def_static("Zero", &Twist::Zero); + twist.def("ReverseSign", &Twist::ReverseSign); + twist.def("RefPoint", &Twist::RefPoint); + twist.def(py::self -= py::self); + twist.def(py::self += py::self); + twist.def(py::self * double()); + twist.def(double() * py::self); + twist.def(py::self / double()); + twist.def(py::self + py::self); + twist.def(py::self - py::self); + twist.def(py::self == py::self); + twist.def(py::self != py::self); + twist.def("__neg__", [](const Twist &a) + { + return operator-(a); + }, py::is_operator()); + twist.def(py::pickle( + [](const Twist &tt) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(tt.vel, tt.rot); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Twist tt(t[0].cast(), t[1].cast()); + return tt; + })); + + m.def("dot", (double (*)(const Twist&, const Wrench&)) &KDL::dot); + m.def("dot", (double (*)(const Wrench&, const Twist&)) &KDL::dot); + m.def("SetToZero", (void (*)(Twist&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const Twist&, const Twist&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Rotation + // -------------------- + py::class_ rotation(m, "Rotation"); + rotation.def(py::init<>()); + rotation.def(py::init()); + rotation.def(py::init()); + rotation.def(py::init()); + rotation.def("__getitem__", [](const Rotation &r, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 2) + throw py::index_error("Rotation index out of range"); + + return r(i, j); + }); + rotation.def("__setitem__", [](Rotation &r, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 2) + throw py::index_error("Rotation index out of range"); + + r(i, j) = value; + }); + rotation.def("__repr__", [](const Rotation &r) + { + std::ostringstream oss; + oss << r; + return oss.str(); + }); + rotation.def("SetInverse", &Rotation::SetInverse); + rotation.def("Inverse", (Rotation (Rotation::*)(void) const) &Rotation::Inverse); + rotation.def("Inverse", (Vector (Rotation::*)(const Vector&) const) &Rotation::Inverse); + rotation.def("Inverse", (Wrench (Rotation::*)(const Wrench&) const) &Rotation::Inverse); + rotation.def("Inverse", (Twist (Rotation::*)(const Twist&) const) &Rotation::Inverse); + rotation.def_static("Identity", &Rotation::Identity); + rotation.def_static("RotX", &Rotation::RotX); + rotation.def_static("RotY", &Rotation::RotY); + rotation.def_static("RotZ", &Rotation::RotZ); + rotation.def_static("Rot", &Rotation::Rot); + rotation.def_static("Rot2", &Rotation::Rot2); + rotation.def_static("EulerZYZ", &Rotation::EulerZYZ); + rotation.def_static("RPY", &Rotation::RPY); + rotation.def_static("EulerZYX", &Rotation::EulerZYX); + rotation.def_static("Quaternion", &Rotation::Quaternion); + rotation.def("DoRotX", &Rotation::DoRotX); + rotation.def("DoRotY", &Rotation::DoRotY); + rotation.def("DoRotZ", &Rotation::DoRotZ); + rotation.def("GetRot", &Rotation::GetRot); + rotation.def("GetRotAngle", [](const Rotation &r, double eps) + { + Vector axis; + double ret = r.GetRotAngle(axis, eps); + return py::make_tuple(ret, axis); + }, py::arg("eps") = epsilon); + rotation.def("GetEulerZYZ", [](const Rotation &r) + { + double Alfa, Beta, Gamma; + r.GetEulerZYZ(Alfa, Beta, Gamma); + return py::make_tuple(Alfa, Beta, Gamma); + }); + rotation.def("GetRPY", [](const Rotation &r) + { + double roll, pitch, yaw; + r.GetRPY(roll, pitch, yaw); + return py::make_tuple(roll, pitch, yaw); + }); + rotation.def("GetEulerZYX", [](const Rotation &r) + { + double Alfa, Beta, Gamma; + r.GetEulerZYX(Alfa, Beta, Gamma); + return py::make_tuple(Alfa, Beta, Gamma); + }); + rotation.def("GetQuaternion", [](const Rotation &r) + { + double x, y, z, w; + r.GetQuaternion(x, y, z, w); + return py::make_tuple(x, y, z, w); + }); + rotation.def("UnitX", (Vector (Rotation::*)() const) &Rotation::UnitX); + rotation.def("UnitY", (Vector (Rotation::*)() const) &Rotation::UnitY); + rotation.def("UnitZ", (Vector (Rotation::*)() const) &Rotation::UnitZ); + rotation.def("UnitX", (void (Rotation::*)(const Vector&)) &Rotation::UnitX); + rotation.def("UnitY", (void (Rotation::*)(const Vector&)) &Rotation::UnitY); + rotation.def("UnitZ", (void (Rotation::*)(const Vector&)) &Rotation::UnitZ); + rotation.def(py::self * Vector()); + rotation.def(py::self * Twist()); + rotation.def(py::self * Wrench()); + rotation.def(py::self == py::self); + rotation.def(py::self != py::self); + rotation.def(py::self * py::self); + rotation.def(py::pickle( + [](const Rotation &rot) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + double roll{0}, pitch{0}, yaw{0}; + rot.GetRPY(roll, pitch, yaw); + return py::make_tuple(roll, pitch, yaw); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 3) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + return Rotation::RPY(t[0].cast(), t[1].cast(), t[2].cast()); + })); + + m.def("Equal", (bool (*)(const Rotation&, const Rotation&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Frame + // -------------------- + py::class_ frame(m, "Frame"); + frame.def(py::init()); + frame.def(py::init()); + frame.def(py::init()); + frame.def(py::init()); + frame.def(py::init<>()); + frame.def_readwrite("M", &Frame::M); + frame.def_readwrite("p", &Frame::p); + frame.def("__getitem__", [](const Frame &frm, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 3) + throw py::index_error("Frame index out of range"); + + return frm(i, j); + }); + frame.def("__setitem__", [](Frame &frm, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 3) + throw py::index_error("Frame index out of range"); + + if (j == 3) + frm.p(i) = value; + else + frm.M(i, j) = value; + }); + frame.def("__repr__", [](const Frame &frm) + { + std::ostringstream oss; + oss << frm; + return oss.str(); + }); + frame.def("DH_Craig1989", &Frame::DH_Craig1989); + frame.def("DH", &Frame::DH); + frame.def("Inverse", (Frame (Frame::*)() const) &Frame::Inverse); + frame.def("Inverse", (Vector (Frame::*)(const Vector&) const) &Frame::Inverse); + frame.def("Inverse", (Wrench (Frame::*)(const Wrench&) const) &Frame::Inverse); + frame.def("Inverse", (Twist (Frame::*)(const Twist&) const) &Frame::Inverse); + frame.def_static("Identity", &Frame::Identity); + frame.def("Integrate", &Frame::Integrate); + frame.def(py::self * Vector()); + frame.def(py::self * Wrench()); + frame.def(py::self * Twist()); + frame.def(py::self * py::self); + frame.def(py::self == py::self); + frame.def(py::self != py::self); + frame.def(py::pickle( + [](const Frame &frm) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(frm.M, frm.p); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Frame frm(t[0].cast(), t[1].cast()); + return frm; + })); + + m.def("Equal", (bool (*)(const Frame&, const Frame&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Global + // -------------------- + m.def("diff", (Vector (*)(const Vector&, const Vector&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Vector (*)(const Rotation&, const Rotation&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Twist (*)(const Frame&, const Frame&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Twist (*)(const Twist&, const Twist&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Wrench (*)(const Wrench&, const Wrench&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("addDelta", (Vector (*)(const Vector&, const Vector&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Rotation (*)(const Rotation&, const Vector&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Frame (*)(const Frame&, const Twist&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Twist (*)(const Twist&, const Twist&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Wrench (*)(const Wrench&, const Wrench&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); +} diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp new file mode 100644 index 000000000..43842136d --- /dev/null +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -0,0 +1,380 @@ +//Copyright (C) 2020 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_framevel(pybind11::module &m) +{ + // -------------------- + // doubleVel + // -------------------- + py::class_ double_vel(m, "doubleVel"); + double_vel.def(py::init<>()); + double_vel.def(py::init()); + double_vel.def(py::init()); + double_vel.def(py::init()); + double_vel.def_readwrite("t", &doubleVel::t); + double_vel.def_readwrite("grad", &doubleVel::grad); + double_vel.def("value", &doubleVel::value); + double_vel.def("deriv", &doubleVel::deriv); + double_vel.def("__repr__", [](const doubleVel &d) + { + std::ostringstream oss; + oss << d; + return oss.str(); + }); + + double_vel.def(py::self == py::self); + double_vel.def(py::self != py::self); + + double_vel.def("__neg__", [](const doubleVel &a) + { + return operator-(a); + }, py::is_operator()); + + m.def("diff", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + m.def("addDelta", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); + m.def("Equal", (bool (*)(const doubleVel&, const doubleVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + + // -------------------- + // VectorVel + // -------------------- + py::class_ vector_vel(m, "VectorVel"); + vector_vel.def_readwrite("p", &VectorVel::p); + vector_vel.def_readwrite("v", &VectorVel::v); + vector_vel.def(py::init<>()); + vector_vel.def(py::init()); + vector_vel.def(py::init()); + vector_vel.def(py::init()); + vector_vel.def("value", &VectorVel::value); + vector_vel.def("deriv", &VectorVel::deriv); + vector_vel.def("__repr__", [](const VectorVel &vv) + { + std::ostringstream oss; + oss << vv; + return oss.str(); + }); + vector_vel.def_static("Zero", &VectorVel::Zero); + vector_vel.def("ReverseSign", &VectorVel::ReverseSign); + vector_vel.def("Norm", &VectorVel::Norm, py::arg("eps")=epsilon); + vector_vel.def(py::self += py::self); + vector_vel.def(py::self -= py::self); + vector_vel.def(py::self + py::self); + vector_vel.def(py::self - py::self); + vector_vel.def(Vector() + py::self); + vector_vel.def(Vector() - py::self); + vector_vel.def(py::self + Vector()); + vector_vel.def(py::self - Vector()); + + vector_vel.def(py::self * py::self); + vector_vel.def(py::self * Vector()); + vector_vel.def(Vector() * py::self); + vector_vel.def(double() * py::self); + vector_vel.def(py::self * double()); + vector_vel.def(doubleVel() * py::self); + vector_vel.def(py::self * doubleVel()); + vector_vel.def(Rotation() * py::self); + + vector_vel.def(py::self / double()); + vector_vel.def(py::self / doubleVel()); + + vector_vel.def(py::self == py::self); + vector_vel.def(py::self != py::self); + vector_vel.def(Vector() == py::self); + vector_vel.def(Vector() != py::self); + vector_vel.def(py::self == Vector()); + vector_vel.def(py::self != Vector()); + vector_vel.def("__neg__", [](const VectorVel &a) + { + return operator-(a); + }, py::is_operator()); + vector_vel.def(py::pickle( + [](const VectorVel &vv) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(vv.p, vv.v); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + VectorVel vv(t[0].cast(), t[1].cast()); + return vv; + })); + + m.def("SetToZero", (void (*)(VectorVel&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const VectorVel&, const VectorVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Vector&, const VectorVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const VectorVel&, const Vector&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + m.def("dot", (doubleVel (*)(const VectorVel&, const VectorVel&)) &KDL::dot); + m.def("dot", (doubleVel (*)(const VectorVel&, const Vector&)) &KDL::dot); + m.def("dot", (doubleVel (*)(const Vector&, const VectorVel&)) &KDL::dot); + + + // -------------------- + // TwistVel + // -------------------- + py::class_ twist_vel(m, "TwistVel"); + twist_vel.def_readwrite("vel", &TwistVel::vel); + twist_vel.def_readwrite("rot", &TwistVel::rot); + twist_vel.def(py::init<>()); + twist_vel.def(py::init()); + twist_vel.def(py::init()); + twist_vel.def(py::init()); + twist_vel.def(py::init()); + twist_vel.def("value", &TwistVel::value); + twist_vel.def("deriv", &TwistVel::deriv); + twist_vel.def("__repr__", [](const TwistVel &tv) + { + std::ostringstream oss; + oss << tv; + return oss.str(); + }); + twist_vel.def_static("Zero", &TwistVel::Zero); + twist_vel.def("ReverseSign", &TwistVel::ReverseSign); + twist_vel.def("RefPoint", &TwistVel::RefPoint); + twist_vel.def("GetTwist", &TwistVel::GetTwist); + twist_vel.def("GetTwistDot", &TwistVel::GetTwistDot); + + twist_vel.def(py::self -= py::self); + twist_vel.def(py::self += py::self); + twist_vel.def(py::self * double()); + twist_vel.def(double() * py::self); + twist_vel.def(py::self / double()); + + twist_vel.def(py::self * doubleVel()); + twist_vel.def(doubleVel() * py::self); + twist_vel.def(py::self / doubleVel()); + + twist_vel.def(py::self + py::self); + twist_vel.def(py::self - py::self); + + twist_vel.def(py::self == py::self); + twist_vel.def(py::self != py::self); + twist_vel.def(Twist() == py::self); + twist_vel.def(Twist() != py::self); + twist_vel.def(py::self == Twist()); + twist_vel.def(py::self != Twist()); + twist_vel.def("__neg__", [](const TwistVel &a) + { + return operator-(a); + }, py::is_operator()); + twist_vel.def(py::pickle( + [](const TwistVel &tv) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(tv.vel, tv.rot); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + TwistVel tv(t[0].cast(), t[1].cast()); + return tv; + })); + + m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const TwistVel&, const TwistVel&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Twist&, const TwistVel&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const TwistVel&, const Twist&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // RotationVel + // -------------------- + py::class_ rotation_vel(m, "RotationVel"); + rotation_vel.def_readwrite("R", &RotationVel::R); + rotation_vel.def_readwrite("w", &RotationVel::w); + rotation_vel.def(py::init<>()); + rotation_vel.def(py::init()); + rotation_vel.def(py::init()); + rotation_vel.def(py::init()); + rotation_vel.def("value", &RotationVel::value); + rotation_vel.def("deriv", &RotationVel::deriv); + rotation_vel.def("__repr__", [](const RotationVel &rv) + { + std::ostringstream oss; + oss << rv; + return oss.str(); + }); + rotation_vel.def("UnitX", &RotationVel::UnitX); + rotation_vel.def("UnitY", &RotationVel::UnitY); + rotation_vel.def("UnitZ", &RotationVel::UnitZ); + rotation_vel.def_static("Identity", &RotationVel::Identity); + rotation_vel.def("Inverse", (RotationVel (RotationVel::*)(void) const) &RotationVel::Inverse); + rotation_vel.def("Inverse", (VectorVel (RotationVel::*)(const VectorVel&) const) &RotationVel::Inverse); + rotation_vel.def("Inverse", (VectorVel (RotationVel::*)(const Vector&) const) &RotationVel::Inverse); + rotation_vel.def("DoRotX", &RotationVel::DoRotX); + rotation_vel.def("DoRotY", &RotationVel::DoRotY); + rotation_vel.def("DoRotZ", &RotationVel::DoRotZ); + rotation_vel.def_static("RotX", &RotationVel::RotX); + rotation_vel.def_static("RotY", &RotationVel::RotY); + rotation_vel.def_static("RotZ", &RotationVel::RotZ); + rotation_vel.def_static("Rot", &RotationVel::Rot); + rotation_vel.def_static("Rot2", &RotationVel::Rot2); + + rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const TwistVel&) const) &RotationVel::Inverse); + rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const Twist&) const) &RotationVel::Inverse); + + rotation_vel.def(py::self * VectorVel()); + rotation_vel.def(py::self * Vector()); + rotation_vel.def(py::self * TwistVel()); + rotation_vel.def(py::self * Twist()); + rotation_vel.def(py::self * py::self); + rotation_vel.def(Rotation() * py::self); + rotation_vel.def(py::self * Rotation()); + + rotation_vel.def(py::self == py::self); + rotation_vel.def(py::self != py::self); + rotation_vel.def(Rotation() == py::self); + rotation_vel.def(Rotation() != py::self); + rotation_vel.def(py::self == Rotation()); + rotation_vel.def(py::self != Rotation()); + rotation_vel.def(py::pickle( + [](const RotationVel &rv) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(rv.R, rv.w); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + RotationVel rv(t[0].cast(), t[1].cast()); + return rv; + })); + + m.def("Equal", (bool (*)(const RotationVel&, const RotationVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Rotation&, const RotationVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const RotationVel&, const Rotation&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + + // -------------------- + // FrameVel + // -------------------- + py::class_ frame_vel(m, "FrameVel"); + frame_vel.def_readwrite("M", &FrameVel::M); + frame_vel.def_readwrite("p", &FrameVel::p); + frame_vel.def(py::init<>()); + frame_vel.def(py::init()); + frame_vel.def(py::init()); + frame_vel.def(py::init()); + frame_vel.def(py::init()); + frame_vel.def("value", &FrameVel::value); + frame_vel.def("deriv", &FrameVel::deriv); + frame_vel.def("__repr__", [](const FrameVel &fv) + { + std::ostringstream oss; + oss << fv; + return oss.str(); + }); + frame_vel.def_static("Identity", &FrameVel::Identity); + frame_vel.def("Inverse", (FrameVel (FrameVel::*)() const) &FrameVel::Inverse); + frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const VectorVel&) const) &FrameVel::Inverse); + frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const Vector&) const) &FrameVel::Inverse); + frame_vel.def(py::self * VectorVel()); + frame_vel.def(py::self * Vector()); + frame_vel.def("GetFrame", &FrameVel::GetFrame); + frame_vel.def("GetTwist", &FrameVel::GetTwist); + frame_vel.def("Inverse", (TwistVel (FrameVel::*)(const TwistVel&) const) &FrameVel::Inverse); + frame_vel.def("Inverse", (TwistVel (FrameVel::*)(const Twist&) const) &FrameVel::Inverse); + frame_vel.def(py::self * TwistVel()); + frame_vel.def(py::self * Twist()); + frame_vel.def(py::self * py::self); + frame_vel.def(Frame() * py::self); + frame_vel.def(py::self * Frame()); + + frame_vel.def(py::self == py::self); + frame_vel.def(py::self != py::self); + frame_vel.def(Frame() == py::self); + frame_vel.def(Frame() != py::self); + frame_vel.def(py::self == Frame()); + frame_vel.def(py::self != Frame()); + frame_vel.def(py::pickle( + [](const FrameVel &fv) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(fv.M, fv.p); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + FrameVel rv(t[0].cast(), t[1].cast()); + return rv; + })); + + m.def("Equal", (bool (*)(const FrameVel&, const FrameVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Frame&, const FrameVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const FrameVel&, const Frame&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + + // -------------------- + // Global + // -------------------- + m.def("diff", (VectorVel (*)(const VectorVel&, const VectorVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + m.def("diff", (VectorVel (*)(const RotationVel&, const RotationVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + m.def("diff", (TwistVel (*)(const FrameVel&, const FrameVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + + m.def("addDelta", (VectorVel (*)(const VectorVel&, const VectorVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); + m.def("addDelta", (RotationVel (*)(const RotationVel&, const VectorVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); + m.def("addDelta", (FrameVel (*)(const FrameVel&, const TwistVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); +} diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp new file mode 100644 index 000000000..8eaee9c5a --- /dev/null +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -0,0 +1,486 @@ +//Copyright (C) 2020 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits Ruben Smits +//Author: Zihan Chen +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library is free software; you can redistribute it and/or +//modify it under the terms of the GNU Lesser General Public +//License as published by the Free Software Foundation; either +//version 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_kinfam(pybind11::module &m) +{ + // -------------------- + // Joint + // -------------------- + py::class_ joint(m, "Joint"); + py::enum_ joint_type(joint, "JointType"); + joint_type.value("RotAxis", Joint::JointType::RotAxis); + joint_type.value("RotX", Joint::JointType::RotX); + joint_type.value("RotY", Joint::JointType::RotY); + joint_type.value("RotZ", Joint::JointType::RotZ); + joint_type.value("TransAxis", Joint::JointType::TransAxis); + joint_type.value("TransX", Joint::JointType::TransX); + joint_type.value("TransY", Joint::JointType::TransY); + joint_type.value("TransZ", Joint::JointType::TransZ); + joint_type.value("Fixed", Joint::JointType::Fixed); +#if PY_VERSION_HEX < 0x03000000 + joint_type.value("None", Joint::JointType::None); +#endif + joint_type.export_values(); + + joint.def(py::init<>()); + joint.def(py::init(), + py::arg("name"), py::arg("type")=Joint::JointType::Fixed, py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init(), + py::arg("type")=Joint::JointType::Fixed, py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init(), + py::arg("name"), py::arg("origin"), py::arg("axis"), py::arg("type"), py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init(), + py::arg("origin"), py::arg("axis"), py::arg("type"), py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init()); + joint.def("pose", &Joint::pose); + joint.def("twist", &Joint::twist); + joint.def("JointAxis", &Joint::JointAxis); + joint.def("JointOrigin", &Joint::JointOrigin); + joint.def("getName", &Joint::getName); + joint.def("getType", &Joint::getType); + joint.def("getTypeName", &Joint::getTypeName); + joint.def("__repr__", [](const Joint &j) + { + std::ostringstream oss; + oss << j; + return oss.str(); + }); + + + // -------------------- + // RotationalInertia + // -------------------- + py::class_ rotational_inertia(m, "RotationalInertia"); + rotational_inertia.def(py::init(), + py::arg("Ixx")=0, py::arg("Iyy")=0, py::arg("Izz")=0, + py::arg("Ixy")=0, py::arg("Ixz")=0, py::arg("Iyz")=0); + rotational_inertia.def_static("Zero", &RotationalInertia::Zero); + rotational_inertia.def("__getitem__", [](const RotationalInertia &inertia, int i) + { + if (i < 0 || i > 8) + throw py::index_error("RotationalInertia index out of range"); + + return inertia.data[i]; + }); + rotational_inertia.def("__setitem__", [](RotationalInertia &inertia, int i, double value) + { + if (i < 0 || i > 8) + throw py::index_error("RotationalInertia index out of range"); + + inertia.data[i] = value; + }); + rotational_inertia.def(double() * py::self); + rotational_inertia.def(py::self + py::self); + rotational_inertia.def(py::self * Vector()); + + + // -------------------- + // RigidBodyInertia + // -------------------- + py::class_ rigid_body_inertia(m, "RigidBodyInertia"); + rigid_body_inertia.def(py::init(), + py::arg("m")=0, py::arg_v("oc", Vector::Zero(), "Vector.Zero"), + py::arg_v("Ic", RotationalInertia::Zero(), "RigidBodyInertia.Zero")); + rigid_body_inertia.def_static("Zero", &RigidBodyInertia::Zero); + rigid_body_inertia.def("RefPoint", &RigidBodyInertia::RefPoint); + rigid_body_inertia.def("getMass", &RigidBodyInertia::getMass); + rigid_body_inertia.def("getCOG", &RigidBodyInertia::getCOG); + rigid_body_inertia.def("getRotationalInertia", &RigidBodyInertia::getRotationalInertia); + rigid_body_inertia.def(double() * py::self); + rigid_body_inertia.def(py::self + py::self); + rigid_body_inertia.def(py::self * Twist()); + rigid_body_inertia.def(Frame() * py::self); + rigid_body_inertia.def(Rotation() * py::self); + + + // -------------------- + // Segment + // -------------------- + py::class_ segment(m, "Segment"); + segment.def(py::init(), + py::arg("name"), py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), + py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")); + segment.def(py::init(), + py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), + py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")); + segment.def(py::init()); + segment.def("getFrameToTip", &Segment::getFrameToTip); + segment.def("pose", &Segment::pose); + segment.def("twist", &Segment::twist); + segment.def("getName", &Segment::getName); + segment.def("getJoint", &Segment::getJoint); + segment.def("getInertia", &Segment::getInertia); + segment.def("setInertia", &Segment::setInertia); + + + // -------------------- + // Chain + // -------------------- + py::class_ chain(m, "Chain"); + chain.def(py::init<>()); + chain.def(py::init()); + chain.def("addSegment", &Chain::addSegment); + chain.def("addChain", &Chain::addChain); + chain.def("getNrOfJoints", &Chain::getNrOfJoints); + chain.def("getNrOfSegments", &Chain::getNrOfSegments); + chain.def("getSegment", (Segment& (Chain::*)(unsigned int)) &Chain::getSegment); + chain.def("getSegment", (const Segment& (Chain::*)(unsigned int) const) &Chain::getSegment); + chain.def("__repr__", [](const Chain &c) + { + std::ostringstream oss; + oss << c; + return oss.str(); + }); + + + // -------------------- + // Tree + // -------------------- + py::class_ tree(m, "Tree"); + tree.def(py::init(), py::arg("root_name")="root"); + tree.def("addSegment", &Tree::addSegment); + tree.def("addChain", &Tree::addChain); + tree.def("addTree", &Tree::addTree); + tree.def("getNrOfJoints", &Tree::getNrOfJoints); + tree.def("getNrOfSegments", &Tree::getNrOfSegments); + tree.def("getChain", [](const Tree &tree, const std::string& chain_root, const std::string& chain_tip) + { + Chain* chain = new Chain(); + tree.getChain(chain_root, chain_tip, *chain); + return chain; + }); + tree.def("__repr__", [](const Tree &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + + + // -------------------- + // Jacobian + // -------------------- + py::class_ jacobian(m, "Jacobian"); + jacobian.def(py::init<>()); + jacobian.def(py::init()); + jacobian.def(py::init()); + jacobian.def("rows", &Jacobian::rows); + jacobian.def("columns", &Jacobian::columns); + jacobian.def("resize", &Jacobian::resize); + jacobian.def("getColumn", &Jacobian::getColumn); + jacobian.def("setColumn", &Jacobian::setColumn); + jacobian.def("changeRefPoint", &Jacobian::changeRefPoint); + jacobian.def("changeBase", &Jacobian::changeBase); + jacobian.def("changeRefFrame", &Jacobian::changeRefFrame); + jacobian.def("__getitem__", [](const Jacobian &jac, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || j >= jac.columns()) + throw py::index_error("Jacobian index out of range"); + return jac((unsigned int)i, (unsigned int)j); + }); + jacobian.def("__setitem__", [](Jacobian &jac, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || j >= jac.columns()) + throw py::index_error("Jacobian index out of range"); + + jac((unsigned int)i, (unsigned int)j) = value; + }); + jacobian.def("__repr__", [](const Jacobian &jac) + { + std::ostringstream oss; + oss << jac; + return oss.str(); + }); + + m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero); + m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint); + m.def("changeBase", (void (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase); + m.def("SetToZero", (void (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame); + + + // -------------------- + // JntArray + // -------------------- + py::class_ jnt_array(m, "JntArray"); + jnt_array.def(py::init<>()); + jnt_array.def(py::init()); + jnt_array.def(py::init()); + jnt_array.def("rows", &JntArray::rows); + jnt_array.def("columns", &JntArray::columns); + jnt_array.def("resize", &JntArray::resize); + jnt_array.def("__getitem__", [](const JntArray &ja, int i) + { + if (i < 0 || i >= ja.rows()) + throw py::index_error("JntArray index out of range"); + + return ja(i); + }); + jnt_array.def("__setitem__", [](JntArray &ja, int i, double value) + { + if (i < 0 || i >= ja.rows()) + throw py::index_error("JntArray index out of range"); + + ja(i) = value; + }); + jnt_array.def("__repr__", [](const JntArray &ja) + { + std::ostringstream oss; + oss << ja; + return oss.str(); + }); + jnt_array.def(py::self == py::self); + + m.def("Add", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Add); + m.def("Subtract", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Subtract); + m.def("Multiply", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Multiply); + m.def("Divide", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Divide); + m.def("MultiplyJacobian", (void (*)(const Jacobian&, const JntArray&, Twist&)) &KDL::MultiplyJacobian); + m.def("SetToZero", (void (*)(JntArray&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const JntArray&, const JntArray&, double)) &KDL::Equal, + py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); + + + // -------------------- + // JntArrayVel + // -------------------- + py::class_ jnt_array_vel(m, "JntArrayVel"); + jnt_array_vel.def_readwrite("q", &JntArrayVel::q); + jnt_array_vel.def_readwrite("qdot", &JntArrayVel::qdot); + jnt_array_vel.def(py::init()); + jnt_array_vel.def(py::init()); + jnt_array_vel.def(py::init()); + jnt_array_vel.def("resize", &JntArrayVel::resize); + jnt_array_vel.def("value", &JntArrayVel::value); + jnt_array_vel.def("deriv", &JntArrayVel::deriv); + + m.def("Add", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Add); + m.def("Add", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Add); + m.def("Subtract", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Subtract); + m.def("Subtract", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Subtract); + m.def("Multiply", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Multiply); + m.def("Multiply", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Multiply); + m.def("Divide", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Divide); + m.def("Divide", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Divide); + m.def("SetToZero", (void (*)(JntArrayVel&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const JntArrayVel&, const JntArrayVel&, double)) &KDL::Equal, + py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); + + + // -------------------- + // SolverI + // -------------------- + py::class_ solver_i(m, "SolverI"); + solver_i.def("getError", &SolverI::getError); + solver_i.def("strError", &SolverI::strError); + solver_i.def("updateInternalDataStructures", &SolverI::updateInternalDataStructures); + + + // -------------------- + // ChainFkSolverPos + // -------------------- + py::class_ chain_fk_solver_pos(m, "ChainFkSolverPos"); + chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, Frame&, int)) &ChainFkSolverPos::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); + chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, std::vector&, int)) &ChainFkSolverPos::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); + + + // -------------------- + // ChainFkSolverVel + // -------------------- + py::class_ chain_fk_solver_vel(m, "ChainFkSolverVel"); + chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, FrameVel&, int)) &ChainFkSolverVel::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); +// Argument by reference doesn't work for container types +// chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, std::vector&, int)) &ChainFkSolverVel::JntToCart, +// py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); + + + // ------------------------------ + // ChainFkSolverPos_recursive + // ------------------------------ + py::class_ chain_fk_solver_pos_recursive(m, "ChainFkSolverPos_recursive"); + chain_fk_solver_pos_recursive.def(py::init()); + + + // ------------------------------ + // ChainFkSolverVel_recursive + // ------------------------------ + py::class_ chain_fk_solver_vel_recursive(m, "ChainFkSolverVel_recursive"); + chain_fk_solver_vel_recursive.def(py::init()); + + + // -------------------- + // ChainIkSolverPos + // -------------------- + py::class_ chain_ik_solver_pos(m, "ChainIkSolverPos"); + chain_ik_solver_pos.def("CartToJnt", (int (ChainIkSolverPos::*)(const JntArray&, const Frame&, JntArray&)) &ChainIkSolverPos::CartToJnt, + py::arg("q_init"), py::arg("p_in"), py::arg("q_out")); + + + // -------------------- + // ChainIkSolverVel + // -------------------- + py::class_ chain_ik_solver_vel(m, "ChainIkSolverVel"); + chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const Twist&, JntArray&)) &ChainIkSolverVel::CartToJnt, + py::arg("q_in"), py::arg("v_in"), py::arg("qdot_out")); +// Argument by reference doesn't work for container types +// chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const FrameVel&, JntArrayVel&)) &ChainIkSolverVel::CartToJnt, +// py::arg("q_init"), py::arg("v_in"), py::arg("q_out")); + + + // ---------------------- + // ChainIkSolverPos_NR + // ---------------------- + py::class_ chain_ik_solver_pos_NR(m, "ChainIkSolverPos_NR"); + chain_ik_solver_pos_NR.def(py::init(), + py::arg("chain"), py::arg("fksolver"), py::arg("iksolver"), + py::arg("maxiter")=100, py::arg("eps")=epsilon); + + + // --------------------------- + // ChainIkSolverPos_NR_JL + // --------------------------- + py::class_ chain_ik_solver_pos_NR_JL(m, "ChainIkSolverPos_NR_JL"); + chain_ik_solver_pos_NR_JL.def(py::init(), + py::arg("chain"), py::arg("q_min"), py::arg("q_max"), py::arg("fksolver"), + py::arg("iksolver"), py::arg("maxiter")=100, py::arg("eps")=epsilon); + + + // ------------------------- + // ChainIkSolverVel_pinv + // ------------------------- + py::class_ chain_ik_solver_vel_pinv(m, "ChainIkSolverVel_pinv"); + chain_ik_solver_vel_pinv.def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150); + + + // ------------------------- + // ChainIkSolverVel_wdls + // ------------------------- + py::class_ chain_ik_solver_vel_wdls(m, "ChainIkSolverVel_wdls"); + chain_ik_solver_vel_wdls.def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150); + chain_ik_solver_vel_wdls.def("setWeightTS", &ChainIkSolverVel_wdls::setWeightTS); + chain_ik_solver_vel_wdls.def("setWeightJS", &ChainIkSolverVel_wdls::setWeightJS); + chain_ik_solver_vel_wdls.def("setLambda", &ChainIkSolverVel_wdls::setLambda); + chain_ik_solver_vel_wdls.def("setEps", &ChainIkSolverVel_wdls::setEps); + chain_ik_solver_vel_wdls.def("setMaxIter", &ChainIkSolverVel_wdls::setMaxIter); + chain_ik_solver_vel_wdls.def("getNrZeroSigmas", &ChainIkSolverVel_wdls::getNrZeroSigmas); + chain_ik_solver_vel_wdls.def("getSigmaMin", &ChainIkSolverVel_wdls::getSigmaMin); + chain_ik_solver_vel_wdls.def("getSigma", &ChainIkSolverVel_wdls::getSigma); + chain_ik_solver_vel_wdls.def("getEps", &ChainIkSolverVel_wdls::getEps); + chain_ik_solver_vel_wdls.def("getLambda", &ChainIkSolverVel_wdls::getLambda); + chain_ik_solver_vel_wdls.def("getLambdaScaled", &ChainIkSolverVel_wdls::getLambdaScaled); + chain_ik_solver_vel_wdls.def("getSVDResult", &ChainIkSolverVel_wdls::getSVDResult); + + + // ------------------------- + // ChainIkSolverPos_LMA + // ------------------------- + py::class_ chain_ik_solver_pos_LMA(m, "ChainIkSolverPos_LMA"); + chain_ik_solver_pos_LMA.def(py::init(), + py::arg("chain"), py::arg("eps")=1e-5, py::arg("maxiter")=500, + py::arg("eps_joints")=1e-15); + + + // ---------------------------- + // ChainIkSolverVel_pinv_nso + // ---------------------------- + py::class_ chain_ik_solver_vel_pinv_nso(m, "ChainIkSolverVel_pinv_nso"); + chain_ik_solver_vel_pinv_nso.def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150, py::arg("alpha")=0.25); + chain_ik_solver_vel_pinv_nso.def("setWeights", &ChainIkSolverVel_pinv_nso::setWeights); + chain_ik_solver_vel_pinv_nso.def("setOptPos", &ChainIkSolverVel_pinv_nso::setOptPos); + chain_ik_solver_vel_pinv_nso.def("setAlpha", &ChainIkSolverVel_pinv_nso::setAlpha); + chain_ik_solver_vel_pinv_nso.def("getWeights", &ChainIkSolverVel_pinv_nso::getWeights); + chain_ik_solver_vel_pinv_nso.def("getOptPos", &ChainIkSolverVel_pinv_nso::getOptPos); + chain_ik_solver_vel_pinv_nso.def("getAlpha", &ChainIkSolverVel_pinv_nso::getAlpha); + + + // ------------------------------- + // ChainIkSolverVel_pinv_givens + // ------------------------------- + py::class_ chain_ik_solver_vel_pinv_givens(m, "ChainIkSolverVel_pinv_givens"); + chain_ik_solver_vel_pinv_givens.def(py::init()); + + + // ------------------------------ + // ChainJntToJacSolver + // ------------------------------ + py::class_ chain_jnt_to_jac_solver(m, "ChainJntToJacSolver"); + chain_jnt_to_jac_solver.def(py::init()); + chain_jnt_to_jac_solver.def("JntToJac", &ChainJntToJacSolver::JntToJac, + py::arg("q_in"), py::arg("jac"), py::arg("segmentNR")=-1); + chain_jnt_to_jac_solver.def("setLockedJoints", &ChainJntToJacSolver::setLockedJoints); + + + // ------------------------------ + // ChainIdSolver + // ------------------------------ + py::class_ chain_id_solver(m, "ChainIdSolver"); + chain_id_solver.def("CartToJnt", &ChainIdSolver::CartToJnt); + + + // ------------------------------ + // ChainIdSolver_RNE + // ------------------------------ + py::class_ chain_id_solver_RNE(m, "ChainIdSolver_RNE"); + chain_id_solver_RNE.def(py::init()); +} diff --git a/python_orocos_kdl/PyKDL/PyKDL.sip b/python_orocos_kdl/PyKDL/sip/PyKDL.sip similarity index 63% rename from python_orocos_kdl/PyKDL/PyKDL.sip rename to python_orocos_kdl/PyKDL/sip/PyKDL.sip index 1de04528c..d5d60756c 100644 --- a/python_orocos_kdl/PyKDL/PyKDL.sip +++ b/python_orocos_kdl/PyKDL/sip/PyKDL.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or @@ -20,13 +20,21 @@ //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -%Module(name = PyKDL,version=0,keyword_arguments="All") +%Module(name = PyKDL, keyword_arguments="All") -%License(type="LGPL",licensee="Ruben Smits",signature="ruben@intermodalics.eu",timestamp="2014") +%License(type="LGPL", licensee="Ruben Smits", signature="ruben@intermodalics.eu", timestamp="2020") %Include std_string.sip +%Include std_vector.sip + %Include frames.sip -%Include kinfam.sip %Include framevel.sip - +%Include kinfam.sip %Include dynamics.sip + +const std::string __version__; + +%ModuleCode +#include +const std::string __version__ = KDL_VERSION_STRING; +%End diff --git a/python_orocos_kdl/PyKDL/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip similarity index 72% rename from python_orocos_kdl/PyKDL/dynamics.sip rename to python_orocos_kdl/PyKDL/sip/dynamics.sip index e0096ddc8..c5efb5b7b 100644 --- a/python_orocos_kdl/PyKDL/dynamics.sip +++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or @@ -19,11 +19,11 @@ //License along with this library; if not, write to the Free Software //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -%Include std_string.sip class JntSpaceInertiaMatrix { %TypeHeaderCode #include +#include using namespace KDL; %End public: @@ -33,17 +33,38 @@ public: void resize(unsigned int newSize); unsigned int rows()const; unsigned int columns()const; - //JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg); double __getitem__(SIP_PYTUPLE); %MethodCode int i,j; PyArg_ParseTuple(a0,"ii",&i,&j); - if (i < 0 || j < 0 || i > (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { + if (i < 0 || j < 0 || i >= (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { PyErr_SetString(PyExc_IndexError, "Inertia index out of range"); - return NULL; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(i,j); } - sipRes=(*sipCpp)(i,j); %End + void __setitem__(SIP_PYTUPLE,double value); +%MethodCode + int i,j; + PyArg_ParseTuple(a0,"ii",&i,&j); + if (i < 0 || j < 0 || i >= (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { + PyErr_SetString(PyExc_IndexError, "Inertia index out of range"); + sipError = sipErrorFail; + } + else { + (*sipCpp)(i,j)=a1; + } +%End + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + //JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg); //double operator()(unsigned int i,unsigned int j)const; //double& operator()(unsigned int i,unsigned int j); //bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); @@ -60,7 +81,7 @@ bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,d bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); class ChainDynParam { - + %TypeHeaderCode #include using namespace KDL; @@ -70,6 +91,6 @@ public: ChainDynParam(const Chain& chain, Vector _grav); int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis); - int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H); - int JntToGravity(const JntArray &q,JntArray &gravity); + int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H); + int JntToGravity(const JntArray &q,JntArray &gravity); }; diff --git a/python_orocos_kdl/PyKDL/frames.sip b/python_orocos_kdl/PyKDL/sip/frames.sip similarity index 89% rename from python_orocos_kdl/PyKDL/frames.sip rename to python_orocos_kdl/PyKDL/sip/frames.sip index 07a9d24c4..d0be8ba58 100644 --- a/python_orocos_kdl/PyKDL/frames.sip +++ b/python_orocos_kdl/PyKDL/sip/frames.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or @@ -45,19 +45,23 @@ public: double __getitem__ (int index) const; %MethodCode if (a0 < 0 || a0 > 2) { - PyErr_SetString(PyExc_IndexError, "Vector index out of range"); - return 0; + PyErr_SetString(PyExc_IndexError, "Vector index out of range"); + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(a0); } - sipRes=(*sipCpp)(a0); %End void __setitem__(int index, double value); %MethodCode if (a0 < 0 || a0 > 2) { PyErr_SetString(PyExc_IndexError, "Vector index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(a0)=a1; } - (*sipCpp)(a0)=a1; %End const std::string* __repr__() const; @@ -75,7 +79,7 @@ public: static Vector Zero()/Factory/; - double Norm(); + double Norm(double eps=epsilon) const; double Normalize(double eps=epsilon); %PickleCode @@ -111,16 +115,18 @@ public: double Xy,double Yy,double Zy, double Xz,double Yz,double Zz); Rotation(const Vector& x,const Vector& y,const Vector& z); - + double __getitem__(SIP_PYTUPLE) const; %MethodCode int i,j; PyArg_ParseTuple(a0, "ii", &i, &j); if (i < 0 || j < 0 || i > 2 || j > 2) { PyErr_SetString(PyExc_IndexError, "Rotation index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=((const Rotation)(*sipCpp))(i,j); } - sipRes=((const Rotation)(*sipCpp))(i,j); %End void __setitem__(SIP_PYTUPLE,double value); @@ -129,9 +135,11 @@ public: PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 2 || j > 2) { PyErr_SetString(PyExc_IndexError, "Rotation index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(i,j)=a1; } - (*sipCpp)(i,j)=a1; %End const std::string* __repr__() const; @@ -141,13 +149,13 @@ public: std::string s(oss.str()); sipRes=&s; %End - + void SetInverse(); Rotation Inverse() const /Factory/; Vector Inverse(const Vector& v) const /Factory/; Wrench Inverse(const Wrench& w) const /Factory/; Twist Inverse(const Twist& t) const /Factory/; - + static Rotation Identity()/Factory/; static Rotation RotX(double angle)/Factory/; static Rotation RotY(double angle)/Factory/; @@ -158,12 +166,12 @@ public: static Rotation RPY(double roll,double pitch,double yaw)/Factory/; static Rotation EulerZYX(double Alfa,double Beta,double Gamma)/Factory/; static Rotation Quaternion(double x, double y, double z, double w)/Factory/; - + void DoRotX(double angle); void DoRotY(double angle); void DoRotZ(double angle); - + Vector GetRot() const /Factory/; double GetRotAngle(Vector& axis /Out/,double eps=epsilon) const; void GetEulerZYZ(double& alfa /Out/,double& beta /Out/,double& gamma /Out/) const; @@ -171,11 +179,11 @@ public: void GetEulerZYX(double& Alfa /Out/,double& Beta /Out/,double& Gamma /Out/) const; void GetQuaternion(double& x /Out/,double& y /Out/,double& z /Out/, double& w) const; - + Vector operator*(const Vector& v) const /Numeric,Factory/; Twist operator*(const Twist& arg) const /Numeric,Factory/; Wrench operator*(const Wrench& arg) const /Numeric,Factory/; - + Vector UnitX() const /Factory/; Vector UnitY() const /Factory/; Vector UnitZ() const /Factory/; @@ -209,19 +217,21 @@ public: Frame(const Vector& V); Frame(const Rotation& R); Frame(); - + Vector p; Rotation M; - + double __getitem__ (SIP_PYTUPLE) const; %MethodCode int i,j; PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 2 || j > 3) { PyErr_SetString(PyExc_IndexError, "Frame index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(i,j); } - sipRes=(*sipCpp)(i,j); %End void __setitem__(SIP_PYTUPLE,double value); @@ -230,12 +240,14 @@ public: PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 2 || j > 3) { PyErr_SetString(PyExc_IndexError, "Frame index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + if(j==3) + (*sipCpp).p(i)=a1; + else + (*sipCpp).M(i,j)=a1; } - if(j==3) - (*sipCpp).p(i)=a1; - else - (*sipCpp).M(i,j)=a1; %End const std::string* __repr__() const; @@ -256,9 +268,8 @@ public: Vector operator*(const Vector& arg) const /Numeric,Factory/; Wrench operator * (const Wrench& arg) const /Numeric,Factory/; Twist operator * (const Twist& arg) const /Numeric,Factory/; - + static Frame Identity() /Factory/; - void Integrate(const Twist& t_this,double frequency); %PickleCode @@ -274,7 +285,7 @@ bool Equal(const Frame& a,const Frame& b,double eps=epsilon); bool operator==(const Frame& a,const Frame& b); bool operator!=(const Frame& a,const Frame& b); -class Twist +class Twist { %TypeHeaderCode @@ -286,10 +297,10 @@ using namespace KDL; public: Vector vel; Vector rot; - + Twist(); Twist(const Vector& _vel,const Vector& _rot); - + Twist& operator-=(const Twist& arg); Twist& operator+=(const Twist& arg); @@ -297,18 +308,22 @@ public: %MethodCode if (a0 < 0 || a0 > 5) { PyErr_SetString(PyExc_IndexError, "Twist index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(a0); } - sipRes=(*sipCpp)(a0); %End void __setitem__(int i, double value); %MethodCode if (a0 < 0 || a0 > 5) { PyErr_SetString(PyExc_IndexError, "Twist index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(a0)=a1; } - (*sipCpp)(a0)=a1; %End const std::string* __repr__() const; @@ -321,9 +336,9 @@ public: static Twist Zero() /Factory/; void ReverseSign(); - + Twist RefPoint(const Vector& v_base_AB) const /Factory/; - + %PickleCode const sipTypeDef *vector_type = sipFindType("Vector"); sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->vel), vector_type, Py_None), @@ -347,7 +362,7 @@ bool operator!=(const Twist& a,const Twist& b); class Wrench { - + %TypeHeaderCode #include #include @@ -358,10 +373,10 @@ using namespace KDL; public: Vector force; Vector torque; - + Wrench(); Wrench(const Vector& force,const Vector& torque); - + Wrench& operator-=(const Wrench& arg); Wrench& operator+=(const Wrench& arg); @@ -369,19 +384,23 @@ public: double __getitem__ (int i) const; %MethodCode if (a0 < 0 || a0 > 5) { - PyErr_SetString(PyExc_IndexError, "Twist index out of range"); - return 0; + PyErr_SetString(PyExc_IndexError, "Wrench index out of range"); + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(a0); } - sipRes=(*sipCpp)(a0); %End void __setitem__(int i, double value); %MethodCode if (a0 < 0 || a0 > 5) { - PyErr_SetString(PyExc_IndexError, "Twist index out of range"); - return 0; + PyErr_SetString(PyExc_IndexError, "Wrench index out of range"); + sipError = sipErrorFail; + } + else { + (*sipCpp)(a0)=a1; } - (*sipCpp)(a0)=a1; %End const std::string* __repr__() const; diff --git a/python_orocos_kdl/PyKDL/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip similarity index 73% rename from python_orocos_kdl/PyKDL/framevel.sip rename to python_orocos_kdl/PyKDL/sip/framevel.sip index a3e0ad0ac..38c561203 100644 --- a/python_orocos_kdl/PyKDL/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or @@ -19,46 +19,74 @@ //License along with this library; if not, write to the Free Software //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + class doubleVel { - + %TypeHeaderCode #include +#include using namespace KDL; %End public: - typedef Rall1d doubleVel; + doubleVel(); + doubleVel(const double c); + doubleVel(const double tn, const double afg); + doubleVel(const doubleVel& r); double t; double grad; + double value(); + double deriv(); + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End }; +doubleVel operator - (const doubleVel& r); + doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0); doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0); + bool Equal(const doubleVel& r1,const doubleVel& r2,double eps=epsilon); -//bool Equal(const double& r1,const doubleVel& r2,double eps=epsilon); -//bool Equal(const doubleVel& r1,const double& r2,double eps=epsilon); + +bool operator==(const doubleVel& r1,const doubleVel& r2); +bool operator!=(const doubleVel& r1,const doubleVel& r2); + class VectorVel { %TypeHeaderCode #include +#include using namespace KDL; %End public: Vector p; - Vector v; - + Vector v; + VectorVel(); VectorVel(const Vector& _p,const Vector& _v); VectorVel(const Vector& _p); Vector value() const; Vector deriv() const; + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + VectorVel& operator += (const VectorVel& arg); VectorVel& operator -= (const VectorVel& arg); static VectorVel Zero(); void ReverseSign(); - doubleVel Norm() const; + doubleVel Norm(double eps=epsilon) const; %PickleCode const sipTypeDef *vector_type = sipFindType("Vector"); @@ -73,6 +101,7 @@ VectorVel operator + (const Vector& r1,const VectorVel& r2); VectorVel operator - (const Vector& r1,const VectorVel& r2); VectorVel operator + (const VectorVel& r1,const Vector& r2); VectorVel operator - (const VectorVel& r1,const Vector& r2); + VectorVel operator * (const VectorVel& r1,const VectorVel& r2); VectorVel operator * (const VectorVel& r1,const Vector& r2); VectorVel operator * (const Vector& r1,const VectorVel& r2); @@ -80,24 +109,33 @@ VectorVel operator * (const VectorVel& r1,double r2); VectorVel operator * (double r1,const VectorVel& r2); VectorVel operator * (const doubleVel& r1,const VectorVel& r2); VectorVel operator * (const VectorVel& r2,const doubleVel& r1); -VectorVel operator*(const Rotation& R,const VectorVel& x); +VectorVel operator * (const Rotation& R,const VectorVel& x); VectorVel operator / (const VectorVel& r1,double r2); VectorVel operator / (const VectorVel& r2,const doubleVel& r1); - +void SetToZero(VectorVel& v); bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon); bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon); bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon); +bool operator==(const VectorVel& r1,const VectorVel& r2); +bool operator!=(const VectorVel& r1,const VectorVel& r2); +bool operator==(const Vector& r1,const VectorVel& r2); +bool operator!=(const Vector& r1,const VectorVel& r2); +bool operator==(const VectorVel& r1,const Vector& r2); +bool operator!=(const VectorVel& r1,const Vector& r2); + VectorVel operator - (const VectorVel& r); doubleVel dot(const VectorVel& lhs,const VectorVel& rhs); doubleVel dot(const VectorVel& lhs,const Vector& rhs); doubleVel dot(const Vector& lhs,const VectorVel& rhs); + class RotationVel { %TypeHeaderCode #include +#include using namespace KDL; %End @@ -111,10 +149,18 @@ public: Rotation value() const; Vector deriv() const; + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End - VectorVel UnitX() const; - VectorVel UnitY() const; - VectorVel UnitZ() const; + + VectorVel UnitX() const; + VectorVel UnitY() const; + VectorVel UnitZ() const; static RotationVel Identity(); RotationVel Inverse() const; VectorVel Inverse(const VectorVel& arg) const; @@ -125,7 +171,7 @@ public: void DoRotY(const doubleVel& angle); void DoRotZ(const doubleVel& angle); static RotationVel RotX(const doubleVel& angle); - static RotationVel RotY(const doubleVel& angle); + static RotationVel RotY(const doubleVel& angle); static RotationVel RotZ(const doubleVel& angle); static RotationVel Rot(const Vector& rotvec,const doubleVel& angle); static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle); @@ -149,14 +195,20 @@ bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon); bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon); bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon); - +bool operator==(const RotationVel& r1,const RotationVel& r2); +bool operator!=(const RotationVel& r1,const RotationVel& r2); +bool operator==(const Rotation& r1,const RotationVel& r2); +bool operator!=(const Rotation& r1,const RotationVel& r2); +bool operator==(const RotationVel& r1,const Rotation& r2); +bool operator!=(const RotationVel& r1,const Rotation& r2); class FrameVel { - + %TypeHeaderCode #include +#include using namespace KDL; %End @@ -168,10 +220,18 @@ public: FrameVel(const Frame& _T); FrameVel(const Frame& _T,const Twist& _t); FrameVel(const RotationVel& _M,const VectorVel& _p); - + Frame value() const; Twist deriv() const; + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + static FrameVel Identity(); FrameVel Inverse() const; @@ -201,12 +261,21 @@ bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon); bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon); bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon); +bool operator==(const FrameVel& r1,const FrameVel& r2); +bool operator!=(const FrameVel& r1,const FrameVel& r2); +bool operator==(const Frame& r1,const FrameVel& r2); +bool operator!=(const Frame& r1,const FrameVel& r2); +bool operator==(const FrameVel& r1,const Frame& r2); +bool operator!=(const FrameVel& r1,const Frame& r2); + + class TwistVel { %TypeHeaderCode #include +#include using namespace KDL; -%End +%End public: VectorVel vel; @@ -219,11 +288,20 @@ public: Twist value() const; Twist deriv() const; + + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + TwistVel& operator-=(const TwistVel& arg); TwistVel& operator+=(const TwistVel& arg); static TwistVel Zero(); - + void ReverseSign(); TwistVel RefPoint(const VectorVel& v_base_AB); @@ -251,6 +329,13 @@ bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon); bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon); bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon); +bool operator==(const TwistVel& r1,const TwistVel& r2); +bool operator!=(const TwistVel& r1,const TwistVel& r2); +bool operator==(const Twist& r1,const TwistVel& r2); +bool operator!=(const Twist& r1,const TwistVel& r2); +bool operator==(const TwistVel& r1,const Twist& r2); +bool operator!=(const TwistVel& r1,const Twist& r2); + VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0); VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0); diff --git a/python_orocos_kdl/PyKDL/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip similarity index 66% rename from python_orocos_kdl/PyKDL/kinfam.sip rename to python_orocos_kdl/PyKDL/sip/kinfam.sip index d87fd00d6..a00bbe230 100644 --- a/python_orocos_kdl/PyKDL/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or @@ -19,6 +19,9 @@ //License along with this library; if not, write to the Free Software //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +%Feature PYTHON2 + class Joint{ %TypeHeaderCode @@ -29,18 +32,23 @@ using namespace KDL; public: - enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None}; - Joint(std::string name, JointType type=None,double scale=1,double offset=0, +%If (!PYTHON2) + enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed}; +%End +%If (PYTHON2) + enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed,None}; +%End + Joint(std::string name, JointType type=Fixed,double scale=1,double offset=0, double inertia=0,double damping=0,double stiffness=0); - Joint(JointType type=None,double scale=1,double offset=0, + Joint(JointType type=Fixed,double scale=1,double offset=0, double inertia=0,const double damping=0,double stiffness=0); Joint(std::string name, Vector origin, Vector axis, JointType type, double scale=1, double offset=0, - double inertia=0, double damping=0, double stiffness=0); + double inertia=0, double damping=0, double stiffness=0); Joint(Vector origin, Vector axis, JointType type, double scale=1, double offset=0, - double inertia=0, double damping=0, double stiffness=0); - + double inertia=0, double damping=0, double stiffness=0); + Joint(const Joint& in); - + Frame pose(const double& q)const /Factory/ ; Twist twist(const double& qdot)const /Factory/ ; Vector JointAxis() const /Factory/; @@ -49,27 +57,48 @@ public: JointType getType() const; std::string getTypeName() const; - const std::string* __repr__(); - %MethodCode - std::ostringstream oss; - oss<<(*sipCpp); - std::string s(oss.str()); - sipRes=&s; - %End + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End }; class RotationalInertia { %TypeHeaderCode #include -#include using namespace KDL; %End public: RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); - + static RotationalInertia Zero()/Factory/; - Vector operator*(Vector omega) const /Factory/; + + double __getitem__(int index); +%MethodCode + if (a0 < 0 || a0 >= 9) { + PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range"); + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp).data[a0]; + } +%End + + void __setitem__(int i, double value); +%MethodCode + if (a0 < 0 || a0 >= 9) { + PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range"); + sipError = sipErrorFail; + } + else { + (*sipCpp).data[a0]=a1; + } +%End + }; RotationalInertia operator*(double a, const RotationalInertia& I)/Factory/; RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib)/Factory/; @@ -79,14 +108,13 @@ class RigidBodyInertia { %TypeHeaderCode #include -#include using namespace KDL; %End public: RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero()); - static RigidBodyInertia Zero() /Factory/; + static RigidBodyInertia Zero() /Factory/; RigidBodyInertia RefPoint(const Vector& p) /Factory/; - double getMass()const /Factory/; + double getMass()const /Factory/; Vector getCOG() const /Factory/; RotationalInertia getRotationalInertia() const /Factory/; }; @@ -98,25 +126,25 @@ RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I) /Factory class Segment { - + %TypeHeaderCode #include #include using namespace KDL; %End public: - Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); - Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + Segment(const std::string& name, const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + Segment(const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); Segment(const Segment& in); - const std::string* __repr__(); + const std::string* __repr__() const; %MethodCode - std::stringstream ss; - ss<<(*sipCpp); - std::string s(ss.str()); + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); sipRes=&s; %End - + const Frame& getFrameToTip()const /Factory/; Frame pose(const double& q)const /Factory/ ; Twist twist(const double& q,const double& qdot)const /Factory/ ; @@ -128,22 +156,31 @@ public: class Chain { - + %TypeHeaderCode #include +#include using namespace KDL; %End public: Chain(); Chain(const Chain& in); - + void addSegment(const Segment& segment); void addChain(const Chain& chain); - + unsigned int getNrOfJoints()const; unsigned int getNrOfSegments()const; - + + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + const Segment& getSegment(unsigned int nr)const /Factory/; }; @@ -151,6 +188,7 @@ public: class Tree { %TypeHeaderCode #include +#include using namespace KDL; %End public: @@ -158,7 +196,16 @@ public: bool addSegment(const Segment& segment, const std::string& hook_name); unsigned int getNrOfJoints()const; unsigned int getNrOfSegments()const; - Chain* getChain(const std::string& chain_root, const std::string& chain_tip)const; + + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + + Chain* getChain(const std::string& chain_root, const std::string& chain_tip)const /Factory/; %MethodCode Chain* chain = new Chain(); sipCpp->getChain(*a0, *a1, *chain); @@ -170,10 +217,12 @@ class JntArray{ %TypeHeaderCode #include +#include using namespace KDL; %End public: + JntArray(); JntArray(unsigned int size); JntArray(const JntArray& arg); unsigned int rows()const; @@ -184,25 +233,29 @@ public: %MethodCode if (a0 < 0 || a0 >= (int)sipCpp->rows()) { PyErr_SetString(PyExc_IndexError, "JntArray index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(a0); } - sipRes=(*sipCpp)(a0); %End - + void __setitem__(int index, double value); %MethodCode if (a0 < 0 || a0 >= (int)sipCpp->rows()) { PyErr_SetString(PyExc_IndexError, "JntArray index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(a0)=a1; } - (*sipCpp)(a0)=a1; %End - const std::string* __repr__(); + const std::string* __repr__() const; %MethodCode - std::stringstream ss; - ss<data; - std::string s(ss.str()); + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); sipRes=&s; %End }; @@ -231,7 +284,7 @@ public: JntArrayVel(const JntArray& q,const JntArray& qdot); JntArrayVel(const JntArray& q); void resize(unsigned int newSize); - + JntArray value()const /Factory/; JntArray deriv()const /Factory/; }; @@ -250,10 +303,12 @@ bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon); class Jacobian { %TypeHeaderCode -#include +#include +#include using namespace KDL; %End public: + Jacobian(); Jacobian(unsigned int size); Jacobian(const Jacobian& arg); unsigned int rows()const; @@ -266,39 +321,43 @@ public: PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) { PyErr_SetString(PyExc_IndexError, "Jacobian index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(i,j); } - sipRes=(*sipCpp)(i,j); %End - + void __setitem__(SIP_PYTUPLE,double value); %MethodCode int i,j; PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) { PyErr_SetString(PyExc_IndexError, "Jacobian index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(i,j)=a1; } - (*sipCpp)(i,j)=a1; %End - const std::string* __repr__(); + const std::string* __repr__() const; %MethodCode - std::stringstream ss; - ss<data; - std::string s(ss.str()); + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); sipRes=&s; %End Twist getColumn(unsigned int i) const /Factory/; void setColumn(unsigned int i,const Twist& t); - + void changeRefPoint(const Vector& base_AB); void changeBase(const Rotation& rot); void changeRefFrame(const Frame& frame); }; void SetToZero(Jacobian& jac); - + void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest); void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest); void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest); @@ -320,7 +379,9 @@ class ChainFkSolverPos : SolverI #include using namespace KDL; %End - virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0; + virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1)=0; +// Argument by reference doesn't work for container types +// virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1)=0; }; class ChainFkSolverVel : SolverI @@ -329,8 +390,9 @@ class ChainFkSolverVel : SolverI #include using namespace KDL; %End - virtual int JntToCart(const JntArrayVel& q_in, FrameVel& p_out,int - segmentNr=-1)=0; + virtual int JntToCart(const JntArrayVel& q_in, FrameVel& p_out, int segmentNr=-1)=0; +// Argument by reference doesn't work for container types +// virtual int JntToCart(const JntArrayVel& q_in, std::vector& p_out, int segmentNr=-1)=0; }; class ChainFkSolverPos_recursive : ChainFkSolverPos @@ -342,7 +404,9 @@ using namespace KDL; public: ChainFkSolverPos_recursive(const Chain& chain); - virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1); + virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1); +// Argument by reference doesn't work for container types +// virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1); virtual void updateInternalDataStructures(); }; @@ -354,8 +418,9 @@ using namespace KDL; %End public: ChainFkSolverVel_recursive(const Chain& chain); - virtual int JntToCart(const JntArrayVel& q_in ,FrameVel& out,int - segmentNr=-1 ); + virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out, int segmentNr=-1); +// Argument by reference doesn't work for container types +// virtual int JntToCart(const JntArrayVel& q_in, std::vector& out, int segmentNr=-1 ); virtual void updateInternalDataStructures(); }; @@ -389,7 +454,7 @@ using namespace KDL; public: ChainIkSolverPos_NR(const Chain& chain,ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver, unsigned int maxiter=100,double eps=epsilon); - + virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); virtual void updateInternalDataStructures(); }; @@ -401,10 +466,10 @@ class ChainIkSolverPos_NR_JL : ChainIkSolverPos using namespace KDL; %End public: - ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray &q_min,const JntArray &q_max, + ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray &q_min,const JntArray &q_max, ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver, unsigned int maxiter=100,double eps=epsilon); - + virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); virtual void updateInternalDataStructures(); }; @@ -417,7 +482,7 @@ using namespace KDL; %End public: ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150); - + virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); virtual void updateInternalDataStructures(); }; @@ -430,7 +495,7 @@ using namespace KDL; %End public: ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150); - + virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); virtual void updateInternalDataStructures(); @@ -447,28 +512,41 @@ public: temp1=PyList_GetItem(list,0); numCols=PyList_Size(temp1); if (numRows!=numCols) { - sipIsErr=1; //todo: raise exception + std::ostringstream oss; + oss << "Number of rows(" << numRows << ") and columns(" << numCols << ") don't match" << std::endl; + PyErr_SetString(PyExc_ValueError, oss.str().c_str()); + sipError = sipErrorFail; } - if (numRows!=6) { - sipIsErr=1; //todo: raise exception + if (sipError==sipErrorNone && numRows!=6) { + std::ostringstream oss; + oss << "Number of rows != 6, but " << numRows << std::endl; + PyErr_SetString(PyExc_ValueError, oss.str().c_str()); + sipError = sipErrorFail; } - Eigen::MatrixXd Mx; - Mx=Eigen::MatrixXd::Identity(numRows,numCols); - - for (Py_ssize_t r=0;rsetWeightTS(Mx); } - sipCpp->setWeightTS(Mx); %End void setWeightJS(SIP_PYLIST); @@ -484,24 +562,34 @@ public: temp1=PyList_GetItem(list,0); numCols=PyList_Size(temp1); if (numRows!=numCols) { - sipIsErr=1; //todo: raise exception + std::ostringstream oss; + oss << "Number of rows(" << numRows << ") and columns(" << numCols << ") don't match" << std::endl; + PyErr_SetString(PyExc_ValueError, oss.str().c_str()); + sipError = sipErrorFail; } - Eigen::MatrixXd Mq; - Mq=Eigen::MatrixXd::Identity(numRows,numCols); - for (Py_ssize_t r=0;rsetWeightJS(Mq); } - sipCpp->setWeightJS(Mq); %End void setLambda(const double& lambda); @@ -531,7 +619,7 @@ using namespace KDL; %End public: ChainIkSolverVel_pinv_nso(const Chain& chain,double eps=0.00001,int maxiter=150, double alpha=0.25); - + virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); virtual void updateInternalDataStructures(); @@ -556,7 +644,7 @@ using namespace KDL; %End public: ChainIkSolverVel_pinv_givens(const Chain& chain); - + virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); virtual void updateInternalDataStructures(); }; @@ -572,3 +660,37 @@ public: int JntToJac(const JntArray& q_in,Jacobian& jac); virtual void updateInternalDataStructures(); }; + +class ChainJntToJacDotSolver : SolverI +{ +%TypeHeaderCode +#include +using namespace KDL; +%End +public: + ChainJntToJacDotSolver(const Chain& chain); + int JntToJacDot(const JntArrayVel& q_in,Jacobian& jac, int seg_nr=-1); + virtual void updateInternalDataStructures(); +}; + +class ChainIdSolver : SolverI +{ +%TypeHeaderCode +#include +using namespace KDL; +%End + virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector& f_ext,JntArray &torques)=0; +}; + +class ChainIdSolver_RNE : ChainIdSolver +{ +%TypeHeaderCode +#include +using namespace KDL; +%End + +public: + ChainIdSolver_RNE(const Chain& chain,Vector grav); + int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector& f_ext,JntArray &torques); + virtual void updateInternalDataStructures(); +}; diff --git a/python_orocos_kdl/PyKDL/std_string.sip b/python_orocos_kdl/PyKDL/sip/std_string.sip similarity index 78% rename from python_orocos_kdl/PyKDL/std_string.sip rename to python_orocos_kdl/PyKDL/sip/std_string.sip index e31324add..e10c4cf0b 100644 --- a/python_orocos_kdl/PyKDL/std_string.sip +++ b/python_orocos_kdl/PyKDL/sip/std_string.sip @@ -15,6 +15,13 @@ //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +%ModuleHeaderCode +#if PY_VERSION_HEX < 0x03000000 +#define PyBytes_AS_STRING PyString_AS_STRING +#define PyBytes_FromString PyString_FromString +#define PyBytes_Check PyString_Check +#endif +%End %MappedType std::string { %TypeHeaderCode @@ -39,7 +46,7 @@ // If argument is a Python string, assume it's UTF-8 if (sipIsErr == NULL) #if PY_MAJOR_VERSION < 3 - return (PyString_Check(sipPy) || PyUnicode_Check(sipPy)); + return (PyBytes_Check(sipPy) || PyUnicode_Check(sipPy)); #else return PyUnicode_Check(sipPy); #endif @@ -48,14 +55,13 @@ return 1; } if (PyUnicode_Check(sipPy)) { - PyObject* s = PyUnicode_AsEncodedString(sipPy, "UTF-8", ""); - *sipCppPtr = new std::string(PyUnicode_AS_DATA(s)); + PyObject* s = PyUnicode_AsUTF8String(sipPy); + *sipCppPtr = new std::string(PyBytes_AS_STRING(s)); Py_DECREF(s); return 1; } -#if PY_MAJOR_VERSION < 3 - if (PyString_Check(sipPy)) { - *sipCppPtr = new std::string(PyString_AS_STRING(sipPy)); + else if (PyBytes_Check(sipPy)) { + *sipCppPtr = new std::string(PyBytes_AS_STRING(sipPy)); return 1; } #endif diff --git a/python_orocos_kdl/PyKDL/sip/std_vector.sip b/python_orocos_kdl/PyKDL/sip/std_vector.sip new file mode 100644 index 000000000..c1545f3bc --- /dev/null +++ b/python_orocos_kdl/PyKDL/sip/std_vector.sip @@ -0,0 +1,353 @@ +// SIP support for std::vector +// by Giovanni Bajo develer.com> +// Public domain + +// **************************************************** +// SIP generic implementation for std::vector<> +// **************************************************** +// ALas, this template-based generic implementation is valid only +// if the element type is a SIP-wrapped type. For basic types (int, double, etc.) +// we are forced to cut & paste to provide a specialization. + +template +%MappedType std::vector +{ +%TypeHeaderCode +#include +%End + +%ConvertFromTypeCode + PyObject *l = PyList_New(sipCpp -> size()); + + // Create the Python list of the correct length. + if (!l) + return NULL; + + // Go through each element in the C++ instance and convert it to a + // wrapped P2d. + for (int i = 0; i < (int)sipCpp->size(); ++i) { + TYPE *cpp = new TYPE(sipCpp->at(i)); + PyObject *pobj = sipConvertFromInstance(cpp, sipClass_TYPE, sipTransferObj); + + // Get the Python wrapper for the Type instance, creating a new + // one if necessary, and handle any ownership transfer. + if (!pobj) { + // There was an error so garbage collect the Python list. + Py_DECREF(l); + return NULL; + } + + // Add the wrapper to the list. + PyList_SET_ITEM(l, i, pobj); + } + + // Return the Python list. + return l; +%End + +%ConvertToTypeCode + // Check if type is compatible + if (!sipIsErr) { + // Must be any iterable + PyObject *i = PyObject_GetIter(sipPy); + bool iterable = (i != NULL); + Py_XDECREF(i); + return iterable; + } + + // Iterate over the object + PyObject *iterator = PyObject_GetIter(sipPy); + PyObject *item; + + std::vector *V = new std::vector(); + + while ((item = PyIter_Next(iterator))) + { + if (!sipCanConvertToInstance(item, sipClass_TYPE, SIP_NOT_NONE)) { + PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to TYPE"); + *sipIsErr = 1; + break; + } + + int state; + TYPE* p = reinterpret_cast( + sipConvertToInstance(item, sipClass_TYPE, 0, SIP_NOT_NONE, &state, sipIsErr)); + + if (!*sipIsErr) + V->push_back(*p); + + sipReleaseInstance(p, sipClass_TYPE, state); + Py_DECREF(item); + } + + Py_DECREF(iterator); + + if (*sipIsErr) { + delete V; + return 0; + } + + *sipCppPtr = V; + return sipGetState(sipTransferObj); +%End +}; + +// **************************************************** +// Specialization for std::vector +// **************************************************** + +%MappedType std::vector +{ +%TypeHeaderCode +#include +%End + +%ConvertFromTypeCode + PyObject *l; + + // Create the Python list of the correct length. + if ((l = PyList_New(sipCpp -> size())) == NULL) + return NULL; + + // Go through each element in the C++ instance and convert it to a + // wrapped object. + for (int i = 0; i < (int)sipCpp -> size(); ++i) + { + // Add the wrapper to the list. + PyList_SET_ITEM(l, i, PyFloat_FromDouble(sipCpp -> at(i))); + } + + // Return the Python list. + return l; +%End + +%ConvertToTypeCode + // Check if type is compatible + if (sipIsErr == NULL) + { + // Must be any iterable + PyObject *i = PyObject_GetIter(sipPy); + bool iterable = (i != NULL); + Py_XDECREF(i); + return iterable; + } + + // Iterate over the object + PyObject *iterator = PyObject_GetIter(sipPy); + PyObject *item; + + // Maximum number of elements + int len = PyObject_Size(sipPy); + std::vector *V = new std::vector(); + V->reserve(len); + + if (len) + { + while ((item = PyIter_Next(iterator))) + { + if (!PyNumber_Check(item)) + { + PyErr_Format(PyExc_TypeError, "object in iterable is not a number"); + *sipIsErr = 1; + break; + } + + PyObject *f = PyNumber_Float(item); + V->push_back(PyFloat_AsDouble(f)); + + Py_DECREF(f); + Py_DECREF(item); + } + + Py_DECREF(iterator); + + if (*sipIsErr) + { + delete V; + return 0; + } + } + + *sipCppPtr = V; + return sipGetState(sipTransferObj); +%End +}; + +// **************************************************** +// Specialization for std::vector +// **************************************************** + +%MappedType std::vector +{ +%TypeHeaderCode +#include + +#if PY_VERSION_HEX < 0x03000000 +#define PyLong_AsLong PyInt_AsLong +#define PyLong_FromLong PyInt_FromLong +#define PyLong_Check PyInt_Check +#endif +%End + +%ConvertFromTypeCode + PyObject *l; + + // Create the Python list of the correct length. + if ((l = PyList_New((SIP_SSIZE_T)sipCpp -> size())) == NULL) + return NULL; + + // Go through each element in the C++ instance and convert it to a + // wrapped object. + for (int i = 0; i < (int)sipCpp -> size(); ++i) + { + // Add the wrapper to the list. + PyList_SET_ITEM(l, i, PyLong_FromLong(sipCpp -> at(i))); + } + + // Return the Python list. + return l; +%End + +%ConvertToTypeCode + // Check if type is compatible + if (sipIsErr == NULL) + { + // Must be any iterable + PyObject *i = PyObject_GetIter(sipPy); + bool iterable = (i != NULL); + Py_XDECREF(i); + return iterable; + } + + // Iterate over the object + PyObject *iterator = PyObject_GetIter(sipPy); + PyObject *item; + + // Maximum number of elements + int len = PyObject_Size(sipPy); + std::vector *V = new std::vector(); + V->reserve(len); + + if (len) + { + while ((item = PyIter_Next(iterator))) + { + if (!PyLong_Check(item)) + { + PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to float"); + *sipIsErr = 1; + break; + } + + int val = PyLong_AsLong(item); + V->push_back(val); + + Py_DECREF(item); + } + + Py_DECREF(iterator); + + if (*sipIsErr) + { + delete V; + return 0; + } + } + + *sipCppPtr = V; + return sipGetState(sipTransferObj); +%End +}; + + +// **************************************************** +// Specialization for std::vector +// **************************************************** + +%MappedType std::vector +{ +%TypeHeaderCode +#include + +#if PY_VERSION_HEX < 0x03000000 +#define PyLong_AsLong PyInt_AsLong +#define PyLong_FromLong PyInt_FromLong +#define PyLong_Check PyInt_Check +#endif +%End + +%ConvertFromTypeCode + PyObject *l; + + // Create the Python list of the correct length. + if ((l = PyList_New(sipCpp -> size())) == NULL) + return NULL; + + // Go through each element in the C++ instance and convert it to a + // wrapped object. + for (int i = 0; i < (int)sipCpp -> size(); ++i) + { + // Add the wrapper to the list. + PyList_SET_ITEM(l, i, PyLong_FromLong(sipCpp -> at(i))); + } + + // Return the Python list. + return l; +%End + +%ConvertToTypeCode + // Check if type is compatible + if (sipIsErr == NULL) + { + // Must be any iterable + PyObject *i = PyObject_GetIter(sipPy); + bool iterable = (i != NULL); + Py_XDECREF(i); + return iterable; + } + + // Iterate over the object + PyObject *iterator = PyObject_GetIter(sipPy); + PyObject *item; + + // Maximum number of elements + Py_ssize_t size = PyObject_Size(sipPy); + if (size == -1) { + Py_DECREF(iterator); + return 0; + } + + unsigned int len = size; + std::vector *V = new std::vector(); + V->reserve(len); + + if (len) + { + while ((item = PyIter_Next(iterator))) + { + if (!PyLong_Check(item)) + { + PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to float"); + *sipIsErr = 1; + break; + } + + unsigned int val = PyLong_AsLong(item); + V->push_back(val); + + Py_DECREF(item); + } + + Py_DECREF(iterator); + + if (*sipIsErr) + { + delete V; + return 0; + } + } + + *sipCppPtr = V; + return sipGetState(sipTransferObj); +%End +}; diff --git a/python_orocos_kdl/cmake/SIPMacros.cmake b/python_orocos_kdl/cmake/SIPMacros.cmake index 4464a9267..b4df94259 100644 --- a/python_orocos_kdl/cmake/SIPMacros.cmake +++ b/python_orocos_kdl/cmake/SIPMacros.cmake @@ -103,11 +103,11 @@ MACRO(ADD_SIP_PYTHON_MODULE MODULE_NAME MODULE_SIP) ENDFOREACH(filename ${_sip_output_files}) ENDIF(NOT WIN32) ADD_CUSTOM_COMMAND( - OUTPUT ${_sip_output_files} + OUTPUT ${_sip_output_files} COMMAND ${CMAKE_COMMAND} -E echo ${message} COMMAND ${TOUCH_COMMAND} ${_sip_output_files} COMMAND ${SIP_EXECUTABLE} ${_sip_tags} ${_sip_x} ${SIP_EXTRA_OPTIONS} -j ${SIP_CONCAT_PARTS} -c ${CMAKE_CURRENT_SIP_OUTPUT_DIR} ${_sip_includes} ${_abs_module_sip} - DEPENDS ${_abs_module_sip} ${SIP_EXTRA_FILES_DEPEND} + DEPENDS ${SIP_INCLUDES} ${SIP_EXTRA_FILES_DEPEND} ) # not sure if type MODULE could be uses anywhere, limit to cygwin for now IF (CYGWIN OR APPLE) diff --git a/python_orocos_kdl/mainpage.dox b/python_orocos_kdl/mainpage.dox deleted file mode 100644 index 0be6e3112..000000000 --- a/python_orocos_kdl/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b python_orocos_kdl is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/python_orocos_kdl/manifest.xml b/python_orocos_kdl/manifest.xml deleted file mode 100644 index ae3cd5ac7..000000000 --- a/python_orocos_kdl/manifest.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - python_orocos_kdl - - - Ruben Smits - LGPL - - http://ros.org/wiki/python_orocos_kdl - - - - - - - - - - - - diff --git a/python_orocos_kdl/package.xml b/python_orocos_kdl/package.xml index 3fe99b324..03c11530e 100644 --- a/python_orocos_kdl/package.xml +++ b/python_orocos_kdl/package.xml @@ -1,9 +1,10 @@ - + + python_orocos_kdl 1.4.0 This package contains the python bindings PyKDL for the Kinematics and Dynamics - Library (KDL), distributed by the Orocos Project. + Library (KDL), distributed by the Orocos Project. Ruben Smits http://wiki.ros.org/python_orocos_kdl @@ -14,12 +15,21 @@ orocos_kdl python-sip - catkin - orocos_kdl - python-sip + catkin + orocos_kdl + python-sip + python3-sip + + python-future + python3-future + python-psutil + python3-psutil + + python-sphinx cmake + diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 new file mode 160000 index 000000000..80d452484 --- /dev/null +++ b/python_orocos_kdl/pybind11 @@ -0,0 +1 @@ +Subproject commit 80d452484c5409444b0ec19383faa84bb7a4d351 diff --git a/python_orocos_kdl/rosdoc.yaml b/python_orocos_kdl/rosdoc.yaml index daa8282ec..b2f4d1f9e 100644 --- a/python_orocos_kdl/rosdoc.yaml +++ b/python_orocos_kdl/rosdoc.yaml @@ -1,3 +1,4 @@ - builder: sphinx output_dir: doc - sphinx_root_dir: doc \ No newline at end of file + sphinx_root_dir: doc + name: Python API diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index 37375da10..06037d137 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -1,9 +1,9 @@ -#!/usr/bin/python -# Copyright (C) 2007 Ruben Smits +#! /usr/bin/env python +# Copyright (C) 2020 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or @@ -19,14 +19,27 @@ # You should have received a copy of the GNU Lesser General Public # License along with this library; if not, write to the Free Software # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + import unittest +import dynamicstest import kinfamtest import framestest import frameveltest suite = unittest.TestSuite() +suite.addTest(dynamicstest.suite()) suite.addTest(framestest.suite()) suite.addTest(frameveltest.suite()) suite.addTest(kinfamtest.suite()) -unittest.TextTestRunner(verbosity=3).run(suite) +if sys.version_info < (3, 0): + import jointtypetest + suite.addTest(jointtypetest.suite()) + +result = unittest.TextTestRunner(verbosity=3).run(suite) + +if result.wasSuccessful(): + sys.exit(0) +else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/dynamicstest.py b/python_orocos_kdl/tests/dynamicstest.py new file mode 100644 index 000000000..d9b889bb7 --- /dev/null +++ b/python_orocos_kdl/tests/dynamicstest.py @@ -0,0 +1,75 @@ +# Copyright (C) 2020 Ruben Smits + +# Version: 1.0 +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits +# URL: http://www.orocos.org/kdl + +# This library is free software; you can redistribute it and/or +# modify it under the terms of the GNU Lesser General Public +# License as published by the Free Software Foundation; either +# version 2.1 of the License, or (at your option) any later version. + +# This library 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 +# Lesser General Public License for more details. + +# You should have received a copy of the GNU Lesser General Public +# License along with this library; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +from PyKDL import * +import unittest + + +class DynamicsTestFunctions(unittest.TestCase): + def testJntSpaceInertiaMatrix(self): + ll = 3 + jm = JntSpaceInertiaMatrix(3) + # __getitem__ + for i in range(ll): + for j in range(ll): + self.assertEqual(jm[i, j], 0) + with self.assertRaises(IndexError): + _ = jm[-1, 0] + with self.assertRaises(IndexError): + _ = jm[3, 0] + with self.assertRaises(IndexError): + _ = jm[2, -1] + with self.assertRaises(IndexError): + _ = jm[2, 3] + + # __setitem__ + for i in range(ll): + for j in range(ll): + jm[i, j] = 3 * i + j + for i in range(ll): + for j in range(ll): + self.assertEqual(jm[i, j], 3 * i + j) + with self.assertRaises(IndexError): + jm[-1, 0] = 1 + with self.assertRaises(IndexError): + jm[3, 0] = 1 + with self.assertRaises(IndexError): + jm[2, -1] = 1 + with self.assertRaises(IndexError): + jm[2, 3] = 1 + + +def suite(): + suite = unittest.TestSuite() + suite.addTest(DynamicsTestFunctions('testJntSpaceInertiaMatrix')) + return suite + + +if __name__ == '__main__': + import sys + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) \ No newline at end of file diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 41429dcf4..e8953cfcc 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -1,8 +1,9 @@ -# Copyright (C) 2007 Ruben Smits +# Copyright (C) 2020 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or @@ -20,167 +21,402 @@ # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -import unittest +from math import radians, sqrt from PyKDL import * -from math import * - +import sys +import unittest + + class FramesTestFunctions(unittest.TestCase): + def testVector(self): + v = Vector(3, 4, 5) + self.testVectorImpl(v) + v = Vector() + self.testVectorImpl(v) + + # Equality + v = Vector(3, 4, 5) + self.assertFalse(v == -v) # Doesn't work for zero vector + self.assertFalse(Equal(v, -v)) # Doesn't work for zero vector + self.assertTrue(v != -v) # Doesn't work for zero vector + self.assertTrue(not Equal(v, -v)) # Doesn't work for zero vector - def testVector2(self,v): - self.assertEqual(2*v-v,v) - self.assertEqual(v*2-v,v) - self.assertEqual(v+v+v-2*v,v) - v2=Vector(v) - self.assertEqual(v,v2) - v2+=v - self.assertEqual(2*v,v2) - v2-=v - self.assertEqual(v,v2) + # Test member get and set functions + self.assertEqual(v.x(), 3) + v.x(1) + self.assertEqual(v, Vector(1, 4, 5)) + self.assertEqual(v.y(), 4) + v.y(1) + self.assertEqual(v, Vector(1, 1, 5)) + self.assertEqual(v.z(), 5) + v.z(1) + self.assertEqual(v, Vector(1, 1, 1)) + + # __getitem__ + self.assertEqual(v[0], 1) + self.assertEqual(v[1], 1) + self.assertEqual(v[2], 1) + with self.assertRaises(IndexError): + _ = v[-1] + with self.assertRaises(IndexError): + _ = v[3] + + # __setitem__ + v[0] = 3 + self.assertEqual(v[0], 3) + v[1] = 4 + self.assertEqual(v[1], 4) + v[2] = 5 + self.assertEqual(v[2], 5) + with self.assertRaises(IndexError): + v[-1] = 1 + with self.assertRaises(IndexError): + v[3] = 1 + + # Zero + SetToZero(v) + self.assertEqual(v, Vector(0, 0, 0)) + self.assertEqual(Vector.Zero(), Vector(0, 0, 0)) + + def testVectorImpl(self, v): + self.assertTrue(v == v) + self.assertTrue(Equal(v, v)) + self.assertFalse(v != v) + self.assertFalse(not Equal(v, v)) + self.assertEqual(2*v-v, v) + self.assertEqual(v*2-v, v) + self.assertEqual(v+v+v-2*v, v) + v2 = Vector(v) + self.assertEqual(v, v2) + v2 += v + self.assertEqual(2*v, v2) + v2 -= v + self.assertEqual(v, v2) v2.ReverseSign() - self.assertEqual(v,-v2) + self.assertEqual(v, -v2) + for v2 in [Vector(v), -Vector(v)]: + self.assertAlmostEqual(v2.Norm()**2, sum(map(lambda i: i * i, v2)), delta=1e-10) + self.assertEqual(v2.Norm(), v2.Normalize()) # Norm before Normalize, so taking norm of un-normalized vector - def testVector(self): - v=Vector(3,4,5) - self.testVector2(v) - v=Vector.Zero() - self.testVector2(v) - - def testTwist2(self,t): - self.assertEqual(2*t-t,t) - self.assertEqual(t*2-t,t) - self.assertEqual(t+t+t-2*t,t) - t2=Twist(t) - self.assertEqual(t,t2) - t2+=t - self.assertEqual(2*t,t2) - t2-=t - self.assertEqual(t,t2) - t.ReverseSign() - self.assertEqual(t,-t2) + self.assertEqual(dot(v, v), sum(map(lambda i: i * i, v))) def testTwist(self): - t=Twist(Vector(6,3,5),Vector(4,-2,7)) - self.testTwist2(t) - t=Twist.Zero() - self.testTwist2(t) - t=Twist(Vector(0,-9,-3),Vector(1,-2,-4)) - - def testWrench2(self,w): - self.assertEqual(2*w-w,w) - self.assertEqual(w*2-w,w) - self.assertEqual(w+w+w-2*w,w) - w2=Wrench(w) - self.assertEqual(w,w2) - w2+=w - self.assertEqual(2*w,w2) - w2-=w - self.assertEqual(w,w2) - w.ReverseSign() - self.assertEqual(w,-w2) + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) + self.testTwistImpl(t) + t = Twist() + self.testTwistImpl(t) + t = Twist(Vector(0, -9, -3), Vector(1, -2, -4)) + self.testTwistImpl(t) + + # Equality + t = Twist(Vector(1, 2, 3), Vector(1, 2, 3)) + self.assertFalse(t == -t) # Doesn't work for zero twist + self.assertFalse(Equal(t, -t)) # Doesn't work for zero twist + self.assertTrue(t != -t) # Doesn't work for zero twist + self.assertTrue(not Equal(t, -t)) # Doesn't work for zero twist + + # Members + v1 = Vector(1, 2, 3) + v2 = Vector(4, 5, 6) + t = Twist(v1, v2) + self.assertEqual(t.vel, v1) + self.assertEqual(t.rot, v2) + self.assertEqual(Twist(t).vel, t.vel) + self.assertEqual(Twist(t).rot, t.rot) + + # __getitem__ + for i in range(6): + self.assertEqual(t[i], i+1) + with self.assertRaises(IndexError): + _ = t[-1] + with self.assertRaises(IndexError): + _ = t[6] + + # __setitem__ + for i in range(6): + t[i] = i + for i in range(6): + self.assertEqual(t[i], i) + with self.assertRaises(IndexError): + t[-1] = 1 + with self.assertRaises(IndexError): + t[6] = 1 + + # Zero + SetToZero(t) + self.assertEqual(t, Twist()) + self.assertEqual(Twist.Zero(), Twist()) + + def testTwistImpl(self, t): + self.assertTrue(t == t) + self.assertTrue(Equal(t, t)) + self.assertFalse(t != t) + self.assertFalse(not Equal(t, t)) + self.assertEqual(2*t-t, t) + self.assertEqual(t*2-t, t) + self.assertEqual(t+t+t-2*t, t) + t2 = Twist(t) + self.assertEqual(t, t2) + t2 += t + self.assertEqual(2*t, t2) + t2 -= t + self.assertEqual(t, t2) + t2.ReverseSign() + self.assertEqual(t, -t2) + v = Vector(1, 2, 3) + t2 = t.RefPoint(v) + self.assertEqual(t2.vel, t.vel + t.rot*v) + self.assertEqual(t2.rot, t.rot) + + # No need to test the dot functions again for Wrench + w = Wrench(v, v) + dot_result = dot(t.vel, w.force) + dot(t.rot, w.torque) + self.assertEqual(dot(t, w), dot_result) + self.assertEqual(dot(w, t), dot_result) def testWrench(self): - w=Wrench(Vector(7,-1,3),Vector(2,-3,3)) - self.testWrench2(w) - w=Wrench.Zero() - self.testWrench2(w) - w=Wrench(Vector(2,1,4),Vector(5,3,1)) - self.testWrench2(w) - - def testRotation2(self,v,a,b,c): - w=Wrench(Vector(7,-1,3),Vector(2,-3,3)) - t=Twist(Vector(6,3,5),Vector(4,-2,7)) - R=Rotation.RPY(a,b,c) - - self.assertAlmostEqual(dot(R.UnitX(),R.UnitX()),1.0,15) - self.assertEqual(dot(R.UnitY(),R.UnitY()),1.0) - self.assertEqual(dot(R.UnitZ(),R.UnitZ()),1.0) - self.assertAlmostEqual(dot(R.UnitX(),R.UnitY()),0.0,15) - self.assertAlmostEqual(dot(R.UnitX(),R.UnitZ()),0.0,15) - self.assertEqual(dot(R.UnitY(),R.UnitZ()),0.0) - R2=Rotation(R) - self.assertEqual(R,R2) - self.assertAlmostEqual((R*v).Norm(),v.Norm(),14) - self.assertEqual(R.Inverse(R*v),v) - self.assertEqual(R.Inverse(R*t),t) - self.assertEqual(R.Inverse(R*w),w) - self.assertEqual(R*R.Inverse(v),v) - self.assertEqual(R*Rotation.Identity(),R) - self.assertEqual(Rotation.Identity()*R,R) - self.assertEqual(R*(R*(R*v)),(R*R*R)*v) - self.assertEqual(R*(R*(R*t)),(R*R*R)*t) - self.assertEqual(R*(R*(R*w)),(R*R*R)*w) - self.assertEqual(R*R.Inverse(),Rotation.Identity()) - self.assertEqual(R.Inverse()*R,Rotation.Identity()) - self.assertEqual(R.Inverse()*v,R.Inverse(v)) - (ra,rb,rc)=R.GetRPY() - self.assertEqual(ra,a) - self.assertEqual(rb,b) - self.assertEqual(rc,c) - R = Rotation.EulerZYX(a,b,c) - (ra,rb,rc)=R.GetEulerZYX() - self.assertEqual(ra,a) - self.assertEqual(rb,b) - self.assertEqual(rc,c) - R = Rotation.EulerZYZ(a,b,c) - (ra,rb,rc)=R.GetEulerZYZ() - self.assertEqual(ra,a) - self.assertEqual(rb,b) - self.assertAlmostEqual(rc,c,15) - (angle,v2)= R.GetRotAngle() - R2=Rotation.Rot(v2,angle) - self.assertEqual(R2,R) - R2=Rotation.Rot(v2*1E20,angle) - self.assertEqual(R,R2) - v2=Vector(6,2,4) - self.assertAlmostEqual(v2.Norm(),sqrt(dot(v2,v2)),14) - + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) + self.testWrenchImpl(w) + w = Wrench() + self.testWrenchImpl(w) + w = Wrench(Vector(2, 1, 4), Vector(5, 3, 1)) + self.testWrenchImpl(w) + + # Equality + w = Wrench(Vector(1, 2, 3), Vector(1, 2, 3)) + self.assertFalse(w == -w) # Doesn't work for zero wrench + self.assertFalse(Equal(w, -w)) # Doesn't work for zero wrench + self.assertTrue(w != -w) # Doesn't work for zero wrench + self.assertTrue(not Equal(w, -w)) # Doesn't work for zero wrench + + # Members + v1 = Vector(1, 2, 3) + v2 = Vector(4, 5, 6) + w = Wrench(v1, v2) + self.assertEqual(w.force, v1) + self.assertEqual(w.torque, v2) + self.assertEqual(Wrench(w).force, w.force) + self.assertEqual(Wrench(w).torque, w.torque) + + # __getitem__ + for i in range(6): + self.assertEqual(w[i], i+1) + with self.assertRaises(IndexError): + _ = w[-1] + with self.assertRaises(IndexError): + _ = w[6] + + # __setitem__ + for i in range(6): + w[i] = i + for i in range(6): + self.assertEqual(w[i], i) + with self.assertRaises(IndexError): + w[-1] = 1 + with self.assertRaises(IndexError): + w[6] = 1 + + # Zero + SetToZero(w) + self.assertEqual(w, Wrench()) + self.assertEqual(Wrench.Zero(), Wrench()) + + def testWrenchImpl(self, w): + self.assertEqual(2*w-w, w) + self.assertEqual(w*2-w, w) + self.assertEqual(w+w+w-2*w, w) + w2 = Wrench(w) + self.assertEqual(w, w2) + w2 += w + self.assertEqual(2*w, w2) + w2 -= w + self.assertEqual(w, w2) + w2.ReverseSign() + self.assertEqual(w, -w2) + + v = Vector(1, 2, 3) + w2 = w.RefPoint(v) + self.assertEqual(w2.force, w.force) + self.assertEqual(w2.torque, w.torque + w.force*v) + def testRotation(self): - self.testRotation2(Vector(3,4,5),radians(10),radians(20),radians(30)) - + self.testRotationImpl(Rotation.RPY(radians(10), radians(20), radians(30)), Vector(3, 4, 5)) + self.testRotationImpl(Rotation.RPY(radians(10), radians(20), radians(30)), Vector()) + self.testRotationImpl(Rotation(), Vector(3, 4, 5)) + self.testRotationImpl(Rotation(), Vector()) + + r = Rotation(*range(1, 10)) + + # __getitem__ + for i in range(3): + for j in range(3): + self.assertEqual(r[i, j], 3*i+j+1) + with self.assertRaises(IndexError): + _ = r[-1, 0] + with self.assertRaises(IndexError): + _ = r[0, -1] + with self.assertRaises(IndexError): + _ = r[3, 2] + with self.assertRaises(IndexError): + _ = r[2, 3] + + # __setitem__ + for i in range(3): + for j in range(3): + r[i, j] = 3*i+j + for i in range(3): + for j in range(3): + self.assertEqual(r[i, j], 3*i+j) + with self.assertRaises(IndexError): + r[-1, 0] = 1 + with self.assertRaises(IndexError): + r[0, -1] = 1 + with self.assertRaises(IndexError): + r[3, 2] = 1 + with self.assertRaises(IndexError): + r[2, 3] = 1 + + def testRotationImpl(self, r, v): + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) + a, b, c = r.GetRPY() + + self.assertAlmostEqual(dot(r.UnitX(), r.UnitX()), 1.0, 15) + self.assertEqual(dot(r.UnitY(), r.UnitY()), 1.0) + self.assertEqual(dot(r.UnitZ(), r.UnitZ()), 1.0) + self.assertAlmostEqual(dot(r.UnitX(), r.UnitY()), 0.0, 15) + self.assertAlmostEqual(dot(r.UnitX(), r.UnitZ()), 0.0, 15) + self.assertAlmostEqual(dot(r.UnitY(), r.UnitZ()), 0.0, 15) + r2 = Rotation(r) + self.assertEqual(r, r2) + self.assertAlmostEqual((r*v).Norm(), v.Norm(), 14) + self.assertEqual(r.Inverse(r*v), v) + self.assertEqual(r.Inverse(r*t), t) + self.assertEqual(r.Inverse(r*w), w) + self.assertEqual(r*r.Inverse(v), v) + self.assertEqual(r*Rotation.Identity(), r) + self.assertEqual(Rotation.Identity()*r, r) + self.assertEqual(r*(r*(r*v)), (r*r*r)*v) + self.assertEqual(r*(r*(r*t)), (r*r*r)*t) + self.assertEqual(r*(r*(r*w)), (r*r*r)*w) + self.assertEqual(r*r.Inverse(), Rotation.Identity()) + self.assertEqual(r.Inverse()*r, Rotation.Identity()) + self.assertEqual(r.Inverse()*v, r.Inverse(v)) + (ra, rb, rc) = r.GetRPY() + self.assertEqual(ra, a) + self.assertEqual(rb, b) + self.assertEqual(rc, c) + r = Rotation.EulerZYX(a, b, c) + (ra, rb, rc) = r.GetEulerZYX() + self.assertEqual(ra, a) + self.assertEqual(rb, b) + self.assertEqual(rc, c) + r = Rotation.EulerZYZ(a, b, c) + (ra, rb, rc) = r.GetEulerZYZ() + self.assertEqual(ra, a) + self.assertEqual(rb, b) + self.assertAlmostEqual(rc, c, 15) + (angle, v2) = r.GetRotAngle() + r2 = Rotation.Rot(v2, angle) + self.assertEqual(r2, r) + r2 = Rotation.Rot(v2*1E20, angle) + self.assertEqual(r, r2) + v2 = Vector(6, 2, 4) + self.assertAlmostEqual(v2.Norm(), sqrt(dot(v2, v2)), 14) + def testFrame(self): - v=Vector(3,4,5) - w=Wrench(Vector(7,-1,3),Vector(2,-3,3)) - t=Twist(Vector(6,3,5),Vector(4,-2,7)) - F = Frame(Rotation.EulerZYX(radians(10),radians(20),radians(-10)),Vector(4,-2,1)) - F2=Frame(F) - self.assertEqual(F,F2) - self.assertEqual(F.Inverse(F*v),v) - self.assertEqual(F.Inverse(F*t),t) - self.assertEqual(F.Inverse(F*w),w) - self.assertEqual(F*F.Inverse(v),v) - self.assertEqual(F*F.Inverse(t),t) - self.assertEqual(F*F.Inverse(w),w) - self.assertEqual(F*Frame.Identity(),F) - self.assertEqual(Frame.Identity()*F,F) - self.assertEqual(F*(F*(F*v)),(F*F*F)*v) - self.assertEqual(F*(F*(F*t)),(F*F*F)*t) - self.assertEqual(F*(F*(F*w)),(F*F*F)*w) - self.assertEqual(F*F.Inverse(),Frame.Identity()) - self.assertEqual(F.Inverse()*F,Frame.Identity()) - self.assertEqual(F.Inverse()*v,F.Inverse(v)) + v = Vector(3, 4, 5) + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) + f = Frame() + self.testFrameImpl(v, w, t, f) + r = Rotation.EulerZYX(radians(10), radians(20), radians(-10)) + v2 = Vector(4, -2, 1) + f = Frame(r, v2) + self.testFrameImpl(v, w, t, f) + + # Equality + f2 = Frame(f) + self.assertEqual(f, f2) + + # Members + self.assertEqual(f.M, r) + self.assertEqual(f.p, v2) + self.assertEqual(Frame(f).M, f.M) + self.assertEqual(Frame(f).p, f.p) + + f = Frame(Rotation(1, 2, 3, + 5, 6, 7, + 9, 10, 11), + Vector(4, 8, 12)) + # __getitem__ + for i in range(3): + for j in range(4): + self.assertEqual(f[i, j], 4*i+j+1) + with self.assertRaises(IndexError): + _ = f[-1, 0] + with self.assertRaises(IndexError): + _ = f[0, -1] + with self.assertRaises(IndexError): + _ = f[3, 3] + with self.assertRaises(IndexError): + _ = f[2, 4] + + # __setitem__ + for i in range(3): + for j in range(4): + f[i, j] = 4*i+j + for i in range(3): + for j in range(4): + self.assertEqual(f[i, j], 4*i+j) + with self.assertRaises(IndexError): + f[-1, 0] = 1 + with self.assertRaises(IndexError): + f[0, -1] = 1 + with self.assertRaises(IndexError): + f[3, 3] = 1 + with self.assertRaises(IndexError): + f[2, 4] = 1 + + def testFrameImpl(self, v, w, t, f): + self.assertEqual(f.Inverse(f*v), v) + self.assertEqual(f.Inverse(f*t), t) + self.assertEqual(f.Inverse(f*w), w) + self.assertEqual(f*f.Inverse(v), v) + self.assertEqual(f*f.Inverse(t), t) + self.assertEqual(f*f.Inverse(w), w) + self.assertEqual(f*Frame.Identity(), f) + self.assertEqual(Frame.Identity()*f, f) + self.assertEqual(f*(f*(f*v)), (f*f*f)*v) + self.assertEqual(f*(f*(f*t)), (f*f*f)*t) + self.assertEqual(f*(f*(f*w)), (f*f*f)*w) + self.assertEqual(f*f.Inverse(), Frame.Identity()) + self.assertEqual(f.Inverse()*f, Frame.Identity()) + self.assertEqual(f.Inverse()*v, f.Inverse(v)) def testPickle(self): - import pickle - data = {} - data['v'] = Vector(1,2,3) - data['rot'] = Rotation.RotX(1.3) + if sys.version_info < (3, 0): + import cPickle as pickle + else: + import pickle + data = dict() + data['v'] = Vector(1, 2, 3) + data['rot'] = Rotation().EulerZYZ(1, 2, 3) data['fr'] = Frame(data['rot'], data['v']) - data['tw'] = Twist(data['v'], Vector(4,5,6)) - data['wr'] = Wrench(Vector(0.1,0.2,0.3), data['v']) - - f = open('/tmp/pickle_test', 'w') - pickle.dump(data, f) - f.close() - - f = open('/tmp/pickle_test', 'r') - data1 = pickle.load(f) - f.close() - + data['tw'] = Twist(data['v'], Vector(4, 5, 6)) + data['wr'] = Wrench(Vector(0.1, 0.2, 0.3), data['v']) + + with open('/tmp/pickle_test', 'wb') as f: + pickle.dump(data, f, 2) + + with open('/tmp/pickle_test', 'rb') as f: + data1 = pickle.load(f) + self.assertEqual(data, data1) def suite(): - suite=unittest.TestSuite() + suite = unittest.TestSuite() suite.addTest(FramesTestFunctions('testVector')) suite.addTest(FramesTestFunctions('testTwist')) suite.addTest(FramesTestFunctions('testWrench')) @@ -188,6 +424,13 @@ def suite(): suite.addTest(FramesTestFunctions('testFrame')) suite.addTest(FramesTestFunctions('testPickle')) return suite - -#suite = suite() -#unittest.TextTestRunner(verbosity=3).run(suite) + + +if __name__ == '__main__': + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index ae0e89051..dc627b0ad 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -1,100 +1,270 @@ -import unittest +# Copyright (C) 2020 Ruben Smits + +# Version: 1.0 +# Author: Ruben Smits +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits +# URL: http://www.orocos.org/kdl + +# This library is free software; you can redistribute it and/or +# modify it under the terms of the GNU Lesser General Public +# License as published by the Free Software Foundation; either +# version 2.1 of the License, or (at your option) any later version. + +# This library 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 +# Lesser General Public License for more details. + +# You should have received a copy of the GNU Lesser General Public +# License along with this library; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +from math import radians from PyKDL import * -from math import * +import sys +import unittest + class FrameVelTestFunctions(unittest.TestCase): + def testdoubleVel(self): + d = doubleVel() + self.assertEqual(doubleVel(d), d) + self.assertEqual(d.t, 0) + self.assertEqual(d.grad, 0) + self.assertEqual(d.value(), 0) + self.assertEqual(d.deriv(), 0) + d2 = -d + self.assertEqual(d2.t, -d.t) + self.assertEqual(d2.grad, -d.grad) def testVectorVel(self): - v=VectorVel(Vector(3,-4,5),Vector(6,3,-5)) - vt = Vector(-4,-6,-8); - self.assertTrue(Equal( 2*v-v,v)) - self.assertTrue(Equal( v*2-v,v)) - self.assertTrue(Equal( v+v+v-2*v,v)) - v2=VectorVel(v) - self.assertTrue(Equal( v,v2)) - v2+=v - self.assertTrue(Equal( 2*v,v2)) - v2-=v - self.assertTrue(Equal( v,v2)) + v = VectorVel() + vt = Vector() + self.testVectorVelImpl(v, vt) + vt = Vector(-4, -6, -8) + self.testVectorVelImpl(v, vt) + v1 = Vector(3, -4, 5) + v2 = Vector(6, 3, -5) + v = VectorVel(v1, v2) + self.testVectorVelImpl(v, vt) + + # Members + self.assertEqual(v.p, v1) + self.assertEqual(v.v, v2) + self.assertEqual(v.value(), v1) + self.assertEqual(v.deriv(), v2) + + # Equality + self.assertEqual(VectorVel(v).p, v.p) + self.assertEqual(VectorVel(v).v, v.v) + self.assertFalse(v == -v) # Doesn't work for zero VectorVel + self.assertFalse(Equal(v, -v)) # Doesn't work for zero VectorVel + self.assertTrue(v != -v) # Doesn't work for zero VectorVel + self.assertTrue(not Equal(v, -v)) # Doesn't work for zero VectorVel + + v = VectorVel(v1) + self.assertEqual(v, v1) + self.assertEqual(v1, v) + + # Zero + v = VectorVel(v1, v2) + SetToZero(v) + self.assertEqual(v, VectorVel()) + self.assertEqual(VectorVel.Zero(), VectorVel()) + + # dot functions + v = VectorVel(v1, v2) + self.assertEqual(dot(v, v), doubleVel(dot(v.p, v.p), dot(v.p, v.v)+dot(v.v, v.p))) + dot_result = doubleVel(dot(v.p, v1), dot(v.v, v1)) + self.assertEqual(dot(v, v1), dot_result) + self.assertEqual(dot(v1, v), dot_result) + + def testVectorVelImpl(self, v, vt): + self.assertEqual(2*v-v, v) + self.assertEqual(v*2-v, v) + self.assertEqual(v+v+v-2*v, v) + v2 = VectorVel(v) + self.assertEqual(v, v2) + v2 += v + self.assertEqual(2*v, v2) + v2 -= v + self.assertEqual(v, v2) v2.ReverseSign() - self.assertTrue(Equal( v,-v2)) - self.assertTrue(Equal( v*vt,-vt*v)) - v2 = VectorVel(Vector(-5,-6,-3),Vector(3,4,5)) - self.assertTrue(Equal( v*v2,-v2*v)) + self.assertEqual(v, -v2) + self.assertEqual(v*vt, -vt*v) + v2 = VectorVel(Vector(-5, -6, -3), Vector(3, 4, 5)) + self.assertEqual(v*v2, -v2*v) + + def testTwistVel(self): + t = TwistVel() + self.testTwistVelImpl(t) + v1 = Vector(1, 2, 3) + v2 = Vector(4, 5, 6) + vv1 = VectorVel(v1, v2) + vv2 = VectorVel(v2, v1) + t = TwistVel(vv1, vv2) + self.testTwistVelImpl(t) + + # Alternative constructor + tw1 = Twist(v1, v2) + tw2 = Twist(v2, v1) + t2 = TwistVel(tw1, tw2) + self.assertEqual(t, t2) + + # Members + self.assertEqual(t.vel, vv1) + self.assertEqual(t.rot, vv2) + self.assertEqual(t2.value(), tw1) + self.assertEqual(t2.deriv(), tw2) + self.assertEqual(t2.GetTwist(), tw1) + self.assertEqual(t2.GetTwistDot(), tw2) + + # Equality + self.assertEqual(TwistVel(t).vel, t.vel) + self.assertEqual(TwistVel(t).rot, t.rot) + self.assertFalse(t == -t) # Doesn't work for zero TwistVel + self.assertFalse(Equal(t, -t)) # Doesn't work for zero TwistVel + self.assertTrue(t != -t) # Doesn't work for zero TwistVel + self.assertTrue(not Equal(t, -t)) # Doesn't work for zero TwistVel + + t = TwistVel(VectorVel(v1), VectorVel(v2)) + t2 = Twist(v1, v2) + self.assertEqual(t, t2) + self.assertEqual(t2, t) + + # Zero + SetToZero(t) + self.assertEqual(t, TwistVel()) + self.assertEqual(TwistVel.Zero(), TwistVel()) + + def testTwistVelImpl(self, t): + self.assertEqual(2*t-t, t) + self.assertEqual(t*2-t, t) + self.assertEqual(t+t+t-2*t, t) + t2 = TwistVel(t) + self.assertEqual(t, t2) + t2 += t + self.assertEqual(2*t, t2) + t2 -= t + self.assertEqual(t, t2) + t2.ReverseSign() + self.assertEqual(t, -t2) + self.assertEqual(t*doubleVel(), doubleVel()*t) + self.assertEqual(t*doubleVel(5), doubleVel(5)*t) + self.assertEqual(t * doubleVel(3, 5), doubleVel(3, 5) * t) def testRotationVel(self): - v=VectorVel(Vector(9,4,-2),Vector(-5,6,-2)) - vt=Vector(2,3,4) - a= radians(-15) - b= radians(20) - c= radians(-80) - R = RotationVel(Rotation.RPY(a,b,c),Vector(2,4,1)) - R2=RotationVel(R) - self.assertTrue(Equal(R,R2)) - self.assertTrue(Equal((R*v).Norm(),(v.Norm()))) - self.assertTrue(Equal(R.Inverse(R*v),v)) - self.assertTrue(Equal(R*R.Inverse(v),v)) - self.assertTrue(Equal(R*Rotation.Identity(),R)) - self.assertTrue(Equal(Rotation.Identity()*R,R)) - self.assertTrue(Equal(R*(R*(R*v)),(R*R*R)*v)) - self.assertTrue(Equal(R*(R*(R*vt)),(R*R*R)*vt)) - self.assertTrue(Equal(R*R.Inverse(),RotationVel.Identity())) - self.assertTrue(Equal(R.Inverse()*R,RotationVel.Identity())) - self.assertTrue(Equal(R.Inverse()*v,R.Inverse(v))) - #v2=v*v-2*v - #print dot(v2,v2) - #self.assertTrue(Equal((v2).Norm(),sqrt(dot(v2,v2)))) + v = VectorVel() + vt = Vector() + r = RotationVel() + self.testRotationVelImpl(r, v, vt) + v = VectorVel(Vector(9, 4, -2), Vector(-5, 6, -2)) + vt = Vector(2, 3, 4) + rot = Rotation.RPY(radians(-15), radians(20), radians(-80)) + vec = Vector(2, 4, 1) + r = RotationVel(rot, vec) + self.testRotationVelImpl(r, v, vt) + + # Members + self.assertEqual(r.R, rot) + self.assertEqual(r.w, vec) + self.assertEqual(r.value(), rot) + self.assertEqual(r.deriv(), vec) + + # Equality + self.assertEqual(RotationVel(r).R, r.R) + self.assertEqual(RotationVel(r).w, r.w) + self.assertEqual(RotationVel(rot), rot) + self.assertEqual(rot, RotationVel(rot)) + + def testRotationVelImpl(self, r, v, vt): + r2 = RotationVel(r) + self.assertEqual(r, r2) + self.assertEqual((r*v).Norm(), v.Norm()) + self.assertEqual(r.Inverse(r*v), v) + self.assertEqual(r*r.Inverse(v), v) + self.assertEqual(r*Rotation.Identity(), r) + self.assertEqual(Rotation.Identity()*r, r) + self.assertEqual(r*(r*(r*v)), (r*r*r)*v) + self.assertEqual(r*(r*(r*vt)), (r*r*r)*vt) + self.assertEqual(r*r.Inverse(), RotationVel.Identity()) + self.assertEqual(r.Inverse()*r, RotationVel.Identity()) + self.assertEqual(r.Inverse()*v, r.Inverse(v)) def testFrameVel(self): - v=VectorVel(Vector(3,4,5),Vector(-2,-4,-1)) - vt=Vector(-1,0,-10) - F = FrameVel(Frame(Rotation.EulerZYX(radians(10),radians(20),radians(-10)),Vector(4,-2,1)), - Twist(Vector(2,-2,-2),Vector(-5,-3,-2))) - F2=FrameVel(F) - self.assertTrue(Equal( F,F2)) - self.assertTrue(Equal( F.Inverse(F*v),v)) - self.assertTrue(Equal( F.Inverse(F*vt), vt)) - self.assertTrue(Equal( F*F.Inverse(v),v)) - self.assertTrue(Equal( F*F.Inverse(vt),vt)) - self.assertTrue(Equal( F*Frame.Identity(),F)) - self.assertTrue(Equal( Frame.Identity()*F,F)) - self.assertTrue(Equal( F*(F*(F*v)),(F*F*F)*v)) - self.assertTrue(Equal( F*(F*(F*vt)),(F*F*F)*vt)) - self.assertTrue(Equal( F*F.Inverse(),FrameVel.Identity())) - self.assertTrue(Equal( F.Inverse()*F,Frame.Identity())) - self.assertTrue(Equal( F.Inverse()*vt,F.Inverse(vt))) + v = VectorVel() + vt = Vector() + f = FrameVel() + self.testFrameVelImpl(f, v, vt) + fr_m = Rotation.EulerZYX(radians(10), radians(20), radians(-10)) + fr_p = Vector(4, -2, 1) + tw_vel = Vector(2, -2, -2) + tw_rot = Vector(-5, -3, -2) + fr = Frame(fr_m, fr_p) + tw = Twist(tw_vel, tw_rot) + f = FrameVel(fr, tw) + self.testFrameVelImpl(f, v, vt) + v = VectorVel(Vector(3, 4, 5), Vector(-2, -4, -1)) + vt = Vector(-1, 0, -10) + self.testFrameVelImpl(f, v, vt) + + # Alternative constructor + rv = RotationVel(fr_m, tw_rot) + vv = VectorVel(fr_p, tw_vel) + f2 = FrameVel(rv, vv) + self.assertEqual(f, f2) + + # Members + self.assertEqual(f.M, rv) + self.assertEqual(f.p, vv) + self.assertEqual(f2.value(), fr) + self.assertEqual(f2.deriv(), tw) + + # Equality + self.assertEqual(FrameVel(f).M, f.M) + self.assertEqual(FrameVel(f).p, f.p) + + f = FrameVel(fr) + self.assertEqual(f, fr) + self.assertEqual(fr, f) + + def testFrameVelImpl(self, f, v, vt): + f2 = FrameVel(f) + self.assertEqual(f, f2) + self.assertEqual(f.Inverse(f*v), v) + self.assertEqual(f.Inverse(f*vt), vt) + self.assertEqual(f*f.Inverse(v), v) + self.assertEqual(f*f.Inverse(vt), vt) + self.assertEqual(f*Frame.Identity(), f) + self.assertEqual(Frame.Identity()*f, f) + self.assertEqual(f*(f*(f*v)), (f*f*f)*v) + self.assertEqual(f*(f*(f*vt)), (f*f*f)*vt) + self.assertEqual(f*f.Inverse(), FrameVel.Identity()) + self.assertEqual(f.Inverse()*f, Frame.Identity()) + self.assertEqual(f.Inverse()*vt, f.Inverse(vt)) def testPickle(self): - rot = Rotation.RotX(1.3) - import pickle + if sys.version_info < (3, 0): + import cPickle as pickle + else: + import pickle data = {} - data['vv'] = VectorVel(Vector(1,2,3), Vector(4,5,6)) - data['rv'] = RotationVel(rot, Vector(4.1,5.1,6.1)) + data['vv'] = VectorVel(Vector(1, 2, 3), Vector(4, 5, 6)) + data['rv'] = RotationVel(Rotation.RotX(1.3), Vector(4.1, 5.1, 6.1)) data['fv'] = FrameVel(data['rv'], data['vv']) data['tv'] = TwistVel(data['vv'], data['vv']) - - f = open('/tmp/pickle_test_kdl_framevel', 'w') - pickle.dump(data, f) - f.close() - - f = open('/tmp/pickle_test_kdl_framevel', 'r') - data1 = pickle.load(f) - f.close() - - self.assertEqual(data['vv'].p, data1['vv'].p) - self.assertEqual(data['vv'].v, data1['vv'].v) - self.assertEqual(data['rv'].R, data1['rv'].R) - self.assertEqual(data['rv'].w, data1['rv'].w) - self.assertEqual(data['fv'].M.R, data1['fv'].M.R) - self.assertEqual(data['fv'].M.w, data1['fv'].M.w) - self.assertEqual(data['fv'].p.p, data1['fv'].p.p) - self.assertEqual(data['fv'].p.v, data1['fv'].p.v) - self.assertEqual(data['tv'].vel.p, data1['tv'].vel.p) - self.assertEqual(data['tv'].vel.v, data1['tv'].vel.v) - self.assertEqual(data['tv'].rot.p, data1['tv'].rot.p) - self.assertEqual(data['tv'].rot.v, data1['tv'].rot.v) + + with open('/tmp/pickle_test_kdl_framevel', 'wb') as f: + pickle.dump(data, f, 2) + + with open('/tmp/pickle_test_kdl_framevel', 'rb') as f: + data1 = pickle.load(f) + + self.assertEqual(data, data1) #void TestTwistVel() { # KDL_CTX; @@ -189,8 +359,10 @@ def testPickle(self): # def suite(): - suite=unittest.TestSuite() + suite = unittest.TestSuite() + suite.addTest(FrameVelTestFunctions('testdoubleVel')) suite.addTest(FrameVelTestFunctions('testVectorVel')) + suite.addTest(FrameVelTestFunctions('testTwistVel')) suite.addTest(FrameVelTestFunctions('testRotationVel')) suite.addTest(FrameVelTestFunctions('testFrameVel')) suite.addTest(FrameVelTestFunctions('testPickle')) @@ -201,3 +373,11 @@ def suite(): +if __name__ == '__main__': + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/jointtypetest.py b/python_orocos_kdl/tests/jointtypetest.py new file mode 100644 index 000000000..7081b962e --- /dev/null +++ b/python_orocos_kdl/tests/jointtypetest.py @@ -0,0 +1,48 @@ +# Copyright (C) 2020 Ruben Smits + +# Version: 1.0 +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits +# URL: http://www.orocos.org/kdl + +# This library is free software; you can redistribute it and/or +# modify it under the terms of the GNU Lesser General Public +# License as published by the Free Software Foundation; either +# version 2.1 of the License, or (at your option) any later version. + +# This library 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 +# Lesser General Public License for more details. + +# You should have received a copy of the GNU Lesser General Public +# License along with this library; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +from PyKDL import Joint +import unittest + + +class JointTypeNoneTest(unittest.TestCase): + def testJointType(self): + self.assertEqual(Joint.Fixed, Joint.None) + self.assertEqual(str(Joint.Fixed), str(Joint.None)) + self.assertEqual(int(Joint.Fixed), int(Joint.None)) + + +def suite(): + suite = unittest.TestSuite() + suite.addTest(JointTypeNoneTest('testJointType')) + return suite + + +if __name__ == '__main__': + import sys + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 19f39c0f5..7f90c1e7d 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -1,8 +1,9 @@ -# Copyright (C) 2007 Ruben Smits +# Copyright (C) 2020 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or @@ -20,138 +21,250 @@ # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -import unittest +from builtins import range + +import gc +import psutil from PyKDL import * -from math import * import random +import sys +import unittest + class KinfamTestFunctions(unittest.TestCase): - def setUp(self): self.chain = Chain() self.chain.addSegment(Segment(Joint(Joint.RotZ), - Frame(Vector(0.0,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.RotX), - Frame(Vector(0.0,0.0,0.9)))) - self.chain.addSegment(Segment(Joint(Joint.None), - Frame(Vector(-0.4,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.9)))) + self.chain.addSegment(Segment(Joint(Joint.Fixed), + Frame(Vector(-0.4, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.RotY), - Frame(Vector(0.0,0.0,1.2)))) - self.chain.addSegment(Segment(Joint(Joint.None), - Frame(Vector(0.4,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 1.2)))) + self.chain.addSegment(Segment(Joint(Joint.Fixed), + Frame(Vector(0.4, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.TransZ), - Frame(Vector(0.0,0.0,1.4)))) + Frame(Vector(0.0, 0.0, 1.4)))) self.chain.addSegment(Segment(Joint(Joint.TransX), - Frame(Vector(0.0,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.TransY), - Frame(Vector(0.0,0.0,0.4)))) - self.chain.addSegment(Segment(Joint(Joint.None), - Frame(Vector(0.0,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.4)))) + self.chain.addSegment(Segment(Joint(Joint.Fixed), + Frame(Vector(0.0, 0.0, 0.0)))) - self.jacsolver = ChainJntToJacSolver(self.chain) + self.jacsolver = ChainJntToJacSolver(self.chain) self.fksolverpos = ChainFkSolverPos_recursive(self.chain) self.fksolvervel = ChainFkSolverVel_recursive(self.chain) self.iksolvervel = ChainIkSolverVel_pinv(self.chain) - self.iksolverpos = ChainIkSolverPos_NR(self.chain,self.fksolverpos,self.iksolvervel) + self.iksolverpos = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel) + + def testRotationalInertia(self): + ri = RotationalInertia(1, 2, 3, 4, 7, 5) + # __getitem__ + for i in range(3): + for j in range(3): + self.assertEqual(ri[3*i+j], 2*abs(i-j) + max(i, j) + 1) + with self.assertRaises(IndexError): + _ = ri[-1] + with self.assertRaises(IndexError): + _ = ri[9] + + # __setitem__ + for i in range(3): + for j in range(3): + ri[i] = i + for i in range(3): + for j in range(3): + self.assertEqual(ri[i], i) + with self.assertRaises(IndexError): + ri[-1] = 1 + with self.assertRaises(IndexError): + ri[9] = 1 + + def testJacobian(self): + jac = Jacobian(3) + for i in range(jac.columns()): + jac.setColumn(i, Twist(Vector(6*i+1, 6*i+2, 6*i+3), Vector(6*i+4, 6*i+5, 6*i+6))) + # __getitem__ + for i in range(6): + for j in range(3): + self.assertEqual(jac[i, j], 6*j+i+1) + with self.assertRaises(IndexError): + _ = jac[-1, 0] + with self.assertRaises(IndexError): + _ = jac[6, 0] + with self.assertRaises(IndexError): + _ = jac[5, -1] + with self.assertRaises(IndexError): + _ = jac[5, 3] + + # __setitem__ + for i in range(6): + for j in range(3): + jac[i, j] = 3*i+j + for i in range(6): + for j in range(3): + self.assertEqual(jac[i, j], 3*i+j) + with self.assertRaises(IndexError): + jac[-1, 0] = 1 + with self.assertRaises(IndexError): + jac[6, 0] = 1 + with self.assertRaises(IndexError): + jac[5, -1] = 1 + with self.assertRaises(IndexError): + jac[5, 3] = 1 + + def testJntArray(self): + ja = JntArray(3) + # __getitem__ + for i in range(3): + self.assertEqual(ja[i], 0) + with self.assertRaises(IndexError): + _ = ja[-1] + with self.assertRaises(IndexError): + _ = ja[3] + + # __setitem__ + for i in range(3): + ja[i] = 2*i + for i in range(3): + self.assertEqual(ja[i], 2*i) + with self.assertRaises(IndexError): + ja[-1] = 1 + with self.assertRaises(IndexError): + ja[3] = 1 def testFkPosAndJac(self): deltaq = 1E-4 epsJ = 1E-4 - F1=Frame() - F2=Frame() + F1 = Frame() + F2 = Frame() + + q = JntArray(self.chain.getNrOfJoints()) + jac = Jacobian(self.chain.getNrOfJoints()) - q=JntArray(self.chain.getNrOfJoints()) - jac=Jacobian(self.chain.getNrOfJoints()) - for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) + + self.jacsolver.JntToJac(q, jac) - self.jacsolver.JntToJac(q,jac) - for i in range(q.rows()): - oldqi=q[i]; - q[i]=oldqi+deltaq - self.assertTrue(0==self.fksolverpos.JntToCart(q,F2)) - q[i]=oldqi-deltaq - self.assertTrue(0==self.fksolverpos.JntToCart(q,F1)) - q[i]=oldqi - Jcol1 = diff(F1,F2,2*deltaq) - Jcol2 = Twist(Vector(jac[0,i],jac[1,i],jac[2,i]), - Vector(jac[3,i],jac[4,i],jac[5,i])) - self.assertEqual(Jcol1,Jcol2); + oldqi = q[i] + q[i] = oldqi+deltaq + self.assertTrue(0 == self.fksolverpos.JntToCart(q, F2)) + q[i] = oldqi-deltaq + self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1)) + q[i] = oldqi + Jcol1 = diff(F1, F2, 2*deltaq) + Jcol2 = Twist(Vector(jac[0, i], jac[1, i], jac[2, i]), + Vector(jac[3, i], jac[4, i], jac[5, i])) + self.assertEqual(Jcol1, Jcol2) def testFkVelAndJac(self): deltaq = 1E-4 - epsJ = 1E-4 - - q=JntArray(self.chain.getNrOfJoints()) - qdot=JntArray(self.chain.getNrOfJoints()) + epsJ = 1E-4 + + q = JntArray(self.chain.getNrOfJoints()) + qdot = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) - qdot[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) + qdot[i] = random.uniform(-3.14, 3.14) - qvel=JntArrayVel(q,qdot); - jac=Jacobian(self.chain.getNrOfJoints()) + qvel = JntArrayVel(q, qdot) + jac = Jacobian(self.chain.getNrOfJoints()) - cart = FrameVel.Identity(); - t = Twist.Zero(); + cart = FrameVel.Identity() + t = Twist.Zero() - self.jacsolver.JntToJac(qvel.q,jac) - self.assertTrue(self.fksolvervel.JntToCart(qvel,cart)==0) - MultiplyJacobian(jac,qvel.qdot,t) - self.assertEqual(cart.deriv(),t) + self.jacsolver.JntToJac(qvel.q, jac) + self.assertTrue(self.fksolvervel.JntToCart(qvel, cart) == 0) + MultiplyJacobian(jac, qvel.qdot, t) + self.assertEqual(cart.deriv(), t) def testFkVelAndIkVel(self): epsJ = 1E-7 - q=JntArray(self.chain.getNrOfJoints()) - qdot=JntArray(self.chain.getNrOfJoints()) + q = JntArray(self.chain.getNrOfJoints()) + qdot = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) - qdot[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) + qdot[i] = random.uniform(-3.14, 3.14) + + qvel = JntArrayVel(q, qdot) + qdot_solved = JntArray(self.chain.getNrOfJoints()) - qvel=JntArrayVel(q,qdot) - qdot_solved=JntArray(self.chain.getNrOfJoints()) - cart = FrameVel() - - self.assertTrue(0==self.fksolvervel.JntToCart(qvel,cart)) - self.assertTrue(0==self.iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved)) - - self.assertEqual(qvel.qdot,qdot_solved); - + + self.assertTrue(0 == self.fksolvervel.JntToCart(qvel, cart)) + self.assertTrue(0 == self.iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved)) + + self.assertEqual(qvel.qdot, qdot_solved) def testFkPosAndIkPos(self): - q=JntArray(self.chain.getNrOfJoints()) + q = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) - - q_init=JntArray(self.chain.getNrOfJoints()) + q[i] = random.uniform(-3.14, 3.14) + + q_init = JntArray(self.chain.getNrOfJoints()) for i in range(q_init.rows()): - q_init[i]=q[i]+0.1*random.random() - - q_solved=JntArray(q.rows()) - - F1=Frame.Identity() - F2=Frame.Identity() - - self.assertTrue(0==self.fksolverpos.JntToCart(q,F1)) - self.assertTrue(0==self.iksolverpos.CartToJnt(q_init,F1,q_solved)) - self.assertTrue(0==self.fksolverpos.JntToCart(q_solved,F2)) - - self.assertEqual(F1,F2) - self.assertEqual(q,q_solved) - - + q_init[i] = q[i]+0.1*random.random() + + q_solved = JntArray(q.rows()) + + F1 = Frame.Identity() + F2 = Frame.Identity() + + self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1)) + self.assertTrue(0 == self.iksolverpos.CartToJnt(q_init, F1, q_solved)) + self.assertTrue(0 == self.fksolverpos.JntToCart(q_solved, F2)) + + self.assertEqual(F1, F2) + self.assertEqual(q, q_solved) + + +class KinfamTestTree(unittest.TestCase): + + def setUp(self): + self.tree = Tree() + self.tree.addSegment(Segment(Joint(Joint.RotZ), + Frame(Vector(0.0, 0.0, 0.0))), "foo") + self.tree.addSegment(Segment(Joint(Joint.Fixed), + Frame(Vector(0.0, 0.0, 0.0))), "bar") + + def testTreeGetChainMemLeak(self): + # test for the memory leak in Tree.getChain described in issue #211 + process = psutil.Process() + self.tree.getChain("foo", "bar") + gc.collect() + mem_before = process.memory_info().vms + # needs at least 2000 iterations on my system to cause a detectable + # difference in memory usage + for _ in range(10000): + self.tree.getChain("foo", "bar") + gc.collect() + mem_after = process.memory_info().vms + self.assertEqual(mem_before, mem_after) + + def suite(): suite = unittest.TestSuite() + suite.addTest(KinfamTestFunctions('testRotationalInertia')) + suite.addTest(KinfamTestFunctions('testJacobian')) + suite.addTest(KinfamTestFunctions('testJntArray')) suite.addTest(KinfamTestFunctions('testFkPosAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndIkVel')) suite.addTest(KinfamTestFunctions('testFkPosAndIkPos')) return suite -#suite = suite() -#unittest.TextTestRunner(verbosity=3).run(suite) - + +if __name__ == '__main__': + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) From 74c5c6c889759530a0571c500010b29175ee597c Mon Sep 17 00:00:00 2001 From: Ruben Smits Date: Thu, 27 Dec 2018 15:39:59 +0100 Subject: [PATCH 47/79] Merge pull request #155 from craigirobot/upstream-master-add-joint-inertia-dynamics Add joint inertia dynamics --- orocos_kdl/src/chaindynparam.cpp | 3 ++- orocos_kdl/src/joint.hpp | 10 ++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/src/chaindynparam.cpp b/orocos_kdl/src/chaindynparam.cpp index ccf6b478b..814a94ef9 100644 --- a/orocos_kdl/src/chaindynparam.cpp +++ b/orocos_kdl/src/chaindynparam.cpp @@ -99,7 +99,8 @@ namespace KDL { F=Ic[i]*S[i]; if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { - H(k,k)=dot(S[i],F); + H(k,k)=dot(S[i],F); + H(k,k)+=chain.getSegment(i).getJoint().getInertia(); // add joint inertia j=k; //countervariable for the joints l=i; //countervariable for the segments while(l!=0) //go from leaf to root starting at i diff --git a/orocos_kdl/src/joint.hpp b/orocos_kdl/src/joint.hpp index ad1cc0cae..49c3e391f 100644 --- a/orocos_kdl/src/joint.hpp +++ b/orocos_kdl/src/joint.hpp @@ -192,6 +192,16 @@ namespace KDL { } }; + /** + * Request the inertia of the joint. + * + * @return const reference to the inertia of the joint + */ + const double& getInertia() const + { + return inertia; + }; + virtual ~Joint(); private: From 0f464cd41fc753f08b94aa916c190624c6464201 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Fri, 4 Feb 2022 10:22:35 +0100 Subject: [PATCH 48/79] fix ABI compatibility after pybind11 integration This patch adds an overload for the methods Vector::Norm() and Vector2::Norm() that would restore the ABI compatibility after breaking it because of #136. --- orocos_kdl/src/frames.cpp | 9 +++++++++ orocos_kdl/src/frames.hpp | 10 ++++++++-- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index 513275479..f33e519fd 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -85,6 +85,11 @@ namespace KDL { ); } + double Vector2::Norm() const + { + return Norm(KDL::epsilon); + } + double Vector2::Norm(double eps) const { double tmp1 = fabs(data[0]); @@ -113,6 +118,10 @@ namespace KDL { } } + double Vector::Norm() const + { + return Norm(KDL::epsilon); + } // do some effort not to lose precision double Vector::Norm(double eps) const diff --git a/orocos_kdl/src/frames.hpp b/orocos_kdl/src/frames.hpp index 0710d75b5..205835f09 100644 --- a/orocos_kdl/src/frames.hpp +++ b/orocos_kdl/src/frames.hpp @@ -237,7 +237,10 @@ class Vector double Normalize(double eps=epsilon); //! @return the norm of the vector - double Norm(double eps=epsilon) const; + double Norm() const; + + //! @return the norm of the vector + double Norm(double eps) const; @@ -1015,7 +1018,10 @@ class Vector2 double Normalize(double eps=epsilon); //! @return the norm of the vector - double Norm(double eps=epsilon) const; + double Norm() const; + + //! @return the norm of the vector + double Norm(double eps) const; //! projects v in its XY plane, and sets *this to these values inline void Set3DXY(const Vector& v); From c315a2306db6c4424688de96c894293f46c90a31 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Fri, 4 Feb 2022 11:33:07 +0100 Subject: [PATCH 49/79] fix pybind11 for overloaded Norm() function The ABI compatibility was restored by overloading the Norm() members of Vector and Vector2. This patch adds the overloads to the python bindings made by pybind11. --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 65afa0f10..2763af56f 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -81,7 +81,9 @@ void init_frames(py::module &m) return operator-(a); }, py::is_operator()); vector.def_static("Zero", &Vector::Zero); - vector.def("Norm", &Vector::Norm, py::arg("eps")=epsilon); + vector.def("Norm", static_cast(&Vector::Norm)); + vector.def("Norm", static_cast(&Vector::Norm), py::arg("eps")); + // vector.def("Norm", static_cast(&Vector::Norm)); vector.def("Normalize", &Vector::Normalize, py::arg("eps")=epsilon); vector.def(py::pickle( [](const Vector &v) From 7f3aa617df9cf290de912af4a9ab9bf91553f216 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 11 May 2020 12:49:03 +0200 Subject: [PATCH 50/79] Merge pull request #180 from martiniil/fix/path_circle_memory_leak Fix memory leak in Path_Circle constructor. Fixes #177 --- orocos_kdl/src/path_circle.cpp | 97 +++++++++++++++++----------------- 1 file changed, 49 insertions(+), 48 deletions(-) diff --git a/orocos_kdl/src/path_circle.cpp b/orocos_kdl/src/path_circle.cpp index 5efd22bc8..53261e832 100644 --- a/orocos_kdl/src/path_circle.cpp +++ b/orocos_kdl/src/path_circle.cpp @@ -45,51 +45,56 @@ namespace KDL { - - Path_Circle::Path_Circle(const Frame& F_base_start, - const Vector& _V_base_center, - const Vector& V_base_p, - const Rotation& R_base_end, - double alpha, - RotationalInterpolation* _orient, - double _eqradius, - bool _aggregate) : - orient(_orient) , - eqradius(_eqradius), - aggregate(_aggregate) - { - F_base_center.p = _V_base_center; - orient->SetStartEnd(F_base_start.M,R_base_end); - double oalpha = orient->Angle(); - - Vector x(F_base_start.p - F_base_center.p); - radius = x.Normalize(); - if (radius < epsilon) throw Error_MotionPlanning_Circle_ToSmall(); - Vector tmpv(V_base_p-F_base_center.p); - tmpv.Normalize(); - Vector z( x * tmpv); - double n = z.Normalize(); - if (n < epsilon) throw Error_MotionPlanning_Circle_No_Plane(); - F_base_center.M = Rotation(x,z*x,z); - double dist = alpha*radius; - // See what has the slowest eq. motion, and adapt - // the other to this slower motion - // use eqradius to transform between rot and transl. - // the same as for lineair motion - if (oalpha*eqradius > dist) { - // rotational_interpolation is the limitation - pathlength = oalpha*eqradius; - scalerot = 1/eqradius; - scalelin = dist/pathlength; - } else { - // translation is the limitation - pathlength = dist; - scalerot = oalpha/pathlength; - scalelin = 1; - } - } - + const Vector& _V_base_center, + const Vector& V_base_p, + const Rotation& R_base_end, + double alpha, + RotationalInterpolation* _orient, + double _eqradius, + bool _aggregate) : + orient(_orient) , + eqradius(_eqradius), + aggregate(_aggregate) +{ + F_base_center.p = _V_base_center; + orient->SetStartEnd(F_base_start.M,R_base_end); + double oalpha = orient->Angle(); + + Vector x(F_base_start.p - F_base_center.p); + radius = x.Normalize(); + if (radius < epsilon) { + if (aggregate) + delete orient; + throw Error_MotionPlanning_Circle_ToSmall(); + } + Vector tmpv(V_base_p-F_base_center.p); + tmpv.Normalize(); + Vector z( x * tmpv); + double n = z.Normalize(); + if (n < epsilon) { + if (aggregate) + delete orient; + throw Error_MotionPlanning_Circle_No_Plane(); + } + F_base_center.M = Rotation(x,z*x,z); + double dist = alpha*radius; + // See what has the slowest eq. motion, and adapt + // the other to this slower motion + // use eqradius to transform between rot and transl. + // the same as for lineair motion + if (oalpha*eqradius > dist) { + // rotational_interpolation is the limitation + pathlength = oalpha*eqradius; + scalerot = 1/eqradius; + scalelin = dist/pathlength; + } else { + // translation is the limitation + pathlength = dist; + scalerot = oalpha/pathlength; + scalelin = 1; + } +} double Path_Circle::LengthToS(double length) { @@ -150,8 +155,6 @@ Path_Circle::~Path_Circle() { delete orient; } - - void Path_Circle::Write(std::ostream& os) { os << "CIRCLE[ "; os << " " << Pos(0) << std::endl; @@ -164,6 +167,4 @@ void Path_Circle::Write(std::ostream& os) { os << "]"<< std::endl; } - } - From 6bc46f24545725de6a4395e904332a6013c3f547 Mon Sep 17 00:00:00 2001 From: Ruben Smits Date: Thu, 9 Jan 2020 11:03:39 +0100 Subject: [PATCH 51/79] Merge pull request #189 from zchen24/patch-3 Fixed PyKDL windows target suffix bug --- python_orocos_kdl/cmake/SIPMacros.cmake | 3 +++ 1 file changed, 3 insertions(+) diff --git a/python_orocos_kdl/cmake/SIPMacros.cmake b/python_orocos_kdl/cmake/SIPMacros.cmake index bda612e2e..836387ad3 100644 --- a/python_orocos_kdl/cmake/SIPMacros.cmake +++ b/python_orocos_kdl/cmake/SIPMacros.cmake @@ -108,6 +108,9 @@ MACRO(ADD_SIP_PYTHON_MODULE MODULE_NAME MODULE_SIP) TARGET_LINK_LIBRARIES(${_logical_name} ${PYTHON_LIBRARY}) TARGET_LINK_LIBRARIES(${_logical_name} ${EXTRA_LINK_LIBRARIES}) SET_TARGET_PROPERTIES(${_logical_name} PROPERTIES PREFIX "" OUTPUT_NAME ${_child_module_name}) + IF (MSVC) + SET_TARGET_PROPERTIES(${_logical_name} PROPERTIES SUFFIX ".pyd") + ENDIF(MSVC) INSTALL(TARGETS ${_logical_name} DESTINATION "${PYTHON_SITE_PACKAGES_INSTALL_DIR}/${_parent_module_path}") From 02372b6599444f459b2c0261ea684b483e39a33d Mon Sep 17 00:00:00 2001 From: Ruben Smits Date: Thu, 9 Jan 2020 10:50:25 +0100 Subject: [PATCH 52/79] Merge pull request #193 from tfoote/python3_support Add compatibility for ROS_PYTHON_VERSION From 2d246960e16049644d281c6ca057b769cec82ced Mon Sep 17 00:00:00 2001 From: Ruben Smits Date: Wed, 23 Oct 2019 16:37:35 +0200 Subject: [PATCH 53/79] Merge pull request #195 from orocos/cherry-pick-109-1.3 Cherry pick some RotAngleAxis related fixes From 8d961883091db57f1cc8263e44202c79afb33f17 Mon Sep 17 00:00:00 2001 From: Ruben Smits Date: Thu, 9 Jan 2020 10:53:07 +0100 Subject: [PATCH 54/79] Merge pull request #199 from maverick-long/fix_rotation_angle_numerical_error back to use a more numerically stable method to compute the rotation angle --- orocos_kdl/src/frames.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index f33e519fd..aaede3c69 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -426,15 +426,13 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { return angle; // return 180 deg rotation } - // If the matrix is slightly non-orthogonal, `f` may be out of the (-1, +1) range. - // Therefore, clamp it between those values to avoid NaNs. double f = (data[0] + data[4] + data[8] - 1) / 2; - angle = acos(std::max(-1.0, std::min(1.0, f))); x = (data[7] - data[5]); y = (data[2] - data[6]); z = (data[3] - data[1]); axis = KDL::Vector(x, y, z); + angle = atan2(axis.Norm()/2,f); axis.Normalize(); return angle; } From a0dbdb160c6cdcff30eb3c882ae63a32638c7482 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Fri, 21 Feb 2020 21:39:45 +0100 Subject: [PATCH 55/79] Merge pull request #208 from release-1.3 Forward port Release 1.3 fixes From df2d031cfb4b1928ab842d9ab557d448efadfa10 Mon Sep 17 00:00:00 2001 From: Ruben Smits Date: Thu, 30 Jan 2020 14:10:16 +0100 Subject: [PATCH 56/79] Merge pull request #209 from mvieth/master General cleanup --- .../chainfdsolver_recursive_newton_euler.cpp | 20 +-- .../chainfdsolver_recursive_newton_euler.hpp | 4 +- orocos_kdl/src/chainiksolverpos_lma.cpp | 2 +- orocos_kdl/src/chainiksolverpos_lma.hpp | 2 +- orocos_kdl/src/chainiksolvervel_pinv.hpp | 2 +- .../src/chainiksolvervel_pinv_givens.cpp | 2 - .../src/chainiksolvervel_pinv_givens.hpp | 8 +- orocos_kdl/src/chainiksolvervel_pinv_nso.hpp | 2 +- orocos_kdl/src/chainiksolvervel_wdls.hpp | 2 +- orocos_kdl/src/jntarray.cpp | 4 +- orocos_kdl/src/path_roundedcomposite.cpp | 4 +- .../src/rotational_interpolation_sa.cpp | 2 +- orocos_kdl/src/utilities/svd_HH.cpp | 2 +- orocos_kdl/src/utilities/svd_eigen_HH.cpp | 24 +-- orocos_kdl/src/utilities/svd_eigen_Macie.cpp | 162 ++++++++++++++++++ orocos_kdl/src/utilities/svd_eigen_Macie.hpp | 141 +-------------- orocos_kdl/src/velocityprofile_trap.cpp | 18 +- orocos_kdl/src/velocityprofile_traphalf.cpp | 14 +- 18 files changed, 218 insertions(+), 197 deletions(-) create mode 100644 orocos_kdl/src/utilities/svd_eigen_Macie.cpp diff --git a/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp index 5da6c50bf..3a1a0c3bf 100644 --- a/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp +++ b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp @@ -70,7 +70,7 @@ namespace KDL{ return (error); // Calculate non-inertial internal torques by inputting zero joint acceleration to ID - for(int i=0;i maxarg2 ? maxarg1 : maxarg2; + anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2; } /* Accumulation of right-hand transformations. */ for (i=cols-1;i>=0;i--) { diff --git a/orocos_kdl/src/utilities/svd_eigen_HH.cpp b/orocos_kdl/src/utilities/svd_eigen_HH.cpp index 36da50914..46ce575dc 100644 --- a/orocos_kdl/src/utilities/svd_eigen_HH.cpp +++ b/orocos_kdl/src/utilities/svd_eigen_HH.cpp @@ -53,14 +53,14 @@ namespace KDL{ s += U(k,i)*U(k,i); } f=U(i,i); // f is the diag elem - if (!(s>=0)) return -3; + if (!(s>=0)) return -3; g = -SIGN(sqrt(s),f); h=f*g-s; U(i,i)=f-g; for (j=ppi;j=0)) return -5; + if (!(s>=0)) return -5; g = -SIGN(sqrt(s),f); h=f*g-s; U(i,ppi)=f-g; - if (!(h!=0)) return -6; + if (!(h!=0)) return -6; for (k=ppi;k maxarg2 ? maxarg1 : maxarg2; + anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2; } /* Accumulation of right-hand transformations. */ for (i=cols-1;i>=0;i--) { if (iepsilon) { - if (!(U(i,ppi)!=0)) return -7; + if (!(U(i,ppi)!=0)) return -7; for (j=ppi;j + +// Version: 1.0 +// Author: Ruben Smits +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library 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 +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "svd_eigen_Macie.hpp" + +namespace KDL{ + int svd_eigen_Macie(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S, Eigen::MatrixXd& V, + Eigen::MatrixXd& B, Eigen::VectorXd& tempi, + double threshold,bool toggle) + { + bool rotate = true; + unsigned int sweeps=0; + unsigned int rotations=0; + if(toggle){ + //Calculate B from new A and previous V + B=A.lazyProduct(V); + while(rotate){ + rotate=false; + rotations=0; + //Perform rotations between columns of B + for(unsigned int i=0;i=0){ + cos=sqrt((c+q)/(2*c)); + sin=p/(c*cos); + }else{ + if(p<0) + sin=-sqrt((c-q)/(2*c)); + else + sin=sqrt((c-q)/(2*c)); + cos=p/(c*sin); + } + + //Apply plane rotation to columns of B + tempi = cos*B.col(i) + sin*B.col(j); + B.col(j) = - sin*B.col(i) + cos*B.col(j); + B.col(i) = tempi; + + //Apply plane rotation to columns of V + tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j); + V.col(j) = - sin*V.col(i) + cos*V.col(j); + V.col(i) = tempi.head(V.rows()); + + rotate=true; + } + } + //Only calculate new U and S if there were any rotations + if(rotations!=0){ + for(unsigned int i=0;i=0){ + cos=sqrt((c+q)/(2*c)); + sin=p/(c*cos); + }else{ + if(p<0) + sin=-sqrt((c-q)/(2*c)); + else + sin=sqrt((c-q)/(2*c)); + cos=p/(c*sin); + } + + //Apply plane rotation to rows of B + tempi.head(B.cols()) = cos*B.row(i) + sin*B.row(j); + B.row(j) = - sin*B.row(i) + cos*B.row(j); + B.row(i) = tempi.head(B.cols()); + + + //Apply plane rotation to rows of U + tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j); + U.col(j) = - sin*U.col(i) + cos*U.col(j); + U.col(i) = tempi.head(U.rows()); + + rotate=true; + } + } + + //Only calculate new U and S if there were any rotations + if(rotations!=0){ + for(unsigned int i=0;i -using namespace Eigen; namespace KDL @@ -56,143 +55,9 @@ namespace KDL * \param toggle [INPUT] toggle this boolean variable on each call of this routine. * \return number of sweeps. */ - int svd_eigen_Macie(const MatrixXd& A,MatrixXd& U,VectorXd& S, MatrixXd& V, - MatrixXd& B, VectorXd& tempi, - double threshold,bool toggle) - { - bool rotate = true; - unsigned int sweeps=0; - unsigned int rotations=0; - if(toggle){ - //Calculate B from new A and previous V - B=A.lazyProduct(V); - while(rotate){ - rotate=false; - rotations=0; - //Perform rotations between columns of B - for(unsigned int i=0;i=0){ - cos=sqrt((c+q)/(2*c)); - sin=p/(c*cos); - }else{ - if(p<0) - sin=-sqrt((c-q)/(2*c)); - else - sin=sqrt((c-q)/(2*c)); - cos=p/(c*sin); - } - - //Apply plane rotation to columns of B - tempi = cos*B.col(i) + sin*B.col(j); - B.col(j) = - sin*B.col(i) + cos*B.col(j); - B.col(i) = tempi; - - //Apply plane rotation to columns of V - tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j); - V.col(j) = - sin*V.col(i) + cos*V.col(j); - V.col(i) = tempi.head(V.rows()); - - rotate=true; - } - } - //Only calculate new U and S if there were any rotations - if(rotations!=0){ - for(unsigned int i=0;i=0){ - cos=sqrt((c+q)/(2*c)); - sin=p/(c*cos); - }else{ - if(p<0) - sin=-sqrt((c-q)/(2*c)); - else - sin=sqrt((c-q)/(2*c)); - cos=p/(c*sin); - } - - //Apply plane rotation to rows of B - tempi.head(B.cols()) = cos*B.row(i) + sin*B.row(j); - B.row(j) = - sin*B.row(i) + cos*B.row(j); - B.row(i) = tempi.head(B.cols()); - - - //Apply plane rotation to rows of U - tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j); - U.col(j) = - sin*U.col(i) + cos*U.col(j); - U.col(i) = tempi.head(U.rows()); - - rotate=true; - } - } - - //Only calculate new U and S if there were any rotations - if(rotations!=0){ - for(unsigned int i=0;i 1) - return; // do not exceed max + if (factor > 1) + return; // do not exceed max a2*=factor; a3*=factor*factor; b2*=factor; @@ -110,9 +110,9 @@ void VelocityProfile_Trap::SetProfileDuration( void VelocityProfile_Trap::SetProfileVelocity( double pos1,double pos2,double newvelocity) { - // Max velocity + // Max velocity SetProfile(pos1,pos2); - // Must be Slower : + // Must be Slower : double factor = newvelocity; // valid = [KDL::epsilon, 1.0] if (1.0 < factor) factor = 1.0; if (KDL::epsilon > factor) factor = KDL::epsilon; @@ -129,7 +129,7 @@ void VelocityProfile_Trap::SetProfileVelocity( void VelocityProfile_Trap::SetMax(double _maxvel,double _maxacc) { - maxvel = _maxvel; maxacc = _maxacc; + maxvel = _maxvel; maxacc = _maxacc; } double VelocityProfile_Trap::Duration() const { @@ -178,8 +178,8 @@ double VelocityProfile_Trap::Acc(double time) const { } VelocityProfile* VelocityProfile_Trap::Clone() const { - VelocityProfile_Trap* res = new VelocityProfile_Trap(maxvel,maxacc); - res->SetProfileDuration( this->startpos, this->endpos, this->duration ); + VelocityProfile_Trap* res = new VelocityProfile_Trap(maxvel,maxacc); + res->SetProfileDuration( this->startpos, this->endpos, this->duration ); return res; } diff --git a/orocos_kdl/src/velocityprofile_traphalf.cpp b/orocos_kdl/src/velocityprofile_traphalf.cpp index 80b63e0d6..05ff180bc 100644 --- a/orocos_kdl/src/velocityprofile_traphalf.cpp +++ b/orocos_kdl/src/velocityprofile_traphalf.cpp @@ -52,7 +52,7 @@ VelocityProfile_TrapHalf::VelocityProfile_TrapHalf(double _maxvel,double _maxacc void VelocityProfile_TrapHalf::SetMax(double _maxvel,double _maxacc, bool _starting) { - maxvel = _maxvel; maxacc = _maxacc; starting = _starting; + maxvel = _maxvel; maxacc = _maxacc; starting = _starting; } void VelocityProfile_TrapHalf::PlanProfile1(double v,double a) { @@ -100,11 +100,11 @@ void VelocityProfile_TrapHalf::SetProfile(double pos1,double pos2) { void VelocityProfile_TrapHalf::SetProfileDuration( double pos1,double pos2,double newduration) { - SetProfile(pos1,pos2); - double factor = duration/newduration; + SetProfile(pos1,pos2); + double factor = duration/newduration; - if ( factor > 1 ) - return; + if ( factor > 1 ) + return; double s = sign(endpos-startpos); double tmp = 2.0*s*(endpos-startpos)/maxvel; @@ -183,8 +183,8 @@ double VelocityProfile_TrapHalf::Acc(double time) const { } VelocityProfile* VelocityProfile_TrapHalf::Clone() const { - VelocityProfile_TrapHalf* res = new VelocityProfile_TrapHalf(maxvel,maxacc, starting); - res->SetProfileDuration( this->startpos, this->endpos, this->duration ); + VelocityProfile_TrapHalf* res = new VelocityProfile_TrapHalf(maxvel,maxacc, starting); + res->SetProfileDuration( this->startpos, this->endpos, this->duration ); return res; } From e5d6d6c67a48dbe683a49d4e17c56603ed0daf5d Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Wed, 19 Feb 2020 14:11:33 +0100 Subject: [PATCH 57/79] Merge pull request #215 from ros2/ivanpauno/fix-build-warning-focal Fix build warnings in gcc 9.2.1 --- orocos_kdl/src/frames.hpp | 4 +++- orocos_kdl/src/frames.inl | 9 ++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/frames.hpp b/orocos_kdl/src/frames.hpp index 205835f09..52f854956 100644 --- a/orocos_kdl/src/frames.hpp +++ b/orocos_kdl/src/frames.hpp @@ -313,8 +313,8 @@ class Rotation double Xy,double Yy,double Zy, double Xz,double Yz,double Zz); inline Rotation(const Vector& x,const Vector& y,const Vector& z); - // default copy constructor is sufficient + inline Rotation(const Rotation& arg); inline Rotation& operator=(const Rotation& arg); @@ -1066,6 +1066,8 @@ class Rotation2 Rotation2(double ca,double sa):s(sa),c(ca){} + Rotation2(const Rotation2& arg); + inline Rotation2& operator=(const Rotation2& arg); inline Vector2 operator*(const Vector2& v) const; //! Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set diff --git a/orocos_kdl/src/frames.inl b/orocos_kdl/src/frames.inl index ac62a997a..d2c51c431 100644 --- a/orocos_kdl/src/frames.inl +++ b/orocos_kdl/src/frames.inl @@ -512,6 +512,11 @@ Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z) data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2]; } +Rotation::Rotation(const Rotation& arg) { + int count=9; + while (count--) data[count] = arg.data[count]; +} + Rotation& Rotation::operator=(const Rotation& arg) { int count=9; while (count--) data[count] = arg.data[count]; @@ -839,7 +844,9 @@ IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_som data[1]=tmp(1); } - +IMETHOD Rotation2::Rotation2(const Rotation2& arg) { + c=arg.c;s=arg.s; +} IMETHOD Rotation2& Rotation2::operator=(const Rotation2& arg) { c=arg.c;s=arg.s; From 2e2b480f83fe9f1b4ae62a8f4327f214c96a3f83 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Thu, 20 Feb 2020 11:57:33 +0100 Subject: [PATCH 58/79] Merge pull request #221 from sloretz/bump_cmake_cmp0048 Use CMP0048 New behavior to suppress warning --- orocos_kdl/CMakeLists.txt | 3 +++ orocos_kinematics_dynamics/CMakeLists.txt | 3 +++ python_orocos_kdl/CMakeLists.txt | 3 +++ 3 files changed, 9 insertions(+) diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index 86f3574b5..2bbc61436 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -2,6 +2,9 @@ # Test CMake version # CMAKE_MINIMUM_REQUIRED(VERSION 2.6) +IF(POLICY CMP0048) + CMAKE_POLICY(SET CMP0048 NEW) +ENDIF() #MARK_AS_ADVANCED( FORCE CMAKE_BACKWARDS_COMPATIBILITY ) diff --git a/orocos_kinematics_dynamics/CMakeLists.txt b/orocos_kinematics_dynamics/CMakeLists.txt index 2b2e95004..6fea52f0d 100644 --- a/orocos_kinematics_dynamics/CMakeLists.txt +++ b/orocos_kinematics_dynamics/CMakeLists.txt @@ -1,4 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) +if(POLICY CMP0048) + cmake_policy(SET CMP0048 NEW) +endif() project(orocos_kinematics_dynamics) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 77eb3552a..98891f584 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -1,4 +1,7 @@ cmake_minimum_required(VERSION 2.4.6) +if(POLICY CMP0048) + cmake_policy(SET CMP0048 NEW) +endif() project(python_orocos_kdl) From f484dab5dcd59f264fced028dd39787a66898878 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Fri, 13 Mar 2020 02:39:21 +0100 Subject: [PATCH 59/79] Merge pull request #226 from sloretz/patch-1 python_orocos_kdl requires orocos_kdl --- python_orocos_kdl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 98891f584..e8c074e23 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -5,7 +5,7 @@ endif() project(python_orocos_kdl) -find_package(orocos_kdl) +find_package(orocos_kdl REQUIRED) include_directories(${orocos_kdl_INCLUDE_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) From adec39717745c4b1841b72003856574f197077bf Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 7 May 2020 08:56:08 +0200 Subject: [PATCH 60/79] Merge pull request #237 from orocos/pybind_2_5_0 (PyKDL) update pybind11 to 2.5.0 --- .gitmodules | 2 +- python_orocos_kdl/pybind11 | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 36b541a9c..6c6a02f83 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,4 @@ [submodule "python_orocos_kdl/pybind11"] path = python_orocos_kdl/pybind11 url = https://github.com/pybind/pybind11.git - branch = v2.4.3 + branch = v2.5.0 diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 80d452484..3b1dbebab 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 80d452484c5409444b0ec19383faa84bb7a4d351 +Subproject commit 3b1dbebabc801c9cf6f0953a4c20b904d444f879 From c8589d2245a7ea0c8dbcad3886943ee856c45e20 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 17 May 2020 21:08:53 +0200 Subject: [PATCH 61/79] Merge pull request #244 from orocos/deepcopy_pybind Add (deep)copy support for PyKDL --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 40 +++++++++++++++ python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 40 +++++++++++++++ python_orocos_kdl/PyKDL/sip/frames.sip | 50 +++++++++++++++++++ python_orocos_kdl/PyKDL/sip/framevel.sip | 49 ++++++++++++++++++ 4 files changed, 179 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 2763af56f..09ce6dd68 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -80,6 +80,14 @@ void init_frames(py::module &m) { return operator-(a); }, py::is_operator()); + vector.def("__copy__", [](const Vector& self) + { + return Vector(self); + }); + vector.def("__deepcopy__", [](const Vector& self, py::dict) + { + return Vector(self); + }, py::arg("memo")); vector.def_static("Zero", &Vector::Zero); vector.def("Norm", static_cast(&Vector::Norm)); vector.def("Norm", static_cast(&Vector::Norm), py::arg("eps")); @@ -136,6 +144,14 @@ void init_frames(py::module &m) oss << t; return oss.str(); }); + wrench.def("__copy__", [](const Wrench& self) + { + return Wrench(self); + }); + wrench.def("__deepcopy__", [](const Wrench& self, py::dict) + { + return Wrench(self); + }, py::arg("memo")); wrench.def_static("Zero", &Wrench::Zero); wrench.def("ReverseSign", &Wrench::ReverseSign); wrench.def("RefPoint", &Wrench::RefPoint); @@ -202,6 +218,14 @@ void init_frames(py::module &m) oss << t; return oss.str(); }); + twist.def("__copy__", [](const Twist& self) + { + return Twist(self); + }); + twist.def("__deepcopy__", [](const Twist& self, py::dict) + { + return Twist(self); + }, py::arg("memo")); twist.def_static("Zero", &Twist::Zero); twist.def("ReverseSign", &Twist::ReverseSign); twist.def("RefPoint", &Twist::RefPoint); @@ -273,6 +297,14 @@ void init_frames(py::module &m) oss << r; return oss.str(); }); + rotation.def("__copy__", [](const Rotation& self) + { + return Rotation(self); + }); + rotation.def("__deepcopy__", [](const Rotation& self, py::dict) + { + return Rotation(self); + }, py::arg("memo")); rotation.def("SetInverse", &Rotation::SetInverse); rotation.def("Inverse", (Rotation (Rotation::*)(void) const) &Rotation::Inverse); rotation.def("Inverse", (Vector (Rotation::*)(const Vector&) const) &Rotation::Inverse); @@ -393,6 +425,14 @@ void init_frames(py::module &m) oss << frm; return oss.str(); }); + frame.def("__copy__", [](const Frame& self) + { + return Frame(self); + }); + frame.def("__deepcopy__", [](const Frame& self, py::dict) + { + return Frame(self); + }, py::arg("memo")); frame.def("DH_Craig1989", &Frame::DH_Craig1989); frame.def("DH", &Frame::DH); frame.def("Inverse", (Frame (Frame::*)() const) &Frame::Inverse); diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index 43842136d..635b4512f 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -50,6 +50,14 @@ void init_framevel(pybind11::module &m) oss << d; return oss.str(); }); + double_vel.def("__copy__", [](const doubleVel& self) + { + return doubleVel(self); + }); + double_vel.def("__deepcopy__", [](const doubleVel& self, py::dict) + { + return doubleVel(self); + }, py::arg("memo")); double_vel.def(py::self == py::self); double_vel.def(py::self != py::self); @@ -85,6 +93,14 @@ void init_framevel(pybind11::module &m) oss << vv; return oss.str(); }); + vector_vel.def("__copy__", [](const VectorVel& self) + { + return VectorVel(self); + }); + vector_vel.def("__deepcopy__", [](const VectorVel& self, py::dict) + { + return VectorVel(self); + }, py::arg("memo")); vector_vel.def_static("Zero", &VectorVel::Zero); vector_vel.def("ReverseSign", &VectorVel::ReverseSign); vector_vel.def("Norm", &VectorVel::Norm, py::arg("eps")=epsilon); @@ -167,6 +183,14 @@ void init_framevel(pybind11::module &m) oss << tv; return oss.str(); }); + twist_vel.def("__copy__", [](const TwistVel& self) + { + return TwistVel(self); + }); + twist_vel.def("__deepcopy__", [](const TwistVel& self, py::dict) + { + return TwistVel(self); + }, py::arg("memo")); twist_vel.def_static("Zero", &TwistVel::Zero); twist_vel.def("ReverseSign", &TwistVel::ReverseSign); twist_vel.def("RefPoint", &TwistVel::RefPoint); @@ -239,6 +263,14 @@ void init_framevel(pybind11::module &m) oss << rv; return oss.str(); }); + rotation_vel.def("__copy__", [](const RotationVel& self) + { + return RotationVel(self); + }); + rotation_vel.def("__deepcopy__", [](const RotationVel& self, py::dict) + { + return RotationVel(self); + }, py::arg("memo")); rotation_vel.def("UnitX", &RotationVel::UnitX); rotation_vel.def("UnitY", &RotationVel::UnitY); rotation_vel.def("UnitZ", &RotationVel::UnitZ); @@ -315,6 +347,14 @@ void init_framevel(pybind11::module &m) oss << fv; return oss.str(); }); + frame_vel.def("__copy__", [](const FrameVel& self) + { + return FrameVel(self); + }); + frame_vel.def("__deepcopy__", [](const FrameVel& self, py::dict) + { + return FrameVel(self); + }, py::arg("memo")); frame_vel.def_static("Identity", &FrameVel::Identity); frame_vel.def("Inverse", (FrameVel (FrameVel::*)() const) &FrameVel::Inverse); frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const VectorVel&) const) &FrameVel::Inverse); diff --git a/python_orocos_kdl/PyKDL/sip/frames.sip b/python_orocos_kdl/PyKDL/sip/frames.sip index d0be8ba58..96feacf75 100644 --- a/python_orocos_kdl/PyKDL/sip/frames.sip +++ b/python_orocos_kdl/PyKDL/sip/frames.sip @@ -72,6 +72,16 @@ public: sipRes=&s; %End + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new Vector(*sipCpp), sipFindType("Vector"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new Vector(*sipCpp), sipFindType("Vector"), NULL); +%End + void ReverseSign(); Vector& operator-=(const Vector& arg); @@ -150,6 +160,16 @@ public: sipRes=&s; %End + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new Rotation(*sipCpp), sipFindType("Rotation"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new Rotation(*sipCpp), sipFindType("Rotation"), NULL); +%End + void SetInverse(); Rotation Inverse() const /Factory/; Vector Inverse(const Vector& v) const /Factory/; @@ -258,6 +278,16 @@ public: sipRes=&s; %End + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new Frame(*sipCpp), sipFindType("Frame"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new Frame(*sipCpp), sipFindType("Frame"), NULL); +%End + Frame DH_Craig1989(double a,double alpha,double d,double theta); Frame DH(double a,double alpha,double d,double theta); Frame Inverse()/Factory/; @@ -334,6 +364,16 @@ public: sipRes=&s; %End + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new Twist(*sipCpp), sipFindType("Twist"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new Twist(*sipCpp), sipFindType("Twist"), NULL); +%End + static Twist Zero() /Factory/; void ReverseSign(); @@ -411,6 +451,16 @@ public: sipRes=&s; %End + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new Wrench(*sipCpp), sipFindType("Wrench"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new Wrench(*sipCpp), sipFindType("Wrench"), NULL); +%End + static Wrench Zero() /Factory/; void ReverseSign(); Wrench RefPoint(const Vector& v_base_AB) const /Factory/; diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 38c561203..b435e3f7a 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -37,6 +37,7 @@ public: double grad; double value(); double deriv(); + const std::string* __repr__() const; %MethodCode std::ostringstream oss; @@ -44,6 +45,16 @@ public: std::string s(oss.str()); sipRes=&s; %End + + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new doubleVel(*sipCpp), sipFindType("doubleVel"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new doubleVel(*sipCpp), sipFindType("doubleVel"), NULL); +%End }; doubleVel operator - (const doubleVel& r); @@ -82,6 +93,16 @@ public: sipRes=&s; %End + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new VectorVel(*sipCpp), sipFindType("VectorVel"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new VectorVel(*sipCpp), sipFindType("VectorVel"), NULL); +%End + VectorVel& operator += (const VectorVel& arg); VectorVel& operator -= (const VectorVel& arg); static VectorVel Zero(); @@ -157,6 +178,15 @@ public: sipRes=&s; %End + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new RotationVel(*sipCpp), sipFindType("RotationVel"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new RotationVel(*sipCpp), sipFindType("RotationVel"), NULL); +%End VectorVel UnitX() const; VectorVel UnitY() const; @@ -232,6 +262,15 @@ public: sipRes=&s; %End + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new FrameVel(*sipCpp), sipFindType("FrameVel"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new FrameVel(*sipCpp), sipFindType("FrameVel"), NULL); +%End static FrameVel Identity(); FrameVel Inverse() const; @@ -297,6 +336,16 @@ public: sipRes=&s; %End + SIP_PYOBJECT __copy__() const; +%MethodCode + sipRes = sipConvertFromNewType(new TwistVel(*sipCpp), sipFindType("TwistVel"), NULL); +%End + + SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const; +%MethodCode + sipRes = sipConvertFromNewType(new TwistVel(*sipCpp), sipFindType("TwistVel"), NULL); +%End + TwistVel& operator-=(const TwistVel& arg); TwistVel& operator+=(const TwistVel& arg); From 3e644811511511d3f88c8cdfc85f5b22ee17d5fa Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 30 Jun 2020 20:58:19 +0200 Subject: [PATCH 62/79] Merge pull request #262 from orocos/fix/is_approx Don't use isApprox, but isZero --- orocos_kdl/src/jacobian.cpp | 2 +- orocos_kdl/src/jntarray.cpp | 2 +- orocos_kdl/src/jntspaceinertiamatrix.cpp | 2 +- orocos_kdl/tests/framestest.cpp | 89 ++++++++++++++---------- orocos_kdl/tests/inertiatest.cpp | 77 ++++++++++---------- orocos_kdl/tests/jacobiantest.cpp | 14 +++- orocos_kdl/tests/jacobiantest.hpp | 4 +- 7 files changed, 107 insertions(+), 83 deletions(-) diff --git a/orocos_kdl/src/jacobian.cpp b/orocos_kdl/src/jacobian.cpp index 7b99f23a7..8c4259321 100644 --- a/orocos_kdl/src/jacobian.cpp +++ b/orocos_kdl/src/jacobian.cpp @@ -138,7 +138,7 @@ namespace KDL bool Equal(const Jacobian& a,const Jacobian& b,double eps) { if(a.rows()==b.rows()&&a.columns()==b.columns()){ - return a.data.isApprox(b.data,eps); + return (a.data-b.data).isZero(eps); }else return false; } diff --git a/orocos_kdl/src/jntarray.cpp b/orocos_kdl/src/jntarray.cpp index b26e9bbb5..daa28a40e 100644 --- a/orocos_kdl/src/jntarray.cpp +++ b/orocos_kdl/src/jntarray.cpp @@ -114,7 +114,7 @@ namespace KDL { if(src1.rows()!=src2.rows()) return false; - return src1.data.isApprox(src2.data,eps); + return (src1.data-src2.data).isZero(eps); } bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);} diff --git a/orocos_kdl/src/jntspaceinertiamatrix.cpp b/orocos_kdl/src/jntspaceinertiamatrix.cpp index dea5cb646..3103c919f 100644 --- a/orocos_kdl/src/jntspaceinertiamatrix.cpp +++ b/orocos_kdl/src/jntspaceinertiamatrix.cpp @@ -112,7 +112,7 @@ namespace KDL { if(src1.rows()!=src2.rows()||src1.columns()!=src2.columns()) return false; - return src1.data.isApprox(src2.data,eps); + return (src1.data-src2.data).isZero(eps); } bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}; diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index bb8c0efb0..b1905cdfc 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -588,50 +588,63 @@ void FramesTest::TestFrame() { CPPUNIT_ASSERT_EQUAL(F.Inverse()*v,F.Inverse(v)); } +JntArray CreateRandomJntArray(int size) +{ + JntArray j(size); + for (int i = 0; i(I0.data).isZero()); RotationalInertia I1; CPPUNIT_ASSERT(Map(I1.data).isZero()); - CPPUNIT_ASSERT(Map(I0.data).isApprox(Map(I1.data))); + CPPUNIT_ASSERT((Map(I0.data)-Map(I1.data)).isZero()); RotationalInertia I2(1,2,3,4,5,6); //Check if copying works fine RotationalInertia I3=I2; - CPPUNIT_ASSERT(Map(I3.data).isApprox(Map(I2.data))); + CPPUNIT_ASSERT((Map(I3.data)-Map(I2.data)).isZero()); I2.data[0]=0.0; - CPPUNIT_ASSERT(!Map(I3.data).isApprox(Map(I2.data))); + CPPUNIT_ASSERT(!(Map(I3.data)-Map(I2.data)).isZero()); //Check if addition and multiplication works fine Map(I0.data).setRandom(); @@ -42,15 +42,14 @@ void InertiaTest::TestRotationalInertia() { CPPUNIT_ASSERT(Map(I1.data).isZero()); //Check if matrix is symmetric - CPPUNIT_ASSERT(Map(I2.data).isApprox(Map(I2.data).transpose())); + CPPUNIT_ASSERT((Map(I2.data)-Map(I2.data).transpose()).isZero()); //Check if angular momentum is correctly calculated: Vector omega; random(omega); Vector momentum=I2*omega; - - CPPUNIT_ASSERT(Map(momentum.data).isApprox(Map(I2.data)*Map(omega.data))); + CPPUNIT_ASSERT((Map(momentum.data)-(Map(I2.data)*Map(omega.data))).isZero()); } void InertiaTest::TestRigidBodyInertia() { @@ -68,13 +67,13 @@ void InertiaTest::TestRigidBodyInertia() { CPPUNIT_ASSERT_EQUAL(I2.getMass(),mass); CPPUNIT_ASSERT(!Map(I2.getRotationalInertia().data).isZero()); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),c); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(Ic.data)-mass*(Map(c.data)*Map(c.data).transpose()-(Map(c.data).dot(Map(c.data))*Matrix3d::Identity())))); + CPPUNIT_ASSERT((Map(I2.getRotationalInertia().data)-(Map(Ic.data)-mass*(Map(c.data)*Map(c.data).transpose()-(Map(c.data).dot(Map(c.data))*Matrix3d::Identity())))).isZero()); RigidBodyInertia I3=I2; //check if copying works fine CPPUNIT_ASSERT_EQUAL(I2.getMass(),I3.getMass()); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I3.getCOG()); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(I3.getRotationalInertia().data))); + CPPUNIT_ASSERT((Map(I2.getRotationalInertia().data)-Map(I3.getRotationalInertia().data)).isZero()); //Check if multiplication and addition works fine RigidBodyInertia I4=-2*I2 +I3+I3; CPPUNIT_ASSERT_DOUBLES_EQUAL(I4.getMass(),0.0,epsilon); @@ -90,13 +89,13 @@ void InertiaTest::TestRigidBodyInertia() { I4 = R.Inverse()*I3; CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(), epsilon); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Map(I2.getRotationalInertia().data)-Map(I4.getRotationalInertia().data)).isZero()); //rotation and total with p=0 Frame T(R); I4 = T*I2; CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(), epsilon); CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I3.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Map(I3.getRotationalInertia().data)-Map(I4.getRotationalInertia().data)).isZero()); //Check only transformation Vector p; @@ -105,12 +104,12 @@ void InertiaTest::TestRigidBodyInertia() { I4 = I3.RefPoint(-p); CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Map(I2.getRotationalInertia().data)-Map(I4.getRotationalInertia().data)).isZero()); T=Frame(-p); I4 = T*I2; CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(),epsilon); CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I3.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Map(I3.getRotationalInertia().data)-Map(I4.getRotationalInertia().data)).isZero()); random(T); I3=T*I2; @@ -126,7 +125,7 @@ void InertiaTest::TestRigidBodyInertia() { I4 = T.Inverse()*I3; CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon); CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG()); - CPPUNIT_ASSERT(Map(I2.getRotationalInertia().data).isApprox(Map(I4.getRotationalInertia().data))); + CPPUNIT_ASSERT((Map(I2.getRotationalInertia().data)-Map(I4.getRotationalInertia().data)).isZero()); } void InertiaTest::TestArticulatedBodyInertia() { @@ -150,18 +149,18 @@ void InertiaTest::TestArticulatedBodyInertia() { CPPUNIT_ASSERT_EQUAL(I2.M,(Matrix3d::Identity()*mass).eval()); CPPUNIT_ASSERT(!I2.I.isZero()); - //CPPUNIT_ASSERT(I2.I.isApprox(Map(Ic.data)-mass*(Map(c.data)*Map(c.data).transpose()-(Map(c.data).dot(Map(c.data))*Matrix3d::Identity())),epsilon)); - //CPPUNIT_ASSERT(I2.H.isApprox(Map(c.data)*Map(c.data).transpose()-(Map(c.data).dot(Map(c.data))*Matrix3d::Identity()),epsilon)); +// CPPUNIT_ASSERT((I2.I-(Map(Ic.data)-mass*(Map(c.data)*Map(c.data).transpose()-(Map(c.data).dot(Map(c.data))*Matrix3d::Identity())))).isZero()); +// CPPUNIT_ASSERT((I2.H-(Map(c.data)*Map(c.data).transpose()-(Map(c.data).dot(Map(c.data))*Matrix3d::Identity()))).isZero()); ArticulatedBodyInertia I3=I2; //check if copying works fine - CPPUNIT_ASSERT(I2.M.isApprox(I3.M)); - CPPUNIT_ASSERT(I2.H.isApprox(I3.H)); - CPPUNIT_ASSERT(I2.I.isApprox(I3.I)); + CPPUNIT_ASSERT((I2.M-I3.M).isZero()); + CPPUNIT_ASSERT((I2.H-I3.H).isZero()); + CPPUNIT_ASSERT((I2.I-I3.I).isZero()); //Check if multiplication and addition works fine ArticulatedBodyInertia I4=-2*I2 +I3+I3; - CPPUNIT_ASSERT(I4.M.isApprox(Matrix3d::Zero().eval())); - CPPUNIT_ASSERT(I4.H.isApprox(Matrix3d::Zero().eval())); - CPPUNIT_ASSERT(I4.I.isApprox(Matrix3d::Zero().eval())); + CPPUNIT_ASSERT(I4.M.isZero()); + CPPUNIT_ASSERT(I4.H.isZero()); + CPPUNIT_ASSERT(I4.I.isZero()); //Check if transformations work fine //Check only rotation transformation @@ -171,36 +170,36 @@ void InertiaTest::TestArticulatedBodyInertia() { I3 = R*I2; Map E(R.data); Matrix3d tmp = E.transpose()*I2.M*E; - CPPUNIT_ASSERT(I3.M.isApprox(tmp)); + CPPUNIT_ASSERT((I3.M-tmp).isZero()); tmp = E.transpose()*I2.H*E; - CPPUNIT_ASSERT(I3.H.isApprox(tmp)); + CPPUNIT_ASSERT((I3.H-tmp).isZero()); tmp = E.transpose()*I2.I*E; - CPPUNIT_ASSERT(I3.I.isApprox(tmp)); + CPPUNIT_ASSERT((I3.I-tmp).isZero()); I4 = R.Inverse()*I3; - CPPUNIT_ASSERT(I2.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I2.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I2.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I2.M-I4.M).isZero()); + CPPUNIT_ASSERT((I2.H-I4.H).isZero()); + CPPUNIT_ASSERT((I2.I-I4.I).isZero()); //rotation and total with p=0 Frame T(R); I4 = T*I2; - CPPUNIT_ASSERT(I3.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I3.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I3.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I3.M-I4.M).isZero()); + CPPUNIT_ASSERT((I3.H-I4.H).isZero()); + CPPUNIT_ASSERT((I3.I-I4.I).isZero()); //Check only transformation Vector p; random(p); I3 = I2.RefPoint(p); I4 = I3.RefPoint(-p); - CPPUNIT_ASSERT(I2.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I2.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I2.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I2.M-I4.M).isZero()); + CPPUNIT_ASSERT((I2.H-I4.H).isZero()); + CPPUNIT_ASSERT((I2.I-I4.I).isZero()); T=Frame(-p); I4 = T*I2; - CPPUNIT_ASSERT(I3.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I3.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I3.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I3.M-I4.M).isZero()); + CPPUNIT_ASSERT((I3.H-I4.H).isZero()); + CPPUNIT_ASSERT((I3.I-I4.I).isZero()); random(T); @@ -215,7 +214,7 @@ void InertiaTest::TestArticulatedBodyInertia() { random(T); I3 = T*I2; I4 = T.Inverse()*I3; - CPPUNIT_ASSERT(I2.M.isApprox(I4.M)); - CPPUNIT_ASSERT(I2.H.isApprox(I4.H)); - CPPUNIT_ASSERT(I2.I.isApprox(I4.I)); + CPPUNIT_ASSERT((I2.M-I4.M).isZero()); + CPPUNIT_ASSERT((I2.H-I4.H).isZero()); + CPPUNIT_ASSERT((I2.I-I4.I).isZero()); } diff --git a/orocos_kdl/tests/jacobiantest.cpp b/orocos_kdl/tests/jacobiantest.cpp index bf58d38d2..a7b792181 100644 --- a/orocos_kdl/tests/jacobiantest.cpp +++ b/orocos_kdl/tests/jacobiantest.cpp @@ -92,6 +92,18 @@ void JacobianTest::TestConstructor(){ CPPUNIT_ASSERT_EQUAL(j2.columns(),(unsigned int)5); } -void JacobianTest::TestGetSetColumn(){} +void JacobianTest::TestEqual(){ + Jacobian j1(1); + Jacobian j2(j1); + for (unsigned int i=0; i<6; ++i) + { + random(j1(i,0)); + j1(i,0) = j1(i,0)*1e-7; + } + CPPUNIT_ASSERT(Equal(j1,j2,1)); + CPPUNIT_ASSERT(Equal(j1,j2,1e-6)); + CPPUNIT_ASSERT(!Equal(j1,j2,1e-8)); + +} diff --git a/orocos_kdl/tests/jacobiantest.hpp b/orocos_kdl/tests/jacobiantest.hpp index f1f9be5fa..5fc9a7fdd 100644 --- a/orocos_kdl/tests/jacobiantest.hpp +++ b/orocos_kdl/tests/jacobiantest.hpp @@ -13,7 +13,7 @@ class JacobianTest : public CppUnit::TestFixture CPPUNIT_TEST(TestChangeRefFrame); CPPUNIT_TEST(TestChangeBase); CPPUNIT_TEST(TestConstructor); - CPPUNIT_TEST(TestGetSetColumn); + CPPUNIT_TEST(TestEqual); CPPUNIT_TEST_SUITE_END(); public: @@ -24,7 +24,7 @@ class JacobianTest : public CppUnit::TestFixture void TestChangeRefFrame(); void TestChangeBase(); void TestConstructor(); - void TestGetSetColumn(); + void TestEqual(); }; #endif From cf1c88996900b8296878b6e76ec795add559641f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 15 Jun 2020 09:48:35 +0200 Subject: [PATCH 63/79] Merge pull request #261 from orocos/fix/207 (kdl) (ChainDynParam) public inherit from SolverI --- orocos_kdl/src/chaindynparam.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/chaindynparam.hpp b/orocos_kdl/src/chaindynparam.hpp index 964c213ae..2641ab1af 100644 --- a/orocos_kdl/src/chaindynparam.hpp +++ b/orocos_kdl/src/chaindynparam.hpp @@ -44,7 +44,7 @@ namespace KDL { * (expressed in the segments reference frame) and the dynamical * parameters of the segments. */ - class ChainDynParam : SolverI + class ChainDynParam : public SolverI { public: ChainDynParam(const Chain& chain, Vector _grav); From fa195f608483f91c56016a988ad04115903c306c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 30 Jun 2020 20:58:19 +0200 Subject: [PATCH 64/79] Merge pull request #262 from orocos/fix/is_approx Don't use isApprox, but isZero From 9782c0791c43aa9e7e4ded318288a80289fc3149 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Sat, 5 Feb 2022 01:48:41 +0100 Subject: [PATCH 65/79] remove .travis.yml --- .travis.yml | 64 ----------------------------------------------------- 1 file changed, 64 deletions(-) delete mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index dad4a432c..000000000 --- a/.travis.yml +++ /dev/null @@ -1,64 +0,0 @@ -language: cpp -os: linux -dist: xenial - -compiler: - - gcc - - clang - -env: - - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=OFF - - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=ON - - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=OFF - - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=ON - -before_script: - - sudo apt-get -qq update - - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python3-sip-dev python-psutil python3-psutil python-future python3-future - # build orocos_kdl - - cd orocos_kdl - - mkdir build - - cd build - - cmake -DENABLE_TESTS:BOOL=ON -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. - # compile and install orocos_kdl - - make - - sudo make install - - cd ../.. - - # build python bindings python 2 - - cd python_orocos_kdl - - mkdir build2 - - cd build2 - - export ROS_PYTHON_VERSION=2 - - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} -DBUILD_PYKDL_PYBIND11=${BUILD_PYKDL_PYBIND11} ./.. - # compile and install python bindings python 2 - - make - - sudo make install - - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib - - sudo ldconfig - - cd ../.. - - # build python bindings python 3 - - cd python_orocos_kdl - - mkdir build3 - - cd build3 - - export ROS_PYTHON_VERSION=3 - - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} -DBUILD_PYKDL_PYBIND11=${BUILD_PYKDL_PYBIND11} ./.. - # compile and install python bindings python 3 - - make - - sudo make install - - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib - - sudo ldconfig - - cd ../.. - -script: - # execute orocos_kdl tests - - cd orocos_kdl/build - - make check - - cd ../.. - # execute python bindings tests - - cd python_orocos_kdl - # python 2 - - python2 tests/PyKDLtest.py - # python 3 - - python3 tests/PyKDLtest.py From a7256277e7f8c259e1b4729c71667d94e276ade3 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Sun, 6 Feb 2022 15:49:23 +0100 Subject: [PATCH 66/79] CI: fix dependencies for python2 tests The tests for python2 require modules from psutil and future packages. This patch fixes those dependencies for CI. Additionally, the main test file imports sys package which is also needed. --- .github/workflows/main.yml | 1 + python_orocos_kdl/tests/PyKDLtest.py | 1 + 2 files changed, 2 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index fac326a8d..1abee5000 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -24,6 +24,7 @@ jobs: - name: Install run: | sudo apt-get install libeigen3-dev libcppunit-dev python-sip-dev + sudo apt-get install python-psutil python-future - name: Build orocos_kdl run: | cd orocos_kdl diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index 06037d137..47a1595c0 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -26,6 +26,7 @@ import kinfamtest import framestest import frameveltest +import sys suite = unittest.TestSuite() suite.addTest(dynamicstest.suite()) From 74d14150558cff972a276a52a1eb02fb7f58004c Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Tue, 8 Feb 2022 12:59:48 +0100 Subject: [PATCH 67/79] CI: add Python3 tests After the support of Python3, this patch adds CI testing for the python_orocos_kdl pacakge. --- .github/workflows/main.yml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 1abee5000..fc10f2534 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -42,6 +42,15 @@ jobs: cmake -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. make sudo make install + - name: Build PyKDL Python 3 + run: | + cd python_orocos_kdl + mkdir build3 + cd build3 + export ROS_PYTHON_VERSION=3 + cmake -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. + make + sudo make install - name: Update LD_LIBRARY_PATH run: | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib @@ -54,6 +63,10 @@ jobs: run: | cd python_orocos_kdl python2 tests/PyKDLtest.py + - name: Test PyKDL Python 3 + run: | + cd python_orocos_kdl + python3 tests/PyKDLtest.py industrial_ci: name: Industrial CI - ${{ matrix.env.ROS_DISTRO }} From a5b8d2884a089a16c85ac41c37981c9280167761 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Tue, 8 Feb 2022 13:14:44 +0100 Subject: [PATCH 68/79] CI: add dependency python3-psutil --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index fc10f2534..21996d174 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -24,7 +24,7 @@ jobs: - name: Install run: | sudo apt-get install libeigen3-dev libcppunit-dev python-sip-dev - sudo apt-get install python-psutil python-future + sudo apt-get install python-psutil python-future python3-psutil - name: Build orocos_kdl run: | cd orocos_kdl From 6352bfb670bad8c9a4b4acb858e06d5586e08bd9 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Tue, 8 Feb 2022 14:01:06 +0100 Subject: [PATCH 69/79] CI: adjust the tolerance of some Python tests Some tests fail because the tolerance of the machine precision is too tight. This patch gives some tolerance to those tests. --- python_orocos_kdl/tests/kinfamtest.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 7f90c1e7d..5db21d2b9 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -200,7 +200,7 @@ def testFkVelAndIkVel(self): self.assertTrue(0 == self.fksolvervel.JntToCart(qvel, cart)) self.assertTrue(0 == self.iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved)) - self.assertEqual(qvel.qdot, qdot_solved) + self.assertTrue(Equal(qvel.qdot, qdot_solved, 1e-4)) def testFkPosAndIkPos(self): q = JntArray(self.chain.getNrOfJoints()) @@ -221,7 +221,7 @@ def testFkPosAndIkPos(self): self.assertTrue(0 == self.fksolverpos.JntToCart(q_solved, F2)) self.assertEqual(F1, F2) - self.assertEqual(q, q_solved) + self.assertTrue(Equal(q, q_solved, 1e-4)) class KinfamTestTree(unittest.TestCase): From eb79a5ec516334f7f86ee6bf4c4711e54372d751 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Thu, 10 Feb 2022 18:54:46 +0100 Subject: [PATCH 70/79] abi_compatibility: add a symbol to restore alignment This patch adds a symbol that is no longer used with the purpose of restore the alginment of the class ChainIkSolverVel_pinv_givens --- orocos_kdl/src/chainiksolvervel_pinv_givens.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/chainiksolvervel_pinv_givens.hpp b/orocos_kdl/src/chainiksolvervel_pinv_givens.hpp index d6967e42b..cbe3f15cd 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv_givens.hpp +++ b/orocos_kdl/src/chainiksolvervel_pinv_givens.hpp @@ -53,7 +53,7 @@ namespace KDL bool transpose,toggle; unsigned int m,n; MatrixXd jac_eigen,U,V,B; - VectorXd S,tempi,UY,SUY,qdot_eigen,v_in_eigen; + VectorXd S,tempi,tempj,UY,SUY,qdot_eigen,v_in_eigen; }; } #endif From cd0936d281776a212fdcdbd3da56d9fe1cd47400 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 26 Jun 2020 20:56:18 +0200 Subject: [PATCH 71/79] Merge pull request #270 from jspricke/private_operator= Declare assignment operator private in SIP, needed for SIP>=4.19.23 --- python_orocos_kdl/PyKDL/sip/dynamics.sip | 3 ++ python_orocos_kdl/PyKDL/sip/kinfam.sip | 49 ++++++++++++++++++++++++ 2 files changed, 52 insertions(+) diff --git a/python_orocos_kdl/PyKDL/sip/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip index c5efb5b7b..2f2e4c2e1 100644 --- a/python_orocos_kdl/PyKDL/sip/dynamics.sip +++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip @@ -93,4 +93,7 @@ public: int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis); int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H); int JntToGravity(const JntArray &q,JntArray &gravity); + +private: + ChainDynParam& operator=(const ChainDynParam&); }; diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index 3d681f982..0302a09a9 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -409,6 +409,9 @@ public: // Argument by reference doesn't work for container types // virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1); virtual void updateInternalDataStructures(); + +private: + ChainFkSolverPos_recursive& operator=(const ChainFkSolverPos_recursive&); }; class ChainFkSolverVel_recursive : ChainFkSolverVel @@ -423,6 +426,9 @@ public: // Argument by reference doesn't work for container types // virtual int JntToCart(const JntArrayVel& q_in, std::vector& out, int segmentNr=-1 ); virtual void updateInternalDataStructures(); + +private: + ChainFkSolverVel_recursive& operator=(const ChainFkSolverVel_recursive&); }; class ChainIkSolverPos : SolverI { @@ -458,6 +464,9 @@ public: virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); virtual void updateInternalDataStructures(); + +private: + ChainIkSolverPos_NR& operator=(const ChainIkSolverPos_NR&); }; class ChainIkSolverPos_NR_JL : ChainIkSolverPos @@ -473,6 +482,9 @@ public: virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); virtual void updateInternalDataStructures(); + +private: + ChainIkSolverPos_NR_JL& operator=(const ChainIkSolverPos_NR_JL&); }; class ChainIkSolverVel_pinv : ChainIkSolverVel @@ -486,6 +498,9 @@ public: virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); virtual void updateInternalDataStructures(); + +private: + ChainIkSolverVel_pinv& operator=(const ChainIkSolverVel_pinv&); }; class ChainIkSolverVel_wdls : ChainIkSolverVel @@ -595,6 +610,8 @@ public: void setLambda(const double& lambda); +private: + ChainIkSolverVel_wdls& operator=(const ChainIkSolverVel_wdls&); }; @@ -609,6 +626,9 @@ public: virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); virtual void updateInternalDataStructures(); + +private: + ChainIkSolverPos_LMA& operator=(const ChainIkSolverPos_LMA&); }; @@ -635,6 +655,9 @@ public: const JntArray& getOptPos()const /Factory/; const double& getAlpha()const /Factory/; + +private: + ChainIkSolverVel_pinv_nso& operator=(const ChainIkSolverVel_pinv_nso&); }; class ChainIkSolverVel_pinv_givens : ChainIkSolverVel @@ -648,6 +671,9 @@ public: virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); virtual void updateInternalDataStructures(); + +private: + ChainIkSolverVel_pinv_givens& operator=(const ChainIkSolverVel_pinv_givens&); }; class ChainJntToJacSolver : SolverI @@ -660,6 +686,9 @@ public: ChainJntToJacSolver(const Chain& chain); int JntToJac(const JntArray& q_in,Jacobian& jac, int seg_nr=-1); virtual void updateInternalDataStructures(); + +private: + ChainJntToJacSolver& operator=(const ChainJntToJacSolver&); }; class ChainJntToJacDotSolver : SolverI @@ -672,6 +701,23 @@ public: ChainJntToJacDotSolver(const Chain& chain); int JntToJacDot(const JntArrayVel& q_in,Jacobian& jac, int seg_nr=-1); virtual void updateInternalDataStructures(); + int setLockedJoints(const std::vector& locked_joints); + + static const int E_JAC_DOT_FAILED; + static const int E_JACSOLVER_FAILED; + static const int E_FKSOLVERPOS_FAILED; + + static const int HYBRID; + static const int BODYFIXED; + static const int INERTIAL; + + void setHybridRepresentation(); + void setBodyFixedRepresentation(); + void setInertialRepresentation(); + void setRepresentation(const int& representation); + +private: + ChainJntToJacDotSolver& operator=(const ChainJntToJacDotSolver&); }; class ChainIdSolver : SolverI @@ -694,4 +740,7 @@ public: ChainIdSolver_RNE(const Chain& chain,Vector grav); int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector& f_ext,JntArray &torques); virtual void updateInternalDataStructures(); + +private: + ChainIdSolver_RNE& operator=(const KDL::ChainIdSolver_RNE&); }; From 8d586055082a07ebff1bec566f0e752675d7a13d Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 7 Aug 2020 23:53:55 +0200 Subject: [PATCH 72/79] Merge pull request #281 from orocos/fix/tests First step into fixing inconsistent test results --- python_orocos_kdl/tests/PyKDLtest.py | 11 ++++---- python_orocos_kdl/tests/kinfamtest.py | 40 ++++++++++++++++++++------- 2 files changed, 36 insertions(+), 15 deletions(-) diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index 47a1595c0..016cf487f 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -38,9 +38,10 @@ import jointtypetest suite.addTest(jointtypetest.suite()) -result = unittest.TextTestRunner(verbosity=3).run(suite) +if __name__ == "__main__": + result = unittest.TextTestRunner(verbosity=3).run(suite) -if result.wasSuccessful(): - sys.exit(0) -else: - sys.exit(1) + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 5db21d2b9..312f0290f 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -57,7 +57,9 @@ def setUp(self): self.fksolverpos = ChainFkSolverPos_recursive(self.chain) self.fksolvervel = ChainFkSolverVel_recursive(self.chain) self.iksolvervel = ChainIkSolverVel_pinv(self.chain) + self.iksolvervel_givens = ChainIkSolverVel_pinv_givens(self.chain) self.iksolverpos = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel) + self.iksolverpos_givens = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel_givens) def testRotationalInertia(self): ri = RotationalInertia(1, 2, 3, 4, 7, 5) @@ -183,9 +185,7 @@ def testFkVelAndJac(self): MultiplyJacobian(jac, qvel.qdot, t) self.assertEqual(cart.deriv(), t) - def testFkVelAndIkVel(self): - epsJ = 1E-7 - + def testFkVelAndIkVelImpl(self, fksolvervel, iksolvervel, epsJ): q = JntArray(self.chain.getNrOfJoints()) qdot = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): @@ -197,12 +197,20 @@ def testFkVelAndIkVel(self): cart = FrameVel() - self.assertTrue(0 == self.fksolvervel.JntToCart(qvel, cart)) - self.assertTrue(0 == self.iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved)) + self.assertTrue(0 == fksolvervel.JntToCart(qvel, cart)) + self.assertTrue(0 == iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved)) self.assertTrue(Equal(qvel.qdot, qdot_solved, 1e-4)) - def testFkPosAndIkPos(self): + def testFkVelAndIkVel(self): + epsJ = 1e-7 + self.testFkVelAndIkVelImpl(self.fksolvervel, self.iksolvervel, epsJ) + + def testFkVelAndIkVelGivens(self): + epsJ = 1e-7 + self.testFkVelAndIkVelImpl(self.fksolvervel, self.iksolvervel_givens, epsJ) + + def testFkPosAndIkPosImpl(self, fksolverpos, iksolverpos, epsJ): q = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): q[i] = random.uniform(-3.14, 3.14) @@ -216,12 +224,20 @@ def testFkPosAndIkPos(self): F1 = Frame.Identity() F2 = Frame.Identity() - self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1)) - self.assertTrue(0 == self.iksolverpos.CartToJnt(q_init, F1, q_solved)) - self.assertTrue(0 == self.fksolverpos.JntToCart(q_solved, F2)) + self.assertTrue(0 == fksolverpos.JntToCart(q, F1)) + self.assertTrue(0 == iksolverpos.CartToJnt(q_init, F1, q_solved)) + self.assertTrue(0 == fksolverpos.JntToCart(q_solved, F2)) self.assertEqual(F1, F2) - self.assertTrue(Equal(q, q_solved, 1e-4)) + self.assertTrue(Equal(q, q_solved, epsJ), "{} != {}".format(q, q_solved)) + + def testFkPosAndIkPos(self): + epsJ = 1e-3 + self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos, epsJ) + + def testFkPosAndIkPosGivens(self): + epsJ = 1e-3 + self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos_givens, epsJ) class KinfamTestTree(unittest.TestCase): @@ -256,7 +272,11 @@ def suite(): suite.addTest(KinfamTestFunctions('testFkPosAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndIkVel')) + suite.addTest(KinfamTestFunctions('testFkVelAndIkVelGivens')) suite.addTest(KinfamTestFunctions('testFkPosAndIkPos')) + suite.addTest(KinfamTestFunctions('testFkPosAndIkPosGivens')) + suite.addTest(KinfamTestFunctions('testJacDot')) + suite.addTest(KinfamTestTree('testTreeGetChainMemLeak')) return suite From bf8dc5d2905274cbaf133d7603596d6d7b381493 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Thu, 10 Feb 2022 19:38:51 +0100 Subject: [PATCH 73/79] Revert "Merge pull request #281 from orocos/fix/tests" This reverts commit 8d586055082a07ebff1bec566f0e752675d7a13d. --- python_orocos_kdl/tests/PyKDLtest.py | 11 ++++---- python_orocos_kdl/tests/kinfamtest.py | 40 +++++++-------------------- 2 files changed, 15 insertions(+), 36 deletions(-) diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index 016cf487f..47a1595c0 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -38,10 +38,9 @@ import jointtypetest suite.addTest(jointtypetest.suite()) -if __name__ == "__main__": - result = unittest.TextTestRunner(verbosity=3).run(suite) +result = unittest.TextTestRunner(verbosity=3).run(suite) - if result.wasSuccessful(): - sys.exit(0) - else: - sys.exit(1) +if result.wasSuccessful(): + sys.exit(0) +else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 312f0290f..5db21d2b9 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -57,9 +57,7 @@ def setUp(self): self.fksolverpos = ChainFkSolverPos_recursive(self.chain) self.fksolvervel = ChainFkSolverVel_recursive(self.chain) self.iksolvervel = ChainIkSolverVel_pinv(self.chain) - self.iksolvervel_givens = ChainIkSolverVel_pinv_givens(self.chain) self.iksolverpos = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel) - self.iksolverpos_givens = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel_givens) def testRotationalInertia(self): ri = RotationalInertia(1, 2, 3, 4, 7, 5) @@ -185,7 +183,9 @@ def testFkVelAndJac(self): MultiplyJacobian(jac, qvel.qdot, t) self.assertEqual(cart.deriv(), t) - def testFkVelAndIkVelImpl(self, fksolvervel, iksolvervel, epsJ): + def testFkVelAndIkVel(self): + epsJ = 1E-7 + q = JntArray(self.chain.getNrOfJoints()) qdot = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): @@ -197,20 +197,12 @@ def testFkVelAndIkVelImpl(self, fksolvervel, iksolvervel, epsJ): cart = FrameVel() - self.assertTrue(0 == fksolvervel.JntToCart(qvel, cart)) - self.assertTrue(0 == iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved)) + self.assertTrue(0 == self.fksolvervel.JntToCart(qvel, cart)) + self.assertTrue(0 == self.iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved)) self.assertTrue(Equal(qvel.qdot, qdot_solved, 1e-4)) - def testFkVelAndIkVel(self): - epsJ = 1e-7 - self.testFkVelAndIkVelImpl(self.fksolvervel, self.iksolvervel, epsJ) - - def testFkVelAndIkVelGivens(self): - epsJ = 1e-7 - self.testFkVelAndIkVelImpl(self.fksolvervel, self.iksolvervel_givens, epsJ) - - def testFkPosAndIkPosImpl(self, fksolverpos, iksolverpos, epsJ): + def testFkPosAndIkPos(self): q = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): q[i] = random.uniform(-3.14, 3.14) @@ -224,20 +216,12 @@ def testFkPosAndIkPosImpl(self, fksolverpos, iksolverpos, epsJ): F1 = Frame.Identity() F2 = Frame.Identity() - self.assertTrue(0 == fksolverpos.JntToCart(q, F1)) - self.assertTrue(0 == iksolverpos.CartToJnt(q_init, F1, q_solved)) - self.assertTrue(0 == fksolverpos.JntToCart(q_solved, F2)) + self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1)) + self.assertTrue(0 == self.iksolverpos.CartToJnt(q_init, F1, q_solved)) + self.assertTrue(0 == self.fksolverpos.JntToCart(q_solved, F2)) self.assertEqual(F1, F2) - self.assertTrue(Equal(q, q_solved, epsJ), "{} != {}".format(q, q_solved)) - - def testFkPosAndIkPos(self): - epsJ = 1e-3 - self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos, epsJ) - - def testFkPosAndIkPosGivens(self): - epsJ = 1e-3 - self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos_givens, epsJ) + self.assertTrue(Equal(q, q_solved, 1e-4)) class KinfamTestTree(unittest.TestCase): @@ -272,11 +256,7 @@ def suite(): suite.addTest(KinfamTestFunctions('testFkPosAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndIkVel')) - suite.addTest(KinfamTestFunctions('testFkVelAndIkVelGivens')) suite.addTest(KinfamTestFunctions('testFkPosAndIkPos')) - suite.addTest(KinfamTestFunctions('testFkPosAndIkPosGivens')) - suite.addTest(KinfamTestFunctions('testJacDot')) - suite.addTest(KinfamTestTree('testTreeGetChainMemLeak')) return suite From 7b5b0f5912bcf44ae016bc0fcb453313d1c9f979 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 31 Aug 2020 21:11:58 +0200 Subject: [PATCH 74/79] Merge pull request #282 from orocos/fix/m_pi Replace M_PI by custom PI --- .../examples/chainiksolverpos_lma_demo.cpp | 6 +- orocos_kdl/examples/geometry.cpp | 11 +- orocos_kdl/examples/trajectory_example.cpp | 15 +- orocos_kdl/models/puma560.cpp | 9 +- orocos_kdl/src/frames.cpp | 7 +- orocos_kdl/src/path_roundedcomposite.hpp | 2 +- orocos_kdl/src/utilities/utility.cxx | 12 +- orocos_kdl/src/utilities/utility.h | 6 + orocos_kdl/tests/framestest.cpp | 222 ++++++++++-------- orocos_kdl/tests/solvertest.cpp | 36 +-- 10 files changed, 186 insertions(+), 140 deletions(-) diff --git a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp index f2121d4d3..01f8f5d71 100644 --- a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp +++ b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp @@ -54,6 +54,8 @@ estimate of shortest time per invposkin (ms) 0.155544 #include #include #include +#include + #include /** @@ -90,9 +92,9 @@ void test_inverseposkin(KDL::Chain& chain) { JntArray q_sol(n); for (int trial=0;trial #include +#include int main() { @@ -87,18 +88,18 @@ int main() //Creating an Identity rotation matrix KDL::Rotation r4=KDL::Rotation::Identity(); //Creating a Rotation matrix from a rotation around X - KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3); + KDL::Rotation r5=KDL::Rotation::RotX(PI/3); //Creating a Rotation matrix from a rotation around Y - KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3); + KDL::Rotation r6=KDL::Rotation::RotY(PI/3); //Creating a Rotation matrix from a rotation around Z - KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3); + KDL::Rotation r7=KDL::Rotation::RotZ(PI/3); //Creating a Rotation matrix from a rotation around a arbitrary //vector, the vector should not be normalised - KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4); + KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),PI_4); //Creating a Rotation matrix from a rotation around a arbitrary //vector, the vector should be normalised KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071), - M_PI/4); + PI_4); //Creating a Rotation matrix from Euler ZYZ rotation angles KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.); //Creating a Rotation matrix from Euler ZYX rotation angles diff --git a/orocos_kdl/examples/trajectory_example.cpp b/orocos_kdl/examples/trajectory_example.cpp index 99908fe6d..da34357e2 100644 --- a/orocos_kdl/examples/trajectory_example.cpp +++ b/orocos_kdl/examples/trajectory_example.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include int main(int argc,char* argv[]) { @@ -35,13 +36,13 @@ int main(int argc,char* argv[]) { // When the routines are parallel, no rounding is needed, and no attempt is made // add constructing a rounding arc. // (It is still not possible when the segments are on top of each other) - // Note that you can only rotate in a deterministic way over an angle less then M_PI! - // With an angle == M_PI, you cannot predict over which side will be rotated. - // With an angle > M_PI, the routine will rotate over 2*M_PI-angle. + // Note that you can only rotate in a deterministic way over an angle less then PI! + // With an angle == PI, you cannot predict over which side will be rotated. + // With an angle > PI, the routine will rotate over 2*PI-angle. // If you need to rotate over a larger angle, you need to introduce intermediate points. // So, there is a common use case for using parallel segments. - path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0))); - path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0))); + path->Add(Frame(Rotation::RPY(PI,0,0), Vector(-1,0,0))); + path->Add(Frame(Rotation::RPY(PI_2,0,0), Vector(-0.5,0,0))); path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1))); path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0))); @@ -61,8 +62,6 @@ int main(int argc,char* argv[]) { ctraject->Add(traject); ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0)))); - - // use the trajectory double dt=0.1; std::ofstream of("./trajectory.dat"); @@ -113,5 +112,3 @@ int main(int argc,char* argv[]) { } } - - diff --git a/orocos_kdl/models/puma560.cpp b/orocos_kdl/models/puma560.cpp index 607442815..d98a5105c 100644 --- a/orocos_kdl/models/puma560.cpp +++ b/orocos_kdl/models/puma560.cpp @@ -21,28 +21,29 @@ #include #include "models.hpp" +#include namespace KDL{ Chain Puma560(){ Chain puma560; puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,M_PI_2,0.0,0.0), + Frame::DH(0.0,PI_2,0.0,0.0), RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.4318,0.0,0.0,0.0), RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0)))); puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0203,-M_PI_2,0.15005,0.0), + Frame::DH(0.0203,-PI_2,0.15005,0.0), RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,M_PI_2,0.4318,0.0), + Frame::DH(0.0,PI_2,0.4318,0.0), RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0)))); puma560.addSegment(Segment()); puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,-M_PI_2,0.0,0.0), + Frame::DH(0.0,-PI_2,0.0,0.0), RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.0,0.0,0.0,0.0), diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index aaede3c69..13d8aa7e8 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -26,9 +26,8 @@ ***************************************************************************/ #include "frames.hpp" +#include "utilities/utility.h" -#define _USE_MATH_DEFINES // For MSVC -#include #include namespace KDL { @@ -260,7 +259,7 @@ void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const { double epsilon=1E-12; pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); - if ( fabs(pitch) > (M_PI/2.0-epsilon) ) { + if ( fabs(pitch) > (PI_2-epsilon) ) { yaw = atan2( -data[1], data[4]); roll = 0.0 ; } else { @@ -393,7 +392,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { } // otherwise this singularity is angle = 180 - angle = M_PI; + angle = PI; double xx = (data[0] + 1) / 2; double yy = (data[4] + 1) / 2; double zz = (data[8] + 1) / 2; diff --git a/orocos_kdl/src/path_roundedcomposite.hpp b/orocos_kdl/src/path_roundedcomposite.hpp index 79c2657f5..2ccb860a0 100644 --- a/orocos_kdl/src/path_roundedcomposite.hpp +++ b/orocos_kdl/src/path_roundedcomposite.hpp @@ -96,7 +96,7 @@ class Path_RoundedComposite : public Path * - 3101 if the eq. radius <= 0 * - 3102 if the first segment in a rounding has zero length. * - 3103 if the second segment in a rounding has zero length. - * - 3104 if the angle between the first and the second segment is close to M_PI. + * - 3104 if the angle between the first and the second segment is close to PI. * (meaning that the segments are on top of each other) * - 3105 if the distance needed for the rounding is larger then the first segment. * - 3106 if the distance needed for the rounding is larger then the second segment. diff --git a/orocos_kdl/src/utilities/utility.cxx b/orocos_kdl/src/utilities/utility.cxx index ad995a047..7cc7e9663 100644 --- a/orocos_kdl/src/utilities/utility.cxx +++ b/orocos_kdl/src/utilities/utility.cxx @@ -11,10 +11,12 @@ namespace KDL { -int STREAMBUFFERSIZE=10000; +int STREAMBUFFERSIZE = 10000; int MAXLENFILENAME = 255; -const double PI= 3.1415926535897932384626433832795; -const double deg2rad = 0.01745329251994329576923690768488; -const double rad2deg = 57.2957795130823208767981548141052; -double epsilon = 0.000001; +const double PI = 3.141592653589793238462643383279502884; // PI +const double PI_2 = 1.570796326794896619231321691639751442; // PI/2 +const double PI_4 = 0.785398163397448309615660845819875721; // PI/4 +const double deg2rad = 0.017453292519943295769236907684886127; // PI/180 +const double rad2deg = 57.29577951308232087679815481410517033; // 180/PI +double epsilon = 1e-6; } diff --git a/orocos_kdl/src/utilities/utility.h b/orocos_kdl/src/utilities/utility.h index 83266ed48..651dd7660 100644 --- a/orocos_kdl/src/utilities/utility.h +++ b/orocos_kdl/src/utilities/utility.h @@ -179,6 +179,12 @@ extern int MAXLENFILENAME; //! the value of pi extern const double PI; +//! the value of pi/2 +extern const double PI_2; + +//! the value of pi/4 +extern const double PI_4; + //! the value pi/180 extern const double deg2rad; diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index b1905cdfc..1f0667d80 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -1,6 +1,7 @@ -#include #include "framestest.hpp" #include +#include + CPPUNIT_TEST_SUITE_REGISTRATION( FramesTest ); using namespace KDL; @@ -237,37 +238,37 @@ void FramesTest::TestEuler() { TESTEULERZYX(0.1,0,0) TESTEULERZYX(0,0,0.3) TESTEULERZYX(0,0,0) - TESTEULERZYX(0.3,0.999*M_PI/2,0.1) - // if beta== +/- M_PI/2 => multiple solutions available, gamma will be chosen to be zero ! + TESTEULERZYX(0.3,0.999*PI_2,0.1) + // if beta== +/- PI/2 => multiple solutions available, gamma will be chosen to be zero ! // so we test with gamma==0 ! - TESTEULERZYX(0.3,0.9999999999*M_PI/2,0) - TESTEULERZYX(0.3,0.99999999*M_PI/2,0) - TESTEULERZYX(0.3,0.999999*M_PI/2,0) - TESTEULERZYX(0.3,0.9999*M_PI/2,0) - TESTEULERZYX(0.3,0.99*M_PI/2,0) - //TESTEULERZYX(0.1,-M_PI/2,0.3) - TESTEULERZYX(0,M_PI/2,0) - - TESTEULERZYX(0.3,-M_PI/2,0) - TESTEULERZYX(0.3,-0.9999999999*M_PI/2,0) - TESTEULERZYX(0.3,-0.99999999*M_PI/2,0) - TESTEULERZYX(0.3,-0.999999*M_PI/2,0) - TESTEULERZYX(0.3,-0.9999*M_PI/2,0) - TESTEULERZYX(0.3,-0.99*M_PI/2,0) - TESTEULERZYX(0,-M_PI/2,0) + TESTEULERZYX(0.3,0.9999999999*PI_2,0) + TESTEULERZYX(0.3,0.99999999*PI_2,0) + TESTEULERZYX(0.3,0.999999*PI_2,0) + TESTEULERZYX(0.3,0.9999*PI_2,0) + TESTEULERZYX(0.3,0.99*PI_2,0) + //TESTEULERZYX(0.1,-PI_2,0.3) + TESTEULERZYX(0,PI_2,0) + + TESTEULERZYX(0.3,-PI_2,0) + TESTEULERZYX(0.3,-0.9999999999*PI_2,0) + TESTEULERZYX(0.3,-0.99999999*PI_2,0) + TESTEULERZYX(0.3,-0.999999*PI_2,0) + TESTEULERZYX(0.3,-0.9999*PI_2,0) + TESTEULERZYX(0.3,-0.99*PI_2,0) + TESTEULERZYX(0,-PI_2,0) // extremes of the range: - TESTEULERZYX(M_PI,-M_PI/2,0) - TESTEULERZYX(-M_PI,-M_PI/2,0) - TESTEULERZYX(M_PI,1,0) - TESTEULERZYX(-M_PI,1,0) - //TESTEULERZYX(0,-M_PI/2,M_PI) gamma will be chosen zero - //TESTEULERZYX(0,M_PI/2,-M_PI) gamma will be chosen zero - TESTEULERZYX(0,1,M_PI) + TESTEULERZYX(PI,-PI_2,0) + TESTEULERZYX(-PI,-PI_2,0) + TESTEULERZYX(PI,1,0) + TESTEULERZYX(-PI,1,0) + //TESTEULERZYX(0,-PI_2,PI) gamma will be chosen zero + //TESTEULERZYX(0,PI_2,-PI) gamma will be chosen zero + TESTEULERZYX(0,1,PI) TESTEULERZYZ(0.1,0.2,0.3) TESTEULERZYZ(-0.1,0.2,0.3) - TESTEULERZYZ(0.1,0.9*M_PI,0.3) + TESTEULERZYZ(0.1,0.9*PI,0.3) TESTEULERZYZ(0.1,0.2,-0.3) TESTEULERZYZ(0,0,0) TESTEULERZYZ(0,0,0) @@ -277,22 +278,22 @@ void FramesTest::TestEuler() { TESTEULERZYZ(0.4,PI,0) TESTEULERZYZ(0,PI,0) TESTEULERZYZ(PI,PI,0) - TESTEULERZYX(0.3,M_PI/2,0) - TESTEULERZYZ(0.3,0.9999999999*M_PI/2,0) - TESTEULERZYZ(0.3,0.99999999*M_PI/2,0) - TESTEULERZYZ(0.3,0.999999*M_PI/2,0) - TESTEULERZYZ(0.3,0.9999*M_PI/2,0) - TESTEULERZYZ(0.3,0.99*M_PI/2,0) - - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3+M_PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3-M_PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3+M_PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3-M_PI); - - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3+M_PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3+M_PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3-M_PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3-M_PI); + TESTEULERZYX(0.3,PI_2,0) + TESTEULERZYZ(0.3,0.9999999999*PI_2,0) + TESTEULERZYZ(0.3,0.99999999*PI_2,0) + TESTEULERZYZ(0.3,0.999999*PI_2,0) + TESTEULERZYZ(0.3,0.9999*PI_2,0) + TESTEULERZYZ(0.3,0.99*PI_2,0) + + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+PI, PI-0.2, 0.3+PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-PI, PI-0.2, 0.3-PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-PI, PI-0.2, 0.3+PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+PI, PI-0.2, 0.3-PI); + + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+PI, -0.2, 0.3+PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-PI, -0.2, 0.3+PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+PI, -0.2, 0.3-PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-PI, -0.2, 0.3-PI); } void FramesTest::TestRangeArbitraryRotation(const std::string& msg, @@ -408,9 +409,8 @@ void FramesTest::TestRotation() { } void FramesTest::TestGetRotAngle() { - static const double pi = atan(1)*4; double roll = -2.9811968953315162; - double pitch = -pi/2; + double pitch = -PI_2; double yaw = -0.1603957582582825; // rpy -> rotation @@ -436,7 +436,7 @@ void FramesTest::TestGetRotAngle() { { Vector axis; double angle = KDL::Rotation(-1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, -1 - 1e-6).GetRotAngle(axis); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", M_PI, angle, epsilon); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", PI, angle, epsilon); } // Tests to show that GetRotAngle does not work for an improper rotation matrix which has a determinant of -1; @@ -509,53 +509,91 @@ void FramesTest::TestQuaternion() { void FramesTest::TestRotationDiff() { - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad)), Vector(M_PI,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad)), Vector(-M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad)), Vector(M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad)), Vector(M_PI,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad)), Vector(-M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad)), Vector(0,0,0)); // no rotation - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad)), Vector(0,M_PI,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad)), Vector(0,-M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad)), Vector(0,M_PI,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad)), Vector(0,-M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad)), Vector(0,0,0)); // no rotation - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad)), Vector(0,0,M_PI)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad)), Vector(0,0,-M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad)), Vector(0,0,M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad)), Vector(0,0,M_PI)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad)), Vector(0,0,-M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad)), Vector(0,0,0)); // no rotation - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad), - Rotation::RPY(-0*deg2rad,0,+90*deg2rad)), - Vector(0,0,M_PI)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad), - Rotation::RPY(-5*deg2rad,0,+0*deg2rad)), - Vector(-10*deg2rad,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(0*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(180*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad), Vector(PI,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(270*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad), Vector(-PI_2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(360*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-360*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-270*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad), Vector(PI_2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-180*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad), Vector(PI,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad), Vector(-PI_2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-0*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad), Vector(0,0,0)); // no rotation + + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(0*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(90*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(180*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad), Vector(0,PI,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(270*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad), Vector(0,-PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(360*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-360*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-270*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad), Vector(0,PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-180*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad), Vector(0,PI,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-90*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad), Vector(0,-PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-0*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad), Vector(0,0,0)); // no rotation + + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(0*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(90*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(180*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad), Vector(0,0,PI)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(270*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad), Vector(0,0,-PI_2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(360*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-360*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-270*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad), Vector(0,0,PI_2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-180*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad), Vector(0,0,PI)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-90*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad), Vector(0,0,-PI_2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-0*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad), Vector(0,0,0)); // no rotation + + TestOneRotationDiff("diff(RotX(0*deg2rad),RotZ(90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotY(90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotZ(90*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); + + TestOneRotationDiff("diff(Identity(),RotX(90*deg2rad))", + Rotation::Identity(), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0)); + TestOneRotationDiff("diff(Identity(),RotY(0*deg2rad))", + Rotation::Identity(), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); + TestOneRotationDiff("diff(Identity(),RotZ(0*deg2rad))", + Rotation::Identity(), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); + + TestOneRotationDiff("diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),Rotation::RPY(-0*deg2rad,0,+90*deg2rad))", + Rotation::RPY(+0*deg2rad,0,-90*deg2rad), + Rotation::RPY(-0*deg2rad,0,+90*deg2rad), + Vector(0,0,PI)); + TestOneRotationDiff("diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),Rotation::RPY(-5*deg2rad,0,+0*deg2rad))", + Rotation::RPY(+5*deg2rad,0,-0*deg2rad), + Rotation::RPY(-5*deg2rad,0,+0*deg2rad), + Vector(-10*deg2rad,0,0)); KDL::Rotation R1 = Rotation::RPY(+5*deg2rad,0,-90*deg2rad); CPPUNIT_ASSERT_EQUAL(KDL::diff(R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad)), diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 7864e9096..914f8a694 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -3,7 +3,7 @@ #include #include #include - +#include CPPUNIT_TEST_SUITE_REGISTRATION( SolverTest ); @@ -106,17 +106,17 @@ void SolverTest::setUp() motomansia10.addSegment(Segment(Joint(Joint::None), Frame::DH_Craig1989(0.0, 0.0, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0))); + Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0))); + Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, -PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), Frame(Rotation::Identity(),Vector(0.0,0.0,0.155)))); @@ -148,22 +148,22 @@ void SolverTest::setUp() KDL::RotationalInertia(0.01, 0.01, 0.1, 0.0, 0.0, 0.0)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), - Frame::DH(0.0, M_PI_2, 0.36, 0.0), + Frame::DH(0.0, PI_2, 0.36, 0.0), inert1)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), - Frame::DH(0.0, -M_PI_2, 0.0, 0.0), + Frame::DH(0.0, -PI_2, 0.0, 0.0), inert2)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), - Frame::DH(0.0, M_PI_2, 0.36, 0.0), + Frame::DH(0.0, PI_2, 0.36, 0.0), inert3)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), - Frame::DH(0.0, -M_PI_2, 0.0, 0.0), + Frame::DH(0.0, -PI_2, 0.0, 0.0), inert4)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), - Frame::DH(0.0, M_PI_2, 0.36, 0.0), + Frame::DH(0.0, PI_2, 0.36, 0.0), inert5)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), - Frame::DH(0.0, -M_PI_2, 0.0, 0.0), + Frame::DH(0.0, -PI_2, 0.0, 0.0), inert6)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), Frame::DH(0.0, 0.0, 0.0, 0.0), @@ -476,7 +476,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -503,7 +503,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.2 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -526,7 +526,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -595,7 +595,7 @@ void SolverTest::IkVelSolverWDLSTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -822,7 +822,7 @@ void SolverTest::VereshchaginTest() // Initial arm position configuration/constraint JntArray jointInitialPose(chaindyn.getNrOfJoints()); jointInitialPose(0) = 0.0; // initial joint0 pose - jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise + jointInitialPose(1) = PI/6.0; //initial joint1 pose, negative in clockwise //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464 //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636 From 8185f6007000c71d0267c518d367d75376fcf202 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Sun, 7 Jun 2020 12:39:26 +0200 Subject: [PATCH 75/79] Merge pull request #197 from orocos/simplify-add-delta-for-rotations Simplify implementation of addDelta() for rotations --- orocos_kdl/src/frames.inl | 2 +- orocos_kdl/tests/framestest.cpp | 145 +++++++++++++++++++++----------- orocos_kdl/tests/framestest.hpp | 4 + 3 files changed, 101 insertions(+), 50 deletions(-) diff --git a/orocos_kdl/src/frames.inl b/orocos_kdl/src/frames.inl index d2c51c431..8349ff108 100644 --- a/orocos_kdl/src/frames.inl +++ b/orocos_kdl/src/frames.inl @@ -1158,7 +1158,7 @@ IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) { } IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) { - return a*Rot(a.Inverse(da)*dt); + return Rot(da*dt)*a; } IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt) { return Frame( diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index b1905cdfc..e36b040ce 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -506,60 +506,107 @@ void FramesTest::TestQuaternion() { CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2, epsilon); } +void FramesTest::TestOneRotationDiff( + const std::string& msg, + const Rotation& R_a_b1, + const Rotation& R_a_b2, + const Vector& expectedDiff) { + CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, diff(R_a_b1, R_a_b2), expectedDiff); + CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, addDelta(R_a_b1, expectedDiff), R_a_b2); +} void FramesTest::TestRotationDiff() { - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad)), Vector(M_PI,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad)), Vector(-M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad)), Vector(M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad)), Vector(M_PI,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad)), Vector(-M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad)), Vector(0,0,0)); // no rotation - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad)), Vector(0,M_PI,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad)), Vector(0,-M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad)), Vector(0,M_PI,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad)), Vector(0,-M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad)), Vector(0,0,0)); // no rotation - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad)), Vector(0,0,M_PI)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad)), Vector(0,0,-M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad)), Vector(0,0,0)); // no rotation - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad)), Vector(0,0,M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad)), Vector(0,0,M_PI)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad)), Vector(0,0,-M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad)), Vector(0,0,0)); // no rotation - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); - - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad), - Rotation::RPY(-0*deg2rad,0,+90*deg2rad)), - Vector(0,0,M_PI)); - CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad), - Rotation::RPY(-5*deg2rad,0,+0*deg2rad)), - Vector(-10*deg2rad,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(0*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad), Vector(M_PI/2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(180*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad), Vector(M_PI,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(270*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad), Vector(-M_PI/2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(360*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-360*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-270*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad), Vector(M_PI/2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-180*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad), Vector(M_PI,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad), Vector(-M_PI/2,0,0)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-0*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad), Vector(0,0,0)); // no rotation + + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(0*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(90*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,M_PI/2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(180*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad), Vector(0,M_PI,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(270*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad), Vector(0,-M_PI/2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(360*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-360*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-270*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad), Vector(0,M_PI/2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-180*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad), Vector(0,M_PI,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-90*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad), Vector(0,-M_PI/2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-0*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad), Vector(0,0,0)); // no rotation + + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(0*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(90*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,M_PI/2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(180*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad), Vector(0,0,M_PI)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(270*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad), Vector(0,0,-M_PI/2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(360*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-360*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad), Vector(0,0,0)); // no rotation + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-270*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad), Vector(0,0,M_PI/2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-180*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad), Vector(0,0,M_PI)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-90*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad), Vector(0,0,-M_PI/2)); + TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-0*deg2rad))", + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad), Vector(0,0,0)); // no rotation + + TestOneRotationDiff("diff(RotX(0*deg2rad),RotZ(90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,M_PI/2)); + TestOneRotationDiff("diff(RotX(0*deg2rad),RotY(90*deg2rad))", + Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,M_PI/2,0)); + TestOneRotationDiff("diff(RotY(0*deg2rad),RotZ(90*deg2rad))", + Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,M_PI/2)); + + TestOneRotationDiff("diff(Identity(),RotX(90*deg2rad))", + Rotation::Identity(), Rotation::RotX(90*deg2rad), Vector(M_PI/2,0,0)); + TestOneRotationDiff("diff(Identity(),RotY(0*deg2rad))", + Rotation::Identity(), Rotation::RotY(90*deg2rad), Vector(0,M_PI/2,0)); + TestOneRotationDiff("diff(Identity(),RotZ(0*deg2rad))", + Rotation::Identity(), Rotation::RotZ(90*deg2rad), Vector(0,0,M_PI/2)); + + TestOneRotationDiff("diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),Rotation::RPY(-0*deg2rad,0,+90*deg2rad))", + Rotation::RPY(+0*deg2rad,0,-90*deg2rad), + Rotation::RPY(-0*deg2rad,0,+90*deg2rad), + Vector(0,0,M_PI)); + TestOneRotationDiff("diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),Rotation::RPY(-5*deg2rad,0,+0*deg2rad))", + Rotation::RPY(+5*deg2rad,0,-0*deg2rad), + Rotation::RPY(-5*deg2rad,0,+0*deg2rad), + Vector(-10*deg2rad,0,0)); KDL::Rotation R1 = Rotation::RPY(+5*deg2rad,0,-90*deg2rad); - CPPUNIT_ASSERT_EQUAL(KDL::diff(R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad)), - R1*Vector(0, 0, 180*deg2rad)); + TestOneRotationDiff("diff(Rotation::RPY(+5*deg2rad,0,-90*deg2rad),Rotation::RPY(-5*deg2rad,0,+90*deg2rad))", + R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad), + R1*Vector(0, 0, 180*deg2rad)); } void FramesTest::TestFrame() { diff --git a/orocos_kdl/tests/framestest.hpp b/orocos_kdl/tests/framestest.hpp index 31f9e3de0..0c3ca1531 100644 --- a/orocos_kdl/tests/framestest.hpp +++ b/orocos_kdl/tests/framestest.hpp @@ -57,6 +57,10 @@ class FramesTest : public CppUnit::TestFixture void TestRangeArbitraryRotation(const std::string& msg, const KDL::Vector& v, const KDL::Vector& expectedVector); + void TestOneRotationDiff(const std::string& msg, + const KDL::Rotation& R_a_b1, + const KDL::Rotation& R_a_b2, + const KDL::Vector& expectedDiff); }; From c3b14db636f74e422650d19a7a9ed0946336a85f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 31 Aug 2020 21:11:58 +0200 Subject: [PATCH 76/79] Merge pull request #282 from orocos/fix/m_pi Replace M_PI by custom PI --- .../examples/chainiksolverpos_lma_demo.cpp | 6 +- orocos_kdl/examples/geometry.cpp | 11 +- orocos_kdl/examples/trajectory_example.cpp | 15 +- orocos_kdl/models/puma560.cpp | 9 +- orocos_kdl/src/frames.cpp | 7 +- orocos_kdl/src/path_roundedcomposite.hpp | 2 +- orocos_kdl/src/utilities/utility.cxx | 12 +- orocos_kdl/src/utilities/utility.h | 6 + orocos_kdl/tests/framestest.cpp | 140 +++++++++--------- orocos_kdl/tests/solvertest.cpp | 36 ++--- 10 files changed, 126 insertions(+), 118 deletions(-) diff --git a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp index f2121d4d3..01f8f5d71 100644 --- a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp +++ b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp @@ -54,6 +54,8 @@ estimate of shortest time per invposkin (ms) 0.155544 #include #include #include +#include + #include /** @@ -90,9 +92,9 @@ void test_inverseposkin(KDL::Chain& chain) { JntArray q_sol(n); for (int trial=0;trial #include +#include int main() { @@ -87,18 +88,18 @@ int main() //Creating an Identity rotation matrix KDL::Rotation r4=KDL::Rotation::Identity(); //Creating a Rotation matrix from a rotation around X - KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3); + KDL::Rotation r5=KDL::Rotation::RotX(PI/3); //Creating a Rotation matrix from a rotation around Y - KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3); + KDL::Rotation r6=KDL::Rotation::RotY(PI/3); //Creating a Rotation matrix from a rotation around Z - KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3); + KDL::Rotation r7=KDL::Rotation::RotZ(PI/3); //Creating a Rotation matrix from a rotation around a arbitrary //vector, the vector should not be normalised - KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4); + KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),PI_4); //Creating a Rotation matrix from a rotation around a arbitrary //vector, the vector should be normalised KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071), - M_PI/4); + PI_4); //Creating a Rotation matrix from Euler ZYZ rotation angles KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.); //Creating a Rotation matrix from Euler ZYX rotation angles diff --git a/orocos_kdl/examples/trajectory_example.cpp b/orocos_kdl/examples/trajectory_example.cpp index 99908fe6d..da34357e2 100644 --- a/orocos_kdl/examples/trajectory_example.cpp +++ b/orocos_kdl/examples/trajectory_example.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include int main(int argc,char* argv[]) { @@ -35,13 +36,13 @@ int main(int argc,char* argv[]) { // When the routines are parallel, no rounding is needed, and no attempt is made // add constructing a rounding arc. // (It is still not possible when the segments are on top of each other) - // Note that you can only rotate in a deterministic way over an angle less then M_PI! - // With an angle == M_PI, you cannot predict over which side will be rotated. - // With an angle > M_PI, the routine will rotate over 2*M_PI-angle. + // Note that you can only rotate in a deterministic way over an angle less then PI! + // With an angle == PI, you cannot predict over which side will be rotated. + // With an angle > PI, the routine will rotate over 2*PI-angle. // If you need to rotate over a larger angle, you need to introduce intermediate points. // So, there is a common use case for using parallel segments. - path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0))); - path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0))); + path->Add(Frame(Rotation::RPY(PI,0,0), Vector(-1,0,0))); + path->Add(Frame(Rotation::RPY(PI_2,0,0), Vector(-0.5,0,0))); path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1))); path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0))); @@ -61,8 +62,6 @@ int main(int argc,char* argv[]) { ctraject->Add(traject); ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0)))); - - // use the trajectory double dt=0.1; std::ofstream of("./trajectory.dat"); @@ -113,5 +112,3 @@ int main(int argc,char* argv[]) { } } - - diff --git a/orocos_kdl/models/puma560.cpp b/orocos_kdl/models/puma560.cpp index 607442815..d98a5105c 100644 --- a/orocos_kdl/models/puma560.cpp +++ b/orocos_kdl/models/puma560.cpp @@ -21,28 +21,29 @@ #include #include "models.hpp" +#include namespace KDL{ Chain Puma560(){ Chain puma560; puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,M_PI_2,0.0,0.0), + Frame::DH(0.0,PI_2,0.0,0.0), RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.4318,0.0,0.0,0.0), RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0)))); puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0203,-M_PI_2,0.15005,0.0), + Frame::DH(0.0203,-PI_2,0.15005,0.0), RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,M_PI_2,0.4318,0.0), + Frame::DH(0.0,PI_2,0.4318,0.0), RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0)))); puma560.addSegment(Segment()); puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,-M_PI_2,0.0,0.0), + Frame::DH(0.0,-PI_2,0.0,0.0), RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.0,0.0,0.0,0.0), diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index aaede3c69..13d8aa7e8 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -26,9 +26,8 @@ ***************************************************************************/ #include "frames.hpp" +#include "utilities/utility.h" -#define _USE_MATH_DEFINES // For MSVC -#include #include namespace KDL { @@ -260,7 +259,7 @@ void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const { double epsilon=1E-12; pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); - if ( fabs(pitch) > (M_PI/2.0-epsilon) ) { + if ( fabs(pitch) > (PI_2-epsilon) ) { yaw = atan2( -data[1], data[4]); roll = 0.0 ; } else { @@ -393,7 +392,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { } // otherwise this singularity is angle = 180 - angle = M_PI; + angle = PI; double xx = (data[0] + 1) / 2; double yy = (data[4] + 1) / 2; double zz = (data[8] + 1) / 2; diff --git a/orocos_kdl/src/path_roundedcomposite.hpp b/orocos_kdl/src/path_roundedcomposite.hpp index 79c2657f5..2ccb860a0 100644 --- a/orocos_kdl/src/path_roundedcomposite.hpp +++ b/orocos_kdl/src/path_roundedcomposite.hpp @@ -96,7 +96,7 @@ class Path_RoundedComposite : public Path * - 3101 if the eq. radius <= 0 * - 3102 if the first segment in a rounding has zero length. * - 3103 if the second segment in a rounding has zero length. - * - 3104 if the angle between the first and the second segment is close to M_PI. + * - 3104 if the angle between the first and the second segment is close to PI. * (meaning that the segments are on top of each other) * - 3105 if the distance needed for the rounding is larger then the first segment. * - 3106 if the distance needed for the rounding is larger then the second segment. diff --git a/orocos_kdl/src/utilities/utility.cxx b/orocos_kdl/src/utilities/utility.cxx index ad995a047..7cc7e9663 100644 --- a/orocos_kdl/src/utilities/utility.cxx +++ b/orocos_kdl/src/utilities/utility.cxx @@ -11,10 +11,12 @@ namespace KDL { -int STREAMBUFFERSIZE=10000; +int STREAMBUFFERSIZE = 10000; int MAXLENFILENAME = 255; -const double PI= 3.1415926535897932384626433832795; -const double deg2rad = 0.01745329251994329576923690768488; -const double rad2deg = 57.2957795130823208767981548141052; -double epsilon = 0.000001; +const double PI = 3.141592653589793238462643383279502884; // PI +const double PI_2 = 1.570796326794896619231321691639751442; // PI/2 +const double PI_4 = 0.785398163397448309615660845819875721; // PI/4 +const double deg2rad = 0.017453292519943295769236907684886127; // PI/180 +const double rad2deg = 57.29577951308232087679815481410517033; // 180/PI +double epsilon = 1e-6; } diff --git a/orocos_kdl/src/utilities/utility.h b/orocos_kdl/src/utilities/utility.h index 83266ed48..651dd7660 100644 --- a/orocos_kdl/src/utilities/utility.h +++ b/orocos_kdl/src/utilities/utility.h @@ -179,6 +179,12 @@ extern int MAXLENFILENAME; //! the value of pi extern const double PI; +//! the value of pi/2 +extern const double PI_2; + +//! the value of pi/4 +extern const double PI_4; + //! the value pi/180 extern const double deg2rad; diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index e36b040ce..6b1e62357 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -1,6 +1,7 @@ -#include #include "framestest.hpp" #include +#include + CPPUNIT_TEST_SUITE_REGISTRATION( FramesTest ); using namespace KDL; @@ -237,37 +238,37 @@ void FramesTest::TestEuler() { TESTEULERZYX(0.1,0,0) TESTEULERZYX(0,0,0.3) TESTEULERZYX(0,0,0) - TESTEULERZYX(0.3,0.999*M_PI/2,0.1) - // if beta== +/- M_PI/2 => multiple solutions available, gamma will be chosen to be zero ! + TESTEULERZYX(0.3,0.999*PI_2,0.1) + // if beta== +/- PI/2 => multiple solutions available, gamma will be chosen to be zero ! // so we test with gamma==0 ! - TESTEULERZYX(0.3,0.9999999999*M_PI/2,0) - TESTEULERZYX(0.3,0.99999999*M_PI/2,0) - TESTEULERZYX(0.3,0.999999*M_PI/2,0) - TESTEULERZYX(0.3,0.9999*M_PI/2,0) - TESTEULERZYX(0.3,0.99*M_PI/2,0) - //TESTEULERZYX(0.1,-M_PI/2,0.3) - TESTEULERZYX(0,M_PI/2,0) - - TESTEULERZYX(0.3,-M_PI/2,0) - TESTEULERZYX(0.3,-0.9999999999*M_PI/2,0) - TESTEULERZYX(0.3,-0.99999999*M_PI/2,0) - TESTEULERZYX(0.3,-0.999999*M_PI/2,0) - TESTEULERZYX(0.3,-0.9999*M_PI/2,0) - TESTEULERZYX(0.3,-0.99*M_PI/2,0) - TESTEULERZYX(0,-M_PI/2,0) + TESTEULERZYX(0.3,0.9999999999*PI_2,0) + TESTEULERZYX(0.3,0.99999999*PI_2,0) + TESTEULERZYX(0.3,0.999999*PI_2,0) + TESTEULERZYX(0.3,0.9999*PI_2,0) + TESTEULERZYX(0.3,0.99*PI_2,0) + //TESTEULERZYX(0.1,-PI_2,0.3) + TESTEULERZYX(0,PI_2,0) + + TESTEULERZYX(0.3,-PI_2,0) + TESTEULERZYX(0.3,-0.9999999999*PI_2,0) + TESTEULERZYX(0.3,-0.99999999*PI_2,0) + TESTEULERZYX(0.3,-0.999999*PI_2,0) + TESTEULERZYX(0.3,-0.9999*PI_2,0) + TESTEULERZYX(0.3,-0.99*PI_2,0) + TESTEULERZYX(0,-PI_2,0) // extremes of the range: - TESTEULERZYX(M_PI,-M_PI/2,0) - TESTEULERZYX(-M_PI,-M_PI/2,0) - TESTEULERZYX(M_PI,1,0) - TESTEULERZYX(-M_PI,1,0) - //TESTEULERZYX(0,-M_PI/2,M_PI) gamma will be chosen zero - //TESTEULERZYX(0,M_PI/2,-M_PI) gamma will be chosen zero - TESTEULERZYX(0,1,M_PI) + TESTEULERZYX(PI,-PI_2,0) + TESTEULERZYX(-PI,-PI_2,0) + TESTEULERZYX(PI,1,0) + TESTEULERZYX(-PI,1,0) + //TESTEULERZYX(0,-PI_2,PI) gamma will be chosen zero + //TESTEULERZYX(0,PI_2,-PI) gamma will be chosen zero + TESTEULERZYX(0,1,PI) TESTEULERZYZ(0.1,0.2,0.3) TESTEULERZYZ(-0.1,0.2,0.3) - TESTEULERZYZ(0.1,0.9*M_PI,0.3) + TESTEULERZYZ(0.1,0.9*PI,0.3) TESTEULERZYZ(0.1,0.2,-0.3) TESTEULERZYZ(0,0,0) TESTEULERZYZ(0,0,0) @@ -277,22 +278,22 @@ void FramesTest::TestEuler() { TESTEULERZYZ(0.4,PI,0) TESTEULERZYZ(0,PI,0) TESTEULERZYZ(PI,PI,0) - TESTEULERZYX(0.3,M_PI/2,0) - TESTEULERZYZ(0.3,0.9999999999*M_PI/2,0) - TESTEULERZYZ(0.3,0.99999999*M_PI/2,0) - TESTEULERZYZ(0.3,0.999999*M_PI/2,0) - TESTEULERZYZ(0.3,0.9999*M_PI/2,0) - TESTEULERZYZ(0.3,0.99*M_PI/2,0) - - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3+M_PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3-M_PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3+M_PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3-M_PI); - - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3+M_PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3+M_PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3-M_PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3-M_PI); + TESTEULERZYX(0.3,PI_2,0) + TESTEULERZYZ(0.3,0.9999999999*PI_2,0) + TESTEULERZYZ(0.3,0.99999999*PI_2,0) + TESTEULERZYZ(0.3,0.999999*PI_2,0) + TESTEULERZYZ(0.3,0.9999*PI_2,0) + TESTEULERZYZ(0.3,0.99*PI_2,0) + + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+PI, PI-0.2, 0.3+PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-PI, PI-0.2, 0.3-PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-PI, PI-0.2, 0.3+PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+PI, PI-0.2, 0.3-PI); + + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+PI, -0.2, 0.3+PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-PI, -0.2, 0.3+PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+PI, -0.2, 0.3-PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-PI, -0.2, 0.3-PI); } void FramesTest::TestRangeArbitraryRotation(const std::string& msg, @@ -408,9 +409,8 @@ void FramesTest::TestRotation() { } void FramesTest::TestGetRotAngle() { - static const double pi = atan(1)*4; double roll = -2.9811968953315162; - double pitch = -pi/2; + double pitch = -PI_2; double yaw = -0.1603957582582825; // rpy -> rotation @@ -436,7 +436,7 @@ void FramesTest::TestGetRotAngle() { { Vector axis; double angle = KDL::Rotation(-1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, -1 - 1e-6).GetRotAngle(axis); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", M_PI, angle, epsilon); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", PI, angle, epsilon); } // Tests to show that GetRotAngle does not work for an improper rotation matrix which has a determinant of -1; @@ -520,84 +520,84 @@ void FramesTest::TestRotationDiff() { TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(0*deg2rad))", Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(90*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad), Vector(M_PI/2,0,0)); + Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0)); TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(180*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad), Vector(M_PI,0,0)); + Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad), Vector(PI,0,0)); TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(270*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad), Vector(-M_PI/2,0,0)); + Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad), Vector(-PI_2,0,0)); TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(360*deg2rad))", Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-360*deg2rad))", Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-270*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad), Vector(M_PI/2,0,0)); + Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad), Vector(PI_2,0,0)); TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-180*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad), Vector(M_PI,0,0)); + Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad), Vector(PI,0,0)); TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-90*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad), Vector(-M_PI/2,0,0)); + Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad), Vector(-PI_2,0,0)); TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-0*deg2rad))", Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(0*deg2rad))", Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(90*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,M_PI/2,0)); + Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(180*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad), Vector(0,M_PI,0)); + Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad), Vector(0,PI,0)); TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(270*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad), Vector(0,-M_PI/2,0)); + Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad), Vector(0,-PI_2,0)); TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(360*deg2rad))", Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-360*deg2rad))", Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-270*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad), Vector(0,M_PI/2,0)); + Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad), Vector(0,PI_2,0)); TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-180*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad), Vector(0,M_PI,0)); + Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad), Vector(0,PI,0)); TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-90*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad), Vector(0,-M_PI/2,0)); + Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad), Vector(0,-PI_2,0)); TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-0*deg2rad))", Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(0*deg2rad))", Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(90*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,M_PI/2)); + Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(180*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad), Vector(0,0,M_PI)); + Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad), Vector(0,0,PI)); TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(270*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad), Vector(0,0,-M_PI/2)); + Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad), Vector(0,0,-PI_2)); TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(360*deg2rad))", Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-360*deg2rad))", Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-270*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad), Vector(0,0,M_PI/2)); + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad), Vector(0,0,PI_2)); TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-180*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad), Vector(0,0,M_PI)); + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad), Vector(0,0,PI)); TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-90*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad), Vector(0,0,-M_PI/2)); + Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad), Vector(0,0,-PI_2)); TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-0*deg2rad))", Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad), Vector(0,0,0)); // no rotation TestOneRotationDiff("diff(RotX(0*deg2rad),RotZ(90*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,M_PI/2)); + Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); TestOneRotationDiff("diff(RotX(0*deg2rad),RotY(90*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,M_PI/2,0)); + Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); TestOneRotationDiff("diff(RotY(0*deg2rad),RotZ(90*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,M_PI/2)); + Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); TestOneRotationDiff("diff(Identity(),RotX(90*deg2rad))", - Rotation::Identity(), Rotation::RotX(90*deg2rad), Vector(M_PI/2,0,0)); + Rotation::Identity(), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0)); TestOneRotationDiff("diff(Identity(),RotY(0*deg2rad))", - Rotation::Identity(), Rotation::RotY(90*deg2rad), Vector(0,M_PI/2,0)); + Rotation::Identity(), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); TestOneRotationDiff("diff(Identity(),RotZ(0*deg2rad))", - Rotation::Identity(), Rotation::RotZ(90*deg2rad), Vector(0,0,M_PI/2)); + Rotation::Identity(), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); TestOneRotationDiff("diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),Rotation::RPY(-0*deg2rad,0,+90*deg2rad))", Rotation::RPY(+0*deg2rad,0,-90*deg2rad), Rotation::RPY(-0*deg2rad,0,+90*deg2rad), - Vector(0,0,M_PI)); + Vector(0,0,PI)); TestOneRotationDiff("diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),Rotation::RPY(-5*deg2rad,0,+0*deg2rad))", Rotation::RPY(+5*deg2rad,0,-0*deg2rad), Rotation::RPY(-5*deg2rad,0,+0*deg2rad), diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 7864e9096..914f8a694 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -3,7 +3,7 @@ #include #include #include - +#include CPPUNIT_TEST_SUITE_REGISTRATION( SolverTest ); @@ -106,17 +106,17 @@ void SolverTest::setUp() motomansia10.addSegment(Segment(Joint(Joint::None), Frame::DH_Craig1989(0.0, 0.0, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0))); + Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0))); + Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -M_PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, -PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), Frame(Rotation::Identity(),Vector(0.0,0.0,0.155)))); @@ -148,22 +148,22 @@ void SolverTest::setUp() KDL::RotationalInertia(0.01, 0.01, 0.1, 0.0, 0.0, 0.0)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), - Frame::DH(0.0, M_PI_2, 0.36, 0.0), + Frame::DH(0.0, PI_2, 0.36, 0.0), inert1)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), - Frame::DH(0.0, -M_PI_2, 0.0, 0.0), + Frame::DH(0.0, -PI_2, 0.0, 0.0), inert2)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), - Frame::DH(0.0, M_PI_2, 0.36, 0.0), + Frame::DH(0.0, PI_2, 0.36, 0.0), inert3)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), - Frame::DH(0.0, -M_PI_2, 0.0, 0.0), + Frame::DH(0.0, -PI_2, 0.0, 0.0), inert4)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), - Frame::DH(0.0, M_PI_2, 0.36, 0.0), + Frame::DH(0.0, PI_2, 0.36, 0.0), inert5)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), - Frame::DH(0.0, -M_PI_2, 0.0, 0.0), + Frame::DH(0.0, -PI_2, 0.0, 0.0), inert6)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), Frame::DH(0.0, 0.0, 0.0, 0.0), @@ -476,7 +476,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -503,7 +503,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.2 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -526,7 +526,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -595,7 +595,7 @@ void SolverTest::IkVelSolverWDLSTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -M_PI_2 ; + q(3) = -PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -822,7 +822,7 @@ void SolverTest::VereshchaginTest() // Initial arm position configuration/constraint JntArray jointInitialPose(chaindyn.getNrOfJoints()); jointInitialPose(0) = 0.0; // initial joint0 pose - jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise + jointInitialPose(1) = PI/6.0; //initial joint1 pose, negative in clockwise //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464 //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636 From 505338225adfad8165a38128e09eeeb0f6f5b39b Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Thu, 10 Feb 2022 20:05:45 +0100 Subject: [PATCH 77/79] Revert "Merge pull request #282 from orocos/fix/m_pi" This reverts commit 7b5b0f5912bcf44ae016bc0fcb453313d1c9f979. --- .../examples/chainiksolverpos_lma_demo.cpp | 6 +- orocos_kdl/examples/geometry.cpp | 11 +- orocos_kdl/examples/trajectory_example.cpp | 15 +- orocos_kdl/models/puma560.cpp | 9 +- orocos_kdl/src/frames.cpp | 7 +- orocos_kdl/src/path_roundedcomposite.hpp | 2 +- orocos_kdl/src/utilities/utility.cxx | 12 +- orocos_kdl/src/utilities/utility.h | 6 - orocos_kdl/tests/framestest.cpp | 222 ++++++++---------- orocos_kdl/tests/solvertest.cpp | 36 +-- 10 files changed, 140 insertions(+), 186 deletions(-) diff --git a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp index 01f8f5d71..f2121d4d3 100644 --- a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp +++ b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp @@ -54,8 +54,6 @@ estimate of shortest time per invposkin (ms) 0.155544 #include #include #include -#include - #include /** @@ -92,9 +90,9 @@ void test_inverseposkin(KDL::Chain& chain) { JntArray q_sol(n); for (int trial=0;trial #include -#include int main() { @@ -88,18 +87,18 @@ int main() //Creating an Identity rotation matrix KDL::Rotation r4=KDL::Rotation::Identity(); //Creating a Rotation matrix from a rotation around X - KDL::Rotation r5=KDL::Rotation::RotX(PI/3); + KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3); //Creating a Rotation matrix from a rotation around Y - KDL::Rotation r6=KDL::Rotation::RotY(PI/3); + KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3); //Creating a Rotation matrix from a rotation around Z - KDL::Rotation r7=KDL::Rotation::RotZ(PI/3); + KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3); //Creating a Rotation matrix from a rotation around a arbitrary //vector, the vector should not be normalised - KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),PI_4); + KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4); //Creating a Rotation matrix from a rotation around a arbitrary //vector, the vector should be normalised KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071), - PI_4); + M_PI/4); //Creating a Rotation matrix from Euler ZYZ rotation angles KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.); //Creating a Rotation matrix from Euler ZYX rotation angles diff --git a/orocos_kdl/examples/trajectory_example.cpp b/orocos_kdl/examples/trajectory_example.cpp index da34357e2..99908fe6d 100644 --- a/orocos_kdl/examples/trajectory_example.cpp +++ b/orocos_kdl/examples/trajectory_example.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include int main(int argc,char* argv[]) { @@ -36,13 +35,13 @@ int main(int argc,char* argv[]) { // When the routines are parallel, no rounding is needed, and no attempt is made // add constructing a rounding arc. // (It is still not possible when the segments are on top of each other) - // Note that you can only rotate in a deterministic way over an angle less then PI! - // With an angle == PI, you cannot predict over which side will be rotated. - // With an angle > PI, the routine will rotate over 2*PI-angle. + // Note that you can only rotate in a deterministic way over an angle less then M_PI! + // With an angle == M_PI, you cannot predict over which side will be rotated. + // With an angle > M_PI, the routine will rotate over 2*M_PI-angle. // If you need to rotate over a larger angle, you need to introduce intermediate points. // So, there is a common use case for using parallel segments. - path->Add(Frame(Rotation::RPY(PI,0,0), Vector(-1,0,0))); - path->Add(Frame(Rotation::RPY(PI_2,0,0), Vector(-0.5,0,0))); + path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0))); + path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0))); path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1))); path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0))); @@ -62,6 +61,8 @@ int main(int argc,char* argv[]) { ctraject->Add(traject); ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0)))); + + // use the trajectory double dt=0.1; std::ofstream of("./trajectory.dat"); @@ -112,3 +113,5 @@ int main(int argc,char* argv[]) { } } + + diff --git a/orocos_kdl/models/puma560.cpp b/orocos_kdl/models/puma560.cpp index d98a5105c..607442815 100644 --- a/orocos_kdl/models/puma560.cpp +++ b/orocos_kdl/models/puma560.cpp @@ -21,29 +21,28 @@ #include #include "models.hpp" -#include namespace KDL{ Chain Puma560(){ Chain puma560; puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,PI_2,0.0,0.0), + Frame::DH(0.0,M_PI_2,0.0,0.0), RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.4318,0.0,0.0,0.0), RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0)))); puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0203,-PI_2,0.15005,0.0), + Frame::DH(0.0203,-M_PI_2,0.15005,0.0), RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,PI_2,0.4318,0.0), + Frame::DH(0.0,M_PI_2,0.4318,0.0), RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0)))); puma560.addSegment(Segment()); puma560.addSegment(Segment()); puma560.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH(0.0,-PI_2,0.0,0.0), + Frame::DH(0.0,-M_PI_2,0.0,0.0), RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0)))); puma560.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.0,0.0,0.0,0.0), diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index 13d8aa7e8..aaede3c69 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -26,8 +26,9 @@ ***************************************************************************/ #include "frames.hpp" -#include "utilities/utility.h" +#define _USE_MATH_DEFINES // For MSVC +#include #include namespace KDL { @@ -259,7 +260,7 @@ void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const { double epsilon=1E-12; pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); - if ( fabs(pitch) > (PI_2-epsilon) ) { + if ( fabs(pitch) > (M_PI/2.0-epsilon) ) { yaw = atan2( -data[1], data[4]); roll = 0.0 ; } else { @@ -392,7 +393,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { } // otherwise this singularity is angle = 180 - angle = PI; + angle = M_PI; double xx = (data[0] + 1) / 2; double yy = (data[4] + 1) / 2; double zz = (data[8] + 1) / 2; diff --git a/orocos_kdl/src/path_roundedcomposite.hpp b/orocos_kdl/src/path_roundedcomposite.hpp index 2ccb860a0..79c2657f5 100644 --- a/orocos_kdl/src/path_roundedcomposite.hpp +++ b/orocos_kdl/src/path_roundedcomposite.hpp @@ -96,7 +96,7 @@ class Path_RoundedComposite : public Path * - 3101 if the eq. radius <= 0 * - 3102 if the first segment in a rounding has zero length. * - 3103 if the second segment in a rounding has zero length. - * - 3104 if the angle between the first and the second segment is close to PI. + * - 3104 if the angle between the first and the second segment is close to M_PI. * (meaning that the segments are on top of each other) * - 3105 if the distance needed for the rounding is larger then the first segment. * - 3106 if the distance needed for the rounding is larger then the second segment. diff --git a/orocos_kdl/src/utilities/utility.cxx b/orocos_kdl/src/utilities/utility.cxx index 7cc7e9663..ad995a047 100644 --- a/orocos_kdl/src/utilities/utility.cxx +++ b/orocos_kdl/src/utilities/utility.cxx @@ -11,12 +11,10 @@ namespace KDL { -int STREAMBUFFERSIZE = 10000; +int STREAMBUFFERSIZE=10000; int MAXLENFILENAME = 255; -const double PI = 3.141592653589793238462643383279502884; // PI -const double PI_2 = 1.570796326794896619231321691639751442; // PI/2 -const double PI_4 = 0.785398163397448309615660845819875721; // PI/4 -const double deg2rad = 0.017453292519943295769236907684886127; // PI/180 -const double rad2deg = 57.29577951308232087679815481410517033; // 180/PI -double epsilon = 1e-6; +const double PI= 3.1415926535897932384626433832795; +const double deg2rad = 0.01745329251994329576923690768488; +const double rad2deg = 57.2957795130823208767981548141052; +double epsilon = 0.000001; } diff --git a/orocos_kdl/src/utilities/utility.h b/orocos_kdl/src/utilities/utility.h index 651dd7660..83266ed48 100644 --- a/orocos_kdl/src/utilities/utility.h +++ b/orocos_kdl/src/utilities/utility.h @@ -179,12 +179,6 @@ extern int MAXLENFILENAME; //! the value of pi extern const double PI; -//! the value of pi/2 -extern const double PI_2; - -//! the value of pi/4 -extern const double PI_4; - //! the value pi/180 extern const double deg2rad; diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index 1f0667d80..b1905cdfc 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -1,7 +1,6 @@ +#include #include "framestest.hpp" #include -#include - CPPUNIT_TEST_SUITE_REGISTRATION( FramesTest ); using namespace KDL; @@ -238,37 +237,37 @@ void FramesTest::TestEuler() { TESTEULERZYX(0.1,0,0) TESTEULERZYX(0,0,0.3) TESTEULERZYX(0,0,0) - TESTEULERZYX(0.3,0.999*PI_2,0.1) - // if beta== +/- PI/2 => multiple solutions available, gamma will be chosen to be zero ! + TESTEULERZYX(0.3,0.999*M_PI/2,0.1) + // if beta== +/- M_PI/2 => multiple solutions available, gamma will be chosen to be zero ! // so we test with gamma==0 ! - TESTEULERZYX(0.3,0.9999999999*PI_2,0) - TESTEULERZYX(0.3,0.99999999*PI_2,0) - TESTEULERZYX(0.3,0.999999*PI_2,0) - TESTEULERZYX(0.3,0.9999*PI_2,0) - TESTEULERZYX(0.3,0.99*PI_2,0) - //TESTEULERZYX(0.1,-PI_2,0.3) - TESTEULERZYX(0,PI_2,0) - - TESTEULERZYX(0.3,-PI_2,0) - TESTEULERZYX(0.3,-0.9999999999*PI_2,0) - TESTEULERZYX(0.3,-0.99999999*PI_2,0) - TESTEULERZYX(0.3,-0.999999*PI_2,0) - TESTEULERZYX(0.3,-0.9999*PI_2,0) - TESTEULERZYX(0.3,-0.99*PI_2,0) - TESTEULERZYX(0,-PI_2,0) + TESTEULERZYX(0.3,0.9999999999*M_PI/2,0) + TESTEULERZYX(0.3,0.99999999*M_PI/2,0) + TESTEULERZYX(0.3,0.999999*M_PI/2,0) + TESTEULERZYX(0.3,0.9999*M_PI/2,0) + TESTEULERZYX(0.3,0.99*M_PI/2,0) + //TESTEULERZYX(0.1,-M_PI/2,0.3) + TESTEULERZYX(0,M_PI/2,0) + + TESTEULERZYX(0.3,-M_PI/2,0) + TESTEULERZYX(0.3,-0.9999999999*M_PI/2,0) + TESTEULERZYX(0.3,-0.99999999*M_PI/2,0) + TESTEULERZYX(0.3,-0.999999*M_PI/2,0) + TESTEULERZYX(0.3,-0.9999*M_PI/2,0) + TESTEULERZYX(0.3,-0.99*M_PI/2,0) + TESTEULERZYX(0,-M_PI/2,0) // extremes of the range: - TESTEULERZYX(PI,-PI_2,0) - TESTEULERZYX(-PI,-PI_2,0) - TESTEULERZYX(PI,1,0) - TESTEULERZYX(-PI,1,0) - //TESTEULERZYX(0,-PI_2,PI) gamma will be chosen zero - //TESTEULERZYX(0,PI_2,-PI) gamma will be chosen zero - TESTEULERZYX(0,1,PI) + TESTEULERZYX(M_PI,-M_PI/2,0) + TESTEULERZYX(-M_PI,-M_PI/2,0) + TESTEULERZYX(M_PI,1,0) + TESTEULERZYX(-M_PI,1,0) + //TESTEULERZYX(0,-M_PI/2,M_PI) gamma will be chosen zero + //TESTEULERZYX(0,M_PI/2,-M_PI) gamma will be chosen zero + TESTEULERZYX(0,1,M_PI) TESTEULERZYZ(0.1,0.2,0.3) TESTEULERZYZ(-0.1,0.2,0.3) - TESTEULERZYZ(0.1,0.9*PI,0.3) + TESTEULERZYZ(0.1,0.9*M_PI,0.3) TESTEULERZYZ(0.1,0.2,-0.3) TESTEULERZYZ(0,0,0) TESTEULERZYZ(0,0,0) @@ -278,22 +277,22 @@ void FramesTest::TestEuler() { TESTEULERZYZ(0.4,PI,0) TESTEULERZYZ(0,PI,0) TESTEULERZYZ(PI,PI,0) - TESTEULERZYX(0.3,PI_2,0) - TESTEULERZYZ(0.3,0.9999999999*PI_2,0) - TESTEULERZYZ(0.3,0.99999999*PI_2,0) - TESTEULERZYZ(0.3,0.999999*PI_2,0) - TESTEULERZYZ(0.3,0.9999*PI_2,0) - TESTEULERZYZ(0.3,0.99*PI_2,0) - - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+PI, PI-0.2, 0.3+PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-PI, PI-0.2, 0.3-PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-PI, PI-0.2, 0.3+PI); - TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+PI, PI-0.2, 0.3-PI); - - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+PI, -0.2, 0.3+PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-PI, -0.2, 0.3+PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+PI, -0.2, 0.3-PI); - TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-PI, -0.2, 0.3-PI); + TESTEULERZYX(0.3,M_PI/2,0) + TESTEULERZYZ(0.3,0.9999999999*M_PI/2,0) + TESTEULERZYZ(0.3,0.99999999*M_PI/2,0) + TESTEULERZYZ(0.3,0.999999*M_PI/2,0) + TESTEULERZYZ(0.3,0.9999*M_PI/2,0) + TESTEULERZYZ(0.3,0.99*M_PI/2,0) + + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3+M_PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3-M_PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3+M_PI); + TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3-M_PI); + + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3+M_PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3+M_PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3-M_PI); + TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3-M_PI); } void FramesTest::TestRangeArbitraryRotation(const std::string& msg, @@ -409,8 +408,9 @@ void FramesTest::TestRotation() { } void FramesTest::TestGetRotAngle() { + static const double pi = atan(1)*4; double roll = -2.9811968953315162; - double pitch = -PI_2; + double pitch = -pi/2; double yaw = -0.1603957582582825; // rpy -> rotation @@ -436,7 +436,7 @@ void FramesTest::TestGetRotAngle() { { Vector axis; double angle = KDL::Rotation(-1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, -1 - 1e-6).GetRotAngle(axis); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", PI, angle, epsilon); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", M_PI, angle, epsilon); } // Tests to show that GetRotAngle does not work for an improper rotation matrix which has a determinant of -1; @@ -509,91 +509,53 @@ void FramesTest::TestQuaternion() { void FramesTest::TestRotationDiff() { - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(0*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad), Vector(0,0,0)); // no rotation - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(90*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0)); - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(180*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad), Vector(PI,0,0)); - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(270*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad), Vector(-PI_2,0,0)); - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(360*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad), Vector(0,0,0)); // no rotation - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-360*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad), Vector(0,0,0)); // no rotation - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-270*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad), Vector(PI_2,0,0)); - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-180*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad), Vector(PI,0,0)); - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-90*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad), Vector(-PI_2,0,0)); - TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-0*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad), Vector(0,0,0)); // no rotation - - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(0*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad), Vector(0,0,0)); // no rotation - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(90*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(180*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad), Vector(0,PI,0)); - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(270*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad), Vector(0,-PI_2,0)); - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(360*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad), Vector(0,0,0)); // no rotation - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-360*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad), Vector(0,0,0)); // no rotation - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-270*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad), Vector(0,PI_2,0)); - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-180*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad), Vector(0,PI,0)); - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-90*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad), Vector(0,-PI_2,0)); - TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-0*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad), Vector(0,0,0)); // no rotation - - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(0*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad), Vector(0,0,0)); // no rotation - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(90*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(180*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad), Vector(0,0,PI)); - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(270*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad), Vector(0,0,-PI_2)); - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(360*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad), Vector(0,0,0)); // no rotation - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-360*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad), Vector(0,0,0)); // no rotation - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-270*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad), Vector(0,0,PI_2)); - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-180*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad), Vector(0,0,PI)); - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-90*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad), Vector(0,0,-PI_2)); - TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-0*deg2rad))", - Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad), Vector(0,0,0)); // no rotation - - TestOneRotationDiff("diff(RotX(0*deg2rad),RotZ(90*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); - TestOneRotationDiff("diff(RotX(0*deg2rad),RotY(90*deg2rad))", - Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); - TestOneRotationDiff("diff(RotY(0*deg2rad),RotZ(90*deg2rad))", - Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); - - TestOneRotationDiff("diff(Identity(),RotX(90*deg2rad))", - Rotation::Identity(), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0)); - TestOneRotationDiff("diff(Identity(),RotY(0*deg2rad))", - Rotation::Identity(), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0)); - TestOneRotationDiff("diff(Identity(),RotZ(0*deg2rad))", - Rotation::Identity(), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2)); - - TestOneRotationDiff("diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),Rotation::RPY(-0*deg2rad,0,+90*deg2rad))", - Rotation::RPY(+0*deg2rad,0,-90*deg2rad), - Rotation::RPY(-0*deg2rad,0,+90*deg2rad), - Vector(0,0,PI)); - TestOneRotationDiff("diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),Rotation::RPY(-5*deg2rad,0,+0*deg2rad))", - Rotation::RPY(+5*deg2rad,0,-0*deg2rad), - Rotation::RPY(-5*deg2rad,0,+0*deg2rad), - Vector(-10*deg2rad,0,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad)), Vector(0,0,0)); // no rotation + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad)), Vector(M_PI,0,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad)), Vector(-M_PI/2,0,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad)), Vector(0,0,0)); // no rotation + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad)), Vector(0,0,0)); // no rotation + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad)), Vector(M_PI/2,0,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad)), Vector(M_PI,0,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad)), Vector(-M_PI/2,0,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad)), Vector(0,0,0)); // no rotation + + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad)), Vector(0,0,0)); // no rotation + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad)), Vector(0,M_PI,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad)), Vector(0,-M_PI/2,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad)), Vector(0,0,0)); // no rotation + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad)), Vector(0,0,0)); // no rotation + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad)), Vector(0,M_PI/2,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad)), Vector(0,M_PI,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad)), Vector(0,-M_PI/2,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad)), Vector(0,0,0)); // no rotation + + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad)), Vector(0,0,0)); // no rotation + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad)), Vector(0,0,M_PI)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad)), Vector(0,0,-M_PI/2)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad)), Vector(0,0,0)); // no rotation + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad)), Vector(0,0,0)); // no rotation + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad)), Vector(0,0,M_PI/2)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad)), Vector(0,0,M_PI)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad)), Vector(0,0,-M_PI/2)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad)), Vector(0,0,0)); // no rotation + + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); + + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2)); + + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad), + Rotation::RPY(-0*deg2rad,0,+90*deg2rad)), + Vector(0,0,M_PI)); + CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad), + Rotation::RPY(-5*deg2rad,0,+0*deg2rad)), + Vector(-10*deg2rad,0,0)); KDL::Rotation R1 = Rotation::RPY(+5*deg2rad,0,-90*deg2rad); CPPUNIT_ASSERT_EQUAL(KDL::diff(R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad)), diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 914f8a694..7864e9096 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -3,7 +3,7 @@ #include #include #include -#include + CPPUNIT_TEST_SUITE_REGISTRATION( SolverTest ); @@ -106,17 +106,17 @@ void SolverTest::setUp() motomansia10.addSegment(Segment(Joint(Joint::None), Frame::DH_Craig1989(0.0, 0.0, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0))); + Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0))); + Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), - Frame::DH_Craig1989(0.0, -PI_2, 0.0, 0.0))); + Frame::DH_Craig1989(0.0, -M_PI_2, 0.0, 0.0))); motomansia10.addSegment(Segment(Joint(Joint::RotZ), Frame(Rotation::Identity(),Vector(0.0,0.0,0.155)))); @@ -148,22 +148,22 @@ void SolverTest::setUp() KDL::RotationalInertia(0.01, 0.01, 0.1, 0.0, 0.0, 0.0)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), - Frame::DH(0.0, PI_2, 0.36, 0.0), + Frame::DH(0.0, M_PI_2, 0.36, 0.0), inert1)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness), - Frame::DH(0.0, -PI_2, 0.0, 0.0), + Frame::DH(0.0, -M_PI_2, 0.0, 0.0), inert2)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), - Frame::DH(0.0, PI_2, 0.36, 0.0), + Frame::DH(0.0, M_PI_2, 0.36, 0.0), inert3)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness), - Frame::DH(0.0, -PI_2, 0.0, 0.0), + Frame::DH(0.0, -M_PI_2, 0.0, 0.0), inert4)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), - Frame::DH(0.0, PI_2, 0.36, 0.0), + Frame::DH(0.0, M_PI_2, 0.36, 0.0), inert5)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), - Frame::DH(0.0, -PI_2, 0.0, 0.0), + Frame::DH(0.0, -M_PI_2, 0.0, 0.0), inert6)); motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness), Frame::DH(0.0, 0.0, 0.0, 0.0), @@ -476,7 +476,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -PI_2 ; + q(3) = -M_PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -503,7 +503,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.2 ; q(2) = 0.4 ; - q(3) = -PI_2 ; + q(3) = -M_PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -526,7 +526,7 @@ void SolverTest::IkSingularValueTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -PI_2 ; + q(3) = -M_PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -595,7 +595,7 @@ void SolverTest::IkVelSolverWDLSTest() q(0) = 0. ; q(1) = 0.5 ; q(2) = 0.4 ; - q(3) = -PI_2 ; + q(3) = -M_PI_2 ; q(4) = 0. ; q(5) = 0. ; q(6) = 0. ; @@ -822,7 +822,7 @@ void SolverTest::VereshchaginTest() // Initial arm position configuration/constraint JntArray jointInitialPose(chaindyn.getNrOfJoints()); jointInitialPose(0) = 0.0; // initial joint0 pose - jointInitialPose(1) = PI/6.0; //initial joint1 pose, negative in clockwise + jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464 //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636 From bf39b8de926b9faeb18158a51ba4cee1e6e5151f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 14 Oct 2020 11:56:54 +0200 Subject: [PATCH 78/79] Merge pull request #290 from orocos/fix/geometry_example Fix/geometry example --- orocos_kdl/examples/geometry.cpp | 37 ++++++++++++++------------------ 1 file changed, 16 insertions(+), 21 deletions(-) diff --git a/orocos_kdl/examples/geometry.cpp b/orocos_kdl/examples/geometry.cpp index 479f43ae9..5f9cfc764 100644 --- a/orocos_kdl/examples/geometry.cpp +++ b/orocos_kdl/examples/geometry.cpp @@ -4,19 +4,18 @@ int main() { - //Creating Vectors KDL::Vector v1;//Default constructor KDL::Vector v2(1.0,2.0,3.0);//Most used constructor KDL::Vector v3(v2);//Copy constructor KDL::Vector v4 = KDL::Vector::Zero();//Static member - + //Use operator << to print the values of your vector std::cout<<"v1 ="< Date: Sun, 25 Oct 2020 09:57:07 +0100 Subject: [PATCH 79/79] Merge pull request #293 from orocos/update/pybind_2_6_0 Update pybind11 to v2.6.0 --- .gitmodules | 2 +- python_orocos_kdl/pybind11 | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 6c6a02f83..2c9699f57 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,4 @@ [submodule "python_orocos_kdl/pybind11"] path = python_orocos_kdl/pybind11 url = https://github.com/pybind/pybind11.git - branch = v2.5.0 + branch = v2.6.0 diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 3b1dbebab..59a2ac274 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 3b1dbebabc801c9cf6f0953a4c20b904d444f879 +Subproject commit 59a2ac2745d8a57ac94c6accced73620d59fb844