Commit ce2b3eda authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub

Merge pull request #123 from jmirabel/devel

Avoid computing the SVD of posture tasks + some optimizations.
parents cea473e0 464345b8
Subproject commit 2a9086eaf5e7ef7cd6992e0fd6c6c615c0893400
Subproject commit a3ab52d6525aa6350b657068fddb41665aec4e08
......@@ -151,7 +151,7 @@ public:
virtual void display(std::ostream &os) const;
public:
void servoCurrentPosition(void);
void servoCurrentPosition(const int& time);
private:
MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
......
......@@ -12,6 +12,7 @@
#include "sot/core/api.hh"
#include "sot/core/feature-abstract.hh"
#include <dynamic-graph/value.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
/* --------------------------------------------------------------------- */
......@@ -64,14 +65,12 @@ public:
protected:
virtual dg::Vector &computeError(dg::Vector &res, int);
virtual dg::Matrix &computeJacobian(dg::Matrix &res, int);
virtual dg::Vector &computeActivation(dg::Vector &res, int);
virtual dg::Vector &computeErrorDot(dg::Vector &res, int time);
signalIn_t state_;
signalIn_t posture_;
signalIn_t postureDot_;
signalOut_t error_;
dg::Matrix jacobian_;
private:
std::vector<bool> activeDofs_;
......
......@@ -14,25 +14,26 @@
#include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace Eigen {
namespace dynamicgraph {
void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
typedef Eigen::JacobiSVD<Matrix> SVD_t;
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold = 1e-6);
void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix,
void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix,
const double threshold = 1e-6);
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
Matrix &Uref, Vector &Sref, Matrix &Vref,
const double threshold = 1e-6);
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold = 1e-6);
} // namespace Eigen
} // namespace dynamicgraph
#endif /* #ifndef __SOT_MATRIX_SVD_H__ */
......@@ -12,6 +12,7 @@
#include "sot/core/api.hh"
#include <sot/core/task-abstract.hh>
#include <sot/core/matrix-svd.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
......@@ -21,44 +22,47 @@ namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract,
public dg::Entity {
class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract {
public: // protected:
typedef Eigen::Map<Matrix, Eigen::internal::traits<Matrix>::Alignment> Kernel_t;
typedef Eigen::Map<const Matrix, Eigen::internal::traits<Matrix>::Alignment> KernelConst_t;
/* Internal memory to reduce the dynamic allocation at task resolution. */
dg::Vector err;
dg::Vector err, tmpTask, tmpVar;
dg::Matrix Jt; //( nJ,mJ );
dg::Matrix Jp;
dg::Matrix PJp;
dg::Matrix JK; //(nJ,mJ);
dg::Matrix Proj;
typedef Eigen::JacobiSVD<dg::Matrix> SVD_t;
SVD_t svd;
Kernel_t kernel;
void resizeKernel(const Matrix::Index r, const Matrix::Index c)
{
if (kernel.rows() != r || kernel.cols() != c) {
if (kernelMem.size() < r*c) kernelMem.resize(r, c);
new (&kernel) Kernel_t(kernelMem.data(), r, c);
}
}
Kernel_t& getKernel(const Matrix::Index r, const Matrix::Index c)
{
resizeKernel(r,c);
return kernel;
}
public:
/**
* \param mJ is the number of joints
* \param nJ the number of feature in the task
**/
MemoryTaskSOT(const std::string &name, const Matrix::Index nJ = 0,
const Matrix::Index mJ = 0);
virtual void initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
bool atConstruction = false);
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display(std::ostream &os) const;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
public: /* --- SIGNALS --- */
dg::Signal<dg::Matrix, int> jacobianInvSINOUT;
dg::Signal<dg::Matrix, int> jacobianConstrainedSINOUT;
dg::Signal<dg::Matrix, int> jacobianProjectedSINOUT;
dg::Signal<dg::Matrix, int> singularBaseImageSINOUT;
dg::Signal<unsigned int, int> rankSINOUT;
MemoryTaskSOT(const Matrix::Index nJ = 0, const Matrix::Index mJ = 0);
void display(std::ostream &os) const;
private:
void initMemory(const Matrix::Index nJ, const Matrix::Index mJ);
Matrix kernelMem;
};
} /* namespace sot */
......
......@@ -75,13 +75,9 @@ protected:
command computed by the stack of tasks. */
unsigned int nbJoints;
/*! \brief Store a pointer to compute the gradient */
TaskAbstract *taskGradient;
// Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor>
// Proj;
/*! Force the recomputation at each step. */
bool recomputeEachTime;
/*! \brief Option to disable the computation of the SVD for the last task
if this task is a Task with a single FeaturePosture */
bool enablePostureTaskAcceleration;
public:
/*! \brief Threshold to compute the dumped pseudo inverse. */
......@@ -174,13 +170,14 @@ public: /* --- SIGNALS --- */
@{
*/
/*! \brief Intrinsec velocity of the robot, that is used to initialized
* the recurence of the SOT (e.g. velocity comming from the other
* the recurence of the SOT (e.g. velocity coming from the other
* OpenHRP plugins).
*/
SignalPtr<dg::Vector, int> q0SIN;
/*! \brief A matrix K whose columns are a base of the desired velocity.
* In other words, \f$ \dot{q} = K * u \f$ where \f$ u \f$ is the free
* parameter to be computed.
* \note K should be an orthonormal matrix.
*/
SignalPtr<dg::Matrix, int> proj0SIN;
/*! \brief This signal allow to change the threshold for the
......
......@@ -106,10 +106,10 @@ FeaturePose<representation>::FeaturePose(const string &pointName)
using namespace dynamicgraph::command;
addCommand(
"keep",
makeCommandVoid0(
makeCommandVoid1(
*this, &FeaturePose<representation>::servoCurrentPosition,
docCommandVoid0(
"modify the desired position to servo at current pos.")));
docCommandVoid1(
"modify the desired position to servo at current pos.", "time")));
}
}
......@@ -319,15 +319,15 @@ Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
* to the current position. The effect on the servo is to maintain the
* current position and correct any drift. */
template <Representation_t representation>
void FeaturePose<representation>::servoCurrentPosition(void) {
void FeaturePose<representation>::servoCurrentPosition(const int& time) {
check(*this);
const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.accessCopy() : Id),
const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
_jaMfa =
(jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
_oMjb = oMjb.accessCopy(),
(jaMfa.isPlugged() ? jaMfa.access(time) : Id),
_oMjb = oMjb.access(time),
_jbMfb =
(jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);
(jbMfb.isPlugged() ? jbMfb.access(time) : Id);
faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
}
......
......@@ -2,7 +2,7 @@
// JRL, CNRS/AIST.
#include <boost/assign/list_of.hpp>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h>
#include <string>
......@@ -11,7 +11,6 @@
#include <sot/core/feature-posture.hh>
namespace dg = ::dynamicgraph;
using ::dynamicgraph::command::Setter;
using dynamicgraph::sot::FeatureAbstract;
namespace dynamicgraph {
......@@ -40,10 +39,11 @@ FeaturePosture::FeaturePosture(const std::string &name)
state_(NULL, "FeaturePosture(" + name + ")::input(Vector)::state"),
posture_(0, "FeaturePosture(" + name + ")::input(Vector)::posture"),
postureDot_(0, "FeaturePosture(" + name + ")::input(Vector)::postureDot"),
jacobian_(), activeDofs_(), nbActiveDofs_(0) {
activeDofs_(), nbActiveDofs_(0) {
signalRegistration(state_ << posture_ << postureDot_);
errorSOUT.addDependency(state_);
jacobianSOUT.setConstant(Matrix());
std::string docstring;
docstring = " \n"
......@@ -83,13 +83,9 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) {
return res;
}
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &res, int) {
res = jacobian_;
return res;
}
dg::Vector &FeaturePosture::computeActivation(dg::Vector &res, int) {
return res;
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &, int) {
throw std::runtime_error ("jacobian signal should be constant."
" This function should never be called");
}
dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
......@@ -139,16 +135,17 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) {
}
}
// recompute jacobian
jacobian_.resize(nbActiveDofs_, dim);
jacobian_.setZero();
Matrix J (Matrix::Zero(nbActiveDofs_, dim));
std::size_t index = 0;
for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
if (activeDofs_[i]) {
jacobian_(index, i) = 1;
J(index, i) = 1;
index++;
}
}
jacobianSOUT.setConstant(J);
}
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture");
......
......@@ -4,14 +4,17 @@
#include <sot/core/debug.hh>
#include <sot/core/matrix-svd.hh>
namespace Eigen {
namespace dynamicgraph {
using Eigen::ComputeThinU;
using Eigen::ComputeThinV;
using Eigen::ComputeFullV;
void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) {
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues =
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
SVD_t::SingularValuesType m_singularValues =
svd.singularValues();
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
SVD_t::SingularValuesType singularValues_inv;
singularValues_inv.resizeLike(m_singularValues);
for (long i = 0; i < m_singularValues.size(); ++i) {
if (m_singularValues(i) > threshold)
......@@ -23,43 +26,34 @@ void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
svd.matrixU().transpose());
}
void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix,
void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix,
const double threshold) {
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t;
ArrayWrapper<const SV_t> sigmas(svd.singularValues());
typedef SVD_t::SingularValuesType SV_t;
Eigen::ArrayWrapper<const SV_t> sigmas(svd.singularValues());
SV_t sv_inv(sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
const dg::Matrix::Index m = sv_inv.size();
const Matrix::Index m = sv_inv.size();
_inverseMatrix.noalias() = (svd.matrixV().leftCols(m) * sv_inv.asDiagonal() *
svd.matrixU().leftCols(m).transpose());
}
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
Matrix &Uref, Vector &Sref, Matrix &Vref,
const double threshold) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
dampedInverse(svd, _inverseMatrix, threshold);
Uref = svd.matrixU();
Vref = svd.matrixV();
Sref = svd.singularValues();
sotDEBUGOUT(15);
}
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeFullV);
SVD_t svd(_inputMatrix, ComputeThinU | ComputeFullV);
dampedInverse(svd, _inverseMatrix, threshold);
sotDEBUGOUT(15);
}
} // namespace Eigen
......@@ -13,47 +13,25 @@
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
const std::string MemoryTaskSOT::CLASS_NAME = "MemoryTaskSOT";
MemoryTaskSOT::MemoryTaskSOT(const std::string &name, const Matrix::Index nJ,
const Matrix::Index mJ)
: Entity(name),
jacobianInvSINOUT("sotTaskAbstract(" + name + ")::inout(matrix)::Jinv"),
jacobianConstrainedSINOUT("sotTaskAbstract(" + name +
")::inout(matrix)::JK"),
jacobianProjectedSINOUT("sotTaskAbstract(" + name +
")::inout(matrix)::Jt"),
singularBaseImageSINOUT("sotTaskAbstract(" + name +
")::inout(matrix)::V"),
rankSINOUT("sotTaskAbstract(" + name + ")::inout(matrix)::rank") {
signalRegistration(jacobianInvSINOUT << singularBaseImageSINOUT << rankSINOUT
<< jacobianConstrainedSINOUT
<< jacobianProjectedSINOUT);
initMemory(nJ, mJ, true);
MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ)
: kernel(NULL, 0, 0) {
initMemory(nJ, mJ);
}
void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
bool atConstruction) {
sotDEBUG(15) << "Task-mermory " << getName() << ": resize " << nJ << "x" << mJ
<< std::endl;
void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) {
err.resize(nJ);
tmpTask.resize(nJ);
tmpVar.resize(mJ);
Jt.resize(nJ, mJ);
Jp.resize(mJ, nJ);
PJp.resize(mJ, nJ);
JK.resize(nJ, mJ);
svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
// If the constraint is well conditioned, the kernel can be pre-allocated.
if (mJ > nJ) kernelMem.resize(mJ-nJ, mJ);
JK.fill(0);
if (atConstruction) {
Jt.setZero();
Jp.setZero();
PJp.setZero();
JK.setZero();
} else {
Eigen::pseudoInverse(Jt, Jp);
}
JK.setZero();
Jt.setZero();
}
void MemoryTaskSOT::display(std::ostream & /*os*/) const {} // TODO
This diff is collapsed.
......@@ -110,7 +110,7 @@ void TaskConti::display(std::ostream &os) const {
<< ": " << endl;
os << "--- LIST --- " << std::endl;
for (std::list<FeatureAbstract *>::const_iterator iter = featureList.begin();
for (FeatureList_t::const_iterator iter = featureList.begin();
iter != featureList.end(); ++iter) {
os << "-> " << (*iter)->getName() << endl;
}
......
......@@ -114,7 +114,7 @@ void Task::addFeatureFromName(const std::string &featureName) {
void Task::clearFeatureList(void) {
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) {
FeatureAbstract &s = **iter;
jacobianSOUT.removeDependency(s.jacobianSOUT);
......@@ -174,7 +174,7 @@ dynamicgraph::Vector &Task::computeError(dynamicgraph::Vector &error,
int cursorError = 0;
/* For each cell of the list, recopy value of s, s_star and error. */
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) {
FeatureAbstract &feature = **iter;
......@@ -211,7 +211,7 @@ Task::computeErrorTimeDerivative(dynamicgraph::Vector &res, int time) {
res.resize(errorSOUT(time).size());
dynamicgraph::Vector::Index cursor = 0;
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) {
FeatureAbstract &feature = **iter;
......@@ -263,7 +263,7 @@ dynamicgraph::Matrix &Task::computeJacobian(dynamicgraph::Matrix &J, int time) {
// const Flags& selection = controlSelectionSIN(time);
/* For each cell of the list, recopy value of s, s_star and error. */
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) {
FeatureAbstract &feature = **iter;
sotDEBUG(25) << "Feature <" << feature.getName() << ">" << endl;
......@@ -315,14 +315,14 @@ void Task::display(std::ostream &os) const {
os << "Task " << name << ": " << endl;
os << "--- LIST --- " << std::endl;
for (std::list<FeatureAbstract *>::const_iterator iter = featureList.begin();
for (FeatureList_t::const_iterator iter = featureList.begin();
iter != featureList.end(); ++iter) {
os << "-> " << (*iter)->getName() << endl;
}
}
std::ostream &Task::writeGraph(std::ostream &os) const {
std::list<FeatureAbstract *>::const_iterator itFeatureAbstract;
FeatureList_t::const_iterator itFeatureAbstract;
itFeatureAbstract = featureList.begin();
while (itFeatureAbstract != featureList.end()) {
os << "\t\"" << (*itFeatureAbstract)->getName() << "\" -> \"" << getName()
......
......@@ -165,13 +165,13 @@ int main(int argc, char **argv) {
START_CHRONO(inv);
for (int i = 0; i < BENCH; ++i)
Eigen::pseudoInverse(M, Minv);
dynamicgraph::pseudoInverse(M, Minv);
STOP_CHRONO(inv, "init");
sotDEBUG(15) << "Minv = " << Minv << endl;
START_CHRONO(inv);
for (int i = 0; i < BENCH; ++i)
Eigen::pseudoInverse(M, Minv);
dynamicgraph::pseudoInverse(M, Minv);
STOP_CHRONO(inv, "M+standard");
cout << dt_inv << endl;
......@@ -232,7 +232,7 @@ int main(int argc, char **argv) {
// dynamicgraph::Matrix Mcreuseinv( Mcreuse.nbCols(),r );
Mcreuseinv.resize(Mcreuse.cols(), r);
Eigen::pseudoInverse(Mcreuse, Mcreuseinv);
dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv);
parc = 0;
Minv.fill(0.);
for (std::list<unsigned int>::iterator iter = nonzeros.begin();
......@@ -281,7 +281,7 @@ int main(int argc, char **argv) {
dynamicgraph::Matrix Mcreuseinv(Mcreuse.cols(), r);
START_CHRONO(inv);
for (int ib = 0; ib < BENCH; ++ib) {
Eigen::pseudoInverse(Mcreuse, Mcreuseinv);
dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv);
}
STOP_CHRONO(inv, "M+creuseseule");
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment