Skip to content

Commit e71ff70

Browse files
mpowelsonmpowelson
committed
More cleanup and add TrajOptIfoptProblem
1 parent 0f2a50e commit e71ff70

File tree

10 files changed

+376
-302
lines changed

10 files changed

+376
-302
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/problem_generators/default_problem_generator.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3636

3737
namespace tesseract_planning
3838
{
39-
std::shared_ptr<ifopt::Problem> DefaultTrajoptProblemGenerator(const std::string& name,
39+
std::shared_ptr<TrajOptIfoptProblem> DefaultTrajoptProblemGenerator(const std::string& name,
4040
const PlannerRequest& request,
4141
const TrajOptIfoptPlanProfileMap& plan_profiles,
4242
const TrajOptIfoptCompositeProfileMap& composite_profiles);

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h

Lines changed: 95 additions & 95 deletions
Original file line numberDiff line numberDiff line change
@@ -45,101 +45,101 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
4545
TrajOptIfoptDefaultCompositeProfile() = default;
4646
TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);
4747

48-
// /** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */
49-
// tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL;
50-
// /** @brief Configuration info for collisions that are modeled as costs */
51-
// CollisionCostConfig collision_cost_config;
52-
// /** @brief Configuration info for collisions that are modeled as constraints */
53-
// CollisionConstraintConfig collision_constraint_config;
54-
// /** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/
55-
// bool smooth_velocities = true;
56-
// /** @brief This default to all ones, but allows you to weight different joints */
57-
// Eigen::VectorXd velocity_coeff;
58-
// /** @brief If true, a joint acceleration cost with a target of 0 will be applied for all timesteps Default: false*/
59-
// bool smooth_accelerations = true;
60-
// /** @brief This default to all ones, but allows you to weight different joints */
61-
// Eigen::VectorXd acceleration_coeff;
62-
// /** @brief If true, a joint jerk cost with a target of 0 will be applied for all timesteps Default: false*/
63-
// bool smooth_jerks = true;
64-
// /** @brief This default to all ones, but allows you to weight different joints */
65-
// Eigen::VectorXd jerk_coeff;
66-
// /** @brief If true, applies a cost to avoid kinematic singularities */
67-
// bool avoid_singularity = false;
68-
// /** @brief Optimization weight associated with kinematic singularity avoidance */
69-
// double avoid_singularity_coeff = 5.0;
70-
71-
// /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
72-
// * to be considered valid in post checking of trajectory returned by TrajOptIfopt.
73-
// *
74-
// * The resolution is equal to longest_valid_segment_fraction * state_space.getMaximumExtent()
75-
// *
76-
// * Note: The planner takes the conservative of either longest_valid_segment_fraction or longest_valid_segment_length.
77-
// */
78-
// double longest_valid_segment_fraction = 0.01; // 1%
79-
80-
// /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
81-
// * to be considered valid. If norm(state1 - state0) > longest_valid_segment_length.
82-
// *
83-
// * Note: This gets converted to longest_valid_segment_fraction.
84-
// * longest_valid_segment_fraction = longest_valid_segment_length / state_space.getMaximumExtent()
85-
// */
86-
// double longest_valid_segment_length = 0.5;
87-
88-
// /**@brief Special link collision cost distances */
89-
// trajopt::SafetyMarginData::Ptr special_collision_cost{ nullptr };
90-
// /**@brief Special link collision constraint distances */
91-
// trajopt::SafetyMarginData::Ptr special_collision_constraint{ nullptr };
92-
93-
// void apply(trajopt::ProblemConstructionInfo& pci,
94-
// int start_index,
95-
// int end_index,
96-
// const ManipulatorInfo& manip_info,
97-
// const std::vector<std::string>& active_links,
98-
// const std::vector<int>& fixed_indices) override;
99-
100-
// tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
101-
102-
//protected:
103-
// void addCollisionCost(trajopt::ProblemConstructionInfo& pci,
104-
// int start_index,
105-
// int end_index,
106-
// const std::vector<int>& fixed_indices) const;
107-
108-
// void addCollisionConstraint(trajopt::ProblemConstructionInfo& pci,
109-
// int start_index,
110-
// int end_index,
111-
// const std::vector<int>& fixed_indices) const;
112-
113-
// void addVelocitySmoothing(trajopt::ProblemConstructionInfo& pci,
114-
// int start_index,
115-
// int end_index,
116-
// const std::vector<int>& fixed_indices) const;
117-
118-
// void addAccelerationSmoothing(trajopt::ProblemConstructionInfo& pci,
119-
// int start_index,
120-
// int end_index,
121-
// const std::vector<int>& fixed_indices) const;
122-
123-
// void addJerkSmoothing(trajopt::ProblemConstructionInfo& pci,
124-
// int start_index,
125-
// int end_index,
126-
// const std::vector<int>& fixed_indices) const;
127-
128-
// void addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci,
129-
// int start_index,
130-
// int end_index,
131-
// const std::vector<int>& fixed_indices) const;
132-
133-
// void addAvoidSingularity(trajopt::ProblemConstructionInfo& pci,
134-
// int start_index,
135-
// int end_index,
136-
// const std::string& link,
137-
// const std::vector<int>& fixed_indices) const;
138-
139-
// void smoothMotionTerms(const tinyxml2::XMLElement& xml_element,
140-
// bool& enabled,
141-
// Eigen::VectorXd& coeff,
142-
// std::size_t& length);
48+
/** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */
49+
tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL;
50+
/** @brief Configuration info for collisions that are modeled as costs */
51+
CollisionCostConfig collision_cost_config;
52+
/** @brief Configuration info for collisions that are modeled as constraints */
53+
CollisionConstraintConfig collision_constraint_config;
54+
/** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/
55+
bool smooth_velocities = true;
56+
/** @brief This default to all ones, but allows you to weight different joints */
57+
Eigen::VectorXd velocity_coeff;
58+
/** @brief If true, a joint acceleration cost with a target of 0 will be applied for all timesteps Default: false*/
59+
bool smooth_accelerations = true;
60+
/** @brief This default to all ones, but allows you to weight different joints */
61+
Eigen::VectorXd acceleration_coeff;
62+
/** @brief If true, a joint jerk cost with a target of 0 will be applied for all timesteps Default: false*/
63+
bool smooth_jerks = true;
64+
/** @brief This default to all ones, but allows you to weight different joints */
65+
Eigen::VectorXd jerk_coeff;
66+
/** @brief If true, applies a cost to avoid kinematic singularities */
67+
bool avoid_singularity = false;
68+
/** @brief Optimization weight associated with kinematic singularity avoidance */
69+
double avoid_singularity_coeff = 5.0;
70+
71+
/** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
72+
* to be considered valid in post checking of trajectory returned by TrajOptIfopt.
73+
*
74+
* The resolution is equal to longest_valid_segment_fraction * state_space.getMaximumExtent()
75+
*
76+
* Note: The planner takes the conservative of either longest_valid_segment_fraction or longest_valid_segment_length.
77+
*/
78+
double longest_valid_segment_fraction = 0.01; // 1%
79+
80+
/** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
81+
* to be considered valid. If norm(state1 - state0) > longest_valid_segment_length.
82+
*
83+
* Note: This gets converted to longest_valid_segment_fraction.
84+
* longest_valid_segment_fraction = longest_valid_segment_length / state_space.getMaximumExtent()
85+
*/
86+
double longest_valid_segment_length = 0.5;
87+
88+
/** @brief Special link collision cost distances */
89+
trajopt::SafetyMarginData::Ptr special_collision_cost{ nullptr };
90+
/** @brief Special link collision constraint distances */
91+
trajopt::SafetyMarginData::Ptr special_collision_constraint{ nullptr };
92+
93+
void apply(TrajOptIfoptProblem& problem,
94+
int start_index,
95+
int end_index,
96+
const ManipulatorInfo& manip_info,
97+
const std::vector<std::string>& active_links,
98+
const std::vector<int>& fixed_indices) override;
99+
100+
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
101+
102+
protected:
103+
void addCollisionCost(TrajOptIfoptProblem& problem,
104+
int start_index,
105+
int end_index,
106+
const std::vector<int>& fixed_indices) const;
107+
108+
void addCollisionConstraint(TrajOptIfoptProblem& problem,
109+
int start_index,
110+
int end_index,
111+
const std::vector<int>& fixed_indices) const;
112+
113+
void addVelocitySmoothing(TrajOptIfoptProblem& problem,
114+
int start_index,
115+
int end_index,
116+
const std::vector<int>& fixed_indices) const;
117+
118+
void addAccelerationSmoothing(TrajOptIfoptProblem& problem,
119+
int start_index,
120+
int end_index,
121+
const std::vector<int>& fixed_indices) const;
122+
123+
void addJerkSmoothing(TrajOptIfoptProblem& problem,
124+
int start_index,
125+
int end_index,
126+
const std::vector<int>& fixed_indices) const;
127+
128+
void addConstraintErrorFunctions(TrajOptIfoptProblem& problem,
129+
int start_index,
130+
int end_index,
131+
const std::vector<int>& fixed_indices) const;
132+
133+
void addAvoidSingularity(TrajOptIfoptProblem& problem,
134+
int start_index,
135+
int end_index,
136+
const std::string& link,
137+
const std::vector<int>& fixed_indices) const;
138+
139+
void smoothMotionTerms(const tinyxml2::XMLElement& xml_element,
140+
bool& enabled,
141+
Eigen::VectorXd& coeff,
142+
std::size_t& length);
143143
};
144144
} // namespace tesseract_planning
145145

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -70,14 +70,14 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
7070
// std::vector<std::tuple<sco::VectorOfVector::func, sco::MatrixOfVector::func, sco::ConstraintType, Eigen::VectorXd>>
7171
// constraint_error_functions;
7272

73-
void apply(std::shared_ptr<ifopt::Problem>& nlp,
73+
void apply(TrajOptIfoptProblem& problem,
7474
const Eigen::Isometry3d& cartesian_waypoint,
7575
const Instruction& parent_instruction,
7676
const ManipulatorInfo& manip_info,
7777
const std::vector<std::string>& active_links,
7878
int index) override;
7979

80-
void apply(std::shared_ptr<ifopt::Problem>& nlp,
80+
void apply(TrajOptIfoptProblem& problem,
8181
const Eigen::VectorXd& joint_waypoint,
8282
const Instruction& parent_instruction,
8383
const ManipulatorInfo& manip_info,

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3636

3737
#include <tesseract_command_language/core/instruction.h>
3838
#include <tesseract_command_language/manipulator_info.h>
39+
#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h>
3940

4041
namespace tesseract_planning
4142
{
@@ -45,14 +46,14 @@ class TrajOptIfoptPlanProfile
4546
using Ptr = std::shared_ptr<TrajOptIfoptPlanProfile>;
4647
using ConstPtr = std::shared_ptr<const TrajOptIfoptPlanProfile>;
4748

48-
virtual void apply(std::shared_ptr<ifopt::Problem>& nlp,
49+
virtual void apply(TrajOptIfoptProblem& problem,
4950
const Eigen::Isometry3d& cartesian_waypoint,
5051
const Instruction& parent_instruction,
5152
const ManipulatorInfo& manip_info,
5253
const std::vector<std::string>& active_links,
5354
int index) = 0;
5455

55-
virtual void apply(std::shared_ptr<ifopt::Problem>& nlp,
56+
virtual void apply(TrajOptIfoptProblem& problem,
5657
const Eigen::VectorXd& joint_waypoint,
5758
const Instruction& parent_instruction,
5859
const ManipulatorInfo& manip_info,
@@ -68,7 +69,7 @@ class TrajOptIfoptCompositeProfile
6869
using Ptr = std::shared_ptr<TrajOptIfoptCompositeProfile>;
6970
using ConstPtr = std::shared_ptr<const TrajOptIfoptCompositeProfile>;
7071

71-
virtual void apply(std::shared_ptr<ifopt::Problem>& nlp,
72+
virtual void apply(TrajOptIfoptProblem& problem,
7273
int start_index,
7374
int end_index,
7475
const ManipulatorInfo& manip_info,

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
/**
2+
* @file trajopt_ifopt_problem.h
3+
* @brief
4+
*
5+
* @author Levi Armstrong
6+
* @date June 18, 2020
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2020, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROBLEM_H
27+
#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROBLEM_H
28+
29+
#include <tesseract_common/macros.h>
30+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31+
#include <ifopt/problem.h>
32+
#include <vector>
33+
#include <memory>
34+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
35+
36+
#include <tesseract/tesseract.h>
37+
#include <tesseract_environment/core/environment.h>
38+
39+
namespace tesseract_planning
40+
{
41+
struct TrajOptIfoptProblem
42+
{
43+
// These are required for Tesseract to configure Descartes
44+
tesseract::Tesseract::ConstPtr tesseract;
45+
tesseract_environment::EnvState::ConstPtr env_state;
46+
47+
// Kinematic Objects
48+
tesseract_kinematics::ForwardKinematics::ConstPtr manip_fwd_kin;
49+
tesseract_kinematics::InverseKinematics::ConstPtr manip_inv_kin;
50+
51+
std::shared_ptr<ifopt::Problem> nlp;
52+
};
53+
54+
} // namespace tesseract_planning
55+
#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_PROBLEM_H

0 commit comments

Comments
 (0)