HebiTrajectory represents a precomputed trajectory path
Trajectories are created by a HebiTrajectoryGenerator and provide
a way to asynchronously move through a trajectory and to
visualize the planned path.
HebiTrajectory Methods:
getStartTime - returns the start time [s]
getEndTime - returns the end time [s]
getDuration - returns the total duration [s]
getState - returns position/velocity/acceleration
at any given time.
getWaypointTime - returns the timestamp for each waypoint
Example
% Create trajectory
trajGen = HebiTrajectoryGenerator(kin);
trajectory = trajGen.newJointMove([start; finish]);
% Visualize trajectory
t = 0:0.01:trajectory.getDuration();
[pos, vel, accel] = trajectory.getState(t);
HebiUtils.plotTrajectory(trajectory);
% Manually execute position/velocity/effort trajectory
cmd = CommandStruct();
t0 = tic();
t = toc(t0);
while t < trajectory.getDuration()
fbk = group.getNextFeedback();
% get state at current time interval
t = toc(t0);
[pos, vel, accel] = trajectory.getState(t);
% compensate for gravity
gravityVec = -[fbk.accelX(1) fbk.accelY(1) fbk.accelZ(1)];
gravCompEffort = kin.getGravCompEfforts(fbk.position, gravityVec);
% compensate for accelerations
accelCompEffort = kin.getDynamicCompEfforts(...
fbk.position, ... % Used for calculating jacobian
pos, vel, accel);
% send to hardware
cmd.position = pos;
cmd.velocity = vel;
cmd.effort = gravCompEffort + accelCompEffort;
group.send(cmd);
end