From 532123fd015a04b2abc52d02457fa37a61e254a6 Mon Sep 17 00:00:00 2001 From: Andrew Price Date: Fri, 8 Jun 2018 09:53:54 -0400 Subject: [PATCH 1/3] Added #define-based namespaces to prevent linking errors. In the current setup, it is impossible to link against the ur5 or ur10 versions of the kinematics library, as all expose the same ur_kinematics::inverse() signature. Since all libraries are exported in the catkin_package() macro, this results in undefined behavior when linking. GCC just picks the first suitable signature it encounters, in this case the ur3 version, and continues without an error. Adding a namespace allows the linker to find the correct library call. --- ur_kinematics/include/ur_kinematics/ur_kin.h | 22 ++++++++++++++++---- ur_kinematics/src/ur_kin.cpp | 11 ++++++---- ur_kinematics/src/ur_moveit_plugin.cpp | 4 ++-- 3 files changed, 27 insertions(+), 10 deletions(-) diff --git a/ur_kinematics/include/ur_kinematics/ur_kin.h b/ur_kinematics/include/ur_kinematics/ur_kin.h index fd28291a7..e5577ca59 100644 --- a/ur_kinematics/include/ur_kinematics/ur_kin.h +++ b/ur_kinematics/include/ur_kinematics/ur_kin.h @@ -38,6 +38,18 @@ #ifndef UR_KIN_H #define UR_KIN_H +#ifdef UR10_PARAMS +#define UR_NAMESPACE ur10 +#endif + +#ifdef UR5_PARAMS +#define UR_NAMESPACE ur5 +#endif + +#ifdef UR3_PARAMS +#define UR_NAMESPACE ur3 +#endif + // These kinematics find the tranfrom from the base link to the end effector. // Though the raw D-H parameters specify a transform from the 0th link to the 6th link, // offset transforms are specified in this formulation. @@ -56,13 +68,14 @@ // 0, 0, 0, 1 namespace ur_kinematics { - // @param q The 6 joint values +namespace UR_NAMESPACE { + // @param q The 6 joint values // @param T The 4x4 end effector pose in row-major ordering void forward(const double* q, double* T); - // @param q The 6 joint values + // @param q The 6 joint values // @param Ti The 4x4 link i pose in row-major ordering. If NULL, nothing is stored. - void forward_all(const double* q, double* T1, double* T2, double* T3, + void forward_all(const double* q, double* T1, double* T2, double* T3, double* T4, double* T5, double* T6); // @param T The 4x4 end effector pose in row-major ordering @@ -71,6 +84,7 @@ namespace ur_kinematics { // in case of an infinite solution on that joint. // @return Number of solutions found (maximum of 8) int inverse(const double* T, double* q_sols, double q6_des=0.0); -}; +} +} #endif //UR_KIN_H diff --git a/ur_kinematics/src/ur_kin.cpp b/ur_kinematics/src/ur_kin.cpp index c1f74bd83..9474e0d91 100644 --- a/ur_kinematics/src/ur_kin.cpp +++ b/ur_kinematics/src/ur_kin.cpp @@ -44,6 +44,8 @@ namespace ur_kinematics { #endif } + namespace UR_NAMESPACE { + void forward(const double* q, double* T) { double s1 = sin(*q), c1 = cos(*q); q++; double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; @@ -348,6 +350,7 @@ namespace ur_kinematics { } return num_sols; } + } }; @@ -397,7 +400,7 @@ IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkRe to_mat44(T, eetrans, eerot); - int num_sols = ur_kinematics::inverse(T, q_sols,pfree[0]); + int num_sols = ur_kinematics::UR_NAMESPACE::inverse(T, q_sols,pfree[0]); std::vector vfree(0); @@ -412,7 +415,7 @@ IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkRe IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) { double T[16]; - ur_kinematics::forward(j,T); + ur_kinematics::UR_NAMESPACE::forward(j,T); from_mat44(T,eetrans,eerot); } @@ -435,7 +438,7 @@ int main(int argc, char* argv[]) { double q[6] = {0.0, 0.0, 1.0, 0.0, 1.0, 0.0}; double* T = new double[16]; - forward(q, T); + UR_NAMESPACE::forward(q, T); for(int i=0;i<4;i++) { for(int j=i*4;j<(i+1)*4;j++) printf("%1.3f ", T[j]); @@ -443,7 +446,7 @@ int main(int argc, char* argv[]) } double q_sols[8*6]; int num_sols; - num_sols = inverse(T, q_sols); + num_sols = UR_NAMESPACE::inverse(T, q_sols); for(int i=0;i Date: Wed, 11 Jul 2018 15:17:48 -0400 Subject: [PATCH 2/3] Added missing-#define error to enforce model specification by the user. Per https://github.com/a-price/universal_robot/pull/1 @mxgrey. --- ur_kinematics/include/ur_kinematics/ur_kin.h | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/ur_kinematics/include/ur_kinematics/ur_kin.h b/ur_kinematics/include/ur_kinematics/ur_kin.h index e5577ca59..09a817f15 100644 --- a/ur_kinematics/include/ur_kinematics/ur_kin.h +++ b/ur_kinematics/include/ur_kinematics/ur_kin.h @@ -38,16 +38,14 @@ #ifndef UR_KIN_H #define UR_KIN_H -#ifdef UR10_PARAMS +#if defined UR10_PARAMS #define UR_NAMESPACE ur10 -#endif - -#ifdef UR5_PARAMS +#elif defined UR5_PARAMS #define UR_NAMESPACE ur5 -#endif - -#ifdef UR3_PARAMS +#elif defined UR3_PARAMS #define UR_NAMESPACE ur3 +#else +#error "You must #define which UR model you wish to use. Options are { UR10_PARAMS, UR5_PARAMS, UR3_PARAMS }." #endif // These kinematics find the tranfrom from the base link to the end effector. From 4fec2b680ce37efe2ebedccf1c0122b2e5fdde43 Mon Sep 17 00:00:00 2001 From: Andrew Price Date: Wed, 11 Jul 2018 15:21:10 -0400 Subject: [PATCH 3/3] Advanced package version on ABI-breaking change. Per ROS REP 9 http://www.ros.org/reps/rep-0009.html --- ur_kinematics/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_kinematics/package.xml b/ur_kinematics/package.xml index 7bd4ce6c5..b683dba28 100644 --- a/ur_kinematics/package.xml +++ b/ur_kinematics/package.xml @@ -1,7 +1,7 @@ ur_kinematics - 1.2.1 + 1.3.0 Provides forward and inverse kinematics for Universal Robots designs. See http://hdl.handle.net/1853/50782 for details.