trajectory-base.hpp

Go to the documentation of this file.

//
// Copyright (c) 2017-2021 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
 
#ifndef __invdyn_trajectory_base_hpp__
#define __invdyn_trajectory_base_hpp__
 
#include "tsid/deprecated.hh"
#include "tsid/macros.hpp"
#include "tsid/math/fwd.hpp"
#include "tsid/math/utils.hpp"
 
#include <string>
 
namespace tsid {
namespace trajectories {
 
typedef Eigen::Map<const Eigen::Matrix<double, 3, 3>> MapMatrix3;
 
class TrajectorySample {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  // TODO rename pos, vel, acc → value, derivative, second_derivative
  TSID_DEPRECATED math::Vector pos, vel, acc;
 
  TSID_DISABLE_WARNING_PUSH
  TSID_DISABLE_WARNING_DEPRECATED
  // getters / setters with updated names for math::Vector
  const math::Vector& getValue() const { return pos; }
  const math::Vector& getDerivative() const { return vel; }
  const math::Vector& getSecondDerivative() const { return acc; }
  void setValue(const math::Vector& value) { pos = value; }
  void setDerivative(const math::Vector& derivative) { vel = derivative; }
  void setSecondDerivative(const math::Vector& second_derivative) {
    acc = second_derivative;
  }
 
  TrajectorySample(unsigned int size = 0) { resize(size); }
 
  TrajectorySample(unsigned int size_value, unsigned int size_derivative) {
    resize(size_value, size_derivative);
  }
 
  void resize(unsigned int size) { resize(size, size); }
 
  void resize(unsigned int size_value, unsigned int size_derivative) {
    pos.setZero(size_value);
    vel.setZero(size_derivative);
    acc.setZero(size_derivative);
  }
 
  // declare default constructors / destructors to disable the deprecation
  // message for them. TODO: Remove this after the
  // pos/vel/acc → value/derivative/second_derivative rename
  ~TrajectorySample() = default;
  TrajectorySample(const TrajectorySample&) = default;
  TSID_DISABLE_WARNING_POP
};
 
class TrajectoryBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  TrajectoryBase(const std::string& name) : m_name(name) {}
 
  virtual ~TrajectoryBase() = default;
 
  virtual unsigned int size() const = 0;
 
  virtual const TrajectorySample& operator()(double time) = 0;
 
  virtual const TrajectorySample& computeNext() = 0;
 
  virtual const TrajectorySample& getLastSample() const { return m_sample; }
 
  virtual void getLastSample(TrajectorySample& sample) const = 0;
 
  virtual bool has_trajectory_ended() const = 0;
 
 protected:
  std::string m_name;
  TrajectorySample m_sample;
};
}  // namespace trajectories
}  // namespace tsid
 
#endif  // ifndef __invdyn_trajectory_base_hpp__