Skip to content

Make TimeParamerization configurable #339

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
rhaschke merged 2 commits into from
May 8, 2022
Merged

Make TimeParamerization configurable #339

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c3b31b4
Make TimeParamerization configurable
rhaschke Mar 3, 2022
3a3da5b
rename connect parameterization property
v4hn Mar 4, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion core/include/moveit/task_constructor/merge.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_parameterization.h>

namespace moveit {
namespace task_constructor {
Expand All @@ -57,6 +58,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
*/
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group);
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
const trajectory_processing::TimeParameterization& time_parameterization);
} // namespace task_constructor
} // namespace moveit
8 changes: 8 additions & 0 deletions core/include/moveit/task_constructor/moveit_compat.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#pragma once

#include <moveit/version.h>
#include <moveit/macros/class_forward.h>

#define MOVEIT_VERSION_GE(major, minor, patch) \
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
Expand All @@ -57,3 +58,10 @@
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)

#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)

#if !MOVEIT_VERSION_GE(1, 1, 9)
// the pointers are not yet available
namespace trajectory_processing {
MOVEIT_CLASS_FORWARD(TimeParameterization);
}
#endif
Comment on lines +61 to +67
Copy link
Contributor

@v4hn v4hn Mar 4, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just saw you backported the polymorphic base class change to melodic. So this should be unnecessary.
But Jafar and you both did not notice the missing pointers in the original patch, so I just filed this PR. It might be worth creating a patch release for melodic with this addition as well. If you should do that, these lines can go away

Suggested change
#if !MOVEIT_VERSION_GE(1, 1, 9)
// the pointers are not yet available
namespace trajectory_processing {
MOVEIT_CLASS_FORWARD(TimeParameterization);
}
#endif

Copy link
Contributor Author

@rhaschke rhaschke Mar 4, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Until we have a new release of MoveIt, this addition is required. Do you propose a patch release of MoveIt before?
I could do that over the weekend indeed...

Copy link
Contributor

@v4hn v4hn Mar 8, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess you can remove this now. :)

17 changes: 12 additions & 5 deletions core/src/container.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,9 @@
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>

#include <ros/console.h>

Expand All @@ -49,6 +51,7 @@
#include <functional>

using namespace std::placeholders;
using namespace trajectory_processing;

namespace moveit {
namespace task_constructor {
Expand Down Expand Up @@ -635,7 +638,7 @@ void SerialContainerPrivate::validateConnectivity() const {
ContainerBasePrivate::validateConnectivity();

InterfaceFlags mine = interfaceFlags();
// check that input / output interface of first / last child matches this' resp. interface
// check that input/output interface of first/last child matches this' resp. interface
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);

Expand All @@ -647,7 +650,7 @@ void SerialContainerPrivate::validateConnectivity() const {
const StagePrivate* const cur_impl = **cur;
InterfaceFlags required = cur_impl->interfaceFlags();

// get iterators to prev / next stage in sequence
// get iterators to prev/next stage in sequence
auto prev = cur;
--prev;
auto next = cur;
Expand Down Expand Up @@ -750,7 +753,7 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child,
void ParallelContainerBasePrivate::validateConnectivity() const {
InterfaceFlags my_interface = interfaceFlags();

// check that input / output interfaces of all children are handled by my interface
// check that input/output interfaces of all children are handled by my interface
for (const auto& child : children())
validateInterfaces(*child->pimpl(), my_interface);

Expand Down Expand Up @@ -1084,7 +1087,10 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) {
}
}

Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {
properties().declare<TimeParameterizationPtr>("time_parameterization",
std::make_shared<TimeOptimalTrajectoryGeneration>());
}

void Merger::reset() {
ParallelContainerBase::reset();
Expand Down Expand Up @@ -1237,7 +1243,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
moveit::core::JointModelGroup* jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged;
try {
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
auto timing = me_->properties().get<TimeParameterizationPtr>("time_parameterization");
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg, *timing);
} catch (const std::runtime_error& e) {
SubTrajectory t;
t.markAsFailure();
Expand Down
7 changes: 3 additions & 4 deletions core/src/merge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,6 @@
/* Authors: Luca Lach, Robert Haschke */

#include <moveit/task_constructor/merge.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include <boost/range/adaptor/transformed.hpp>
#include <boost/algorithm/string/join.hpp>
Expand Down Expand Up @@ -106,7 +105,8 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint

robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) {
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
const trajectory_processing::TimeParameterization& time_parameterization) {
if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions");

Expand Down Expand Up @@ -166,8 +166,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
}

// add timing
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*merged_traj, 1.0, 1.0);
time_parameterization.computeTimeStamps(*merged_traj, 1.0, 1.0);
Copy link
Contributor

@v4hn v4hn Mar 4, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This can (and could before) break a lower specified dynamics limit in one of the merged trajectories. The merged trajectory might suddenly be "inexplicably" faster.

But the real solution to this is not adding yet another set of parameters, but implement a "proper" merge, that does not need to compute a whole new timing for the trajectory, but resamples the trajectories as needed.
So I guess it's best to leave this as is until then.

Copy link
Contributor Author

@rhaschke rhaschke Mar 4, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please file an issue as a reminder...

return merged_traj;
}
} // namespace task_constructor
Expand Down
10 changes: 6 additions & 4 deletions core/src/solvers/cartesian_path.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,13 @@
#include <moveit/task_constructor/moveit_compat.h>

#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
#include <moveit/robot_state/cartesian_interpolator.h>
#endif

using namespace trajectory_processing;

namespace moveit {
namespace task_constructor {
namespace solvers {
Expand Down Expand Up @@ -107,9 +109,9 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
for (const auto& waypoint : trajectory)
result->addSuffixWayPoint(waypoint, 0.0);

trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));

return achieved_fraction >= props.get<double>("min_fraction");
}
Expand Down
12 changes: 7 additions & 5 deletions core/src/solvers/joint_interpolation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,15 +37,18 @@
*/

#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/trajectory_processing/time_parameterization.h>

#include <chrono>

namespace moveit {
namespace task_constructor {
namespace solvers {

using namespace trajectory_processing;

JointInterpolationPlanner::JointInterpolationPlanner() {
auto& p = properties();
p.declare<double>("max_step", 0.1, "max joint step");
Expand Down Expand Up @@ -89,10 +92,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
if (from->isStateColliding(to_state, jmg->getName()))
return false;

// add timing, TODO: use a generic method to add timing via plugins
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));

return true;
}
Expand Down
5 changes: 5 additions & 0 deletions core/src/solvers/planner_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,10 @@
*/

#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>

using namespace trajectory_processing;

namespace moveit {
namespace task_constructor {
Expand All @@ -46,6 +50,7 @@ PlannerInterface::PlannerInterface() {
auto& p = properties();
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
}
} // namespace solvers
} // namespace task_constructor
Expand Down
11 changes: 9 additions & 2 deletions core/src/stages/connect.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,9 +38,13 @@

#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/cost_terms.h>

#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>

#include <moveit/task_constructor/cost_terms.h>
using namespace trajectory_processing;

namespace moveit {
namespace task_constructor {
Expand All @@ -54,6 +58,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory");
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
std::make_shared<TimeOptimalTrajectoryGeneration>());
}

void Connect::reset() {
Expand Down Expand Up @@ -219,7 +225,8 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject

auto jmg = merged_jmg_.get();
assert(jmg);
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
auto timing = properties().get<TimeParameterizationPtr>("merge_time_parameterization");
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing);
if (!trajectory)
return SubTrajectoryPtr();

Expand Down