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 
% space

Answer 1

[Torsten]

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

[Torsten]

"fmincon" is also not able to reduce the value of the objective.

global 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);
waypointTimes0 = waypointTimes_start(2:end-1);
generateEnergy(waypointTimes0)

ans = 20.1394

waypointTimes = fmincon(@generateEnergy,waypointTimes0)

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the value of the step size tolerance and constraints are 
satisfied to within the value of the constraint tolerance.

waypointTimes = 1x3

5 10 15

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

generateEnergy(waypointTimes)

ans = 20.1394

function Energyconsumption = generateEnergy(waypointTimes_red)
global trajTimes
waypointTimes = [0 waypointTimes_red 20];
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

[Tim De Witte]

now its doing the reverse so I think I am close but it should be the other way around which is weird. But what do you think I should take for step size?

[Torsten]

You should first use a professional optimizer like "fmincon" in order to see if you even have a chance to succeed with your own code. At the moment, this does not seem to be the case.

[Tim De Witte]

My promotor already did this so it is possible

[Torsten]

It seems that your function "generateEnergy" has discontinuities in its derivatives at integers. This will cause problems for the optimization.

global 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);
waypoints2 = 5:0.05:9.9;
for i = 1:numel(waypoints2)
    energy2(i) = generateEnergy([waypoints2(i),10,15]);
end
plot(waypoints2,energy2)

function Energyconsumption = generateEnergy(waypointTimes_red)
global trajTimes
waypointTimes = [0 waypointTimes_red 20];
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