memory-task-sot.cpp 867 Bytes
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1 2 3 4
/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
5
 *
Thomas Moulard's avatar
Thomas Moulard committed
6
 * CNRS/AIST
7
 *
Thomas Moulard's avatar
Thomas Moulard committed
8
 */
9

10
#include <sot/core/debug.hh>
11
#include <sot/core/matrix-svd.hh>
Guilhem Saurel's avatar
Guilhem Saurel committed
12
#include <sot/core/memory-task-sot.hh>
13
using namespace dynamicgraph::sot;
14
using namespace dynamicgraph;
15

Joseph Mirabel's avatar
Joseph Mirabel committed
16
MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ)
Olivier Stasse's avatar
Olivier Stasse committed
17
    : kernel(NULL, 0, 0) {
Joseph Mirabel's avatar
Joseph Mirabel committed
18
  initMemory(nJ, mJ);
19 20
}

Joseph Mirabel's avatar
Joseph Mirabel committed
21 22 23 24
void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) {
  err.resize(nJ);
  tmpTask.resize(nJ);
  tmpVar.resize(mJ);
Guilhem Saurel's avatar
Guilhem Saurel committed
25
  Jt.resize(nJ, mJ);
26

Guilhem Saurel's avatar
Guilhem Saurel committed
27
  JK.resize(nJ, mJ);
28

Guilhem Saurel's avatar
Guilhem Saurel committed
29
  svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
Joseph Mirabel's avatar
Joseph Mirabel committed
30
  // If the constraint is well conditioned, the kernel can be pre-allocated.
Olivier Stasse's avatar
Olivier Stasse committed
31 32
  if (mJ > nJ)
    kernelMem.resize(mJ - nJ, mJ);
33

Joseph Mirabel's avatar
Joseph Mirabel committed
34 35
  JK.setZero();
  Jt.setZero();
Guilhem Saurel's avatar
Guilhem Saurel committed
36
}
37

Guilhem Saurel's avatar
Guilhem Saurel committed
38
void MemoryTaskSOT::display(std::ostream & /*os*/) const {} // TODO