[jog_arm] Fix initial end effector transform jump #1871

AndyZe · 2020-02-02T02:15:02Z

Fix a bug where the end effector transform jumped initially, when the first jog command is received.
Reduce duplicate code between joint jogging and Cartesian jogging.
Remove the joint velocity low-pass filters. I justify this because joint velocities are derived from joint positions (which do get filtered).
This required some re-ordering of the algorithm. I think the flow makes a lot more sense now.

codecov-io · 2020-02-04T14:33:03Z

Codecov Report

Merging #1871 into master will increase coverage by 0.01%.
The diff coverage is 67.94%.

@@            Coverage Diff             @@
##           master    #1871      +/-   ##
==========================================
+ Coverage   50.25%   50.27%   +0.01%     
==========================================
  Files         313      313              
  Lines       24644    24622      -22     
==========================================
- Hits        12385    12378       -7     
+ Misses      12259    12244      -15

Impacted Files	Coverage Δ
.../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h	`100% <ø> (ø)`	⬆️
...veit_jog_arm/include/moveit_jog_arm/jog_arm_data.h	`100% <ø> (ø)`	⬆️
...erimental/moveit_jog_arm/src/jog_cpp_interface.cpp	`11.9% <0%> (ø)`	⬆️
...veit_experimental/moveit_jog_arm/src/jog_calcs.cpp	`72.79% <72.6%> (+2.62%)`	⬆️
...nning_scene_monitor/src/planning_scene_monitor.cpp	`55.53% <0%> (+0.14%)`	⬆️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update b36d514...8372871. Read the comment docs.

henningkayser

I don't see anything wrong with this PR. However, I find it's increasingly hard to comprehend what's going on and that we should make an effort to simplify the control loop or at least make it more readable. +1 after comments are addressed

henningkayser · 2020-02-04T16:33:11Z

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

+    // Get the transform from MoveIt planning frame to jogging command frame
+    // We solve (planning_frame -> base -> robot_link_command_frame)
+    // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame)
+    tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() *


Does this really fix the issue or is it just much more unlikely to occur? To me it seems like there is still the possibility of a race condition when getCommandFrameTransform() is called before we have an updated joint state. Also, wouldn't it be better to compute tf_moveit_to_cmd_frame inside getCommandFrameTransform() in order to reduce redundant computations? I don't see the transform being used here anymore. Maybe we should additionally let getCommandFrameTransform() return bool or add a blocking function waitForValidJointState().

Line 94 ensures joint_state_ is updated before this, so no race condition.

~~Yeah, I guess it does make sense to calculate tf_moveit_to_cmd_frame inside the getter method.~~

I started doing this but realized that moving kinematic_state_->getGlobalLinkTransform() into jog_cpp_interface::getCommandFrameTransform() would also mean ensuring that kinematic_state_ is updated... and that would mean tracking the joint state, too. So it would require some big changes to move that calculation, I think.

However, I did notice that tf_planning_to_cmd_frame was redundant with tf_moveit_to_cmd_frame_ and could be deleted

I added an isInitialized() function and std::atomic<bool> is_initialized_ like you added previously for stop_requested_

I thought it would be enough to just access shared_variables.joints for getting the latest state. Anyway, this doesn't really belong into this PR and I'm fine with the current solution

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

henningkayser · 2020-02-04T16:37:35Z

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

-        shared_variables.outgoing_command = outgoing_command_;
-        shared_variables.ok_to_publish = true;
+        mutex.lock();
+        cartesian_deltas = shared_variables.command_deltas;


All these mutexes make the code unnecessarily hard to read. We should add thread safe accessor functions to shared_variables

OK, but in a different PR, right?

It should go a long way if you refactored JogArmShared into a class with mutexed setters and getters. That should ensure no race conditions can occur. Also note that some of the variables can just be converted into atomics.

Another thing I would recommend, is to use std::lock_guard<foo>. The thing about locking and unlocking is, that eventually you forget to unlock and there will be problems. Lock guard always unlocks the mutex at the end of scope so you can't really forget anything. You can also do this:

{ std::lock_guard<std::mutex> lock(mutex) // Do things here } // automatic unlocking.

In other words,, you set a temporary scope just to guide the lock_guard.

I'll work on this in a followup PR. This one's already pretty big

I'm going to approve this so we can move on to "fix mutexes" phase :)

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

henningkayser · 2020-02-04T17:52:29Z

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

-    // Initialize the position filters to initial robot joints
-    while (!updateJoints() && ros::ok() && !stop_requested_)
+    // Only do jogging calculations if the robot should move, for efficiency
+    if (stop_requested_)


Am I reading this wrong or does stop_requested_ now only mean that the positions are reset, but the loop is kept alive? That's not what stop_requested_ is supposed to do, we need an API to actually stop the loop as well. Stopping motions should be done by resetting the velocities in the command.

The robot will not move if stop_requested_ is true but the node will continue to make the end_effector_transform available (for closed loop controllers). It's a server. It does look like the usage of stop_requested_ needs updating in some places, though.

I think the way to exit from the loop is to destroy the object. Otherwise, we want it to continue to spin and provide the end effector transform (unless it was a mistake to put getEndEffectorTransform() here).

Over a year ago, the design decision was made that you shouldn't need to publish zeros to stop motion. Because
a) ROS1 messages might not arrive, or the network connection could die or something unexpected like that. So there's a timeout, for safety's sake.
b) if you're spamming zeros to the controller, you shut down other ways of moving the robot. People may have multiple motion control modes, not just jogging. E.g. trajectory control and joint jogging possibly could operate with the same ros_control controller

OK, after some thought I decided to do both things:

haltOutgoingJogCmds() to temporarily halt outgoing messages but continue updating the transform

stopMainLoop() to completely quit the loop

jliukkonen · 2020-02-04T22:25:44Z

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

-        shared_variables.outgoing_command = outgoing_command_;
-        shared_variables.ok_to_publish = true;
+        mutex.lock();
+        cartesian_deltas = shared_variables.command_deltas;


It should go a long way if you refactored JogArmShared into a class with mutexed setters and getters. That should ensure no race conditions can occur. Also note that some of the variables can just be converted into atomics.

Another thing I would recommend, is to use std::lock_guard<foo>. The thing about locking and unlocking is, that eventually you forget to unlock and there will be problems. Lock guard always unlocks the mutex at the end of scope so you can't really forget anything. You can also do this:

{ std::lock_guard<std::mutex> lock(mutex) // Do things here } // automatic unlocking.

In other words,, you set a temporary scope just to guide the lock_guard.

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

…gging Delete a redundant transform variable Add an isInitialized() function Add a method to temporarily pause message publication Use ros::Rate instead of constant sleeps in while loops Check for an uninitialized transform

AndyZe changed the title ~~Fix initial end effector transform jump~~ [jog_arm] Fix initial end effector transform jump Feb 2, 2020

tylerjw approved these changes Feb 3, 2020

View reviewed changes

henningkayser reviewed Feb 4, 2020

View reviewed changes

jliukkonen reviewed Feb 4, 2020

View reviewed changes

jliukkonen approved these changes Feb 5, 2020

View reviewed changes

AndyZe added 2 commits February 5, 2020 12:14

Fix the publishWarning() functionality

8372871

henningkayser approved these changes Feb 5, 2020

View reviewed changes

henningkayser merged commit dc70886 into moveit:master Feb 5, 2020

henningkayser mentioned this pull request Feb 17, 2020

Use incoming joint state for computing command frame transform #1862

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[jog_arm] Fix initial end effector transform jump #1871

[jog_arm] Fix initial end effector transform jump #1871

AndyZe commented Feb 2, 2020

codecov-io commented Feb 4, 2020 •

edited

Loading

henningkayser left a comment •

edited

Loading

henningkayser Feb 4, 2020

AndyZe Feb 4, 2020

AndyZe Feb 4, 2020 •

edited

Loading

AndyZe Feb 4, 2020

AndyZe Feb 4, 2020

henningkayser Feb 5, 2020

henningkayser Feb 4, 2020

AndyZe Feb 4, 2020

jliukkonen Feb 4, 2020 •

edited

Loading

AndyZe Feb 5, 2020

jliukkonen Feb 5, 2020 •

edited

Loading

henningkayser Feb 4, 2020 •

edited

Loading

AndyZe Feb 4, 2020 •

edited

Loading

AndyZe Feb 4, 2020 •

edited

Loading

AndyZe Feb 5, 2020

jliukkonen Feb 4, 2020 •

edited

Loading

[jog_arm] Fix initial end effector transform jump #1871

[jog_arm] Fix initial end effector transform jump #1871

Conversation

AndyZe commented Feb 2, 2020

codecov-io commented Feb 4, 2020 • edited Loading

Codecov Report

henningkayser left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

AndyZe Feb 4, 2020 • edited Loading

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

jliukkonen Feb 4, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jliukkonen Feb 5, 2020 • edited Loading

Choose a reason for hiding this comment

henningkayser Feb 4, 2020 • edited Loading

Choose a reason for hiding this comment

AndyZe Feb 4, 2020 • edited Loading

Choose a reason for hiding this comment

AndyZe Feb 4, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jliukkonen Feb 4, 2020 • edited Loading

Choose a reason for hiding this comment

codecov-io commented Feb 4, 2020 •

edited

Loading

henningkayser left a comment •

edited

Loading

AndyZe Feb 4, 2020 •

edited

Loading

jliukkonen Feb 4, 2020 •

edited

Loading

jliukkonen Feb 5, 2020 •

edited

Loading

henningkayser Feb 4, 2020 •

edited

Loading

AndyZe Feb 4, 2020 •

edited

Loading

AndyZe Feb 4, 2020 •

edited

Loading

jliukkonen Feb 4, 2020 •

edited

Loading