ConcreteRobot.hpp

Go to the documentation of this file.

#ifndef AIKIDO_ROBOT_CONCRETEROBOT_HPP_
#define AIKIDO_ROBOT_CONCRETEROBOT_HPP_
 
#include <chrono>
#include <string>
#include <unordered_map>
 
#include <Eigen/Core>
#include <dart/dart.hpp>
 
#include "aikido/common/ExecutorThread.hpp"
#include "aikido/common/RNG.hpp"
#include "aikido/constraint/dart/CollisionFree.hpp"
#include "aikido/constraint/dart/TSR.hpp"
#include "aikido/control/TrajectoryExecutor.hpp"
#include "aikido/distance/ConfigurationRanker.hpp"
#include "aikido/planner/parabolic/ParabolicSmoother.hpp"
#include "aikido/planner/parabolic/ParabolicTimer.hpp"
#include "aikido/robot/Robot.hpp"
#include "aikido/robot/util.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
#include "aikido/trajectory/Trajectory.hpp"
 
namespace aikido {
namespace robot {
 
AIKIDO_DECLARE_POINTERS(ConcreteRobot)
 
class ConcreteRobot : public Robot
{
public:
  // Expose base class functions
  using Robot::getMetaSkeleton;
  using Robot::getStateSpace;
 
  ConcreteRobot(
      const std::string& name,
      dart::dynamics::MetaSkeletonPtr metaSkeleton,
      bool simulation,
      aikido::common::UniqueRNGPtr rng,
      aikido::control::TrajectoryExecutorPtr trajectoryExecutor,
      dart::collision::CollisionDetectorPtr collisionDetector,
      std::shared_ptr<dart::collision::BodyNodeCollisionFilter>
          selfCollisionFilter);
 
  virtual ~ConcreteRobot() = default;
 
  // Documentation inherited.
  virtual aikido::trajectory::UniqueSplinePtr smoothPath(
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const aikido::trajectory::Trajectory* path,
      const constraint::TestablePtr& constraint) override;
 
  // Documentation inherited.
  virtual aikido::trajectory::UniqueSplinePtr retimePath(
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const aikido::trajectory::Trajectory* path) override;
 
  // Documentation inherited.
  virtual std::unique_ptr<aikido::trajectory::Spline> retimePathWithKunz(
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const aikido::trajectory::Trajectory* path,
      double maxDeviation,
      double timestep) override;
 
  // Documentation inherited.
  virtual std::future<void> executeTrajectory(
      const trajectory::TrajectoryPtr& trajectory) const override;
 
  // Documentation inherited.
  virtual boost::optional<Eigen::VectorXd> getNamedConfiguration(
      const std::string& name) const override;
 
  // Documentation inherited.
  virtual void setNamedConfigurations(
      std::unordered_map<std::string, const Eigen::VectorXd>
          namedConfigurations) override;
 
  // Documentation inherited.
  virtual std::string getName() const override;
 
  // Documentation inherited.
  virtual dart::dynamics::ConstMetaSkeletonPtr getMetaSkeleton() const override;
 
  // Documentation inherited.
  virtual aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr
  getStateSpace() const override;
 
  // Documentation inherited.
  virtual void setRoot(Robot* robot) override;
 
  // Documentation inherited.
  virtual void step(
      const std::chrono::system_clock::time_point& timepoint) override;
 
  // Documentation inherited.
  virtual aikido::constraint::dart::CollisionFreePtr getSelfCollisionConstraint(
      const statespace::dart::ConstMetaSkeletonStateSpacePtr& space,
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton) const override;
 
  // Documentation inherited.
  virtual aikido::constraint::TestablePtr getFullCollisionConstraint(
      const statespace::dart::ConstMetaSkeletonStateSpacePtr& space,
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const constraint::dart::CollisionFreePtr& collisionFree) const override;
 
  // Get a smoothing post postprocessor that respects velocity and acceleration
  // limits, as well as the passed constraint.
  std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
  getTrajectoryPostProcessor(
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      bool enableShortcut,
      bool enableBlend,
      double shortcutTimelimit,
      double blendRadius,
      int blendIterations,
      double feasibilityCheckResolution,
      double feasibilityApproxTolerance) const;
 
  aikido::trajectory::TrajectoryPtr planToConfiguration(
      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const aikido::statespace::StateSpace::State* goalState,
      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
      double timelimit);
 
  aikido::trajectory::TrajectoryPtr planToConfiguration(
      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const Eigen::VectorXd& goal,
      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
      double timelimit);
 
  aikido::trajectory::TrajectoryPtr planToConfigurations(
      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const std::vector<aikido::statespace::StateSpace::State*>& goalStates,
      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
      double timelimit);
 
  aikido::trajectory::TrajectoryPtr planToConfigurations(
      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const std::vector<Eigen::VectorXd>& goals,
      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
      double timelimit);
 
  aikido::trajectory::TrajectoryPtr planToTSR(
      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const dart::dynamics::BodyNodePtr& bodyNode,
      const aikido::constraint::dart::TSRPtr& tsr,
      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
      double timelimit,
      std::size_t maxNumTrials,
      const distance::ConstConfigurationRankerPtr& ranker = nullptr);
 
  aikido::trajectory::TrajectoryPtr planToTSRwithTrajectoryConstraint(
      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
      const dart::dynamics::BodyNodePtr& bodyNode,
      const aikido::constraint::dart::TSRPtr& goalTsr,
      const aikido::constraint::dart::TSRPtr& constraintTsr,
      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
      double timelimit);
 
  aikido::trajectory::TrajectoryPtr planToNamedConfiguration(
      const std::string& name,
      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
      double timelimit);
 
  void setCRRTPlannerParameters(
      const util::CRRTPlannerParameters& crrtParameters);
 
  Eigen::VectorXd getVelocityLimits(
      const dart::dynamics::MetaSkeleton& metaSkeleton) const;
 
  Eigen::VectorXd getAccelerationLimits(
      const dart::dynamics::MetaSkeleton& metaSkeleton) const;
 
private:
  // Named Configurations are read from a YAML file
  using ConfigurationMap
      = std::unordered_map<std::string, const Eigen::VectorXd>;
 
  std::unique_ptr<aikido::common::RNG> cloneRNG();
 
  Robot* mRootRobot;
 
  std::string mName;
 
  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
  statespace::dart::MetaSkeletonStateSpacePtr mStateSpace;
 
  dart::dynamics::SkeletonPtr mParentSkeleton;
 
  // bool mSimulation;
  // TODO(JS): Disabled since not used in this class
 
  std::unique_ptr<common::RNG> mRng;
 
  std::shared_ptr<control::TrajectoryExecutor> mTrajectoryExecutor;
 
  // double mCollisionResolution;
  // TODO(JS): Disabled since not used in this class
 
  ConfigurationMap mNamedConfigurations;
 
  ::dart::collision::CollisionDetectorPtr mCollisionDetector;
  std::shared_ptr<dart::collision::BodyNodeCollisionFilter>
      mSelfCollisionFilter;
 
  util::CRRTPlannerParameters mCRRTParameters;
};
 
} // namespace robot
} // namespace aikido
 
#endif // AIKIDO_ROBOT_CONCRETEROBOT_HPP_