[DWB] Question about StandardTrajectoryGenerator::generateTrajectory in ROS2

[oym1994]

dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(
  const geometry_msgs::msg::Pose2D & start_pose,
  const nav_2d_msgs::msg::Twist2D & start_vel,
  const nav_2d_msgs::msg::Twist2D & cmd_vel)
{
  dwb_msgs::msg::Trajectory2D traj;
  traj.velocity = cmd_vel;
  //  simulate the trajectory
  geometry_msgs::msg::Pose2D pose = start_pose;
  nav_2d_msgs::msg::Twist2D vel = start_vel;
  double running_time = 0.0;
  std::vector<double> steps = getTimeSteps(cmd_vel);
  traj.poses.push_back(start_pose);
  for (double dt : steps) {
    //  calculate velocities
    vel = computeNewVelocity(cmd_vel, vel, dt);

    //  update the position of the robot using the velocities passed in
    pose = computeNewPosition(pose, vel, dt);

    traj.poses.push_back(pose);
    traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
    running_time += dt;
  }  //  end for simulation steps

  if (include_last_point_) {
    traj.poses.push_back(pose);
    traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
  }

  return traj;
}

Here why set traj.velocity = cmd_vel, instead set traj.velocity to be the first sampling velocity in the for loop? The experiment shows that the result velocity may be against the acceleration limit.

[SteveMacenski]

In general, don't copy paste code, just give a link to it in the github interface for viewing.

I don't see any issue with this. Its starting from the initial (eg current) velocity and only using the traj.velocity / cmd_vel for defining the maximum velocity to achieve over the course of the trajectory using the proper acceleration limits established in computeNewVelocity. The traj.velocity is mostly used for storage for scoring and recovering out the actual cmd_vel that generated the trajectory that was rolled out in the loop you provide the code for.

This trajectory looks correct to me, unless you had a specific change you wanted to suggest that would change my view of things. Note that I didn't write any of this so I'm just a 3rd party like you looking at this - so please correct me if you see something problematic, but I don't.

[oym1994]

Thank for your kind reply.
It will not influence the trajectory, just influence the velocity result. In my experiment, if the robot start to move, it may give
a large velocity at the beginning, which may conflict with the acceleration limits. Then I changed to use the first velocity of the trajectory, then it becomes OK. While by using "traj.velocity = cmd_vel;", it equals to use the last velocity of the trajectory.

[SteveMacenski]

Cmd_vel is the velocity represented by the trajectory to be scored. We can see where the current speed is used to integrate towards the desired cmd_vel in the code. So I think you need to show some proof of that claim based on the acceleration parameters set by the user

[oym1994]

Thank u again!
Below is a comparison:
Original logic where the target velocity is utilized:

Let the cmd_vel equal to the first velocity of the sampling trajectory:

some related configurations:
max_speed_xy: 1.5
max_vel_x: 1.5
max_vel_y: 0.0
max_vel_x_backwards: 0.2
acc_lim_x: 3.0
acc_lim_y: 0.0
acc_lim_theta: 3.0
sim_time: 2.0

From the velocity and cmd_vel curve, we can see that with the original logic, the velocity would increase rapidly(especially at the beginning), which may make the acceleration to exceed the configuration.

[SteveMacenski]

Can you show numerical evidence that it exceeds acceleration limits? I again looked at the code and again see no error.

The code appears to clearly take the current speed and march up via the maximum acceleration as it attempts to achieve the cmd_vel command.

[oym1994]

At the beginning, the linear speed is 0（red curve）, and the output cmd_vel.linear.x(light green curve) is about 1.3m/s, which means it want to increase the speed from 0 to 1.3 at once. The same to the cmd_vel.angular.z and angular speed (blue and purple).
From the code logic, the target velocity is the final velocity during the sim_time. It is valid to go to the target velocity in sim_time(such as 2s). But it may be invalid in control duration dt( such as 0.1 s).

[SteveMacenski]

I see the first cycle achieving ~0.3m/s (guestimating on your graph image) in the first cycle while the odometry is reading 0. You mention before that your acceleration is set to 3 and your time dt is 0.1. v = v0 + at tells me that the max achievable is 0.3m/s, which seems to check out.

Then it stays flat at that same ~0.3m/s (which is probably exactly 0.3m/s if you had the direct data) as long as the odometry is at 0m/s and then a little bit after for the controller to catch up (that delay in cmd_vel at 1347) for which then the commanded velocity spikes complimentary. Lets say that the first peak just after we move from ~0.3m/s is ~0.65m/s and the odometry was reading ~0.45m/s.

v = v0 + at says that we can achieve 0.45m/s + 0.1 * 3 = 0.95, for which the ~0.65 m/s is well within.

Again, even analyzing your data, this all looks absolutely correct to me.

is about 1.3m/s, which means it want to increase the speed from 0 to 1.3 at once

1.3m/s is trivial to achieve in 1s when you have your acceleration set to 3.0m/s^2. It seems to be more that your acceleration parameters are unrealistically high for what you're expecting behaviorally.

[oym1994]

I think the graph u mentioned is the second one. I have modified a logic here as below. Using the original logic, u can get the first graph, where u can see rapid linear and rotation speed acceleration.

dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(
  const geometry_msgs::msg::Pose2D & start_pose,
  const nav_2d_msgs::msg::Twist2D & start_vel,
  const nav_2d_msgs::msg::Twist2D & cmd_vel)
{
  dwb_msgs::msg::Trajectory2D traj;
  traj.velocity = cmd_vel;                                                                 ---delete this line
  //  simulate the trajectory
  geometry_msgs::msg::Pose2D pose = start_pose;
  nav_2d_msgs::msg::Twist2D vel = start_vel;
  double running_time = 0.0;
  std::vector<double> steps = getTimeSteps(cmd_vel);
  traj.poses.push_back(start_pose);
  bool getFirstVel = false;                                                                ---add this line
  for (double dt : steps) {
    //  calculate velocities
    vel = computeNewVelocity(cmd_vel, vel, dt);
    if(!getFirstVel ){                                                                     ---add these lines
      getFirstVel  = true;
      traj.velocity = vel;
    }
    //  update the position of the robot using the velocities passed in
    pose = computeNewPosition(pose, vel, dt);

    traj.poses.push_back(pose);
    traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
    running_time += dt;
  }  //  end for simulation steps

  if (include_last_point_) {
    traj.poses.push_back(pose);
    traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
  }

  return traj;
}

[SteveMacenski]

traj.velocity must be the value of the cmd_vel that is requested to be scored and generated because its recovered here to send that trajectory to the base after scoring. The role of the function generateTrajectory() is to take in a proposed command velocity that should already be in the dynamic window of feasible velocities from the current velocity (via the trajectory generator itself) and roll it out into poses by dynamic limits (e.g. computeNewVelocity()). It is not the role of this function to change this velocity.

Its understood that in the immediate time-step its not guaranteed to be drivable, that's why computeNewVelocity() rolls out using the acceleration limits to converge to the requested cmd_vel over the course of projecting the future trajectory that would be generated if that command was sent for that time period. The trajectory generator using the iterators finds the max/min possible velocities achievable over the simulation time set (default 1.7s) not the dt time-step of the planner's execution to generate the trajectories over in the dynamic window possible of future simulation time. So they're all dynamically achievable within that simulation time. So if you get a cmd_vel of 1.3m/s from 0, its because over that sim_time, it can be achieved. Its a bit unintuitive because the trajectory is baking in already that the vehicle won't go from 0-1.3m/s in a single timestep, but take some time as set by your acceleration characteristics of your platform to achieve it.

So you bring up potentially a good-faith discussion on what is the most "reasonable" behavior. On one hand, current behavior isn't technically incorrect, because it assumes what you've set is your actual dynamic limits of your vehicle so if you set 1.3m/s from 0, it will take some N cycles to get there, which were modeled in the generateTrajectory() function via computeNewVelocity(). Or optionally you have a velocity smoother with your limits set (e.g. yocs or nav2's) so when you set 1.3m/s, its being interpolated properly and dishing out the incremental commands over the sim_time to achieve that.

On the other hand, if you're using DWB to try to artificially limit the acceleration of the platform for the purposes of tuning the behavior and you're not using a velocity smoother, then your robot will shoot up faster than your set limits because the platform supports higher accelerations than the parameters suggested and there's not intermediary node to dish out incremental commands. In that situation, something like what you suggest seems reasonable on first glance. What I'd want is some data-based analysis showing that the acceleration used is that from the settings because my intuition tells me that doing it this way would actually result in further delays and slowing beyond that at which is the acceleration properties.

If we find that your solution does indeed limit to the acceleration correctly, then I think it would be reasonable to create a parameter to allow a user to select what they would prefer to use. If someone has a velocity smoother setup, actually sending the higher value could be beneficial for smoother motion since they can have that run at a higher rate (e.g. > 1kHz) to interpolate smoothly. If you didn't have something like that though and you were trying to model the robot's behavioral acceleration below at which its technically able to achieve, then having the option to send the next-time-step-feasible velocity of a proposed trajectory would be valuable as you describe.

Thanks for the analysis and working through this. It took me a hot minute (obviously) to see what you mean. @DLu do you have thoughts here? I assume you have it this way because you have a velocity smoother shimmed between the controller and base by default and also now the default of Nav2 as well.

[oym1994]

Thanks for your patient and kind response. Hope this analysis could really help!

[SteveMacenski]

Can you show that your changes are numerically in line with the set accelerations? I want to make sure this doesn’t result in undershooting.

[oym1994]

Sorry for late response. What kind of evidence do u need? May the second curve chart prove that?

[SteveMacenski]

Yeah, running on some robots and making sure that the acceleration that the robot is achieving is in line with the parameter being set (e.g. isn't substantially undershooting). Something like a robot from stand still to fill speed forward and checking the acceleration on the plot with the setting. Then change the settings of acceleration to a couple of different things just to make sure the trend holds

[SteveMacenski]

Hey! Just following up here after some time, did you get a chance to test / verify?

[oym1994]

Hey! Just following up here after some time, did you get a chance to test / verify?

Hi, here is a comparison.

The acc_lim_x is set to be 1.0

Original Version

Improved Version

The acc_lim_x is set to be 3.0

Original Version

Improved Version

It proves that the improved logic could provide a better result while the acceleration of the original version always exceed the set one!

[SteveMacenski]

Great! Can you submit the PR to add this behavior as an option! I appreciate very much your follow up here

[oym1994]

Ok, I will submit such PR at my convenient time. This is my first time to do that, any advice or notice?

[SteveMacenski]

Just make sure to fill in the PR template when it pops up with all the details + make sure there's test coverage if any new code is added to make sure its exercising and working properly. Also, base against the main branch, and I'll backport if possible to existing distributions. That's really it :-)

[SteveMacenski]

PR #4663 resolves and will be merged shortly with some documentation updates