gripper-control.hh

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_SOTGRIPPERCONTROL_H__
#define __SOT_SOTGRIPPERCONTROL_H__

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* Matrix */
#include <dynamic-graph/linear-algebra.h>

/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>

#include <sot/core/flags.hh>

/* STD */
#include <string>

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined(WIN32)
#if defined(gripper_control_EXPORTS)
#define SOTGRIPPERCONTROL_EXPORT __declspec(dllexport)
#else
#define SOTGRIPPERCONTROL_EXPORT __declspec(dllimport)
#endif
#else
#define SOTGRIPPERCONTROL_EXPORT
#endif

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

namespace dynamicgraph {
namespace sot {

class SOTGRIPPERCONTROL_EXPORT GripperControl {
 protected:
  double offset;
  static const double OFFSET_DEFAULT;
  dynamicgraph::Vector factor;

 public:
  GripperControl(void);

  // if the torque limit is reached, the normalized position is reduced by
  // (offset)
  void computeIncrement(const dynamicgraph::Vector &torques,
                        const dynamicgraph::Vector &torqueLimits,
                        const dynamicgraph::Vector &currentNormVel);

  dynamicgraph::Vector &computeDesiredPosition(
      const dynamicgraph::Vector &currentPos,
      const dynamicgraph::Vector &desiredPos,
      const dynamicgraph::Vector &torques,
      const dynamicgraph::Vector &torqueLimits,
      dynamicgraph::Vector &referencePos);

  static dynamicgraph::Vector &selector(const dynamicgraph::Vector &fullsize,
                                        const Flags &selec,
                                        dynamicgraph::Vector &desPos);
};

/* --------------------------------------------------------------------- */
/* --- PLUGIN ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */

class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin
    : public dynamicgraph::Entity,
      public GripperControl {
  DYNAMIC_GRAPH_ENTITY_DECL();

 public:
  bool calibrationStarted;

 public: /* --- CONSTRUCTION --- */
  GripperControlPlugin(const std::string &name);
  virtual ~GripperControlPlugin(void);

  /* --- DOCUMENTATION --- */
  virtual std::string getDocString() const;

 public: /* --- SIGNAL --- */
  /* --- INPUTS --- */
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionSIN;
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionDesSIN;
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> torqueSIN;
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> torqueLimitSIN;
  dynamicgraph::SignalPtr<Flags, int> selectionSIN;

  /* --- INTERMEDIARY --- */
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionFullSizeSIN;
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
      positionReduceSOUT;
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> torqueFullSizeSIN;
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> torqueReduceSOUT;
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> torqueLimitFullSizeSIN;
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
      torqueLimitReduceSOUT;

  /* --- OUTPUTS --- */
  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
      desiredPositionSOUT;

 public: /* --- COMMANDLINE --- */
  void initCommands();

  void setOffset(const double &value);
};

} /* namespace sot */
} /* namespace dynamicgraph */

#endif  // #ifndef __SOT_SOTGRIPPERCONTROL_H__