1 |
|
|
/* |
2 |
|
|
* Copyright 2010, |
3 |
|
|
* François Bleibel, |
4 |
|
|
* Olivier Stasse, |
5 |
|
|
* |
6 |
|
|
* CNRS/AIST |
7 |
|
|
* |
8 |
|
|
*/ |
9 |
|
|
|
10 |
|
|
#include <sot/core/debug.hh> |
11 |
|
|
#include <sot/core/matrix-svd.hh> |
12 |
|
|
#include <sot/core/memory-task-sot.hh> |
13 |
|
|
using namespace dynamicgraph::sot; |
14 |
|
|
using namespace dynamicgraph; |
15 |
|
|
|
16 |
|
|
MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ) |
17 |
|
|
: kernel(NULL, 0, 0) { |
18 |
|
|
initMemory(nJ, mJ); |
19 |
|
|
} |
20 |
|
|
|
21 |
|
|
void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) { |
22 |
|
|
err.resize(nJ); |
23 |
|
|
tmpTask.resize(nJ); |
24 |
|
|
tmpVar.resize(mJ); |
25 |
|
|
tmpControl.resize(mJ); |
26 |
|
|
Jt.resize(nJ, mJ); |
27 |
|
|
|
28 |
|
|
JK.resize(nJ, mJ); |
29 |
|
|
|
30 |
|
|
svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV); |
31 |
|
|
// If the constraint is well conditioned, the kernel can be pre-allocated. |
32 |
|
|
if (mJ > nJ) kernelMem.resize(mJ - nJ, mJ); |
33 |
|
|
|
34 |
|
|
JK.setZero(); |
35 |
|
|
Jt.setZero(); |
36 |
|
|
} |
37 |
|
|
|
38 |
|
|
void MemoryTaskSOT::display(std::ostream& /*os*/) const {} // TODO |