gradient descent optimization doesnt work in code?

This is the code right now it does the oppisite of a descent optimization. Do you know what is wrong with it?
I tried to minimize the energy of a robot path by changing the waypointtimes, right now it maximizes it for some reason?
clear all
clc
close all
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 = 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;
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-1; % Small perturbation
numWaypoints = numel(waypointTimes);
gradient = zeros(size(waypointTimes)-1);

for i = 2:numWaypoints-1 % Excluding the first and last waypoint times
% Perturb the waypoint time

global energy_perturbed;

waypointTimes_perturbed = waypointTimes;
waypointTimes_perturbed(i) = waypointTimes_perturbed(i) + epsilon;

% Compute energy consumption with perturbed waypoint times
energy_perturbed = generateEnergy(waypointTimes_perturbed);
Original_energy = generateEnergy(waypointTimes);
% Compute gradient using central difference
gradient(i-1) = (energy_perturbed – Original_energy) / epsilon;
end
end
% EXERCISE: adapt the script so to interpolate waypoints in the task
% spaceThis is the code right now it does the oppisite of a descent optimization. Do you know what is wrong with it?
I tried to minimize the energy of a robot path by changing the waypointtimes, right now it maximizes it for some reason?
clear all
clc
close all
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 = 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;
number_iterations(iter)=iter;

case ‘cubic’
[q,qd,qdd] = cubicpolytraj(jointWaypoints,waypointTimes,trajTimes, …
‘VelocityBoundaryCondition’,waypointVels);

case ‘quintic’
[q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, …
‘VelocityBoundaryCondition’,waypointVels, …
‘AccelerationBoundaryCondition’,waypointAccels);

end
%calculating energyconsumption
Energyconsumption = trapz(trajTimes,TotalPower);
end
function gradient = computeGradient(waypointTimes)
% Compute gradient using numerical differentiation

epsilon = 1e-1; % Small perturbation
numWaypoints = numel(waypointTimes);
gradient = zeros(size(waypointTimes)-1);

for i = 2:numWaypoints-1 % Excluding the first and last waypoint times
% Perturb the waypoint time

global energy_perturbed;

waypointTimes_perturbed = waypointTimes;
waypointTimes_perturbed(i) = waypointTimes_perturbed(i) + epsilon;

% Compute energy consumption with perturbed waypoint times
energy_perturbed = generateEnergy(waypointTimes_perturbed);
Original_energy = generateEnergy(waypointTimes);
% Compute gradient using central difference
gradient(i-1) = (energy_perturbed – Original_energy) / epsilon;
end
end
% EXERCISE: adapt the script so to interpolate waypoints in the task
% space This is the code right now it does the oppisite of a descent optimization. Do you know what is wrong with it?
I tried to minimize the energy of a robot path by changing the waypointtimes, right now it maximizes it for some reason?
clear all
clc
close all
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 = 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;
number_iterations(iter)=iter;

case ‘cubic’
[q,qd,qdd] = cubicpolytraj(jointWaypoints,waypointTimes,trajTimes, …
‘VelocityBoundaryCondition’,waypointVels);

case ‘quintic’
[q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, …
‘VelocityBoundaryCondition’,waypointVels, …
‘AccelerationBoundaryCondition’,waypointAccels);

end
%calculating energyconsumption
Energyconsumption = trapz(trajTimes,TotalPower);
end
function gradient = computeGradient(waypointTimes)
% Compute gradient using numerical differentiation

epsilon = 1e-1; % Small perturbation
numWaypoints = numel(waypointTimes);
gradient = zeros(size(waypointTimes)-1);

for i = 2:numWaypoints-1 % Excluding the first and last waypoint times
% Perturb the waypoint time

global energy_perturbed;

waypointTimes_perturbed = waypointTimes;
waypointTimes_perturbed(i) = waypointTimes_perturbed(i) + epsilon;

Cart

Cart