diff options
author | Benoit Bolsee <benoit.bolsee@online.be> | 2012-06-07 12:16:41 +0400 |
---|---|---|
committer | Benoit Bolsee <benoit.bolsee@online.be> | 2012-06-07 12:16:41 +0400 |
commit | 2b889eea8d1e43e46c9a1dfb2ab3a253eab60e55 (patch) | |
tree | 020c97f7c11fa09938a29812215e05194ed7fa03 | |
parent | ef850d75f52726e9d479cd9873ec8b6343cdf3f2 (diff) |
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.
-rw-r--r-- | intern/itasc/Armature.cpp | 28 | ||||
-rw-r--r-- | intern/itasc/Distance.cpp | 2 | ||||
-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 | ||||
-rw-r--r-- | source/blender/ikplugin/intern/itasc_plugin.cpp | 6 |
11 files changed, 68 insertions, 45 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, ×tamp); 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. diff --git a/source/blender/ikplugin/intern/itasc_plugin.cpp b/source/blender/ikplugin/intern/itasc_plugin.cpp index ebbb201de8e..048dd955726 100644 --- a/source/blender/ikplugin/intern/itasc_plugin.cpp +++ b/source/blender/ikplugin/intern/itasc_plugin.cpp @@ -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; |