| Line |
Branch |
Exec |
Source |
| 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 |
| 39 |
|
|
|