Upload
angel-hampton
View
226
Download
2
Tags:
Embed Size (px)
Citation preview
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