Robotics CITS3241

Exercise Sheet 6: Generating Trajectories using Bezier Curves

function P = bezier(t, CP)
%
% bezier evaluates a point along a Bezier curve
%
% Arguments:
% t is parameter along the curve 0 <= t <= 1
% CP is a nxm vector of n control points in m-space eg.   [x y z;
%                                                          x y z;
%                                                            ... ]
% Note that for some applications you may want m to be larger than 3.
% For example, if you are interpolating a robot path in 6D joint space m
% will have a value of 6.
%
% Returns
% P  a 1xm vector   eg [x,y,z] giving the point on the curve

[n, m] = size(CP)   % will give you values of n and m

MATLAB provides a function nchoosek that calculates n down i - the number of ways of choosing i from n.

2) Write a matlab function

function plotbezier(dt, CP)

%  Arguments:
%  dt   - the parameter increment size that you want points along the 
%         curve to be plotted at.  Remember the parameter varies from 
%         0 - 1 along the curve, dt = .01 will give 100 points
%  CP   - a nxm vector of n control points in 2 or 3-space 
%
%  Produces:
%  - a plot of the characteristic polygon
%  - a plot of the curve as a series of equi-spaced points in terms of
%    the curve parameter t.
%  - a separate plot of velocity variation as a function of the parameter t

You may need conditional statements to control the type of plot depending on whether the data are 2D or 3D.

The velocity plot is a plot of the distance moved between each successive parameter point t as one travels along the curve (if the parameter t is time, this plot represents the velocity variation along the curve). Note: MATLAB has a 'norm' function that you will find useful.

To produce two plots in the one window you can use the 'subplot' command (see its help file). For example, to display your trajectory plot and your velocity plots side-by-side use the following:

subplot(1,2,1), plot3(X,Y,Z,'.'), title('trajectory');
subplot(1,2,2), plot([0:npts-1],velocityData), title('velocity');

The code above displays the trajectory as a series of dots and labels the two plots. Displaying the trajectory as a series of dots allows one to 'see' the velocity along the curve. Where the dots are close together the velocity will be low, where they are far apart the velocity will be high.

Generate a variety of curves, experimenting with control point placement. Try curves with 2 and 3 control points coincident at each end. Points of high curvature on Bezier curves tend to have the trajectory points close together. This is a nice side effect - we would want the robot to slow down on sharp corners.

Blending cubic Bezier curves together:

The 'trouble' with Bezier curves is that they can be too smooth. It can be hard to construct a path that twists and turns to a great degree. A way around this is to use a number of low-order curves spliced together.

Cubic Bezier curves can be blended together smoothly if one ensures that the end point of one curve matches the start of the next, and the end velocity of one curve matches the start velocity of the next. This is simply achieved by placing control points as shown below.

Your task is to write a series of functions that allow you to build up a sequence of control points for a series of blended cubic Bezier curves. The suggested functions are:

  function CP = startCubicBez(P, vel)

  x0 y0 z0
  x1 y1 z1

  function CP = goThrough(CP, P, vel)

  x0 y0 z0 |- from a previous call to startCubicBez or goThrough
  x1 y1 z1 |

  x2 y2 z2 |- two new control points to complete previous curve
  x3 y3 z3 |

  x0 y0 z0 |- two new control points to start next curve
  x1 y1 z1 |

  function CP = stopCubicBez(CP, P, vel)

Please note that the velocity parameter to the startCubicBez, goThrough and stopCubicBez functions is a 3-vector, not a scalar value. The direction of the vector specifies the direction of motion through the point, its magnitude specifies the speed.

These functions give us the basis of an elementary 'language' for specifying robot motion. Finally we want to see the resulting path so we need a function with the following specification:

  function plotCubicBez(dt, CP)

% This function treats the matrix of control points as a sequence of
% groupings of 4 control points at a time.  Each group of 4 control
% points forms a cubic Bezier curve.  The function simply plots each
% cubic curve in sequence.
%
%  Arguments:
%  dt   - the parameter increment size that you want points along the 
%         curve to be plotted at.  Remember the parameter varies from 
%         0 - 1 along the curve, dt = .01 will give 100 points per curve
%  CP   - a nxm vector of n control points in 3-space.  The  number of
%         control points must be a multiple of 4.
%
%  Produces:
%  - a plot of the characteristic polygon
%  - a plot of the curve as a series of equi-spaced points in terms of
%    the curve parameter t.
%  - a separate plot of velocity variation as a function of the parameter t

These functions startCubicBez, goThrough, stopCubicBez, plotCubicBez will be used later when you implement Jacobian motion rate control. In the meantime you can use your bezier function to implement joint-space interpolation control of your 6 DOF stanford arm.

Write a function that meets the following specification

function stanfordJointInterp(offset, initialJoints, Tfinal, dt)

% Function to perform joint space interpolation control of a stanford type robot
% arm.  
%
% Arguments:
%            offset        - [offset1 offset6] specifyig the two fixed offsets 
%                            that define the arm.
%            initialJoints - a 6-vector specifying the joint positions 
%                            of the robot at its initial position.
%            Tfinal        - The homogeneous transform that describes the
%                            desired effector frame at the robots final position.
%            dt            - The parameter increment rate to use as the robot
%                            travels along the bezier path in joint space.
%
%
% The initial position of the robot arm is specified by the set of initial joint
% positions and the final position is specified by a desired end effector 
% transformation frame.  The function solves the inverse kinematics of the arm for
% the final position and selects the solution that is closest to the initial set of
% joint positions.  A straight line Bezier path in joint space is then generated 
% from the initial joint positions to the final joint positions.  The Bezier path
% is a simple cubic path with control points superimposed at each end of the path
% to ensure start and finish velocities of the joints are zero.  An animation of
% the robot is displayed as it travels along the path.
%