[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [22987] branches/itasc: Implement Max Velocity parameter on DLS solver.

Benoit Bolsee benoit.bolsee at online.be
Fri Sep 4 12:00:25 CEST 2009


Revision: 22987
          http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=22987
Author:   ben2610
Date:     2009-09-04 12:00:25 +0200 (Fri, 04 Sep 2009)

Log Message:
-----------
Implement Max Velocity parameter on DLS solver. This allows to controle the dynamic of the armature by limiting the maximum velocity of the joints. Put back the parameter in the UI.

Modified Paths:
--------------
    branches/itasc/intern/itasc/Solver.hpp
    branches/itasc/intern/itasc/WDLSSolver.cpp
    branches/itasc/intern/itasc/WDLSSolver.hpp
    branches/itasc/intern/itasc/WSDLSSolver.hpp
    branches/itasc/release/ui/buttons_data_bone.py
    branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
    branches/itasc/source/blender/makesrna/intern/rna_pose.c

Modified: branches/itasc/intern/itasc/Solver.hpp
===================================================================
--- branches/itasc/intern/itasc/Solver.hpp	2009-09-04 08:49:11 UTC (rev 22986)
+++ branches/itasc/intern/itasc/Solver.hpp	2009-09-04 10:00:25 UTC (rev 22987)
@@ -16,7 +16,7 @@
 class Solver{
 public:
 	enum SolverParam {
-		SDLS_QMAX = 0,
+		DLS_QMAX = 0,
 		DLS_LAMBDA_MAX,
 		DLS_EPSILON
 	};

Modified: branches/itasc/intern/itasc/WDLSSolver.cpp
===================================================================
--- branches/itasc/intern/itasc/WDLSSolver.cpp	2009-09-04 08:49:11 UTC (rev 22986)
+++ branches/itasc/intern/itasc/WDLSSolver.cpp	2009-09-04 10:00:25 UTC (rev 22987)
@@ -10,7 +10,10 @@
 
 namespace iTaSC {
 
-WDLSSolver::WDLSSolver() : m_lambda(0.5), m_epsilon(0.1) {
+WDLSSolver::WDLSSolver() : m_lambda(0.5), m_epsilon(0.1) 
+{
+	// maximum joint velocity
+	m_qmax = 50.0;
 }
 
 WDLSSolver::~WDLSSolver() {
@@ -18,21 +21,22 @@
 
 bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)
 {
+	m_ns = std::min(nc,nq);
     m_AWq = e_zero_matrix(nc,nq);
     m_WyAWq = e_zero_matrix(nc,nq);
     m_U = e_zero_matrix(nc,nq);
 	m_S = e_zero_vector(std::max(nc,nq));
     m_temp = e_zero_vector(nq);
     m_V = e_zero_matrix(nq,nq);
-	m_WyU = e_zero_matrix(nc,std::min(nc,nq));
     m_WqV = e_zero_matrix(nq,nq);
-	m_WyUt_ydot = e_zero_vector(std::min(nc,nq));
-    m_SinvWyUt_ydot = e_zero_vector(nq);
+	m_Wy_ydot = e_zero_vector(nc);
     return true;
 }
 
-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
+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)
+{
+	double alpha, vmax, norm;
+	// Create the Weighted jacobian
     m_AWq = A*Wq;
 	for (int i=0; i<Wy.size(); i++)
 		m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
@@ -42,21 +46,17 @@
     if(ret<0)
         return false;
 
-    //Pre-multiply m_U and V by the task space and joint space weighting matrix respectively
-	Eigen::Block<e_matrix> U_reduct(m_U,0,0,m_U.rows(),m_WyU.cols());
-	for (int i=0; i<Wy.size(); i++)
-		m_WyU.row(i) = Wy(i)*U_reduct.row(i);
     m_WqV = (Wq*m_V).lazy();
 
-    //U'*Wy'*ydot
-	m_WyUt_ydot = (m_WyU.transpose()*ydot).lazy();
-    //S^-1*U'*Wy'*ydot
+    //Wy*ydot
+	m_Wy_ydot = Wy.cwise() * ydot;
+    //S^-1*U'*Wy*ydot
 	e_scalar maxDeltaS = e_scalar(0.0);
 	e_scalar prevS = e_scalar(0.0);
 	e_scalar maxS = e_scalar(1.0);
 	e_scalar S, lambda;
-	m_SinvWyUt_ydot.setZero();
-	for(int i=0;i<m_WyUt_ydot.size();++i) {
+	qdot.setZero();
+	for(int i=0;i<m_ns;++i) {
 		S = m_S(i);
 		if (S <= KDL::epsilon)
 			break;
@@ -65,11 +65,16 @@
 			maxS = prevS;
 		}
 		lambda = (S < m_epsilon) ? (e_scalar(1.0)-KDL::sqr(S/m_epsilon))*m_lambda*m_lambda : e_scalar(0.0);
-        m_SinvWyUt_ydot(i) = m_WyUt_ydot(i)*S/(S*S+lambda);
+		alpha = m_U.col(i).dot(m_Wy_ydot)*S/(S*S+lambda);
+		vmax = m_WqV.col(i).cwise().abs().maxCoeff();
+		norm = fabs(alpha*vmax);
+		if (norm > m_qmax) {
+			qdot += m_WqV.col(i)*(alpha*m_qmax/norm);
+		} else {
+			qdot += m_WqV.col(i)*alpha;
+		}
 		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(KDL::epsilon);
 	else

Modified: branches/itasc/intern/itasc/WDLSSolver.hpp
===================================================================
--- branches/itasc/intern/itasc/WDLSSolver.hpp	2009-09-04 08:49:11 UTC (rev 22986)
+++ branches/itasc/intern/itasc/WDLSSolver.hpp	2009-09-04 10:00:25 UTC (rev 22987)
@@ -14,10 +14,12 @@
 
 class WDLSSolver: public iTaSC::Solver {
 private:
-    e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WyU,m_WqV;
-    e_vector m_S,m_temp,m_WyUt_ydot,m_SinvWyUt_ydot;
+    e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WqV;
+    e_vector m_S,m_temp,m_Wy_ydot;
     double m_lambda;
     double m_epsilon;
+	double m_qmax;
+	int m_ns;
 public:
     WDLSSolver();
     virtual ~WDLSSolver();
@@ -27,6 +29,9 @@
 	virtual void setParam(SolverParam param, double value)
 	{
 		switch (param) {
+		case DLS_QMAX:
+			m_qmax = value;
+			break;
 		case DLS_LAMBDA_MAX:
 			m_lambda = value;
 			break;

Modified: branches/itasc/intern/itasc/WSDLSSolver.hpp
===================================================================
--- branches/itasc/intern/itasc/WSDLSSolver.hpp	2009-09-04 08:49:11 UTC (rev 22986)
+++ branches/itasc/intern/itasc/WSDLSSolver.hpp	2009-09-04 10:00:25 UTC (rev 22987)
@@ -28,7 +28,7 @@
 	virtual void setParam(SolverParam param, double value)
 	{
 		switch (param) {
-		case SDLS_QMAX:
+		case DLS_QMAX:
 			m_qmax = value;
 			break;
 		}

Modified: branches/itasc/release/ui/buttons_data_bone.py
===================================================================
--- branches/itasc/release/ui/buttons_data_bone.py	2009-09-04 08:49:11 UTC (rev 22986)
+++ branches/itasc/release/ui/buttons_data_bone.py	2009-09-04 10:00:25 UTC (rev 22987)
@@ -271,8 +271,7 @@
 		flow.itemR(itasc, "num_iter")
 		flow.active = itasc.initial_reiteration or itasc.reiteration
 		
-		row = layout.row()
-		row.itemR(itasc, "auto_step")
+		layout.itemR(itasc, "auto_step")
 		
 		row = layout.row()
 		if itasc.auto_step:
@@ -282,8 +281,8 @@
 			row.itemR(itasc, "num_step")
 			
 		layout.itemR(itasc, "solver")
-		row = layout.row()
-		row.itemR(itasc, "feedback")
+		layout.itemR(itasc, "feedback")
+		layout.itemR(itasc, "max_velocity")
 		if itasc.solver == "DLS":
 			row = layout.row()
 			row.itemR(itasc, "dampmax")

Modified: branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
===================================================================
--- branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp	2009-09-04 08:49:11 UTC (rev 22986)
+++ branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp	2009-09-04 10:00:25 UTC (rev 22987)
@@ -1220,7 +1220,7 @@
 		// all constraint disabled
 		return;
 	// update parameters
-	ikscene->solver->setParam(iTaSC::Solver::SDLS_QMAX, ikparam->maxvel);
+	ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
 	// compute timestep
 	double timestamp = ctime * frtime + 2147483.648;
 	double timestep = frtime;
@@ -1418,7 +1418,7 @@
 			double armlength = ikscene->armature->getArmLength();
 			ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, ikparam->minstep);
 			ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, ikparam->maxstep);
-			ikscene->solver->setParam(iTaSC::Solver::SDLS_QMAX, ikparam->maxvel);
+			ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
 			ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax*armlength);
 			ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps*armlength);
 			ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, ikparam->feedback);

Modified: branches/itasc/source/blender/makesrna/intern/rna_pose.c
===================================================================
--- branches/itasc/source/blender/makesrna/intern/rna_pose.c	2009-09-04 08:49:11 UTC (rev 22986)
+++ branches/itasc/source/blender/makesrna/intern/rna_pose.c	2009-09-04 10:00:25 UTC (rev 22987)
@@ -785,7 +785,7 @@
 	prop= RNA_def_property(srna, "max_velocity", PROP_FLOAT, PROP_NONE);
 	RNA_def_property_float_sdna(prop, NULL, "maxvel");
 	RNA_def_property_range(prop, 0.0f,100.0f);
-	RNA_def_property_ui_text(prop, "Max vel", "Maximum joint velocity in rad/s. Default=50.");
+	RNA_def_property_ui_text(prop, "Max Velocity", "Maximum joint velocity in rad/s. Default=50.");
 	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
 
 	prop= RNA_def_property(srna, "solver", PROP_ENUM, PROP_NONE);
@@ -797,13 +797,13 @@
 	prop= RNA_def_property(srna, "dampmax", PROP_FLOAT, PROP_NONE);
 	RNA_def_property_float_sdna(prop, NULL, "dampmax");
 	RNA_def_property_range(prop, 0.0f,1.0f);
-	RNA_def_property_ui_text(prop, "Damp max", "Maximum damping coefficient when singular value is nearly 0. Default=0.5");
+	RNA_def_property_ui_text(prop, "Damp", "Maximum damping coefficient when singular value is nearly 0. Higher values=more stability, less reactivity. Default=0.5");
 	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
 
 	prop= RNA_def_property(srna, "dampeps", PROP_FLOAT, PROP_NONE);
 	RNA_def_property_float_sdna(prop, NULL, "dampeps");
 	RNA_def_property_range(prop, 0.0f,1.0f);
-	RNA_def_property_ui_text(prop, "Damp eps", "Singular value under which damping is progressively applied. Default=0.1");
+	RNA_def_property_ui_text(prop, "Epsilon", "Singular value under which damping is progressively applied. Higher values=more stability, less reactivity. Default=0.1");
 	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
 }
 





More information about the Bf-blender-cvs mailing list