Revision: 19905
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=19905
Author: ben2610
Date: 2009-04-23 15:29:54 +0200 (Thu, 23 Apr 2009)
Log Message:
-----------
Add itasc library. This library has no binding yet to Blender and no C API, but
it compiles. It comes with KDL, yet another motion library, similar to moto and
TNT but with specific support for armatures. Tested with cmake, MSVC project
files and scons. Makefiles updated but untested.
Modified Paths:
--------------
branches/ge_dev/CMakeLists.txt
branches/ge_dev/intern/CMakeLists.txt
branches/ge_dev/intern/Makefile
branches/ge_dev/intern/SConscript
branches/ge_dev/projectfiles_vc9/blender/blender.sln
branches/ge_dev/source/creator/CMakeLists.txt
branches/ge_dev/source/nan_definitions.mk
Added Paths:
-----------
branches/ge_dev/intern/itasc/
branches/ge_dev/intern/itasc/Armature.cpp
branches/ge_dev/intern/itasc/Armature.hpp
branches/ge_dev/intern/itasc/CMakeLists.txt
branches/ge_dev/intern/itasc/Cache.cpp
branches/ge_dev/intern/itasc/Cache.hpp
branches/ge_dev/intern/itasc/ConstraintSet.cpp
branches/ge_dev/intern/itasc/ConstraintSet.hpp
branches/ge_dev/intern/itasc/ControlledObject.cpp
branches/ge_dev/intern/itasc/ControlledObject.hpp
branches/ge_dev/intern/itasc/CopyPose.cpp
branches/ge_dev/intern/itasc/CopyPose.hpp
branches/ge_dev/intern/itasc/Distance.cpp
branches/ge_dev/intern/itasc/Distance.hpp
branches/ge_dev/intern/itasc/FixedObject.cpp
branches/ge_dev/intern/itasc/FixedObject.hpp
branches/ge_dev/intern/itasc/Makefile
branches/ge_dev/intern/itasc/MovingFrame.cpp
branches/ge_dev/intern/itasc/MovingFrame.hpp
branches/ge_dev/intern/itasc/Object.hpp
branches/ge_dev/intern/itasc/SConscript
branches/ge_dev/intern/itasc/Scene.cpp
branches/ge_dev/intern/itasc/Scene.hpp
branches/ge_dev/intern/itasc/Solver.hpp
branches/ge_dev/intern/itasc/UncontrolledObject.cpp
branches/ge_dev/intern/itasc/UncontrolledObject.hpp
branches/ge_dev/intern/itasc/WDLSSolver.cpp
branches/ge_dev/intern/itasc/WDLSSolver.hpp
branches/ge_dev/intern/itasc/WSDLSSolver.cpp
branches/ge_dev/intern/itasc/WSDLSSolver.hpp
branches/ge_dev/intern/itasc/WorldObject.cpp
branches/ge_dev/intern/itasc/WorldObject.hpp
branches/ge_dev/intern/itasc/eigen_types.cpp
branches/ge_dev/intern/itasc/eigen_types.hpp
branches/ge_dev/intern/itasc/kdl/
branches/ge_dev/intern/itasc/kdl/Makefile
branches/ge_dev/intern/itasc/kdl/chain.cpp
branches/ge_dev/intern/itasc/kdl/chain.hpp
branches/ge_dev/intern/itasc/kdl/chainfksolver.hpp
branches/ge_dev/intern/itasc/kdl/chainfksolverpos_recursive.cpp
branches/ge_dev/intern/itasc/kdl/chainfksolverpos_recursive.hpp
branches/ge_dev/intern/itasc/kdl/chainjnttojacsolver.cpp
branches/ge_dev/intern/itasc/kdl/chainjnttojacsolver.hpp
branches/ge_dev/intern/itasc/kdl/frameacc.cpp
branches/ge_dev/intern/itasc/kdl/frameacc.hpp
branches/ge_dev/intern/itasc/kdl/frameacc.inl
branches/ge_dev/intern/itasc/kdl/frames.cpp
branches/ge_dev/intern/itasc/kdl/frames.hpp
branches/ge_dev/intern/itasc/kdl/frames.inl
branches/ge_dev/intern/itasc/kdl/frames_io.cpp
branches/ge_dev/intern/itasc/kdl/frames_io.hpp
branches/ge_dev/intern/itasc/kdl/framevel.cpp
branches/ge_dev/intern/itasc/kdl/framevel.hpp
branches/ge_dev/intern/itasc/kdl/framevel.inl
branches/ge_dev/intern/itasc/kdl/inertia.cpp
branches/ge_dev/intern/itasc/kdl/inertia.hpp
branches/ge_dev/intern/itasc/kdl/jacobian.cpp
branches/ge_dev/intern/itasc/kdl/jacobian.hpp
branches/ge_dev/intern/itasc/kdl/jntarray.cpp
branches/ge_dev/intern/itasc/kdl/jntarray.hpp
branches/ge_dev/intern/itasc/kdl/jntarrayacc.cpp
branches/ge_dev/intern/itasc/kdl/jntarrayacc.hpp
branches/ge_dev/intern/itasc/kdl/jntarrayvel.cpp
branches/ge_dev/intern/itasc/kdl/jntarrayvel.hpp
branches/ge_dev/intern/itasc/kdl/joint.cpp
branches/ge_dev/intern/itasc/kdl/joint.hpp
branches/ge_dev/intern/itasc/kdl/kinfam_io.cpp
branches/ge_dev/intern/itasc/kdl/kinfam_io.hpp
branches/ge_dev/intern/itasc/kdl/segment.cpp
branches/ge_dev/intern/itasc/kdl/segment.hpp
branches/ge_dev/intern/itasc/kdl/tree.cpp
branches/ge_dev/intern/itasc/kdl/tree.hpp
branches/ge_dev/intern/itasc/kdl/treefksolver.hpp
branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.cpp
branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.hpp
branches/ge_dev/intern/itasc/kdl/treejnttojacsolver.cpp
branches/ge_dev/intern/itasc/kdl/treejnttojacsolver.hpp
branches/ge_dev/intern/itasc/kdl/utilities/
branches/ge_dev/intern/itasc/kdl/utilities/Makefile
branches/ge_dev/intern/itasc/kdl/utilities/error.h
branches/ge_dev/intern/itasc/kdl/utilities/error_stack.cpp
branches/ge_dev/intern/itasc/kdl/utilities/error_stack.h
branches/ge_dev/intern/itasc/kdl/utilities/kdl-config.h
branches/ge_dev/intern/itasc/kdl/utilities/rall1d.h
branches/ge_dev/intern/itasc/kdl/utilities/rall2d.h
branches/ge_dev/intern/itasc/kdl/utilities/svd_eigen_HH.hpp
branches/ge_dev/intern/itasc/kdl/utilities/traits.h
branches/ge_dev/intern/itasc/kdl/utilities/utility.cpp
branches/ge_dev/intern/itasc/kdl/utilities/utility.h
branches/ge_dev/intern/itasc/kdl/utilities/utility_io.cpp
branches/ge_dev/intern/itasc/kdl/utilities/utility_io.h
branches/ge_dev/intern/itasc/make/
branches/ge_dev/intern/itasc/make/msvc_9_0/
branches/ge_dev/intern/itasc/make/msvc_9_0/itasc.vcproj
branches/ge_dev/intern/itasc/ublas_types.hpp
Modified: branches/ge_dev/CMakeLists.txt
===================================================================
--- branches/ge_dev/CMakeLists.txt 2009-04-23 11:59:13 UTC (rev 19904)
+++ branches/ge_dev/CMakeLists.txt 2009-04-23 13:29:54 UTC (rev 19905)
@@ -285,7 +285,7 @@
SET(FFMPEG ${LIBDIR}/ffmpeg)
SET(FFMPEG_INC ${FFMPEG}/include)
- SET(FFMPEG_LIB avcodec-51 avformat-52 avdevice-52 avutil-49 swscale-0)
+ SET(FFMPEG_LIB avcodec-52 avformat-52 avdevice-52 avutil-50 swscale-0)
SET(FFMPEG_LIBPATH ${FFMPEG}/lib)
IF(CMAKE_CL_64)
Modified: branches/ge_dev/intern/CMakeLists.txt
===================================================================
--- branches/ge_dev/intern/CMakeLists.txt 2009-04-23 11:59:13 UTC (rev
19904)
+++ branches/ge_dev/intern/CMakeLists.txt 2009-04-23 13:29:54 UTC (rev
19905)
@@ -24,7 +24,7 @@
#
# ***** END GPL LICENSE BLOCK *****
-SUBDIRS(SoundSystem string ghost guardedalloc bmfont moto container memutil
decimation iksolver boolop opennl)
+SUBDIRS(SoundSystem string ghost guardedalloc bmfont moto container memutil
decimation iksolver itasc boolop opennl)
IF(WITH_ELBEEM)
SUBDIRS(elbeem)
Modified: branches/ge_dev/intern/Makefile
===================================================================
--- branches/ge_dev/intern/Makefile 2009-04-23 11:59:13 UTC (rev 19904)
+++ branches/ge_dev/intern/Makefile 2009-04-23 13:29:54 UTC (rev 19905)
@@ -32,7 +32,7 @@
# include nan_subdirs.mk
ALLDIRS = string ghost guardedalloc bmfont 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/ge_dev/intern/SConscript
===================================================================
--- branches/ge_dev/intern/SConscript 2009-04-23 11:59:13 UTC (rev 19904)
+++ branches/ge_dev/intern/SConscript 2009-04-23 13:29:54 UTC (rev 19905)
@@ -11,6 +11,7 @@
'memutil/SConscript/',
'decimation/SConscript',
'iksolver/SConscript',
+ 'itasc/SConscript',
'boolop/SConscript',
'opennl/SConscript'])
Added: branches/ge_dev/intern/itasc/Armature.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.cpp (rev 0)
+++ branches/ge_dev/intern/itasc/Armature.cpp 2009-04-23 13:29:54 UTC (rev
19905)
@@ -0,0 +1,415 @@
+/* $Id$
+ * 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 = sizeof(double)*5;
+
+Armature::Armature():
+ ControlledObject(),
+ m_tree(),
+ m_njoint(0),
+ m_nconstraint(0),
+ m_neffector(0),
+ m_finalized(false),
+ m_cache(NULL),
+ m_qCCh(-1),
+ m_qCTs(0),
+ m_yCCh(-1),
+ m_yCTs(0),
+ m_qKdl(),
+ 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);
+ }
+ m_constraints.clear();
+}
+
+void Armature::initCache(Cache *_cache)
+{
+ m_cache = _cache;
+ m_qCCh = -1;
+ m_yCCh = -1;
+ 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);
+ }
+ // store the initial cache position at timestamp 0
+ pushQ(0);
+ pushConstraints(0);
+ }
+}
+
+void Armature::pushQ(CacheTS timestamp)
+{
+ if (m_qCCh >= 0) {
+ m_cache->addCacheItem(this, m_qCCh, timestamp, &m_qKdl(0),
m_qKdl.rows()*sizeof(double));
+ 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 *item = (double*)m_cache->addCacheItem(this, m_yCCh,
timestamp, NULL, m_nconstraint*constraintCacheSize);
+ if (item) {
+ 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_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, 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;
+
+ if (joint.getType() != Joint::None) {
+ m_joints.push_back(q_rest);
+ m_njoint++;
+ }
+ return true;
+}
+
+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);
+ if (segment_it == m_tree.getSegments().end() ||
segment_it->second.segment.getJoint().getType() == Joint::None) {
+ 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];
+ 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().getType() == Joint::None) {
+ return -1;
+ }
+ if (segment_it->second.segment.getJoint().getType() < Joint::TransX) {
+ // for rotation joint, the limit is given in degree, convert to
radian
+ _min *= KDL::deg2rad;
+ _max *= KDL::deg2rad;
+ _threshold *= KDL::deg2rad;
+ }
+ LimitConstraintParam_struct* param = new LimitConstraintParam_struct;
+ param->max = _max;
+ param->min = _min;
+ param->threshold = _threshold;
+ param->maxThreshold = _max - _threshold;
+ param->minThreshold = _min + _threshold;
+ param->maxWeight = _maxWeight;
+ param->slope = (_maxWeight-_slope)/_slope;
+ return addConstraint(segment_name, JointLimitCallback, param, true,
true);
+}
+
+int Armature::addEndEffector(const std::string& name)
+{
+ const SegmentMap& segments = m_tree.getSegments();
+ if (segments.find(name) == segments.end())
+ return -1;
+
+ EffectorList::const_iterator it;
+ int ee;
+ for (it=m_effectors.begin(), ee=0; it!=m_effectors.end(); it++, ee++) {
+ if (it->name == name)
+ return ee;
+ }
+ if (m_finalized)
+ return -1;
+ Effector_struct effector(name);
+ m_effectors.push_back(effector);
+ return m_neffector++;
+}
+
+void Armature::finalize()
+{
+ unsigned int i;
+ if (m_finalized)
+ return;
+ initialize(m_njoint, m_nconstraint, m_neffector);
@@ Diff output truncated at 10240 characters. @@
_______________________________________________
Bf-blender-cvs mailing list
[email protected]
http://lists.blender.org/mailman/listinfo/bf-blender-cvs
