[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [47561] trunk/blender: Fix [#31430] part 2 : crash in iTaSC when end effector is a fixed bone.

Benoit Bolsee benoit.bolsee at online.be
Thu Jun 7 10:16:57 CEST 2012


Revision: 47561
          http://projects.blender.org/scm/viewvc.php?view=rev&root=bf-blender&revision=47561
Author:   ben2610
Date:     2012-06-07 08:16:41 +0000 (Thu, 07 Jun 2012)
Log Message:
-----------
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.

Modified Paths:
--------------
    trunk/blender/intern/itasc/Armature.cpp
    trunk/blender/intern/itasc/Distance.cpp
    trunk/blender/intern/itasc/kdl/jntarray.cpp
    trunk/blender/intern/itasc/kdl/jntarray.hpp
    trunk/blender/intern/itasc/kdl/joint.cpp
    trunk/blender/intern/itasc/kdl/joint.hpp
    trunk/blender/intern/itasc/kdl/kinfam_io.cpp
    trunk/blender/intern/itasc/kdl/segment.cpp
    trunk/blender/intern/itasc/kdl/segment.hpp
    trunk/blender/intern/itasc/kdl/utilities/utility.h
    trunk/blender/source/blender/ikplugin/intern/itasc_plugin.cpp

Modified: trunk/blender/intern/itasc/Armature.cpp
===================================================================
--- trunk/blender/intern/itasc/Armature.cpp	2012-06-07 08:04:58 UTC (rev 47560)
+++ trunk/blender/intern/itasc/Armature.cpp	2012-06-07 08:16:41 UTC (rev 47561)
@@ -158,7 +158,7 @@
 {
 	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 @@
 		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 @@
 	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 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 @@
 	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 @@
 	// 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 @@
 		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 @@
 
 	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 @@
 		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);

Modified: trunk/blender/intern/itasc/Distance.cpp
===================================================================
--- trunk/blender/intern/itasc/Distance.cpp	2012-06-07 08:04:58 UTC (rev 47560)
+++ trunk/blender/intern/itasc/Distance.cpp	2012-06-07 08:16:41 UTC (rev 47561)
@@ -189,7 +189,7 @@
 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);

Modified: trunk/blender/intern/itasc/kdl/jntarray.cpp
===================================================================
--- trunk/blender/intern/itasc/kdl/jntarray.cpp	2012-06-07 08:04:58 UTC (rev 47560)
+++ trunk/blender/intern/itasc/kdl/jntarray.cpp	2012-06-07 08:16:41 UTC (rev 47561)
@@ -71,20 +71,25 @@
         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;

Modified: trunk/blender/intern/itasc/kdl/jntarray.hpp
===================================================================
--- trunk/blender/intern/itasc/kdl/jntarray.hpp	2012-06-07 08:04:58 UTC (rev 47560)
+++ trunk/blender/intern/itasc/kdl/jntarray.hpp	2012-06-07 08:16:41 UTC (rev 47561)
@@ -107,24 +107,30 @@
 		
         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
          *
          */

Modified: trunk/blender/intern/itasc/kdl/joint.cpp
===================================================================
--- trunk/blender/intern/itasc/kdl/joint.cpp	2012-06-07 08:04:58 UTC (rev 47560)
+++ trunk/blender/intern/itasc/kdl/joint.cpp	2012-06-07 08:16:41 UTC (rev 47561)
@@ -55,37 +55,45 @@
     {
     }
 
-    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();

Modified: trunk/blender/intern/itasc/kdl/joint.hpp
===================================================================
--- trunk/blender/intern/itasc/kdl/joint.hpp	2012-06-07 08:04:58 UTC (rev 47560)
+++ trunk/blender/intern/itasc/kdl/joint.hpp	2012-06-07 08:16:41 UTC (rev 47561)
@@ -70,7 +70,7 @@
          *
          * @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
          *

Modified: trunk/blender/intern/itasc/kdl/kinfam_io.cpp
===================================================================
--- trunk/blender/intern/itasc/kdl/kinfam_io.cpp	2012-06-07 08:04:58 UTC (rev 47560)
+++ trunk/blender/intern/itasc/kdl/kinfam_io.cpp	2012-06-07 08:16:41 UTC (rev 47561)
@@ -76,7 +76,7 @@
 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;
 }

Modified: trunk/blender/intern/itasc/kdl/segment.cpp

@@ Diff output truncated at 10240 characters. @@



More information about the Bf-blender-cvs mailing list