Revision: 20631
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=20631
Author: ben2610
Date: 2009-06-04 18:31:50 +0200 (Thu, 04 Jun 2009)
Log Message:
-----------
iTaSC: substep evaluation. Max delta joint measured on the whole interval and not just the last substep during initial iteration. Substep computed as whole fraction of timestep for more stability.
Modified Paths:
--------------
branches/ge_dev/intern/itasc/Armature.cpp
branches/ge_dev/intern/itasc/Armature.hpp
branches/ge_dev/intern/itasc/Scene.cpp
branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
Modified: branches/ge_dev/intern/itasc/Armature.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.cpp 2009-06-04 15:58:47 UTC (rev 20630)
+++ branches/ge_dev/intern/itasc/Armature.cpp 2009-06-04 16:31:50 UTC (rev 20631)
@@ -30,6 +30,7 @@
m_yCCh(-1),
m_yCTs(0),
m_qKdl(),
+ m_oldqKdl(),
m_qdotKdl(),
m_jac(NULL),
m_jacsolver(NULL),
@@ -173,13 +174,13 @@
return true;
}
-double Armature::getMaxJointChange(double timestep)
+double Armature::getMaxJointChange()
{
if (!m_finalized)
return 0.0;
double maxJoint = 0.0;
for (unsigned int i=0; i<m_njoint; i++) {
- double joint = fabs(m_qdot(i)*timestep);
+ double joint = fabs(m_oldqKdl(i)-m_qKdl(i));
if (maxJoint < joint)
maxJoint = joint;
}
@@ -313,9 +314,10 @@
m_fksolver = new KDL::TreeFkSolverPos_recursive(m_tree);
m_jac = new Jacobian(m_njoint);
m_qKdl.resize(m_njoint);
+ m_oldqKdl.resize(m_njoint);
m_qdotKdl.resize(m_njoint);
for (i=0; i<m_njoint; i++) {
- m_qKdl(i) = m_joints[i];
+ m_oldqKdl(i) = m_qKdl(i) = m_joints[i];
}
updateJacobian();
m_finalized = true;
@@ -385,11 +387,17 @@
if (!m_finalized)
return;
+
if (!timestamp.substep && !timestamp.reiterate) {
popQ(timestamp.cacheTimestamp);
popConstraints(timestamp.cacheTimestamp);
}
+ if (!timestamp.substep) {
+ // save previous joint state for getMaxJointChange()
+ memcpy(&m_oldqKdl(0), &m_qKdl(0), sizeof(double)*m_qKdl.rows());
+ }
+
JointConstraintList::iterator it;
unsigned int iConstraint;
Modified: branches/ge_dev/intern/itasc/Armature.hpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.hpp 2009-06-04 15:58:47 UTC (rev 20630)
+++ branches/ge_dev/intern/itasc/Armature.hpp 2009-06-04 16:31:50 UTC (rev 20631)
@@ -27,7 +27,7 @@
// specific limit constraint on joint
int addLimitConstraint(const std::string& segment_name, double _min, double _max, double _threshold, double _maxWeight=1000.0, double _slope=1.0);
double getJoint(unsigned int joint);
- double getMaxJointChange(double timestep);
+ double getMaxJointChange();
bool getSegment(const std::string& segment_name, 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);
@@ -99,6 +99,7 @@
int m_yCCh;
CacheTS m_yCTs;
JntArray m_qKdl;
+ JntArray m_oldqKdl;
JntArray m_qdotKdl;
Jacobian* m_jac;
KDL::TreeJntToJacSolver* m_jacsolver;
Modified: branches/ge_dev/intern/itasc/Scene.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Scene.cpp 2009-06-04 15:58:47 UTC (rev 20630)
+++ branches/ge_dev/intern/itasc/Scene.cpp 2009-06-04 16:31:50 UTC (rev 20631)
@@ -397,6 +397,8 @@
ConstraintSet_struct* cs = it->second;
cs->task->getMaxTimestep(timesubstep);
}
+ // use substep that are dividers of timestep for more regularity
+ timesubstep = timestep/(1.0+floor((timestep/timesubstep)-0.5));
if (timesubstep >= timeleft-(m_minstep/2.0)) {
timesubstep = timeleft;
numsubstep = 1;
Modified: branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
===================================================================
--- branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-06-04 15:58:47 UTC (rev 20630)
+++ branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-06-04 16:31:50 UTC (rev 20631)
@@ -834,7 +834,7 @@
}
double timestamp = ctime * G.scene->r.frs_sec_base / G.scene->r.frs_sec;
- double timestep = 0.040;
+ double timestep = 1.0/G.scene->r.frs_sec;
bool reiterate = false;
if (ikscene->cache) {
iTaSC::CacheTS sts, cts, dts;
@@ -853,7 +853,7 @@
if (reiterate) {
// how many times do we reiterate?
for (i=0; i<100; i++) {
- if (ikscene->armature->getMaxJointChange(timestep) < 0.005)
+ if (ikscene->armature->getMaxJointChange() < 0.005)
break;
ikscene->scene->update(timestamp, timestep, 0, true, false);
}