-
Notifications
You must be signed in to change notification settings - Fork 88
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add Tesseract planner for TrajOpt Ifopt
- Loading branch information
Showing
21 changed files
with
1,627 additions
and
27 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
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
46 changes: 46 additions & 0 deletions
46
...de/tesseract_motion_planners/trajopt_ifopt/problem_generators/default_problem_generator.h
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,46 @@ | ||
| /** | ||
| * @file default_problem_generator.h | ||
| * @brief Generates a trajopt problem from a planner request | ||
| * | ||
| * @author Levi Armstrong | ||
| * @date April 18, 2018 | ||
| * @version TODO | ||
| * @bug No known bugs | ||
| * | ||
| * @copyright Copyright (c) 2020, Southwest Research Institute | ||
| * | ||
| * @par License | ||
| * Software License Agreement (Apache License) | ||
| * @par | ||
| * Licensed under the Apache License, Version 2.0 (the "License"); | ||
| * you may not use this file except in compliance with the License. | ||
| * You may obtain a copy of the License at | ||
| * http://www.apache.org/licenses/LICENSE-2.0 | ||
| * @par | ||
| * Unless required by applicable law or agreed to in writing, software | ||
| * distributed under the License is distributed on an "AS IS" BASIS, | ||
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| * See the License for the specific language governing permissions and | ||
| * limitations under the License. | ||
| */ | ||
| #ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PROBLEM_GENERATOR_H | ||
| #define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PROBLEM_GENERATOR_H | ||
|
|
||
| #include <tesseract_common/macros.h> | ||
| TESSERACT_COMMON_IGNORE_WARNINGS_PUSH | ||
| #include <ifopt/problem.h> | ||
| TESSERACT_COMMON_IGNORE_WARNINGS_POP | ||
|
|
||
| #include <tesseract_motion_planners/core/types.h> | ||
| #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h> | ||
|
|
||
| namespace tesseract_planning | ||
| { | ||
| std::shared_ptr<TrajOptIfoptProblem> | ||
| DefaultTrajoptProblemGenerator(const std::string& name, | ||
| const PlannerRequest& request, | ||
| const TrajOptIfoptPlanProfileMap& plan_profiles, | ||
| const TrajOptIfoptCompositeProfileMap& composite_profiles); | ||
|
|
||
| } // namespace tesseract_planning | ||
| #endif |
98 changes: 98 additions & 0 deletions
98
...tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h
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,98 @@ | ||
| /** | ||
| * @file trajopt_default_composite_profile.h | ||
| * @brief | ||
| * | ||
| * @author Levi Armstrong | ||
| * @date June 18, 2020 | ||
| * @version TODO | ||
| * @bug No known bugs | ||
| * | ||
| * @copyright Copyright (c) 2020, Southwest Research Institute | ||
| * | ||
| * @par License | ||
| * Software License Agreement (Apache License) | ||
| * @par | ||
| * Licensed under the Apache License, Version 2.0 (the "License"); | ||
| * you may not use this file except in compliance with the License. | ||
| * You may obtain a copy of the License at | ||
| * http://www.apache.org/licenses/LICENSE-2.0 | ||
| * @par | ||
| * Unless required by applicable law or agreed to in writing, software | ||
| * distributed under the License is distributed on an "AS IS" BASIS, | ||
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| * See the License for the specific language governing permissions and | ||
| * limitations under the License. | ||
| */ | ||
|
|
||
| #ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H | ||
| #define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H | ||
|
|
||
| #include <tesseract_common/macros.h> | ||
| TESSERACT_COMMON_IGNORE_WARNINGS_PUSH | ||
| #include <vector> | ||
| #include <Eigen/Geometry> | ||
| #include <Eigen/Core> | ||
| #include <trajopt_ifopt/constraints/collision_evaluators.h> | ||
| TESSERACT_COMMON_IGNORE_WARNINGS_POP | ||
|
|
||
| #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h> | ||
|
|
||
| namespace tesseract_planning | ||
| { | ||
| class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile | ||
| { | ||
| public: | ||
| TrajOptIfoptDefaultCompositeProfile() = default; | ||
| TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element); | ||
|
|
||
| /** @brief Configuration info for collisions that are modeled as costs */ | ||
| trajopt::TrajOptCollisionConfig::Ptr collision_cost_config; | ||
| /** @brief Configuration info for collisions that are modeled as constraints */ | ||
| trajopt::TrajOptCollisionConfig::Ptr collision_constraint_config; | ||
| /** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/ | ||
| bool smooth_velocities = true; | ||
| /** @brief This default to all ones, but allows you to weight different joints */ | ||
| Eigen::VectorXd velocity_coeff; | ||
| /** @brief If true, a joint acceleration cost with a target of 0 will be applied for all timesteps Default: false*/ | ||
| bool smooth_accelerations = true; | ||
| /** @brief This default to all ones, but allows you to weight different joints */ | ||
| Eigen::VectorXd acceleration_coeff; | ||
| /** @brief If true, a joint jerk cost with a target of 0 will be applied for all timesteps Default: false*/ | ||
| bool smooth_jerks = true; | ||
| /** @brief This default to all ones, but allows you to weight different joints */ | ||
| Eigen::VectorXd jerk_coeff; | ||
|
|
||
| /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states | ||
| * to be considered valid in post checking of trajectory returned by TrajOptIfopt. | ||
| * | ||
| * The resolution is equal to longest_valid_segment_fraction * state_space.getMaximumExtent() | ||
| * | ||
| * Note: The planner takes the conservative of either longest_valid_segment_fraction or longest_valid_segment_length. | ||
| */ | ||
| double longest_valid_segment_fraction = 0.01; // 1% | ||
|
|
||
| /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states | ||
| * to be considered valid. If norm(state1 - state0) > longest_valid_segment_length. | ||
| * | ||
| * Note: This gets converted to longest_valid_segment_fraction. | ||
| * longest_valid_segment_fraction = longest_valid_segment_length / state_space.getMaximumExtent() | ||
| */ | ||
| double longest_valid_segment_length = 0.5; | ||
|
|
||
| /** @brief Special link collision cost distances */ | ||
| trajopt::SafetyMarginData::Ptr special_collision_cost{ nullptr }; | ||
| /** @brief Special link collision constraint distances */ | ||
| trajopt::SafetyMarginData::Ptr special_collision_constraint{ nullptr }; | ||
|
|
||
| void apply(TrajOptIfoptProblem& problem, | ||
| int start_index, | ||
| int end_index, | ||
| const ManipulatorInfo& manip_info, | ||
| const std::vector<std::string>& active_links, | ||
| const std::vector<int>& fixed_indices) const override; | ||
|
|
||
| tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; | ||
| }; | ||
| } // namespace tesseract_planning | ||
|
|
||
| #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H |
78 changes: 78 additions & 0 deletions
78
...lude/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h
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,78 @@ | ||
| /** | ||
| * @file TrajOptIfopt_default_plan_profile.h | ||
| * @brief | ||
| * | ||
| * @author Levi Armstrong | ||
| * @date June 18, 2020 | ||
| * @version TODO | ||
| * @bug No known bugs | ||
| * | ||
| * @copyright Copyright (c) 2020, Southwest Research Institute | ||
| * | ||
| * @par License | ||
| * Software License Agreement (Apache License) | ||
| * @par | ||
| * Licensed under the Apache License, Version 2.0 (the "License"); | ||
| * you may not use this file except in compliance with the License. | ||
| * You may obtain a copy of the License at | ||
| * http://www.apache.org/licenses/LICENSE-2.0 | ||
| * @par | ||
| * Unless required by applicable law or agreed to in writing, software | ||
| * distributed under the License is distributed on an "AS IS" BASIS, | ||
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| * See the License for the specific language governing permissions and | ||
| * limitations under the License. | ||
| */ | ||
|
|
||
| #ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PLAN_PROFILE_H | ||
| #define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PLAN_PROFILE_H | ||
|
|
||
| #include <tesseract_common/macros.h> | ||
| TESSERACT_COMMON_IGNORE_WARNINGS_PUSH | ||
| #include <Eigen/Geometry> | ||
| TESSERACT_COMMON_IGNORE_WARNINGS_POP | ||
|
|
||
| #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h> | ||
| #include <tesseract_command_language/joint_waypoint.h> | ||
| #include <tesseract_command_language/cartesian_waypoint.h> | ||
|
|
||
| namespace tesseract_planning | ||
| { | ||
| class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile | ||
| { | ||
| public: | ||
| using Ptr = std::shared_ptr<TrajOptIfoptDefaultPlanProfile>; | ||
| using ConstPtr = std::shared_ptr<const TrajOptIfoptDefaultPlanProfile>; | ||
|
|
||
| TrajOptIfoptDefaultPlanProfile() = default; | ||
| TrajOptIfoptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); | ||
|
|
||
| Eigen::VectorXd cartesian_coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; | ||
| Eigen::VectorXd joint_coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; | ||
| TrajOptIfoptTermType term_type{ TrajOptIfoptTermType::CONSTRAINT }; | ||
|
|
||
| void apply(TrajOptIfoptProblem& problem, | ||
| const CartesianWaypoint& cartesian_waypoint, | ||
| const Instruction& parent_instruction, | ||
| const ManipulatorInfo& manip_info, | ||
| const std::vector<std::string>& active_links, | ||
| int index) const override; | ||
|
|
||
| void apply(TrajOptIfoptProblem& problem, | ||
| const JointWaypoint& joint_waypoint, | ||
| const Instruction& parent_instruction, | ||
| const ManipulatorInfo& manip_info, | ||
| const std::vector<std::string>& active_links, | ||
| int index) const override; | ||
|
|
||
| tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; | ||
|
|
||
| // protected: | ||
| // void addConstraintErrorFunctions(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) | ||
| // const; | ||
|
|
||
| // void addAvoidSingularity(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;s | ||
| }; | ||
| } // namespace tesseract_planning | ||
|
|
||
| #endif // TESSERACT_MOTION_PLANNERS_TrajOptIfopt_IFOPT_DEFAULT_PLAN_PROFILE_H |
Oops, something went wrong.