integrator.hh

Go to the documentation of this file.

// BSD 2-Clause License

// Copyright (c) 2023, CNRS
// Author: Florent Lamiraux

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:

// * Redistributions of source code must retain the above copyright
//   notice, this list of conditions and the following disclaimer.

// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in
//   the documentation and/or other materials provided with the
//   distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef SOT_DYNAMIC_PINOCCHIO_INTEGRATOR_HH
#define SOT_DYNAMIC_PINOCCHIO_INTEGRATOR_HH

#include <pinocchio/algorithm/joint-configuration.hpp>
#include <sot/core/config.hh>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-ptr.h>
#include "sot/core/periodic-call.hh"

namespace dynamicgraph {
namespace sot {
namespace internal {
class Signal : public ::dynamicgraph::Signal<Vector, int> {
 protected:
  enum SignalType { CONSTANT, REFERENCE, REFERENCE_NON_CONST, FUNCTION };
  static const SignalType SIGNAL_TYPE_DEFAULT = CONSTANT;

  const Vector *Treference;
  Vector *TreferenceNonConst;
  boost::function2<Vector &, Vector &, int> Tfunction;

  bool keepReference;
  const static bool KEEP_REFERENCE_DEFAULT = false;

 public:
#ifdef HAVE_LIBBOOST_THREAD
  typedef boost::try_mutex Mutex;
  typedef boost::lock_error MutexError;
#else
  typedef int *Mutex;
  typedef int *MutexError;
#endif

 protected:
  Mutex *providerMutex;
  using SignalBase<int>::signalTime;

 public:
  using SignalBase<int>::setReady;

 public:
  /* --- Constructor/destrusctor --- */
  Signal(std::string name);
  virtual ~Signal() {}

  /* --- Generic In/Out function --- */
  virtual void get(std::ostream &value) const;
  virtual void set(std::istringstream &value);
  virtual void trace(std::ostream &os) const;

  /* --- Generic Set function --- */
  virtual void setConstant(const Vector &t);
  virtual void setReference(const Vector *t, Mutex *mutexref = NULL);
  virtual void setReferenceNonConstant(Vector *t, Mutex *mutexref = NULL);
  virtual void setFunction(boost::function2<Vector &, Vector &, int> t,
                           Mutex *mutexref = NULL);

  /* --- Signal computation --- */
  virtual const Vector &access(const int &t);
  virtual inline void recompute(const int &t) { access(t); }
  virtual const Vector &accessCopy() const;

  virtual std::ostream &display(std::ostream &os) const;

  /* --- Operators --- */
  virtual inline const Vector &operator()(const int &t) { return access(t); }
  virtual Signal &operator=(const Vector &t);
  inline operator const Vector &() const { return accessCopy(); }
  virtual void getClassName(std::string &aClassName) const {
    aClassName = typeid(this).name();
  }
};

}  // namespace internal
// Integrates a constant velocity for a given timestep
//
// Initial and final configurations as well as velocity follow pinocchio
// standard
// The timestep is the time elapsed since last computation of the output in
// microseconds.
class SOT_CORE_DLLEXPORT Integrator : public Entity {
 public:
  // Time corresponding to incrementing signal velocity by 1
  static const double dt;
  static const std::string CLASS_NAME;
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
  Integrator(const std::string &name);

  // Get pointer to the model
  ::pinocchio::Model *getModel();
  // Set pointer to the model
  void setModel(::pinocchio::Model *model);
  // Set Initial configuration
  void setInitialConfig(const Vector &initConfig);

  PeriodicCall &periodicCallBefore() { return periodicCallBefore_; }
  PeriodicCall &periodicCallAfter() { return periodicCallAfter_; }

 private:
  PeriodicCall periodicCallBefore_;
  PeriodicCall periodicCallAfter_;

  Vector &integrate(Vector &configuration, int time);
  // Signals
  SignalPtr<Vector, int> velocitySIN_;
  internal::Signal configurationSOUT_;
  // Pointer to pinocchio model
  ::pinocchio::Model *model_;
  Vector configuration_;
  int lastComputationTime_;
  int recursivityLevel_;
};  // class Integrator

}  // namespace sot
}  // namespace dynamicgraph
#endif  // SOT_DYNAMIC_PINOCCHIO_INTEGRATOR_HH