feature-pose.hh

Go to the documentation of this file.

/*
 * Copyright 2019,
 * Joseph Mirabel
 *
 * LAAS-CNRS
 *
 */
 
#ifndef __SOT_FEATURE_POSE_HH__
#define __SOT_FEATURE_POSE_HH__
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
/* SOT */
#include <sot/core/config.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/matrix-geometry.hh>
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
namespace dynamicgraph {
namespace sot {
 
enum Representation_t { SE3Representation, R3xSO3Representation };
 
template <Representation_t representation = R3xSO3Representation>
class SOT_CORE_DLLAPI FeaturePose : public FeatureAbstract {
 public:
  static const std::string CLASS_NAME;
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
 
 public:
  dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> oMja;
  dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> jaMfa;
  dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> oMjb;
  dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> jbMfb;
  dynamicgraph::SignalPtr<Matrix, sigtime_t> jaJja;
  dynamicgraph::SignalPtr<Matrix, sigtime_t> jbJjb;
 
  dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> faMfbDes;
  dynamicgraph::SignalPtr<Vector, sigtime_t> faNufafbDes;
  SignalTimeDependent<MatrixHomogeneous, sigtime_t> faMfb;
 
  SignalTimeDependent<Vector7, sigtime_t> q_faMfb;
 
  SignalTimeDependent<Vector7, sigtime_t> q_faMfbDes;
  using FeatureAbstract::errorSOUT;
  using FeatureAbstract::jacobianSOUT;
  using FeatureAbstract::selectionSIN;
 
  DECLARE_NO_REFERENCE();
 public:
  FeaturePose(const std::string &name);
  virtual ~FeaturePose(void);
 
  virtual size_type &getDimension(size_type &dim, sigtime_t time);
 
  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
                                             sigtime_t time);
  virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
                                                sigtime_t time);
  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
                                                sigtime_t time);
 
  virtual void display(std::ostream &os) const;
 
 public:
  void servoCurrentPosition(const sigtime_t &time);
 
 private:
  MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, sigtime_t time);
  Vector7 &computeQfaMfb(Vector7 &res, sigtime_t time);
  Vector7 &computeQfaMfbDes(Vector7 &res, sigtime_t time);
 
};
 
template <typename T>
Vector6d convertVelocity(const MatrixHomogeneous &M,
                         const MatrixHomogeneous &Mdes,
                         const Vector &faNufafbDes);
 
template <>
const std::string FeaturePose<SE3Representation>::CLASS_NAME;
template <>
const std::string FeaturePose<R3xSO3Representation>::CLASS_NAME;
 
#if __cplusplus >= 201103L
extern template class FeaturePose<SE3Representation>;
extern template class FeaturePose<R3xSO3Representation>;
#endif
 
typedef FeaturePose<R3xSO3Representation> FeaturePose_t;
typedef FeaturePose<SE3Representation> FeaturePoseSE3_t;
 
} /* namespace sot */
} /* namespace dynamicgraph */
 
#endif  // #ifndef __SOT_FEATURE_POSE_HH__
 
/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */