VisualServoingVelocityExecutor.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONTROL_VISUALSERVOINGVELOCITYEXECUTOR_HPP_
#define AIKIDO_CONTROL_VISUALSERVOINGVELOCITYEXECUTOR_HPP_
 
#include <future>
#include <mutex>
 
#include <dart/dynamics/Skeleton.hpp>
 
#include "aikido/control/Executor.hpp"
#include "aikido/control/JacobianExecutor.hpp"
#include "aikido/control/util.hpp"
 
namespace aikido {
namespace control {
 
class VisualServoingVelocityExecutor : public Executor
{
public:
  struct Properties
  {
    static constexpr double DEFAULT_GOAL_TOLERANCE = 0.01; // m
    static constexpr double DEFAULT_APPROACH_VEL = 0.05;   // m/s
    static constexpr double DEFAULT_TIMEOUT = 0.5;         // s
    static constexpr long DEFAULT_THREAD_PERIOD = 10L;     // ms
    Properties(
        double goalTolerance = DEFAULT_GOAL_TOLERANCE,
        double approachVelocity = DEFAULT_APPROACH_VEL,
        std::chrono::duration<double> timeout
        = std::chrono::duration<double>(DEFAULT_TIMEOUT),
        std::chrono::milliseconds threadPeriod
        = std::chrono::milliseconds(DEFAULT_THREAD_PERIOD))
      : mGoalTolerance(goalTolerance)
      , mApproachVelocity(approachVelocity)
      , mTimeout(timeout)
      , mThreadPeriod(threadPeriod){
            // Do nothing
        };
 
    double mGoalTolerance;
    double mApproachVelocity;
    std::chrono::duration<double> mTimeout;
    std::chrono::milliseconds mThreadPeriod;
  };
 
  explicit VisualServoingVelocityExecutor(
      ::dart::dynamics::BodyNode* eeNode,
      std::shared_ptr<JacobianVelocityExecutor> executor = nullptr,
      Properties properties = Properties());
 
  virtual ~VisualServoingVelocityExecutor();
 
  void resetProperties(Properties properties = Properties());
 
  Properties getProperties() const;
 
  virtual std::future<int> execute(
      std::function<std::shared_ptr<Eigen::Isometry3d>(void)> perception)
  {
    return this->execute(perception, std::chrono::system_clock::now());
  }
  virtual std::future<int> execute(
      std::function<std::shared_ptr<Eigen::Isometry3d>(void)> perception,
      const std::chrono::system_clock::time_point& timepoint);
 
  void step(const std::chrono::system_clock::time_point& timepoint) override;
 
  void cancel();
 
private:
  ::dart::dynamics::BodyNode* mEENode;
 
  std::shared_ptr<JacobianVelocityExecutor> mExecutor;
 
  std::function<std::shared_ptr<Eigen::Isometry3d>(void)> mPerception;
 
  std::unique_ptr<std::promise<int>> mPromise;
 
  mutable std::mutex mMutex;
 
  Properties mProperties;
 
  std::future<int> mFuture;
 
  std::chrono::system_clock::time_point mLastPerceivedTime;
};
 
} // namespace control
} // namespace aikido
 
#endif