GCC Code Coverage Report


Directory: ./
File: src/feedback-controller.cpp
Date: 2024-11-12 12:07:01
Exec Total Coverage
Lines: 0 32 0.0%
Branches: 0 80 0.0%

Line Branch Exec Source
1 /*
2 * Copyright 2010 CNRS
3 *
4 * Florent Lamiraux
5 */
6
7 #include "dynamic-graph/tutorial/feedback-controller.hh"
8
9 #include <dynamic-graph/command-getter.h>
10 #include <dynamic-graph/command-setter.h>
11 #include <dynamic-graph/factory.h>
12
13 #include "constant.hh"
14
15 using namespace dynamicgraph;
16 using namespace dynamicgraph::tutorial;
17
18 // Register new Entity type in the factory
19 // Note that the second argument is the type name of the python class
20 // that will be created when importing the python module.
21 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeedbackController, "FeedbackController");
22
23 FeedbackController::FeedbackController(const std::string& inName)
24 : Entity(inName),
25 stateSIN(NULL,
26 "FeedbackController(" + inName + ")::input(vector)::state"),
27 forceSOUT(stateSIN,
28 "FeedbackController(" + inName + ")::output(double)::force"),
29 gain_(Matrix(4, 1)) {
30 // Register signals into the entity.
31 signalRegistration(stateSIN);
32 signalRegistration(forceSOUT);
33
34 // Set signals as constant to size them
35 double force = 0.;
36 Vector state(4);
37 state.fill(0.);
38 forceSOUT.setConstant(force);
39 stateSIN.setConstant(state);
40
41 // Define refresh function for output signal
42 boost::function2<double&, double&, const int&> ftest =
43 boost::bind(&FeedbackController::computeForceFeedback, this, _1, _2);
44
45 forceSOUT.setFunction(
46 boost::bind(&FeedbackController::computeForceFeedback, this, _1, _2));
47 std::string docstring;
48 // setGain
49 docstring =
50 "\n"
51 " Set gain of controller\n"
52 " takes a tuple of 4 floating point numbers as input\n"
53 "\n";
54 addCommand(std::string("setGain"),
55 new ::dynamicgraph::command::Setter<FeedbackController, Matrix>(
56 *this, &FeedbackController::setGain, docstring));
57
58 // getGain
59 docstring =
60 "\n"
61 " Get gain of controller\n"
62 " return a tuple of 4 floating point numbers\n"
63 "\n";
64 addCommand(std::string("getGain"),
65 new ::dynamicgraph::command::Getter<FeedbackController, Matrix>(
66 *this, &FeedbackController::getGain, docstring));
67 }
68
69 FeedbackController::~FeedbackController() {}
70
71 double& FeedbackController::computeForceFeedback(double& force,
72 const int& inTime) {
73 const Vector& state = stateSIN(inTime);
74
75 if (state.size() != 4)
76 throw dynamicgraph::ExceptionSignal(dynamicgraph::ExceptionSignal::GENERIC,
77 "state signal size is ",
78 "%d, should be 4.", state.size());
79 Vector v(-gain_ * state);
80 force = v(0);
81 return force;
82 }
83