Trajectory Generation• Goal:
– Turn a specified Cartesian-space trajectory of Pe into appropriate joint position reference values
• Steps:– Use inverse kinematics of a robot
manipulator arm to find joint values for any particular location of Pe
– Use sampling and curve fitting to reduce computation
• Output:– a series of joint position/velocity reference
values to send to the controller
5 Step Process for Trajectory Generation
1. Obtain function for workspace path
2. Sample function to get discrete joint pts
3. Apply IK & Jacobian calculations
4. Fit function to joint points5. Sample to get discrete
reference points
1. C
2. D
3. D
4. C
5. D
C=continuous
D=discrete
Step One: Continuous Fcn
• Obtain an analytic function to describe motion with respect to the base frame
• Obtain rate of change of location
Step 2: Sample
• Sample the trajectory to obtain a finite number, m, of sample points on the continuous trajectory:
• Sample rate of change
Step 3: IK & J
• (a) Use inverse kinematics to convert each Cartesian trajectory sample point vector, into a corresponding joint space vector,– Handle multiple solutions,
admissibility, etc.
Step 3: IK & J
• (b) Use the inverse Jacobian relation to convert each velocity vector, into a corresponding joint speed vector, – Handle singular configurations
Step 4: Fit Continuous Curve to Joint Points
• Use the sequence of vectors and i=1,…,m to generate continuous expressions for each joint and j=1,…,dof which pass through or sufficiently near to each of joint space sample points, and rate of change sample points, to produce continuous joint space trajectories for each joint.
Step 4: Fit Continuous Curve to Joint Points
Spline or Polynomial Fit
& derivatives:
Step 4: Fit Continuous Curve to Joint Points
Let’s look at fitting a curve to one interval
Step 4. Fit Continuous Curve to Joint Points
• Fit a continuous function, q(t) to the points:
• Time info – from original sampling• For now use notation (get rid of
subscripts i and i+1):
t=t0
t=tf
Step 4. Fit Continuous Curve to Joint Points
• Splines, polynomials,…• To match position, velocity and
acceleration at end points use a quintic polynomial (6 parameters to match the 6 unknowns):
Step 4. Fit Continuous Curve to Joint Points
• Note: To match only position and velocity at end points use a cubic polynomial (4 parameters to match the 4 unknowns):
Step 4. Fit Continuous Curve to Joint Points
• Use endpoints and time values in quintic polynomial (6 linear equations, 6 unknowns)
Step 4. Fit Continuous Curve to Joint Points
• In matrix form:
Step 4. Fit Continuous Curve to Joint Points
• In matrix form:
Step 4: Computational Thoughts
• Need to perform fit for each joint but…
on each TIME interval, this matrix is the same for each joint – compute inverse only once
Step 4: Fit Continuous Curve to Joint Points
Piecewise polynomials: one polynomial for each joint for each timte interval (and we can easily take derivatives)
…..
one for each time interval(i, i+1)
Step 5: Sample Joint Curve
• Sample each continuous joint trajectory to generate a sequence of discrete reference values for each joint, where ttotal/N is the sampling period used.
Sample
Step 5: Sample Joint Curve
• Sample joint speeds.
Sample
5 Step Process for Trajectory Generation
1. Workspace path function2. Sample -- discrete joint pts3. Apply IK & Jacobian
calculations4. Joint function5. Sample -- discrete reference
points
1. C2. m pts3. m pts
4. C5. N pts
Note: N >> m
m ~ 10 N ~ 1000+
Example: Linear Motion• Step One: Express line as continuous
function (i.e. parameterize by time):– x(t), y(t), (t)
• Suppose we specify the line y= mx +b• Want to move along line with constant
speed, v
Parameterize the Line
• Equation of line
• Differentiate
• Velocity vector
• Substitute
Parameterize the Line
• Velocity
• Speed (magnitude)
• Solve (pick appropriate sign)
Parameterize the Line
• Velocity,
Sample Continuous Path Fcn
• Use M samples for total time, ttotal
time
Example Line• Let’s take a 2-link planar arm with
– Link lengths: l1 = 4, l2 = 3 m– Line y= - x + l1+l2/4 or y=-x +4.75
with speed, v = 3m/s– Start point = (0, l1 + l2 /4)=(0,4.75)– endpoint = (l1 + l2 /4,0)=(4.75,0)– constant speed
• Note, in practice, speed usually follows a trapezoidal profile with acceleration/deceleration at start/end of the motion
Example Line
• Using our 2-link planar data the path has
• And for constant speed will take
Example Line
• We will use M=9 sample points
• And for constant speed we will have
Step 3: IK & Jacobian
• inverse kinematics:
– where
Step 3: IK & Jacobian
• Jacobian relationship:
Step 3: Discrete joint values
Step 3: Discrete Joint Velocity Values
Step 4: Fit Cubic To Positions/Speeds
rmse = 0.014 rad ~ 1 deg
Curve Fitting Comments
• Typically, a single cubic is not sufficient for the entire motion
• A quintic polynomial can fit position, velocity and accelerations of end-points
• What are the implications of fitting only position/velocity?
Reference Positions with 0.02s timesteps (sample cubic function)
Cartesian vs. Joint spaceCONS • Actual robot
position is sometimes unclear, particularly in presence of known obstacles
PROS • Actual control of
the robot occurs in joint space
• Simpler to plan trajectories in real-time; less computation
• No problem with singularities
Cartesian vs. Joint spaceCONS • Need Inverse kinematics
(can be multi-valued)• Smooth trajectory in
Cartesian space may not map to continuous joint space trajectory
• Joint limits may prevent position from being realized
• End-effector paths may not be safe for rest of manipulator
• computational demand and analytic complexity
PROS • We usually desire a
Cartesian path.• Easy to visualize the
trajectory• Occurs in many robotic
activities• Shortest Euclidean path • Straight line path
minimizes inertial forces • Cartesian trajectory can
be robot independent
Path not in workspace
May need to flip between configurations
Start & goal in different solution branches