Simulation of Robotic Systems

Particle Dynamics, Rigid Body Dynamics, Collision Detection

To Simulate…

Is to use a model of real system for experimentation.

For robots, these models are typically implemented using kinematics or dynamics.– Unlike kinematics, dynamics involves the changes

of velocity over time, which raises issues such as momentum, forces and torques, inertia, and mass.

Why Simulate?

Test a robotic system away from the dangers and unpredictability of the natural world.– Robotic systems are costly, and could be

damaged during testing.– Difficult to reach terrain can be simulated virtually.

Open up robotics questions to computational processes and searches.

Explore the design options.

Designing a Stair Climbing Robot

Articulated Body Forward Dynamics

Articulated Body: Series of rigid links connected by joints.

Forward Dynamics: Given a set of forces and torques on the joints, calculate accelerations and trajectories.

Initial Value Problems

An initial value problem is one in which we want to trace an unknown function given its starting state and how it changes.

They are solved using ordinary differential equations of the form

Particle Dynamics

The movement of a particle can be calculated by the above method.

To get a first order ODE, we need to work in phase space, the space composed of position and velocity.

The derivative of the state is then [v, F/m].

Particle Dynamics Implementation

Derivative fromprevious velocityand from forces.

Rigid Body Dynamics

Algorithm Overview:state = Initialize()for (t = 0; t < t_final; t += time_step)

ClearForces(state)AccumulateForces(state, t)derivative = Derive(state)Scale(derivative, time_step)Add(state, derivative)

Rigid Bodies

Rigid bodies represent all objects in the Rigid Body Dynamics simulation.

Each rigid body is a non-deformable shape. – The distance between any two points is constant.

Rigid bodies have an orientation:– Angular state– Angular velocity– Angular accelerations

Coordinates

The body frame is shown translated and rotated into world space.

Position and Orientation

The translation of the body’s basis gives it its position, a vector from the world origin to the body’s center of mass.

The rotation of the body’s basis gives it its orientation, a matrix in which each column corresponds to the new orientation of one of the basis axes.

Velocity

We’re interested in how the position and orientation of the bodies change over time.– Linear velocity:– Angular velocity:

The direction of (t) gives the axis

The magnitude of (t) gives the speed

Change of Orientation

The instantaneous change in the vector r(t) is (t) x r(t). This expands easily to the rotation matrix as a whole.

Acceleration

The acceleration of a rigid body depends on its various physical properties:– Inertia – Forces and Torques– Momentum

Inertia

3x3 matrix describing how the shape and mass distribution of the body affects the relationship between the angular velocity and the angular momentum I(t)

Similar to mass – like rotational mass.

Forces and Torques

Forces are applied to the body from contacts and the environment.

Momentum, Angular and Linear

Linear momentum– P(t) = m v(t)– dP(t)/dt = m a(t) = F(t)

Angular Momentum– L(t) = I(t) (t) (t) = I(t)-1 L(t)– It can be shown that dL(t)/dt = (t)

State Vector

We’ve now defined the concepts necessary to describe the state of a body:

positionorientationlinear momentumangular

momentum

Derivative of State Vector

Now that we have a state vector and its derivative defined, we can use the same approach we used for the 2D initial value problem.

Implementation

We now know everything we need to make a rigid body.

Implementation Contitued

This simulation runs for 10 seconds with a time step of 1/30 of a second.

The ode function works the same way as the one described for the initial value problem, we just need to define dydt.

Implementation Continued

Forces and torques are added to the system, and the derivative is saved.

Implementation Continued

The derivative vector is filled in:

Velocity comes from the current state.

dR(t)/dt is calculated with omega(t) and R(t), both known, and saved.

Forces and torques are added.

Star Operator

New Velocity, I-1, and Omega

These variables are not directly part of the state, they are simply used in the calculation.

Collision Detection

Given two object, how would you check: – If they intersect with each other while moving?– If they do not interpenetrate each other, how far

are they apart?– If they overlap, how much is the amount of

penetration

Classes of Objects & Problems

– 2D vs. 3D– Convex vs. Non-Convex– Polygonal vs. Non-Polygonal– Open surfaces vs. Closed volumes– Geometric vs. Volumetric– Rigid vs. Non-rigid (deformable/flexible)– Pairwise vs. Multiple (N-Body)– CSG vs. B-Rep– Static vs. Dynamic

And so on…

2D Graphics

Raster:

Pixels– X11 bitmap, XBM– X11 pixmap, XPM– GIF– TIFF– PNG– JPG

Lossy, jaggies when transforming, good for photos.

Vector:

Drawing instructions– Postscript– CGM– Fig– DWG

Non-lossy, smooth when scaling, good for line art and diagrams.

Representing 3D Objects

Approximate– Facet / Mesh

Just surfaces

– Voxel Volume info

Exact– Wireframe– Parametric Surface– Solid Model

CSG BRep Implicit Solid Modeling

Representing 3D Objects

Exact– Precise model of

object topology– Mathematically

represent all geometry

Approximate– A discretization of

the 3D object– Use simple

primitives to model topology and geometry

Negatives when Representing 3D Objects

Exact– Complex data structures– Expensive algorithms– Wide variety of formats,

each with subtle nuances– Hard to acquire data– Translation required for

rendering

Approximate– Lossy– Data structure sizes can get

HUGE, if you want good fidelity

– Easy to break (i.e. cracks can appear)

– Not good for certain applications

Lots of interpolation and guess work

Positives when Representing 3D Objects

Exact– Precision

Simulation, modeling, etc– Lots of modeling

environments– Physical properties– Many applications (tool path

generation, motion, etc.)– Compact

Approximate– Easy to implement– Easy to acquire

3D scanner, CT

– Easy to render Direct mapping to the

graphics pipeline

– Lots of algorithms

Two Major Types to Care About(for this class)

Mesh-based representations Solid Models

– As generated from CAD or modeling systems

3D Mesh File Formats

Some common formats STL

SMF

OpenInventor

VRML

Minimal

Vertex + Face

No colors, normals, or texture

Primarily used to demonstrate geometry algorithms

Full-Featured

Colors / Transparency Vertex-Face Normals

(optional, can be computed)

Scene Graph Lights Textures Views and Navigation

Subdivision Surfaces

Coarse Mesh & Subdivision Rule– Define smooth surface as limit of sequence of

algorithmic refinements

Simple Mesh Format (SMF)

Michael Garland http://graphics.cs.uiuc.edu/~garland/

Triangle data

Vertex indices begin at 1

Stereolithography (STL)

Triangle data +Face Normal

The de-facto standard for rapid prototyping

How STL Works

           

                                                                                              

                     

                                    

                                                                                                                   

                                              

                                                                             

Open Inventor

Developed by SGI Predecessor to VRML

– Scene Graph

Virtual Reality Modeling Language (VRML)

SGML Based

Scene-Graph

Full Featured

Issues with 3D “mesh” formats

Easy to acquire Easy to render Harder to model with Error prone

– split faces, holes, gaps, etc

Solid Representations

3D solid model representations

Implicit models Super/quadrics Blobbies Swept objects Boundary representations Spatial enumerations Distance fields Quadtrees/octrees Stochastic models

Boundary Representation Solid Modeling

The de facto standard for CAD since ~1987– BReps integrated into CAGD surfaces + analytic

surfaces + boolean modeling

Models are defined by their boundaries Topological and geometric integrity

constraints are enforced for the boundaries– Faces meet at shared edges, vertices are shared,

etc.

Solids and Solid Modeling

Solid modeling introduces a mathematical theory of solid shape– Domain of objects– Set of operations on the domain of objects– Representation that is

Unambiguous Accurate Unique Compact Efficient

Solid Objects and Operations

Solids are point sets– Boundary and interior

Point sets can be operated on with boolean algebra (union, intersect, etc)

Foley/VanDam, 1990/1994

Solid Object Definitions

Boundary points– Points where distance to the object and the

object’s complement is zero

Interior points– All the other points in the object

Closure– Union of interior points and boundary points

State of the Art: BRep Solid Modeling

… but much more than polyhedra Two main (commercial) alternatives

– All NURBS, all the time Pro/E, SDRC, …

– Analytic surfaces + parametric surfaces + NURBS + …. all stitched together at edges

Parasolid, ACIS, …

Issues in Boundary Representation Solid Modeling

Very complex data structures– NURBS-based winged-edges, etc

Complex algorithms– manipulation, booleans, collision detection

Robustness Integrity Translation Features Constraints and Parametrics

Spatial Occupancy Enumerations

Spatial Occupancy Enumeration

Brute force– A grid

Pixels– Picture elements

Voxels– Volume elements

Quadtrees– 2D representation

Octrees– 3D representation– Extension of quadtrees

Brute Force Spatial Occupancy Enumeration

Impose a 2D/3D grid– Like graph paper or

sugar cubes

Identify occupied cells Problems

– High fidelity requires many cells

“Modified”– Partial occupancy

Foley/VanDam, 1990/1994

Quadtree

Hierarchically represent spatial occupancy

Tree with four regions– NE, NW, SE, SW– “dark” if occupied

Foley/VanDam, 1990/1994

Octree

8 octants 3D space– Left, Right, Up, Down,

Front, Back

Foley/VanDam, 1990/1994

Applications for Spatial Occupancy Enumeration

Many different applications

– GIS– Medical– Engineering Simulation– Volume Rendering– Video Gaming– Approximating real-world

data– ….

Issues with Spatial Occupancy Enumeration

Approximate– Kind of like faceting a surface, discretizing 3D

space– Operationally, the combinatorics (as opposed to

the numerics) can be challenging– Not as good for applications wanting exact

computation (e.g. tool path programming)

Other Techniques: Surface Models

Basic idea:– Represent a model as a set of faces/patches

Limitations:– Topological integrity; how do faces “line up”?;

which way is ‘inside’/ ‘outside’?

Used in many CAD applications– Why? They are fine for drafting and rendering,

not as good for creating true physical models

Other Techniques:Implicit Solid Modeling

Computer Algebra meets CAD Idea:

– Represents solid as the set of points where an implicit global function takes on certain value

F(x,y,z) < val

– Primitive solids are combined using CSG – Composition operations are implemented by

functionals which provide an implicit function for the resulting solid

From M.Ganter, D. Storti, G. Turkiyyah @ UW

Collision Detection

Where do the forces mentioned above come from?– Motors– Gravity– Joints– Collisions

Collision Detection is the process of discovering whether objects have intersected and, if so, how much they interpenetrated.

Loops Colliding

Basics

Check for edge-edge intersection in 2D

(Check for edge-face intersection in 3D) Check every point of A inside of B & every point

of B inside of A Check for pair-wise edge-edge intersections

Useful Geometric Concepts

Convex Hull

Convex Decomposition

Voronoi Regions

Convex Hull

The convex hull of a set S is the intersection of all convex sets that contains S.

The convex hull of S is the smallest convex polygon that contains S and that the extreme points of S are just the corners of that polygon.

Solving the convex hull problem implicitly solves the extreme point problem.

Convex Decomposition

The process to divide up a non-convex polyhedron into pieces of convex polyhedra

Optimal convex decomposition of general non-convex polyhedra can be NP-hard.

To partition a non-degenerate simple polyhedron takes O((n + r2) log r) time, where n is the number of vertices and r is the number of reflex edges of the original non-convex object.

In general, a non-convex polyhedron of n vertices can be partitioned into O(n2) convex pieces.

Voronoi Diagram

Given a set S of n points in R2 , for each point pi in S, there is the set of points (x, y) in the plane that are closer to pi than any other point in S, called Voronoi polygons. The collection of n Voronoi polygons given the n points in the set S is the "Voronoi diagram", Vor(S), of the point set S.

Intuition: To partition the plane into regions, each of these is the set of points that are closer to a point pi in S than any other. The partition is based on the set of closest points, e.g. bisectors that have 2 or 3 closest points.

Voronoi Diagram

Voronoi Regions

A Voronoi region associated with a feature is a set of points that are closer to that feature than any other.

FACTS:– The Voronoi regions form a partition of space outside

of the polyhedron according to the closest feature. – The collection of Voronoi regions of each polyhedron is

the generalized Voronoi diagram of the polyhedron. – The generalized Voronoi diagram of a convex

polyhedron has linear size and consists of polyhedral regions. And, all Voronoi regions are convex.

Voronoi Marching

Basic Ideas: Coherence: local geometry does not change much,

when computations repetitively performed over successive small time intervals

Locality: to "track" the pair of closest features between 2 moving convex polygons(polyhedra) w/ Voronoi regions

Performance: expected constant running time, independent of the geometric complexity

2D Example

Objects A & B and their Voronoi regions: P1 and P2 are the pair of closest points between A and B. Note P1 and P2 lie within the Voronoi regions of each other.

A

B

P1

P2

Minkowski Sums/Differences

Minkowski Sum (A, B) = { a + b | a A, b B } Minkowski Diff (A, B) = { a - b | a A, b B } A and B collide iff Minkowski Difference(A,B)

contains the point 0.

Some Minkowski Differences

A B

A B

Minkowski Difference & Translation

Minkowski-Diff(Trans(A, t1), Trans(B, t2)) = Trans(Minkowski-Diff(A,B), t1 - t2)

Trans(A, t1) and Trans(B, t2) intersect iff Minkowski-Diff(A,B) contains point (t2 - t1).

Properties

Distance– distance(A,B) = min a A, b B || a - b ||2– distance(A,B) = min c Minkowski-Diff(A,B) || c ||2– if A and B disjoint, c is a point on boundary of Minkowski

difference

Penetration Depth – pd(A,B) = min{ || t ||2 | A Translated(B,t) = }– pd(A,B) = mint Minkowski-Diff(A,B) || t ||2 – if A and B intersect, t is a point on boundary of Minkowski

difference

Practicality

Expensive to compute boundary of Minkowski difference:– For convex polyhedra, Minkowski difference may

take O(n2)– For general polyhedra, no known algorithm of

complexity less than O(n6) is known

General Methods

Decompose into convex pieces, and take minimum over all pairs of pieces:– Optimal (minimal) model decomposition is NP-

hard. – Approximation algorithms exist for closed solids,

but what about a list of triangles?

Collection of triangles/polygons:– n*m pairs of triangles - brute force expensive– Hierarchical representations used to accelerate

minimum finding

Hierarchical Representations

Two Common Types:– Bounding Volume Hierarchies – trees of spheres, ellipses, cubes,

axis-aligned bounding boxes (AABBs), oriented bounding boxes (OBBs), K-dop, SSV, etc.

– Spatial Decomposition - BSP, K-d trees, octrees, MSP tree, R-trees, grids/cells, space-time bounds, etc.

Do very well in “rejection tests”, when objects are far apart.

Performance may slow down, when the two objects are in close proximity and can have multiple contacts .

BVH vs. Spatial Partitioning

BVH: SP:- Object centric - Space centric

- Spatial redundancy - Object redundancy

BVH vs. Spatial Partitioning

BVH: SP:- Object centric - Space centric

- Spatial redundancy - Object redundancy

BVH vs. Spatial Partitioning

BVH: SP:- Object centric - Space centric

- Spatial redundancy - Object redundancy

BVH vs. Spatial Partitioning

BVH: SP:- Object centric - Space centric

- Spatial redundancy - Object redundancy

Spatial Data Structures & Subdivision

Many others……

Uniform Spatial Sub Quadtree/Octree kd-tree BSP-tree

Uniform Spatial Subdivision

Decompose the objects (the entire simulated environment) into identical cells arranged in a fixed, regular grids (equal size boxes or voxels)

To represent an object, only need to decide which cells are occupied. To perform collision detection, check if any cell is occupied by two object

Storage: to represent an object at resolution of n voxels per dimension requires upto n3 cells

Octrees

Quadtree is derived by subdividing a 2D-plane in both dimensions to form quadrants

Octrees are a 3D-extension of quadtree

Use divide-and-conquer

Reduce storage requirements (in comparison to grids/voxels)

Bounding Volume Hierarchies

Model Hierarchy: – each node has a simple volume that bounds a set of

triangles – children contain volumes that each bound a different portion

of the parent’s triangles – The leaves of the hierarchy usually contain individual

triangles

A binary bounding volume hierarchy:

Type of Bounding Volumes

Spheres Ellipsoids Axis-Aligned Bounding Boxes (AABB) Oriented Bounding Boxes (OBBs) Convex Hulls k-Discrete Orientation Polytopes (k-dop) Spherical Shells Swept-Sphere Volumes (SSVs)

– Point Swept Spheres (PSS)– Line Swept Spheres (LSS)– Rectangle Swept Spheres (RSS)– Triangle Swept Spheres (TSS)

BVH-Based Collision Detection

Collision Detection using BVH

1. Check for collision between two parent nodes (starting from the roots of two given trees)

2. If there is no interference between two parents, 3.  Then stop and report “no collision”4. Else All children of one parent node are checked against all children of the other node5. If there is a collision between the children6.  Then If at leave nodes7.  Then report “collision”8. Else go to Step 49. Else stop and report “no collision”

Separating Axis Theorem

The separating axis theorem tells us that, given two convex shapes, if we can find an axis along which the projection of the two shapes does not overlap, then the shapes don't overlap.

Seperating Axis Theorem

Two polytopes A and B are disjoint iff there exists a separating axis which is perpendicular to a face from either or perpedicular to an edge from each.

Responding to Collisions

Two ways to deal with collision:– penalty-force: use spring forces to pull objects out

of collision.– impulse-based: use instantaneous impulses

(changes in velocity) to prevent objects from interpenetrating.

Find the time of collisionwithin some epsilon.

Change the object velocities at this time, accounting for bounce and friction as desired.

Ethics Revisited

"I hear its horrible brushes at night." -Graney

Roomba Violates All Three Laws Of Roombotics

-The Onion