My guess is that either "Energyconsumption" depends discontinuously on waypointTimes, that epsilon in the compuation of the gradient is not adequate or that the alpha in the steplength of the gradient must be chosen more carefully.
clear all
clc
close all
format long
global waypointTimes_start trajTimes;
waypointTimes_start = 1*[0 5 10 15 20];
% The trajectory will be divided into "Number_of_steps" steps:
Number_of_steps =20 ;
% Time is discretized in steps of duration ts:
ts = waypointTimes_start(length(waypointTimes_start))/Number_of_steps;
% Vector of discretized times:
trajTimes = 0:ts:waypointTimes_start(end);
waypointTimes_guess = 1 * [0 5 10 15 20];
% Fix the first and last waypoint times
waypointTimes_guess(1) = 0; % Fix the first waypoint time
waypointTimes_guess(end) = 20; % Fix the last waypoint time
% Set optimization parameters
alfa = 1e-2;
maxIterations = 10;%100;
tolerance = 1e-4;
% Perform gradient descent optimization
waypointTimes = waypointTimes_guess;
for iter = 1:maxIterations
energy(iter)=generateEnergy(waypointTimes);
generateEnergy(waypointTimes);
gradient = computeGradient(waypointTimes);
waypointTimes(2:end-1)=waypointTimes(2:end-1)-alfa*gradient(2:end-1);
number_iterations(iter)=iter;
end
plot(number_iterations,energy,'b -');
xlabel('iterations')
ylabel('Energy [W]')
function Energyconsumption = generateEnergy(waypointTimes)
global trajTimes;
robot = loadrobot("kinovaGen3");
robot.Gravity = [0 0 -9.81];
robot.DataFormat = 'column';
q_home = [0 15 180 -130 0 55 90]'*pi/180;
eeName = string(robot.BodyNames(length(robot.BodyNames)));
T_home = getTransform(robot, q_home, eeName);
toolPositionHome = T_home(1:3,4);
% Define a Set of Waypoints Based on the Tool Position
waypoints = toolPositionHome + ...
[0 0 0 ; -0.1 0.2 0.2 ; -0.2 0 0.1 ; -0.1 -0.2 0.2 ; 0 0 0]';
orientations = [pi/2 0 pi/2;
(pi/2)+(pi/4) 0 pi/2;
pi/2 0 pi;
(pi/2)-(pi/4) 0 pi/2;
pi/2 0 pi/2]';
%time from the start to the waypoints
% The trajectory will be divided into "Number_of_steps" steps:
% Time is discretized in steps of duration ts:
% Vector of discretized times:
% Velocities joints at waypoint:
waypointVels= 1*[0 0 0 0 0 0 0;
-1 -1 1 -1 1 0 -1;
-1 1 1 1 1 0 -1;
1 1 -1 -1 1 0 1;
0 0 0 0 0 0 0]';
% Accelerations at waypoints is computed from velocity. But first we must
% allocate memory for accelerations:
%accelerations joints at waypoints
waypointAccels = 1*[0 0 0 0 0 0 0;
-1 -1 1 -1 1 0 -1;
-1 1 1 1 1 0 -1;
1 1 -1 -1 1 0 1;
0 0 0 0 0 0 0]' ;
waypointAccelTimes = diff(waypointTimes)/4;
ik = inverseKinematics('RigidBodyTree',robot);
% Weights for pose tolerances, specified as a six-element vector.
% The first three elements of the vector correspond to the weights
% on the error in orientation for the desired pose.
% The last three elements of the vector correspond to the weights on
% the error in the xyz position for the desired pose.
ikWeights = [1 1 1 1 1 1];
% inverseKinematics has several additional options (e.g. max number of
% interations, joint limits, tolerances, etc.)
% The numerical solution starts from an initial guess, taken here as the
% home configuration:
ikInitGuess = q_home';
% Angles are adapted so to be between -pi e +pi:
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;
% The inverse kinematics problem can be solved with reference to
% positions only, or we can also include the orientation of the EE
% by setting the following Boolean flag to "true":
includeOrientation = false;
numWaypoints = size(waypoints,2); % Number of waypoints
numJoints = numel(robot.homeConfiguration);
% Allocate memory:
jointWaypoints = zeros(numJoints,numWaypoints);
% Waypoints have been defined in Cartesian coordinates. They must be
% converted to the corresponding homogeneous transformation. This is done
% using "trvec2tform".
% Orientations have been described using Euler's angles (Tait-Bryan,
% extrinsic ZYX). They must be transformed into homogeneous transformation.
% This is done by the command "eul2tform".
%trajectory following loop joint space
for idx = 1:numWaypoints
if includeOrientation
tgtPose = trvec2tform(waypoints(:,idx)') * eul2tform(orientations(:,idx)');
else
tgtPose = trvec2tform(waypoints(:,idx)') * eul2tform([pi/2 0 pi/2]);
end
%IK = inverse kinematics
[config,~] = ik(eeName,tgtPose,ikWeights',ikInitGuess');
jointWaypoints(:,idx) = config';
end
trajType = 'trap';
switch trajType
case 'trap'
[q,qd,qdd] = trapveltraj(jointWaypoints,numel(trajTimes), ...
'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ...
'EndTime',repmat(diff(waypointTimes),[numJoints 1]));
case 'cubic'
[q,qd,qdd] = cubicpolytraj(jointWaypoints,waypointTimes,trajTimes, ...
'VelocityBoundaryCondition',waypointVels);
case 'quintic'
[q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, ...
'VelocityBoundaryCondition',waypointVels, ...
'AccelerationBoundaryCondition',waypointAccels);
case 'bspline'
ctrlpoints = jointWaypoints; % Can adapt this as needed
[q,qd,qdd] = bsplinepolytraj(ctrlpoints,waypointTimes([1 end]),trajTimes);
% Remove the first velocity sample
qd(:,1) = zeros (7,1);
otherwise
error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline''');
end
% Let's compute joint torques and power.
%
% Number of time intervals:
N = numel(trajTimes);
% jointTorq = inverseDynamics(gen3,q(:,1)',qd(:,1)',qdd(:,1)')
% preallocate memory:
jointTorques = zeros(numJoints,N);
Power = zeros(1,N); % vector with N 0's
TotalPower = zeros(1,N);
JointPower = zeros(numJoints,N);
for i = 1:N
jointTorques(:,i) = inverseDynamics(robot,q(:,i),qd(:,i),qdd(:,i));
Power(i) = dot(qd(:,i),jointTorques(:,i)); % scalar product
JointPower(:,i) = qd(:,i).*jointTorques(:,i); % element-wise product
TotalPower(i) = sumabs(JointPower(:,i)); % sum of absolute values
end
%calculating energyconsumption
Energyconsumption = trapz(trajTimes,TotalPower);
end
function gradient = computeGradient(waypointTimes)
% Compute gradient using numerical differentiation
epsilon = 1e-4; % Small perturbation
numWaypoints = numel(waypointTimes);
gradient = zeros(size(waypointTimes));
Original_energy = generateEnergy(waypointTimes);
for i = 2:numWaypoints-1 % Excluding the first and last waypoint times
% Perturb the waypoint time
wp = waypointTimes(i);
waypointTimes(i) = waypointTimes(i) + epsilon;
% Compute energy consumption with perturbed waypoint times
energy_perturbed = generateEnergy(waypointTimes);
% Compute gradient using central difference
gradient(i) = (energy_perturbed - Original_energy) / epsilon;
waypointTimes(i) = wp;
end
end
% EXERCISE: adapt the script so to interpolate waypoints in the task
% space
