integrator.cpp
Go to the documentation of this file.
1 // BSD 2-Clause License
2 
3 // Copyright (c) 2023, CNRS
4 // Author: Florent Lamiraux
5 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions
8 // are met:
9 
10 // * Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 
13 // * Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in
15 // the documentation and/or other materials provided with the
16 // distribution.
17 
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21 // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
22 // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
23 // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
24 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25 // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
26 // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
27 // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
29 // OF THE POSSIBILITY OF SUCH DAMAGE.
30 
31 #include <sot/core/integrator.hh>
32 #include <dynamic-graph/factory.h>
35 #include <dynamic-graph/signal.h>
36 
37 namespace dynamicgraph {
38 namespace sot {
39 namespace internal {
40 
41 Signal::Signal(std::string name) : ::dynamicgraph::Signal<Vector, int>(name) {}
42 
43 /* ------------------------------------------------------------------------ */
44 
45 void Signal::set(std::istringstream &stringValue) {
46  (*this) = signal_io<Vector>::cast(stringValue);
47 }
48 
49 void Signal::get(std::ostream &os) const {
51 }
52 
53 void Signal::trace(std::ostream &os) const {
54  try {
56  } catch DG_RETHROW catch (...) {
58  "TRACE operation not possible with this signal. ",
59  "(bad cast while getting value from %s).",
60  SignalBase<int>::getName().c_str());
61  }
62 }
63 
64 void Signal::setConstant(const Vector &) {
65  throw std::runtime_error("Not implemented.");
66 }
67 
68 void Signal::setReference(const Vector *, Mutex *) {
69  throw std::runtime_error("Not implemented.");
70 }
71 
73  throw std::runtime_error("Not implemented.");
74 }
75 
76 void Signal::setFunction(boost::function2<Vector &, Vector &, int> t,
77  Mutex *mutexref) {
79  Tfunction = t;
80  providerMutex = mutexref;
81  copyInit = false;
82  setReady();
83 }
84 
85 const Vector &Signal::accessCopy() const { return Tcopy1; }
86 
87 const Vector &Signal::access(const int &t) {
88  if (NULL == providerMutex) {
89  signalTime = t;
90  Tfunction(Tcopy1, t);
91  return Tcopy1;
92  } else {
93  try {
94 #ifdef HAVE_LIBBOOST_THREAD
95  boost::try_mutex::scoped_try_lock lock(*providerMutex);
96 #endif
97  signalTime = t;
98  Tfunction(Tcopy1, t);
99  return Tcopy1;
100  } catch (const MutexError &) {
101  return accessCopy();
102  }
103  }
104 }
105 
107  throw std::runtime_error("Output signal cannot be assigned a value.");
108  return *this;
109 }
110 
111 std::ostream &Signal::display(std::ostream &os) const {
112  os << "Sig:" << this->name << " (Type ";
113  switch (this->signalType) {
114  case Signal::CONSTANT:
115  os << "Cst";
116  break;
117  case Signal::REFERENCE:
118  os << "Ref";
119  break;
121  os << "RefNonCst";
122  break;
123  case Signal::FUNCTION:
124  os << "Fun";
125  break;
126  }
127  return os << ")";
128 }
129 
130 } // namespace internal
132 
133 const double Integrator::dt = 1e-6;
134 
135 Integrator::Integrator(const std::string &name)
136  : Entity(name),
137  velocitySIN_(0x0, "Integrator(" + name + ")::input(vector)::velocity"),
138  configurationSOUT_("Integrator(" + name +
139  ")::output(vector)::configuration"),
140  model_(0x0),
141  configuration_(),
142  lastComputationTime_(-1),
143  recursivityLevel_(0) {
145  boost::bind(&Integrator::integrate, this, _1, _2));
148 }
149 
151 
153  model_ = model;
154  configuration_.resize(model->nq);
155  ::pinocchio::neutral(*model_, configuration_);
156 }
157 
158 void Integrator::setInitialConfig(const Vector &initConfig) {
159  configuration_ = initConfig;
160 }
161 
162 Vector &Integrator::integrate(Vector &configuration, int time) {
164  if (recursivityLevel_ == 2) {
165  configuration = configuration_;
167  return configuration;
168  }
169  configuration.resize(configuration_.size());
170  assert(model_);
171  Vector velocity(velocitySIN_(time));
172  // Run Synchronous commands and evaluate signals outside the main
173  // connected component of the graph.
174  try {
175  periodicCallBefore_.run(time);
176  } catch (const std::exception &e) {
177  dgRTLOG() << "exception caught while running periodical commands (before): "
178  << e.what() << std::endl;
179  }
180  if (lastComputationTime_ == -1 && !velocity.isZero(0)) {
181  recursivityLevel_ = 0;
182  std::ostringstream os;
183  os << "Integrator entity expects zero velocity input for the first "
184  "computation. Got instead "
185  << velocity.transpose() << ".";
186  throw std::runtime_error(os.str().c_str());
187  }
188  double delta_t = dt * (time - lastComputationTime_);
189  ::pinocchio::integrate(*model_, configuration_, delta_t * velocity,
190  configuration);
191  configuration_ = configuration;
192  lastComputationTime_ = time;
193  try {
194  periodicCallAfter_.run(time);
195  } catch (const std::exception &e) {
196  dgRTLOG() << "exception caught while running periodical commands (after): "
197  << e.what() << std::endl;
198  }
200  return configuration;
201 }
202 
203 } // namespace sot
204 } // namespace dynamicgraph
Eigen::VectorXd Vector
void setModel(::pinocchio::Model *model)
Definition: integrator.cpp:152
virtual void get(std::ostream &value) const
Definition: integrator.cpp:49
Vector & integrate(Vector &configuration, int time)
Definition: integrator.cpp:162
void signalRegistration(const SignalArray< int > &signals)
virtual void setReference(const Vector *t, Mutex *mutexref=NULL)
Definition: integrator.cpp:68
internal::Signal configurationSOUT_
Definition: integrator.hh:137
::pinocchio::Model * model_
Definition: integrator.hh:139
::pinocchio::Model * getModel()
Definition: integrator.cpp:150
virtual void setConstant(const Vector &t)
Definition: integrator.cpp:64
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture")
static T cast(std::istringstream &is)
virtual void setReferenceNonConstant(Vector *t, Mutex *mutexref=NULL)
Definition: integrator.cpp:72
virtual const Vector & accessCopy() const
Definition: integrator.cpp:85
virtual void setFunction(boost::function2< Vector &, Vector &, int > t, Mutex *mutexref=NULL)
Definition: integrator.cpp:76
Integrator(const std::string &name)
Definition: integrator.cpp:135
virtual void set(std::istringstream &value)
Definition: integrator.cpp:45
static const double dt
Definition: integrator.hh:115
SignalPtr< Vector, int > velocitySIN_
Definition: integrator.hh:136
virtual std::ostream & display(std::ostream &os) const
Definition: integrator.cpp:111
boost::function2< Vector &, Vector &, int > Tfunction
Definition: integrator.hh:51
virtual Signal & operator=(const Vector &t)
Definition: integrator.cpp:106
#define DG_RETHROW
#define dgRTLOG()
static void disp(const T &value, std::ostream &os)
Transform3f t
#define DG_THROW
virtual void trace(std::ostream &os) const
Definition: integrator.cpp:53
void setReady(const bool sready=true)
void setInitialConfig(const Vector &initConfig)
Definition: integrator.cpp:158
static void trace(const T &value, std::ostream &os)
virtual const Vector & access(const int &t)
Definition: integrator.cpp:87
x0


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26