Speed optimization in itasc when with armature with many bones and few targets. Thanks to Brecht who pointed out a simple but efficient optimization in SVD decomposition.
This commit is contained in:
parent
2fef3dbaa3
commit
0cbc87b428
|
@ -24,12 +24,22 @@ bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector<bool>&
|
|||
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_WyAWqt = e_zero_matrix(nq,nc);
|
||||
m_S = e_zero_vector(std::max(nc,nq));
|
||||
m_temp = e_zero_vector(nq);
|
||||
m_V = e_zero_matrix(nq,nq);
|
||||
m_WqV = e_zero_matrix(nq,nq);
|
||||
m_Wy_ydot = e_zero_vector(nc);
|
||||
if (nq > nc) {
|
||||
m_transpose = true;
|
||||
m_temp = e_zero_vector(nc);
|
||||
m_U = e_zero_matrix(nc,nc);
|
||||
m_V = e_zero_matrix(nq,nc);
|
||||
m_WqV = e_zero_matrix(nq,nc);
|
||||
} else {
|
||||
m_transpose = false;
|
||||
m_temp = e_zero_vector(nq);
|
||||
m_U = e_zero_matrix(nc,nq);
|
||||
m_V = e_zero_matrix(nq,nq);
|
||||
m_WqV = e_zero_matrix(nq,nq);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -42,7 +52,13 @@ bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& yd
|
|||
m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
|
||||
|
||||
// Compute the SVD of the weighted jacobian
|
||||
int ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
|
||||
int ret;
|
||||
if (m_transpose) {
|
||||
m_WyAWqt = m_WyAWq.transpose();
|
||||
ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
|
||||
} else {
|
||||
ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
|
||||
}
|
||||
if(ret<0)
|
||||
return false;
|
||||
|
||||
|
|
|
@ -14,12 +14,13 @@ namespace iTaSC {
|
|||
|
||||
class WDLSSolver: public iTaSC::Solver {
|
||||
private:
|
||||
e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WqV;
|
||||
e_matrix m_AWq,m_WyAWq,m_WyAWqt,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;
|
||||
bool m_transpose;
|
||||
public:
|
||||
WDLSSolver();
|
||||
virtual ~WDLSSolver();
|
||||
|
|
|
@ -31,13 +31,23 @@ bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<boo
|
|||
m_ns = std::min(m_nc,m_nq);
|
||||
m_AWq = e_zero_matrix(m_nc,m_nq);
|
||||
m_WyAWq = e_zero_matrix(m_nc,m_nq);
|
||||
m_U = e_zero_matrix(m_nc,m_nq);
|
||||
m_WyAWqt = e_zero_matrix(m_nq,m_nc);
|
||||
m_S = e_zero_vector(std::max(m_nc,m_nq));
|
||||
m_temp = e_zero_vector(m_nq);
|
||||
m_V = e_zero_matrix(m_nq,m_nq);
|
||||
m_WqV = e_zero_matrix(m_nq,m_nq);
|
||||
m_Wy_ydot = e_zero_vector(m_nc);
|
||||
m_ytask = gc;
|
||||
if (m_nq > m_nc) {
|
||||
m_transpose = true;
|
||||
m_temp = e_zero_vector(m_nc);
|
||||
m_U = e_zero_matrix(m_nc,m_nc);
|
||||
m_V = e_zero_matrix(m_nq,m_nc);
|
||||
m_WqV = e_zero_matrix(m_nq,m_nc);
|
||||
} else {
|
||||
m_transpose = false;
|
||||
m_temp = e_zero_vector(m_nq);
|
||||
m_U = e_zero_matrix(m_nc,m_nq);
|
||||
m_V = e_zero_matrix(m_nq,m_nq);
|
||||
m_WqV = e_zero_matrix(m_nq,m_nq);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -52,7 +62,13 @@ bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& y
|
|||
m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
|
||||
|
||||
// Compute the SVD of the weighted jacobian
|
||||
int ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
|
||||
int ret;
|
||||
if (m_transpose) {
|
||||
m_WyAWqt = m_WyAWq.transpose();
|
||||
ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
|
||||
} else {
|
||||
ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
|
||||
}
|
||||
if(ret<0)
|
||||
return false;
|
||||
|
||||
|
|
|
@ -14,11 +14,12 @@ namespace iTaSC {
|
|||
|
||||
class WSDLSSolver: public iTaSC::Solver {
|
||||
private:
|
||||
e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WqV;
|
||||
e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV;
|
||||
e_vector m_S,m_temp,m_Wy_ydot;
|
||||
std::vector<bool> m_ytask;
|
||||
e_scalar m_qmax;
|
||||
unsigned int m_ns, m_nc, m_nq;
|
||||
bool m_transpose;
|
||||
public:
|
||||
WSDLSSolver();
|
||||
virtual ~WSDLSSolver();
|
||||
|
|
Loading…
Reference in New Issue