[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [21579] branches/itasc/intern: iTaSC 2.5: add iTaSC library.
Benoit Bolsee
benoit.bolsee at online.be
Tue Jul 14 17:36:21 CEST 2009
Revision: 21579
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=21579
Author: ben2610
Date: 2009-07-14 17:36:21 +0200 (Tue, 14 Jul 2009)
Log Message:
-----------
iTaSC 2.5: add iTaSC library.
Modified Paths:
--------------
branches/itasc/intern/CMakeLists.txt
branches/itasc/intern/Makefile
branches/itasc/intern/SConscript
Added Paths:
-----------
branches/itasc/intern/itasc/
branches/itasc/intern/itasc/Armature.cpp
branches/itasc/intern/itasc/Armature.hpp
branches/itasc/intern/itasc/CMakeLists.txt
branches/itasc/intern/itasc/Cache.cpp
branches/itasc/intern/itasc/Cache.hpp
branches/itasc/intern/itasc/ConstraintSet.cpp
branches/itasc/intern/itasc/ConstraintSet.hpp
branches/itasc/intern/itasc/ControlledObject.cpp
branches/itasc/intern/itasc/ControlledObject.hpp
branches/itasc/intern/itasc/CopyPose.cpp
branches/itasc/intern/itasc/CopyPose.hpp
branches/itasc/intern/itasc/Distance.cpp
branches/itasc/intern/itasc/Distance.hpp
branches/itasc/intern/itasc/FixedObject.cpp
branches/itasc/intern/itasc/FixedObject.hpp
branches/itasc/intern/itasc/Makefile
branches/itasc/intern/itasc/MovingFrame.cpp
branches/itasc/intern/itasc/MovingFrame.hpp
branches/itasc/intern/itasc/Object.hpp
branches/itasc/intern/itasc/SConscript
branches/itasc/intern/itasc/Scene.cpp
branches/itasc/intern/itasc/Scene.hpp
branches/itasc/intern/itasc/Solver.hpp
branches/itasc/intern/itasc/UncontrolledObject.cpp
branches/itasc/intern/itasc/UncontrolledObject.hpp
branches/itasc/intern/itasc/WDLSSolver.cpp
branches/itasc/intern/itasc/WDLSSolver.hpp
branches/itasc/intern/itasc/WSDLSSolver.cpp
branches/itasc/intern/itasc/WSDLSSolver.hpp
branches/itasc/intern/itasc/WorldObject.cpp
branches/itasc/intern/itasc/WorldObject.hpp
branches/itasc/intern/itasc/eigen_types.cpp
branches/itasc/intern/itasc/eigen_types.hpp
branches/itasc/intern/itasc/kdl/
branches/itasc/intern/itasc/kdl/Makefile
branches/itasc/intern/itasc/kdl/chain.cpp
branches/itasc/intern/itasc/kdl/chain.hpp
branches/itasc/intern/itasc/kdl/chainfksolver.hpp
branches/itasc/intern/itasc/kdl/chainfksolverpos_recursive.cpp
branches/itasc/intern/itasc/kdl/chainfksolverpos_recursive.hpp
branches/itasc/intern/itasc/kdl/chainjnttojacsolver.cpp
branches/itasc/intern/itasc/kdl/chainjnttojacsolver.hpp
branches/itasc/intern/itasc/kdl/frameacc.cpp
branches/itasc/intern/itasc/kdl/frameacc.hpp
branches/itasc/intern/itasc/kdl/frameacc.inl
branches/itasc/intern/itasc/kdl/frames.cpp
branches/itasc/intern/itasc/kdl/frames.hpp
branches/itasc/intern/itasc/kdl/frames.inl
branches/itasc/intern/itasc/kdl/frames_io.cpp
branches/itasc/intern/itasc/kdl/frames_io.hpp
branches/itasc/intern/itasc/kdl/framevel.cpp
branches/itasc/intern/itasc/kdl/framevel.hpp
branches/itasc/intern/itasc/kdl/framevel.inl
branches/itasc/intern/itasc/kdl/inertia.cpp
branches/itasc/intern/itasc/kdl/inertia.hpp
branches/itasc/intern/itasc/kdl/jacobian.cpp
branches/itasc/intern/itasc/kdl/jacobian.hpp
branches/itasc/intern/itasc/kdl/jntarray.cpp
branches/itasc/intern/itasc/kdl/jntarray.hpp
branches/itasc/intern/itasc/kdl/jntarrayacc.cpp
branches/itasc/intern/itasc/kdl/jntarrayacc.hpp
branches/itasc/intern/itasc/kdl/jntarrayvel.cpp
branches/itasc/intern/itasc/kdl/jntarrayvel.hpp
branches/itasc/intern/itasc/kdl/joint.cpp
branches/itasc/intern/itasc/kdl/joint.hpp
branches/itasc/intern/itasc/kdl/kinfam_io.cpp
branches/itasc/intern/itasc/kdl/kinfam_io.hpp
branches/itasc/intern/itasc/kdl/segment.cpp
branches/itasc/intern/itasc/kdl/segment.hpp
branches/itasc/intern/itasc/kdl/tree.cpp
branches/itasc/intern/itasc/kdl/tree.hpp
branches/itasc/intern/itasc/kdl/treefksolver.hpp
branches/itasc/intern/itasc/kdl/treefksolverpos_recursive.cpp
branches/itasc/intern/itasc/kdl/treefksolverpos_recursive.hpp
branches/itasc/intern/itasc/kdl/treejnttojacsolver.cpp
branches/itasc/intern/itasc/kdl/treejnttojacsolver.hpp
branches/itasc/intern/itasc/kdl/utilities/
branches/itasc/intern/itasc/kdl/utilities/Makefile
branches/itasc/intern/itasc/kdl/utilities/error.h
branches/itasc/intern/itasc/kdl/utilities/error_stack.cpp
branches/itasc/intern/itasc/kdl/utilities/error_stack.h
branches/itasc/intern/itasc/kdl/utilities/kdl-config.h
branches/itasc/intern/itasc/kdl/utilities/rall1d.h
branches/itasc/intern/itasc/kdl/utilities/rall2d.h
branches/itasc/intern/itasc/kdl/utilities/svd_eigen_HH.hpp
branches/itasc/intern/itasc/kdl/utilities/traits.h
branches/itasc/intern/itasc/kdl/utilities/utility.cpp
branches/itasc/intern/itasc/kdl/utilities/utility.h
branches/itasc/intern/itasc/kdl/utilities/utility_io.cpp
branches/itasc/intern/itasc/kdl/utilities/utility_io.h
branches/itasc/intern/itasc/make/
branches/itasc/intern/itasc/make/msvc_9_0/
branches/itasc/intern/itasc/make/msvc_9_0/itasc.vcproj
branches/itasc/intern/itasc/ublas_types.hpp
Modified: branches/itasc/intern/CMakeLists.txt
===================================================================
--- branches/itasc/intern/CMakeLists.txt 2009-07-14 15:34:14 UTC (rev 21578)
+++ branches/itasc/intern/CMakeLists.txt 2009-07-14 15:36:21 UTC (rev 21579)
@@ -33,6 +33,7 @@
ADD_SUBDIRECTORY(memutil)
ADD_SUBDIRECTORY(decimation)
ADD_SUBDIRECTORY(iksolver)
+ADD_SUBDIRECTORY(itasc)
ADD_SUBDIRECTORY(boolop)
ADD_SUBDIRECTORY(opennl)
Modified: branches/itasc/intern/Makefile
===================================================================
--- branches/itasc/intern/Makefile 2009-07-14 15:34:14 UTC (rev 21578)
+++ branches/itasc/intern/Makefile 2009-07-14 15:36:21 UTC (rev 21579)
@@ -32,7 +32,7 @@
# include nan_subdirs.mk
ALLDIRS = string ghost guardedalloc moto container memutil
-ALLDIRS += decimation iksolver bsp SoundSystem opennl elbeem boolop
+ALLDIRS += decimation iksolver itasc bsp SoundSystem opennl elbeem boolop
all::
@for i in $(ALLDIRS); do \
Modified: branches/itasc/intern/SConscript
===================================================================
--- branches/itasc/intern/SConscript 2009-07-14 15:34:14 UTC (rev 21578)
+++ branches/itasc/intern/SConscript 2009-07-14 15:36:21 UTC (rev 21579)
@@ -10,6 +10,7 @@
'memutil/SConscript/',
'decimation/SConscript',
'iksolver/SConscript',
+ 'itasc/SConscript',
'boolop/SConscript',
'opennl/SConscript'])
Added: branches/itasc/intern/itasc/Armature.cpp
===================================================================
--- branches/itasc/intern/itasc/Armature.cpp (rev 0)
+++ branches/itasc/intern/itasc/Armature.cpp 2009-07-14 15:36:21 UTC (rev 21579)
@@ -0,0 +1,550 @@
+/* $Id: Armature.cpp 21152 2009-06-25 11:57:19Z ben2610 $
+ * Armature.cpp
+ *
+ * Created on: Feb 3, 2009
+ * Author: benoitbolsee
+ */
+
+#include "Armature.hpp"
+#include <algorithm>
+#include <malloc.h>
+#include <string.h>
+
+namespace iTaSC {
+
+// a joint constraint is characterized by 5 values: tolerance, K, alpha, yd, yddot
+static const unsigned int constraintCacheSize = 5;
+std::string Armature::m_root = "root";
+
+Armature::Armature():
+ ControlledObject(),
+ m_tree(),
+ m_njoint(0),
+ m_nconstraint(0),
+ m_neffector(0),
+ m_finalized(false),
+ m_cache(NULL),
+ m_buf(NULL),
+ m_qCCh(-1),
+ m_qCTs(0),
+ m_yCCh(-1),
+ m_yCTs(0),
+ m_qKdl(),
+ m_oldqKdl(),
+ m_qdotKdl(),
+ m_jac(NULL),
+ m_jacsolver(NULL),
+ m_fksolver(NULL)
+{
+}
+
+Armature::~Armature()
+{
+ if (m_jac)
+ delete m_jac;
+ if (m_jacsolver)
+ delete m_jacsolver;
+ if (m_fksolver)
+ delete m_fksolver;
+ for (JointConstraintList::iterator it=m_constraints.begin(); it != m_constraints.end(); it++) {
+ if (*it != NULL)
+ delete (*it);
+ }
+ if (m_buf)
+ delete [] m_buf;
+ m_constraints.clear();
+}
+
+void Armature::initCache(Cache *_cache)
+{
+ m_cache = _cache;
+ m_qCCh = -1;
+ m_yCCh = -1;
+ m_buf = NULL;
+ if (m_cache) {
+ // add a special channel for the joint
+ m_qCCh = m_cache->addChannel(this, "q", m_qKdl.rows()*sizeof(double));
+ // for the constraints, instead of creating many different channels, we will
+ // create a single channel for all the constraints
+ if (m_nconstraint) {
+ m_yCCh = m_cache->addChannel(this, "y", m_nconstraint*constraintCacheSize*sizeof(double));
+ }
+ if (m_nconstraint)
+ m_buf = new double[m_nconstraint*constraintCacheSize];
+ // store the initial cache position at timestamp 0
+ pushQ(0);
+ pushConstraints(0);
+ }
+}
+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_qCTs = timestamp;
+ }
+}
+
+/* return true if a m_cache position was loaded */
+bool Armature::popQ(CacheTS timestamp)
+{
+ if (m_qCCh >= 0) {
+ 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));
+ m_qCTs = timestamp;
+ // changing the joint => recompute the jacobian
+ updateJacobian();
+ }
+ return (item) ? true : false;
+ }
+ return true;
+}
+
+void Armature::pushConstraints(CacheTS timestamp)
+{
+ if (m_yCCh >= 0) {
+ double *buf = NULL;
+ if (m_nconstraint) {
+ double *item = m_buf;
+ for (unsigned int i=0; i<m_nconstraint; i++) {
+ JointConstraint_struct* pConstraint = m_constraints[i];
+ *item++ = pConstraint->values.feedback;
+ *item++ = pConstraint->values.tolerance;
+ *item++ = pConstraint->value.yd;
+ *item++ = pConstraint->value.yddot;
+ *item++ = pConstraint->values.alpha;
+ }
+ }
+ m_cache->addCacheVectorIfDifferent(this, m_yCCh, timestamp, m_buf, m_nconstraint*constraintCacheSize, KDL::epsilon);
+ m_yCTs = timestamp;
+ }
+}
+
+/* return true if a cache position was loaded */
+bool Armature::popConstraints(CacheTS timestamp)
+{
+ if (m_yCCh >= 0) {
+ double *item = (double*)m_cache->getPreviousCacheItem(this, m_yCCh, ×tamp);
+ if (item && m_yCTs != timestamp) {
+ for (unsigned int i=0; i<m_nconstraint; i++) {
+ JointConstraint_struct* pConstraint = m_constraints[i];
+ pConstraint->values.feedback = *item++;
+ pConstraint->values.tolerance = *item++;
+ pConstraint->value.yd = *item++;
+ pConstraint->value.yddot = *item++;
+ pConstraint->values.alpha = *item++;
+ }
+ m_yCTs = timestamp;
+ }
+ return (item) ? true : false;
+ }
+ return true;
+}
+
+bool Armature::addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip, const Inertia& M)
+{
+ if (m_finalized)
+ return false;
+
+ Segment segment(joint, f_tip, M);
+ if (!m_tree.addSegment(segment, segment_name, hook_name))
+ return false;
+ int ndof = joint.getNDof();
+ for (int dof=0; dof<ndof; dof++) {
+ Joint_struct js((&q_rest)[dof], joint.getType(), ndof);
+ m_joints.push_back(js);
+ }
+ m_njoint+=ndof;
+ return true;
+}
+
+bool Armature::getSegment(const std::string& name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip)
+{
+ SegmentMap::const_iterator sit = m_tree.getSegment(name);
+ if (sit == m_tree.getSegments().end())
+ return false;
+ p_joint = &sit->second.segment.getJoint();
+ if (q_size < p_joint->getNDof())
+ return false;
+ 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);
+ }
+ return true;
+}
+
+double Armature::getMaxJointChange()
+{
+ if (!m_finalized)
+ return 0.0;
+ 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));
+ if (maxJoint < joint)
+ maxJoint = joint;
+ }
+ return maxJoint;
+}
+
+double Armature::getMaxEndEffectorChange()
+{
+ if (!m_finalized)
+ return 0.0;
+ double maxDelta = 0.0;
+ double delta;
+ Twist twist;
+ for (unsigned int i = 0; i<m_neffector; i++) {
+ twist = diff(m_effectors[i].pose, m_effectors[i].oldpose);
+ delta = twist.rot.Norm();
+ if (delta > maxDelta)
+ maxDelta = delta;
+ delta = twist.vel.Norm();
+ if (delta > maxDelta)
+ maxDelta = delta;
+ }
+ return maxDelta;
+}
+
+int Armature::addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep)
+{
+ SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
+ // not suitable for NDof joints
+ if (segment_it == m_tree.getSegments().end() || segment_it->second.segment.getJoint().getNDof() != 1) {
+ if (_freeParam && _param)
+ free(_param);
+ return -1;
+ }
+ JointConstraintList::iterator constraint_it;
+ JointConstraint_struct* pConstraint;
+ int iConstraint;
+ for (iConstraint=0, constraint_it=m_constraints.begin(); constraint_it != m_constraints.end(); constraint_it++, iConstraint++) {
+ pConstraint = *constraint_it;
+ if (pConstraint->segment == segment_it) {
+ // redefining a constraint
+ if (pConstraint->freeParam && pConstraint->param) {
+ free(pConstraint->param);
+ }
+ pConstraint->function = _function;
+ pConstraint->param = _param;
+ pConstraint->freeParam = _freeParam;
+ pConstraint->substep = _substep;
+ return iConstraint;
+ }
+ }
+ if (m_finalized) {
+ if (_freeParam && _param)
+ free(_param);
+ return -1;
+ }
+ // new constraint, append
+ pConstraint = new JointConstraint_struct(segment_it, ID_JOINT, _function, _param, _freeParam, _substep);
+ m_constraints.push_back(pConstraint);
+ // desired value = rest position
+ //(suitable for joint limit constraint, maybe changed by user in callback)
+ pConstraint->value.yd = m_joints[segment_it->second.q_nr].rest;
+ return m_nconstraint++;
+}
+
+bool Armature::JointLimitCallback(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+{
+ // called from updateControlOutput() when a limit is set on a joint
+ // update the parameters
+ LimitConstraintParam_struct* pLimit = (LimitConstraintParam_struct*)_param;
+ double y = _values->values->y;
+ double x;
+ if (y > pLimit->maxThreshold) {
+ if (y < pLimit->max) {
+ x = (pLimit->max-y)/pLimit->threshold;
+ _values->alpha = pLimit->maxWeight*(1.0-x)/(1.0+pLimit->slope*x);
+ } else {
+ _values->alpha = pLimit->maxWeight;
+ }
+ // change the limit to the threshold value so that there is no oscillation
+ _values->values->yd = pLimit->maxThreshold;
+ } else if (y < pLimit->minThreshold) {
+ if (y > pLimit->min) {
+ x = (y-pLimit->min)/pLimit->threshold;
+ _values->alpha = pLimit->maxWeight*(1.0-x)/(1.0+pLimit->slope*x);
+ } else {
+ _values->alpha = pLimit->maxWeight;
+ }
+ // change the limit to the threshold value so that there is no oscillation
+ _values->values->yd = pLimit->minThreshold;
+ } else {
+ _values->alpha = 0.0;
+ }
+ return true;
+}
+
+int Armature::addLimitConstraint(const std::string& segment_name, double _min, double _max, double _threshold, double _maxWeight, double _slope)
+{
+ SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
+ if (segment_it == m_tree.getSegments().end() || segment_it->second.segment.getJoint().getNDof() != 1) {
+ // not suitable for NDof joints
+ return -1;
+ }
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list