diff --git a/ur_kinematics/include/ur_kinematics/ur_kin.h b/ur_kinematics/include/ur_kinematics/ur_kin.h index fd28291a7..09a817f15 100644 --- a/ur_kinematics/include/ur_kinematics/ur_kin.h +++ b/ur_kinematics/include/ur_kinematics/ur_kin.h @@ -38,6 +38,16 @@ #ifndef UR_KIN_H #define UR_KIN_H +#if defined UR10_PARAMS +#define UR_NAMESPACE ur10 +#elif defined UR5_PARAMS +#define UR_NAMESPACE ur5 +#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. // 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 +66,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 +82,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/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. 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