Skip to content

Commit 2b09f7e

Browse files
mpowelsonmpowelson
committed
Fix joint velocity smoothing
1 parent c7175c5 commit 2b09f7e

File tree

17 files changed

+285
-516
lines changed

17 files changed

+285
-516
lines changed

‎tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -257,9 +257,11 @@ class CartesianWaypoint
257257

258258
/** @brief The Cartesian Waypoint */
259259
Eigen::Isometry3d waypoint;
260-
/** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
260+
/** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
261+
* elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
261262
Eigen::VectorXd lower_tolerance;
262-
/** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
263+
/** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
264+
* elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
263265
Eigen::VectorXd upper_tolerance;
264266

265267
bool isToleranced() const

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -37,12 +37,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3737
namespace tesseract_planning
3838
{
3939
trajopt::TermInfo::Ptr createCartesianPositionConstraint(const Eigen::Isometry3d& c_wp,
40-
int index,
41-
std::string working_frame,
42-
Eigen::Isometry3d tcp,
43-
const Eigen::VectorXd& coeffs,
44-
std::string link,
45-
trajopt::TermType type);
40+
int index,
41+
std::string working_frame,
42+
Eigen::Isometry3d tcp,
43+
const Eigen::VectorXd& coeffs,
44+
std::string link,
45+
trajopt::TermType type);
4646

4747
trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(const Eigen::Isometry3d& c_wp,
4848
int index,
@@ -66,10 +66,9 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
6666
trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
6767
const Eigen::VectorXd& lower_tol,
6868
const Eigen::VectorXd& upper_tol,
69-
int index,
70-
const Eigen::VectorXd& coeffs,
71-
trajopt::TermType type);
72-
69+
int index,
70+
const Eigen::VectorXd& coeffs,
71+
trajopt::TermType type);
7372

7473
trajopt::TermInfo::Ptr createCollisionTermInfo(
7574
int start_index,

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

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -36,10 +36,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3636

3737
namespace tesseract_planning
3838
{
39-
std::shared_ptr<TrajOptIfoptProblem> DefaultTrajoptProblemGenerator(const std::string& name,
40-
const PlannerRequest& request,
41-
const TrajOptIfoptPlanProfileMap& plan_profiles,
42-
const TrajOptIfoptCompositeProfileMap& composite_profiles);
39+
std::shared_ptr<TrajOptIfoptProblem>
40+
DefaultTrajoptProblemGenerator(const std::string& name,
41+
const PlannerRequest& request,
42+
const TrajOptIfoptPlanProfileMap& plan_profiles,
43+
const TrajOptIfoptCompositeProfileMap& composite_profiles);
4344

4445
} // namespace tesseract_planning
4546
#endif

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

Lines changed: 0 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,6 @@ 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;
5048
/** @brief Configuration info for collisions that are modeled as costs */
5149
CollisionCostConfig collision_cost_config;
5250
/** @brief Configuration info for collisions that are modeled as constraints */
@@ -63,10 +61,6 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
6361
bool smooth_jerks = true;
6462
/** @brief This default to all ones, but allows you to weight different joints */
6563
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;
7064

7165
/** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
7266
* to be considered valid in post checking of trajectory returned by TrajOptIfopt.
@@ -98,48 +92,6 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
9892
const std::vector<int>& fixed_indices) override;
9993

10094
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);
14395
};
14496
} // namespace tesseract_planning
14597

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

Lines changed: 4 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -38,12 +38,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3838

3939
namespace tesseract_planning
4040
{
41-
enum class TrajOptIfoptTermType
42-
{
43-
CONSTRAINT,
44-
SQUARED_COST
45-
};
46-
4741
class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
4842
{
4943
public:
@@ -57,7 +51,6 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
5751
Eigen::VectorXd joint_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
5852
TrajOptIfoptTermType term_type{ TrajOptIfoptTermType::CONSTRAINT };
5953

60-
6154
void apply(TrajOptIfoptProblem& problem,
6255
const CartesianWaypoint& cartesian_waypoint,
6356
const Instruction& parent_instruction,
@@ -74,10 +67,11 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
7467

7568
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
7669

77-
//protected:
78-
// void addConstraintErrorFunctions(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;
70+
// protected:
71+
// void addConstraintErrorFunctions(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps)
72+
// const;
7973

80-
// void addAvoidSingularity(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;s
74+
// void addAvoidSingularity(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;s
8175
};
8276
} // namespace tesseract_planning
8377

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

Lines changed: 34 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -34,16 +34,20 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3434
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3535

3636
#include <tesseract_common/utils.h>
37+
#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h>
3738

3839
namespace tesseract_planning
3940
{
4041
/**
41-
* @brief Config settings for collision cost terms.
42+
* @brief Config settings for collision terms.
4243
*/
43-
struct CollisionCostConfig
44+
struct CollisionConfig
4445
{
45-
CollisionCostConfig() = default;
46-
CollisionCostConfig(const tinyxml2::XMLElement& xml_element);
46+
using Ptr = std::shared_ptr<CollisionConfig>;
47+
using ConstPtr = std::shared_ptr<const CollisionConfig>;
48+
49+
CollisionConfig() = default;
50+
CollisionConfig(const tinyxml2::XMLElement& xml_element);
4751

4852
/** @brief If true, a collision cost term will be added to the problem. Default: true*/
4953
bool enabled = true;
@@ -52,6 +56,8 @@ struct CollisionCostConfig
5256
* If set to true, it is recommended to start with the coeff set to one
5357
*/
5458
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;
5561
/** @brief The evaluator type that will be used for collision checking. */
5662
trajopt::CollisionEvaluatorType type = trajopt::CollisionEvaluatorType::CAST_CONTINUOUS;
5763

@@ -66,31 +72,38 @@ struct CollisionCostConfig
6672
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
6773
};
6874

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);
85+
86+
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
87+
88+
protected:
89+
TrajOptIfoptTermType term_type_ = TrajOptIfoptTermType::SQUARED_COST;
90+
};
91+
6992
/**
7093
* @brief Config settings for collision constraint terms.
7194
*/
72-
struct CollisionConstraintConfig
95+
struct CollisionConstraintConfig : public CollisionConfig
7396
{
97+
using Ptr = std::shared_ptr<CollisionConstraintConfig>;
98+
using ConstPtr = std::shared_ptr<const CollisionConstraintConfig>;
99+
74100
CollisionConstraintConfig() = default;
75101
CollisionConstraintConfig(const tinyxml2::XMLElement& xml_element);
76102

77-
/** @brief If true, a collision cost term will be added to the problem. Default: true*/
78-
bool enabled = true;
79-
/**
80-
* @brief Use the weighted sum for each link pair. This reduces the number equations added to the problem
81-
* If set to true, it is recommended to start with the coeff set to one.
82-
*/
83-
bool use_weighted_sum = false;
84-
/** @brief The evaluator type that will be used for collision checking. */
85-
trajopt::CollisionEvaluatorType type = trajopt::CollisionEvaluatorType::CAST_CONTINUOUS;
86-
/** @brief Max distance in which collision constraints will be evaluated. */
87-
double safety_margin = 0.01;
88-
/** @brief Distance beyond safety_margin in which collision optimization will be evaluated. */
89-
double safety_margin_buffer = 0.05;
90-
/** @brief The collision coeff/weight */
91-
double coeff = 20;
92-
93103
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
104+
105+
protected:
106+
TrajOptIfoptTermType term_type_ = TrajOptIfoptTermType::CONSTRAINT;
94107
};
95108
} // namespace tesseract_planning
96109

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

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -38,10 +38,11 @@ namespace tesseract_planning
3838
{
3939
class TrajOptIfoptMotionPlannerStatusCategory;
4040

41-
using TrajOptIfoptProblemGeneratorFn = std::function<std::shared_ptr<ifopt::Problem>(const std::string&,
42-
const PlannerRequest&,
43-
const TrajOptIfoptPlanProfileMap&,
44-
const TrajOptIfoptCompositeProfileMap&)>;
41+
using TrajOptIfoptProblemGeneratorFn =
42+
std::function<std::shared_ptr<ifopt::Problem>(const std::string&,
43+
const PlannerRequest&,
44+
const TrajOptIfoptPlanProfileMap&,
45+
const TrajOptIfoptCompositeProfileMap&)>;
4546

4647
class TrajOptIfoptMotionPlanner : public MotionPlanner
4748
{
@@ -75,10 +76,10 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner
7576
TrajOptIfoptPlanProfileMap plan_profiles;
7677

7778
/** @brief Optimization parameters to be used (Optional) */
78-
// sco::BasicTrustRegionSQPParameters params;
79+
// sco::BasicTrustRegionSQPParameters params;
7980

8081
/** @brief Callback functions called on each iteration of the optimization (Optional) */
81-
// std::vector<sco::Optimizer::Callback> callbacks;
82+
// std::vector<sco::Optimizer::Callback> callbacks;
8283

8384
/**
8485
* @brief Sets up the opimizer and solves a SQP problem read from json with no callbacks and dafault parameterss
@@ -99,7 +100,8 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner
99100
void clear() override;
100101

101102
protected:
102-
std::shared_ptr<const TrajOptIfoptMotionPlannerStatusCategory> status_category_; /** @brief The planners status codes */
103+
std::shared_ptr<const TrajOptIfoptMotionPlannerStatusCategory> status_category_; /** @brief The planners status codes
104+
*/
103105
};
104106

105107
class TrajOptIfoptMotionPlannerStatusCategory : public tesseract_common::StatusCategory

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

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3939

4040
namespace tesseract_planning
4141
{
42+
enum class TrajOptIfoptTermType
43+
{
44+
CONSTRAINT,
45+
SQUARED_COST
46+
};
47+
4248
struct TrajOptIfoptProblem
4349
{
4450
// These are required for Tesseract to configure Descartes

0 commit comments

Comments
 (0)