Add servo solver #2673

sjahr · 2024-01-31T16:21:03Z

Description

Ports over the servo-based local planner from the UR welding demo

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

sjahr

Porting over @pac48's review from sjahr#16

Overall looks good! I added a few comments in the main local planner solve function on how to possibly use servo's command queue more effectively. One other thing, it may be too much work to set up, but this draft PR to ros2_control shows how to add data_tamer to log the values sent to the hardware interface ros-controls/ros2_control#1255. If the command queue is setup correctly, you should see very smooth motion.

...al_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp

sjahr · 2024-01-31T16:23:02Z

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp

+  while (!trajectory_msg)
+  {
+    // Calculate next servo command
+    moveit_servo::KinematicState joint_state = servo_->getNextJointState(current_state, target_twist);


This point may cause issues if not handled with caution. Servo has a parameter called publish_period which is the time difference used for numerical integration. So if you ask servo to move at a velocity of 1 m/s and publish_period was .1 sec, then servo will return a new position that correspondence to a .1 m offset from the current position. So you should consider setting a faction of local trajectory length. For example, if you are supposed to arrive at your next point in .3 sec, then the update period should be .3/n, where n is the number of points you want in the command queue. I would recommend n=3.

I am setting the (hopefully) correct publish_period in the initialize() function (see diff). Do you mind validating this?

This is correct as long as this loop always gets called three times. I think you should replace the whole loop with a for loop of length 3 to make sure this is the case. Then check at the end to make sure the message is not empty.

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp

codecov · 2024-01-31T16:33:57Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 50.73%. Comparing base (d962501) to head (881e55c).
Report is 30 commits behind head on main.

❗ Current head 881e55c differs from pull request most recent head 975b94a. Consider uploading reports for the commit 975b94a to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2673      +/-   ##
==========================================
- Coverage   50.74%   50.73%   -0.00%     
==========================================
  Files         392      392              
  Lines       32553    32553              
==========================================
- Hits        16517    16514       -3     
- Misses      16036    16039       +3

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

henningkayser · 2024-02-01T14:09:23Z

...ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp

@@ -36,6 +36,7 @@
 #include <moveit/local_planner/feedback_types.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/robot_state/conversions.h>
+#include <forward_trajectory_parameters.hpp>


Is it time to do a repo-wide refactoring for .hpp file type extensions?

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp

pac48 · 2024-02-05T15:21:19Z

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp

+bool ServoSolver::reset()
+{
+  RCLCPP_INFO(getLogger(), "Reset Servo Solver");
+  joint_cmd_rolling_window_.clear();


Does this get called on every iteration of of local planning or is it once per global plan?

Only once per global plan. Would it make sense to reset the rolling window when the reference pose changes?

That is interesting. I thought it may have been called on each iteration. In that case, won't the joint_cmd_rolling_window_ queue grow indefinitely as solve continues to be called? In that case, what I would do is remove old commands from joint_cmd_rolling_window_ in the solve method. Then I think you can set the timestep of servo queal to the time step of the local planner.

pac48

@sjahr I added a few more comments

mergify · 2024-02-13T17:56:52Z

This pull request is in conflict. Could you fix it @sjahr?

github-actions · 2024-04-01T12:09:32Z

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

github-actions · 2024-05-21T12:36:08Z

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

mergify · 2024-10-17T12:48:02Z

This pull request is in conflict. Could you fix it @sjahr?

sjahr · 2024-10-30T14:11:51Z

Closing this due to not having time to work further on this

sjahr added 3 commits January 31, 2024 17:17

Add servo solver plugin

188f17c

Cleanup + add description

9ffda7a

Small update

881e55c

sjahr commented Jan 31, 2024

View reviewed changes

henningkayser reviewed Feb 1, 2024

View reviewed changes

Address review

96160da

sjahr requested review from pac48 and henningkayser February 5, 2024 10:44

Merge branch 'main' into pr-add_servo_solver

975b94a

pac48 reviewed Feb 5, 2024

View reviewed changes

github-actions bot added stale Inactive issues and PRs are marked as stale and may be closed automatically. and removed stale Inactive issues and PRs are marked as stale and may be closed automatically. labels Apr 1, 2024

github-actions bot added the stale Inactive issues and PRs are marked as stale and may be closed automatically. label May 21, 2024

github-actions bot removed the stale Inactive issues and PRs are marked as stale and may be closed automatically. label Oct 17, 2024

sjahr closed this Oct 30, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add servo solver #2673

Add servo solver #2673

sjahr commented Jan 31, 2024

sjahr left a comment

sjahr Jan 31, 2024

sjahr Feb 5, 2024

pac48 Feb 5, 2024

codecov bot commented Jan 31, 2024 •

edited

Loading

henningkayser Feb 1, 2024

sjahr Feb 5, 2024

pac48 Feb 5, 2024

sjahr Feb 6, 2024

pac48 Feb 6, 2024

pac48 left a comment •

edited

Loading

mergify bot commented Feb 13, 2024

github-actions bot commented Apr 1, 2024

github-actions bot commented May 21, 2024

mergify bot commented Oct 17, 2024

sjahr commented Oct 30, 2024

Add servo solver #2673

Add servo solver #2673

Conversation

sjahr commented Jan 31, 2024

Description

Checklist

sjahr 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

codecov bot commented Jan 31, 2024 • 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

pac48 left a comment • edited Loading

Choose a reason for hiding this comment

mergify bot commented Feb 13, 2024

github-actions bot commented Apr 1, 2024

github-actions bot commented May 21, 2024

mergify bot commented Oct 17, 2024

sjahr commented Oct 30, 2024

codecov bot commented Jan 31, 2024 •

edited

Loading

pac48 left a comment •

edited

Loading