[Servo] Improve support for switching planning group #2333

ibrahiminfinite · 2023-08-29T18:09:16Z

Description

This PR adds improved support for changing the planning group used by Servo by following approach 3 mentioned in the issue.

AndyZe

I'm not sure about this. Lot of complexity added and I don't see anything to prevent a switch when the robot is in motion.

Perhaps it would make more sense to launch 2 Servo instances, one for each move_group? That will be needed, anyway, for something like a dual-arm system.

ibrahiminfinite · 2023-08-30T06:35:25Z

Lot of complexity added

I don't think this adds much complexity.
The only new infrastructure added is the code for switching the move group, and a new service called ServoMoveGroup to moveit_msgs.

Other than that the changes are:

The num_joints and joint_model_group have been made into member variables and is updated only through the updateMoveGroup method. This required the joint_model_group to be passed in to a couple of methods.
The parameter move_group_name is made read-only, it is only used once during initialization of Servo and is copied into the member variable active_move_group_, which is then used everywhere else where the move group name is needed, this is also the variable updated by the service.

I don't see anything to prevent a switch when the robot is in motion.

The user would have to manage that when using the C++ API like with most of the functionality , but for the ROS API which I assume most users will be using, the service call automatically pauses the servo loop before updating the move group and then resumes it after update.

I think this functionality would be useful in the long run since it lets us use different groups without having to start a new node.
But you might be right @AndyZe, if the move group changes, the parameters like ee_frame also potentially need to be changed. In which case it makes more sense to just launch a new servo node with the required parameters (but at the cost of resource overhead).

Would be good to have some feedback from the MoveIt Studio team.

AndyZe

Blocking until the comments above get resolved

codecov · 2023-09-04T09:29:23Z

Codecov Report

Patch coverage: 69.00% and project coverage change: +0.12% 🎉

Comparison is base (6482350) 50.73% compared to head (19b7c1f) 50.84%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2333      +/-   ##
==========================================
+ Coverage   50.73%   50.84%   +0.12%     
==========================================
  Files         386      386              
  Lines       31970    31974       +4     
==========================================
+ Hits        16216    16255      +39     
+ Misses      15754    15719      -35

Files Changed	Coverage Δ
...eit_servo/include/moveit_servo/utils/datatypes.hpp	`100.00% <ø> (ø)`
moveit_ros/moveit_servo/src/servo_node.cpp	`79.77% <40.00%> (+2.26%)`	⬆️
moveit_ros/moveit_servo/src/servo.cpp	`67.66% <70.53%> (+4.09%)`	⬆️

... and 4 files with indirect coverage changes

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

ibrahiminfinite · 2023-09-04T09:37:38Z

This update allows the user to change the planning group and smoothing plugin without restarting the servo node.
A safety check is also in place that will not allow for update of the above parameters unless servo is paused.

Another important change is that the choice of updating the parameters when using C++ API has been delegated to the user.
This has two benefits:

Allows for a clean usage of the Servo::updateParams() method from ServoNode
Users of C++ API can gain better performance by skipping the call to Servo::updateParams() if they know that paramters will not be updated during runtime.

ibrahiminfinite · 2023-09-04T13:24:32Z

@sea-bass

sea-bass

I kind of agree with @AndyZe about the extra complexity, but at the same time don't think we should require multiple servo nodes to switch planning groups.

To reduce complexity, I'd like to clarify: Why do we need to explicitly call updateParameters() with these changes? If this continues to be called automatically when you get the next servo command, this should be fine and you'll still get the warnings if you change planning groups/smoothing plugin while servo is running. Then, the changes in this PR also become much less disruptive.

As a much smaller comment: Seems with these changes we got rid of the RCLCPP_INFO_STREAM() printouts that mentioned what parameters were changed to. Can we get those back as well?

ibrahiminfinite · 2023-09-04T14:56:02Z

Why do we need to explicitly call updateParameters() with these changes? If this continues to be called automatically when you get the next servo command, this should be fine and you'll still get the warnings if you change planning groups/smoothing plugin while servo is running

If the updateParams() is called automatically from getNextJointState() we would have call it separately again in the ServoLoop when servo is paused, which did not feel "right" .

One reason for making it manual is that users who do not need to update params during runtime don't need to pay the cost for load of checks that happen when calling updateParams(), and the change really only affects the C++ API users (which I assume no one is using at the moment).
For the ROS API users, there are no changes to any of the APIs, but they just gained the ability to switch planning groups or smoothing plugins.

I don't find this this change disruptive, but we can go back to putting the upadateParams() in getNextJointState() if this feels like too much.

Seems with these changes we got rid of the RCLCPP_INFO_STREAM() printouts that mentioned what parameters were changed to. Can we get those back as well?

Yeah, only override_velocity_scaing_factor and move_group_name were being printed, but since any of the parameters could be updated, I was thinking if we should add printing for other params as well.

EDIT : This de-coupling will most likely be useful for future changes as well.

sea-bass

Looking at this again, I'm OK with the non-node examples explicitly calling updateParams().

There is a test failure here based on inability to load the smoothing plugin, so take a look at that and this should be good.

moveit_ros/moveit_servo/src/servo.cpp

AndyZe · 2023-09-10T19:15:16Z

moveit_ros/moveit_servo/src/servo.cpp

-
-  // Update the parameters
-  updateParams();
+  setStatus(StatusCode::NO_WARNING);


I think this should be done outside of this function, for readability. It's kind of hidden away here in an unrelated function.

This is the only place we can set it automatically.
Otherwise user would have to manually call it.

If you have the function either return the status or accept it as a mutable input arg, this could be done!

Reading through the code again, I realize that additionally to everything already said, using the setStatus() function the way we use it now is why you had to make all those other member functions non-const. So another data point for figuring this out in a more functional way.

moveit_ros/moveit_servo/src/servo_node.cpp

ibrahiminfinite · 2023-09-15T16:03:52Z

How about we allow update of simple parameters like collision toggle etc normally, and check against servo paused status for just planning group change. If not paused we can set status of servo to invalid and print a warning, setting the status to invalid will also skip rest of computations in getNextJointState and robot would be send a command .

Have implemented this, and kept the call to updateParams explicit as before.
Used a invalid_parameter_update_ flag to handle the setting of status to INVALID

sea-bass

Read through and I think this looks good.

My biggest thing remaining is ensuring we don't have to un-const a lot of our functions. If anything, we need more of them to be const. To that point:

I'll +1 @AndyZe's comment on using setStatus() outside the bodies of existing functions so it's more readable and gives us more const member functions
The getCurrentRobotState() is now being used both as a getter and to mutate internal state, so I think those 2 things should use 2 separate functions.

sea-bass · 2023-09-15T16:41:11Z

moveit_ros/moveit_servo/src/servo.cpp

-                                                              << params.move_group_name);
+        if (servo_status_ == StatusCode::PAUSED)
+        {
+          invalid_parameter_update_ = false;


invalid_parameter_update_ is synonymous with servo_status != StatusCode::PAUSED now, so no need for this member variable, right?

sea-bass · 2023-09-15T16:44:05Z

moveit_ros/moveit_servo/src/servo.cpp

-
-  // Update the parameters
-  updateParams();
+  setStatus(StatusCode::NO_WARNING);


Reading through the code again, I realize that additionally to everything already said, using the setStatus() function the way we use it now is why you had to make all those other member functions non-const. So another data point for figuring this out in a more functional way.

sea-bass · 2023-09-15T16:46:57Z

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -122,14 +128,19 @@ class Servo
   * \brief Get the current state of the robot as given by planning scene monitor.
   * @return The current state of the robot.
   */
-  KinematicState getCurrentRobotState() const;
+  KinematicState getCurrentRobotState();


Why is this not const if it just gets the robot state variable?

This is impacting the constness of a bunch of other functions as well

sea-bass · 2023-09-15T16:50:00Z

moveit_ros/moveit_servo/src/servo.cpp

+KinematicState Servo::getCurrentRobotState()
 {
-  moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
-  const moveit::core::JointModelGroup* joint_model_group =
-      robot_state->getJointModelGroup(servo_params_.move_group_name);
-  const auto joint_names = joint_model_group->getActiveJointModelNames();
+  robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();

-  KinematicState current_state(joint_names.size());
-  current_state.joint_names = joint_names;
-  robot_state->copyJointGroupPositions(joint_model_group, current_state.positions);
-  robot_state->copyJointGroupVelocities(joint_model_group, current_state.velocities);
-  robot_state->copyJointGroupAccelerations(joint_model_group, current_state.accelerations);
+  KinematicState current_state(joint_names_.size());
+  current_state.joint_names = joint_names_;
+  robot_state_->copyJointGroupPositions(joint_model_group_, current_state.positions);
+  robot_state_->copyJointGroupVelocities(joint_model_group_, current_state.velocities);
+  robot_state_->copyJointGroupAccelerations(joint_model_group_, current_state.accelerations);

  return current_state;
 }


Yeah, so this function seems to have 2 uses right now, which is why it's not const. I think what we need is:

An updateCurrentRobotState() that does everything done here but doesn't return current_state, it simply updates robot_state_, to be used in functions that calculate commands.

A getCurrentRobotState() function that is const and simply returns robot_state_, to be used in other tools.

sea-bass · 2023-09-15T16:50:17Z

moveit_ros/moveit_servo/src/servo.cpp

 {
  bool stopped = false;
  auto target_state = halt_state;
  const auto current_state = getCurrentRobotState();

-  const size_t num_joints = current_state.joint_names.size();
-  for (size_t i = 0; i < num_joints; i++)
+  for (size_t i = 0; i < joint_names_.size(); i++)


Suggested change

for (size_t i = 0; i < joint_names_.size(); i++)

for (size_t i = 0; i < joint_names_.size(); ++i)

sea-bass · 2023-09-15T16:51:19Z

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

+  const moveit::core::JointModelGroup* joint_model_group_;
+  std::vector<std::string> joint_names_;
+
+  bool invalid_parameter_update_;


I don't think we need this if it's used as synonymous with servo being paused.

I also can't find that line in the PR review, but I think your changes make you able to remove the servo_paused_ member variable as well?

sea-bass · 2023-09-15T23:33:07Z

I had one more random thought... if we switch planning groups, things like the planning frame, controller command topic, etc. possibly will need to change too.

If this is getting a bit convoluted, we can take it on at PickNik. Offer still stands!

ibrahiminfinite · 2023-09-16T00:19:51Z

I had one more random thought... if we switch planning groups, things like the planning frame, controller command topic, etc. possibly will need to change too.

Yeah, I had mentioned the need for additional parameter changes in one of the earlier comments on this PR.
If things like command topic needs to change, I think the best thing to do is to start a new node.

If this is getting a bit convoluted, we can take it on at PickNik. Offer still stands!

Yeah that might be a better choice here since you guys will have more grasp on the requirements internally. And I can work on the optimisation PR instead.

AndyZe · 2023-09-16T03:58:56Z

Yeah, I had mentioned the need for additional parameter changes in one of the earlier comments on this PR. If things like command topic needs to change, I think the best thing to do is to start a new node.

👍 to this, and coming back full circle now

sea-bass · 2023-09-16T12:51:49Z

Thanks! I'll go ahead and close this PR.

Based on this, I think we can stick with the multiple node solution until/unless we can put some engineering resources on something else.

AndyZe reviewed Aug 29, 2023

View reviewed changes

AndyZe requested changes Aug 31, 2023

View reviewed changes

ibrahiminfinite force-pushed the jmg_switch_service branch from dd0262b to 405ccf3 Compare September 4, 2023 09:29

ibrahiminfinite marked this pull request as ready for review September 4, 2023 09:40

ibrahiminfinite changed the title ~~WIP : [Servo] Improve support for switching planning group~~ [Servo] Improve support for switching planning group Sep 4, 2023

ibrahiminfinite requested a review from AndyZe September 4, 2023 13:24

sea-bass reviewed Sep 4, 2023

View reviewed changes

sea-bass reviewed Sep 7, 2023

View reviewed changes

ibrahiminfinite marked this pull request as draft September 10, 2023 09:35

AndyZe reviewed Sep 10, 2023

View reviewed changes

ibrahiminfinite added 9 commits September 15, 2023 12:01

Add PAUSED status

da1e351

Add setStatus() method

2617460

Make updateParams public + update only when paused

bee7b76

Make robot state a member variable

4cc5705

Make joint model group a member variable

5daf8c2

Make joint names a member variable

c91feee

Get number of joint from joint names

466b286

Allow updating planning group

824c1c2

set servo status in pause service

3bf17df

ibrahiminfinite force-pushed the jmg_switch_service branch from 405ccf3 to 3bf17df Compare September 15, 2023 08:07

ibrahiminfinite added 3 commits September 15, 2023 13:39

Reload smoothing plugin on planning group change

3dd18b1

Always load the smoothing plugin but use based on the parameter

58f6fbc

Initialize smoothing plugin withing try block

5d21659

sea-bass reviewed Sep 15, 2023

View reviewed changes

moveit_ros/moveit_servo/src/servo_node.cpp Outdated Show resolved Hide resolved

moveit deleted a comment from mergify bot Sep 15, 2023

ibrahiminfinite added 2 commits September 15, 2023 21:03

Better handling for paramter updates

3998cf5

Use flag for invalid update

19b7c1f

sea-bass reviewed Sep 15, 2023

View reviewed changes

sea-bass closed this Sep 16, 2023

ibrahiminfinite deleted the jmg_switch_service branch February 24, 2024 10:08

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[Servo] Improve support for switching planning group #2333

[Servo] Improve support for switching planning group #2333

ibrahiminfinite commented Aug 29, 2023 •

edited

Loading

AndyZe left a comment

ibrahiminfinite commented Aug 30, 2023 •

edited

Loading

AndyZe left a comment

codecov bot commented Sep 4, 2023 •

edited

Loading

ibrahiminfinite commented Sep 4, 2023

ibrahiminfinite commented Sep 4, 2023

sea-bass left a comment

ibrahiminfinite commented Sep 4, 2023 •

edited

Loading

sea-bass left a comment

AndyZe Sep 10, 2023

ibrahiminfinite Sep 14, 2023

sea-bass Sep 15, 2023

sea-bass Sep 15, 2023

ibrahiminfinite commented Sep 15, 2023

sea-bass left a comment

sea-bass Sep 15, 2023

sea-bass Sep 15, 2023

sea-bass Sep 15, 2023

sea-bass Sep 15, 2023

sea-bass Sep 15, 2023

sea-bass Sep 15, 2023

sea-bass commented Sep 15, 2023 •

edited

Loading

ibrahiminfinite commented Sep 16, 2023

AndyZe commented Sep 16, 2023

sea-bass commented Sep 16, 2023

	for (size_t i = 0; i < joint_names_.size(); i++)
	for (size_t i = 0; i < joint_names_.size(); ++i)

[Servo] Improve support for switching planning group #2333

[Servo] Improve support for switching planning group #2333

Conversation

ibrahiminfinite commented Aug 29, 2023 • edited Loading

Description

AndyZe left a comment

Choose a reason for hiding this comment

ibrahiminfinite commented Aug 30, 2023 • edited Loading

AndyZe left a comment

Choose a reason for hiding this comment

codecov bot commented Sep 4, 2023 • edited Loading

Codecov Report

ibrahiminfinite commented Sep 4, 2023

ibrahiminfinite commented Sep 4, 2023

sea-bass left a comment

Choose a reason for hiding this comment

ibrahiminfinite commented Sep 4, 2023 • edited Loading

sea-bass left a 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

Choose a reason for hiding this comment

ibrahiminfinite commented Sep 15, 2023

sea-bass left a 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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass commented Sep 15, 2023 • edited Loading

ibrahiminfinite commented Sep 16, 2023

AndyZe commented Sep 16, 2023

sea-bass commented Sep 16, 2023

ibrahiminfinite commented Aug 29, 2023 •

edited

Loading

ibrahiminfinite commented Aug 30, 2023 •

edited

Loading

codecov bot commented Sep 4, 2023 •

edited

Loading

ibrahiminfinite commented Sep 4, 2023 •

edited

Loading

sea-bass commented Sep 15, 2023 •

edited

Loading