getDynamicCompEfforts calculates joint efforts that
compensate for dynamic motions
This method computes the efforts that are required to
accelerate the body masses as determined from the specified
positions, velocities, and accelerations.
A recommended way of determining a set of desired
positions, velocities, and accelerations is to use the
HebiTrajectoryGenerator to create minimum-jerk trajectories
for the motion of the system.
'Positions' argument expects a vector of positions of
all degrees of freedom, used for computing the Jacobian,
where (effort = J' * desiredForces)
'TargetPositions', 'TargetVelocities', and
'TargetAccelerations' typically come from some sort of
trajectory generation function, such as a minimum-jerk
trajectory, or a sinusoidal trajectory (example below).
% Compensate for dynamics of sinusoidal motion
fbk = group.getNextFeedback();
time = 0.0;
freq = 1.0 * (2*pi); % 1 Hz
amp = 1.0; % rad
position = amp * sin( freq * time );
velocity = freq * amp * cos( freq*time );
accel = -freq^2 * amp * sin( freq*time );
cmdPositions = position * ones(1,group.getNumModules);
cmdVelocities = velocity * ones(1,group.getNumModules);
cmdAccelerations = accel * ones(1,group.getNumModules);
efforts = kin.getDynamicCompEfforts(...