Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--intern/itasc/Armature.cpp28
-rw-r--r--intern/itasc/Distance.cpp2
-rw-r--r--intern/itasc/kdl/jntarray.cpp17
-rw-r--r--intern/itasc/kdl/jntarray.hpp18
-rw-r--r--intern/itasc/kdl/joint.cpp26
-rw-r--r--intern/itasc/kdl/joint.hpp2
-rw-r--r--intern/itasc/kdl/kinfam_io.cpp2
-rw-r--r--intern/itasc/kdl/segment.cpp4
-rw-r--r--intern/itasc/kdl/segment.hpp4
-rw-r--r--intern/itasc/kdl/utilities/utility.h4
-rw-r--r--source/blender/ikplugin/intern/itasc_plugin.cpp6
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, &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);
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;
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-30 11:17:42 +0300