[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [20622] branches/ge_dev: iTaSC: better auto substep calculation with formula to evaluate the amount of singularity .
Benoit Bolsee
benoit.bolsee at online.be
Thu Jun 4 14:47:59 CEST 2009
Revision: 20622
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=20622
Author: ben2610
Date: 2009-06-04 14:47:59 +0200 (Thu, 04 Jun 2009)
Log Message:
-----------
iTaSC: better auto substep calculation with formula to evaluate the amount of singularity.
Modified Paths:
--------------
branches/ge_dev/intern/itasc/CopyPose.cpp
branches/ge_dev/intern/itasc/CopyPose.hpp
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/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/source/blender/ikplugin/intern/itasc_plugin.cpp
Modified: branches/ge_dev/intern/itasc/CopyPose.cpp
===================================================================
--- branches/ge_dev/intern/itasc/CopyPose.cpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/intern/itasc/CopyPose.cpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -457,4 +457,16 @@
return m_values;
}
+double CopyPose::getMaxTimestep(double& timestep)
+{
+ // CopyPose should not have any limit on linear velocity:
+ // in case the target is out of reach, this can be very high.
+ // We will simply limit on rotation
+ e_scalar maxChidot = m_chidot.block(3,0,3,1).cwise().abs().maxCoeff();
+ if (timestep*maxChidot > m_maxDeltaChi) {
+ timestep = m_maxDeltaChi/maxChidot;
+ }
+ return timestep;
}
+
+}
Modified: branches/ge_dev/intern/itasc/CopyPose.hpp
===================================================================
--- branches/ge_dev/intern/itasc/CopyPose.hpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/intern/itasc/CopyPose.hpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -22,6 +22,7 @@
virtual void initCache(Cache *_cache);
virtual void updateControlOutput(const Timestamp& timestamp);
virtual void CopyPose::modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
+ virtual double getMaxTimestep(double& timestep);
public:
enum ID { // constraint ID in callback and setControlParameter
Modified: branches/ge_dev/intern/itasc/Scene.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Scene.cpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/intern/itasc/Scene.cpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -29,7 +29,7 @@
m_ncTotal(0),m_nqTotal(0),m_nuTotal(0),m_nsets(0),
m_solver(NULL),m_cache(NULL)
{
-
+ m_minstep = 0.01;
}
Scene::~Scene()
@@ -230,6 +230,7 @@
numsubstep = 1;
double timesubstep = timestep/numsubstep;
double timeleft = timestep;
+ e_scalar nlcoef;
// initially we keep timestep unchanged so that update function compute the velocity over
while (numsubstep > 0) {
// get objects
@@ -347,7 +348,7 @@
}
//Call the solver with A, Wq, Wy, ydot to solver qdot:
- if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot))
+ if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot,nlcoef))
// this should never happen
return false;
//send result to the objects
@@ -382,6 +383,11 @@
// and joint limit gain variation
// We will pass the joint velocity to each object and they will recommend a maximum timestep
timesubstep = timeleft;
+ double maxsubstep = timestep/nlcoef;
+ if (maxsubstep < m_minstep)
+ maxsubstep = m_minstep;
+ if (timesubstep > maxsubstep)
+ timesubstep = maxsubstep;
for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
Object_struct* os = it->second;
if(os->object->getType()==Object::Controlled)
@@ -391,7 +397,7 @@
ConstraintSet_struct* cs = it->second;
cs->task->getMaxTimestep(timesubstep);
}
- if (timesubstep >= timeleft-KDL::epsilon) {
+ if (timesubstep >= timeleft-(m_minstep/2.0)) {
timesubstep = timeleft;
numsubstep = 1;
timeleft = 0.;
Modified: branches/ge_dev/intern/itasc/Scene.hpp
===================================================================
--- branches/ge_dev/intern/itasc/Scene.hpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/intern/itasc/Scene.hpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -39,7 +39,7 @@
e_matrix6 m_Vf,m_Uf;
e_vector m_Wy,m_ydot,m_qdot,m_xdot;
e_vector6 m_Sf,m_tempf;
-
+ double m_minstep;
unsigned int m_ncTotal,m_nqTotal,m_nuTotal,m_nsets;
std::vector<bool> m_ytask;
Modified: branches/ge_dev/intern/itasc/Solver.hpp
===================================================================
--- branches/ge_dev/intern/itasc/Solver.hpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/intern/itasc/Solver.hpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -20,7 +20,7 @@
// gc = grouping of constraint output ,
// size of vector = nc, alternance of true / false to indicate the grouping of output
virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)=0;
- virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot)=0;
+ virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)=0;
};
}
Modified: branches/ge_dev/intern/itasc/WDLSSolver.cpp
===================================================================
--- branches/ge_dev/intern/itasc/WDLSSolver.cpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/intern/itasc/WDLSSolver.cpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -32,7 +32,7 @@
return true;
}
-bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot){
+bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef){
// Create the Weighted jacobian
m_AWq = A*Wq;
for (int i=0; i<Wy.size(); i++)
@@ -52,10 +52,26 @@
//U'*Wy'*ydot
m_WyUt_ydot = (m_WyU.transpose()*ydot).lazy();
//S^-1*U'*Wy'*ydot
- for(int i=0;i<m_WyUt_ydot.size();++i)
- m_SinvWyUt_ydot(i) = m_WyUt_ydot(i)*m_S(i)/(m_S(i)*m_S(i)+m_lambda*m_lambda);
+ e_scalar maxDeltaS = e_scalar(0.0);
+ e_scalar prevS = e_scalar(0.0);
+ e_scalar maxS = e_scalar(1.0);
+ for(int i=0;i<m_WyUt_ydot.size();++i) {
+ e_scalar S = m_S(i);
+ if (i > 0 && S > KDL::epsilon) {
+ if ((prevS-S) > maxDeltaS) {
+ maxDeltaS = (prevS-S);
+ maxS = prevS;
+ }
+ }
+ m_SinvWyUt_ydot(i) = m_WyUt_ydot(i)*S/(S*S+m_lambda*m_lambda);
+ prevS = S;
+ }
//qdot=Wq*V*S^-1*U'*Wy'*ydot
qdot=(m_WqV*m_SinvWyUt_ydot).lazy();
+ if (maxDeltaS == e_scalar(0.0))
+ nlcoef = e_scalar(1.0/KDL::epsilon);
+ else
+ nlcoef = maxS/(maxS-maxDeltaS)/e_scalar(2.0);
return true;
}
Modified: branches/ge_dev/intern/itasc/WDLSSolver.hpp
===================================================================
--- branches/ge_dev/intern/itasc/WDLSSolver.hpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/intern/itasc/WDLSSolver.hpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -22,7 +22,7 @@
virtual ~WDLSSolver();
virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc);
- virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot);
+ virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef);
void setLambda(double _lambda){m_lambda=_lambda;};
Modified: branches/ge_dev/intern/itasc/WSDLSSolver.cpp
===================================================================
--- branches/ge_dev/intern/itasc/WSDLSSolver.cpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/intern/itasc/WSDLSSolver.cpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -41,7 +41,7 @@
return true;
}
-bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot)
+bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)
{
unsigned int i, j, l;
e_scalar N, M;
@@ -59,12 +59,22 @@
m_Wy_ydot = Wy.cwise() * ydot;
m_WqV = (Wq*m_V).lazy();
qdot.fill(e_scalar(0.));
+ e_scalar maxDeltaS = e_scalar(0.0);
+ e_scalar prevS = e_scalar(0.0);
+ e_scalar maxS = e_scalar(1.0);
for(i=0;i<m_ns;++i) {
e_scalar norm, mag, alpha, _qmax, Sinv, vmax, damp;
+ e_scalar S = m_S(i);
bool prev;
- if (m_S(i) < KDL::epsilon)
+ if (S < KDL::epsilon)
break;
- Sinv = e_scalar(1.)/m_S(i);
+ Sinv = e_scalar(1.)/S;
+ if (i > 0) {
+ if ((prevS-S) > maxDeltaS) {
+ maxDeltaS = (prevS-S);
+ maxS = prevS;
+ }
+ }
N = M = e_scalar(0.);
for (l=0, prev=m_ytask[0], norm=e_scalar(0.); l<m_nc; l++) {
if (prev == m_ytask[l]) {
@@ -100,7 +110,12 @@
damp = Sinv*alpha;
}
qdot += m_WqV.col(i)*damp;
+ prevS = S;
}
+ if (maxDeltaS == e_scalar(0.0))
+ nlcoef = e_scalar(1.0/KDL::epsilon);
+ else
+ nlcoef = maxS/(maxS-maxDeltaS)/e_scalar(2.0);
return true;
}
Modified: branches/ge_dev/intern/itasc/WSDLSSolver.hpp
===================================================================
--- branches/ge_dev/intern/itasc/WSDLSSolver.hpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/intern/itasc/WSDLSSolver.hpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -24,7 +24,7 @@
virtual ~WSDLSSolver();
virtual bool init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc);
- virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot);
+ virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef);
void setQmax(double _qmax){m_qmax=_qmax;};
Modified: branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
===================================================================
--- branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-06-04 11:16:56 UTC (rev 20621)
+++ branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-06-04 12:47:59 UTC (rev 20622)
@@ -480,6 +480,7 @@
ikscene->scene = scene;
ikscene->cache = new iTaSC::Cache();;
ikscene->solver = new iTaSC::WSDLSSolver();
+ //ikscene->solver->setQmax(10.0);
ikscene->blArmature = ob;
std::string joint;
@@ -852,7 +853,7 @@
if (reiterate) {
// how many times do we reiterate?
for (i=0; i<100; i++) {
- if (ikscene->armature->getMaxJointChange(timestep) < 0.001)
+ if (ikscene->armature->getMaxJointChange(timestep) < 0.005)
break;
ikscene->scene->update(timestamp, timestep, 0, true, false);
}
@@ -961,7 +962,7 @@
arm = get_armature(ob);
if (arm->ikdata) {
IK_Data* ikdata = (IK_Data*)arm->ikdata;
- for (IK_Scene* scene = ikdata->first; scene; scene = ikdata->first) {
+ for (IK_Scene* scene = ikdata->first; scene; scene = scene->next) {
if (scene->channels[0].pchan == pchan) {
execute_scene(scene, ctime);
break;
More information about the Bf-blender-cvs
mailing list