Floating Joint Support in CurrentStateMonitor #748

v4hn · 2018-01-23T13:49:27Z

MoveIt supports Floating and Planar joints throughout the pipeline... almost throughout the pipeline.
Whenever someone actually tried to use it, they encountered problems with how to get the current state information into the MoveIt system. For over at least 4(!!!) years.

See among others here and here.

This works (rather hacked) for floating joints as root joint of the robot, but they are also important for online calibration procedures and mobile objects that are inherent to the setup and should be part of the urdf.

Turns out most things that are required to get this to work in general have been done a long time ago.
I simply implement a proposal by @hersh and add a tf callback that reads the state of all multi-dof joints from TF. This still does not allow to get velocities/effort readings into moveit, but it covers most everyday use-cases for multi-dof joints.
Also because it uses TF updates, it "just works" in practice and you don't have to explain the difference between transforms and joint states of floating joints to users. There simply isn't any.

This is mostly a generalization of the previous code. The only behavior that is supposed to change is that joint states for multi-dof joints are updated even if no JointState messages are received and are updated whenever a transform is published instead.

Please note that the class already subscribes to TF anyway (it is passed a tf-listener) and that I only lock the state if an actual update happens, so that most TF callbacks don't block the RobotState.

This breaks ABI, so it will not be back-ported to indigo.

Lastly please note the proposed code intentionally supports compiling on indigo.

Previously this was limited to the root link of the robot.

rhaschke

Nice improvement.
What do you think about changing the joint_time_ map, such that it uses JointModel ptrs instead of joint names? Former ones should be more faster comparable.

rhaschke · 2018-01-23T14:38:50Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

+    if (tf_ && tf_connection_)
+    {
+      tf_->removeTransformsChangedListener(*tf_connection_);
+      tf_connection_ = nullptr;


Better use tf_connection_.reset()?

rhaschke · 2018-01-23T14:43:44Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

-      tf::StampedTransform transf;
-      bool ok = false;
-      if (tf_->getLatestCommonTime(parent_frame, child_frame, tm, &err) == tf::NO_ERROR)
+      if (!tf_->getLatestCommonTime(parent_frame, child_frame, tm, &err) == tf::NO_ERROR)


Better write as != NO_ERROR

rhaschke · 2018-01-23T14:46:28Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

+
+      const std::vector<std::string>& joint_vars = joint->getVariableNames();
+      assert(joint_vars.size() > 0);
+      if (joint_time_[joint_vars[0]] != tm)


I think this should use the joint name instead of joint's variable name(s). This also resolves ambiguity, which one to use - why did you choose joint_vars[0]?

Is it safe to read-access joint_time_ without locking? A writing thread may change the map meanwhile...

of course you are right about locking.. fixed.

Also I adjusted the map as you proposed. Nice idea 👍
The previous implementation was correct though.
joint_vars[0] represents the first dof of the joint including the joint name prefix, e.g. joint_name/trans_x. I chose the first one as a representation for all dof of this joint.
It's much better to just store the time per joint instead and apparently it doesn't break anything else in MoveIt.

rhaschke · 2018-01-23T14:51:59Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

-        const std::vector<std::string>& vars = robot_model_->getRootJoint()->getVariableNames();
-        for (std::size_t j = 0; j < vars.size(); ++j)
-          joint_time_[vars[j]] = tm;
+        for (const std::string& var : joint_vars)


Here it suffices to use the joint name as well. If there are several floating joints, their joint vars will be identical and hence, the map will not be unique anymore...

rhaschke · 2018-01-23T14:53:32Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

+      if (joint_time_[joint_vars[0]] != tm)
+      {
+        // only lock if we actually update something
+        boost::mutex::scoped_lock _(state_update_lock_);


Instead of locking the state multiple times, I suggest to accumulate all TF updates in a map and apply them together.

makes sense, implemented.

rhaschke · 2018-01-23T14:55:59Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

+
+void planning_scene_monitor::CurrentStateMonitor::tfCallback()
+{
+  bool update = false;


I don't see a difference between update and changes.

The implementation of the monitor always triggers the update signal at the end of the JointState callback, because some thread might be waiting for the joint states at the current time.
But it calls the update_callbacks_ only if something actually changed (see here).

With the tf callback we can't even be sure the time of any joint updated and we don't want to trigger the notify unless we actually updated something.
This is represented in bool update.

bool changes represents whether something actually changed.
Because Multi-DOF joints tend to be rather noisy, I decided to use a threshold instead of just comparing the values.
Afair the update callbacks in MoveIt mark the planning scene as changed which can trigger safety stops in currently executing plans, re-validation of current plans, and publishing of the monitored_planning_scene topic. So unnecessary update callbacks can cause a lot of overhead.

Sorry, still don't get the difference: I guess you update something only if it has changed, don't you?

Got it now from reading the code: If there is only a tiny change, you apply this change, but you don't want to call the callbacks.

remove unused private method

doesn't matter really...

v4hn · 2018-01-23T18:38:57Z

Thanks @rhaschke .
I believe I addressed all your feedback.

rhaschke

Isn't it overkill to have four similar implementations of haveCompleState()?

v4hn · 2018-01-24T13:56:01Z

Thanks for the review.

Isn't it overkill to have four similar implementations of haveCompleState()?

I agree, but they are exposed API.
If you feel they should all use the same implementation, go ahead.
Personally I don't mind.

AustinDeric · 2018-10-11T21:12:50Z

@v4hn if i am to understand this correctly, the work flow is:

Make a floating joint between the robot base and world frame
update the transform between frames during run time using a TF publisher
The internal moveit scen should update

If so this, isn't working for us. I just wanted to make sure there is no other setup necessary besides floating joints and a tf publisher. We have a solution that updates the robot_description parameter and then forces moveit to update its internal model but we would like to use floating joints and TF.

rhaschke · 2018-10-12T06:21:31Z

Although this is merged, it's not yet released. Are you using a MoveIt source build?

v4hn · 2018-11-02T16:05:51Z

@v4hn if i am to understand this correctly, the work flow is:

That's the workflow, although you can actually have the floating joint anywhere, not just between base and world.
I used this a few weeks ago with a source build for a mobile base and it worked.

If you still encounter problems, please file questions at answers.ros.org

allow CSM to update any multi-dof joint via TF

a691350

Previously this was limited to the root link of the robot.

rhaschke requested changes Jan 23, 2018

View reviewed changes

v4hn added 2 commits January 23, 2018 19:17

CSM: change key of time map from string to JointModel*

a6bf4b5

remove unused private method

reset shared_ptr

0849444

doesn't matter really...

rhaschke approved these changes Jan 24, 2018

View reviewed changes

rhaschke merged commit 739a85a into moveit:kinetic-devel Jan 24, 2018

rhaschke mentioned this pull request Feb 7, 2018

fix newly introduced error #758

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Floating Joint Support in CurrentStateMonitor #748

Floating Joint Support in CurrentStateMonitor #748

v4hn commented Jan 23, 2018 •

edited

Loading

rhaschke left a comment

rhaschke Jan 23, 2018

v4hn Jan 23, 2018

rhaschke Jan 23, 2018

rhaschke Jan 23, 2018

rhaschke Jan 23, 2018

v4hn Jan 23, 2018

v4hn Jan 23, 2018

rhaschke Jan 23, 2018

rhaschke Jan 23, 2018

v4hn Jan 23, 2018

rhaschke Jan 23, 2018

v4hn Jan 23, 2018

rhaschke Jan 24, 2018

rhaschke Jan 24, 2018

v4hn commented Jan 23, 2018

rhaschke left a comment

v4hn commented Jan 24, 2018

AustinDeric commented Oct 11, 2018

rhaschke commented Oct 12, 2018

v4hn commented Nov 2, 2018

Floating Joint Support in CurrentStateMonitor #748

Floating Joint Support in CurrentStateMonitor #748

Conversation

v4hn commented Jan 23, 2018 • edited Loading

rhaschke 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

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

v4hn commented Jan 23, 2018

rhaschke left a comment

Choose a reason for hiding this comment

v4hn commented Jan 24, 2018

AustinDeric commented Oct 11, 2018

rhaschke commented Oct 12, 2018

v4hn commented Nov 2, 2018

v4hn commented Jan 23, 2018 •

edited

Loading