JacobianExecutor.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONTROL_JACOBIANEXECUTOR_HPP_
#define AIKIDO_CONTROL_JACOBIANEXECUTOR_HPP_
 
#include <future>
#include <mutex>
 
#include <dart/dynamics/Skeleton.hpp>
 
#include "aikido/control/Executor.hpp"
#include "aikido/control/JointCommandExecutor.hpp"
#include "aikido/control/util.hpp"
 
namespace aikido {
namespace control {
 
template <ExecutorType T>
class JacobianExecutor : public JointCommandExecutor<T>
{
public:
  static constexpr size_t SE3_SIZE = 6;
  static constexpr double DEFAULT_LAMBDA = 1E-8;
 
  explicit JacobianExecutor(
      ::dart::dynamics::BodyNode* eeNode,
      std::shared_ptr<JointCommandExecutor<T>> executor = nullptr,
      double lambda = DEFAULT_LAMBDA);
 
  virtual ~JacobianExecutor();
 
  virtual std::future<int> execute(
      const Eigen::Vector6d command,
      const std::chrono::duration<double>& timeout
      = std::chrono::duration<double>(1))
  {
    return this->execute(command, timeout, std::chrono::system_clock::now());
  }
  virtual std::future<int> execute(
      const Eigen::Vector6d command,
      const std::chrono::duration<double>& timeout,
      const std::chrono::system_clock::time_point& timepoint);
 
  virtual std::future<int> execute(
      const std::vector<double>& command,
      const std::chrono::duration<double>& timeout
      = std::chrono::duration<double>(1))
  {
    return this->execute(command, timeout, std::chrono::system_clock::now());
  }
  virtual std::future<int> execute(
      const std::vector<double>& command,
      const std::chrono::duration<double>& timeout,
      const std::chrono::system_clock::time_point& timepoint);
 
  void step(const std::chrono::system_clock::time_point& timepoint) override;
 
  void cancel();
 
private:
  std::vector<double> SE3ToJoint(const Eigen::Vector6d command);
 
  ::dart::dynamics::BodyNode* mEENode;
 
  std::shared_ptr<JointCommandExecutor<T>> mExecutor;
 
  Eigen::Vector6d mCommand;
 
  double mLambda;
 
  bool mInProgress;
 
  std::unique_ptr<std::promise<int>> mPromise;
 
  mutable std::mutex mMutex;
 
  std::chrono::system_clock::time_point mExecutionStartTime;
 
  std::chrono::duration<double> mTimeout;
 
  std::future<int> mFuture;
};
 
// Define common template arguments
using JacobianVelocityExecutor = JacobianExecutor<ExecutorType::VELOCITY>;
using JacobianEffortExecutor = JacobianExecutor<ExecutorType::EFFORT>;
// Note: No position, as jacobian should only be used with derivatives.
 
// Define constexprs (required for linking)
template <ExecutorType T>
constexpr size_t JacobianExecutor<T>::SE3_SIZE;
template <ExecutorType T>
constexpr double JacobianExecutor<T>::DEFAULT_LAMBDA;
 
} // namespace control
} // namespace aikido
 
#include "detail/JacobianExecutor-impl.hpp"
 
#endif