[Servo] Change planning frame to base frame of active joint subgroup #2515

sea-bass · 2023-11-08T03:10:56Z

Description

This PR seeks to solve an issue in which changing the active_subgroup parameter still enforces using the servo_params.planning_frame planning frame... which is not necessarily the correct frame for all subgroups.

So instead, we're grabbing the planning frame (for twist/pose commands) to always be the base frame of the IK solver and doing the frame conversions from there. This fixes the immediate issue, but the implementation leaves much to be desired.

sea-bass · 2023-11-08T03:19:02Z

moveit_ros/moveit_servo/src/servo.cpp

+        const auto ik_solver = active_joint_model_group->getSolverInstance();
+        if (ik_solver)
+        {
+          const auto planning_frame = ik_solver->getBaseFrame();


Is there a better way to get the base link of a joint model group without having to grab the IK solver like this?

Can't you do this?

const JointModel* root_joint_model = group->getJointModels().front(); const LinkModel* root_link_model = root_joint_model->getParentLinkModel();

But now I wonder if you rather need the IK solver base frame (i.e. your code as is), since the underlying IK solver could be working on a different frame, not necessarily the group root?
If that's the case, maybe create a getIKSolverBaseFrame() function that can be called here and below?

Thanks!

This raises a follow up question that, if these changes are the right thing for changing subgroups, can we actually get rid of the planning frame parameter and have servo always do its own internal conversions to active group's IK base frame?

Hmm.. looking deeper into Servo I got more confused about what the 'planning frame' is, but also how this fix works, since the IK base frame for the manipulator group is the one tilted 45 deg. right? so I would expect this to still move the wrong way.
Another q: the command has a frame_id. Shouldn't that be all that is needed to move in the same frame (e.g. world), independently of the active group? Sorry I have more questions than answers!

You should have more questions than answers, I think there's a lot here that needs to be disentangled for clarity.

Basically, Servo has this rule that enforces that all commands passed into it are expressed w.r.t. a common frame -- this is the "planning frame" we speak of.

All we're trying to do, perhaps in an overly complex way, is the following:

Receive a Cartesian command w.r.t. some arbitrary frame ID

Convert it to a reference frame that agrees with the IK solver (this is our planning frame)

???

Profit

Hmm... I just saw some other piece of code that reminded me of the fact that a planning group can have multiple tip frames. So can we really get rid of moveit_params.ee_frame after all?

The other alternative, which is way more destructive, would be to modify the command message definitions for twist and pose tracking to contain both base and tip frames... which would basically require making custom messages for both instead of relying on existing geometry_msgs definitions.

I think we can, at least the way it is now, because ee_frame doesn't seem to be used anywhere. The IK computation is done for ik_solver->getTipFrame(), so it will always use the first tip.
Longer term what about having Servo expose a /configure service, where the user could set the planning group and tip frame?

Given all the struggles we've had trying to make sure that parameters stay consistent w.r.t. each other, I quite like the idea of dedicated services that touch these interrelated parameters like planning group + ee frame.

Also, having this configure service / method means that configuring can implicitly do the task of pausing servo, changing values, and then unpausing it.

Also, I guess to be fair this code was calling getTipFrame() already, and also there is an error logged when you call getTipFrame() on a planning group with multiple tips.

So maybe this is just a known limitation for now and we proceed as is?

A service to update the parameters might be a good idea b/c it would fix this issue: #2412

i.e. do the parameter validation in a callback in a different thread

Hate to change the API so drastically, so soon, though. Have to think carefully about this.

I think servo has changed enough with the refactor since we kept running into limitations trying to keep the API as stable as possible with the old version. It might be beneficial to do all the big changes as soon as possible in hopes that we get to a somewhat more stable API before people actually start using this new version.

That said, we should try do all this in a way that minimizes our need to keep twiddling the interfaces going forward.

codecov · 2023-11-08T04:11:26Z

Codecov Report

Attention: 18 lines in your changes are missing coverage. Please review.

Comparison is base (4b73edb) 50.77% compared to head (0ad1d85) 50.82%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2515      +/-   ##
==========================================
+ Coverage   50.77%   50.82%   +0.05%     
==========================================
  Files         392      392              
  Lines       32170    32194      +24     
==========================================
+ Hits        16332    16358      +26     
+ Misses      15838    15836       -2

Files	Coverage Δ
...oveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp	`100.00% <100.00%> (ø)`
...oveit_ros/moveit_servo/tests/servo_ros_fixture.hpp	`93.23% <100.00%> (ø)`
moveit_ros/moveit_servo/tests/test_integration.cpp	`100.00% <100.00%> (ø)`
moveit_ros/moveit_servo/src/utils/common.cpp	`84.71% <83.34%> (-0.29%)`	⬇️
moveit_ros/moveit_servo/src/utils/command.cpp	`70.28% <62.50%> (ø)`
moveit_ros/moveit_servo/src/servo.cpp	`67.89% <77.97%> (+1.80%)`	⬆️

... and 1 file with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

marioprats

This looks good to me. A couple of minor comments that can be done in follow-up changes too, since they are not really related to this PR changes.

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

marioprats · 2023-11-10T10:29:54Z

moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp

+ * @param group_name The currently active joint group name.
+ * @return The IK solver base frame, if one exists, otherwise std::nullopt.
+ */
+std::optional<std::string> getIKSolverBaseFrame(const moveit::core::RobotStatePtr& robot_state,


Can we avoid the pointer here and below and just do const moveit::core::RobotState &robot_state?

I think the robot state which is extracted from

moveit::core::RobotStatePtr CurrentStateMonitor::getCurrentState() const

already returns in pointer form, so is it fine to keep as is? I don't particularly want to dereference what's already there.

That's fine if this is only used in this context. If we ever put it somewhere more general I think we should just take the const&.
shared_ptr is viral, it easily spreads everywhere. If this takes a shared_ptr, it can't really can't be used unless you have a shared_ptr already. Whereas if this takes a const&, you can call it directly with a RobotState object, but also with a shared_ptr or unique_ptr by derefencing, so it's more usable.

sjahr

Nice work! Most of my comments are around more error messages and documentations. Besides that I think it is good to go 👍

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

moveit_ros/moveit_servo/src/servo.cpp

moveit_ros/moveit_servo/src/utils/command.cpp

sea-bass · 2023-11-13T20:19:25Z

@AndyZe we're cutting a release tomorrow so I'm going to merge this in now. If you have any follow-on comments related to this PR, let's do a new issue/PR and take it from there?

Change planning frame to base of active subgroup

09eb17e

sea-bass requested review from sjahr and eholum November 8, 2023 03:16

sea-bass commented Nov 8, 2023

View reviewed changes

sea-bass changed the title ~~Change planning frame to base of active subgroup~~ [Servo] Change planning frame to base frame of active joint subgroup Nov 8, 2023

sea-bass added 2 commits November 8, 2023 09:45

Make function to get IK base frame

b741853

Catch tf2 exception nearer to actual code, use std::optionals

f003315

AndyZe self-requested a review November 8, 2023 15:06

Remove ee_frame and planning_frame params

5612d88

sea-bass marked this pull request as ready for review November 8, 2023 16:54

marioprats reviewed Nov 10, 2023

View reviewed changes

sea-bass added 3 commits November 10, 2023 08:50

PR comments

7601fc6

Merge remote-tracking branch 'origin/main' into fix-servo-subgroup-cmds

0c134b7

Helper function doesn't need to be public actually

b20b14e

sea-bass requested review from marioprats and ibrahiminfinite November 10, 2023 13:53

marioprats approved these changes Nov 10, 2023

View reviewed changes

sea-bass and others added 3 commits November 10, 2023 10:15

Weird clang-tidy style changes

daf5f6f

Move test fixture code from constructors to setup

85a382f

Merge branch 'main' into fix-servo-subgroup-cmds

b193aa4

sjahr reviewed Nov 13, 2023

View reviewed changes

sea-bass and others added 2 commits November 13, 2023 10:51

PR comments: errors and comments

db97165

Merge branch 'main' into fix-servo-subgroup-cmds

0ad1d85

sea-bass requested a review from sjahr November 13, 2023 15:51

sjahr approved these changes Nov 13, 2023

View reviewed changes

sea-bass merged commit 2487b24 into main Nov 13, 2023
14 of 15 checks passed

sea-bass deleted the fix-servo-subgroup-cmds branch November 13, 2023 20:19

sea-bass mentioned this pull request Dec 8, 2023

Adding planning_frame and ee_frame to panda_simulated_config.yaml #2591

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[Servo] Change planning frame to base frame of active joint subgroup #2515

[Servo] Change planning frame to base frame of active joint subgroup #2515

sea-bass commented Nov 8, 2023 •

edited

Loading

sea-bass Nov 8, 2023

marioprats Nov 8, 2023

sea-bass Nov 8, 2023

marioprats Nov 8, 2023

sea-bass Nov 8, 2023

sea-bass Nov 9, 2023 •

edited

Loading

marioprats Nov 9, 2023

sea-bass Nov 9, 2023 •

edited

Loading

AndyZe Nov 10, 2023

sea-bass Nov 10, 2023

codecov bot commented Nov 8, 2023 •

edited

Loading

marioprats left a comment

marioprats Nov 10, 2023

sea-bass Nov 10, 2023 •

edited

Loading

marioprats Nov 10, 2023

sjahr left a comment

sea-bass commented Nov 13, 2023

[Servo] Change planning frame to base frame of active joint subgroup #2515

[Servo] Change planning frame to base frame of active joint subgroup #2515

Conversation

sea-bass commented Nov 8, 2023 • edited Loading

Description

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass Nov 9, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass Nov 9, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov bot commented Nov 8, 2023 • edited Loading

Codecov Report

marioprats left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass Nov 10, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sjahr left a comment

Choose a reason for hiding this comment

sea-bass commented Nov 13, 2023

sea-bass commented Nov 8, 2023 •

edited

Loading

sea-bass Nov 9, 2023 •

edited

Loading

sea-bass Nov 9, 2023 •

edited

Loading

codecov bot commented Nov 8, 2023 •

edited

Loading

sea-bass Nov 10, 2023 •

edited

Loading