Fix [#31430] part 2: crash in iTaSC when end effector is a fixed bone. This situation...
authorBenoit Bolsee <benoit.bolsee@online.be>
Thu, 7 Jun 2012 08:16:41 +0000 (08:16 +0000)
committerBenoit Bolsee <benoit.bolsee@online.be>
Thu, 7 Jun 2012 08:16:41 +0000 (08:16 +0000)
intern/itasc/Armature.cpp
intern/itasc/Distance.cpp
intern/itasc/kdl/jntarray.cpp
intern/itasc/kdl/jntarray.hpp
intern/itasc/kdl/joint.cpp
intern/itasc/kdl/joint.hpp
intern/itasc/kdl/kinfam_io.cpp
intern/itasc/kdl/segment.cpp
intern/itasc/kdl/segment.hpp
intern/itasc/kdl/utilities/utility.h
source/blender/ikplugin/intern/itasc_plugin.cpp

index dd5c1921a98ca716d25648e1e9edd5b653bed571..1dacb8bc184069309198e277b7c01021ed6cd929 100644 (file)
@@ -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);
index 7cf04367a4ee16f12fdcd3cf2d905872ff7cdaf9..c9efca101e35c52c2535318536b1d7850c0865c7 100644 (file)
@@ -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);
index 77c75e6af6c4f66f98e9b86df33324a4463b024e..db2c913a532fec5e4dbf701371fb3eb49ff8fa74 100644 (file)
@@ -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;
index 8db4cd6f2b32ee7ef26623d52242ddae203e800b..ece6b0bdb6b5dc0859729fc8bf03fe70b636afc0 100644 (file)
@@ -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
          *
          */
index 5458efc4fcf58111ee6b41f69285188648357cf3..161794ddd72dcab540d7c7df9cc61841c672053e 100644 (file)
@@ -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();
index a1291509f0fa051abd3bbaf6835f393f82ec7f53..9d25b4274999da73d06cbc845e51aa9f04d188f4 100644 (file)
@@ -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
          *
index 15557ab5f05fe6bb484cc2110f086b49d3bbfe6b..ff4cab862ce6915084f11bfaf038023179555351 100644 (file)
@@ -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;
 }
index cba797899e15a8dcfbd9919c4fd2246d59279362..f963559c4c8d91d82376328cbc4f65af2adf961f 100644 (file)
@@ -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);
     }
index 87d972ead705a5d8d9059e46a293c61020697dc1..130bfb13f8b0a4a41bed1ab1787846ea66008e58 100644 (file)
@@ -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
index fbf9982665a2f43e00b4e5ed0cdf51aab3a758ce..892b375d4428f4150106e0edbbcb76450f513f31 100644 (file)
 #include <cassert>
 #include <cmath>
 
+#ifdef NDEBUG
+#undef assert
+#define assert(e) ((void)0)
+#endif
 
 /////////////////////////////////////////////////////////////
 // configurable options for the frames library.
index ebbb201de8e2c4308406715ff0b39bd376930e89..048dd9557265958df18938fa2953d8e32b0326ff 100644 (file)
@@ -1000,7 +1000,7 @@ static void convert_pose(IK_Scene *ikscene)
 
        // assume uniform scaling and take Y scale as general scale for the armature
        scale = len_v3(ikscene->blArmature->obmat[1]);
-       rot = (ikscene->jointArray.rows() > 0) ? &ikscene->jointArray(0) : NULL;
+       rot = ikscene->jointArray(0);
        for (joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) {
                pchan= ikchan->pchan;
                bone= pchan->bone;
@@ -1041,7 +1041,7 @@ static void BKE_pose_rest(IK_Scene *ikscene)
        // rest pose is 0 
        SetToZero(ikscene->jointArray);
        // except for transY joints
-       rot = (ikscene->jointArray.rows() > 0) ? &ikscene->jointArray(0) : NULL;
+       rot = ikscene->jointArray(0);
        for (joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) {
                pchan= ikchan->pchan;
                bone= pchan->bone;
@@ -1140,7 +1140,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
                // in Blender, the rest pose is always 0 for joints
                BKE_pose_rest(ikscene);
        }
-       rot = (ikscene->jointArray.rows() > 0) ? &ikscene->jointArray(0) : NULL;
+       rot = ikscene->jointArray(0);
        for (a=0, ikchan = ikscene->channels; a<tree->totchannel; ++a, ++ikchan) {
                pchan= ikchan->pchan;
                bone= pchan->bone;