Add servo solver plugin #16
Conversation
| /** | ||
| * @brief Initializes moveit servo | ||
| * | ||
| * @param node |
There was a problem hiding this comment.
Maybe you could add a small description to node. For example, I am unsure if the node needs to be spinning to be used, but a comment such as "node is used to run servo's ROS callbacks". Maybe that would be more clear.
| Eigen::Isometry3d diff_pose = current_pose.inverse() * target_pose; | ||
| Eigen::AngleAxisd axis_angle(diff_pose.linear()); | ||
|
|
||
| constexpr double fixed_trans_vel = 0.05; |
There was a problem hiding this comment.
Do you think it would make sense to make these into parameters?
| 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.
| } | ||
|
|
||
| // Get current state | ||
| const auto current_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); |
There was a problem hiding this comment.
If you are certain that the current state is actually current, then I would add it into the command queue as the first state.
|
|
||
| std::optional<trajectory_msgs::msg::JointTrajectory> trajectory_msg; | ||
| // Create servo commands until a trajectory message can be generated | ||
| while (!trajectory_msg) |
There was a problem hiding this comment.
Before beginning the loop, delete any values in the command queue that are in than 1 update period in the past. So if the local planner is called at 100 Hz, then keep values as long as they are not older than 0.01 seconds.
There was a problem hiding this comment.
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.
sjahr
force-pushed
the
pr-add_servo_solver
branch
from
674e10f
to
881e55c
Compare
Description
This PR depends on moveit#2603 and will be redirected to the main repos once it is merged
Checklist