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 int &time);
 
  dynamicgraph::Vector &getNextCoM(dynamicgraph::Vector &com, const int &time);
 
  dynamicgraph::Vector &getNextCoP(dynamicgraph::Vector &cop, const int &time);
 
  sot::MatrixHomogeneous &getNextWaist(sot::MatrixHomogeneous &waist,
                                       const int &time);
 
  unsigned int &getSeqId(unsigned int &seqid, const int &time);
 
  sot::MatrixHomogeneous XYZThetaToMatrixHomogeneous(
      const dynamicgraph::Vector &xyztheta);
 
  int &OneStepOfUpdate(int &dummy, const int &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 int Dummy;
 
  dynamicgraph::SignalTimeDependent<int, int> refresherSINTERN;
 
  SignalTimeDependent<Dummy, int> OneStepOfUpdateS;
 
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> positionSOUT;
 
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> comSOUT;
 
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> zmpSOUT;
 
  dynamicgraph::SignalTimeDependent<sot::MatrixHomogeneous, int> waistSOUT;
 
  dynamicgraph::SignalTimeDependent<unsigned int, int> seqIdSOUT;
 
  dynamicgraph::SignalPtr<Trajectory, int> trajectorySIN;
 
 protected:
  std::deque<sot::Trajectory>::size_type index_;
 
  timestamp traj_timestamp_;
 
  dynamicgraph::Vector pose_;
 
  dynamicgraph::Vector com_;
 
  dynamicgraph::Vector cop_;
 
  sot::MatrixHomogeneous waist_;
 
  unsigned int 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