1 Trajectories in CLARAty Mihail Pivtoraiko

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 + cs 2 + ds 3 v = trapezoid, etc. R (s)(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_Sequence; STOP

8 Wheel Motions Motion_Control module Motion_Sequence –double ** _motor_positions; –Matrix _motor_commands (*); –std::vector _drive_commands;

9 Arm Motions Kinematic_Model module serial_manipulator_model.h template 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.” ( ) 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 + cx 2 … –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 ≥ 0 y(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-A …

26 Composite Pattern: Implementation

27 Composite Pattern Pros: 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 Pattern Pros: 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. Function HAS-A Polynomial_ Function Sine_ Function Template ONE OF …

30 Composite Pattern Modified template class Polynomial_Function operator () set_coeffs(*coeffs) protected: _coeffs[Order + 1] template class Piecewise_Function operator() add_function() remove_function() protected: vector _list; vector _function_domains; Template

31 Composite Pattern Modified Implementation

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 Trajectory Helpful to derive and add convenience functions

34 Trajectory_Trapezoid Piecewise_Function > - operator() - add_function() - remove_function() - modify_function(fn_number, slope, offset) No virtual functions! Trajectory_Trapezoid::operator() calls Piecewise_Function directly Polynomial_ Function

35 Proposed Trajectories Trajectory_Trapezoid : public Piecewise_Function > Trajectory_Clothoid_Cubic : public Piecewise_Function > Trajectory_Constant : public Piecewise_Function >

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 ) –*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 Trajectory Generator Body-frame Trajectory Wheel Trajectories Motor Trajectory/ Setpoints Generator joint setpoints Pose Estimate uses Wheel_Locomotor Model compute_body_motion() + IMU + visual odometry … Controller position & heading estimated position and heading cmd. relative pose Motor Controller Mechanism/ terrain dynamics Mechanism motion Mechanism Model Body-frame  Wheel Trajectories Science planner/ 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 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

48 Prototype Implementation [animation]

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

51 Thank you Q / A

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

54 Customized Motor Trajectories

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 Trajectory Generator Body-frame Trajectory Wheel Trajectories Motor Trajectory/ Setpoints Generator joint setpoints Pose Estimate uses Wheel_Locomotor Model compute_body_motion() + IMU + visual odometry … Controller position & heading estimated position and heading cmd. relative pose Motor Controller Mechanism/ terrain dynamics Mechanism motion Mechanism Model Body-frame  Wheel Trajectories Science planner/ navigator Trajectory Following

58 References [1] Eric W. Weisstein. "Cornu Spiral." From MathWorld --A Wolfram Web Resource. MathWorld