1 trajectories in claraty mihail pivtoraiko. 2 overview motivation –providing infrastructure to...

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 + cs2 + ds3

v = trapezoid, etc.

R

(s)

s

5





manipulation



• 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





manipulation




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


– Adding a new motion to CLARAty locomotor– Reviewing locomotor and trajectories

• What is a trajectory?• Design Proposal


• 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

…

26

Composite Pattern: Implementation

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

31

Composite Pattern ModifiedImplementation

32



• What is a trajectory?• Design Proposal



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



• What is a trajectory?• Design



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






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

Trajectory Generator

Start, Goal (HTrans)

Terrain

Body-frame Trajectory

Mechanism Model

Motor Controller

Motor Trajectories

M

: Trajectory I/O

44


TrajectoryGenerator


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





• 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


• 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


• [animation]

49






50

Summary• Need for more capable infrastructure to support integration of

currently funded MTP based tasks


• 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


55


• 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


TrajectoryGenerator


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

http://mathworld.wolfram.com/

http://mathworld.wolfram.com/CornuSpiral.html

1 trajectories in claraty mihail pivtoraiko. 2 overview motivation –providing infrastructure to...

Documents

trajectory representation

trajectory apidrive

sequencetraj trajectory

claraty locomotor

new motion primitive

funded mtp

claraty uptodate

drive module