JointCommandExecutor.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONTROL_JOINTCOMMANDEXECUTOR_HPP_
#define AIKIDO_CONTROL_JOINTCOMMANDEXECUTOR_HPP_
 
#include <chrono>
#include <future>
#include <set>
 
#include <dart/dart.hpp>
 
#include "aikido/common/pointers.hpp"
#include "aikido/control/Executor.hpp"
#include "aikido/control/util.hpp"
 
namespace aikido {
namespace control {
 
template <ExecutorType T>
class JointCommandExecutor : public Executor
{
public:
  JointCommandExecutor(
      const std::vector<dart::dynamics::DegreeOfFreedom*> dofs,
      const std::set<ExecutorType> otherTypes = std::set<ExecutorType>(),
      const std::chrono::milliseconds threadRate = defaultThreadRate)
    : Executor(
        concatenateTypes(std::set<ExecutorType>{T}, otherTypes),
        dofs,
        threadRate)
  {
  }
 
  virtual ~JointCommandExecutor()
  {
  }
 
  virtual std::future<int> execute(
      const std::vector<double>& command,
      const std::chrono::duration<double>& timeout,
      const std::chrono::system_clock::time_point& timepoint)
      = 0;
 
  virtual std::future<int> execute(
      const std::vector<double>& command,
      const std::chrono::duration<double>& timeout)
  {
    return this->execute(command, timeout, std::chrono::system_clock::now());
  }
 
  // Documentation inherited.
  virtual void step(
      const std::chrono::system_clock::time_point& timepoint) override = 0;
 
  virtual void cancel() = 0;
};
 
// Define common executor types
using PositionExecutor = JointCommandExecutor<ExecutorType::POSITION>;
using VelocityExecutor = JointCommandExecutor<ExecutorType::VELOCITY>;
using EffortExecutor = JointCommandExecutor<ExecutorType::EFFORT>;
 
} // namespace control
} // namespace aikido
 
#endif