[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [19905] branches/ge_dev: Add itasc library .

Benoit Bolsee benoit.bolsee at online.be
Thu Apr 23 15:29:55 CEST 2009


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, &timestamp);
+		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, &timestamp);
+		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. @@




More information about the Bf-blender-cvs mailing list