Skip to content

Commit c761031

Browse files
florent-lamirauxflorent-lamiraux
committed
[Integrator] Implement specific type of signals.
 - for a mysterious reason, defining an output Signal with callback function does not work.
1 parent da9caf5 commit c761031

File tree

3 files changed

+223
-9
lines changed

3 files changed

+223
-9
lines changed

‎include/sot/core/integrator.hh

Lines changed: 76 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -34,12 +34,76 @@
3434
#include <pinocchio/algorithm/joint-configuration.hpp>
3535
#include <sot/core/config.hh>
3636
#include <dynamic-graph/entity.h>
37-
#include <dynamic-graph/signal-ptr.t.cpp>
38-
#include <dynamic-graph/signal-time-dependent.h>
37+
#include <dynamic-graph/signal.h>
38+
#include <dynamic-graph/signal-ptr.h>
39+
#include "sot/core/periodic-call.hh"
3940

4041
namespace dynamicgraph {
4142
namespace sot{
43+
namespace internal{
44+
class Signal : public ::dynamicgraph::Signal<Vector, int> {
45+
protected:
46+
enum SignalType { CONSTANT, REFERENCE, REFERENCE_NON_CONST, FUNCTION };
47+
static const SignalType SIGNAL_TYPE_DEFAULT = CONSTANT;
4248

49+
const Vector *Treference;
50+
Vector *TreferenceNonConst;
51+
boost::function2<Vector &, Vector &, int> Tfunction;
52+
53+
bool keepReference;
54+
const static bool KEEP_REFERENCE_DEFAULT = false;
55+
56+
public:
57+
#ifdef HAVE_LIBBOOST_THREAD
58+
typedef boost::try_mutex Mutex;
59+
typedef boost::lock_error MutexError;
60+
#else
61+
typedef int *Mutex;
62+
typedef int *MutexError;
63+
#endif
64+
65+
protected:
66+
Mutex *providerMutex;
67+
using SignalBase<int>::signalTime;
68+
69+
public:
70+
using SignalBase<int>::setReady;
71+
72+
public:
73+
/* --- Constructor/destrusctor --- */
74+
Signal(std::string name);
75+
virtual ~Signal() {}
76+
77+
/* --- Generic In/Out function --- */
78+
virtual void get(std::ostream &value) const;
79+
virtual void set(std::istringstream &value);
80+
virtual void trace(std::ostream &os) const;
81+
82+
/* --- Generic Set function --- */
83+
virtual void setConstant(const Vector &t);
84+
virtual void setReference(const Vector *t, Mutex *mutexref = NULL);
85+
virtual void setReferenceNonConstant(Vector *t, Mutex *mutexref = NULL);
86+
virtual void setFunction(boost::function2<Vector &, Vector &, int> t,
87+
Mutex *mutexref = NULL);
88+
89+
/* --- Signal computation --- */
90+
virtual const Vector &access(const int &t);
91+
virtual inline void recompute(const int &t) { access(t); }
92+
virtual const Vector &accessCopy() const;
93+
94+
virtual std::ostream &display(std::ostream &os) const;
95+
96+
/* --- Operators --- */
97+
virtual inline const Vector &operator()(const int &t) { return access(t); }
98+
virtual Signal &operator=(const Vector &t);
99+
inline operator const Vector &() const { return accessCopy(); }
100+
virtual void getClassName(std::string &aClassName) const {
101+
aClassName = typeid(this).name();
102+
}
103+
104+
};
105+
106+
} // namespace internal
43107
// Integrates a constant velocity for a given timestep
44108
//
45109
// Initial and final configurations as well as velocity follow pinocchio
@@ -54,23 +118,30 @@ public:
54118
static const std::string CLASS_NAME;
55119
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
56120
Integrator(const std::string& name);
57-
121+
58122
// Get pointer to the model
59123
::pinocchio::Model* getModel();
60124
// Set pointer to the model
61125
void setModel(::pinocchio::Model* model);
62126
// Set Initial configuration
63127
void setInitialConfig(const Vector& initConfig);
64-
128+
129+
PeriodicCall &periodicCallBefore() { return periodicCallBefore_; }
130+
PeriodicCall &periodicCallAfter() { return periodicCallAfter_; }
131+
65132
private:
133+
PeriodicCall periodicCallBefore_;
134+
PeriodicCall periodicCallAfter_;
135+
66136
Vector& integrate(Vector& configuration, int time);
67137
// Signals
68138
SignalPtr<Vector, int> velocitySIN_;
69-
SignalTimeDependent<Vector, int> configurationSOUT_;
139+
internal::Signal configurationSOUT_;
70140
// Pointer to pinocchio model
71141
::pinocchio::Model* model_;
72142
Vector configuration_;
73143
int lastComputationTime_;
144+
int recursivityLevel_;
74145
}; // class Integrator
75146

76147
} // namespace sot

‎src/python-module.cc

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -112,4 +112,15 @@ BOOST_PYTHON_MODULE(wrap) {
112112
bp::make_function(&dgs::Integrator::setModel))
113113
.def("setModel", &dgs::Integrator::setModel)
114114
.def("setInitialConfig", &dgs::Integrator::setInitialConfig);
115+
116+
typedef dgs::internal::Signal S_t;
117+
bp::class_<S_t, bp::bases<dg::Signal<dg::Vector, int> >, boost::noncopyable> obj(
118+
"SignalIntegratorVector", bp::init<std::string>());
119+
obj.add_property(
120+
"value",
121+
bp::make_function(&S_t::accessCopy,
122+
bp::return_value_policy<bp::copy_const_reference>()),
123+
&S_t::setConstant, // TODO check the setter
124+
"the signal value.\n"
125+
"warning: for Eigen objects, sig.value[0] = 1. may not work).");
115126
}

‎src/tools/integrator.cpp

Lines changed: 136 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -30,22 +30,131 @@
3030

3131
#include <sot/core/integrator.hh>
3232
#include <dynamic-graph/factory.h>
33+
#include <dynamic-graph/real-time-logger.h>
34+
#include <dynamic-graph/signal-caster.h>
35+
#include <dynamic-graph/signal.h>
3336

3437
namespace dynamicgraph {
3538
namespace sot{
39+
namespace internal {
3640

41+
Signal::Signal(std::string name) :
42+
::dynamicgraph::Signal<Vector, int>(name)
43+
{
44+
}
45+
46+
/* ------------------------------------------------------------------------ */
47+
48+
49+
void Signal::set(std::istringstream &stringValue) {
50+
(*this) = signal_io<Vector>::cast(stringValue);
51+
}
52+
53+
54+
void Signal::get(std::ostream &os) const {
55+
signal_io<Vector>::disp(this->accessCopy(), os);
56+
}
57+
58+
59+
void Signal::trace(std::ostream &os) const {
60+
try {
61+
signal_io<Vector>::trace(this->accessCopy(), os);
62+
} catch DG_RETHROW catch (...) {
63+
DG_THROW ExceptionSignal(ExceptionSignal::SET_IMPOSSIBLE,
64+
"TRACE operation not possible with this signal. ",
65+
"(bad cast while getting value from %s).",
66+
SignalBase<int>::getName().c_str());
67+
}
68+
}
69+
70+
void Signal::setConstant(const Vector &) {
71+
throw std::runtime_error("Not implemented.");
72+
}
73+
74+
75+
void Signal::setReference(const Vector *, Mutex *) {
76+
throw std::runtime_error("Not implemented.");
77+
}
78+
79+
80+
void Signal::setReferenceNonConstant(Vector *, Mutex *) {
81+
throw std::runtime_error("Not implemented.");
82+
}
83+
84+
85+
void Signal::setFunction(boost::function2<Vector &, Vector &, int> t,
86+
Mutex *mutexref) {
87+
signalType = ::dynamicgraph::Signal<Vector, int>::FUNCTION;
88+
Tfunction = t;
89+
providerMutex = mutexref;
90+
copyInit = false;
91+
setReady();
92+
}
93+
94+
95+
const Vector &Signal::accessCopy() const {
96+
return Tcopy1;
97+
}
98+
99+
100+
const Vector &Signal::access(const int &t) {
101+
if (NULL == providerMutex) {
102+
signalTime = t;
103+
Tfunction(Tcopy1, t);
104+
return Tcopy1;
105+
} else {
106+
try {
107+
#ifdef HAVE_LIBBOOST_THREAD
108+
boost::try_mutex::scoped_try_lock lock(*providerMutex);
109+
#endif
110+
signalTime = t;
111+
Tfunction(Tcopy1, t);
112+
return Tcopy1;
113+
} catch (const MutexError &) {
114+
return accessCopy();
115+
}
116+
}
117+
}
118+
119+
120+
Signal &Signal::operator=(const Vector &t) {
121+
throw std::runtime_error("Output signal cannot be assigned a value.");
122+
return *this;
123+
}
124+
125+
126+
std::ostream &Signal::display(std::ostream &os) const {
127+
os << "Sig:" << this->name << " (Type ";
128+
switch (this->signalType) {
129+
case Signal::CONSTANT:
130+
os << "Cst";
131+
break;
132+
case Signal::REFERENCE:
133+
os << "Ref";
134+
break;
135+
case Signal::REFERENCE_NON_CONST:
136+
os << "RefNonCst";
137+
break;
138+
case Signal::FUNCTION:
139+
os << "Fun";
140+
break;
141+
}
142+
return os << ")";
143+
}
144+
145+
} // namespace internal
37146
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Integrator, "Integrator");
38147

39148
const double Integrator::dt = 1e-6;
40149

41150
Integrator::Integrator(const std::string& name) :
42151
Entity(name),
43152
velocitySIN_(0x0, "Integrator(" + name + ")::input(vector)::velocity"),
44-
configurationSOUT_(boost::bind(&Integrator::integrate, this, _1, _2),
45-
velocitySIN_, "Integrator(" + name +
46-
")::output(vector)::configuration"), model_(0x0),
47-
configuration_(), lastComputationTime_(-1)
153+
configurationSOUT_("Integrator(" + name + ")::output(vector)::configuration"),
154+
model_(0x0), configuration_(), lastComputationTime_(-1), recursivityLevel_(0)
48155
{
156+
configurationSOUT_.setFunction(boost::bind(&Integrator::integrate, this, _1,
157+
_2));
49158
signalRegistration(velocitySIN_);
50159
signalRegistration(configurationSOUT_);
51160
}
@@ -69,9 +178,25 @@ void Integrator::setInitialConfig(const Vector& initConfig)
69178

70179
Vector& Integrator::integrate(Vector& configuration, int time)
71180
{
181+
++recursivityLevel_;
182+
if (recursivityLevel_ == 2){
183+
configuration = configuration_;
184+
--recursivityLevel_;
185+
return configuration;
186+
}
187+
configuration.resize(configuration_.size());
72188
assert(model_);
73189
Vector velocity(velocitySIN_(time));
190+
// Run Synchronous commands and evaluate signals outside the main
191+
// connected component of the graph.
192+
try {
193+
periodicCallBefore_.run(time);
194+
} catch (const std::exception &e) {
195+
dgRTLOG() << "exception caught while running periodical commands (before): "
196+
<< e.what() << std::endl;
197+
}
74198
if (lastComputationTime_ == -1 && !velocity.isZero(0)) {
199+
recursivityLevel_ = 0;
75200
std::ostringstream os;
76201
os << "Integrator entity expects zero velocity input for the first "
77202
"computation. Got instead " << velocity.transpose() << ".";
@@ -82,6 +207,13 @@ Vector& Integrator::integrate(Vector& configuration, int time)
82207
configuration);
83208
configuration_ = configuration;
84209
lastComputationTime_ = time;
210+
try {
211+
periodicCallAfter_.run(time);
212+
} catch (const std::exception &e) {
213+
dgRTLOG() << "exception caught while running periodical commands (after): "
214+
<< e.what() << std::endl;
215+
}
216+
--recursivityLevel_;
85217
return configuration;
86218
}
87219

0 commit comments

Comments
 (0)