MATLAB File Help: HebiTrajectoryGenerator
HebiTrajectoryGenerator
  HebiTrajectoryGenerator provides support for creating trajectories
 
  For series elastic actuators such as the X-series modules it is
  important to command smooth trajectories to avoid oscillations
  induced by their mechanical compliance. Additionally, it helps to not 
  only command positions, but also velocities and efforts (torques). 
 
    HebiTrajectoryGenerator Methods (non-blocking):
 
       newJointMove   - Creates a trajectory through joint waypoints
                        that does not make use of kinematics.
 
       newLinearMove  - Creates a trajectory through xyz waypoints
                        that uses knowledge of kinematics to travel in
                        straight lines in workspace.
 
       setMinDuration - Sets the minimum duration that gets used when
                        relying on the trajectory generator's internal
                        timing heuristic.
       getMinDuration - Returns the currently set minimum trajectory
                        duration.
 
       setSpeedFactor - Sets a scalar that adjusts the speed of all
                        trajectories. A value of 1.0 will run at full
                        speed, and smaller values proportionally
                        slow down the timing of the trajectory. This
                        applies to both the internal timing heuristic
                        as well as user supplied time vectors.
       getSpeedFactor - Returns currently set speed factor.
 
       setAlgorithm   - Allows use of legacy trajectory algorithms.
       getAlgorithm   - Returns the active trajectory algorithm.
 
    HebiTrajectoryGenerator Methods (blocking):
 
       executeTrajectory - Executes a trajectory that is generated with
                           either newJointMove or newLinearMove.  This
                           method provides more control on how the
                           trajectory is setup, allowing multiple
                           intermediate waypoints and additional
                           constraints such as user-defined waypoint
                           timing and non-zero waypoint velocities and
                           accelerations.                   
 
  Note that the following parts of the API require additional knowledge 
  about the hardware:
 
    - The internal heuristic to automatically determine an appropriate
      time vector requires knowledge of joint velocity limits
 
    - Linear movements (newLinearMove) require a fully 
      specified kinematic model (HebiKinematics object) for IK
 
    - executeTrajectory requires a fully specified kinematic model
      (HebiKinematics object) if gravity or acceleration compensation 
      are enabled.
 
  Execution of a trajectory using the simplified 'blocking' API
  additionally requires a HebiGroup in order to communicate with the
  actuator hardware.
 
    Example (blocking API)
       kin = HebiKinematics();
       trajGen = HebiTrajectoryGenerator();
 
       % Move between a set of waypoints using blocking calls
       % (Assumes a group has already been created)
       numWaypoints = 5;
       timeToMove = 3; % seconds
 
       positions = rand(numWaypoints, group.getNumModules);
       time = [0 timeToMove];
 
       for i = 2:numWaypoints
            start = positions(i-1, :);
            finish = positions(i, :);
            trajectory = trajGen.newJointMove(...
                [start; finish], ...
                'time', time);
            trajGen.executeTrajectory(group, trajectory);
       end
 
    Example (non-blocking API)
       kin = HebiKinematics();
       trajGen = HebiTrajectoryGenerator();
 
       % Move between a set of waypoints using blocking calls
       % (Assumes a group has already been created)
       numWaypoints = 5;
       timeToMove = 3; % seconds
 
       positions = rand(numWaypoints, group.getNumModules);
       time = [0 timeToMove];
 
       % Execute trajectory open-loop in position and velocity
       cmd = CommandStruct();
 
       for i = 2:numWaypoints
          start = positions(i-1, :);
          finish = positions(i, :);
          trajectory = trajGen.newJointMove(...
                [start; finish], ...
                'time', time);
 
          fbk = group.getNextFeedback();
          t0 = fbk.time;
          t = 0;
  
          while t < trajectory.getDuration()
             fbk = group.getNextFeedback();
             t = fbk.time - t0;
             [cmd.position, cmd.velocity, ~] = trajectory.getState(t);
             group.send(cmd);
          end
       end
 
    Example (automatic waypoint timing) 
       % Generate a trajectory for XY velocities of a mobile base where
       % speeds are limited to 1 m/s (or whatever units you are using).
       velocityLimits = [ -1 1;
                          -1 1 ];
       trajGen = HebiTrajectoryGenerator(velocityLimits);
       positions = [ 0 0;
                    -2 1 ];
       trajectory = trajGen.newJointMove(positions);
 
       % Visualize points along the trajectory
       HebiUtils.plotTrajectory(trajectory);
 
       % Execute trajectory open-loop in position and velocity
       cmd = CommandStruct();
       t0 = tic();
       t = toc(t0);
       while t < trajectory.getDuration()
           t = toc(t0);
           fbk = group.getNextFeedback();  % limits loop rate
           [cmd.position, cmd.velocity, ~] = trajectory.getState(t);
           group.send(cmd);
       end
See also
Class Details
Sealed true
Construct on load false
Constructor Summary
HebiTrajectoryGenerator Provides methods to generate trajectories 
Method Summary
  executeTrajectory executes a trajectory and returns when done 
  getAlgorithm gets the trajectory algorithm that is currently 
  getMinDuration returns the minimum duration between two  
  getSpeedFactor gets the speed factor that adjusts the speed 
  moveJoint creates a joint-space trajectorythat moves  
  moveLinear moves between waypoints in straight lines in 
  newJointMove creates a trajectory through joint waypoints 
  newLinearMove creates a trajectory through xyz waypoints 
  setAlgorithm sets the trajectory algorithm 
  setMinDuration sets the minimum duration between two adjacent  
  setSpeedFactor sets the speed factor that adjusts the speed