| Directory: | ./ |
|---|---|
| File: | tests/sot/tsot.cpp |
| Date: | 2024-08-13 12:13:25 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 30 | 35 | 85.7% |
| Branches: | 38 | 80 | 47.5% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Copyright 2010, | ||
| 3 | * François Bleibel, | ||
| 4 | * Olivier Stasse, | ||
| 5 | * | ||
| 6 | * CNRS/AIST | ||
| 7 | * | ||
| 8 | */ | ||
| 9 | |||
| 10 | /* -------------------------------------------------------------------------- */ | ||
| 11 | /* --- INCLUDES ------------------------------------------------------------- */ | ||
| 12 | /* -------------------------------------------------------------------------- */ | ||
| 13 | #include <iostream> | ||
| 14 | //#include <sot/core/sot-h.hh> | ||
| 15 | #include <dynamic-graph/linear-algebra.h> | ||
| 16 | |||
| 17 | #include <sot/core/debug.hh> | ||
| 18 | #include <sot/core/feature-abstract.hh> | ||
| 19 | #include <sot/core/feature-visual-point.hh> | ||
| 20 | #include <sot/core/gain-adaptive.hh> | ||
| 21 | #include <sot/core/task.hh> | ||
| 22 | |||
| 23 | using namespace std; | ||
| 24 | using namespace dynamicgraph::sot; | ||
| 25 | |||
| 26 | ✗ | double drand(void) { return 2 * ((double)rand()) / RAND_MAX - 1; } | |
| 27 | ✗ | dynamicgraph::Matrix &mrand(dynamicgraph::Matrix &J) { | |
| 28 | ✗ | for (int i = 0; i < J.rows(); ++i) | |
| 29 | ✗ | for (int j = 0; j < J.cols(); ++j) J(i, j) = drand(); | |
| 30 | ✗ | return J; | |
| 31 | } | ||
| 32 | |||
| 33 | 1 | int main(void) { | |
| 34 | 1 | sotDEBUGF("# In {"); | |
| 35 | |||
| 36 | 1 | srand(12); | |
| 37 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | dynamicgraph::Matrix Jq(6, 6); |
| 38 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Jq.setIdentity(); |
| 39 | |||
| 40 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | dynamicgraph::Vector p1xy(2); |
| 41 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | p1xy(0) = 1.; |
| 42 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | p1xy(1) = -2; |
| 43 | |||
| 44 | 1 | sotDEBUGF("Create feature"); | |
| 45 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
1 | FeatureVisualPoint *p1 = new FeatureVisualPoint("p1"); |
| 46 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
1 | FeatureVisualPoint *p1des = new FeatureVisualPoint("p1des"); |
| 47 | |||
| 48 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | p1->articularJacobianSIN.setReference(&Jq); |
| 49 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | p1->selectionSIN = Flags(true); |
| 50 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | p1->setReference(p1des); |
| 51 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | p1->xySIN = p1xy; |
| 52 | |||
| 53 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | p1des->xySIN = dynamicgraph::Vector(2); |
| 54 | |||
| 55 | 1 | sotDEBUGF("Create Task"); | |
| 56 | // sotDEBUG(0) << dynamicgraph::MATLAB; | ||
| 57 | |||
| 58 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
1 | Task *task = new Task("task"); |
| 59 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | task->addFeature(*p1); |
| 60 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | task->addFeature(*p1); |
| 61 | |||
| 62 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
1 | GainAdaptive *lambda = new GainAdaptive("g"); |
| 63 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | lambda->errorSIN.plug(&task->errorSOUT); |
| 64 | |||
| 65 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | task->controlGainSIN.plug(&lambda->gainSOUT); |
| 66 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | task->dampingGainSINOUT = .1; |
| 67 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | task->controlSelectionSIN = Flags(true); |
| 68 | |||
| 69 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | task->jacobianSOUT.display(cout) << endl; |
| 70 |
5/10✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
1 | task->jacobianSOUT.displayDependencies(cout) << endl; |
| 71 | |||
| 72 | sotDEBUG(0) << "J" << task->jacobianSOUT(2); | ||
| 73 | sotDEBUG(0) << "e" << task->errorSOUT(2) << endl; | ||
| 74 | |||
| 75 | 1 | sotDEBUGF("# Out }"); | |
| 76 | |||
| 77 | 1 | return 0; | |
| 78 | 1 | } | |
| 79 |