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