From 8352af782af23fd53708b3baf928591086b10151 Mon Sep 17 00:00:00 2001 From: Daniel Claes Date: Fri, 29 Sep 2017 11:56:21 +0200 Subject: [PATCH 01/13] fix(GetRotAngle): New GetRotAngle function to prevent erros of some flipping --- orocos_kdl/CMakeLists.txt | 2 +- orocos_kdl/src/frames.cpp | 126 ++++++++++++++++++++++++++++---------- 2 files changed, 95 insertions(+), 33 deletions(-) diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index 5cc27028c..5d2209807 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -74,7 +74,7 @@ endif(KDL_USE_NEW_TREE_INTERFACE) INCLUDE (${PROJ_SOURCE_DIR}/config/DependentOption.cmake) -OPTION(ENABLE_TESTS OFF "Enable building of tests") +OPTION(ENABLE_TESTS ON "Enable building of tests") IF( ENABLE_TESTS ) # If not in standard paths, set CMAKE_xxx_PATH's in environment, eg. # export CMAKE_INCLUDE_PATH=/opt/local/include diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index 4b9ec2de5..d98692b46 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -350,38 +350,100 @@ Vector Rotation::GetRot() const * Check corresponding routines in rframes and rrframes */ double Rotation::GetRotAngle(Vector& axis,double eps) const { - double ca = (data[0]+data[4]+data[8]-1)/2.0; - double t= eps*eps/2.0; - if (ca>1-t) { - // undefined choose the Z-axis, and angle 0 - axis = Vector(0,0,1); - return 0; - } - if (ca < -1+t) { - // The case of angles consisting of multiples of M_PI: - // two solutions, choose a positive Z-component of the axis - double x = sqrt( (data[0]+1.0)/2); - double y = sqrt( (data[4]+1.0)/2); - double z = sqrt( (data[8]+1.0)/2); - if ( data[2] < 0) x=-x; - if ( data[7] < 0) y=-y; - if ( x*y*data[1] < 0) x=-x; // this last line can be necessary when z is 0 - // z always >= 0 - // if z equal to zero - axis = Vector( x,y,z ); - return PI; - } - double angle; - double mod_axis; - double axisx, axisy, axisz; - axisx = data[7]-data[5]; - axisy = data[2]-data[6]; - axisz = data[3]-data[1]; - mod_axis = sqrt(axisx*axisx+axisy*axisy+axisz*axisz); - axis = Vector(axisx/mod_axis, - axisy/mod_axis, - axisz/mod_axis ); - angle = atan2(mod_axis/2,ca); + double angle,x,y,z; // variables for result + double epsilon = eps; // margin to allow for rounding errors + double epsilon2 = eps*10; // margin to distinguish between 0 and 180 degrees + + // optional check that input is pure rotation, 'isRotationMatrix' is defined at: + // http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/ + + if ((std::abs(data[1] - data[3]) < epsilon) + && (std::abs(data[2] - data[6])< epsilon) + && (std::abs(data[5] - data[7]) < epsilon)) + { + // singularity found + // first check for identity matrix which must have +1 for all terms + // in leading diagonal and zero in other terms + if ((std::abs(data[1] + data[3]) < epsilon2) + && (std::abs(data[2] + data[6]) < epsilon2) + && (std::abs(data[5] + data[7]) < epsilon2) + && (std::abs(data[0] + data[4] + data[8]-3) < epsilon2)) + { + // this singularity is identity matrix so angle = 0, axis is arbitrary + axis = KDL::Vector(1,0,0); + angle = 0.0; + return angle; + } + + // otherwise this singularity is angle = 180 + angle = M_PI; + double xx = (data[0] + 1) / 2; + double yy = (data[4] + 1) / 2; + double zz = (data[8] + 1) / 2; + double xy = (data[1] + data[3]) / 4; + double xz = (data[2] + data[6]) / 4; + double yz = (data[5] + data[7]) / 4; + + double half_sqrt_2 = 0.5 * sqrt(2.0); + + 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; + } + } + 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; + } + } + 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; + } + } + axis = KDL::Vector(x, y, z); + return angle; // return 180 deg rotation + } + + angle = acos(( data[0] + data[4] + data[8] - 1)/2); + x = (data[7] - data[5]); + y = (data[2] - data[6]); + z = (data[3] - data[1]); + axis = KDL::Vector(x, y, z); + axis.Normalize(); return angle; } From 30fc63e7506a188e1e928daffaf5b62e224e8ade Mon Sep 17 00:00:00 2001 From: Daniel Claes Date: Mon, 2 Oct 2017 15:34:03 +0200 Subject: [PATCH 02/13] fix(): disable tests in CMakeLists.txt --- orocos_kdl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index 5d2209807..5cc27028c 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -74,7 +74,7 @@ endif(KDL_USE_NEW_TREE_INTERFACE) INCLUDE (${PROJ_SOURCE_DIR}/config/DependentOption.cmake) -OPTION(ENABLE_TESTS ON "Enable building of tests") +OPTION(ENABLE_TESTS OFF "Enable building of tests") IF( ENABLE_TESTS ) # If not in standard paths, set CMAKE_xxx_PATH's in environment, eg. # export CMAKE_INCLUDE_PATH=/opt/local/include From f647ef513f6fe5dc966cedbd9ad41baaf657fce8 Mon Sep 17 00:00:00 2001 From: Daniel Claes Date: Mon, 2 Oct 2017 17:35:44 +0200 Subject: [PATCH 03/13] fix(): make arbitrary axis the same as for orocos convention --- orocos_kdl/src/frames.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index d98692b46..aa808cfe4 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -370,7 +370,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { && (std::abs(data[0] + data[4] + data[8]-3) < epsilon2)) { // this singularity is identity matrix so angle = 0, axis is arbitrary - axis = KDL::Vector(1,0,0); + axis = KDL::Vector(0,0,1); angle = 0.0; return angle; } From 3e13a684401d4d26b263264a0412515650074ee2 Mon Sep 17 00:00:00 2001 From: Daniel Claes Date: Mon, 2 Oct 2017 18:15:57 +0200 Subject: [PATCH 04/13] fix(): Orocos tests additions --- orocos_kdl/src/frames.cpp | 1 + orocos_kdl/tests/framestest.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index aa808cfe4..e501182fc 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -370,6 +370,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { && (std::abs(data[0] + data[4] + data[8]-3) < epsilon2)) { // this singularity is identity matrix so angle = 0, axis is arbitrary + // Choose 0, 0, 1 to pass orocos tests axis = KDL::Vector(0,0,1); angle = 0.0; return angle; diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index ed659d23a..62e72549c 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -375,8 +375,8 @@ void FramesTest::TestRotation() { TestOneRotation("rot([1,1,0],180)", KDL::Rotation::Rot(KDL::Vector(1,1,0),180*deg2rad), 180*deg2rad, Vector(1,1,0)/sqrt(2.0)); // opposite of +180 - TestOneRotation("rot([1,1,0],-180)", KDL::Rotation::Rot(KDL::Vector(1,1,0),-180*deg2rad), 180*deg2rad, -Vector(1,1,0)/sqrt(2.0)); - TestOneRotation("rot([-1,-1,0],180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,0),180*deg2rad), 180*deg2rad, -Vector(1,1,0)/sqrt(2.0)); + TestOneRotation("rot([1,1,0],-180)", KDL::Rotation::Rot(KDL::Vector(1,1,0),-180*deg2rad), 180*deg2rad, Vector(1,1,0)/sqrt(2.0)); + TestOneRotation("rot([-1,-1,0],180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,0),180*deg2rad), 180*deg2rad, Vector(1,1,0)/sqrt(2.0)); // opposite of +180 TestOneRotation("rot([-1,-1,0],-180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,0),-180*deg2rad), 180*deg2rad, Vector(1,1,0)/sqrt(2.0)); From 2fe2bf1d38105533459b1ce1eec19641852d1e07 Mon Sep 17 00:00:00 2001 From: Ruben Smits Date: Wed, 6 Dec 2017 11:37:54 +0100 Subject: [PATCH 05/13] Add edge testcase for GetRotAngle --- orocos_kdl/tests/framestest.cpp | 23 +++++++++++++++++++++++ orocos_kdl/tests/framestest.hpp | 2 ++ 2 files changed, 25 insertions(+) diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index 62e72549c..5eaa7c94d 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -393,6 +393,29 @@ void FramesTest::TestRotation() { TestOneRotation("rot([-1,-1,-1],180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,-1),180*deg2rad), 180*deg2rad, Vector(1,1,1)/sqrt(3.0)); // same as +180 TestOneRotation("rot([-1,-1,-1],-180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,-1),-180*deg2rad), 180*deg2rad, Vector(1,1,1)/sqrt(3.0)); + + TestOneRotation("rot([0.707107, 0, 0.707107", KDL::Rotation::RPY(-2.9811968953315162, -atan(1)*2, -0.1603957582582825), 180*deg2rad, Vector(0.707107,0,0.707107) ); +} + +void FramesTest::TestGetRotAngle() { + static const double pi = atan(1)*4; + double roll = -2.9811968953315162; + double pitch = -pi/2; + double yaw = -0.1603957582582825; + + // rpy -> rotation + KDL::Rotation kdlRotation1 = KDL::Rotation::RPY(roll, pitch, yaw); + + // rotation -> angle-axis (with KDL::GetRotAngle) + KDL::Vector kdlAxis; + double theta = kdlRotation1.GetRotAngle(kdlAxis); + + + CPPUNIT_ASSERT(0==isnan(theta)); + CPPUNIT_ASSERT(0==isnan(kdlAxis[0])); + CPPUNIT_ASSERT(0==isnan(kdlAxis[1])); + CPPUNIT_ASSERT(0==isnan(kdlAxis[2])); + } void FramesTest::TestQuaternion() { diff --git a/orocos_kdl/tests/framestest.hpp b/orocos_kdl/tests/framestest.hpp index 9d0e25a49..95f1257f2 100644 --- a/orocos_kdl/tests/framestest.hpp +++ b/orocos_kdl/tests/framestest.hpp @@ -19,6 +19,7 @@ class FramesTest : public CppUnit::TestFixture CPPUNIT_TEST(TestJntArray); CPPUNIT_TEST(TestRotationDiff); CPPUNIT_TEST(TestEuler); + CPPUNIT_TEST(TestGetRotAngle); CPPUNIT_TEST_SUITE_END(); public: @@ -35,6 +36,7 @@ class FramesTest : public CppUnit::TestFixture void TestJntArrayWhenEmpty(); void TestRotationDiff(); void TestEuler(); + void TestGetRotAngle(); private: void TestVector2(Vector& v); From 2528a01855183ed9f5e5829477393440dab8173d Mon Sep 17 00:00:00 2001 From: Sjoerd van den Dries Date: Mon, 6 Nov 2017 11:50:36 +0100 Subject: [PATCH 06/13] tests: added test for GetRotAngle on slightly non-orthogonal matrices --- orocos_kdl/tests/framestest.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index 5eaa7c94d..4af54bee3 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -416,6 +416,18 @@ void FramesTest::TestGetRotAngle() { CPPUNIT_ASSERT(0==isnan(kdlAxis[1])); CPPUNIT_ASSERT(0==isnan(kdlAxis[2])); + // Test GetRotAngle on slightly non-orthogonal rotation matrices + { + 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, 0)", 0.0, angle, epsilon); + } + + { + 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); + } } void FramesTest::TestQuaternion() { From 865f53fd754483e545a1788a93d7ec70ae84a680 Mon Sep 17 00:00:00 2001 From: Sjoerd van den Dries Date: Mon, 6 Nov 2017 11:57:07 +0100 Subject: [PATCH 07/13] fix: GetRotAngle no longer causes NaNs on slightly non-orthogonal rotation matrices --- orocos_kdl/src/frames.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index e501182fc..ba1b6e0fe 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -29,6 +29,7 @@ #define _USE_MATH_DEFINES // For MSVC #include +#include namespace KDL { @@ -439,7 +440,11 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { return angle; // return 180 deg rotation } - angle = acos(( data[0] + data[4] + data[8] - 1)/2); + // 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]); From a866449fafe4d407568183a9f5958b137ec5aa04 Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Tue, 10 Apr 2018 11:49:21 -0400 Subject: [PATCH 08/13] 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 ba1b6e0fe..74e2dc8e0 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -391,50 +391,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 1e9f50c5e77ed614746c44193f15cc2ee433ad9c Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Wed, 28 Mar 2018 11:37:34 -0400 Subject: [PATCH 09/13] 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 4af54bee3..00860943c 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -428,6 +428,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 2c210c32b00f9b056fc394d9756d3aca4f4c9b5e Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Mon, 27 May 2019 16:23:17 +0200 Subject: [PATCH 10/13] 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 198f3ffb3..cd2b9d340 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 c42c4dc126c7902117a914cbc8a97c8cdfe56d7d Mon Sep 17 00:00:00 2001 From: Ruben Smits Date: Wed, 23 Oct 2019 16:46:35 +0200 Subject: [PATCH 11/13] 1.3.2 --- orocos_kdl/CMakeLists.txt | 2 +- orocos_kdl/package.xml | 2 +- orocos_kinematics_dynamics/package.xml | 2 +- python_orocos_kdl/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index 5cc27028c..7daeb0345 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -13,7 +13,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(orocos_kdl) -SET( KDL_VERSION 1.3.1) +SET( KDL_VERSION 1.3.2) STRING( REGEX MATCHALL "[0-9]+" KDL_VERSIONS ${KDL_VERSION} ) LIST( GET KDL_VERSIONS 0 KDL_VERSION_MAJOR) LIST( GET KDL_VERSIONS 1 KDL_VERSION_MINOR) diff --git a/orocos_kdl/package.xml b/orocos_kdl/package.xml index 6f8ee687b..8894540f7 100644 --- a/orocos_kdl/package.xml +++ b/orocos_kdl/package.xml @@ -1,6 +1,6 @@ orocos_kdl - 1.3.1 + 1.3.2 This package contains a recent version of the Kinematics and Dynamics Library (KDL), distributed by the Orocos Project. diff --git a/orocos_kinematics_dynamics/package.xml b/orocos_kinematics_dynamics/package.xml index 65a73be2b..95f17d6cc 100644 --- a/orocos_kinematics_dynamics/package.xml +++ b/orocos_kinematics_dynamics/package.xml @@ -1,6 +1,6 @@ orocos_kinematics_dynamics - 1.3.1 + 1.3.2 This package depends on a recent version of the Kinematics and Dynamics Library (KDL), distributed by the Orocos Project. It is a meta-package that diff --git a/python_orocos_kdl/package.xml b/python_orocos_kdl/package.xml index fd66f6625..d66d775ea 100644 --- a/python_orocos_kdl/package.xml +++ b/python_orocos_kdl/package.xml @@ -1,6 +1,6 @@ python_orocos_kdl - 1.3.1 + 1.3.2 This package contains the python bindings PyKDL for the Kinematics and Dynamics Library (KDL), distributed by the Orocos Project. From 02d204810d6e8404b5f27a9ca39d89fe05b72710 Mon Sep 17 00:00:00 2001 From: Xianchao Date: Fri, 8 Nov 2019 17:14:49 -0500 Subject: [PATCH 12/13] back to a more numerically stable method to compute the rotation angle --- orocos_kdl/src/frames.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index 74e2dc8e0..7ae3fb578 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -416,12 +416,12 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { // 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 713c39ab7d99f89b3a0941e594d042cd23097692 Mon Sep 17 00:00:00 2001 From: Xianchao Date: Fri, 8 Nov 2019 17:31:09 -0500 Subject: [PATCH 13/13] remove comment --- orocos_kdl/src/frames.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index 7ae3fb578..05eee3de5 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -413,8 +413,6 @@ 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; x = (data[7] - data[5]);