GCC Code Coverage Report

Directory: ./
File: src/control/control-gr.cpp
Date: 2024-11-13 12:35:17
Exec Total Coverage
Lines: 0 34 0.0%
Branches: 0 52 0.0%
Line Branch Exec Source
1 /*
2 * Copyright 2010,
3 * François Bleibel,
4 * Olivier Stasse,
5 *
6 * CNRS/AIST
7 *
8 */
9
10 /* SOT */
11
12 #include <sot/core/debug.hh>
13 class ControlGR__INIT {
14 public:
15 ✗ ControlGR__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
16 };
17 ControlGR__INIT ControlGR_initiator;
18 /* --------------------------------------------------------------------- */
19 /* --------------------------------------------------------------------- */
20 /* --------------------------------------------------------------------- */
21 #include <dynamic-graph/factory.h>
22
23 #include <sot/core/binary-op.hh>
24 #include <sot/core/control-gr.hh>
25
26 using namespace dynamicgraph;
27 using namespace dynamicgraph::sot;
28
29 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlGR, "ControlGR");
30
31 const double ControlGR::TIME_STEP_DEFAULT = .001;
32
33 /* --------------------------------------------------------------------- */
34 /* --------------------------------------------------------------------- */
35 /* --------------------------------------------------------------------- */
36
37 #define __SOT_ControlGR_INIT
38
39 ✗ ControlGR::ControlGR(const std::string &name)
40 : Entity(name),
41 ✗ TimeStep(0),
42 ✗ matrixASIN(NULL, "ControlGR(" + name + ")::input(matrix)::matrixA"),
43 ✗ accelerationSIN(NULL,
44 ✗ "ControlGR(" + name + ")::input(vector)::acceleration"),
45 ✗ gravitySIN(NULL, "ControlGR(" + name + ")::input(vector)::gravity"),
46 ✗ controlSOUT(boost::bind(&ControlGR::computeControl, this, _1, _2),
47 ✗ matrixASIN << accelerationSIN << gravitySIN,
48 ✗ "ControlGR(" + name + ")::output(vector)::control") {
49 ✗ init(TimeStep);
50 ✗ Entity::signalRegistration(matrixASIN << accelerationSIN << gravitySIN
51 ✗ << controlSOUT);
52 }
53
54 /* --------------------------------------------------------------------- */
55 /* --------------------------------------------------------------------- */
56 /* --------------------------------------------------------------------- */
57
58 ✗ void ControlGR::init(const double &Stept) {
59 ✗ TimeStep = Stept;
60
61 ✗ return;
62 }
63
64 /* --------------------------------------------------------------------- */
65 /* --------------------------------------------------------------------- */
66 /* --------------------------------------------------------------------- */
67
68 ✗ void ControlGR::display(std::ostream &os) const {
69 ✗ os << "ControlGR " << getName();
70 try {
71 ✗ os << "control = " << controlSOUT;
72 ✗ } catch (ExceptionSignal e) {
73 }
74 ✗ os << " (" << TimeStep << ") ";
75 }
76
77 /* --------------------------------------------------------------------- */
78 /* --------------------------------------------------------------------- */
79 /* --------------------------------------------------------------------- */
80
81 ✗ double &ControlGR::setsize(int dimension)
82
83 {
84 ✗ _dimension = dimension;
85 ✗ return _dimension;
86 }
87
88 ✗ dynamicgraph::Vector &ControlGR::computeControl(dynamicgraph::Vector &tau,
89 int t) {
90 sotDEBUGIN(15);
91
92 ✗ const dynamicgraph::Matrix &matrixA = matrixASIN(t);
93 ✗ const dynamicgraph::Vector &acceleration = accelerationSIN(t);
94 ✗ const dynamicgraph::Vector &gravity = gravitySIN(t);
95 ✗ dynamicgraph::Vector::Index size = acceleration.size();
96 ✗ tau.resize(size);
97 // tau*=0;
98 /* for(unsigned i = 0u; i < size; ++i)
99 {
100 tp = gravity(i);
101 tau(i) = acceleration;
102 tau(i) *= matrixA;
103 tau(i) += tp;
104 }*/
105
106 ✗ tau = matrixA * acceleration;
107 sotDEBUG(15) << "torque = A*ddot(q)= " << matrixA * acceleration << std::endl;
108 ✗ tau += gravity;
109
110 /*
111 tau(1) *= 0;
112 tau(7) *= 0;
113 tau(24) *=0;
114 tau(17)=0;*/
115
116 sotDEBUG(15) << "matrixA =" << matrixA << std::endl;
117 sotDEBUG(15) << "acceleration =" << acceleration << std::endl;
118 sotDEBUG(15) << "gravity =" << gravity << std::endl;
119 sotDEBUG(15) << "gravity compensation torque =" << tau << std::endl;
120 sotDEBUGOUT(15);
121
122 ✗ return tau;
123 }
124