[Servo] Correct for drift by using previously commanded state instead of current state #2349

sea-bass · 2023-09-08T12:45:40Z

This PR implements the suggestion in #1857 (comment) to try correct drift for robots without great joint tracking control (like the MoveIt Studio Gazebo simulation).

This adds an apply_anti_drift parameter to MoveIt Servo, which when enabled will use the previously commanded joint state instead of the current state to compute joint deltas.

… of current state

sea-bass · 2023-09-08T12:45:47Z

@ibrahiminfinite

ibrahiminfinite · 2023-09-08T14:04:14Z

The std::optional passing approach seems a bit complicated.

How about something like this :

1 . Move last_commanded_state_ from ServoNode to Servo
2 . Compute error here and apply it to target_joint_positions.

 
// Compute error from last commanded state
joint_position_error = last_commanded_positions_  - current_joint_positions
// Compute the next joint positions based on the joint position deltas
target_joint_positions = current_joint_positions + joint_position_delta + joint_position_error;

or

 
// Compute error from last commanded state
joint_position_error = last_commanded_positions_  - current_joint_positions
// Compute the next joint positions based on the joint position deltas
target_joint_positions = current_joint_positions + joint_position_delta;

if(servo_params_.apply_anti_drift)
    target_joint_positions += joint_position_error;

last_commanded_positions_ will be an Eigen::Map of last_commanded_state_.positions
This way none of the public APIs have to change.

The smoothHalt() method can then just use the last_command_state_ without needing to pass it in, since it will already be available as a member variable in Servo.

sea-bass · 2023-09-08T14:10:38Z

I actually started doing it exactly that way you suggested... but then realized that we actually want to be computing the joint delta from the last commanded state instead of from the current state like in this line. This affects results when responding to twist or pose commands.

That said, the suggestion to move away from optionals and stick the last commanded state in Servo instead of ServoNode is valid. That lets the optional argument to getNextJointState() simply be a boolean. I can look at that when I find some more time.

codecov · 2023-09-08T14:49:16Z

Codecov Report

Patch coverage: 64.45% and project coverage change: -0.03% ⚠️

Comparison is base (84421ea) 50.74% compared to head (567469c) 50.71%.

❗ Current head 567469c differs from pull request most recent head 2078dd3. Consider uploading reports for the commit 2078dd3 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2349      +/-   ##
==========================================
- Coverage   50.74%   50.71%   -0.03%     
==========================================
  Files         386      386              
  Lines       31958    31968      +10     
==========================================
- Hits        16215    16210       -5     
- Misses      15743    15758      +15

Files Changed	Coverage Δ
moveit_ros/moveit_servo/src/servo.cpp	`63.14% <61.54%> (-0.73%)`	⬇️
moveit_ros/moveit_servo/src/servo_node.cpp	`77.52% <83.34%> (-3.43%)`	⬇️

... and 2 files with indirect coverage changes

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

moveit_ros/moveit_servo/src/servo_node.cpp

moveit_ros/moveit_servo/src/servo.cpp

ibrahiminfinite · 2023-09-09T06:39:15Z

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -126,10 +127,21 @@ class Servo

  /**
   * \brief Smoothly halt at a commanded state when command goes stale.
-   * @param The last commanded joint states.
+   * @param halt_state_maybe A std::optional containing the last commanded joint states, if available.


Suggested change

* @param halt_state_maybe A std::optional containing the last commanded joint states, if available.

Outdated comment

This seems like the type of thing that should be a free function, eh? Transparent inputs and outputs

It is a public API of Servo which is used by ServoNode, not sure if making it a free function would make sense contextually even though it can easily be a free function.

ibrahiminfinite · 2023-09-09T06:40:49Z

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -200,6 +212,8 @@ class Servo
  std::shared_ptr<const servo::ParamListener> servo_param_listener_;
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;

+  std::optional<KinematicState> last_commanded_state_;


Suggested change

std::optional<KinematicState> last_commanded_state_;

KinematicState last_commanded_state_;

I think this should not be optional, since last_commanded_state_ should technically always have a value.

i'm ok with the optional. Maybe it wouldn't have a value if the program were just started or somebody just called resetLastCommandedState()

Maybe it wouldn't have a value if the program were just started or somebody just called resetLastCommandedState()

Would it make sense for resetLastCommandedState() to reset it to the current state rather than a std::nullopt.
We could then call this same function in the constructor to initialize the last_commanded_state_.

ibrahiminfinite · 2023-09-09T07:53:15Z

I am not sure about computing the required joint delta by setting robot_state positions to last_commanded_state_.
This would mean we are computing the delta assuming that the robot actually reached the last_commanded_state_.

What I think should happen is:
In case of a drift, the robot is currently at positions q_current (under/overshoot), but should have been at q_target.
So when we compute the new joint delta, we want the error q_target - q_current to be accounted for.
If we use the last commanded positions q_target, the delta would be smaller/larger than what it should be, depending on if the drift was an undershoot or overshoot.

AndyZe · 2023-09-10T03:18:31Z

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -74,6 +74,7 @@ class Servo
  /**
   * \brief Computes the joint state required to follow the given command.
   * @param command The command to follow, std::variant type, can handle JointJog, Twist and Pose.
+   * is used to calculate an error between the current state and the previous commanded state for anti-drift correction.


Continuing the formatting nitpick theme ;)

Suggested change

* is used to calculate an error between the current state and the previous commanded state for anti-drift correction.

* Is used to calculate an error between the current state and the previous commanded state for anti-drift correction.

AndyZe · 2023-09-10T03:19:35Z

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -126,10 +127,21 @@ class Servo

  /**
   * \brief Smoothly halt at a commanded state when command goes stale.
-   * @param The last commanded joint states.
+   * @param halt_state_maybe A std::optional containing the last commanded joint states, if available.


This seems like the type of thing that should be a free function, eh? Transparent inputs and outputs

AndyZe · 2023-09-10T03:20:10Z

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -126,10 +127,21 @@ class Servo

  /**
   * \brief Smoothly halt at a commanded state when command goes stale.
-   * @param The last commanded joint states.
+   * @param halt_state_maybe A std::optional containing the last commanded joint states, if available.
   * @return The next state stepping towards the required halting state.


What does the bool component of the returned value mean?

AndyZe · 2023-09-10T03:22:49Z

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -200,6 +212,8 @@ class Servo
  std::shared_ptr<const servo::ParamListener> servo_param_listener_;
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;

+  std::optional<KinematicState> last_commanded_state_;


i'm ok with the optional. Maybe it wouldn't have a value if the program were just started or somebody just called resetLastCommandedState()

AndyZe · 2023-09-10T03:25:05Z

moveit_ros/moveit_servo/src/servo.cpp

+  auto initial_state = current_state;
+  if (servo_params_.apply_anti_drift && last_commanded_state_.has_value())
+  {
+    robot_state->setJointGroupPositions(joint_model_group, last_commanded_state_.value().positions);


Since we know last_commanded_state_ has a value

Suggested change

robot_state->setJointGroupPositions(joint_model_group, last_commanded_state_.value().positions);

robot_state->setJointGroupPositions(joint_model_group, *last_commanded_state_.positions);

AndyZe · 2023-09-10T03:28:11Z

moveit_ros/moveit_servo/src/servo.cpp

@@ -423,7 +432,7 @@ KinematicState Servo::getNextJointState(const ServoInput& command)
    joint_position_delta *= collision_velocity_scale_;

    // Compute the next joint positions based on the joint position deltas
-    target_joint_positions = current_joint_positions + joint_position_delta;
+    target_joint_positions = initial_joint_positions + joint_position_delta;


Maybe i missed it but i don't see that current_joint_positions was modified at all between L410 and here. So why not use current_joint_positions and don't bother creating initial_joint_positions?

AndyZe · 2023-09-10T03:29:24Z

moveit_ros/moveit_servo/src/servo.cpp

-  }
+    auto& halt_state = last_commanded_state_.value();
+    const size_t num_joints = current_state.joint_names.size();
+    for (size_t i = 0; i < num_joints; i++)


nit

Suggested change

for (size_t i = 0; i < num_joints; i++)

for (size_t i = 0; i < num_joints; ++i)

AndyZe · 2023-09-10T03:30:43Z

moveit_ros/moveit_servo/src/servo.cpp

  {
-    smoother_->reset(current_state.positions);
-    smoother_->doSmoothing(halt_state.positions);
+    return std::make_pair(stopped, current_state);


stopped appears to be false here, I think it should be true

moveit_ros/moveit_servo/src/servo.cpp

sea-bass · 2023-09-10T11:48:57Z

Guys, I am not sure this ended up helping at all, so I may close this PR. Still need to do some more testing.

[Servo] Correct for drift by using previously commanded state instead…

bdff331

… of current state

sea-bass added 2 commits September 8, 2023 09:15

Remove unnecessary lines

2a9813e

Reset last commanded state properly

e9e7ec2

sea-bass added 4 commits September 8, 2023 11:04

Only use positions from previous state

77539f7

Move last commanded state to core servo class

4aabfd3

Add missing comments

03f5985

Fix some logic

2078dd3

sea-bass force-pushed the servo-anti-drift branch from 3554b86 to 567469c Compare September 8, 2023 21:15

sea-bass commented Sep 8, 2023

View reviewed changes

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

sea-bass commented Sep 8, 2023

View reviewed changes

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

sea-bass force-pushed the servo-anti-drift branch from 567469c to 2078dd3 Compare September 8, 2023 22:07

sea-bass mentioned this pull request Sep 8, 2023

[Servo] Fix bugs when halting for collision + transforming commands to planning frame #2350

Merged

ibrahiminfinite reviewed Sep 9, 2023

View reviewed changes

AndyZe reviewed Sep 10, 2023

View reviewed changes

sea-bass marked this pull request as draft September 10, 2023 12:01

sea-bass closed this Sep 10, 2023

sea-bass deleted the servo-anti-drift branch July 31, 2024 00:40

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[Servo] Correct for drift by using previously commanded state instead of current state #2349

[Servo] Correct for drift by using previously commanded state instead of current state #2349

sea-bass commented Sep 8, 2023 •

edited

Loading

sea-bass commented Sep 8, 2023

ibrahiminfinite commented Sep 8, 2023

sea-bass commented Sep 8, 2023 •

edited

Loading

codecov bot commented Sep 8, 2023 •

edited

Loading

ibrahiminfinite Sep 9, 2023

AndyZe Sep 10, 2023

ibrahiminfinite Sep 10, 2023

ibrahiminfinite Sep 9, 2023

AndyZe Sep 10, 2023

ibrahiminfinite Sep 10, 2023

ibrahiminfinite commented Sep 9, 2023

AndyZe Sep 10, 2023

AndyZe Sep 10, 2023

AndyZe Sep 10, 2023

AndyZe Sep 10, 2023

AndyZe Sep 10, 2023

AndyZe Sep 10, 2023

AndyZe Sep 10, 2023

AndyZe Sep 10, 2023

sea-bass commented Sep 10, 2023

	std::optional<KinematicState> last_commanded_state_;
	KinematicState last_commanded_state_;

	* is used to calculate an error between the current state and the previous commanded state for anti-drift correction.
	* Is used to calculate an error between the current state and the previous commanded state for anti-drift correction.

	robot_state->setJointGroupPositions(joint_model_group, last_commanded_state_.value().positions);
	robot_state->setJointGroupPositions(joint_model_group, *last_commanded_state_.positions);

	for (size_t i = 0; i < num_joints; i++)
	for (size_t i = 0; i < num_joints; ++i)

[Servo] Correct for drift by using previously commanded state instead of current state #2349

[Servo] Correct for drift by using previously commanded state instead of current state #2349

Conversation

sea-bass commented Sep 8, 2023 • edited Loading

sea-bass commented Sep 8, 2023

ibrahiminfinite commented Sep 8, 2023

sea-bass commented Sep 8, 2023 • edited Loading

codecov bot commented Sep 8, 2023 • edited Loading

Codecov Report

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

ibrahiminfinite commented Sep 9, 2023

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

Choose a reason for hiding this comment

sea-bass commented Sep 10, 2023

sea-bass commented Sep 8, 2023 •

edited

Loading

sea-bass commented Sep 8, 2023 •

edited

Loading

codecov bot commented Sep 8, 2023 •

edited

Loading