Upload
sandra-simon
View
216
Download
0
Tags:
Embed Size (px)
Citation preview
2
Overview• Motivation
– Providing infrastructure to support integration of currently funded MTP based tasks
• Adding a new motion primitive to CLARAty locomotor: continuous driving
• Reviewing trajectories in CLARAty• Using the same trajectory representation for locomotion and
manipulation
• Overview of trajectories• Design Proposal
– Functions– Trajectories– Trajectory Generators– Locomotor
• Prototype implementation• Summary
3
Continuous Driving• Helps avoid steering while stopped
(e.g. point-turns)
• Helps maintain wheel contact, while stationary steering could be risky (e.g. slippery slopes)
• Can be more efficient in terms of time of traversal (turn while moving)
• Can result in energy savings
• Would help maintain CLARAty up-to-date with developments elsewhere
4
Clothoid Trajectory
• Arc: curvature is constant
• Clothoid: curvature is a polynomial
= 1 / R = const.
v = trapezoid, etc.
(s) = polynomial
e.g. (s) = a + bs + cs2 + ds3
v = trapezoid, etc.
R
(s)
s
5
Overview• Motivation
– Providing infrastructure to support integration of currently funded MTP based tasks
• Adding a new motion primitive to CLARAty locomotor: continuous driving
• Reviewing trajectories in CLARAty• Using the same trajectory representation for locomotion and
manipulation
• Overview of trajectories• Design Proposal
– Functions– Trajectories– Trajectory Generators– Locomotor
• Prototype implementation• Further work
6
Motions in CLARAty
• Drive_Command• Drive_Sequence• Motion_Sequence• Via_Sequence• Trajectory• Multi_Segment_Trajectory• …
STOP
7
Body-frame Motions• Sequence_Drive module:• Drive_Command
– Mode (line, arc, turn-in-place, etc.)– Distance– Heading change– Crab direction
• Drive_Sequence– typedef std::list<Drive_Command> Drive_Sequence;
STOP
8
Wheel Motions
• Motion_Control module
• Motion_Sequence– double ** _motor_positions;– Matrix<double> _motor_commands (*);– std::vector<Drive_Command> _drive_commands;
9
Arm Motions
• Kinematic_Model module• serial_manipulator_model.h• template<T> struct Via_Sequence
– unsigned short step; //current via point– T pt[MAX_VIA_SEQ_LEN];
10
Motor Motions
• Trapezoidal Trajectories• Trajectory_Generator module• Trajectory class
– double _final_position;– double _max_velocity;– double _accel;– double _decel;
• Works with any initial/final velocities
Time
Velocity
11
Motor Motions
• Trapezoidal Trajectories• Trajectory_Generator module• Multi_Segment_Trajectory class
– double _final_position[];– double _max_velocity[];– double _accel[];– Double _decel[];
Time
Velocity
12
Motions in CLARAty
• Drive_Command• Drive_Sequence• Motion_Sequence• Via_Sequence• Trajectory• Multi_Segment_Trajectory
STOP
13
Goal 1: Add a New Motion
• Drive_Command• Drive_Sequence• Motion_Sequence• Via_Sequence• Trajectory• Multi_Segment_Trajectory
• Clothoid Trajectory (also known as polynomial spiral (Dillen 1990), Cornu
spiral)
STOP
14
Goal 2: Trajectory API
• Drive_Command• Drive_Sequence• Motion_Sequence• Via_Sequence• Trajectory• Multi_Segment_Tra
-jectory
• Trajectory_Trapezoid
• Trajectory_Arc …
15
Overview• Motivation
– Providing infrastructure to support integration of currently funded MTP based tasks
• Adding a new motion primitive to CLARAty locomotor: continuous driving
• Reviewing trajectories in CLARAty• Using the same trajectory representation for locomotion and
manipulation
• Overview of trajectories• Design Proposal
– Functions– Trajectories– Trajectory Generators– Locomotor
• Prototype implementation• Summary
16
Trajectories as Functions
Typically, a trajectory is a function: t S
where t is time, S is a space
For example, S could be W – workspace (manifold)
C – configuration space U – control space X – phase or state space
Time
Velocity
17
Trajectories as Functions
Why is : t S a function?
Definition: “a function from A to B is an object f such that every a in A is uniquely associated with an object f(a) in B.” (www.mathworld.com)
For robots, this means that a robot can’t be at more than one place at a time.
There is an IS-A relationship between Trajectories and Functions
18
Function_1D
• Function_1D already exists in CLARAty to facilitate using the numeric_solver
• Used for estimating the cost of a multi-arc maneuver
• Provides: double cost_function(double)
• Other trajectory generators also need numeric solvers, but a more extended “function” capabilities
• Our goal is to strengthen Function_1D and build a more capable version
19
Potential Uses for Functions
• Potentiometer calibration– Typically desired at least
higher order polynomial than we use
– To avoid typing a+bx+cx2…– Use a Function class
• Wheel slip– Could be a non-trivial
function of slope, soil properties, wheel parameters, etc.
– May use a Function class to do the math
20
Summary
• Capable and available Functions are useful math objects
• Have Function_1D already, but would like to strengthen
• Strong similarities between Functions and Trajectories
• Utilize the similarities to create a single, uniform interface
21
Overview• Motivation
– Adding a new motion to CLARAty locomotor– Reviewing locomotor and trajectories
• What is a trajectory?• Design Proposal
– Functions– Trajectories– Trajectory Generators– Locomotor
• Initial implementation• Summary
22
Piecewise Functions
• Often need sequences of functions• A piecewise function:
y = |x| can be expressed as
• Piecewise function IS-A function
• A sequence of trajectories is a piecewise function = function
• A sequence of trajectories = trajectory
-x, x < 0
x, x ≥ 0y(x) =
23
Composite Pattern
• Ideal for such relationships• Collection of units is of type unit• Used in modern word processors• Glyphs:
A
24
Composite Pattern
Component
• Pure virtual
• “Blueprint”
• Defines the interfaces
Composite
• Pure virtual
• Extended “Blueprint” for collections of components
• Defines the collections interfaces: add, remove, etc.
Component
IS-A
HAS-A
25
Composite Pattern
Function
• Pure virtual
• “Blueprint”
• Defines the interfaces
Composite Function
• Pure virtual
• Extended “Blueprint” for collections of components
• Defines the collections interfaces: add, remove, etc.
Component
IS-A
HAS-A
Polynomial_Function
Sine_ Function
Piecewise_Function
IS-AIS-A
…
27
Composite PatternPros: • Preserves the meaning of collection = unit
• Handles heterogeneous function types (e.g. polynomial and sine) in uniform fashion
• Built-in safety mechanisms
Cons:• Built using virtual inheritance: costly v_tables
28
Composite PatternPros: • Preserves the meaning of collection = unit
• Handles heterogeneous function types (e.g. polynomial and sine) in uniform fashion
• Built-in safety mechanisms
Cons:• Built using virtual inheritance: costly v_tables
29
Composite Pattern Modified
Function
• Fully functional
• Follows the established interface
Piecewise_Function
• Fully functional
• Follows the collections interfaces: add, remove, etc.
FunctionHAS-A
Polynomial_Function
Sine_ Function
Template
ONE OF
…
30
Composite Pattern Modified
template<int Order> class Polynomial_Function
• operator ()
• set_coeffs(*coeffs)
protected:
• _coeffs[Order + 1]
template<class Function> class Piecewise_Function
• operator()
• add_function()
• remove_function()
protected:
• vector<Function> _list;
• vector<double> _function_domains;
Template
32
Overview• Motivation
– Adding a new motion to CLARAty locomotor– Reviewing locomotor and trajectories
• What is a trajectory?• Design Proposal
– Functions– Trajectories– Trajectory Generators– Locomotor
• Initial implementation• Summary
33
Trajectory• Is-a piecewise function Y = f(X)
• Attaches meaning: – X (argument) = time– Y (value) = quantity this trajectory describes
• Could simply: typedef Piecewise_Function<Polynomial_F> Trajectory
• Helpful to derive and add convenience functions
34
Trajectory_Trapezoid
Trajectory_Trapezoid
Piecewise_Function <Polynomial_Function<1> >
- operator()
- add_function()
- remove_function()
- modify_function(fn_number, slope, offset)
• No virtual functions!• Trajectory_Trapezoid::operator() calls
Piecewise_Function directly
Polynomial_ Function<1>
35
Proposed Trajectories
• Trajectory_Trapezoid : public Piecewise_Function<Polynomial<1> >
• Trajectory_Clothoid_Cubic : public Piecewise_Function<Polynomial<3> >
• Trajectory_Constant : public Piecewise_Function<Polynomial<0> >
36
Overview• Motivation
– Adding a new motion to CLARAty locomotor– Reviewing locomotor and trajectories
• What is a trajectory?• Design
– Functions– Trajectories– Trajectory Generators– Locomotor
• Initial implementation• Summary
37
Trajectories vs. Generators
• Trajectory Generators use Trajectories as storage for results
• Generators– Topic of on-going research– New may be added later– E.g.: adding [Howard, Kelly 2005] in-progress– Future generators/planners might take
considerable computation
• Trajectories– Should not change– Should be “light-weight” for efficiency
38
Trajectory Generator Interface
• Input:– Start & Goal (HTrans<double>)– *What vehicle is this?
(Mechanism_Model)– *Are we going up/down-hill, how slippery
is it? (Terrain)
• Output:– Trajectory
* Optional
39
Trajectory Generator Interface
Trajectory_Generator
bool generate_trajectory(…)
Start, Goal (HTrans)
Mechanism Model *
Terrain *
* Optional
Trajectory
40
Trajectory Generator Design
• Use inheritance or convention to enforce the interface
• Communicate with other componets as parameters to corresponding functions
• Minimize dependance on other components
41
Overview• Motivation
– Adding a new motion to CLARAty locomotor– Reviewing locomotor and trajectories
• What is a trajectory?• Design
– Functions– Trajectories– Trajectory Generators– Locomotor
• Initial implementation• Summary
42
Locomotor and Trajectories
• Locomotor is the focal point of components that affect motions
• Locomotor’s key requirements: – Execute trajectories– Coordinate actuators– Accept body-frame trajectories– Accept wheel trajectories (some
trajectory generators produce these directly)
43
Locomotor and Trajectories
Locomotor
Trajectory Generator
Start, Goal (HTrans)
Terrain
Body-frame Trajectory
Mechanism Model
Motor Controller
Motor Trajectories
M
: Trajectory I/O
44
Locomotor and Trajectories
TrajectoryGenerator
Body-frame Trajectory
Wheel Trajectories
Motor Trajectory/SetpointsGenerator
jointsetpoints
Pose Estimateuses Wheel_Locomotor Model
compute_body_motion() +IMU + visual odometry …
Controller
position & heading
estimated position and heading
cmd. relativepose
MotorController
Mechanism/terrain
dynamics
Mechanism motion
Mechanism ModelBody-frame Wheel
Trajectories
Scienceplanner/navigator
: Trajectory I/O
45
Overview• Motivation
– Adding a new motion to CLARAty locomotor– Reviewing locomotor and trajectories
• What is a trajectory?• Design
– Functions– Trajectories– Trajectory Generators– Locomotor
• Prototype implementation• Further work
46
Prototype Implementation
• Added to Wheel_Locomotor:move_continuous(Htrans<double> Goal)
• Added to Wheel_Locomotor_Interface:move_continuous(Trajectory)
• The design proposed here is fully implemented and functional
• The Trajectory Generator delivered by Tom Howard, CMU (summer 2005) integrated with new framework
47
Prototype Implementation
• Given a start and goal (x, y, heading) generates a continuous path in a number of millisec.
• Passes the result to Wheel_Locomotor_Interface for execution
• New design tested and working in Linux and VxWorks
• Working on the benchtop, will try on the rover shortly
49
Overview• Motivation
– Adding a new motion to CLARAty locomotor– Reviewing locomotor and trajectories
• What is a trajectory?• Design
– Functions– Trajectories– Trajectory Generators– Locomotor
• Prototype implementation• Summary
50
Summary• Need for more capable infrastructure to support integration of
currently funded MTP based tasks
• Adding a new motion primitive to CLARAty locomotor: continuous driving
• Reviewing trajectories in CLARAty
• Overview of trajectories in general
• A complete Design Proposal– Functions– Trajectories– Trajectory Generators– Locomotor
• A working prototype implementation
52
Appendix
• Class declarations– Function_Element– Piecewise_Function– Trajectory_Trapezoid– Trajectory_Cubic_Clothoid– Trajectory_Generator_Cubic
53
Customized Motor Trajectories
• Using SW_TRAJ_CONTROL, an arbitrary profile can be executed by a motor
• Motor is really in POS_CONTROL
• A new setpoint (position) is sent to motor on every iteration of Periodic_Task
• This new setpoint can be anything: e.g. follow a clothoid trajectory
• We can execute clothoid paths in position, rather than velocity mode
55
Customized Motor Trajectories
• Caveat: transforming body-frame trajectory into wheel trajectories is a non-linear operation
• Wheel trajectories can’t be the same type as body trajectories
• Solution 1: keep a body trajectory in the wheel trajectory and post-process each operator()
• Solution 2: pre-process and create a sampled (non-parametric) version of the wheel trajectory
56
Locomotor and Trajectory Following
• There are different ways of following a trajectory, e.g.:– Pure pursuit– Predictive control on following error– Reactive trajectory generation
• Trajectory_Follower – an initial stage in trajectory execution
• Trajectory following already included in locomotor design
57
Locomotor and Trajectories
TrajectoryGenerator
Body-frame Trajectory
Wheel Trajectories
Motor Trajectory/SetpointsGenerator
jointsetpoints
Pose Estimateuses Wheel_Locomotor Model
compute_body_motion() +IMU + visual odometry …
Controller
position & heading
estimated position and heading
cmd. relativepose
MotorController
Mechanism/terrain
dynamics
Mechanism motion
Mechanism ModelBody-frame Wheel
Trajectories
Scienceplanner/navigator
Trajectory Following
58
References[1] Eric W. Weisstein. "Cornu Spiral." From MathWorld--A Wolfram Web Resource.
http://mathworld.wolfram.com/CornuSpiral.html