Transcript Simulation of Robotic Systems

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 from
previous velocity
and 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:
position
orientation
linear momentum
angular 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

–
Easy to render

–
3D scanner, CT
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
–

Interior points
–

Points where distance to the object and the
object’s complement is zero
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
–

Complex algorithms
–





NURBS-based winged-edges, etc
manipulation, booleans, collision detection
Robustness
Integrity
Translation
Features
Constraints and Parametrics
Spatial Occupancy Enumerations
Spatial Occupancy Enumeration

Brute force
–

Pixels
–

Volume elements
Quadtrees
–

Picture elements
Voxels
–

A grid
2D representation
Octrees
–
–
3D representation
Extension of quadtrees
Brute Force Spatial Occupancy
Enumeration

Impose a 2D/3D grid
–


Identify occupied cells
Problems
–

Like graph paper or
sugar cubes
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:
–

Limitations:
–

Represent a model as a set of faces/patches
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 nonconvex 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.
P2
P1
A
B
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
A
B
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 NPhard.
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, Rtrees, 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:
- Object centric
- Spatial redundancy
SP:
- Space centric
- Object redundancy
BVH vs. Spatial Partitioning
BVH:
- Object centric
- Spatial redundancy
SP:
- Space centric
- Object redundancy
BVH vs. Spatial Partitioning
BVH:
- Object centric
- Spatial redundancy
SP:
- Space centric
- Object redundancy
BVH vs. Spatial Partitioning
BVH:
- Object centric
- Spatial redundancy
SP:
- Space centric
- Object redundancy
Spatial Data Structures & Subdivision
Uniform Spatial Sub Quadtree/Octree
Many others……
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 node
5. If there is a collision between the children
6.
Then If at leave nodes
7.
Then report “collision”
8.
Else go to Step 4
9.
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 collision
within some epsilon.
Change the object
velocities at this time,
accounting for bounce and
friction as desired.
Ethics Revisited
Roomba
Violates All
Three Laws Of
Roombotics
-The Onion
"I hear its horrible brushes at night."
-Graney