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 |
|
|
} |