Preserve gaussian proposals and enforce accel limits at command boundary #5615
Conversation
|
Thanks for the PR, i tested it on kilted with below config 
The below plot especially the angular vel is kind of spiking alot.(This could be because my motor/odom is poor) |
|
Thanks for the test. I'll look into it. Just to be sure, the first point of cmd_vel_nav angular vel should be |
Yes, you are right, my initial odom value for angular twist velocity is 0.0, I expect the next velocity, it should stay below 0.025. |
|
@chanhhoang99 thanks for the review, I was going to pull you in to test so thanks for beating me to it! @adivardi can you as well since you were involved in that previous work that was reverted? Let me know if the updated changes fixes your issue. Not much of a purpose of me reviewing the PR for code quality until it meets the need 😉 |
|
Okay there were two problems. First one is if the The second problem was the SG filter. We were applying the SG filter after enforcing the limits, however, the SG filter may violate the value/rate limits and returns unfeasible commands. Moreover, the control history is also build up based on the sequence of unfeasible commands. I tested this by while constant The following is the difference between commanded velocities and odom twists, where the The tested drive model is It could be great if you can test this version. @chanhhoang99 @adivardi |
|
Nice, I have been working on very similar changes myself :) This PR does a few changes, I will comment on them separately. Sorry for the length...
This PR does not solve the wobbliness issue. You can see the optimal solution jumping side to side when driving straight here: Your PR code without this particular change: 
The change that solves both low acceleration limits and delay is this one (or something similar).
With the change - speed only reaches ~0.54m/s Without the change, speed reaches the max 0.6m/s
My params are very similar to the defaults. I only changed the kinematic limits and increase the horizon & prune distance |
|
Thanks for the test and sharing your insights. Let me comment them.
This pr contains several parts that may highly effect the trajectory scoring, and post processing of the commands. A configuration tuned for previous logic, may not be proper for this version. So the unstable/oscillatory movements are expected without tuning. I am expecting this PR to solve the oscillations observed at the first pr of the #5266, since at that pr it was guaranteed the commanded velocities were not obeying the acceleration limits. Obviously this pr cannot prevent oscillations due to other effects (odom erros, poor tuning etc.).
This PR is not designed for handling the delayed systems. This mainly fixes problems due to the high asymmetric acceleration limits since its messing up with the costs/weights. Moreover, this PR makes sure the commanded velocity is intended one with the proper limits. #5617 basically violates the acceleration limits. In simpler terms acceleration params became smoothing factors for the given command without considering the actual state of the robot. This pr gives consistent maximum allowed value for constant odom twist values. So actually, this version can give your desired consistent command. If that does not work, it may be
Instead of #5617 you can increase your acceleration limits/model_dt or both. (Note that you are already accept to violate acceleration limits with #5617 at least you can make the divergence bounded by increasing your acceleration limits)
This is hard to comment without observing the odom plots. I'm not seeing this behavior in the simulation.
Yes this change mainly required if the proposals are not clamped.
It basically doubles the acceleration limits.
The proposed change makes the command history inconsistent with the actual commands. Moreover, I think it is better to make SG filter pure helper and stateless. |
But this PR does not solve the oscillations from #5266. You can see the plots and videos I added, which are in simulation with no delay and perfect steering. If you have a configuration that works, feel free to share it and I will test it.
Do you mean the trajectory? It is just a straight line starting from the vehicle.
That's fine then. My only comment is that for multiple iterations you are only applying |
|
kilted tb4 loopback simulation, params default except the without this pr tb4 cannot make any progress, 
with this pr tb4 can make progress again, however the 
Increase the PathFollowCritic's weight to 10, and tb4 can reach the max vel again. 
Without this pr tb4 cannot make progress with highly asymmetric acceleration limits, even-though we increase the PathFollowCritic's weight. Just changed one weight to obtain the desired vx, also could eliminate the oscillations of wz by adjusting other weights/params. This pr aims to fix the trajectory scoring and kinematic violations of the current version. It can easily observed with highly asymmetric acceleration limits as mentioned at #5465. It can only resolve the oscillations due to internal inconsistency of the trajectory scoring and kinematic violations, which definitely bring along with #5266. There may still be points that could be improved further. I noticed that the control history should be cleared after a goal is reached to prevent it from influencing the initial commands for following goals. |
|
I tried a bunch more changes today:
All have the same oscillations in the vx & wz commands. |
|
@adivardi , #!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from geometry_msgs.msg import TwistStamped
from nav_msgs.msg import Odometry
def clamp(x, lo, hi):
return max(lo, min(hi, x))
class OneSecondCmdVelFixedDT(Node):
def __init__(self):
super().__init__('one_second_cmd_vel_fixed_dt')
# Parameters
self.declare_parameter('cmd_vel_topic', '/cmd_vel')
self.declare_parameter('odom_topic', '/odom')
self.declare_parameter('dt_s', 0.1) # <-- fixed period (20 Hz)
self.declare_parameter('duration_s', 1.0) # 1 second total
self.declare_parameter('v_max', 1.0) # m/s
self.declare_parameter('a_max', 0.5) # m/s^2
self.declare_parameter('forward_only', True)
self.cmd_vel_topic = self.get_parameter('cmd_vel_topic').value
self.odom_topic = self.get_parameter('odom_topic').value
self.dt = float(self.get_parameter('dt_s').value)
self.duration_s = float(self.get_parameter('duration_s').value)
self.v_max = float(self.get_parameter('v_max').value)
self.a_max = float(self.get_parameter('a_max').value)
self.forward_only = bool(self.get_parameter('forward_only').value)
# Derived
self.steps_total = max(1, int(round(self.duration_s / self.dt)))
self.steps_sent = 0
self.started = False
# IO
self.pub = self.create_publisher(TwistStamped, self.cmd_vel_topic, 10)
self.sub = self.create_subscription(Odometry, self.odom_topic, self._odom_cb,
qos_profile_sensor_data)
# State from odom
self.v_meas = 0.0
self.have_odom = False
# Timer runs strictly at dt
self.timer = self.create_timer(self.dt, self._tick)
self.get_logger().info(
f"Will publish to {self.cmd_vel_topic} at {1.0/self.dt:.1f} Hz "
f"for {self.duration_s:.2f}s (steps={self.steps_total}), "
f"v_max={self.v_max} m/s, a_max={self.a_max} m/s², based on {self.odom_topic}"
)
# Use longitudinal velocity from odom (base_link frame)
def _odom_cb(self, msg: Odometry):
self.v_meas = float(msg.twist.twist.linear.x)
self.have_odom = True
def _tick(self):
if not self.have_odom:
self.get_logger().warn_throttle(2.0, f"Waiting for odom on {self.odom_topic}...")
return
# Start counting steps only after we have odom
if not self.started:
self.started = True
self.steps_sent = 0
if self.steps_sent >= self.steps_total:
# Done: send stop and exit
self.pub.publish(TwistStamped())
self.get_logger().info("Finished 1.0 s at 20 Hz. Stopping and shutting down.")
rclpy.shutdown()
return
# Target is forward v_max (you can change this to any target profile)
v_target = self.v_max
if self.forward_only:
# Don’t command reverse
v_target = max(0.0, v_target)
# Compute one-step change limited by ±a_max*dt, based on *measured* speed
dv_allowed = self.a_max * self.dt
delta_to_target = v_target - self.v_meas
step_delta = clamp(delta_to_target, -dv_allowed, dv_allowed)
v_cmd = self.v_meas + step_delta
if self.forward_only:
v_cmd = max(0.0, v_cmd) # keep forward-only if requested
v_cmd = clamp(v_cmd, -self.v_max, self.v_max)
self.get_logger().info(f"v_cmd {v_cmd}")
# Publish command
msg = TwistStamped()
msg.twist.linear.x = v_cmd
msg.twist.angular.z = 0.0
self.pub.publish(msg)
self.steps_sent += 1
def main():
rclpy.init()
node = OneSecondCmdVelFixedDT()
try:
rclpy.spin(node)
finally:
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()This is a small script that simulate cmd_vel sent by MPPI. |
|
All of the results I posted here are in simulation with no additional delay. But I have confirmed similar results on our robot. I have played again with a bunch of params, but the results are the same. Only improvement is when setting high az_max reduces the oscillations, but doesn't remove them. |
|
0.47/0.5 in 1 second with dt is 0.05 and a max is 0.5 right ? |
yes |
4 participants
Basic Info
Description of contribution in a few bullet points
Do not clamp the sampled control proposals
state.cvx/cvy/cwz. These are the gaussian proposals around the current nominal sequence and represent the algorithm’s exploration. Clamping them distorts the proposal distribution (no more gaussian) and biases the costs/weights.Derive
state.vx/vy/wzfrom the unclampedstate.cvx/cvy/cwzand apply rate & value limits. Then integrate poses fromstate.vx/vy/wz. This guarantees that all evaluated trajectories are physically feasible while keeping the proposal distribution intact.The control sequence is the softmax-weighted average of the proposals (an optimal proposal), so it does not have to satisfy acceleration limits.
applyControlSequenceConstraints()projects that proposal into the corresponding trajectory (which should be the optimal trajectory). However, at first acceleration limits should be applied to the first element of the control sequence wrt the robot’s current velocity. Otherwise, given command does not respect the acceleration limits and potentially wobble, even though the rest of the trajectory respects the acceleration limits internally.Description of documentation updates required from your changes
Description of how this change was tested
Future work that may be required in bullet points
For Maintainers:
backport-*.