force-compensation.h

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
#ifndef __SOT_SOTFORCECOMPENSATION_H__
#define __SOT_SOTFORCECOMPENSATION_H__
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
 
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
 
#include <sot/core/matrix-geometry.hh>
 
/* STD */
#include <string>
 
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#if defined(WIN32)
#if defined(force_compensation_EXPORTS)
#define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
#else
#define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
#endif
#else
#define SOTFORCECOMPENSATION_EXPORT
#endif
 
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
class SOTFORCECOMPENSATION_EXPORT ForceCompensation {
 private:
  static MatrixRotation I3;
 
 protected:
  bool usingPrecompensation;
 
 public:
  ForceCompensation(void);
  static MatrixForce& computeHandXworld(
      const MatrixRotation& worldRhand,
      const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
 
  static MatrixForce& computeHandVsensor(const MatrixRotation& sensorRhand,
                                         MatrixForce& res);
  static MatrixForce& computeSensorXhand(
      const MatrixRotation& sensorRhand,
      const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
  /*   static dynamicgraph::Matrix& computeInertiaSensor( const
   * dynamicgraph::Matrix& inertiaJoint, */
  /*                       const MatrixForce& sensorXhand, */
  /*                       dynamicgraph::Matrix& res ); */
 
  static dynamicgraph::Vector& computeTorsorCompensated(
      const dynamicgraph::Vector& torqueInput,
      const dynamicgraph::Vector& torquePrecompensation,
      const dynamicgraph::Vector& gravity, const MatrixForce& handXworld,
      const MatrixForce& handVsensor, const dynamicgraph::Matrix& gainSensor,
      const dynamicgraph::Vector& momentum, dynamicgraph::Vector& res);
 
  static dynamicgraph::Vector& crossProduct_V_F(
      const dynamicgraph::Vector& velocity, const dynamicgraph::Vector& force,
      dynamicgraph::Vector& res);
  static dynamicgraph::Vector& computeMomentum(
      const dynamicgraph::Vector& velocity,
      const dynamicgraph::Vector& acceleration, const MatrixForce& sensorXhand,
      const dynamicgraph::Matrix& inertiaJoint, dynamicgraph::Vector& res);
 
  static dynamicgraph::Vector& computeDeadZone(
      const dynamicgraph::Vector& torqueInput,
      const dynamicgraph::Vector& deadZoneLimit, dynamicgraph::Vector& res);
 
 public:  // CALIBRATION
  std::list<dynamicgraph::Vector> torsorList;
  std::list<MatrixRotation> rotationList;
 
  void clearCalibration(void);
  void addCalibrationValue(const dynamicgraph::Vector& torsor,
                           const MatrixRotation& worldRhand);
 
  dynamicgraph::Vector calibrateTransSensorCom(
      const dynamicgraph::Vector& gravity, const MatrixRotation& handRsensor);
  dynamicgraph::Vector calibrateGravity(
      const MatrixRotation& handRsensor,
      bool precompensationCalibration = false,
      const MatrixRotation& hand0Rsensor = I3);
};
 
/* --------------------------------------------------------------------- */
/* --- PLUGIN ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
class SOTFORCECOMPENSATION_EXPORT ForceCompensationPlugin
    : public dg::Entity,
      public ForceCompensation {
 public:
  static const std::string CLASS_NAME;
  virtual const std::string& getClassName(void) const { return CLASS_NAME; }
  bool calibrationStarted;
 
 public: /* --- CONSTRUCTION --- */
  ForceCompensationPlugin(const std::string& name);
  virtual ~ForceCompensationPlugin(void);
 
 public: /* --- SIGNAL --- */
  /* --- INPUTS --- */
  dg::SignalPtr<dynamicgraph::Vector, int> torsorSIN;
  dg::SignalPtr<MatrixRotation, int> worldRhandSIN;
 
  /* --- CONSTANTS --- */
  dg::SignalPtr<MatrixRotation, int> handRsensorSIN;
  dg::SignalPtr<dynamicgraph::Vector, int> translationSensorComSIN;
  dg::SignalPtr<dynamicgraph::Vector, int> gravitySIN;
  dg::SignalPtr<dynamicgraph::Vector, int> precompensationSIN;
  dg::SignalPtr<dynamicgraph::Matrix, int> gainSensorSIN;
  dg::SignalPtr<dynamicgraph::Vector, int> deadZoneLimitSIN;
  dg::SignalPtr<dynamicgraph::Vector, int> transSensorJointSIN;
  dg::SignalPtr<dynamicgraph::Matrix, int> inertiaJointSIN;
 
  dg::SignalPtr<dynamicgraph::Vector, int> velocitySIN;
  dg::SignalPtr<dynamicgraph::Vector, int> accelerationSIN;
 
  /* --- INTERMEDIATE OUTPUTS --- */
  dg::SignalTimeDependent<MatrixForce, int> handXworldSOUT;
  dg::SignalTimeDependent<MatrixForce, int> handVsensorSOUT;
  dg::SignalPtr<dynamicgraph::Vector, int> torsorDeadZoneSIN;
 
  dg::SignalTimeDependent<MatrixForce, int> sensorXhandSOUT;
  // dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaSensorSOUT;
  dg::SignalTimeDependent<dynamicgraph::Vector, int> momentumSOUT;
  dg::SignalPtr<dynamicgraph::Vector, int> momentumSIN;
 
  /* --- OUTPUTS --- */
  dg::SignalTimeDependent<dynamicgraph::Vector, int> torsorCompensatedSOUT;
  dg::SignalTimeDependent<dynamicgraph::Vector, int> torsorDeadZoneSOUT;
 
  typedef int sotDummyType;
  dg::SignalTimeDependent<sotDummyType, int> calibrationTrigerSOUT;
 
 public: /* --- COMMANDLINE --- */
  sotDummyType& calibrationTriger(sotDummyType& dummy, int time);
};
 
}  // namespace sot
}  // namespace dynamicgraph
 
#endif  // #ifndef __SOT_SOTFORCECOMPENSATION_H__