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