generic_gripper.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_control/api.h>
#include <mc_rbdyn/RobotModule.h>
 
#include <map>
#include <string>
#include <vector>
 
namespace mc_rbdyn
{
struct Robot;
} // namespace mc_rbdyn
 
namespace mc_control
{
 
struct MC_RBDYN_DLLAPI Gripper
{
 
  Gripper(const mc_rbdyn::Robot & robot,
          const std::vector<std::string> & jointNames,
          const std::string & robot_urdf,
          bool reverseLimits,
          const mc_rbdyn::RobotModule::Gripper::Safety & safety);
 
  Gripper(const mc_rbdyn::Robot & robot,
          const std::vector<std::string> & jointNames,
          const std::vector<mc_rbdyn::Mimic> & mimics,
          bool reverseLimits,
          const mc_rbdyn::RobotModule::Gripper::Safety & safety);
 
  void resetDefaults();
 
  void saveConfig();
 
  void restoreConfig();
 
  void configure(const mc_rtc::Configuration & config);
 
  void reset(const std::vector<double> & currentQ);
 
  void reset(const Gripper & gripper);
 
  void run(double timeStep, mc_rbdyn::Robot & robot, mc_rbdyn::Robot & real);
 
  void setTargetQ(const std::vector<double> & targetQ);
 
  void setTargetQ(const std::string & jointName, double targetQ);
 
  double getTargetQ(const std::string & jointName) const;
 
  std::vector<double> getTargetQ() const;
 
  void setTargetOpening(double targetOpening);
 
  void setTargetOpening(const std::string & jointName, double targetOpening);
 
  double getTargetOpening(const std::string & jointName) const;
 
  std::vector<double> curPosition() const;
 
  std::vector<double> curOpening() const;
 
  double curOpening(const std::string & jointName) const;
 
  inline const std::vector<std::string> & joints() const { return names; }
 
  inline const std::vector<std::string> & activeJoints() const { return active_joints; }
 
  /* \brief Checks whether a joint is an active gripper joint */
  inline bool hasActiveJoint(const std::string & jointName) const
  {
    return std::find(active_joints.begin(), active_joints.end(), jointName) != active_joints.end();
  }
 
  inline const std::vector<double> & q() const { return _q; }
 
  double opening() const;
 
  void percentVMAX(double percent);
  double percentVMAX() const;
 
  void actualCommandDiffTrigger(double d) { config_.actualCommandDiffTrigger = d; }
  double actualCommandDiffTrigger() const { return config_.actualCommandDiffTrigger; }
 
  void overCommandLimitIterN(unsigned int N) { config_.overCommandLimitIterN = std::max(N, 1u); }
  unsigned int overCommandLimitIterN() const { return config_.overCommandLimitIterN; }
 
  void releaseSafetyOffset(double offset) { config_.releaseSafetyOffset = offset; }
  double releaseSafetyOffset() const { return config_.releaseSafetyOffset; }
 
  bool complete() const;
 
  inline bool is_metric() const noexcept { return is_metric_; }
 
  inline bool reversed() const noexcept { return reversed_; }
 
protected:
  void setTargetOpening(size_t activeJointId, double targetOpening);
 
  void setTargetQ(size_t activeJointId, double targetQ);
 
  void setTargetQ_(size_t activeJoints, double targetQ);
  void setTargetQ_(const std::vector<double> & targetQ);
 
  double curPosition(size_t jointId) const;
 
  double curOpening(size_t jointId) const;
 
  double targetOpening(size_t jointId) const;
 
  double getTargetQ(size_t jointId) const;
 
  double clampQ(size_t activeJoint, double q);
 
protected:
  std::vector<std::string> names;
  std::vector<std::string> active_joints;
  std::vector<size_t> active_joints_idx;
  std::vector<int> joints_mbc_idx;
 
  bool is_metric_;
 
  std::vector<double> closeP;
  std::vector<double> openP;
  std::vector<double> vmax;
 
  std::vector<std::pair<size_t, double>> mult;
  std::vector<double> offset;
  std::vector<double> _q;
 
  std::vector<double> targetQIn;
  std::vector<double> * targetQ;
 
  std::vector<double> reached_threshold_;
 
  std::vector<double> actualQ;
 
protected:
  using Config = mc_rbdyn::RobotModule::Gripper::Safety;
  Config config_;
  Config savedConfig_;
  Config defaultConfig_;
 
  std::vector<double> percentOpen;
  std::vector<bool> overCommandLimit;
  std::vector<unsigned int> overCommandLimitIter;
  bool reversed_ = false;
};
 
using GripperPtr = std::unique_ptr<Gripper>;
using GripperRef = std::reference_wrapper<Gripper>;
 
} // namespace mc_control