Optional forced use of JointModelStateSpaceFactory#541
Conversation
|
This is another patch I supervised. The state space for OMPL is created for each request through a number of rather over-engineered factory abstractions. As a result MoveIt can on-the-fly choose to use either the (traditional) joint space or a cartesian state space that invokes IK for each state (the However, it might happen that paths planned through the latter space flip the chosen IK solutions during the trajectory, rendering the trajectory invalid. This ROS-answers question illustrates the phenomena quite well. A fix for the particular problem would have to restrict the distance of IK solutions for consecutive points. On the other hand, I also don't see a reason to require the use of the pose state space for orientation constraint problems. Especially with weak constraints (e.g. restricting yaw to within pi/4), the joint state space works well enough. So I proposed to add a parameter here, that can enforce the use of the See here for an example configuration we use at the moment. While this can fix some issues with path-constraint planning, it will also increase planning time for more-strict constraints because of the rejection sampling. Please review @davetcoleman , this is the last upstream change we need to run off-the-shelf MoveIt with one of our demos. |
davetcoleman
left a comment
There was a problem hiding this comment.
This works for me. I think @mamoll should be given a heads up
| // the set of planning parameters that can be specific for the group (inherited by configurations of that group) | ||
| static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator", "longest_valid_segment_fraction" }; | ||
| static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator", "longest_valid_segment_fraction", | ||
| "force_joint_model_state_space" }; |
There was a problem hiding this comment.
i don't like the term "force" because i keep thinking you are referring to power/torque. how about "mandate_joint_model_state_space"?
There was a problem hiding this comment.
Could you agree to "enforce" instead? I see your point, but I don't really like mandate...
|
|
||
| ModelBasedPlanningContextPtr context = | ||
| getPlanningContext(pc->second, boost::bind(&PlanningContextManager::getStateSpaceFactory2, this, _1, req), req); | ||
| // Check for forced use of JointModelStateSpace |
There was a problem hiding this comment.
please expand this comment (just copy some of the PR description) because this is somewhat of a hack
Finally someone understands! I made a state space for humanoids once... OMPL interface was so complicated to wade through especially since there is a lot of dead code :) |
Once this is merged, I'll put it on my todo list :) |
|
@zkingston, can you have a look at this? |
Expands workaround comment.
|
@mamoll I'd like to merge unless you or @zkingston have something to add? |
|
Seems fine to me, although I am probably not the best person to review this. |
* Implements optional ompl_planning config parameter 'force_joint_model_state_space'. * Renames parameter to 'enforce_joint_model_state_space'. Expands workaround comment.
* Implements optional ompl_planning config parameter 'force_joint_model_state_space'. * Renames parameter to 'enforce_joint_model_state_space'. Expands workaround comment.
|
Cherry picked to jade and kinetic |
|
Hello, i cannot find the file, |
|
It's part of the moveit git repository. But if you want to use the feature in this branch, you have to set the ros-parameter. It has nothing to do with editing the file. |
|
@v4hn I noticed you never documented this extra parameter, could you please do that by simply copying your explanation into the tutorials? |
|
It's on my TODO list (now) |
|
@v4hn I used the parameter as you suggested with Approximated Constraint Manifolds and it works well. But I have a doubt: I used RRTConnect and I'd like to known how to guarantee that the interpolated point in joint space also satisfies the orientation constraints. For example, the extend step in RRTConnect involves interpolating between the nearest point and the sampled new point from the Approximated Constraint Manifolds. |
|
You can't get that guarantee with this method.
However, you can reduce the `longest_valid_segment_fraction` parameter to reduce the step width
of the interpolation, to ensure reasonable worst-case bounds.
|
|
@v4hn That's exactly what I understands about interpolation. But I found in moveit that the interpolated points in the growTree function in RRTConnect always satisfy the constraint (end effector kept upright). The parameter controls the interpolation step is the range parameter in RRTConnect. I'm using UR5 and below is an example of interpolating between two joint configurations: |
This should avoid trajectory jumps (sudden, discontinuous reconfigurations) when using cartesian constraints, at the cost of making planning slower. See moveit/moveit#541 (comment).
This will enforce JointModelStateSpaceFactory for problem representing see moveit/moveit#541 for detail basically this would allow path constraints in moveit_commander to work properly
|
Hey,
I wonder if I missed something or what could be wrong. |
|
On Mon, Feb 21, 2022 at 12:19:32PM -0800, rbtchun3 wrote:
Hey,
Does `enforce_joint_model_state_space` work with `melodic devel` branch?
I changed ompl_planning.yaml like this. But the end effector still flips in the path.
`RRTConnectkConfigDefault:
type: geometric::RRTConnect
range: 0.0
enforce_joint_model_state_space: True`
It's rather unfortunate from the config point of view, but `enforce_joint_model_state_space` is a child of the *planning group* and not the planner configuration. So you need to put it together with your `longest_valid_segment_fraction`, `default_planner_config`, et. al.
|
Constrained-based planning enables the PoseModelStateSpace by default, meaning that state samples are created by sampling in SE(3) and then performing IK. This state space frequently caused issues, leading to the introduction of the parameter in PR moveit#541 as a work-around. A more detailed analysis shows that also interpolation between states was performed in Cartesian space. If the two interpolation ends are close in Cartesian but distant in joint space, the interpolated state (obtained from IK) can be far away from both start and end as well (particularly for redundant arms), while its Cartesian pose is naturally close to both interpolation states. This will lead to consecutive waypoints being close in Cartesian space but distant in joint space. If such a trajectory is executed, the controller will interpolate in joint space, leading to a large and unchecked motion. To avoid this issue, interpolation should be performed in joint space during planning as well. The only drawback of this change is that trajectories are less linear in Cartesian space (see videos). However, as the trajectory is random-sampled anyway, this shouldn't be a big issue.
Constrained-based planning enables the PoseModelStateSpace by default, meaning that state samples are created by sampling in SE(3) and then performing IK. This state space frequently caused issues, eventually leading to the introduction of the ROS parameter enforce_joint_model_state_space in PR moveit#541 as a work-around. A more detailed analysis shows that the state space's interpolation approach is the culprit - performing interpolation in Cartesian space. If the two interpolation ends are close in Cartesian but distant in joint space, the interpolated state (obtained from IK) can be far away from both start and end (particularly for redundant arms), while its Cartesian pose is naturally close to both interpolation states. This will lead to consecutive waypoints being close in Cartesian space but distant in joint space. If such a trajectory is executed, the controller will interpolate in joint space, leading to a large and unchecked motion. To avoid this issue, interpolation should be performed in joint space during planning as well. The only drawback of this change is that trajectories are less linear in Cartesian space. However, as the trajectory is random-sampled anyway, this shouldn't be a big issue.
* Fix constrained-based planning Constrained-based planning enables the PoseModelStateSpace by default, meaning that state samples are created by sampling in SE(3) and then performing IK. This state space frequently caused issues, eventually leading to the introduction of the ROS parameter enforce_joint_model_state_space in PR #541 as a work-around. A more detailed analysis shows that the state space's interpolation approach is the culprit - performing interpolation in Cartesian space. If the two interpolation ends are close in Cartesian but distant in joint space, the interpolated state (obtained from IK) can be far away from both start and end (particularly for redundant arms), while its Cartesian pose is naturally close to both interpolation states. This will lead to consecutive waypoints being close in Cartesian space but distant in joint space. If such a trajectory is executed, the controller will interpolate in joint space, leading to a large and unchecked motion. To avoid this issue, interpolation should be performed in joint space during planning as well. The only drawback of this change is that trajectories are less linear in Cartesian space. However, as the trajectory is random-sampled anyway, this shouldn't be a big issue. * PoseModelStateSpace: Use joint-space distance Configurations may be similar in Cartesian EE pose, but very far apart in joint space. What actually matters when connecting states is the joint-space distance.
* Fix constrained-based planning Constrained-based planning enables the PoseModelStateSpace by default, meaning that state samples are created by sampling in SE(3) and then performing IK. This state space frequently caused issues, eventually leading to the introduction of the ROS parameter enforce_joint_model_state_space in PR moveit#541 as a work-around. A more detailed analysis shows that the state space's interpolation approach is the culprit - performing interpolation in Cartesian space. If the two interpolation ends are close in Cartesian but distant in joint space, the interpolated state (obtained from IK) can be far away from both start and end (particularly for redundant arms), while its Cartesian pose is naturally close to both interpolation states. This will lead to consecutive waypoints being close in Cartesian space but distant in joint space. If such a trajectory is executed, the controller will interpolate in joint space, leading to a large and unchecked motion. To avoid this issue, interpolation should be performed in joint space during planning as well. The only drawback of this change is that trajectories are less linear in Cartesian space. However, as the trajectory is random-sampled anyway, this shouldn't be a big issue. * PoseModelStateSpace: Use joint-space distance Configurations may be similar in Cartesian EE pose, but very far apart in joint space. What actually matters when connecting states is the joint-space distance.
Adds an optional ompl group parameter 'force_joint_model_state_space' to enforce the use of JointModelStateSpaceFactory for problem representation. This is convenient to work around issues with planning problems represented in PoseModelStateSpace, as is the case with some cartesian path planning scenarios.