GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: tests/task/test_task.cpp Lines: 30 35 85.7 %
Date: 2023-03-13 12:09:37 Branches: 41 86 47.7 %

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 <dynamic-graph/linear-algebra.h>
14
15
#include <iostream>
16
#include <sot/core/debug.hh>
17
#include <sot/core/feature-abstract.hh>
18
#include <sot/core/feature-visual-point.hh>
19
#include <sot/core/gain-adaptive.hh>
20
#include <sot/core/sot.hh>
21
#include <sot/core/task.hh>
22
using namespace std;
23
using namespace dynamicgraph::sot;
24
25
double drand(void) { return 2 * ((double)rand()) / RAND_MAX - 1; }
26
dynamicgraph::Matrix &mrand(dynamicgraph::Matrix &J) {
27
  for (int i = 0; i < J.rows(); ++i)
28
    for (int j = 0; j < J.cols(); ++j) J(i, j) = drand();
29
  return J;
30
}
31
32
1
int main(void) {
33
1
  srand(12);
34
2
  dynamicgraph::Matrix Jq(6, 6);
35
1
  Jq.setIdentity();
36
37
1
  dynamicgraph::Vector p1xy(6);
38
1
  p1xy(0) = 1.;
39
1
  p1xy(1) = -2;
40
1
  p1xy(2) = 1.;
41
1
  p1xy(3) = 1.;
42
1
  p1xy(4) = -2;
43
1
  p1xy(5) = 1.;
44
45
1
  sotDEBUGF("Create feature");
46

1
  FeatureVisualPoint *p1 = new FeatureVisualPoint("p1");
47

1
  FeatureVisualPoint *p1des = new FeatureVisualPoint("p1d");
48
49
1
  p1->articularJacobianSIN.setReference(&Jq);
50

1
  p1->selectionSIN = Flags(true);
51
1
  p1->setReference(p1des);
52
1
  p1->xySIN = p1xy;
53
54

1
  p1des->xySIN = dynamicgraph::Vector(6);
55
56
1
  sotDEBUGF("Create Task");
57
  //  sotDEBUG(0) << dynamicgraph::MATLAB;
58
59

1
  Task *task = new Task("t");
60
1
  task->addFeature(*p1);
61
62

1
  GainAdaptive *lambda = new GainAdaptive("g");
63
1
  lambda->errorSIN.plug(&task->errorSOUT);
64
65
1
  task->controlGainSIN.plug(&lambda->gainSOUT);
66
1
  task->dampingGainSINOUT = .1;
67

1
  task->controlSelectionSIN = Flags(true);
68
69

1
  task->jacobianSOUT.display(cout) << endl;
70


1
  task->jacobianSOUT.displayDependencies(cout) << endl;
71
72
  //  sotDEBUG(0) << dynamicgraph::MATLAB << "J"<< task->jacobianSOUT(2);
73
  sotDEBUG(0) << "e" << task->errorSOUT(2) << endl;
74
75
1
  return 0;
76
}