forked from bartvdbraak/blender
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.
This commit is contained in:
parent
ef850d75f5
commit
2b889eea8d
@ -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);
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
*
|
||||
*/
|
||||
|
@ -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();
|
||||
|
@ -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
|
||||
*
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -27,6 +27,10 @@
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
|
||||
#ifdef NDEBUG
|
||||
#undef assert
|
||||
#define assert(e) ((void)0)
|
||||
#endif
|
||||
|
||||
/////////////////////////////////////////////////////////////
|
||||
// configurable options for the frames library.
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user