Gazebo: Simulator for the DARPA VRC Steven Peters, John Hsu - - PowerPoint PPT Presentation

Gazebo: Simulator for the DARPA VRC

Steven Peters, John Hsu

gazebosim.org

Outline

Overview of the Open Source Gazebo Simulator DARPA Virtual Robotics Challenge Challenges with real-time simulation Open Dynamics Engine (ODE) Projected Gauss-Seidel (PGS) Overview ODE Enhancements Walking Results Future Work

Gazebo Simulator: Overview and Purpose

Goal: Best possible substitute for physical robot Architecture: Use cases: Design and testing of robot components and control Software testing and verification Competitions Physics Sensors Interfaces GUI

gazebosim.org

Gazebo Features

Physics Rigid body dynamics Multiple physics engines Sensors Lidar, stereo, force-torque, IMU, GPS Mimics real sensor output with noise Interfaces Control platforms: ROS, Player Open API for custom interfaces Cloud-based simulation

Wide range of robots Wheeled robots, PR2, Pioneer2, Husky Stationary: Baxter, Barrett WAM Legged: Atlas, Robonaut Community-Generated Content Online model repository Open to all contributions Public API

Simulated Robots and Models

Community

Large community of users Academic Industrial Closely connected with ROS community answers.gazebosim.org bitbucket.org/osrf/gazebo

DARPA Virtual Robotics Challenge

Disaster first responder scenario: what is needed to fight fires? Drive a utility vehicle (ie. water truck). Walk across various terrains. Thread a fire hose into standpipe.

VRC Simulation Requirements

Performance goals Near real-time performance. Stable physics no matter what the user inputs are. Stable contacts for both manipulation and walking. Modeling choices Use fixed time step and fixed number of iterations. Prefer dissipative simulation (inelastic impacts, viscous damping). Approximate meshes with primitive shapes (sphere, box, cylinder). Artificial screw joint for firehose threading.

VRC Physics Summary

Trade-off between physics accuracy and real-time performance. We used Open Dynamics Engine (ODE) Maximal coordinates LCP with iterative Projected Gauss Seidel solver It also has a pivoting solver We did some crazy things to make simulation more stable. But first, an overview of ODE.

Open Dynamics Engine (Maximal Coordinates)

Rigid body kinematics Unconstrained dynamics Extended to N rigid bodies Articulation constraints

Open Dynamics Engine (cont.)

Normal 2 Friction Directions

Friction cone approximated with 2 friction directions Constraint for each direction

Open Dynamics Engine (cont.)

Discretized dynamics Velocity constraints Solve linear system Projected Gauss Seidel (PGS) Iterative solver

A b =

where is projected...

Then semi-implicit integration

Open Dynamics Engine (cont.)

Constraint force mixing Add diagonal matrix C (CFM) Constraint error correction Error h, parameter β (ERP) ERP and CFM derived from constraint stiffness (kp) and damping (kd)

Extensions to Open Dynamics Engine

Improve physics stability Implicit joint damping Row reordering Split impulse Accelerate PGS convergence Reduce inertia ratios Warm starting

Extensions to Open Dynamics Engine

Constraint error correction causes overshoot and bounce: Compute two velocities: Uncorrected velocity: Use as velocity Corrected velocity: Only use to compute position Split Impulse

Extensions to Open Dynamics Engine

Constraint error correction causes overshoot and bounce: Split Impulse

Overlap on startup Box Drop

Extensions to Open Dynamics Engine

Large inertia ratios slow PGS convergence Not just m1 / m2 Ixx1 / Ixx2 is important for maximal coordinates For bilateral constraints, blend inertia values along constrained directions Inertia Ratio Reduction Ixx ratio ~6000 : 1 Atlas ~9000 : 1

• Sandia hand model is unstable at 1ms with any explicit viscous joint

damping model. Implicit damping is needed for stability.

• Atlas robot is unstable with large explicit controller d-gains.
• Catto[1] showed equivalence between CFM/ERP and spring stiffness and

damping. Implicit Joint Damping

[1] E. Catto, “Soft Constraints reinventing the spring,” Game Developer Conference (GDC), 2011.

Extensions to Open Dynamics Engine

Atlas + Sandia Hands: Overall Kinetic Energy Content vs Sim Time. Implicit Joint Damping

PID Controller Unstable Damping = 0.01 Nms/rad 50 PGS Iterations KE ~28J PID Controller Stable Damping = 0.02 Nms/rad 50 PGS Iterations KE ~1e-6J PID Controller Stable Damping = 0.02 Nms/rad 100 PGS Iterations KE ~1e-7J

Extensions to Open Dynamics Engine

PGS Row Reordering, Friction Iterations

Standard ODE

Constraint row solution order: 1) bilateral constraints 2) contact normal and friction constraint groups Extensions to Open Dynamics Engines

Reordered PGS

Constraint row solution order: 1) bilateral constraints 2) contact normal constraints 3) frictional force constraints Additional iterations on friction force constraints.

Row Reordering, Smoothing, Extra Friction Iterations

Atlas grasp with Sandia hand (payload = 5.39kg) Extensions to Open Dynamics Engines Standard ODE vs. Reordered PGS + Extra Friction Iters. + Contact Residual Smoothing Cylinder slip distance vs. sim time.

Row Reordering, Smoothing, Extra Friction Iterations

Atlas Dynamic Stand Feet Contact Drift Test Extensions to Open Dynamics Engines Standard ODE vs. Reordered PGS + Extra Friction Iters. + Contact Residual Smoothing Left foot CG drift dist. vs. sim time

PGS Warm Start, Residual Smoothing

• Warm starting by using solution from previous time step as initial value:
• Warm starting PGS appear to help reduce constraint error (figure in next

slide).

• But to prevent unwanted instability, we reduced β to 0.5 or VRC.
• Additional contact constraint λ-smoothing added to help stabilize physics:

Extensions to Open Dynamics Engines

PGS Warm Start

Atlas dynamic stand behavior. Warm start values: 0, 0.5 and 1.0 Extensions to Open Dynamics Engines

• sim. time (sec)

β=0 β=0.5 β=1 Bilateral constraint error (m/s)

Results and Discussions

• Atlas walking demo

PGS Warm Start

• Atlas walking demo

Extensions to Open Dynamics Engines

Warm Start β: 0 0.5 1.0 KE RMS Jv bilateral RMS Δλ

Results and Discussions

• Atlas walking demo

KE RMS Jv bilateral PGS Iterations: 50 100 200 RMS Δλ

Results and Discussions

• Atlas walking demo

VRC

Future Work

New physics engines in Gazebo Bullet: Erwin Coumans, AMD (gaming) Simbody: Michael Sherman, Stanford (biomechanics) DART (RTQL8): Karen Liu, Georgia Tech (animation) Plugin architecture for physics Common data formats (HDF5)

gazebosim.org

OSRF Sponsors