Add ConfigurationToTSRwithTrajectoryConstraint

include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp

@@ -0,0 +1,53 @@
+#ifndef AIKIDO_PLANNER_DART_CRRTCONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINTPLANNER_HPP_
+#define AIKIDO_PLANNER_DART_CRRTCONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINTPLANNER_HPP_
+
+#include "aikido/planner/dart/ConfigurationToTSRPlanner.hpp"
+#include "aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp"
+#include "aikido/robot/util.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "aikido/trajectory/Trajectory.hpp"
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+class CRRTConfigurationToTSRwithTrajectoryConstraintPlanner
+  : public dart::SingleProblemPlanner<
+        CRRTConfigurationToTSRwithTrajectoryConstraintPlanner,
+        ConfigurationToTSRwithTrajectoryConstraint>
+{
+public:
+  // Expose the implementation of Planner::plan(const Problem&, Result*) in
+  // SingleProblemPlanner. Note that plan() of the base class takes Problem
+  // while the virtual function defined in this class takes SolvableProblem,
+  // which is simply ConfigurationToTSRwithTrajectoryConstraint.
+  using SingleProblemPlanner::plan;
+
+  /// Constructor
+  ///
+  /// \param[in] stateSpace State space that this planner associated with.
+  /// \param[in] metaSkeleton MetaSkeleton to use for planning.
+  /// \param[in] timelimit Timelimit for planning.
+  /// \param[in] crrtParameters CRRT planner parameters.
+  CRRTConfigurationToTSRwithTrajectoryConstraintPlanner(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
+      double timelimit,
+      robot::util::CRRTPlannerParameters crrtParameters);
+
+  /// \copydoc SingleProblemPlanner::plan()
+  virtual trajectory::TrajectoryPtr plan(
+      const SolvableProblem& problem, Result* result = nullptr) final;
+
+protected:
+  const robot::util::CRRTPlannerParameters mCRRTParameters;
+
+  // Planning timelimit
+  const double mTimelimit;
+};
+
+} // namespace dart
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_DART_CRRTCONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINTPLANNER_HPP_

include/aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp

@@ -0,0 +1,68 @@
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINT_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINT_HPP_
+
+#include "aikido/planner/dart/ConfigurationToTSR.hpp"
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+/// Planning problem to plan to a given single Task Space Region (TSR).
+class ConfigurationToTSRwithTrajectoryConstraint : public ConfigurationToTSR
+{
+public:
+  /// Constructor. Note that this constructor takes the start state from the
+  /// current state of the passed MetaSkeleton.
+  ///
+  /// \param[in] stateSpace State space.
+  /// \param[in] metaSkeleton MetaSkeleton that getStartState will return the
+  /// current state of when called.
+  /// \param[in] endEffectorBodyNode BodyNode to be planned to move to a desired
+  /// TSR.
+  /// \param[in] goalTSR Goal TSR.
+  /// \param[in] constraintTsr The constraint TSR for the trajectory.
+  /// \param[in] constraint Trajectory-wide constraint that must be satisfied.
+  /// \throw If \c stateSpace is not compatible with \c constraint's state
+  /// space.
+  ConfigurationToTSRwithTrajectoryConstraint(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton,
+      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      constraint::dart::ConstTSRPtr goalTSR,
+      constraint::dart::ConstTSRPtr constraintTSR,
+      constraint::ConstTestablePtr constraint = nullptr);
+
+  /// Constructor. Note that this constructor sets the start state on
+  /// construction.
+  ///
+  /// \param[in] stateSpace State space.
+  /// \param[in] startState Start state to plan from.
+  /// \param[in] endEffectorBodyNode BodyNode to be planned to move to a desired
+  /// TSR.
+  /// \param[in] maxSamples Maximum number of TSR samples to plan to.
+  /// \param[in] goalTSR Goal TSR.
+  /// \param[in] constraintTsr The constraint TSR for the trajectory
+  /// \param[in] constraint Trajectory-wide constraint that must be satisfied.
+  /// \throw If \c stateSpace is not compatible with \c constraint's state
+  /// space.
+  ConfigurationToTSRwithTrajectoryConstraint(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      const statespace::dart::MetaSkeletonStateSpace::State* startState,
+      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      constraint::dart::ConstTSRPtr goalTSR,
+      constraint::dart::ConstTSRPtr constraintTSR,
+      constraint::ConstTestablePtr constraint = nullptr);
+
+  /// Return the trajectory constraint TSR
+  constraint::dart::ConstTSRPtr getConstraintTSR() const;
+
+protected:
+  /// Constraint TSR
+  const constraint::dart::ConstTSRPtr mConstraintTSR;
+};
+
+} // namespace dart
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINT_HPP_

src/planner/dart/CMakeLists.txt

@@ -23,6 +23,8 @@ set(sources
   ConfigurationToEndEffectorOffsetPlanner.cpp
   ConfigurationToTSR.cpp
   ConfigurationToTSRPlanner.cpp
+  ConfigurationToTSRwithTrajectoryConstraint.cpp
+  CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp
   util.cpp
 )
 

src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp

@@ -0,0 +1,164 @@
+#include "aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp"
+
+#include <ompl/geometric/planners/rrt/RRTConnect.h>
+
+#include "aikido/constraint.hpp"
+#include "aikido/constraint/CyclicSampleable.hpp"
+#include "aikido/constraint/FiniteSampleable.hpp"
+#include "aikido/constraint/NewtonsMethodProjectable.hpp"
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/constraint/TestableIntersection.hpp"
+#include "aikido/constraint/dart/InverseKinematicsSampleable.hpp"
+#include "aikido/distance/defaults.hpp"
+#include "aikido/planner/ompl/Planner.hpp"
+#include "aikido/statespace/GeodesicInterpolator.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+using constraint::CyclicSampleable;
+using constraint::NewtonsMethodProjectable;
+using constraint::Sampleable;
+using constraint::Testable;
+using constraint::dart::createProjectableBounds;
+using constraint::dart::createSampleableBounds;
+using constraint::dart::createTestableBounds;
+using constraint::dart::FrameDifferentiable;
+using constraint::dart::FrameTestable;
+using constraint::dart::InverseKinematicsSampleable;
+using constraint::dart::TSR;
+using distance::createDistanceMetric;
+using planner::ompl::planCRRTConnect;
+using statespace::GeodesicInterpolator;
+using statespace::dart::MetaSkeletonStateSaver;
+using statespace::dart::MetaSkeletonStateSpace;
+
+using ::dart::dynamics::BodyNode;
+using ::dart::dynamics::InverseKinematics;
+
+//==============================================================================
+CRRTConfigurationToTSRwithTrajectoryConstraintPlanner::
+    CRRTConfigurationToTSRwithTrajectoryConstraintPlanner(
+        statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+        ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
+        double timelimit,
+        robot::util::CRRTPlannerParameters crrtParameters)
+  : dart::SingleProblemPlanner<
+        CRRTConfigurationToTSRwithTrajectoryConstraintPlanner,
+        ConfigurationToTSRwithTrajectoryConstraint>(
+        std::move(stateSpace), std::move(metaSkeleton))
+  , mCRRTParameters(std::move(crrtParameters))
+  , mTimelimit(timelimit)
+{
+  // Do nothing
+}
+
+//==============================================================================
+trajectory::TrajectoryPtr
+CRRTConfigurationToTSRwithTrajectoryConstraintPlanner::plan(
+    const SolvableProblem& problem, Result* /*result*/)
+{
+  std::size_t projectionMaxIteration = mCRRTParameters.projectionMaxIteration;
+  double projectionTolerance = mCRRTParameters.projectionTolerance;
+
+  auto robot = mMetaSkeleton->getBodyNode(0)->getSkeleton();
+  std::lock_guard<std::mutex> lock(robot->getMutex());
+
+  // Save the current state of the stateSpace
+  auto saver = MetaSkeletonStateSaver(mMetaSkeleton);
+  DART_UNUSED(saver);
+
+  // Create seed constraint
+  std::shared_ptr<Sampleable> seedConstraint = createSampleableBounds(
+      mMetaSkeletonStateSpace, mCRRTParameters.rng->clone());
+
+  // TODO: DART may be updated to check for single skeleton
+  if (mMetaSkeleton->getNumDofs() == 0)
+    throw std::invalid_argument("MetaSkeleton has 0 degrees of freedom.");
+
+  auto skeleton = mMetaSkeleton->getDof(0)->getSkeleton();
+  for (size_t i = 1; i < mMetaSkeleton->getNumDofs(); ++i)
+  {
+    if (mMetaSkeleton->getDof(i)->getSkeleton() != skeleton)
+      throw std::invalid_argument("MetaSkeleton has more than 1 skeleton.");
+  }
+
+  auto bodyNode = problem.getEndEffectorBodyNode();
+  auto goalTsr = problem.getGoalTSR();
+  auto constraintTsr = problem.getConstraintTSR();
+
+  // Create an IK solver with metaSkeleton dofs
+  auto ik = InverseKinematics::create(const_cast<BodyNode*>(bodyNode.get()));
+
+  ik->setDofs(mMetaSkeleton->getDofs());
+
+  // create goal sampleable
+  auto goalSampleable = std::make_shared<InverseKinematicsSampleable>(
+      mMetaSkeletonStateSpace,
+      mMetaSkeleton,
+      std::make_shared<CyclicSampleable>(std::const_pointer_cast<TSR>(goalTsr)),
+      seedConstraint,
+      ik,
+      mCRRTParameters.maxNumTrials);
+
+  // create goal testable
+  auto goalTestable = std::make_shared<FrameTestable>(
+      std::const_pointer_cast<MetaSkeletonStateSpace>(mMetaSkeletonStateSpace),
+      mMetaSkeleton,
+      bodyNode.get(),
+      std::const_pointer_cast<TSR>(goalTsr));
+
+  // create constraint sampleable
+  auto constraintSampleable = std::make_shared<InverseKinematicsSampleable>(
+      mMetaSkeletonStateSpace,
+      mMetaSkeleton,
+      std::const_pointer_cast<TSR>(constraintTsr),
+      seedConstraint,
+      ik,
+      mCRRTParameters.maxNumTrials);
+
+  // create constraint projectable
+  auto frameDiff = std::make_shared<FrameDifferentiable>(
+      std::const_pointer_cast<MetaSkeletonStateSpace>(mMetaSkeletonStateSpace),
+      mMetaSkeleton,
+      bodyNode.get(),
+      std::const_pointer_cast<TSR>(constraintTsr));
+
+  std::vector<double> projectionToleranceVec(
+      frameDiff->getConstraintDimension(), projectionTolerance);
+  auto constraintProjectable = std::make_shared<NewtonsMethodProjectable>(
+      frameDiff, projectionToleranceVec, projectionMaxIteration);
+
+  // Current state
+  auto startState = mMetaSkeletonStateSpace->getScopedStateFromMetaSkeleton(
+      mMetaSkeleton.get());
+
+  // Call planner
+  auto traj = planCRRTConnect(
+      startState,
+      goalTestable,
+      goalSampleable,
+      constraintProjectable,
+      mMetaSkeletonStateSpace,
+      std::make_shared<GeodesicInterpolator>(mMetaSkeletonStateSpace),
+      createDistanceMetric(mMetaSkeletonStateSpace),
+      constraintSampleable,
+      std::const_pointer_cast<constraint::Testable>(problem.getConstraint()),
+      createTestableBounds(mMetaSkeletonStateSpace),
+      createProjectableBounds(mMetaSkeletonStateSpace),
+      mTimelimit,
+      mCRRTParameters.maxExtensionDistance,
+      mCRRTParameters.maxDistanceBtwProjections,
+      mCRRTParameters.minStepSize,
+      mCRRTParameters.minTreeConnectionDistance);
+
+  return traj;
+}
+
+} // namespace dart
+} // namespace planner
+} // namespace aikido

src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp

@@ -0,0 +1,60 @@
+#include "aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp"
+
+#include "aikido/constraint/Testable.hpp"
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+//==============================================================================
+ConfigurationToTSRwithTrajectoryConstraint::
+    ConfigurationToTSRwithTrajectoryConstraint(
+        statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+        ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton,
+        ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+        constraint::dart::ConstTSRPtr goalTSR,
+        constraint::dart::ConstTSRPtr constraintTSR,
+        constraint::ConstTestablePtr constraint)
+  : ConfigurationToTSR(
+        std::move(stateSpace),
+        std::move(metaSkeleton),
+        std::move(endEffectorBodyNode),
+        0, // This parameter is not used in this class.
+        std::move(goalTSR),
+        std::move(constraint))
+  , mConstraintTSR(constraintTSR)
+{
+  // Do nothing.
+}
+
+//==============================================================================
+ConfigurationToTSRwithTrajectoryConstraint::
+    ConfigurationToTSRwithTrajectoryConstraint(
+        statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+        const statespace::dart::MetaSkeletonStateSpace::State* startState,
+        ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+        constraint::dart::ConstTSRPtr goalTSR,
+        constraint::dart::ConstTSRPtr constraintTSR,
+        constraint::ConstTestablePtr constraint)
+  : ConfigurationToTSR(
+        std::move(stateSpace),
+        startState,
+        std::move(endEffectorBodyNode),
+        0, // This parameter is not used in this class.
+        std::move(goalTSR),
+        std::move(constraint))
+  , mConstraintTSR(constraintTSR)
+{
+  // Do nothing.
+}
+
+//==============================================================================
+constraint::dart::ConstTSRPtr
+ConfigurationToTSRwithTrajectoryConstraint::getConstraintTSR() const
+{
+  return mConstraintTSR;
+}
+
+} // namespace dart
+} // namespace planner
+} // namespace aikido