[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