joint-trajectory-entity.hh

Go to the documentation of this file.

/*
 * Copyright 2013,
 * Olivier Stasse,
 *
 * CNRS
 *
 */
 
#ifndef SOT_JOINT_TRAJECTORY_ENTITY_HH
#define SOT_JOINT_TRAJECTORY_ENTITY_HH
 
#include <deque>
#include <list>
 
// Maal
#include <dynamic-graph/linear-algebra.h>
 
// SOT
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
 
#include <sot/core/matrix-geometry.hh>
#include <sot/core/trajectory.hh>
#include <sstream>
// API
 
#if defined(WIN32)
#if defined(joint_trajectory_entity_EXPORTS)
#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllexport)
#else
#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllimport)
#endif
#else
#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT
#endif
 
// Class
 
namespace dynamicgraph {
namespace sot {
 
class SOTJOINT_TRAJECTORY_ENTITY_EXPORT SotJointTrajectoryEntity
    : public dynamicgraph::Entity {
 public:
  DYNAMIC_GRAPH_ENTITY_DECL();
 
  SotJointTrajectoryEntity(const std::string &name);
  virtual ~SotJointTrajectoryEntity() {}
 
  void loadFile(const std::string &name);
 
  dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos,
                                        const sigtime_t &time);
 
  dynamicgraph::Vector &getNextCoM(dynamicgraph::Vector &com,
                                   const sigtime_t &time);
 
  dynamicgraph::Vector &getNextCoP(dynamicgraph::Vector &cop,
                                   const sigtime_t &time);
 
  sot::MatrixHomogeneous &getNextWaist(sot::MatrixHomogeneous &waist,
                                       const sigtime_t &time);
 
  std::size_t &getSeqId(std::size_t &seqid, const sigtime_t &time);
 
  sot::MatrixHomogeneous XYZThetaToMatrixHomogeneous(
      const dynamicgraph::Vector &xyztheta);
 
  size_type &OneStepOfUpdate(size_type &dummy, const sigtime_t &time);
 
  virtual void display(std::ostream &os) const;
  SOTJOINT_TRAJECTORY_ENTITY_EXPORT
  friend std::ostream &operator<<(std::ostream &os,
                                  const SotJointTrajectoryEntity &r) {
    r.display(os);
    return os;
  }
 
 public:
  typedef size_type Dummy;
 
  dynamicgraph::SignalTimeDependent<size_type, sigtime_t> refresherSINTERN;
 
  SignalTimeDependent<Dummy, sigtime_t> OneStepOfUpdateS;
 
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, sigtime_t>
      positionSOUT;
 
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> comSOUT;
 
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> zmpSOUT;
 
  dynamicgraph::SignalTimeDependent<sot::MatrixHomogeneous, sigtime_t>
      waistSOUT;
 
  dynamicgraph::SignalTimeDependent<std::size_t, sigtime_t> seqIdSOUT;
 
  dynamicgraph::SignalPtr<Trajectory, sigtime_t> trajectorySIN;
 
 protected:
  std::deque<sot::Trajectory>::size_type index_;
 
  timestamp traj_timestamp_;
 
  dynamicgraph::Vector pose_;
 
  dynamicgraph::Vector com_;
 
  dynamicgraph::Vector cop_;
 
  sot::MatrixHomogeneous waist_;
 
  std::size_t seqid_;
 
  sot::Trajectory init_traj_;
 
  std::deque<sot::Trajectory> deque_traj_;
 
  void UpdatePoint(const JointTrajectoryPoint &aJTP);
 
  void UpdateTrajectory(const Trajectory &aTrajectory);
 
  void setInitTraj(const std::string &os);
};
 
} /* namespace sot */
} /* namespace dynamicgraph */
 
#endif  // SOT_JOINT_TRAJECTORY_ENTITY_HH