Skip to content

Commit ac64c54

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Update time parameterization interface to leverage profiles to support cartesian time parameterization
1 parent 3f179ed commit ac64c54

File tree

45 files changed

+954
-900
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

45 files changed

+954
-900
lines changed

‎tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h

Lines changed: 0 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -29,14 +29,11 @@
2929
#include <tesseract_common/macros.h>
3030
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3131
#include <Eigen/Geometry>
32-
#include <console_bridge/console.h>
33-
#include <boost/core/demangle.hpp>
3432
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3533

3634
#include <tesseract_command_language/constants.h>
3735
#include <tesseract_kinematics/core/joint_group.h>
3836
#include <tesseract_common/kinematic_limits.h>
39-
#include <tesseract_common/profile_dictionary.h>
4037
#include <tesseract_motion_planners/robot_config.h>
4138
#include <tesseract_motion_planners/core/types.h>
4239

@@ -97,36 +94,6 @@ bool isValidState(const tesseract_kinematics::JointGroup& joint_group,
9794
return !(robot_config != RobotConfig::FUT && robot_config != RobotConfig::NUT); // NOLINT
9895
}
9996

100-
/**
101-
* @brief Gets the profile specified from the profile map
102-
* @param ns The namespace to search for requested profile
103-
* @param profile The requested profile
104-
* @param profile_map map that contains the profiles
105-
* @param default_profile Profile that is returned if the requested profile is not found in the map. Default = nullptr
106-
* @return The profile requested if found. Otherwise the default_profile
107-
*/
108-
template <typename ProfileType>
109-
std::shared_ptr<const ProfileType> getProfile(const std::string& ns,
110-
const std::string& profile,
111-
const tesseract_common::ProfileDictionary& profile_dictionary,
112-
std::shared_ptr<const ProfileType> default_profile = nullptr)
113-
{
114-
if (profile_dictionary.hasProfile(ProfileType::getStaticKey(), ns, profile))
115-
return std::static_pointer_cast<const ProfileType>(
116-
profile_dictionary.getProfile(ProfileType::getStaticKey(), ns, profile));
117-
118-
std::stringstream ss;
119-
ss << "Profile '" << profile << "' was not found in namespace '" << ns << "' for type '"
120-
<< boost::core::demangle(typeid(ProfileType).name()) << "'. Using default if available. Available profiles: [";
121-
if (profile_dictionary.hasProfileEntry(ProfileType::getStaticKey(), ns))
122-
for (const auto& pair : profile_dictionary.getProfileEntry(ProfileType::getStaticKey(), ns))
123-
ss << pair.first << ", ";
124-
ss << "]";
125-
126-
CONSOLE_BRIDGE_logDebug(ss.str().c_str());
127-
128-
return default_profile;
129-
}
13097
} // namespace tesseract_planning
13198

13299
#endif // TESSERACT_MOTION_PLANNERS_PLANNER_UTILS_H

‎tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3333
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3434

3535
#include <tesseract_common/kinematic_limits.h>
36+
#include <tesseract_common/profile_dictionary.h>
3637
#include <tesseract_kinematics/core/kinematic_group.h>
3738
#include <tesseract_environment/environment.h>
3839

@@ -69,11 +70,8 @@ PlannerResponse DescartesMotionPlanner<FloatType>::solve(const PlannerRequest& r
6970
PlannerResponse response;
7071

7172
// Get solver config
72-
auto solver_profile =
73-
getProfile<DescartesSolverProfile<FloatType>>(name_,
74-
request.instructions.getProfile(name_),
75-
*request.profiles,
76-
std::make_shared<DescartesLadderGraphSolverProfile<FloatType>>());
73+
auto solver_profile = request.profiles->getProfile<DescartesSolverProfile<FloatType>>(
74+
name_, request.instructions.getProfile(name_), std::make_shared<DescartesLadderGraphSolverProfile<FloatType>>());
7775

7876
auto solver = solver_profile->create();
7977

@@ -96,11 +94,8 @@ PlannerResponse DescartesMotionPlanner<FloatType>::solve(const PlannerRequest& r
9694
const auto& move_instruction = instruction.get().template as<MoveInstructionPoly>();
9795

9896
// Get Plan Profile
99-
auto cur_move_profile =
100-
getProfile<DescartesMoveProfile<FloatType>>(name_,
101-
move_instruction.getProfile(name_),
102-
*request.profiles,
103-
std::make_shared<DescartesDefaultMoveProfile<FloatType>>());
97+
auto cur_move_profile = request.profiles->getProfile<DescartesMoveProfile<FloatType>>(
98+
name_, move_instruction.getProfile(name_), std::make_shared<DescartesDefaultMoveProfile<FloatType>>());
10499

105100
if (!cur_move_profile)
106101
throw std::runtime_error("DescartesMotionPlanner: Invalid profile");

‎tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4444
#include <tesseract_motion_planners/ompl/profile/ompl_real_vector_move_profile.h>
4545
#include <tesseract_motion_planners/core/types.h>
4646

47+
#include <tesseract_common/profile_dictionary.h>
4748
#include <tesseract_kinematics/core/joint_group.h>
4849
#include <tesseract_kinematics/core/kinematic_group.h>
4950
#include <tesseract_collision/core/discrete_contact_manager.h>
@@ -299,10 +300,8 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
299300
continue;
300301

301302
// Get Plan Profile
302-
auto cur_move_profile = getProfile<OMPLMoveProfile>(name_,
303-
end_move_instruction.getProfile(name_),
304-
*request.profiles,
305-
std::make_shared<OMPLRealVectorMoveProfile>());
303+
auto cur_move_profile = request.profiles->getProfile<OMPLMoveProfile>(
304+
name_, end_move_instruction.getProfile(name_), std::make_shared<OMPLRealVectorMoveProfile>());
306305

307306
if (!cur_move_profile)
308307
throw std::runtime_error("OMPLMotionPlanner: Invalid profile");

‎tesseract_motion_planners/simple/src/simple_motion_planner.cpp

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3838
#include <tesseract_motion_planners/planner_utils.h>
3939

4040
#include <tesseract_common/joint_state.h>
41+
#include <tesseract_common/profile_dictionary.h>
4142

4243
#include <tesseract_kinematics/core/joint_group.h>
4344

@@ -234,17 +235,13 @@ SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instr
234235
SimplePlannerMoveProfile::ConstPtr move_profile;
235236
if (base_instruction.getPathProfile().empty())
236237
{
237-
move_profile = getProfile<SimplePlannerMoveProfile>(name_,
238-
base_instruction.getProfile(name_),
239-
*request.profiles,
240-
std::make_shared<SimplePlannerLVSNoIKMoveProfile>());
238+
move_profile = request.profiles->getProfile<SimplePlannerMoveProfile>(
239+
name_, base_instruction.getProfile(name_), std::make_shared<SimplePlannerLVSNoIKMoveProfile>());
241240
}
242241
else
243242
{
244-
move_profile = getProfile<SimplePlannerMoveProfile>(name_,
245-
base_instruction.getPathProfile(name_),
246-
*request.profiles,
247-
std::make_shared<SimplePlannerLVSNoIKMoveProfile>());
243+
move_profile = request.profiles->getProfile<SimplePlannerMoveProfile>(
244+
name_, base_instruction.getPathProfile(name_), std::make_shared<SimplePlannerLVSNoIKMoveProfile>());
248245
}
249246

250247
if (!move_profile)

‎tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4848
#include <tesseract_command_language/utils.h>
4949

5050
#include <tesseract_common/joint_state.h>
51+
#include <tesseract_common/profile_dictionary.h>
5152

5253
#include <tesseract_kinematics/core/kinematic_group.h>
5354

@@ -206,8 +207,8 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
206207
pci->basic_info.use_time = false;
207208

208209
// Apply Solver parameters
209-
TrajOptSolverProfile::ConstPtr solver_profile = getProfile<TrajOptSolverProfile>(
210-
name_, request.instructions.getProfile(name_), *request.profiles, std::make_shared<TrajOptOSQPSolverProfile>());
210+
TrajOptSolverProfile::ConstPtr solver_profile = request.profiles->getProfile<TrajOptSolverProfile>(
211+
name_, request.instructions.getProfile(name_), std::make_shared<TrajOptOSQPSolverProfile>());
211212
if (!solver_profile)
212213
throw std::runtime_error("TrajOptMotionPlanner: Invalid profile");
213214

@@ -228,8 +229,8 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
228229
const auto& move_instruction = move_instructions[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>();
229230

230231
// Get Plan Profile
231-
TrajOptMoveProfile::ConstPtr cur_move_profile = getProfile<TrajOptMoveProfile>(
232-
name_, move_instruction.getProfile(name_), *request.profiles, std::make_shared<TrajOptDefaultMoveProfile>());
232+
TrajOptMoveProfile::ConstPtr cur_move_profile = request.profiles->getProfile<TrajOptMoveProfile>(
233+
name_, move_instruction.getProfile(name_), std::make_shared<TrajOptDefaultMoveProfile>());
233234
if (!cur_move_profile)
234235
throw std::runtime_error("TrajOptMotionPlanner: Invalid profile");
235236

@@ -263,11 +264,8 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
263264
pci->init_info.data.row(i) = seed_states[static_cast<std::size_t>(i)];
264265

265266
// Get composite cost and constraints
266-
TrajOptCompositeProfile::ConstPtr composite_profile =
267-
getProfile<TrajOptCompositeProfile>(name_,
268-
request.instructions.getProfile(name_),
269-
*request.profiles,
270-
std::make_shared<TrajOptDefaultCompositeProfile>());
267+
TrajOptCompositeProfile::ConstPtr composite_profile = request.profiles->getProfile<TrajOptCompositeProfile>(
268+
name_, request.instructions.getProfile(name_), std::make_shared<TrajOptDefaultCompositeProfile>());
271269

272270
if (!composite_profile)
273271
throw std::runtime_error("TrajOptMotionPlanner: Invalid composite profile");

‎tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp

Lines changed: 6 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -102,11 +102,8 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request)
102102
const auto& move_instruction = move_instructions[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>();
103103

104104
// Get Plan Profile
105-
TrajOptIfoptMoveProfile::ConstPtr cur_move_profile =
106-
getProfile<TrajOptIfoptMoveProfile>(name_,
107-
move_instruction.getProfile(name_),
108-
*request.profiles,
109-
std::make_shared<TrajOptIfoptDefaultMoveProfile>());
105+
TrajOptIfoptMoveProfile::ConstPtr cur_move_profile = request.profiles->getProfile<TrajOptIfoptMoveProfile>(
106+
name_, move_instruction.getProfile(name_), std::make_shared<TrajOptIfoptDefaultMoveProfile>());
110107
if (!cur_move_profile)
111108
throw std::runtime_error("TrajOptIfoptMotionPlanner: Invalid profile");
112109

@@ -139,10 +136,8 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request)
139136
// Translate TCL for CompositeInstructions
140137
// ----------------
141138
TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile =
142-
getProfile<TrajOptIfoptCompositeProfile>(name_,
143-
request.instructions.getProfile(name_),
144-
*request.profiles,
145-
std::make_shared<TrajOptIfoptDefaultCompositeProfile>());
139+
request.profiles->getProfile<TrajOptIfoptCompositeProfile>(
140+
name_, request.instructions.getProfile(name_), std::make_shared<TrajOptIfoptDefaultCompositeProfile>());
146141

147142
if (!cur_composite_profile)
148143
throw std::runtime_error("TrajOptIfoptMotionPlanner: Invalid profile");
@@ -166,11 +161,8 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request)
166161
nlp->setup();
167162

168163
// Create Solver
169-
TrajOptIfoptSolverProfile::ConstPtr solver_profile =
170-
getProfile<TrajOptIfoptSolverProfile>(name_,
171-
request.instructions.getProfile(name_),
172-
*request.profiles,
173-
std::make_shared<TrajOptIfoptOSQPSolverProfile>());
164+
TrajOptIfoptSolverProfile::ConstPtr solver_profile = request.profiles->getProfile<TrajOptIfoptSolverProfile>(
165+
name_, request.instructions.getProfile(name_), std::make_shared<TrajOptIfoptOSQPSolverProfile>());
174166

175167
if (!solver_profile)
176168
throw std::runtime_error("TrajOptIfoptMotionPlanner: Invalid profile");

‎tesseract_task_composer/planning/CMakeLists.txt

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,12 +34,9 @@ set(LIB_SOURCE_FILES
3434
src/profiles/contact_check_profile.cpp
3535
src/profiles/fix_state_bounds_profile.cpp
3636
src/profiles/fix_state_collision_profile.cpp
37-
src/profiles/iterative_spline_parameterization_profile.cpp
3837
src/profiles/kinematic_limits_check_profile.cpp
3938
src/profiles/min_length_profile.cpp
4039
src/profiles/profile_switch_profile.cpp
41-
src/profiles/ruckig_trajectory_smoothing_profile.cpp
42-
src/profiles/time_optimal_parameterization_profile.cpp
4340
src/profiles/upsample_trajectory_profile.cpp)
4441

4542
set(LIB_SOURCE_LINK_LIBRARIES

‎tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/iterative_spline_parameterization_task.h

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -59,8 +59,7 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT IterativeSplineParameterizat
5959
std::string input_environment_key,
6060
std::string input_profiles_key,
6161
std::string output_program_key,
62-
bool conditional = true,
63-
bool add_points = true);
62+
bool conditional = true);
6463
explicit IterativeSplineParameterizationTask(std::string name,
6564
const YAML::Node& config,
6665
const TaskComposerPluginFactory& plugin_factory);
@@ -74,7 +73,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT IterativeSplineParameterizat
7473
bool operator!=(const IterativeSplineParameterizationTask& rhs) const;
7574

7675
private:
77-
bool add_points_{ true };
7876
IterativeSplineParameterization solver_;
7977

8078
static TaskComposerNodePorts ports();

‎tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h

Lines changed: 0 additions & 68 deletions
This file was deleted.

‎tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3333
#include <console_bridge/console.h>
3434

3535
#include <tesseract_common/serialization.h>
36+
#include <tesseract_common/profile_dictionary.h>
3637

3738
#include <tesseract_collision/core/continuous_contact_manager.h>
3839
#include <tesseract_collision/core/serialization.h>
@@ -139,7 +140,7 @@ TaskComposerNodeInfo ContinuousContactCheckTask::runImpl(TaskComposerContext& co
139140
const auto& ci = input_data_poly.as<CompositeInstruction>();
140141
auto default_profile = std::make_shared<ContactCheckProfile>();
141142
default_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
142-
auto cur_composite_profile = getProfile<ContactCheckProfile>(ns_, ci.getProfile(ns_), *profiles, default_profile);
143+
auto cur_composite_profile = profiles->getProfile<ContactCheckProfile>(ns_, ci.getProfile(ns_), default_profile);
143144

144145
// Get state solver
145146
tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo();

‎tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3333
#include <boost/serialization/shared_ptr.hpp>
3434

3535
#include <tesseract_common/serialization.h>
36+
#include <tesseract_common/profile_dictionary.h>
3637

3738
#include <tesseract_collision/core/discrete_contact_manager.h>
3839
#include <tesseract_collision/core/serialization.h>
@@ -135,7 +136,7 @@ TaskComposerNodeInfo DiscreteContactCheckTask::runImpl(TaskComposerContext& cont
135136
getData(*context.data_storage, INPUT_PROFILES_PORT).as<std::shared_ptr<tesseract_common::ProfileDictionary>>();
136137
const auto& ci = input_data_poly.as<CompositeInstruction>();
137138
auto cur_composite_profile =
138-
getProfile<ContactCheckProfile>(ns_, ci.getProfile(ns_), *profiles, std::make_shared<ContactCheckProfile>());
139+
profiles->getProfile<ContactCheckProfile>(ns_, ci.getProfile(ns_), std::make_shared<ContactCheckProfile>());
139140

140141
// Get state solver
141142
tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo();

‎tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3030
#include <boost/serialization/string.hpp>
3131

3232
#include <tesseract_common/serialization.h>
33+
#include <tesseract_common/profile_dictionary.h>
3334

3435
#include <tesseract_environment/environment.h>
3536
TESSERACT_COMMON_IGNORE_WARNINGS_POP
@@ -132,7 +133,7 @@ TaskComposerNodeInfo FixStateBoundsTask::runImpl(TaskComposerContext& context,
132133

133134
// Get Composite Profile
134135
auto cur_composite_profile =
135-
getProfile<FixStateBoundsProfile>(ns_, ci.getProfile(ns_), *profiles, std::make_shared<FixStateBoundsProfile>());
136+
profiles->getProfile<FixStateBoundsProfile>(ns_, ci.getProfile(ns_), std::make_shared<FixStateBoundsProfile>());
136137

137138
limits.joint_limits.col(0) = limits.joint_limits.col(0).array() + cur_composite_profile->lower_bounds_reduction;
138139
limits.joint_limits.col(1) = limits.joint_limits.col(1).array() - cur_composite_profile->upper_bounds_reduction;

0 commit comments

Comments
 (0)