-
Notifications
You must be signed in to change notification settings - Fork 30
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Refactor VFP with new Planner API (#426)
- Loading branch information
1 parent
4c64b85
commit 7b6bce6
Showing
13 changed files
with
314 additions
and
60 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
52 changes: 52 additions & 0 deletions
52
include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,52 @@ | ||
| #ifndef AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ | ||
| #define AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ | ||
|
|
||
| #include "aikido/planner/ConfigurationToEndEffectorOffset.hpp" | ||
| #include "aikido/planner/SingleProblemPlanner.hpp" | ||
| #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" | ||
| #include "aikido/trajectory/Trajectory.hpp" | ||
|
|
||
| namespace aikido { | ||
| namespace planner { | ||
|
|
||
| /// Base planner class for ConfigurationToEndEffectorOffset planning problem. | ||
| class ConfigurationToEndEffectorOffsetPlanner | ||
| : public SingleProblemPlanner<ConfigurationToEndEffectorOffsetPlanner, | ||
| ConfigurationToEndEffectorOffset> | ||
| { | ||
| 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 SolverbleProblem, | ||
| // which is simply ConfigurationToEndEffectorOffset. | ||
| using SingleProblemPlanner::plan; | ||
|
|
||
| /// Constructor | ||
| /// | ||
| /// \param[in] stateSpace State space that this planner associated with. | ||
| explicit ConfigurationToEndEffectorOffsetPlanner( | ||
| statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace); | ||
|
|
||
| /// Solves \c problem returning the result to \c result. | ||
| /// | ||
| /// \param[in] problem Planning problem to be solved by the planner. | ||
| /// \param[out] result Result of planning procedure. | ||
| virtual trajectory::TrajectoryPtr plan( | ||
| const SolvableProblem& problem, Result* result = nullptr) | ||
| = 0; | ||
| // Note: SolvableProblem is defined in SingleProblemPlanner. | ||
|
|
||
| /// Return ConstMetaSkeletonStateSpacePtr by performing a static cast on | ||
| /// mStateSpace. | ||
| statespace::dart::ConstMetaSkeletonStateSpacePtr getMetaSkeletonStateSpace(); | ||
|
|
||
| protected: | ||
| /// Stores stateSpace pointer as aConstMetaSkeletonStateSpacePtr. Prevents | ||
| /// use of an expensive dynamic cast. | ||
| statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace; | ||
| }; | ||
|
|
||
| } // namespace planner | ||
| } // namespace aikido | ||
|
|
||
| #endif // AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ |
92 changes: 92 additions & 0 deletions
92
include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| #ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ | ||
| #define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ | ||
|
|
||
| #include "aikido/planner/ConfigurationToEndEffectorOffset.hpp" | ||
| #include "aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp" | ||
|
|
||
| namespace aikido { | ||
| namespace planner { | ||
| namespace vectorfield { | ||
|
|
||
| /// Planner that generates a trajectory that moves the end-effector by a given | ||
| /// direction and distance. | ||
| class VectorFieldConfigurationToEndEffectorOffsetPlanner | ||
| : public ConfigurationToEndEffectorOffsetPlanner | ||
| { | ||
| public: | ||
| /// Constructor. | ||
| /// | ||
| /// \param[in] stateSpace State space that this planner is associated with. | ||
| /// \param[in] metaskeleton MetaSkeleton to plan with. | ||
| /// \param[in] distanceTolerance How much a planned trajectory is allowed to | ||
| /// deviate from the requested distance to move the end-effector. | ||
| /// \param[in] positionTolerance How a planned trajectory is allowed to | ||
| /// deviated from a straight line segment defined by the direction and the | ||
| /// distance. | ||
| /// \param[in] angularTolerance How a planned trajectory is allowed to deviate | ||
| /// from a given direction. | ||
| /// \param[in] initialStepSize Initial step size. | ||
| /// \param[in] jointLimitTolerance If less then this distance to joint | ||
| /// limit, velocity is bounded in that direction to 0. | ||
| /// \param[in] constraintCheckResolution Resolution used in constraint | ||
| /// checking. | ||
| /// \param[in] timelimit timeout in seconds. | ||
| VectorFieldConfigurationToEndEffectorOffsetPlanner( | ||
| statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, | ||
| dart::dynamics::MetaSkeletonPtr metaskeleton, | ||
| double distanceTolerance, | ||
| double positionTolerance, | ||
| double angularTolerance, | ||
| double initialStepSize, | ||
| double jointLimitTolerance, | ||
| double constraintCheckResolution, | ||
| std::chrono::duration<double> timelimit); | ||
|
|
||
| /// Plan to a trajectory that moves the end-effector by a given direction and | ||
| /// distance. | ||
| /// | ||
| /// The planner returns success if the resulting trajectory satisfies | ||
| /// constraint at some resolution and failure (returning \c nullptr) | ||
| /// otherwise. The reason for the failure is stored in the \c result output | ||
| /// parameter. | ||
| /// | ||
| /// \param[in] problem Planning problem. | ||
| /// \param[out] result Information about success or failure. | ||
| /// \return Trajectory or \c nullptr if planning failed. | ||
| trajectory::TrajectoryPtr plan( | ||
| const SolvableProblem& problem, Result* result = nullptr) override; | ||
|
|
||
| protected: | ||
| /// MetaSkeleton to plan with. | ||
| dart::dynamics::MetaSkeletonPtr mMetaskeleton; | ||
|
|
||
| /// How much a planned trajectory is allowed to deviate from the requested | ||
| /// distance to move the end-effector. | ||
| double mDistanceTolerance; | ||
|
|
||
| /// How a planned trajectory is allowed to deviated from a straight line | ||
| /// segment defined by the direction and the distance. | ||
| double mPositionTolerance; | ||
|
|
||
| /// How a planned trajectory is allowed to deviate from a given direction. | ||
| double mAngularTolerance; | ||
|
|
||
| /// Initial step size. | ||
| double mInitialStepSize; | ||
|
|
||
| /// If less then this distance to joint limit, velocity is bounded in that | ||
| /// direction to 0. | ||
| double mJointLimitTolerance; | ||
|
|
||
| /// Resolution used in constraint checking. | ||
| double mConstraintCheckResolution; | ||
|
|
||
| /// Timeout in seconds. | ||
| std::chrono::duration<double> mTimelimit; | ||
| }; | ||
|
|
||
| } // namespace vectorfield | ||
| } // namespace planner | ||
| } // namespace aikido | ||
|
|
||
| #endif // AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,25 @@ | ||
| #include "aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp" | ||
|
|
||
| namespace aikido { | ||
| namespace planner { | ||
|
|
||
| //============================================================================== | ||
| ConfigurationToEndEffectorOffsetPlanner:: | ||
| ConfigurationToEndEffectorOffsetPlanner( | ||
| statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace) | ||
| : SingleProblemPlanner<ConfigurationToEndEffectorOffsetPlanner, | ||
| ConfigurationToEndEffectorOffset>(stateSpace) | ||
| , mMetaSkeletonStateSpace(std::move(stateSpace)) | ||
| { | ||
| // Do nothing | ||
| } | ||
|
|
||
| //============================================================================== | ||
| statespace::dart::ConstMetaSkeletonStateSpacePtr | ||
| ConfigurationToEndEffectorOffsetPlanner::getMetaSkeletonStateSpace() | ||
| { | ||
| return mMetaSkeletonStateSpace; | ||
| } | ||
|
|
||
| } // namespace planner | ||
| } // namespace aikido |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
88 changes: 88 additions & 0 deletions
88
src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,88 @@ | ||
| #include "aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp" | ||
|
|
||
| #include "aikido/planner/vectorfield/VectorFieldPlanner.hpp" | ||
|
|
||
| namespace aikido { | ||
| namespace planner { | ||
| namespace vectorfield { | ||
|
|
||
| //============================================================================== | ||
| VectorFieldConfigurationToEndEffectorOffsetPlanner:: | ||
| VectorFieldConfigurationToEndEffectorOffsetPlanner( | ||
| statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, | ||
| dart::dynamics::MetaSkeletonPtr metaskeleton, | ||
| double distanceTolerance, | ||
| double positionTolerance, | ||
| double angularTolerance, | ||
| double initialStepSize, | ||
| double jointLimitTolerance, | ||
| double constraintCheckResolution, | ||
| std::chrono::duration<double> timelimit) | ||
| : ConfigurationToEndEffectorOffsetPlanner(std::move(stateSpace)) | ||
| , mMetaskeleton(std::move(metaskeleton)) | ||
| , mDistanceTolerance(distanceTolerance) | ||
| , mPositionTolerance(positionTolerance) | ||
| , mAngularTolerance(angularTolerance) | ||
| , mInitialStepSize(initialStepSize) | ||
| , mJointLimitTolerance(jointLimitTolerance) | ||
| , mConstraintCheckResolution(constraintCheckResolution) | ||
| , mTimelimit(timelimit) | ||
| { | ||
| // Do nothing here. | ||
| } | ||
|
|
||
| //============================================================================== | ||
| trajectory::TrajectoryPtr | ||
| VectorFieldConfigurationToEndEffectorOffsetPlanner::plan( | ||
| const SolvableProblem& problem, Result* result) | ||
| { | ||
| // TODO (sniyaz): Check equality between state space of this planner and given | ||
| // problem. | ||
|
|
||
| using aikido::planner::vectorfield::planToEndEffectorOffset; | ||
| using aikido::statespace::dart::MetaSkeletonStateSpace; | ||
|
|
||
| // Handle the fact that distance can be negative. | ||
| double distance = problem.getDistance(); | ||
| Eigen::Vector3d direction = problem.getDirection(); | ||
|
|
||
| if (distance < 0) | ||
| { | ||
| distance = -1.0 * distance; | ||
| direction = -1.0 * direction; | ||
| } | ||
|
|
||
| double minDistance = distance - mDistanceTolerance; | ||
| double maxDistance = distance + mDistanceTolerance; | ||
|
|
||
| if (minDistance < 0.) | ||
| { | ||
| std::stringstream ss; | ||
| ss << "Distance must be non-negative; min distance to move is " | ||
| << problem.getDistance() - mDistanceTolerance << "."; | ||
| throw std::runtime_error(ss.str()); | ||
| } | ||
|
|
||
| // Just call the core VFP function. | ||
| // TODO: How should start state be handled? | ||
| return planToEndEffectorOffset( | ||
| *problem.getStartState(), | ||
| getMetaSkeletonStateSpace(), | ||
| mMetaskeleton, | ||
| problem.getEndEffectorBodyNode(), | ||
| problem.getConstraint(), | ||
| direction, | ||
| minDistance, | ||
| maxDistance, | ||
| mPositionTolerance, | ||
| mAngularTolerance, | ||
| mInitialStepSize, | ||
| mJointLimitTolerance, | ||
| mConstraintCheckResolution, | ||
| mTimelimit, | ||
| result); | ||
| } | ||
|
|
||
| } // namespace vectorfield | ||
| } // namespace planner | ||
| } // namespace aikido |
Oops, something went wrong.