diff options
Diffstat (limited to 'intern/itasc/kdl')
| -rw-r--r-- | intern/itasc/kdl/jntarray.cpp | 17 | ||||
| -rw-r--r-- | intern/itasc/kdl/jntarray.hpp | 18 | ||||
| -rw-r--r-- | intern/itasc/kdl/joint.cpp | 26 | ||||
| -rw-r--r-- | intern/itasc/kdl/joint.hpp | 2 | ||||
| -rw-r--r-- | intern/itasc/kdl/kinfam_io.cpp | 2 | ||||
| -rw-r--r-- | intern/itasc/kdl/segment.cpp | 4 | ||||
| -rw-r--r-- | intern/itasc/kdl/segment.hpp | 4 | ||||
| -rw-r--r-- | intern/itasc/kdl/utilities/utility.h | 4 |
8 files changed, 50 insertions, 27 deletions
diff --git a/intern/itasc/kdl/jntarray.cpp b/intern/itasc/kdl/jntarray.cpp index 77c75e6af6c..db2c913a532 100644 --- a/intern/itasc/kdl/jntarray.cpp +++ b/intern/itasc/kdl/jntarray.cpp @@ -71,20 +71,25 @@ namespace KDL SetToZero(*this); } - double JntArray::operator()(unsigned int i,unsigned int j)const + double JntArray::operator[](unsigned int i)const { - assert(i<size&&j==0); - assert(0 != size); // found JntArray containing no data + assert(i<size); return data[i]; } - double& JntArray::operator()(unsigned int i,unsigned int j) + double& JntArray::operator[](unsigned int i) { - assert(i<size&&j==0); - assert(0 != size); // found JntArray containing no data + assert(i<size); return data[i]; } + double* JntArray::operator()(unsigned int i) + { + if (i>=size) + return NULL; + return &data[i]; + } + unsigned int JntArray::rows()const { return size; diff --git a/intern/itasc/kdl/jntarray.hpp b/intern/itasc/kdl/jntarray.hpp index 8db4cd6f2b3..ece6b0bdb6b 100644 --- a/intern/itasc/kdl/jntarray.hpp +++ b/intern/itasc/kdl/jntarray.hpp @@ -107,24 +107,30 @@ class MyTask : public RTT::TaskContext JntArray& operator = ( const JntArray& arg); /** - * get_item operator for the joint array, if a second value is - * given it should be zero, since a JntArray resembles a column. + * get_item operator for the joint array * * * @return the joint value at position i, starting from 0 * @pre 0 != size (ie non-default constructor or resize() called) */ - double operator()(unsigned int i,unsigned int j=0)const; + double operator[](unsigned int i) const; /** - * set_item operator, again if a second value is given it - *should be zero. + * set_item operator * * @return reference to the joint value at position i,starting *from zero. * @pre 0 != size (ie non-default constructor or resize() called) */ - double& operator()(unsigned int i,unsigned int j=0); + double& operator[](unsigned int i); /** + * access operator for the joint array. Use pointer here to allow + * access to sequential joint angles (required for ndof joints) + * + * + * @return the joint value at position i, NULL if i is outside the valid range + */ + double* operator()(unsigned int i); + /** * Returns the number of rows (size) of the array * */ diff --git a/intern/itasc/kdl/joint.cpp b/intern/itasc/kdl/joint.cpp index 5458efc4fcf..161794ddd72 100644 --- a/intern/itasc/kdl/joint.cpp +++ b/intern/itasc/kdl/joint.cpp @@ -55,37 +55,45 @@ namespace KDL { { } - Frame Joint::pose(const double& q)const + Frame Joint::pose(const double* q)const { switch(type){ case RotX: - return Frame(Rotation::RotX(scale*q+offset)); + assert(q); + return Frame(Rotation::RotX(scale*q[0]+offset)); break; case RotY: - return Frame(Rotation::RotY(scale*q+offset)); + assert(q); + return Frame(Rotation::RotY(scale*q[0]+offset)); break; case RotZ: - return Frame(Rotation::RotZ(scale*q+offset)); + assert(q); + return Frame(Rotation::RotZ(scale*q[0]+offset)); break; case TransX: - return Frame(Vector(scale*q+offset,0.0,0.0)); + assert(q); + return Frame(Vector(scale*q[0]+offset,0.0,0.0)); break; case TransY: - return Frame(Vector(0.0,scale*q+offset,0.0)); + assert(q); + return Frame(Vector(0.0,scale*q[0]+offset,0.0)); break; case TransZ: - return Frame(Vector(0.0,0.0,scale*q+offset)); + assert(q); + return Frame(Vector(0.0,0.0,scale*q[0]+offset)); break; case Sphere: // the joint angles represent a rotation vector expressed in the base frame of the joint // (= the frame you get when there is no offset nor rotation) - return Frame(Rot(Vector((&q)[0], (&q)[1], (&q)[2]))); + assert(q); + return Frame(Rot(Vector(q[0], q[1], q[2]))); break; case Swing: // the joint angles represent a 2D rotation vector in the XZ planee of the base frame of the joint // (= the frame you get when there is no offset nor rotation) - return Frame(Rot(Vector((&q)[0], 0.0, (&q)[1]))); + assert(q); + return Frame(Rot(Vector(q[0], 0.0, q[1]))); break; default: return Frame::Identity(); diff --git a/intern/itasc/kdl/joint.hpp b/intern/itasc/kdl/joint.hpp index a1291509f0f..9d25b427499 100644 --- a/intern/itasc/kdl/joint.hpp +++ b/intern/itasc/kdl/joint.hpp @@ -70,7 +70,7 @@ namespace KDL { * * @return the resulting 6D-pose */ - Frame pose(const double& q)const; + Frame pose(const double* q)const; /** * Request the resulting 6D-velocity with a joint velocity qdot * diff --git a/intern/itasc/kdl/kinfam_io.cpp b/intern/itasc/kdl/kinfam_io.cpp index 15557ab5f05..ff4cab862ce 100644 --- a/intern/itasc/kdl/kinfam_io.cpp +++ b/intern/itasc/kdl/kinfam_io.cpp @@ -76,7 +76,7 @@ std::istream& operator >>(std::istream& is, Tree& tree) { std::ostream& operator <<(std::ostream& os, const JntArray& array) { os << "["; for (unsigned int i = 0; i < array.rows(); i++) - os << std::setw(KDL_FRAME_WIDTH) << array(i); + os << std::setw(KDL_FRAME_WIDTH) << array[i]; os << "]"; return os; } diff --git a/intern/itasc/kdl/segment.cpp b/intern/itasc/kdl/segment.cpp index cba797899e1..f963559c4c8 100644 --- a/intern/itasc/kdl/segment.cpp +++ b/intern/itasc/kdl/segment.cpp @@ -48,12 +48,12 @@ namespace KDL { { } - Frame Segment::pose(const double& q)const + Frame Segment::pose(const double* q)const { return joint.pose(q)*f_tip; } - Twist Segment::twist(const double& q, const double& qdot, int dof)const + Twist Segment::twist(const double* q, const double& qdot, int dof)const { return joint.twist(qdot, dof).RefPoint(pose(q).p); } diff --git a/intern/itasc/kdl/segment.hpp b/intern/itasc/kdl/segment.hpp index 87d972ead70..130bfb13f8b 100644 --- a/intern/itasc/kdl/segment.hpp +++ b/intern/itasc/kdl/segment.hpp @@ -73,7 +73,7 @@ namespace KDL { * * @return pose from the root to the tip of the segment */ - Frame pose(const double& q)const; + Frame pose(const double* q)const; /** * Request the 6D-velocity of the tip of the segment, given * the joint position q and the joint velocity qdot. @@ -85,7 +85,7 @@ namespace KDL { *in the base-frame of the segment(root) and with the tip of *the segment as reference point. */ - Twist twist(const double& q,const double& qdot, int dof=0)const; + Twist twist(const double* q,const double& qdot, int dof=0)const; /** * Request the 6D-velocity at a given point p, relative to base frame of the segment diff --git a/intern/itasc/kdl/utilities/utility.h b/intern/itasc/kdl/utilities/utility.h index fbf9982665a..892b375d442 100644 --- a/intern/itasc/kdl/utilities/utility.h +++ b/intern/itasc/kdl/utilities/utility.h @@ -27,6 +27,10 @@ #include <cassert> #include <cmath> +#ifdef NDEBUG +#undef assert +#define assert(e) ((void)0) +#endif ///////////////////////////////////////////////////////////// // configurable options for the frames library. |