Commit 7ff85af4 authored by Joseph Mirabel's avatar Joseph Mirabel

Clean MemoryTaskSOT

parent b14c0c52
......@@ -22,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 */
......
......@@ -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
......@@ -420,19 +420,14 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
taskVectorToMlVector(task.taskSOUT(iterTime), mem->err);
const dynamicgraph::Vector &err = mem->err;
Jp.resize(mJ, nJ);
Jt.resize(nJ, mJ);
JK.resize(nJ, mJ);
if ((recomputeEachTime) ||
(task.jacobianSOUT.getTime() > mem->jacobianInvSINOUT.getTime()) ||
(mem->jacobianInvSINOUT.accessCopy().rows() != mJ) ||
(mem->jacobianInvSINOUT.accessCopy().cols() != nJ) ||
(task.jacobianSOUT.getTime() >
mem->jacobianConstrainedSINOUT.getTime()) ||
(task.jacobianSOUT.getTime() > mem->rankSINOUT.getTime()) ||
(task.jacobianSOUT.getTime() >
mem->singularBaseImageSINOUT.getTime())) {
(task.jacobianSOUT.getTime() > mem->rankSINOUT.getTime())) {
sotDEBUG(2) << "Recompute inverse." << endl;
/* --- FIRST ALLOCS --- */
......@@ -489,8 +484,6 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
mem->jacobianConstrainedSINOUT.setTime(iterTime);
mem->jacobianProjectedSINOUT = Jt;
mem->jacobianProjectedSINOUT.setTime(iterTime);
mem->singularBaseImageSINOUT = svd.matrixV().leftCols(rankJ);
mem->singularBaseImageSINOUT.setTime(iterTime);
mem->rankSINOUT = rankJ;
mem->rankSINOUT.setTime(iterTime);
......
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