diff options
author | Tamito Kajiyama <rd6t-kjym@asahi-net.or.jp> | 2012-06-11 00:50:43 +0400 |
---|---|---|
committer | Tamito Kajiyama <rd6t-kjym@asahi-net.or.jp> | 2012-06-11 00:50:43 +0400 |
commit | 8135cc9f954e0d63ab3e97d4a7c52ff5e573eef0 (patch) | |
tree | a12ec0daccfc45b7e3c68e4a2d7099655daf619d /intern/itasc | |
parent | 0f33d5719fd0adc666e7e92e0f062281f4285f13 (diff) | |
parent | 298feff39006c14aa28b5e0232aa7ed70a83a496 (diff) |
Merged changes in the trunk up to revision 47700.
Conflicts resolved:
source/blender/blenkernel/BKE_main.h
source/blender/blenkernel/CMakeLists.txt
source/blender/blenkernel/intern/library.c
source/blender/blenloader/intern/readfile.c
source/blender/blenloader/intern/writefile.c
source/blender/editors/interface/resources.c
source/blender/makesdna/DNA_ID.h
source/blender/makesdna/DNA_action_types.h
source/blender/makesdna/intern/makesdna.c
source/blender/makesrna/SConscript
source/blender/makesrna/intern/rna_internal.h
source/blender/makesrna/intern/rna_main.c
source/blender/makesrna/intern/rna_main_api.c
source/blender/windowmanager/WM_types.h
Diffstat (limited to 'intern/itasc')
-rw-r--r-- | intern/itasc/Armature.cpp | 35 | ||||
-rw-r--r-- | intern/itasc/Armature.hpp | 2 | ||||
-rw-r--r-- | intern/itasc/CMakeLists.txt | 3 | ||||
-rw-r--r-- | intern/itasc/Distance.cpp | 2 | ||||
-rw-r--r-- | intern/itasc/FixedObject.cpp | 5 | ||||
-rw-r--r-- | intern/itasc/FixedObject.hpp | 2 | ||||
-rw-r--r-- | intern/itasc/MovingFrame.cpp | 3 | ||||
-rw-r--r-- | intern/itasc/MovingFrame.hpp | 2 | ||||
-rw-r--r-- | intern/itasc/Object.hpp | 2 | ||||
-rw-r--r-- | intern/itasc/SConscript | 3 | ||||
-rw-r--r-- | intern/itasc/Scene.cpp | 3 | ||||
-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 |
19 files changed, 81 insertions, 58 deletions
diff --git a/intern/itasc/Armature.cpp b/intern/itasc/Armature.cpp index 916b0bc7bf3..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; } @@ -369,11 +369,13 @@ int Armature::addEndEffector(const std::string& name) return m_neffector++; } -void Armature::finalize() +bool Armature::finalize() { unsigned int i, j, c; if (m_finalized) - return; + return true; + if (m_njoint == 0) + return false; initialize(m_njoint, m_noutput, m_neffector); for (i=c=0; i<m_nconstraint; i++) { JointConstraint_struct* pConstraint = m_constraints[i]; @@ -390,7 +392,7 @@ void 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 @@ -410,6 +412,7 @@ void Armature::finalize() if (m_armlength < KDL::epsilon) m_armlength = KDL::epsilon; m_finalized = true; + return true; } void Armature::pushCache(const Timestamp& timestamp) @@ -444,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]; @@ -621,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. @@ -674,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; } @@ -693,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/Armature.hpp b/intern/itasc/Armature.hpp index 0f4abeab4f0..63d1899f4e9 100644 --- a/intern/itasc/Armature.hpp +++ b/intern/itasc/Armature.hpp @@ -31,7 +31,7 @@ public: bool getSegment(const std::string& segment_name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip); bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root); - virtual void finalize(); + virtual bool finalize(); virtual int addEndEffector(const std::string& name); virtual const Frame& getPose(const unsigned int end_effector); diff --git a/intern/itasc/CMakeLists.txt b/intern/itasc/CMakeLists.txt index 3d24a0cb8c6..f4bc0326ea1 100644 --- a/intern/itasc/CMakeLists.txt +++ b/intern/itasc/CMakeLists.txt @@ -318,8 +318,5 @@ set(SRC ../../extern/Eigen3/Eigen/src/Cholesky/LLT.h ) -if(WIN32) - add_definitions(-DEIGEN_DONT_ALIGN_STATICALLY) -endif() blender_add_lib(bf_intern_itasc "${SRC}" "${INC}" "${INC_SYS}") 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/FixedObject.cpp b/intern/itasc/FixedObject.cpp index 9dc4d69878a..2a1e66f9c6f 100644 --- a/intern/itasc/FixedObject.cpp +++ b/intern/itasc/FixedObject.cpp @@ -53,12 +53,13 @@ int FixedObject::addEndEffector(const std::string& name) return -1; } -void FixedObject::finalize() +bool FixedObject::finalize() { if (m_finalized) - return; + return true; initialize(0, m_nframe); m_finalized = true; + return true; } const Frame& FixedObject::getPose(const unsigned int frameIndex) diff --git a/intern/itasc/FixedObject.hpp b/intern/itasc/FixedObject.hpp index b4279d9adcb..ad26e7cb2d6 100644 --- a/intern/itasc/FixedObject.hpp +++ b/intern/itasc/FixedObject.hpp @@ -23,7 +23,7 @@ public: virtual void updateCoordinates(const Timestamp& timestamp) {}; virtual int addEndEffector(const std::string& name); - virtual void finalize(); + virtual bool finalize(); virtual const Frame& getPose(const unsigned int frameIndex); virtual void updateKinematics(const Timestamp& timestamp) {}; virtual void pushCache(const Timestamp& timestamp) {}; diff --git a/intern/itasc/MovingFrame.cpp b/intern/itasc/MovingFrame.cpp index 3b0ee7842ca..90ebe091eb5 100644 --- a/intern/itasc/MovingFrame.cpp +++ b/intern/itasc/MovingFrame.cpp @@ -27,9 +27,10 @@ MovingFrame::~MovingFrame() { } -void MovingFrame::finalize() +bool MovingFrame::finalize() { updateJacobian(); + return true; } void MovingFrame::initCache(Cache *_cache) diff --git a/intern/itasc/MovingFrame.hpp b/intern/itasc/MovingFrame.hpp index 21cb344980a..d2a956d7312 100644 --- a/intern/itasc/MovingFrame.hpp +++ b/intern/itasc/MovingFrame.hpp @@ -28,7 +28,7 @@ public: virtual void updateKinematics(const Timestamp& timestamp); virtual void pushCache(const Timestamp& timestamp); virtual void initCache(Cache *_cache); - virtual void finalize(); + virtual bool finalize(); protected: virtual void updateJacobian(); diff --git a/intern/itasc/Object.hpp b/intern/itasc/Object.hpp index 5aa4346fc4b..bf80d83e5aa 100644 --- a/intern/itasc/Object.hpp +++ b/intern/itasc/Object.hpp @@ -33,7 +33,7 @@ public: virtual ~Object(){}; virtual int addEndEffector(const std::string& name){return 0;}; - virtual void finalize(){}; + virtual bool finalize(){return true;}; virtual const KDL::Frame& getPose(const unsigned int end_effector=0){return m_internalPose;}; virtual const ObjectType getType(){return m_type;}; virtual const unsigned int getNrOfCoordinates(){return 0;}; diff --git a/intern/itasc/SConscript b/intern/itasc/SConscript index 69dddf40228..c1ad931c665 100644 --- a/intern/itasc/SConscript +++ b/intern/itasc/SConscript @@ -9,8 +9,5 @@ incs = '. ../../extern/Eigen3' defs = [] -if env['PLATFORM'] == 'win32': - defs.append('EIGEN_DONT_ALIGN_STATICALLY') - env.BlenderLib ('bf_intern_itasc', sources, Split(incs), defs, libtype=['intern','player'], priority=[20,100] ) diff --git a/intern/itasc/Scene.cpp b/intern/itasc/Scene.cpp index 16f8551bfc7..877cd883208 100644 --- a/intern/itasc/Scene.cpp +++ b/intern/itasc/Scene.cpp @@ -91,7 +91,8 @@ bool Scene::setParam(SceneParam paramId, double value) bool Scene::addObject(const std::string& name, Object* object, UncontrolledObject* base, const std::string& baseFrame) { // finalize the object before adding - object->finalize(); + if (!object->finalize()) + return false; //Check if Object is controlled or uncontrolled. if(object->getType()==Object::Controlled){ int baseFrameIndex = base->addEndEffector(baseFrame); 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. |