Skip to content
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

[ros2] Support for dynamic joint limits in MotionRequests + other features #144

Open
wants to merge 6 commits into
base: ros2
Choose a base branch
from
Open

[ros2] Support for dynamic joint limits in MotionRequests + other features #144

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
08f72ef
add ChangeJointLimits service
Apr 25, 2022
670d4a2
Add JointTrajectory field to hybrid planner actions
Apr 14, 2022
374cd3b
update motionrequest message
May 11, 2022
7fe1740
add skip_smoothing parameter
May 15, 2022
419f787
address some feedback
Dec 27, 2022
81735e6
remove comment
Jan 3, 2023
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
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ set(srv_files
"srv/RenameRobotStateInWarehouse.srv"
"srv/DeleteRobotStateFromWarehouse.srv"
"srv/ServoCommandType.srv"
"srv/ChangeJointLimits.srv"
)

set(action_files
Expand Down
1 change: 1 addition & 0 deletions action/GlobalPlanner.action
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
# Motion planning request to the global planner
string planning_group
trajectory_msgs/JointTrajectory trajectory
Copy link
Member

Choose a reason for hiding this comment

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

I don't really follow why this should be needed, but why not add a reference trajectory here? https://github.com/ros-planning/moveit_msgs/blob/master/msg/MotionPlanRequest.msg#L25

Copy link
Member

Choose a reason for hiding this comment

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

So, the moveit_msgs/MotionSequenceRequest contains a JointTrajectory if you dig deep enough.

MotionSequenceRequest-> MotionSequenceItem[] -> MotionPlanRequest-> GenericTrajectory[]-> JointTrajectory[]

I guess this new field is probably not needed :(

What do you think @henningkayser? Is it too hard to dig all the way down to JointTrajectory in the current message type?

Copy link
Author

Choose a reason for hiding this comment

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

I'm cool with either approach. I guess using what currently exists (even if you do need to dig down somewhat deep) makes the most sense so I can make that change

Copy link
Author

@mechwiz mechwiz Jan 4, 2023 •
edited
Loading

Choose a reason for hiding this comment

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

I'm reconsidering this. I think having a dedicated "trajectory" field is more intuitive to the user to assign than having to dig deep in the motionplanrequest. This is for three reasons:

  1. The way the HP will check to see if it should process the already planned trajectory (as opposed to planning one) is by specifically looking in this "trajectory" field to see if has a multi-point trajectory. This makes this field pretty important to this action. Having it buried down within the MotionSequenceRequest could therefore be less intuitive to the user.
  2. Having this trajectory field outside the MotionSequenceRequest highlights that this action isn't so much a request to plan as it is to just bypass planning and execute via the local planner.
  3. The action already conains a field for the planning_group so wouldn't be a big stretch to add the accompanying trajectory on the same level if applicable.

All this said, if you think using the reference-trajectory field is best, I can run with that and make the necessary changes.

moveit_msgs/MotionSequenceRequest motion_sequence
---
# Motion planning response from the global planner
Expand Down
1 change: 1 addition & 0 deletions action/HybridPlanner.action
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
# Motion planning request to the hybrid planning architecture
string planning_group
trajectory_msgs/JointTrajectory trajectory
moveit_msgs/MotionSequenceRequest motion_sequence
Comment on lines +3 to 4
Copy link
Member

Choose a reason for hiding this comment

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

Same comment about MotionSequenceRequest containing a JointTrajectory already, if you dig deep enough.

---
# Result of the hybrid planning
Expand Down
10 changes: 10 additions & 0 deletions msg/MotionPlanRequest.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,16 @@ float64 allowed_planning_time
float64 max_velocity_scaling_factor
float64 max_acceleration_scaling_factor

# New joint limits to use when running time-parameterization and smoothing.
# Note, this does not replace the values in the robot model.
# Set the 'has_limits' field to true for velocity, acceleration, and/or jerk
# in order for that limit to be applied. Position limits are not supported.
JointLimits[] joint_limits

# If true, smoothing will be skipped when applying a smoothing adapter during
# trajectory processing (if it is defined as a request adapter).
bool skip_smoothing
Copy link
Member

Choose a reason for hiding this comment

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

This makes me thing that a separate post processing config would make sense by now, there are already so many related fields.

Copy link
Member

Choose a reason for hiding this comment

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

Hmm, say TOTG and Ruckig are configured to do smoothing by default in ompl_planning.yaml. I'm trying to think of how the motion planning pipeline would know to skip both of those smoothers but not the other OMPL planning request adapters.

Maybe it should be a vector of strings containing the plugins to skip, like this:

string[] ompl_adapters_to_skip

So to skip those two plugins, you would have:

["default_planner_request_adapters/AddTimeOptimalParameterization"
"default_planner_request_adapters/AddRuckigTrajectorySmoothing"]

Copy link
Author

@mechwiz mechwiz Sep 22, 2022 •
edited
Loading

Choose a reason for hiding this comment

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

I'm not sure we'd want to skip a time-parameterization adapter like TOTG. I think this is only relevant for smoothing (which occurs after the time-parameterization step) in which case I think ruckig is the only one currently. In any case though, if multiple smoothing adapters do exist down the road, then it's not too hard to use the "skip_smoothing" field when the adapter is called like I have done on my fork here for Ruckig https://github.com/mechwiz/moveit2/blob/mwiz/hp_fixes/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp#L68

Copy link
Author

Choose a reason for hiding this comment

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

bumping this again.
I feel like defining a vector of strings for this is maybe a little overkill. I can think of usecases where even though a smoothing adapter (like Ruckig) is applied after TOTG in the list of ompl adapters, there may be exceptions/times where we don't want to smooth the resulting trajectory (whether to save on time or because its a very simple motion like closing/opening a gripper, etc..). Are there other usecases where we'd want to skip an adapter that is not smoothing?


# Maximum cartesian speed for the given link.
# If max_cartesian_speed <= 0 the trajectory is not modified.
# These fields require the following planning request adapter: default_planner_request_adapters/LimitMaxCartesianLinkSpeed
Expand Down
4 changes: 4 additions & 0 deletions srv/ChangeJointLimits.srv
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
# A list of joint limits to modify
JointLimits[] limits
---
bool success