Pilz Industrial Motion Planner blend generates points with same time_from_start #2741
Comments
|
Hey, !!BE CAREFUL before you test this script with a real robot, In my case not only the time was off, but the velocity and acceleration at these points too, so the robot moves in an abrupt and jittery way when transitioning!! In the MoveIt Humble Docs I found 'Time Parameterization' and the Time-Optimal Parameter Generation method which fixed time, acceleration and velocity for me. |
|
First of all thank your proposed solution and heads-up. I haven't tested this on a real robot yet, though this is something that I will make a priority because of your experience.
The problem with this solution is that adding the I might have to look into other ways of smoothing the trajectory. |
|
Oh yeah, forgot to mention that the adapter didn't work for me either. I had to manually create a class object of |
|
@AGuthmann could you be so kind to share a snippet of your code showcasing the approach you took? I am facing same problem, in my case , the code is in C++. |
|
sure, here you go: #include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_core/moveit/trajectory_processing/time_optimal_trajectory_generation.h>
void optimize_trajectory(moveit_msgs::msg::RobotTrajectory &trajectory, double max_velocity_scaling_factor, double max_acceleration_scaling_factor){
moveit::core::RobotState robot_state(move_group_interface_->getRobotModel());
robot_trajectory::RobotTrajectory robot_trajectory(move_group_interface_->getRobotModel(), robot_state.getJointModelGroup(PLANNING_GROUP)->getName());
robot_trajectory.setRobotTrajectoryMsg(robot_state, trajectory); // convert moveit_msgs::msg::RobotTrajectory into robot_trajectory::RobotTrajectory
trajectory_processing::TimeOptimalTrajectoryGeneration totg; // Create Optimization Object
totg.computeTimeStamps(robot_trajectory, 0.1, 0.1); // First optimization with default scalings
totg.computeTimeStamps(robot_trajectory,max_velocity_scaling_factor, max_acceleration_scaling_factor); // Second optimization with user chosen scalings
RCLCPP_INFO(logger_, "First Iteration: %d, Second Iteration: %d", first, second);
robot_trajectory.getRobotTrajectoryMsg(trajectory); // convert robot_trajectory::RobotTrajectory back to moveit_msgs::msg::RobotTrajectory
}I had to optimize twice because sometimes longer trajectories weren't optimized at all. Couldn't figure out where this comes from... Additionally if you have 180° turns in your trajectory the optimization will probably fail, because these aren't implemented / supported yet. For my tests it was sufficient to offset the Points before or after the 180° turn by 1mm perpendicular to the movement of the trajectory. |
|
I have tested the 'dirty solution' on a real robot and it indeed jitters a lot at these points. I also found that Pilz normally creates points with an interval of 0.1 seconds, so i opted to offset the point with 0.1 seconds instead of 1 ns and it works very smoothly. def shift_robottrajectory(trajectory: RobotTrajectory) -> None:
time_shift_ns = 0
for index, point in enumerate(trajectory.joint_trajectory.points):
if index == 0:
continue
point.time_from_start.nanosec += time_shift_ns
if trajectory.joint_trajectory.points[index-1].time_from_start == point.time_from_start:
# Not sure if this works for seconds rollover but I didn't have any problem
point.time_from_start.nanosec += int(1e8)
time_shift_ns += int(1e8) |
|
Thank you @AGuthmann . |
|
@TheSpeedM just so I understand correctly: offsetting the point with identical @ilijaljubicic you're welcome! |
|
@AGuthmann yes you're correct. The Pilz planner generates points at a 0.1s interval, except at the start and end of a blend. So the 'wrong' points - with the same time as the previous points - of a blend have to be offset by 0.1s and all of the following points have to be shifted too (so they won't overlap). This shifting adds up the more points there have to be corrected, see the diagram below.
|
|
This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups. |
github-actions
bot
added
the
stale
|
Hi @TheSpeedM. Im trying to reproduce your steps but the code gets stuck on Indeed, a I am using |
|
Yes, it is #2707 |
|
I have encountered the same issue as well. There is an ongoing issue at Moveit1 here moveit/moveit#3583. I could solve it by doing the bypass commented here moveit/moveit#3583 (comment) and making some changes for ROS2: // Ensure time_from_start is strictly increasing
double epsilon = 0.01;
for (size_t i = 1; i < plan.trajectory_.joint_trajectory.points.size(); ++i)
{
auto& current_point = plan.trajectory_.joint_trajectory.points[i];
auto& prev_point = plan.trajectory_.joint_trajectory.points[i - 1];
if (current_point.time_from_start == prev_point.time_from_start)
{
// Shift current point by 0.1 seconds (100 milliseconds)
current_point.time_from_start.sec = prev_point.time_from_start.sec;
current_point.time_from_start.nanosec += 100000000;
RCLCPP_WARN(this->get_logger(), "Adjusted time_from_start to be strictly increasing at point %zu", i);
}
}I do still rarely encounter the issues described in this moveit/moveit#3553 with accelerations when working on the real robot only. Definitely solving that issue and being able to backport to MoveIt2 would be appreciated. |
github-actions
bot
removed
the
stale
|
@TheSpeedM I just ran into this and think I have a band-aid fix in #2943 -- want to give it a try? EDIT: I fixed it for real in #2961 |
|
Commenting here to keep it in the same place, else I can open an issue if more convenient. Having a backport of #2961 would be greatly appreciated since I have not seen it in https://github.com/moveit/moveit2/commits/humble/. Another issue that may be or not related to this comes from MoveIt1: moveit/moveit#3553. I have yet to test this in the real robot with the changes made once the backport to humble binaries are made but I did find that PILZ was ignoring the limits set and moreover giving me an error when accelerations and velocities were wrong. If I test it in the real robot and find the same issue I could open a clone of the issue in MoveIt2 for traceability. Thanks in advance. |
|
Thanks for the reminder @Danilrivero -- mergify wasn't working properly at the time of this PR. |
Description
Using
pilz_industrial_motion_plannerto generate a motionsequence with a blend radius generates (multiple) points with the same value forRobotTrajectory.joint_trajectory.points[].time_from_start. Looks like it generates two pairs of wrong points per point with a blend.It does generate and execute the trajectory when blend radius is set to 0.0. Had the problem using the Universal_Robots_ROS2_Driver and was able to reproduce it with the MoveIt example robot (used for this bug report).
Also ran into this issue from MoveIt1 while trying to reproduce. Looks related.
Your environment
Steps to reproduce
Send MoveGroupSequence goal to
/sequence_move_groupwith a blend radius using the following Python code.Expected behaviour
Generate and execute a trajectory through these points with them blended.
Actual behaviour
Generates the points and sends them to executor, but that throws an error.
Backtrace or Console output
Dirty fix
Set
plan_onlyto true and pass the planned_trajectories to the following function before execution. This shifts the generated points with the same time away from eachother by one ns. Trajectory does execute when doing this.The text was updated successfully, but these errors were encountered: