Single Car Drake/ROS Demo Velocity > 1m/s Crash

[wilkosch]

Problem Definition

Irregular behaviour (vehicle rolls over and falls through floor) for velocities > 1m/s
Issue is referenced in #2606.

Replication Procedure

$ roslaunch drake_cars_examples single_car_in_stata_garage.launch
$ rosrun ackermann_drive_teleop ackermann_drive_keyop.py 5.0 0.7
Start driving around and experience behaviour.

Tested so far

• Increase thickness of floor in both sdf/urdf to 3m.
  drake_catkin_workspace/src/drake_ros_integration/drake_cars_examples/models/stata_garage_p1.urdf
drake_catkin_workspace/src/drake/drake/examples/Cars/models/stata_garage_p1.sdf (Car does not fall through floor anymore, but still crashes)
• Parameter in SetRigidBodySystemParameters. (Increasing both stiffness and damping has helped but it still is uncontrollable and either starts spinning / rolling or slowly sinking into the floor.)
• The throttle and steering gains. (No improvement to crashing. The p-gains seem to be too low though, especially for steering.)
  

[liangfok]

Below is a video of the original problem described by this issue. The vehicle is issued a throttle reference function that is a step from 0 m/s to 5 m/s. The reference steering angle is kept at 0 radians. The vehicle's steering angle almost immediately changes to a non-zero value resulting in the vehicle swerving, flipping over, and falling through the ground.

The vehicle falls through the floor after flipping over due to the collision models being overly simplistic. Currently, collision models are only provided for the wheels (see: #2450). This means the vehicle's chassis will fall through the ground when it rolls over.

What's more problematic is the fact that the vehicle flips over when turning. This can be due to the projection of the center of mass on the ground along the gravity vector falling outside of the support polygon formed by the four contact regions of the wheels, or by the moments around the vehicle's sagittal axis due to centrifugal forces overwhelming the opposite moments due to gravity.

In the video above, the first problem I see is the fact that the steering angle does not remain at zero as the vehicle accelerates. This may be due to the actuators in the wheels exerting asymmetric torques on the tie rod arms, which results in forces in the tie rod arm.

TODO 1

See if the problem persists if the throttle command is a ramp rather than a step function.

TODO 2

See if the steering PID controller can be made more stiff.

TODO 3

Adjust the reference velocities of the two front wheels to account for differences due to the steering angle.

[liangfok]

I enhanced my feature/multi_car_sim_2 branch to allow users to set numerous simulation parameters by changing a .launch file (no need to recompile). Details are available here:

https://github.com/liangfok/drake/tree/feature/multi_car_sim_2/ros/drake_cars_examples#tuning-tips

One particular change includes increasing the Kp/Kd gains of the steering angle controller, see: liangfok@94fd740. I made this change because I noticed that in the vehicle above, the steering angle would deviate significantly from zero while the vehicle was accelerating causing it to swerve and ultimately flip over.

This allowed the vehicle to reach > 8m/s before running out of room:

Note that the video above shows a new node that publishes a maximum acceleration trajectory for the vehicle to follow.

TODO 4

Test whether increasing the steering controller's gains also resolves this issue in the multi-vehicle demo.

[shensquared]

Hi, @liangfok

I'm also trying to understand this issue as suggested by @RussTedrake . To make sure we’re on the same page, here’s what I found so far.

Testing on @ 94fd740, (the newer commit somehow broke my build but those are irrelevant anyways) it seems the increased PD gains doesn’t solve the original problem decribed by @wilkosch i.e. Demo1 that uses keyboard commanding the acceleration still results in the car flipping over when exceeding 1m/s? However, Demo3 behaves normal reaching a higher speed, where the acceleration is automatic according to a fitted function.

I feel that the absolute velocity is not the cause of the problem, but rather how it is reached. I tweaked Demo3 a bit, making the car accelerate according to the fitted curve function for 2 seconds and then stay at the constant velocity from then on, (at which point the velocity is well beyond 1m/s) and the behavior is normal as well.

Your diagnosis 1

See if the problem persists if the throttle command is a ramp rather than a step function

and 3

Adjust the reference velocities of the two front wheels to account for differences due to the steering angle.

sounded very reasonable to me. Not sure if you've tried them? If not, I’d be happy to jump in on them. I might need a few pointers as I’m totally new to ROS (it took me four days to just install it and replicate the bug), I’m wondering if we could talk a bit after I try a few things out over the weekend? Thanks.

[liangfok]

Yes, I agree that absolute speed is not the original problem. Instead, I think the underlying problem relates to the two problems listed at the bottom of this post. I would appreciate if you could help investigate those two suspected problems.

Replacing the step reference function with a curved one did not fix the problem, though I intuitively believe it increased the system's stability since the controller was not subjected to such a large initial error.

I have not tried adjusting the reference velocities of the two front wheels based on the steering angle, car width, and wheelbase. I would appreciate if you could investigate this.

It was also suggested that the vehicle model be enhanced with a full suspension system to better distribute the reaction forces among the wheels, though I have not tried that yet. Maybe you have time?

Please ask me any question. I'll be happy to meet next week.

Suspected Problem 1 - Bad Inertia Modeling

The mass and spatial inertia modeling of the vehicle are wrong. They are specified here. The car is not 57.833 kg! Hopefully, by switching to a more realistic (i.e., heavier) model with a very low center of mass, the vehicle won't flip over so easily.

I created a simple spatial inertia calculator for cuboids in #3387, which should be useful when updating the spatial inertia in drake-distro/drake/examples/Cars/models/prius/prius_with_lidar.sdf.

Suspected Problem 2 - Contact Model Gains Need Tuning

The current demo models contacts between rigid bodies as virtual springs. The stiffness and damping gains of these virtual springs likely need tuning. I've extracted these values onto the ROS parameter server so they can be quickly changed without recompiling Drake. To set them, edit drake/ros/drake_cars_examples/launch/single_car_in_stata_garage.launch.

Another simulation parameter that may require adjustment is the simulation time step. This is done by changing parameter initial_time_step, which is defined in the same launch file mentioned above. For example, as the mass the vehicle is increased and the contact force gains are increased, the simulation time step may need to be decreased to avoid numerical instability. This will come at the cost of decreasing the simulation's real-time factor.

[liangfok]

Using liangfok@deba69d, I was able to drive the vehicle around by sending it a reference trajectory containing ± 1 m/s and ±0.7 radian steps.

This gives me confidence that getting this to work with higher velocities is a matter of tuning the vehicles' inertia model and collision stiffness & damping gains.

[liangfok]

Suspected Problem 3 - Modify ackermann_drive_teleop to Upperbound the Rate At Which Reference Speed and Steering Angle Can Change

I believe @wilkosch is observing the vehicle flip over when he issues reference speed and steering angle trajectories through ackermann_drive_teleop. To my knowledge, ackermann_drive_teleop is issuing step functions as the reference trajectories, which is problematic since it results in points along the trajectories with infinite accelerations. We should time-synchronize ackermann_drive_teleop with the simulator and upperbound the rates at which the reference speed and steering angle can change.

Update: This was investigated in #3258 (comment). The results indicate it is not a major contributor to the problem.

[liangfok]

I "manually" controlled ackermann_drive_teleop to limit the rate change in the reference speed / steering angle trajectories and was able to make the vehicle do the following loop in only 13.44 seconds of simulation time. I believe this is super fast though I haven't analyzed the actual speed of the vehicle. Regardless, I believe this lends further credence to Suspected Problem 3 above.

[liangfok]

TODO 5

Verify that the units of the reference speed trajectory that Drake consumes is m/s. I recall the reference signal goes through a gain before going into the PD controller, which has its own Kp / Kd gains. Figure out what this initial gain is for.

[liangfok]

I modified the keyboard teleoperation node to upper bound the rate of change of the reference speed and steering angle trajectories. See:

https://github.com/liangfok/ackermann-drive-teleop/tree/feature/upperbounded_first_derivative.

This is towards investigating the above-mentioned Suspected Problem 3.

To get this update, execute the following:

$ roscd ackermann_drive_teleop
$ git fetch origin
$ git checkout feature/upperbounded_first_derivative

Then, to run the updated keyboard teleoperation node, execute:

$ roslaunch ackermann_drive_teleop ackermann_drive_keyop.launch

While this decreases the magnitude of the reference step functions, the car still rolls over when trying to make it turn -0.7 radians while going 3 m/s. Thus, I no longer think Suspected Problem 3 is a major culprit of this issue.

Suspected Problem 4 - Invalid Units Within Drake

I'm fairly certain that the problem is due to Drake not using the correct units. When I command the vehicle to move at 1 m/s, it appears to be moving faster than 1 m/s based on visual inspection. For example, see the screenshot below.

The screenshot shows the state at simulation time 10 seconds. Earlier in the simulation, I issued a 1 m/s reference speed trajectory and would thus expect the car to have traveled a bit less than 10 meters, since it takes time for me to issue the trajectory and for the trajectory to ramp up to 1 m/s. In the simulation, however, the vehicle appears to have traveled > 50 m in less than 10 seconds of simulation time (each square in the grid represents 1 m).

Thus, I believe the next step is to investigate TODO 5.

[liangfok]

Drake-Only Simulation

Description

I ran the Drake-only version of the vehicle simulation to see if it behaves differently than the Drake + ROS version. (Running the Drake-only version again is now possible given the fixes in #3403.) In all of the experiments in this post, the vehicle was commanded to go forward and then make an abrupt right turn.

Commands

The latest commands are officially given here. The exact commands I executed are given below.

$ cd drake-distro/drake/examples/Cars
$ ../../../build/install/bin/drake-visualizer
$ python steering_command_driver.py
$ ../../../build/drake/bin/car_sim_lcm models/prius/prius.urdf models/stata_garage_p1.sdf

Update: Note that the recently-merged #3436 collapsed directory "examples/Cars" into "automotive".

Video

The following video shows what happens when the vehicle is commanded to go forward with a speed of 1 m/s, and then is issued a reference steering angle of -0.7854 radians. It is using SHA 4245480.

The following video shows what happens when the reference speed is increased from 1 m/s to 3 m/s. (This was done by modifying this variable). The vehicle flips over just like it does in the ROS version.

Conclusions

The Drake-only simulation is able to avoid rollovers when the reference throttle is only 1 m/s. However, it rolls over in a similar manner when the reference throttle is increased to 3 m/s.

[liangfok]

@shensquared: I found the bug you mentioned that prevents the contact penetration and stiffness gains from being correctly set from launch files. See the following two commits, which fix the bug:

• liangfok@d99ebdf - single vehicle demo
• liangfok@e3b1c7a - multi vehicle demo

The gains were being correctly read from the ROS parameter server, but were then subsequently being overriden by a call to drake::automotive::SetRigidBodySystemParameters().

I verified that the collision model gains are correctly set via a launch file by first running the single car demo using SHA e3b1c7a and verifying that the vehicle stays above the ground:

$ roslaunch drake_cars_examples single_car_in_stata_garage.launch

I then edited drake_catkin_workspace/src/drake_ros_integration/drake_cars_examples/launch/single_car_in_stata_garage.launch and changed parameters penetration_stiffness and penetration_damping to both be zero. After making this change, I ran the demo again and saw that the vehicle falls through the floor. This indicates that the gains are being correctly set through the launch file.

[amcastro-tri]

@liangfok, as you very well observed the inertia properties for this model are completely wrong. I would start there first. Most likely that will also impact the contact stiffnesses and time step you will need.

[liangfok]

Yes! I believe @shensquared will be looking into fixing the inertia model for the vehicle. @shensquared is this correct?

[shensquared]

Thanks, @liangfok and @amcastro-tri . I have actually tested using the more realistic mass and inertia paired with fixed high penetration stiffness, and indeed flipping over no longer happens. See e66d9a0

[amcastro-tri]

What speeds are you targeting? and how do you select your time step? If the length of the car is L0 and the target speed is U0 a good rule would be to define a reference time scale T0 = L0/U0 and define the time step as a fraction of that. For instance time_step = T0/100.

[liangfok]

@shensquared, I see in e66d9a0 that you've changed the contact gains. Where are the new inertia model values?

[liangfok]

Let's use realistic worst-case values:

• Top speed of vehicle (U0): 100 mph = 44.7 m/s
• Length of vehicle (L0): 179" = 4.5466 m
• T0 = L0 / U0 = 4.5466 / 44.7 = 0.1017 s
• Nominal time step (time_step) = 0.1017 / 100 = 0.001017136 s = 1 ms

@amcastro-tri: Is there a prinicipled way to derive the necessary contact penetration gains based on the above numbers and the vehicle's model?

[shensquared]

@liangfok I was doing the testing on drake side, as we discussed I think it shouldn't matter if it's tested on drake or ROS. The new model I used is in https://github.com/shensquared/drake/blob/ss-fix-car-sim/drake/automotive/models/prius/prius.urdf#L15 The mass is from your earlier comment, and the inertia was scaled up from the unrealistically light model.

[shensquared]

@amcastro-tri I used the throttle scale up to 4, so the target speed is 4m/s. I didn't know the rule of selecting time steps, in my testing the time step was left as is, but I'll certainly try out the more rigorous steps. Thanks for the tip btw.

[liangfok]

@shensquared: Thanks. I see you're using a vehicle mass of 1360 kg here. In e66d9a0, the contact gains are being specified as command line parameters. What gains are you using?

[shensquared]

@liangfok stiffness and damping are scaled up 100x corresponding to the order of mass scaling up, so 750,000 and 75,000, respectively, and the friction coefficient is still 10.

update: double checked, contact damping was actually left unchanged as 750. Scaling it up to 75,000 causes a BulletModel error, which looks like a visualization error? Should I be concerned about this? @liangfok

[shensquared]

There's still abnormal behavior though. For example, while pure throttle command drives the car straight without flipping over, if the throttle stops, the car slows down as one would expect but at the same time it also always steers slightly despite having no steering command issued, sometimes to the left and sometimes to the right but never straight. I suspect it's directly caused by torque steering @liangfok pointed me to, I'm working on this right now. I'm also working on the reference unit and center of mass projection.

I was planning on posting here a full investigation, but now I feel like I should've updated my intermediate findings also for better discussions, I will do a better job on this. :)

[liangfok]

Yes! Please post new findings as you discover them!

Regarding the steering-while-slowing-down issue, does the reference velocity drop from a high value down to zero at a single point in time, i.e., a step function? If so, it may be worth verifying that the undesirable steering goes away when the reference trajectory is a smooth curve like one derived using a cubic spline.

Perhaps increasing the gains of the steering controller would fix the problem.

Another idea is to add suspension to the vehicle, which can reduce the "shock" felt by the front wheels when the vehicle suddenly decelerates.

[amcastro-tri]

@shensquared: this is just a toy model and steering is probably the weakest model. Just out of curiosity, what happens if instead you are driving straight back and then you go to a sudden stop? do you still get this unexpected steering?

Regarding the constants. In steady state this means weight = delta_z * stiffness. Therefore multiplying the by the same factor mass and stiffness has the effect of keeping delta_z constant. This is not what you want I think. What you want is to keep delta_z/L0 constant (with L0 the length of the car). Then you could compute the stiffness as stiffness = old_stiffness * new_mass/old_mass * old_legnth/new_length where x_length refers to the car length.

For the damping I think what you want is to keep the "dimensionless" damping to be constant. You can make damping dimensionless with the factor U0^2/L0/weight. This is just traditional dimensional analysis.

[shensquared]

Yes, @amcastro-tri driving backwards and stopping leads to the same problem. Here’s what I’ve tried so far, none of them fixed the problem:

• smoothening out the sudden stopping;
• increasing the PD gains;
• decreasing the initial time step from 5ms to 1ms;
• verifying the speed unit (the unit actually looks correct, at least on Drake side, as inspected by plotting out the front wheel position over time. I’ll double check on ROS side to try reproducing what @liangfok observed Single Car Drake/ROS Demo Velocity > 1m/s Crash #3258 (comment))

Plotting out the front wheel positions also reveals that the left and right front wheels have a position difference at the timestamp when the throttle command just stops, which could explain the steering later on. I’m sure adding a PD controller between the two front wheels for angle and position correction could fix this and I’m working on it, I’m not sure though which part in the code could cause this initial position difference? From car_simulation.cc it seems the left and right wheels are controlled as a whole for acceleration. Could this be entirely numerical?

Another issue I came across is when the car is simply placed on the flat ground, with no throttle or steering command issued, it actually moves... (like it's really self-driving). This only happens under some penetration stiffness, damping, and friction coefficient combinations though, 750000 7500 2 is one example if you want to reproduce this. One possible explanation is that the car model itself can not dissipate the energy pumped in by the near-zero initial state, and I tried adding damping and even friction in the wheel joints, but this didn’t fix the problem. I’m hoping visualizing the contact forces could lend more insight to this, if you know any existing examples that does the visualization that’d be very helpful.

[liangfok]

@sherm1, since I believe this is fundamentally a dynamics modelling issue, I'm going to add you as an assignee. Feel free to un-assign yourself or delegate.

[sherm1]

@edrumwri will have an error-controlled integrator up soon; we can try this simulation using that to see if this is just a stability problem. However, if it is more than that then this system is probably not the right place to start debugging.

[liangfok]

Thanks. I'll add @edrumwri to the list of assignees. I'm currently porting the Drake-only version of this demo (car_sim_lcm) to System 2.0, which will allow us to test the new integrator.

[liangfok]

I have a WIP feature branch where I've converted car_sim_lcm to System 2.0:

https://github.com/liangfok/drake/tree/feature/sys2_car_sim

Using System 2.0 and Simulator 2.0, I'm able to drive the vehicle defined by prius_with_lidar.sdf at 2m/s without it flipping over:

There's still much work to be done in terms of increasing the accuracy of the simulator. A few other PRs need to be merged before I can merge in this new car_sim_lcm demo.

[sherm1]

@edrumwri's PR #3876 will add a variable step integrator to Simulator 2 that should prevent stability problems (by taking small steps if necessary).

[shensquared]

@liangfok that sounds great. I'm also wrapping up my fix of car_sim_lcm, in which I think the issues we noted before are more or less fixed, and the car is able to ramp up to 60mph under 20s stably.

They currently live in https://github.com/shensquared/drake/tree/ss-fix-car-sim/drake/automotive, and the README file documents the commands needed to run the demo. Here’s the list of the issues and the fix:

Wrong reference velocity:

This is a previously unconfirmed issue, but by cout the body velocity at steady state, it’s clear that when issuing what we believed to be 1 m/s reference velocity, the car body was in fact aiming at reaching ~6.5 m/s. I think there are two causes for this. First, the velocity command was incorrectly magnified 20x before being passed as the desired state into pd_control_system (this line). Second, there’s the lack of rad/s to m/s unit conversion, specifically, the command was issued at the wheel joint as angular velocity but not as the body linear velocity, which when accounting for the wheel radius results in another ~0.323x scaling. Combining the two factors explains the 6.5x (as 20*0.323), and correcting them fixes the issue.

Flipping over at high velocity:

Fixed by correcting the chassis mass and inertia, decreasing the simulation time-step from 5ms to 2ms, tuning on the throttle and steering gains, and smoothening out the reference velocity.

Essentially the fix to this issue is refining the derivatives, and the tricks are standard. The gains tuning is just manual trial and error, and steering_kd and steering_kp are very tamable, I’ve had luck placing them anywhere up to 1000. Throttle_kd was a bit tricky though. For quite some time, to get the body velocity to ramp up to 60mph, throttle_kd gain can not go above 30 and the acceleration was smooth and steady but very very slow.

At first I thought it's just because the reference steering is small in comparison with the velocity, but later I realized it's also because reference steering is passed in at 1/100th incremental (this line) while reference velocity was dumped all at once. When velocity is handled in the same incremental fashion, throttle_kd can go up to 1000. I believe to get to a higher velocity faster can be a matter of refining the reference incremental and tuning the throttle_kd.

Moving when no driving command is issued:

Fixed by tuning the contact parameters: increased stiffness to stop sinking, increased damping to stop bouncing, and increased friction coefficient to stop sliding. (thanks to @tkoolen for the tip, btw)

Unintended veering when slows down:

Fixed by and adding a separate PD controller for steering on the right front wheel (currently there's only one steering actuator on the left wheel), and increasing the wheels inertia and mass to match the scaling of chassis (the cause of this one is stupid btw. when I increased the chassis mass and inertia to better reflect the reality I forgot to correspondingly scale up those of the wheels and the body, which then led to much increased contact forces applied on the same old light-weighted wheels...)

My fork of the drake master is unfortunately outdated and I’ll submit a PR after resolving the conflicts.

[amcastro-tri]

This is so great! I am glad you are working on setting up this case with the right physical quantities corresponding to reality. Probably car and tires masses and sizes can be obtained from their corresponding manufacturer's specs?
It is reasonable to expect a change in time step when changing the physical properties. As a measure of what is needed to run a case like this? how many time steps does the simulation run for the car to advance a distance equal to its length? this is a very good measure in a wide variety of simulators (I used that routinely for the simulation of the fluid mechanics around moving ships).

Also good job figuring out the controllers' constants and the right conversion factors!

[liangfok]

@shensquared, thank you for the analysis. I've updated the System 2.0 version of car_sim_lcm to use the correct conversion from user command space to actuator command space. This fix was part of #3886.

Here are two videos showing the updated behavior using c35e036.

The following video shows the vehicle commanded to go 1m/s:

The following video shows the vehicle commanded to go 10m/s:

[amcastro-tri]

Actually I have another thought on how to choose the time step. The fastest dynamics in this case is the rotation of the wheels. You should make sure that your time step properly discretizes one revolution of a wheel at the cruise speed (or the maximum expected speed).
For instance, you should compute the angular velocity of the wheels at say, 60 mph, and then make sure that with that angular velocity a wheel doesn't perform more than say 2 degrees per time step. That would give you 180 time steps per wheel revolution which should be more than engough to properly resolve its dynamics (or at least a good start).

Probably good numbers to report are:

1. Wheel angular velocity at car cruise speed (60 mph?)
2. Time step used so that at that angular velocity the wheel only rotates 2 (maybe 3) degrees.
3. With that time step, how many time steps do you need for the car to advance one length?
4. How many wheel revolutions are needed for the car to advance one length (needed to answer question 3)?

[sherm1]

@amcastro-tri, that's not the right way to think about wheel rotations. They aren't actually cyclic. In internal coordinates they have a more-or-less constant generalized speed v, that integrates into a more-or-less linear growth in q. They are numerically indistinguishable from a steady translation along a sliding axis. So the step size does not need to account for wheel rotation.

[amcastro-tri]

@sherm1, that is very true. I confused this much simpler problem with the problem of solving the fluid dynamics of an entire ship with rotating propellers. In that case the time step is chosen based on the rotational speed of the propeller since you need to resolve the smaller time scale features.

In this case it is true the time step won't affect things much for the wheels. That is actually good news when selecting the time step!
Just curious, what about angular momentum conservation of the wheels? does your analysis still apply?

[sherm1]

Yeah, definitely makes sense for propellers!

Angular momentum conservation is just an accuracy consideration here -- if the ODEs are integrated accurately it will be conserved. Best handled by @edrumwri's flashy new error-controlled integrator! But it won't choose small steps due to wheel rotation because those state variables will appear so harmless: vdot=0, v=constant, q=linear (roughly).