Add servo solver #2673
Add servo solver #2673
Conversation
There was a problem hiding this comment.
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
Show resolved
Hide resolved
moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp
Outdated
Show resolved
Hide resolved
| while (!trajectory_msg) | ||
| { | ||
| // Calculate next servo command | ||
| moveit_servo::KinematicState joint_state = servo_->getNextJointState(current_state, target_twist); |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
I am setting the (hopefully) correct publish_period in the initialize() function (see diff). Do you mind validating this?
There was a problem hiding this comment.
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
Show resolved
Hide resolved
moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp
Show resolved
Hide resolved
Codecov ReportAll modified and coverable lines are covered by tests ✅
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. |
| @@ -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> | |||
There was a problem hiding this comment.
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
Outdated
Show resolved
Hide resolved
moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp
Outdated
Show resolved
Hide resolved
| bool ServoSolver::reset() | ||
| { | ||
| RCLCPP_INFO(getLogger(), "Reset Servo Solver"); | ||
| joint_cmd_rolling_window_.clear(); |
There was a problem hiding this comment.
Does this get called on every iteration of of local planning or is it once per global plan?
There was a problem hiding this comment.
Only once per global plan. Would it make sense to reset the rolling window when the reference pose changes?
There was a problem hiding this comment.
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.
|
This pull request is in conflict. Could you fix it @sjahr? |
|
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
bot
added
stale
|
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
bot
added
the
stale
Description
Ports over the servo-based local planner from the UR welding demo
Checklist