SLIDE 1
Trajectory Optimization
Optimization over vector spaces
n
Optimization over Manifolds with applications to Robotic Needle - - PowerPoint PPT Presentation
Optimization over Manifolds with applications to Robotic Needle - - PowerPoint PPT Presentation
Optimization over Manifolds with applications to Robotic Needle Steering and Channel Layout Design Sachin Patil Guest Lecture: CS287 Advanced Robotics Trajectory Optimization Optimization over n vector spaces Not All State- Spaces are
SLIDE 2
SLIDE 3
Not All State-Spaces are ‘Nice’
- Nonholonomic system cannot move in arbitrary directions in
its state space
- For a simple car: Configuration space is in
(the SE(2) group)
2 1 : [ , , ]
x y
SLIDE 4
Nonholonomy Examples
Car pulling trailers:
2 1 1 1
Bicycle:
2 1 1
Rolling Ball: ?
2
(3) SO
SLIDE 5
C-Spaces as Manifolds
Manifold: Topological space that near each point resembles Euclidean space Other examples:
SLIDE 6
Optimization over Manifolds
n
?
SLIDE 7
Optimization over Manifolds
n
SLIDE 8
Optimization over Manifolds
n
Define projection operator from tangent space to manifold
SLIDE 9
Case Study: Rotation Group (SO(3))
3 3
Optimization over SO(3) arises in robotics, graphics, vision etc. Rotation matrices:
- Unique representation
- ‘Smooth’
SLIDE 10
: Incremental rotation to reference rotation defined in terms of axis-angle
Parameterization: Incremental Rotations
Why not directly optimize over rotation matrix entries?
Over-constrained (orthonormality) Larger number of optimization variables
Define local parameterization in terms of incremental rotation
r r
SLIDE 11
Projection Operator
r
[ ]
e r
: Point on SO(3) that can be reached by traveling along the geodesic in direction
[ ]
e r
r [ ]
z y z x y x
r r r r r r
r
where
1
X k k
e X k
and is the matrix exponential operator
SLIDE 12
Optimization Procedure
1) Seed trajectory: 2) Objective subject to: Constraints 3) Compute new trajectory: 4) Reset increments:
1
[ , , ]
i i i n
r r
min
1
ˆ ˆ [ , , ]
i i i n
R R
1
1 [ ] [ ] 1
ˆ ˆ [ · , , · ]
i i n
i i i n
R e R e
r r 1
[ , , ]
i
r
[ ]
e r
SLIDE 13
Steerable Needle
Steerable needle Target Bladder Prostate Pelvis Skin Cowper’s gland
Steerable needles inside phantom tissue Steerable needles navigate around sensitive structures (simulated)
SLIDE 14
Steerable Needle
[Webster, Okamura, Cowan, Chirikjian, Goldberg, Alterovitz United States Patent 7,822,458. 2010]
Bevel-tip Highly flexible Reaction forces from tissue Follows constant curvature paths State (needle tip)
- Position: 3D
- Orientation: 3D
3
(3) : (3) SE SO
SLIDE 15
Steerable Needle: Opt Formulation
SLIDE 16
Steerable Needle Plans
SLIDE 17
Results
Why is minimizing twist important?
SLIDE 18
Channel Layout (Brachytherapy Implants)
SLIDE 19
Channel Layout: Opt Formulation
SLIDE 20
Results
SLIDE 21
Optimization over manifolds – Generalization of optimization
- ver Euclidean spaces
Define incremental parameterization and projection
- perators between tangent space and manifold
Optimize over increments; reset after each SQP iteration!
Takeaways
SLIDE 22
Parameterization: Euler Angles
Euler angles What problems do you foresee in directly using Euler angles in optimization?
SLIDE 23
Parameterization: Euler Angles
Topology not preserved: Not unique, discontinuous Gimbal lock
[0,2 ] [0,2 ] [0,2 ]
SLIDE 24
Parameterization: Axis-Angles
Orientation defined as rotation around axis
- 3-vector; norm of vector
is the angle
SLIDE 25