Fix [#31430] part 2: crash in iTaSC when end effector is a fixed bone. This situation was causing access to invalid index in the joint angle array although the end effector doesn't need any joint angle to compute its pause. Fixed this by changing the internal API of joint array: return pointer instead of reference so that NULL pointer can be returned instead of crashing when the index is invalid.

10 files changed, 65 insertions, 42 deletions

diff --git a/intern/itasc/Armature.cpp b/intern/itasc/Armature.cpp
index dd5c1921a98..1dacb8bc184 100644
--- a/intern/itasc/Armature.cpp
+++ b/intern/itasc/Armature.cpp
@@ -158,7 +158,7 @@ void Armature::pushQ(CacheTS timestamp)
 {
 	if (m_qCCh >= 0) {
 		// try to keep the cache if the joints are the same
-		m_cache->addCacheVectorIfDifferent(this, m_qCCh, timestamp, &m_qKdl(0), m_qKdl.rows(), KDL::epsilon);
+		m_cache->addCacheVectorIfDifferent(this, m_qCCh, timestamp, m_qKdl(0), m_qKdl.rows(), KDL::epsilon);
 		m_qCTs = timestamp;
 	}
 }
@@ -170,8 +170,8 @@ bool Armature::popQ(CacheTS timestamp)
 		double* item;
 		item = (double*)m_cache->getPreviousCacheItem(this, m_qCCh, &timestamp);
 		if (item && m_qCTs != timestamp) {
-			double& q = m_qKdl(0);
-			memcpy(&q, item, m_qKdl.rows()*sizeof(q));
+			double* q = m_qKdl(0);
+			memcpy(q, item, m_qKdl.rows()*sizeof(double));
 			m_qCTs = timestamp;
 			// changing the joint => recompute the jacobian
 			updateJacobian();
@@ -255,7 +255,7 @@ bool Armature::getSegment(const std::string& name, const unsigned int q_size, co
 	p_tip = &sit->second.segment.getFrameToTip();
 	for (unsigned int dof=0; dof<p_joint->getNDof(); dof++) {
 		(&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest;
-		(&q)[dof] = m_qKdl(sit->second.q_nr+dof);
+		(&q)[dof] = m_qKdl[sit->second.q_nr+dof];
 	}
 	return true;
 }
@@ -267,7 +267,7 @@ double Armature::getMaxJointChange()
 	double maxJoint = 0.0;
 	for (unsigned int i=0; i<m_njoint; i++) {
 		// this is a very rough calculation, it doesn't work well for spherical joint
-		double joint = fabs(m_oldqKdl(i)-m_qKdl(i));
+		double joint = fabs(m_oldqKdl[i]-m_qKdl[i]);
 		if (maxJoint < joint)
 			maxJoint = joint;
 	}
@@ -392,7 +392,7 @@ bool Armature::finalize()
 	m_newqKdl.resize(m_njoint);
 	m_qdotKdl.resize(m_njoint);
 	for (i=0; i<m_njoint; i++) {
-		m_newqKdl(i) = m_oldqKdl(i) = m_qKdl(i) = m_joints[i].rest;
+		m_newqKdl[i] = m_oldqKdl[i] = m_qKdl[i] = m_joints[i].rest;
 	}
 	updateJacobian();
 	// estimate the maximum size of the robot arms
@@ -447,15 +447,15 @@ bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callba
 	// integration and joint limit
 	// for spherical joint we must use a more sophisticated method
 	unsigned int q_nr;
-	double* qdot=&m_qdotKdl(0);
-	double* q=&m_qKdl(0);
-	double* newq=&m_newqKdl(0);
+	double* qdot=m_qdotKdl(0);
+	double* q=m_qKdl(0);
+	double* newq=m_newqKdl(0);
 	double norm, qx, qz, CX, CZ, sx, sz;
 	bool locked = false;
 	int unlocked = 0;
 
 	for (q_nr=0; q_nr<m_nq; ++q_nr)
-		m_qdotKdl(q_nr)=m_qdot(q_nr);
+		qdot[q_nr]=m_qdot[q_nr];
 
 	for (q_nr=0; q_nr<m_nq; ) {
 		Joint_struct* joint = &m_joints[q_nr];
@@ -624,7 +624,7 @@ void Armature::updateKinematics(const Timestamp& timestamp){
 		return;
 
 	// the new joint value have been computed already, just copy
-	memcpy(&m_qKdl(0), &m_newqKdl(0), sizeof(double)*m_qKdl.rows());
+	memcpy(m_qKdl(0), m_newqKdl(0), sizeof(double)*m_qKdl.rows());
 	pushCache(timestamp);
 	updateJacobian();
 	// here update the desired output.
@@ -677,7 +677,7 @@ void Armature::updateControlOutput(const Timestamp& timestamp)
 
 	if (!timestamp.substep) {
 		// save previous joint state for getMaxJointChange()
-		memcpy(&m_oldqKdl(0), &m_qKdl(0), sizeof(double)*m_qKdl.rows());
+		memcpy(m_oldqKdl(0), m_qKdl(0), sizeof(double)*m_qKdl.rows());
 		for (unsigned int i=0; i<m_neffector; i++) {
 			m_effectors[i].oldpose = m_effectors[i].pose;
 		}
@@ -696,8 +696,8 @@ void Armature::updateControlOutput(const Timestamp& timestamp)
 		JointConstraint_struct* pConstraint = *it;
 		unsigned int nr, i;
 		for (i=0, nr = pConstraint->segment->second.q_nr; i<pConstraint->v_nr; i++, nr++) {
-			*(double*)&pConstraint->value[i].y = m_qKdl(nr);
-			*(double*)&pConstraint->value[i].ydot = m_qdotKdl(nr);
+			*(double*)&pConstraint->value[i].y = m_qKdl[nr];
+			*(double*)&pConstraint->value[i].ydot = m_qdotKdl[nr];
 		}
 		if (pConstraint->function && (pConstraint->substep || (!timestamp.reiterate && !timestamp.substep))) {
 			(*pConstraint->function)(timestamp, pConstraint->values, pConstraint->v_nr, pConstraint->param);
diff --git a/intern/itasc/Distance.cpp b/intern/itasc/Distance.cpp
index 7cf04367a4e..c9efca101e3 100644
--- a/intern/itasc/Distance.cpp
+++ b/intern/itasc/Distance.cpp
@@ -189,7 +189,7 @@ void Distance::updateKinematics(const Timestamp& timestamp)
 void Distance::updateJacobian()
 {
     for(unsigned int i=0;i<6;i++)
-        m_chiKdl(i)=m_chi(i);
+        m_chiKdl[i]=m_chi[i];
 
     m_fksolver->JntToCart(m_chiKdl,m_internalPose);
     m_jacsolver->JntToJac(m_chiKdl,m_jac);
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.