KinematicSimulationJointCommandExecutor.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONTROL_KINEMATICSIMULATIONJOINTCOMMANDEXECUTOR_HPP_
#define AIKIDO_CONTROL_KINEMATICSIMULATIONJOINTCOMMANDEXECUTOR_HPP_
 
#include <future>
#include <mutex>
 
#include <dart/dynamics/Skeleton.hpp>
 
#include "aikido/control/JointCommandExecutor.hpp"
#include "aikido/control/util.hpp"
 
namespace aikido {
namespace control {
 
template <ExecutorType T>
class KinematicSimulationJointCommandExecutor : public JointCommandExecutor<T>
{
public:
  explicit KinematicSimulationJointCommandExecutor(
      ::dart::dynamics::MetaSkeletonPtr metaskeleton);
 
  explicit KinematicSimulationJointCommandExecutor(
      std::vector<::dart::dynamics::DegreeOfFreedom*> dofs);
 
  virtual ~KinematicSimulationJointCommandExecutor();
 
  // Documentation inherited
  virtual std::future<int> execute(
      const std::vector<double>& command,
      const std::chrono::duration<double>& timeout,
      const std::chrono::system_clock::time_point& timepoint) override;
 
  // Documentation inherited
  virtual std::future<int> execute(
      const std::vector<double>& command,
      const std::chrono::duration<double>& timeout
      = std::chrono::duration<double>(1.0)) override
  {
    return this->execute(command, timeout, std::chrono::system_clock::now());
  }
 
  void step(const std::chrono::system_clock::time_point& timepoint) override;
 
  void cancel() override;
 
private:
  std::vector<double> mCommand;
 
  ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
 
  bool mInProgress;
 
  std::unique_ptr<std::promise<int>> mPromise;
 
  mutable std::mutex mMutex;
 
  std::chrono::system_clock::time_point mExecutionStartTime;
 
  std::vector<double> mStartPosition;
 
  std::chrono::duration<double> mTimeout;
};
 
// Define common template arguments
using KinematicSimulationPositionExecutor
    = KinematicSimulationJointCommandExecutor<ExecutorType::POSITION>;
using KinematicSimulationVelocityExecutor
    = KinematicSimulationJointCommandExecutor<ExecutorType::VELOCITY>;
// Note: No effort simulation without dynamics, treat like velocity.
using KinematicSimulationEffortExecutor
    = KinematicSimulationJointCommandExecutor<ExecutorType::EFFORT>;
 
} // namespace control
} // namespace aikido
 
#include "detail/KinematicSimulationJointCommandExecutor-impl.hpp"
 
#endif