[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [23162] branches/itasc: iTaSC: add animation mode to IK solver.
Benoit Bolsee
benoit.bolsee at online.be
Sat Sep 12 23:39:51 CEST 2009
Revision: 23162
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=23162
Author: ben2610
Date: 2009-09-12 23:39:50 +0200 (Sat, 12 Sep 2009)
Log Message:
-----------
iTaSC: add animation mode to IK solver.
New option 'simulation' is available in the iTaSC panel.
When this option is selected, the solver runs in simulation
mode: the solver is stateful, runs in real-time context and
ignores actions and non-IK constraints on the bones it controls
(i.e. the solver is in full charge of the IK chain).
This is the normal mode of the solver and it is selected
by default. It is also the mode you should use the GE because
of its high performance.
When this option is not selected, the solver runs in animation
mode. It works like the old solver: it is stateless and executes
the actions and non-IK constraints before converging to the
targets. Of course it has also the same problems: pose flipping,
slower, etc. However, it is still significantly faster than the
old solver and it benefits from the iTaSC intrinsic capabilities:
support for different type of IK constraints and multiple
constraints per bones.
Modified Paths:
--------------
branches/itasc/intern/itasc/Armature.cpp
branches/itasc/intern/itasc/Armature.hpp
branches/itasc/intern/itasc/Cache.hpp
branches/itasc/intern/itasc/ConstraintSet.cpp
branches/itasc/intern/itasc/ConstraintSet.hpp
branches/itasc/intern/itasc/CopyPose.cpp
branches/itasc/intern/itasc/CopyPose.hpp
branches/itasc/intern/itasc/Distance.cpp
branches/itasc/intern/itasc/MovingFrame.cpp
branches/itasc/intern/itasc/Object.hpp
branches/itasc/intern/itasc/Scene.cpp
branches/itasc/intern/itasc/Scene.hpp
branches/itasc/release/ui/buttons_data_bone.py
branches/itasc/source/blender/blenkernel/intern/action.c
branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
branches/itasc/source/blender/makesdna/DNA_action_types.h
branches/itasc/source/blender/makesrna/intern/rna_pose.c
Modified: branches/itasc/intern/itasc/Armature.cpp
===================================================================
--- branches/itasc/intern/itasc/Armature.cpp 2009-09-12 20:47:53 UTC (rev 23161)
+++ branches/itasc/intern/itasc/Armature.cpp 2009-09-12 21:39:50 UTC (rev 23162)
@@ -420,6 +420,22 @@
}
}
+bool Armature::setJointArray(const KDL::JntArray& joints)
+{
+ if (!m_finalized)
+ return false;
+ if (joints.rows() != m_qKdl.rows())
+ return false;
+ m_qKdl = joints;
+ updateJacobian();
+ return true;
+}
+
+const KDL::JntArray& Armature::getJointArray()
+{
+ return m_qKdl;
+}
+
bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callback)
{
if (!m_finalized)
@@ -627,6 +643,8 @@
Jq(i,j)=(*m_jac)(i,j);
}
}
+ // remember that this object has moved
+ m_updated = true;
}
const Frame& Armature::getPose(const unsigned int ee)
@@ -649,7 +667,7 @@
return;
- if (!timestamp.substep && !timestamp.reiterate) {
+ if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) {
popQ(timestamp.cacheTimestamp);
//popConstraints(timestamp.cacheTimestamp);
}
Modified: branches/itasc/intern/itasc/Armature.hpp
===================================================================
--- branches/itasc/intern/itasc/Armature.hpp 2009-09-12 20:47:53 UTC (rev 23161)
+++ branches/itasc/intern/itasc/Armature.hpp 2009-09-12 21:39:50 UTC (rev 23162)
@@ -41,6 +41,9 @@
virtual void updateControlOutput(const Timestamp& timestamp);
virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0);
virtual void initCache(Cache *_cache);
+ virtual bool setJointArray(const KDL::JntArray& joints);
+ virtual const KDL::JntArray& getJointArray();
+
virtual double getArmLength()
{
return m_armlength;
Modified: branches/itasc/intern/itasc/Cache.hpp
===================================================================
--- branches/itasc/intern/itasc/Cache.hpp 2009-09-12 20:47:53 UTC (rev 23161)
+++ branches/itasc/intern/itasc/Cache.hpp 2009-09-12 21:39:50 UTC (rev 23162)
@@ -41,7 +41,8 @@
unsigned int reiterate:1;
unsigned int cache:1;
unsigned int update:1;
- unsigned int dummy:20;
+ unsigned int interpolate:1;
+ unsigned int dummy:19;
Timestamp() { memset(this, 0, sizeof(Timestamp)); }
};
Modified: branches/itasc/intern/itasc/ConstraintSet.cpp
===================================================================
--- branches/itasc/intern/itasc/ConstraintSet.cpp 2009-09-12 20:47:53 UTC (rev 23161)
+++ branches/itasc/intern/itasc/ConstraintSet.cpp 2009-09-12 21:39:50 UTC (rev 23162)
@@ -21,7 +21,6 @@
m_S(6),m_temp(6),m_tdelta(6),
m_internalPose(F_identity), m_externalPose(F_identity),
m_constraintCallback(NULL), m_constraintParam(NULL),
- m_poseCallback(NULL), m_poseParam(NULL),
m_toggle(false),m_substep(false),
m_threshold(accuracy),m_maxIter(maximum_iterations)
{
@@ -32,7 +31,6 @@
m_nc(0),
m_internalPose(F_identity), m_externalPose(F_identity),
m_constraintCallback(NULL), m_constraintParam(NULL),
- m_poseCallback(NULL), m_poseParam(NULL),
m_toggle(false),m_substep(false),
m_threshold(0.0),m_maxIter(0)
{
@@ -101,19 +99,6 @@
return false;
}
-bool ConstraintSet::getExternalPose(KDL::Frame& pose)
-{
- if (m_poseCallback == NULL)
- return false;
- return (*m_poseCallback)(this, m_poseParam, pose);
-}
-
-bool ConstraintSet::modelRefresh()
-{
- Frame _external_pose;
- return (getExternalPose(_external_pose) && initialise(_external_pose));
-}
-
bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep)
{
ConstraintValues values;
Modified: branches/itasc/intern/itasc/ConstraintSet.hpp
===================================================================
--- branches/itasc/intern/itasc/ConstraintSet.hpp 2009-09-12 20:47:53 UTC (rev 23161)
+++ branches/itasc/intern/itasc/ConstraintSet.hpp 2009-09-12 21:39:50 UTC (rev 23162)
@@ -47,7 +47,6 @@
class ConstraintSet;
typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param);
-typedef bool (*ExternalPoseCallback)(ConstraintSet* self, void *_param, KDL::Frame& _pose);
class ConstraintSet {
protected:
@@ -60,7 +59,6 @@
KDL::Frame m_internalPose,m_externalPose;
ConstraintCallback m_constraintCallback;
void* m_constraintParam;
- ExternalPoseCallback m_poseCallback;
void* m_poseParam;
bool m_toggle;
bool m_substep;
@@ -77,7 +75,6 @@
virtual bool initialise(KDL::Frame& init_pose);
virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations);
virtual bool closeLoop();
- virtual bool getExternalPose(KDL::Frame& pose);
virtual double getMaxTimestep(double& timestep);
@@ -95,17 +92,9 @@
return true;
}
- virtual bool registerPoseCallback(ExternalPoseCallback _function, void *_param)
- {
- m_poseCallback = _function;
- m_poseParam = _param;
- return true;
- }
-
virtual const e_vector& getControlOutput()const{return m_ydot;};
virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues) = 0;
virtual bool setControlParameters(ConstraintValues* _values, unsigned int _nvalues, double timestep=0.0) = 0;
- virtual bool modelRefresh();
bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0);
virtual const e_matrix6& getJf() const{return m_Jf;};
Modified: branches/itasc/intern/itasc/CopyPose.cpp
===================================================================
--- branches/itasc/intern/itasc/CopyPose.cpp 2009-09-12 20:47:53 UTC (rev 23161)
+++ branches/itasc/intern/itasc/CopyPose.cpp 2009-09-12 21:39:50 UTC (rev 23162)
@@ -203,9 +203,9 @@
return item;
}
-bool CopyPose::popPose(CacheTS timestamp, bool& found)
+bool CopyPose::popPose(CacheTS timestamp)
{
- found = false;
+ bool found = false;
if (m_poseCCh >= 0) {
double *item = (double*)m_cache->getPreviousCacheItem(this, m_poseCCh, ×tamp);
if (item) {
@@ -228,17 +228,8 @@
item = NULL;
}
}
- if (!item) {
- // The time is not contiguous or cache is empty, should also load the joint.
- // But this constraint has no joint, only a full rotation matrix that we don't save.
- // We must get the pose from the scene using the callback. Hopefully the constraint
- // are updated after the objects, so we can trust the object position
- getExternalPose(m_internalPose);
- updateJacobian();
- return true;
- }
}
- return false;
+ return found;
}
void CopyPose::interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp)
@@ -249,7 +240,7 @@
for (i=0, _yval=_state->output; i<_state->ny; mask <<= 1) {
if (m_outputControl & mask) {
if (m_outputDynamic & mask) {
- if (timestamp.substep) {
+ if (timestamp.substep && timestamp.interpolate) {
_yval->yd += _yval->yddot*timestamp.realTimestep;
} else {
_yval->yd = _yval->nextyd;
@@ -271,24 +262,25 @@
void CopyPose::updateKinematics(const Timestamp& timestamp)
{
- if (m_outputDynamic & CTL_POSITION)
- interpolateOutput(&m_pos, CTL_POSITIONX, timestamp);
- if (m_outputDynamic & CTL_ROTATION)
- interpolateOutput(&m_rot, CTL_ROTATIONX, timestamp);
+ if (timestamp.interpolate) {
+ if (m_outputDynamic & CTL_POSITION)
+ interpolateOutput(&m_pos, CTL_POSITIONX, timestamp);
+ if (m_outputDynamic & CTL_ROTATION)
+ interpolateOutput(&m_rot, CTL_ROTATIONX, timestamp);
+ }
pushCache(timestamp);
}
void CopyPose::updateJacobian()
{
- //Jacobian is always identity at the end of the constraint,
- //must change the reference to the begining of the constraint
+ //Jacobian is always identity at the start of the constraint chain
//instead of going through complicated jacobian operation, implemented direct formula
- m_Jf(1,3) = m_internalPose.p.z();
- m_Jf(2,3) = -m_internalPose.p.y();
- m_Jf(0,4) = -m_internalPose.p.z();
- m_Jf(2,4) = m_internalPose.p.x();
- m_Jf(0,5) = m_internalPose.p.y();
- m_Jf(1,5) = -m_internalPose.p.x();
+ //m_Jf(1,3) = m_internalPose.p.z();
+ //m_Jf(2,3) = -m_internalPose.p.y();
+ //m_Jf(0,4) = -m_internalPose.p.z();
+ //m_Jf(2,4) = m_internalPose.p.x();
+ //m_Jf(0,5) = m_internalPose.p.y();
+ //m_Jf(1,5) = -m_internalPose.p.x();
}
void CopyPose::updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep)
@@ -389,7 +381,7 @@
for (i=_state->firsty, j=0, _yval=_state->output, _data=_values->values; j<3; j++, mask<<=1) {
if (m_outputControl & mask) {
- *(double*)&_data->y = -vel(j);
+ *(double*)&_data->y = vel(j);
*(double*)&_data->ydot = m_ydot(i);
_data->yd = _yval->yd;
_data->yddot = _yval->yddot;
@@ -411,7 +403,7 @@
double len=0.0;
for (j=0, _yval=_state->output; j<3; j++) {
if (m_outputControl & (mask<<j)) {
- len += KDL::sqr(_yval->yd+vel(j));
+ len += KDL::sqr(_yval->yd-vel(j));
_yval++;
}
}
@@ -421,7 +413,7 @@
}
for (i=_state->firsty, j=0, _yval=_state->output; j<3; j++) {
if (m_outputControl & (mask<<j)) {
- m_ydot(i)=_yval->yddot+_state->K*coef*(_yval->yd+vel(j));
+ m_ydot(i)=_yval->yddot+_state->K*coef*(_yval->yd-vel(j));
_yval++;
i++;
}
@@ -431,13 +423,11 @@
void CopyPose::updateControlOutput(const Timestamp& timestamp)
{
//IMO this should be done, no idea if it is enough (wrt Distance impl)
- Twist y = diff(m_internalPose,F_identity);
+ Twist y = diff(F_identity, m_internalPose);
bool found = true;
if (!timestamp.substep) {
if (!timestamp.reiterate) {
- if (popPose(timestamp.cacheTimestamp, found))
- // pose updated, recalculate the rotation
- y = diff(m_internalPose,F_identity);
+ found = popPose(timestamp.cacheTimestamp);
}
}
if (m_constraintCallback && (!timestamp.substep || m_substep)) {
@@ -450,7 +440,7 @@
updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
}
if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
- setControlParameters(m_values, m_nvalues, (found)?timestamp.realTimestep:0.0);
+ setControlParameters(m_values, m_nvalues, (found && timestamp.interpolate)?timestamp.realTimestep:0.0);
}
}
if (m_outputControl & CTL_POSITION) {
Modified: branches/itasc/intern/itasc/CopyPose.hpp
===================================================================
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list