sandey/blender
1
0
Fork 0
forked from bartvdbraak/blender
Code Pull Requests Activity

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.

Browse Source
This commit is contained in:
Benoit Bolsee 2012-06-07 08:16:41 +00:00
parent ef850d75f5
commit 2b889eea8d
11 changed files with 68 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
intern/itasc/Armature.cpp
View 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);

2
intern/itasc/Distance.cpp
View 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);

17
intern/itasc/kdl/jntarray.cpp
View 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;

18
intern/itasc/kdl/jntarray.hpp
View 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
*
*/

26
intern/itasc/kdl/joint.cpp
View 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();

2
intern/itasc/kdl/joint.hpp
View 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
*

2
intern/itasc/kdl/kinfam_io.cpp
View 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;
}

4
intern/itasc/kdl/segment.cpp
View 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);
}

4
intern/itasc/kdl/segment.hpp
View 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

4
intern/itasc/kdl/utilities/utility.h
View File

@ -27,6 +27,10 @@
#include <cassert>
#include <cmath>
#ifdef NDEBUG
#undef assert
#define assert(e) ((void)0)
#endif
/////////////////////////////////////////////////////////////
// configurable options for the frames library.

6
source/blender/ikplugin/intern/itasc_plugin.cpp
View 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;