Assistant: load additional file with kinematics parameters #1997

simonschmeisser · 2020-03-30T12:06:27Z

Description

This adds an option in MoveIt setup assistant to setup an additional yaml file with parameters for the kinematic solver(s). The main motivation for this is to improve integration of opw_kinematics [1] via the moveit_opw_kinematics_plugin[2]. Currently the kinematic chain needs to be described in parameters in the kinematics.yaml file. This makes it inconvenient to reuse the information from outside MoveIt (e.g. descartes) as other setups typically have no access to the group names defined in SRDF. This and alternative approaches was discussed at length in [3].

The plan is to distribute a MoveIt - independent YAML file along with the URDF description as can be seen in [4]. The user would then select the YAML file distributed along with the URDF file by ROS-Industrial. A separate entry is added in launch/planning_scene.launch per group to load this parameter file.

Adding this feature would allow dropping ikfast generated code for ros-i repositories, significantly reducing maintenance effort by going from 1k+ lines of code per robot to 9 lines of parameters.

1: https://github.com/Jmeyer1292/opw_kinematics
2: https://github.com/JeroenDM/moveit_opw_kinematics_plugin
3: ros-industrial/fanuc#245
4: ros-industrial/fanuc#284

@gavanderhoorn @JeroenDM

As I'm currently running melodic-devel and would hope for this to be backported, this PR is against melodic-devel. Once discussed I can rebase against master if this is more convenient for whoever does the merge.

gavanderhoorn · 2020-03-30T12:17:44Z

Thanks for submitting this PR @simonschmeisser.

Just to make it extra clear: the functionality contributed is not OPW specific, correct? It is essentially a way to configure an additional .yaml file to be loaded, next to MoveIts default kinematics.yaml.

This mechanism could be used by / with other plugins as well, it seems?

simonschmeisser · 2020-03-30T12:31:14Z

This mechanism could be used by / with other plugins as well, it seems?

Yes of course, thanks for pointing this out. This is completely agnostic of course and could be used with other plugins that require configuration (eg caching_ik_plugin) or also for the likes of UR (and "clones" like Techman) that have an configurable analytical solver

moveit_setup_assistant/src/widgets/configuration_files_widget.cpp

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch

JeroenDM · 2020-03-30T13:55:17Z

Funny coincidence, I actually started implementing something to address the same problem this weekend and planned to post something about it today. :)

But you're approach is way more generic. I experimented with adding extra attributes to GroupMetaData in setup_assistant/tools/moveit_config_data.h. Their values are then loaded from the configuration file in the support package when running the setup assistant and added to the correct group in the kinematics.yaml file.

Does this mean we have to start thinking about releasing the plugin?

gavanderhoorn · 2020-03-30T16:12:24Z

Does this mean we have to start thinking about releasing the plugin?

With the current implementation, the extra file is completely optional. So there is no requirement for the OPW kinematics plugin to be present (or any plugin for that matter).

But from my point-of-view, I would say: yes, please release the plugin.

Don't let @davetcoleman know though: before you know it, he'll want to integrate it into ros-planning/moveit ;)

simonschmeisser · 2020-04-08T06:43:50Z

@v4hn I changed this to master in order to follow the normal workflow, could you please do a review or tag someone who could/should ;) @davetcoleman is probably too busy to even read this tag

codecov-io · 2020-04-15T10:41:23Z

Codecov Report

Merging #1997 into master will increase coverage by 0.02%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##           master    #1997      +/-   ##
==========================================
+ Coverage   54.04%   54.06%   +0.02%     
==========================================
  Files         319      319              
  Lines       24997    25030      +33     
==========================================
+ Hits        13509    13533      +24     
- Misses      11488    11497       +9

Impacted Files	Coverage Δ
.../moveit/setup_assistant/tools/moveit_config_data.h	`36.36% <ø> (ø)`
...t_setup_assistant/src/tools/moveit_config_data.cpp	`19.97% <0.00%> (-0.94%)`	⬇️
...veit_experimental/moveit_jog_arm/src/jog_calcs.cpp	`75.00% <0.00%> (+5.37%)`	⬆️
.../ompl_interface/src/detail/constrained_sampler.cpp	`64.10% <0.00%> (+17.94%)`	⬆️

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 4d30621...e4fb611. Read the comment docs.

rhaschke

Some remarks...

rhaschke · 2020-04-15T19:28:31Z

moveit_setup_assistant/src/tools/moveit_config_data.cpp

+  for (TiXmlElement* kinematics_parameter_file = kinematics_group->FirstChildElement("rosparam");
+       kinematics_parameter_file; kinematics_parameter_file = kinematics_parameter_file->NextSiblingElement("rosparam"))
+  {
+    const char* ns = kinematics_parameter_file->Attribute("ns");


I don't get the role of the ns attribute here, which doesn't exist in the template file:
https://github.com/ros-planning/moveit/blob/4d3062164bf03ebb74c57d5eba0e3e50438481ba/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch#L19-L22
Shouldn't you check for the existence of the file attribute instead?

the namespace is written in the block template "KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK" (which is a bit ugly because there is no proper templating library with for loops like grantlee ...) and contains the name of the JointModelGroup

rhaschke · 2020-04-15T19:29:23Z

moveit_setup_assistant/src/tools/moveit_config_data.cpp

+  // find the kinematics section
+  TiXmlHandle doc(&launch_document);
+  TiXmlElement* kinematics_group = doc.FirstChild("launch").FirstChild("group").ToElement();
+  if (!kinematics_group)


The check and warning here are somewhat superfluous given the other check below.

rhaschke · 2020-04-15T19:30:39Z

moveit_setup_assistant/src/tools/moveit_config_data.cpp

+bool MoveItConfigData::extractPackageNameFromPath(const std::string& path, std::string& package_name,
+                                                  std::string& relative_filepath) const
+{
+  // Get the path to urdf, save filename


Adapt the comment?

rhaschke · 2020-04-15T19:32:15Z

moveit_setup_assistant/src/tools/moveit_config_data.cpp

+  std::vector<std::string> path_parts;  // holds each folder name in vector
+
+  // Copy path into vector of parts
+  for (fs::path::iterator it = file_directory.begin(); it != file_directory.end(); ++it)


Use range-based for?

rhaschke · 2020-04-15T19:35:13Z

moveit_setup_assistant/src/tools/moveit_config_data.cpp

+      sub_path /= path_parts[segment_count];
+
+      // decide if we should remember this directory name because it is topmost, in case it is the package/stack name
+      if (segment_count == segment_length - 1)


Instead of checking this condition in the loop, you can move the assignment out from the loop:
package_name = path_parts[segment_length - 1]

rhaschke · 2020-04-15T19:50:03Z

moveit_setup_assistant/src/widgets/configuration_files_widget.cpp

@@ -1118,6 +1121,18 @@ void ConfigurationFilesWidget::loadTemplateStrings()
    addTemplateString("[ROS_CONTROLLERS]", controllers.str());
  }

+  std::string kinematics_parameters_files_block;


Can you add a comment to this section as for the others?

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch

rhaschke · 2020-04-15T19:57:28Z

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch

@@ -19,6 +19,7 @@
  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
  <group ns="$(arg robot_description)_kinematics">
    <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/kinematics.yaml"/>
+    [KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]


See below

Suggested change

[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]

[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]

moveit_setup_assistant/src/widgets/configuration_files_widget.cpp

rhaschke

Thanks a lot for this great simplification of the (old) code. This makes it much more readable.

rhaschke · 2020-04-16T13:41:35Z

moveit_setup_assistant/src/tools/moveit_config_data.cpp

-    // Create a subpath based on the outer loops length
-    for (int segment_count = 0; segment_count < segment_length; ++segment_count)
+    ROS_DEBUG_STREAM("checking in " << sub_path.make_preferred().string());
+    if (fs::is_regular_file(sub_path / "package.xml") || fs::is_regular_file(sub_path / "manifest.xml"))


Didn't you want to drop support for manifest.xml? I would strongly support this.

v4hn · 2020-04-23T11:09:12Z

Thanks for your work @simonschmeisser !
I'm glad we have you providing such nice additions and seeing them through. 😼

simonschmeisser · 2020-04-23T11:11:59Z

I'd love to see this backported to melodic as well, what would I need to do for that?

And thanks for reviewing and merging!

v4hn · 2020-04-23T11:27:12Z

File a backport PR or wait for someone (we'll talk about it at the meeting later) to look through all new patches in master and backport everything as needed). :-)

add extra field kinematics_parameters_file add kinematics parameters files in launchfiles read in kinematics_parameter_files from planning_context.launch add button and file dialog to select file resolve choosen file to relative file move template cache invalidation to beginning of export - cache has to be cleaned before *each* export, not just the first Otherwise changes will not be included in the second run stop loading kinematic parameters in run_benchmark_ompl.launch - It's redundant there Remove support for manifest.xml - obsolete since ... indigo?

davetcoleman · 2020-06-02T05:39:29Z

@davetcoleman is probably too busy to even read this tag

haha! i love these remarks. i catch up on my github emails every World MoveIt Day...

v4hn reviewed Mar 30, 2020

View reviewed changes

moveit_setup_assistant/src/widgets/configuration_files_widget.cpp Outdated Show resolved Hide resolved

v4hn reviewed Mar 30, 2020

View reviewed changes

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch Outdated Show resolved Hide resolved

v4hn reviewed Mar 30, 2020

View reviewed changes

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch Outdated Show resolved Hide resolved

simonschmeisser force-pushed the assistant-kin-param-file branch from 192d52f to 24140ac Compare March 30, 2020 16:05

simonschmeisser force-pushed the assistant-kin-param-file branch from 37dcd53 to c5687f6 Compare March 31, 2020 16:06

simonschmeisser changed the base branch from melodic-devel to master April 8, 2020 06:23

simonschmeisser changed the base branch from master to melodic-devel April 8, 2020 06:25

simonschmeisser force-pushed the assistant-kin-param-file branch from be3c1e5 to 9a14e19 Compare April 8, 2020 06:40

simonschmeisser changed the base branch from melodic-devel to master April 8, 2020 06:41

simonschmeisser added 10 commits April 15, 2020 10:05

add extra field kinematics_parameters_file

6b784cb

add kinematics parameters files in launchfiles

734ff36

clang format

9af0426

read in kinematics_parameter_files from planning_context.launch

4e9d8bb

add button and file dialog to select file

31e5ba3

resolve choosen file to relative file

0bf0db2

move template cache invalidation to beginning of export

a6b3eb4

fix generated roslaunch and remove unneeded params

cc53407

also fix loading

acaaf88

stop loading kinematic parameters in run_benchmark_ompl.launch

12aa7d7

simonschmeisser force-pushed the assistant-kin-param-file branch from 9a14e19 to 12aa7d7 Compare April 15, 2020 08:06

simonschmeisser requested a review from rhaschke as a code owner April 15, 2020 08:06

fix clang-tidy issue

e4fb611

rhaschke reviewed Apr 15, 2020

View reviewed changes

remove superflous check

2ce895c

simonschmeisser added 2 commits April 16, 2020 12:46

simplify MoveItConfigData::extractPackageNameFromPath

9b9beb5

comment and fix formatting of planning_context.launch

c8f39b6

rhaschke approved these changes Apr 16, 2020

View reviewed changes

Remove support for manifest.xml

40b9ecf

rhaschke added the awaits 2nd review one maintainer approved this request label Apr 16, 2020

simonschmeisser mentioned this pull request Apr 18, 2020

Release? Jmeyer1292/opw_kinematics#39

Closed

JeroenDM mentioned this pull request Apr 18, 2020

Load the opw parameters in the new way JeroenDM/kuka_test_resources#2

Merged

v4hn merged commit 2a98624 into moveit:master Apr 23, 2020

simonschmeisser mentioned this pull request Apr 24, 2020

[backport to melodic] Assistant: load additional file with kinematics parameters (#1997) #2035

Merged

gavanderhoorn mentioned this pull request May 5, 2020

Add opw_kinematics as a git submodule JeroenDM/moveit_opw_kinematics_plugin#48

Merged

JeroenDM mentioned this pull request Aug 13, 2020

Add support for constants/symbolic values (offsets, geometric parameters) JeroenDM/moveit_opw_kinematics_plugin#67

Closed

gavanderhoorn mentioned this pull request Jan 27, 2021

OPW kinematic parameters ros-industrial/fanuc_experimental#61

Closed

mlautman mentioned this pull request Feb 25, 2021

Default Planning Groups Image (and _Kin. parameters file_ field) moveit/moveit_tutorials#531

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Assistant: load additional file with kinematics parameters #1997

Assistant: load additional file with kinematics parameters #1997

simonschmeisser commented Mar 30, 2020

gavanderhoorn commented Mar 30, 2020

simonschmeisser commented Mar 30, 2020

JeroenDM commented Mar 30, 2020

gavanderhoorn commented Mar 30, 2020 •

edited

Loading

simonschmeisser commented Apr 8, 2020

codecov-io commented Apr 15, 2020

rhaschke left a comment

rhaschke Apr 15, 2020

simonschmeisser Apr 15, 2020

rhaschke Apr 15, 2020 •

edited

Loading

rhaschke Apr 15, 2020

rhaschke Apr 15, 2020

rhaschke Apr 15, 2020

rhaschke Apr 15, 2020

rhaschke Apr 15, 2020 •

edited

Loading

rhaschke left a comment

rhaschke Apr 16, 2020

v4hn commented Apr 23, 2020

simonschmeisser commented Apr 23, 2020 •

edited

Loading

v4hn commented Apr 23, 2020 via email

davetcoleman commented Jun 2, 2020

	[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]
	[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]

Assistant: load additional file with kinematics parameters #1997

Assistant: load additional file with kinematics parameters #1997

Conversation

simonschmeisser commented Mar 30, 2020

Description

gavanderhoorn commented Mar 30, 2020

simonschmeisser commented Mar 30, 2020

JeroenDM commented Mar 30, 2020

gavanderhoorn commented Mar 30, 2020 • edited Loading

simonschmeisser commented Apr 8, 2020

codecov-io commented Apr 15, 2020

Codecov Report

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Apr 15, 2020

Choose a reason for hiding this comment

simonschmeisser Apr 15, 2020

Choose a reason for hiding this comment

rhaschke Apr 15, 2020 • edited Loading

Choose a reason for hiding this comment

rhaschke Apr 15, 2020

Choose a reason for hiding this comment

rhaschke Apr 15, 2020

Choose a reason for hiding this comment

rhaschke Apr 15, 2020

Choose a reason for hiding this comment

rhaschke Apr 15, 2020

Choose a reason for hiding this comment

rhaschke Apr 15, 2020 • edited Loading

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Apr 16, 2020

Choose a reason for hiding this comment

v4hn commented Apr 23, 2020

simonschmeisser commented Apr 23, 2020 • edited Loading

v4hn commented Apr 23, 2020 via email

davetcoleman commented Jun 2, 2020

gavanderhoorn commented Mar 30, 2020 •

edited

Loading

rhaschke Apr 15, 2020 •

edited

Loading

rhaschke Apr 15, 2020 •

edited

Loading

simonschmeisser commented Apr 23, 2020 •

edited

Loading