Robot.hpp

Go to the documentation of this file.

#ifndef AIKIDO_ROBOT_ROBOT_HPP_
#define AIKIDO_ROBOT_ROBOT_HPP_
 
#include <chrono>
#include <string>
#include <unordered_map>
 
#include <Eigen/Dense>
#include <dart/collision/CollisionDetector.hpp>
#include <dart/collision/CollisionFilter.hpp>
#include <dart/dynamics/dynamics.hpp>
 
#include "aikido/common/util.hpp"
#include "aikido/constraint/dart/CollisionFree.hpp"
#include "aikido/constraint/dart/TSR.hpp"
#include "aikido/control/Executor.hpp"
#include "aikido/control/KinematicSimulationJointCommandExecutor.hpp"
#include "aikido/control/KinematicSimulationTrajectoryExecutor.hpp"
#include "aikido/control/TrajectoryExecutor.hpp"
#include "aikido/distance/ConfigurationRanker.hpp"
#include "aikido/planner/ConfigurationToConfigurationPlanner.hpp"
#include "aikido/planner/Planner.hpp"
#include "aikido/planner/World.hpp"
#include "aikido/planner/dart/ConfigurationToEndEffectorOffsetPlanner.hpp"
#include "aikido/planner/dart/ConfigurationToEndEffectorPosePlanner.hpp"
#include "aikido/planner/kunzretimer/KunzRetimer.hpp"
#include "aikido/robot/util.hpp"
#include "aikido/statespace/StateSpace.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
#include "aikido/trajectory/Trajectory.hpp"
 
namespace aikido {
namespace robot {
 
AIKIDO_DECLARE_POINTERS(Robot)
 
class Robot
{
public:
  Robot(
      dart::dynamics::SkeletonPtr skeleton,
      const std::string name = "robot",
      bool addDefaultExecutors = true);
 
  Robot(
      dart::dynamics::ReferentialSkeletonPtr refSkeleton,
      Robot* rootRobot,
      dart::collision::CollisionDetectorPtr collisionDetector,
      std::shared_ptr<dart::collision::BodyNodeCollisionFilter> collisionFilter,
      const std::string name = "subrobot");
 
  virtual ~Robot() = default;
 
  virtual void ignoreSelfCollisionPairs(
      const std::vector<std::pair<std::string, std::string>> bodyNodeList);
 
  virtual void enforceSelfCollisionPairs(
      const std::vector<std::pair<std::string, std::string>> bodyNodeList);
 
  virtual void step(const std::chrono::system_clock::time_point& timepoint);
 
  constraint::dart::CollisionFreePtr getSelfCollisionConstraint() const;
 
  constraint::TestablePtr combineCollisionConstraint(
      const constraint::dart::CollisionFreePtr& collisionFree) const;
 
  constraint::TestablePtr getWorldCollisionConstraint(
      const std::vector<std::string> bodyNames
      = std::vector<std::string>()) const;
 
  virtual RobotPtr registerSubRobot(
      dart::dynamics::ReferentialSkeletonPtr refSkeleton,
      const std::string& name);
 
  RobotPtr getSubRobotByName(const std::string& name) const;
 
  Eigen::VectorXd getNamedConfiguration(const std::string& name) const;
 
  void setNamedConfigurations(
      std::unordered_map<std::string, const Eigen::VectorXd>
          namedConfigurations);
 
  trajectory::TrajectoryPtr planToConfiguration(
      const Eigen::VectorXd& goalConf,
      const constraint::TestablePtr& testableConstraint,
      const std::shared_ptr<planner::ConfigurationToConfigurationPlanner>&
          planner
      = nullptr,
      const std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
          trajPostProcessor
      = nullptr) const;
 
  trajectory::TrajectoryPtr planToConfiguration(
      const Eigen::VectorXd& goalConf,
      const std::shared_ptr<planner::ConfigurationToConfigurationPlanner>&
          planner
      = nullptr,
      const std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
          trajPostProcessor
      = nullptr) const;
 
  trajectory::TrajectoryPtr planToOffset(
      const std::string bodyNodeName,
      const Eigen::Vector3d& offset,
      const constraint::TestablePtr& testableConstraint,
      const std::shared_ptr<
          planner::dart::ConfigurationToEndEffectorOffsetPlanner>& planner
      = nullptr,
      const std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
          trajPostProcessor
      = nullptr) const;
 
  trajectory::TrajectoryPtr planToOffset(
      const std::string bodyNodeName,
      const Eigen::Vector3d& offset,
      const std::shared_ptr<
          planner::dart::ConfigurationToEndEffectorOffsetPlanner>& planner
      = nullptr,
      const std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
          trajPostProcessor
      = nullptr) const;
 
  trajectory::TrajectoryPtr planToPoseOffset(
      const std::string bodyNodeName,
      const Eigen::Vector3d& offset,
      const Eigen::Vector3d& rotation,
      const constraint::TestablePtr& testableConstraint,
      const std::shared_ptr<
          planner::dart::ConfigurationToEndEffectorPosePlanner>& planner
      = nullptr,
      const std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
          trajPostProcessor
      = nullptr) const;
 
  trajectory::TrajectoryPtr planToPoseOffset(
      const std::string bodyNodeName,
      const Eigen::Vector3d& offset,
      const Eigen::Vector3d& rotation,
      const std::shared_ptr<
          planner::dart::ConfigurationToEndEffectorPosePlanner>& planner
      = nullptr,
      const std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
          trajPostProcessor
      = nullptr) const;
 
  trajectory::TrajectoryPtr planToTSR(
      const std::string bodyNodeName,
      const constraint::dart::TSRPtr& tsr,
      const constraint::TestablePtr& testableConstraint,
      const util::PlanToTSRParameters& params = util::PlanToTSRParameters(),
      const std::shared_ptr<planner::ConfigurationToConfigurationPlanner>&
          planner
      = nullptr,
      const std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
          trajPostProcessor
      = nullptr,
      const distance::ConstConfigurationRankerPtr& ranker = nullptr) const;
 
  trajectory::TrajectoryPtr planToTSR(
      const std::string bodyNodeName,
      const constraint::dart::TSRPtr& tsr,
      const util::PlanToTSRParameters& params = util::PlanToTSRParameters(),
      const std::shared_ptr<planner::ConfigurationToConfigurationPlanner>&
          planner
      = nullptr,
      const std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
          trajPostProcessor
      = nullptr,
      const distance::ConstConfigurationRankerPtr& ranker = nullptr) const;
 
 
  virtual void clearExecutors();
 
  virtual void deactivateExecutor();
 
  virtual aikido::control::ExecutorPtr getActiveExecutor();
 
  virtual int registerExecutor(
      aikido::control::ExecutorPtr executor, std::string desiredName = "");
 
  virtual bool activateExecutor(const int id);
 
  virtual bool activateExecutor(const std::string name);
 
  virtual bool activateExecutor(const aikido::control::ExecutorType type);
 
  template <aikido::control::ExecutorType T>
  std::future<int> executeJointCommand(
      const std::vector<double>& command,
      const std::chrono::duration<double>& timeout);
 
  std::future<void> executeTrajectory(
      const trajectory::TrajectoryPtr& trajectory);
 
  virtual void cancelAllCommands(
      bool includeSubrobots = true,
      bool includeParents = false,
      const std::vector<std::string> excludeSubrobots
      = std::vector<std::string>());
 
 
  // Gets the (sub)robot's name
  std::string getName() const;
 
  // Gets a copy of the (sub)robot's metaskeleton at the current state
  dart::dynamics::MetaSkeletonPtr getMetaSkeletonClone() const;
 
  dart::dynamics::MetaSkeletonPtr getMetaSkeleton();
 
  const dart::dynamics::MetaSkeletonPtr getMetaSkeleton() const;
 
  // Retrieves current state of the robot
  Eigen::VectorXd getCurrentConfiguration() const;
 
  // Clones the RNG pointer (or nullptr if not set)
  common::UniqueRNGPtr cloneRNG() const;
 
  // Sets the RNG pointer
  void setRNG(common::UniqueRNGPtr rng);
 
  // Gets this robot's (mutable) planning world
  aikido::planner::WorldPtr getWorld() const;
 
  // Set's this robot's planning world
  // Adds robot to world if not already present
  void setWorld(aikido::planner::WorldPtr world);
 
  // Sets the default trajectory post-processor.
  // Also enables automatic post-processing for all planning functions.
  void setDefaultPostProcessor(
      const std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
          trajPostProcessor);
 
  // Get current default postprocessor
  std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
  getDefaultPostProcessor() const;
 
  // Enables/disables automatic post-processing for all planning functions.
  void setEnablePostProcessing(bool enable = true);
 
  // Utility function to get root skeleton
  dart::dynamics::ConstSkeletonPtr getRootSkeleton() const;
 
  // Utility function to get root skeleton
  dart::dynamics::SkeletonPtr getRootSkeleton();
 
  // Utility function to get Default VF Params
  util::VectorFieldPlannerParameters getVFParams() const;
 
  // Utility function to set Default VF Params
  void setVFParams(util::VectorFieldPlannerParameters params);
 
protected:
  std::string mName;
  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
  aikido::statespace::dart::MetaSkeletonStateSpacePtr mStateSpace;
 
  // Currently active executor
  int mActiveExecutor{-1};
  // Executors indexed by id (higher id has more priority)
  std::vector<aikido::control::ExecutorPtr> mExecutors;
  // Keeps track of executor name (if used)
  std::unordered_map<std::string, int> mExecutorsNameMap;
 
  // Subrobot and Joint Management
  Robot* mParentRobot{nullptr};
  // Managed degrees of freedom (= mMetaSkeleton->getDofs()->getName())
  std::set<std::string> mDofs;
  // Subrobots indexed by name
  std::unordered_map<std::string, RobotPtr> mSubRobots;
 
  // Collision objects.
  dart::collision::CollisionDetectorPtr mCollisionDetector;
  std::shared_ptr<dart::collision::BodyNodeCollisionFilter>
      mSelfCollisionFilter;
 
  // Planning post-processor
  bool mEnablePostProcessing{false};
  std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
      mDefaultPostProcessor{nullptr};
 
  // Custom random-number-generator for repeatable planning and post-processing
  std::unique_ptr<common::RNG> mRng;
 
  // Planning world in which the robot resides
  aikido::planner::WorldPtr mWorld{nullptr};
 
  // Map of commonly-used configurations
  using ConfigurationMap
      = std::unordered_map<std::string, const Eigen::VectorXd>;
  ConfigurationMap mNamedConfigurations;
 
  virtual bool validateSubRobot(
      dart::dynamics::ReferentialSkeletonPtr refSkeleton,
      const std::string& name);
 
  // Parameters for Default VF Planners
  util::VectorFieldPlannerParameters mVFParams;
};
 
} // namespace robot
} // namespace aikido
 
#include "detail/Robot-impl.hpp"
 
#endif // AIKIDO_ROBOT_ROBOT_HPP_