Skip to content

Commit 86b676e

Browse files
mpowelsonmpowelson
committed
Collision term fixes
1 parent 5fe6f45 commit 86b676e

File tree

6 files changed

+80
-318
lines changed

6 files changed

+80
-318
lines changed

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

+2-2
Original file line numberDiff line numberDiff line change
@@ -46,9 +46,9 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
4646
TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);
4747

4848
/** @brief Configuration info for collisions that are modeled as costs */
49-
CollisionCostConfig collision_cost_config;
49+
TrajOptIfoptCollisionConfig::Ptr collision_cost_config;
5050
/** @brief Configuration info for collisions that are modeled as constraints */
51-
CollisionConstraintConfig collision_constraint_config;
51+
TrajOptIfoptCollisionConfig::Ptr collision_constraint_config;
5252
/** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/
5353
bool smooth_velocities = true;
5454
/** @brief This default to all ones, but allows you to weight different joints */

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

+7-47
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@
2828

2929
#include <tesseract_common/macros.h>
3030
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31-
#include <trajopt/problem_description.hpp>
31+
#include <tesseract_collision/core/types.h>
3232
#include <tinyxml2.h>
3333
#include <boost/algorithm/string.hpp>
3434
TESSERACT_COMMON_IGNORE_WARNINGS_POP
@@ -41,13 +41,13 @@ namespace tesseract_planning
4141
/**
4242
* @brief Config settings for collision terms.
4343
*/
44-
struct CollisionConfig
44+
struct TrajOptIfoptCollisionConfig : public tesseract_collision::CollisionCheckConfig
4545
{
46-
using Ptr = std::shared_ptr<CollisionConfig>;
47-
using ConstPtr = std::shared_ptr<const CollisionConfig>;
46+
using Ptr = std::shared_ptr<TrajOptIfoptCollisionConfig>;
47+
using ConstPtr = std::shared_ptr<const TrajOptIfoptCollisionConfig>;
4848

49-
CollisionConfig() = default;
50-
CollisionConfig(const tinyxml2::XMLElement& xml_element);
49+
TrajOptIfoptCollisionConfig() = default;
50+
TrajOptIfoptCollisionConfig(const tinyxml2::XMLElement& xml_element);
5151

5252
/** @brief If true, a collision cost term will be added to the problem. Default: true*/
5353
bool enabled = true;
@@ -56,55 +56,15 @@ struct CollisionConfig
5656
* If set to true, it is recommended to start with the coeff set to one
5757
*/
5858
bool use_weighted_sum = false;
59-
/** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */
60-
tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL;
61-
/** @brief The evaluator type that will be used for collision checking. */
62-
trajopt::CollisionEvaluatorType type = trajopt::CollisionEvaluatorType::CAST_CONTINUOUS;
6359

64-
/** @brief Max distance in which collision costs will be evaluated. */
65-
double buffer_margin = 0.025;
66-
/** @brief Distance beyond buffer_margin in which collision optimization will be evaluated.
67-
This is set to 0 by default (effectively disabled) for collision costs.*/
68-
double safety_margin_buffer = 0.0;
6960
/** @brief The collision coeff/weight */
7061
double coeff = 20;
7162

72-
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
73-
};
74-
75-
/**
76-
* @brief Config settings for collision cost terms.
77-
*/
78-
struct CollisionCostConfig : public CollisionConfig
79-
{
80-
using Ptr = std::shared_ptr<CollisionCostConfig>;
81-
using ConstPtr = std::shared_ptr<const CollisionCostConfig>;
82-
83-
CollisionCostConfig() = default;
84-
CollisionCostConfig(const tinyxml2::XMLElement& xml_element);
63+
tesseract_environment::Environment::Ptr environment;
8564

8665
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
87-
88-
protected:
89-
TrajOptIfoptTermType term_type_ = TrajOptIfoptTermType::SQUARED_COST;
9066
};
9167

92-
/**
93-
* @brief Config settings for collision constraint terms.
94-
*/
95-
struct CollisionConstraintConfig : public CollisionConfig
96-
{
97-
using Ptr = std::shared_ptr<CollisionConstraintConfig>;
98-
using ConstPtr = std::shared_ptr<const CollisionConstraintConfig>;
99-
100-
CollisionConstraintConfig() = default;
101-
CollisionConstraintConfig(const tinyxml2::XMLElement& xml_element);
102-
103-
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
104-
105-
protected:
106-
TrajOptIfoptTermType term_type_ = TrajOptIfoptTermType::CONSTRAINT;
107-
};
10868
} // namespace tesseract_planning
10969

11070
#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_CONFIG_TRAJOPT_IFOPT_COLLISION_CONFIG_H

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

+12-9
Original file line numberDiff line numberDiff line change
@@ -29,9 +29,9 @@
2929
#include <tesseract_common/macros.h>
3030
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3131
#include <trajopt_ifopt/trajopt_ifopt.h>
32+
#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_collision_config.h>
3233
#include <ifopt/constraint_set.h>
3334
#include <ifopt/problem.h>
34-
#include <trajopt/problem_description.hpp>
3535
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3636

3737
#include <tesseract_command_language/cartesian_waypoint.h>
@@ -70,15 +70,18 @@ bool addJointPositionSquaredCost(std::shared_ptr<ifopt::Problem> nlp,
7070
trajopt::JointPosition::ConstPtr var,
7171
const Eigen::Ref<const Eigen::VectorXd>& coeff);
7272

73-
ifopt::ConstraintSet::Ptr createCollisionTermInfo(
73+
std::vector<ifopt::ConstraintSet::Ptr> createCollisionConstraints(
7474
std::vector<trajopt::JointPosition::ConstPtr> vars,
75-
double collision_safety_margin,
76-
double collision_safety_margin_buffer,
77-
trajopt::CollisionEvaluatorType evaluator_type,
78-
bool use_weighted_sum = false,
79-
double coeff = 20.0,
80-
tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL,
81-
double longest_valid_segment_length = 0.5);
75+
const TrajOptIfoptCollisionConfig::ConstPtr& config);
76+
77+
78+
bool addCollisionConstraint(std::shared_ptr<ifopt::Problem> nlp,
79+
std::vector<trajopt::JointPosition::ConstPtr> vars,
80+
const TrajOptIfoptCollisionConfig::ConstPtr& config);
81+
82+
bool addCollisionSquaredCost(std::shared_ptr<ifopt::Problem> nlp,
83+
std::vector<trajopt::JointPosition::ConstPtr> vars,
84+
const TrajOptIfoptCollisionConfig::ConstPtr& config);
8285

8386
ifopt::ConstraintSet::Ptr createJointVelocityConstraint(const Eigen::Ref<const Eigen::VectorXd>& target,
8487
const std::vector<trajopt::JointPosition::ConstPtr>& vars,

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -48,15 +48,15 @@ void TrajOptIfoptDefaultCompositeProfile::apply(TrajOptIfoptProblem& problem,
4848
int end_index,
4949
const ManipulatorInfo& /*manip_info*/,
5050
const std::vector<std::string>& /*active_links*/,
51-
const std::vector<int>& fixed_indices)
51+
const std::vector<int>& /*fixed_indices*/)
5252
{
5353
const std::vector<trajopt::JointPosition::ConstPtr> vars(&problem.vars[start_index], &problem.vars[end_index]);
5454

55-
// if (collision_constraint_config.enabled)
56-
// addCollisionConstraint(problem, start_index, end_index, fixed_indices);
55+
if (collision_constraint_config->enabled)
56+
addCollisionConstraint(problem.nlp, vars, collision_constraint_config);
5757

58-
// if (collision_cost_config.enabled)
59-
// addCollisionCost(problem, start_index, end_index, fixed_indices);
58+
if (collision_cost_config->enabled)
59+
addCollisionSquaredCost(problem.nlp, vars, collision_cost_config);
6060

6161
if (smooth_velocities)
6262
addJointVelocitySquaredCost(problem.nlp, vars, velocity_coeff);

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_collision_config.cpp

-206
Original file line numberDiff line numberDiff line change
@@ -35,211 +35,5 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3535

3636
namespace tesseract_planning
3737
{
38-
// CollisionCostConfig::CollisionCostConfig(const tinyxml2::XMLElement& xml_element)
39-
//{
40-
// const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled");
41-
// const tinyxml2::XMLElement* use_weighted_sum_element = xml_element.FirstChildElement("UseWeightedSum");
42-
// const tinyxml2::XMLElement* type_element = xml_element.FirstChildElement("CollisionEvaluator");
43-
// const tinyxml2::XMLElement* buffer_margin_element = xml_element.FirstChildElement("BufferMargin");
44-
// const tinyxml2::XMLElement* safety_margin_buffer_element = xml_element.FirstChildElement("SafetyMarginBuffer");
45-
// const tinyxml2::XMLElement* coeff_element = xml_element.FirstChildElement("Coefficient");
4638

47-
// if (!enabled_element)
48-
// throw std::runtime_error("CollisionCostConfig: Must have Enabled element.");
49-
50-
// tinyxml2::XMLError status = enabled_element->QueryBoolText(&enabled);
51-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
52-
// throw std::runtime_error("CollisionCostConfig: Error parsing Enabled string");
53-
54-
// if (use_weighted_sum_element)
55-
// {
56-
// status = use_weighted_sum_element->QueryBoolText(&use_weighted_sum);
57-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
58-
// throw std::runtime_error("CollisionCostConfig: Error parsing UseWeightedSum string");
59-
// }
60-
61-
// if (type_element)
62-
// {
63-
// int coll_type = static_cast<int>(trajopt::CollisionEvaluatorType::CAST_CONTINUOUS);
64-
// status = type_element->QueryIntAttribute("type", &coll_type);
65-
// if (status != tinyxml2::XML_SUCCESS)
66-
// throw std::runtime_error("CollisionCostConfig: Error parsing CollisionEvaluator type attribute.");
67-
68-
// type = static_cast<trajopt::CollisionEvaluatorType>(coll_type);
69-
// }
70-
71-
// if (buffer_margin_element)
72-
// {
73-
// std::string buffer_margin_string;
74-
// status = tesseract_common::QueryStringText(buffer_margin_element, buffer_margin_string);
75-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
76-
// throw std::runtime_error("CollisionCostConfig: Error parsing BufferMargin string");
77-
78-
// if (!tesseract_common::isNumeric(buffer_margin_string))
79-
// throw std::runtime_error("CollisionCostConfig: BufferMargin is not a numeric values.");
80-
81-
// tesseract_common::toNumeric<double>(buffer_margin_string, buffer_margin);
82-
// }
83-
84-
// if (safety_margin_buffer_element)
85-
// {
86-
// std::string safety_margin_buffer_string;
87-
// status = tesseract_common::QueryStringText(safety_margin_buffer_element, safety_margin_buffer_string);
88-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
89-
// throw std::runtime_error("CollisionCostConfig: Error parsing SafetyMarginBuffer string");
90-
91-
// if (!tesseract_common::isNumeric(safety_margin_buffer_string))
92-
// throw std::runtime_error("CollisionCostConfig: SafetyMarginBuffer is not a numeric values.");
93-
94-
// tesseract_common::toNumeric<double>(safety_margin_buffer_string, safety_margin_buffer);
95-
// }
96-
97-
// if (coeff_element)
98-
// {
99-
// std::string coeff_string;
100-
// status = tesseract_common::QueryStringText(coeff_element, coeff_string);
101-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
102-
// throw std::runtime_error("CollisionCostConfig: Error parsing Coefficient string");
103-
104-
// if (!tesseract_common::isNumeric(coeff_string))
105-
// throw std::runtime_error("CollisionCostConfig: Coefficient is not a numeric values.");
106-
107-
// tesseract_common::toNumeric<double>(coeff_string, coeff);
108-
// }
109-
//}
110-
111-
// tinyxml2::XMLElement* CollisionCostConfig::toXML(tinyxml2::XMLDocument& doc) const
112-
//{
113-
// tinyxml2::XMLElement* xml_coll_cost_config = doc.NewElement("CollisionCostConfig");
114-
115-
// tinyxml2::XMLElement* xml_enabled = doc.NewElement("Enabled");
116-
// xml_enabled->SetText(enabled);
117-
// xml_coll_cost_config->InsertEndChild(xml_enabled);
118-
119-
// tinyxml2::XMLElement* xml_use_weighted_sum = doc.NewElement("UseWeightedSum");
120-
// xml_use_weighted_sum->SetText(use_weighted_sum);
121-
// xml_coll_cost_config->InsertEndChild(xml_use_weighted_sum);
122-
123-
// tinyxml2::XMLElement* xml_type = doc.NewElement("CollisionEvaluator");
124-
// xml_type->SetAttribute("type", std::to_string(static_cast<int>(type)).c_str());
125-
// xml_coll_cost_config->InsertEndChild(xml_type);
126-
127-
// tinyxml2::XMLElement* xml_buffer_margin = doc.NewElement("BufferMargin");
128-
// xml_buffer_margin->SetText(buffer_margin);
129-
// xml_coll_cost_config->InsertEndChild(xml_buffer_margin);
130-
131-
// tinyxml2::XMLElement* xml_safety_margin_buffer = doc.NewElement("SafetyMarginBuffer");
132-
// xml_safety_margin_buffer->SetText(safety_margin_buffer);
133-
// xml_coll_cost_config->InsertEndChild(xml_safety_margin_buffer);
134-
135-
// tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficient");
136-
// xml_coeff->SetText(coeff);
137-
// xml_coll_cost_config->InsertEndChild(xml_coeff);
138-
139-
// return xml_coll_cost_config;
140-
//}
141-
142-
// CollisionConstraintConfig::CollisionConstraintConfig(const tinyxml2::XMLElement& xml_element)
143-
//{
144-
// const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled");
145-
// const tinyxml2::XMLElement* use_weighted_sum_element = xml_element.FirstChildElement("UseWeightedSum");
146-
// const tinyxml2::XMLElement* type_element = xml_element.FirstChildElement("CollisionEvaluator");
147-
// const tinyxml2::XMLElement* safety_margin_element = xml_element.FirstChildElement("SafetyMargin");
148-
// const tinyxml2::XMLElement* safety_margin_buffer_element = xml_element.FirstChildElement("SafetyMarginBuffer");
149-
// const tinyxml2::XMLElement* coeff_element = xml_element.FirstChildElement("Coefficient");
150-
151-
// if (!enabled_element)
152-
// throw std::runtime_error("CollisionConstraintConfig: Must have Enabled element.");
153-
154-
// tinyxml2::XMLError status = enabled_element->QueryBoolText(&enabled);
155-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
156-
// throw std::runtime_error("CollisionConstraintConfig: Error parsing Enabled string");
157-
158-
// if (use_weighted_sum_element)
159-
// {
160-
// status = use_weighted_sum_element->QueryBoolText(&use_weighted_sum);
161-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
162-
// throw std::runtime_error("CollisionConstraintConfig: Error parsing UseWeightedSum string");
163-
// }
164-
165-
// if (type_element)
166-
// {
167-
// int coll_type = static_cast<int>(trajopt::CollisionEvaluatorType::CAST_CONTINUOUS);
168-
// status = type_element->QueryIntAttribute("type", &coll_type);
169-
// if (status != tinyxml2::XML_SUCCESS)
170-
// throw std::runtime_error("CollisionConstraintConfig: Error parsing CollisionEvaluator type attribute.");
171-
172-
// type = static_cast<trajopt::CollisionEvaluatorType>(coll_type);
173-
// }
174-
175-
// if (safety_margin_element)
176-
// {
177-
// std::string safety_margin_string;
178-
// status = tesseract_common::QueryStringText(safety_margin_element, safety_margin_string);
179-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
180-
// throw std::runtime_error("CollisionConstraintConfig: Error parsing SafetyMargin string");
181-
182-
// if (!tesseract_common::isNumeric(safety_margin_string))
183-
// throw std::runtime_error("CollisionConstraintConfig: SafetyMargin is not a numeric values.");
184-
185-
// tesseract_common::toNumeric<double>(safety_margin_string, safety_margin);
186-
// }
187-
188-
// if (safety_margin_buffer_element)
189-
// {
190-
// std::string safety_margin_buffer_string;
191-
// status = tesseract_common::QueryStringText(safety_margin_buffer_element, safety_margin_buffer_string);
192-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
193-
// throw std::runtime_error("CollisionConstraintConfig: Error parsing SafetyMarginBuffer string");
194-
195-
// if (!tesseract_common::isNumeric(safety_margin_buffer_string))
196-
// throw std::runtime_error("CollisionConstraintConfig: SafetyMarginBuffer is not a numeric values.");
197-
198-
// tesseract_common::toNumeric<double>(safety_margin_buffer_string, safety_margin_buffer);
199-
// }
200-
201-
// if (coeff_element)
202-
// {
203-
// std::string coeff_string;
204-
// status = tesseract_common::QueryStringText(coeff_element, coeff_string);
205-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
206-
// throw std::runtime_error("CollisionConstraintConfig: Error parsing Coefficient string");
207-
208-
// if (!tesseract_common::isNumeric(coeff_string))
209-
// throw std::runtime_error("CollisionConstraintConfig: Coefficient is not a numeric values.");
210-
211-
// tesseract_common::toNumeric<double>(coeff_string, coeff);
212-
// }
213-
//}
214-
215-
// tinyxml2::XMLElement* CollisionConstraintConfig::toXML(tinyxml2::XMLDocument& doc) const
216-
//{
217-
// tinyxml2::XMLElement* xml_coll_cnt_config = doc.NewElement("CollisionConstraintConfig");
218-
219-
// tinyxml2::XMLElement* xml_enabled = doc.NewElement("Enabled");
220-
// xml_enabled->SetText(enabled);
221-
// xml_coll_cnt_config->InsertEndChild(xml_enabled);
222-
223-
// tinyxml2::XMLElement* xml_use_weighted_sum = doc.NewElement("UseWeightedSum");
224-
// xml_use_weighted_sum->SetText(use_weighted_sum);
225-
// xml_coll_cnt_config->InsertEndChild(xml_use_weighted_sum);
226-
227-
// tinyxml2::XMLElement* xml_type = doc.NewElement("CollisionEvaluator");
228-
// xml_type->SetAttribute("type", std::to_string(static_cast<int>(type)).c_str());
229-
// xml_coll_cnt_config->InsertEndChild(xml_type);
230-
231-
// tinyxml2::XMLElement* xml_safety_margin = doc.NewElement("SafetyMargin");
232-
// xml_safety_margin->SetText(safety_margin);
233-
// xml_coll_cnt_config->InsertEndChild(xml_safety_margin);
234-
235-
// tinyxml2::XMLElement* xml_safety_margin_buffer = doc.NewElement("SafetyMarginBuffer");
236-
// xml_safety_margin_buffer->SetText(safety_margin_buffer);
237-
// xml_coll_cnt_config->InsertEndChild(xml_safety_margin_buffer);
238-
239-
// tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficient");
240-
// xml_coeff->SetText(coeff);
241-
// xml_coll_cnt_config->InsertEndChild(xml_coeff);
242-
243-
// return xml_coll_cnt_config;
244-
//}
24539
} // namespace tesseract_planning

0 commit comments

Comments
 (0)