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 |