Energy optimization robot paths

% Adapted from:
% https://nl.mathworks.com/help/supportpkg/robotmanipulator/ug/
% generate-trajectory-endeffector.html
clear all
clc
close all
global  robot numJoints trajTimes waypointTimes ;
robot = loadrobot("kinovaGen3"); 
robot.Gravity = [0 0 -9.81];
robot.DataFormat = 'column';
%time from the start to the waypoints
waypointTimes = 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(length(waypointTimes))/Number_of_steps;
% Vector of discretized times:
trajTimes = 0:ts:waypointTimes(end);
numJoints = numel(robot.homeConfiguration);
% Allocate memory:
% Initial guess for waypoint times
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
learningRate = 0.1;
maxIterations = 100;
tolerance = 1e-6;
% Perform gradient descent optimization
waypointTimes = waypointTimes_guess;
for iter = 1:maxIterations
    % Compute trajectory using current waypoint times
    [q,qd,qdd] =generateTrajectory(waypointTimes,trajTimes);
    % Compute energy consumption
    energy = computeEnergyConsumption(q, qd, qdd, trajTimes);
    % Compute gradient
    gradient = computeGradient(q, qd, qdd, waypointTimes,trajTimes);
    % Update waypoint times using gradient descent
    waypointTimes(2:end-1) = waypointTimes(2:end-1) - learningRate * gradient;
    % Check convergence
    if norm(gradient) < tolerance
        disp('Converged.');
        break;
    end
end

Arrays have incompatible sizes for this operation.

Error in solution>generateTrajectory (line 67)
waypoints = toolPositionHome + ...

% Final optimized waypoint times
finalWaypointTimes = waypointTimes;
disp(finalWaypointTimes);

%generateTrajectory
function [q,qd,qdd] =generateTrajectory(waypointTimes,trajTimes)


global robot;
global numJoints;

eeName = string(robot.BodyNames(length(robot.BodyNames)));
q_home = [0 15 180 -130 0 55 90]'*pi/180;
% The home configuration is described by a homogeneous transformation
% matrix:
T_home = getTransform(robot, q_home, eeName);
% From T_home we can determine the position of the end-effector (we 
% extract the first 3 components of the last 4x1 column):
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]';
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]';
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);
ikWeights = [1 1 1 1 1 1];
ikInitGuess = q_home';
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;
includeOrientation = false;
numWaypoints = size(waypoints,2); % Number of waypoints
jointWaypoints = zeros(numJoints,numWaypoints);
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,info] = ik(eeName,tgtPose,ikWeights',ikInitGuess');
    jointWaypoints(:,idx) = config';
end
% The trajectory is generated by interpolating between waypoints. The 
% interpolation can be done in differen ways:
% - trap: trapezoidal
% - cubic
% - quintic
% - bspline
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
end    

% Define the objective function to compute energy consumption given the trajectory and waypoint times
function energy = computeEnergyConsumption(q, qd, qdd, trajTimes)
global numJoints;
global robot;
jointTorques = zeros(numJoints, numel(trajTimes));
for i = 1:numel(trajTimes)
    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

% Compute power


global energy;
% Compute energy consumption
Energy = trapz(trajTimes, TotalPower); % Integrate over time to get energy
energy = Energy;
end
% Define the gradient of the objective function
function gradient = computeGradient( waypointTimes,trajTimes)
% Compute gradient using numerical differentiation

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

for i = 2:numWaypoints-2  % Excluding the first and last waypoint times
    % Perturb the waypoint time
    global Number_of_steps;
    waypointTimes_perturbed = waypointTimes;
    waypointTimes_perturbed(i) = waypointTimes_perturbed(i) + epsilon;
    trajTimes_perturbed = trajTimes;
    trajTimes_perturbed(1+(i-1)*Number_of_steps/4)=waypointTimes_perturbed(i);
    
    [q_perturbed,qd_perturbed,qdd_perturbed ] = generateTrajectory(waypointTimes_perturbed,trajTimes_perturbed);
    % Compute energy consumption with perturbed waypoint times
    energy_perturbed = computeEnergyConsumption(q_perturbed, qd_perturbed, qdd_perturbed, trajTimes_perturbed);
    global energy;
    % Compute gradient using central difference
    gradient(i) = (energy_perturbed - energy) / epsilon;
end
end

this code i usded to optimize the energy by changing waypointtimes. The optimization strategy is a gradient descent.

For now it doesnt work because of the generate trajectory function. I always get this error:

Arrays have incompatible sizes for this operation.

Error in optimization_test2>generateTrajectory (line 106)

waypoints = toolPositionHome + ...

Error in optimization_test2 (line 62)

[q,qd,qdd] =generateTrajectory(waypointTimes,trajTimes);

Related documentation

I am not really a coding expert on matlab so can somebody help me?

Answer 1

[Torsten]

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];

toolPositionHome is a 3x1 vector.

[0 0 0 ; -0.1 0.2 0.2 ; -0.2 0 0.1 ; -0.1 -0.2 0.2 ; 0 0 0] is a 5x3 matrix.

What do you expect to get when you sum the two ? In MATLAB at least, the result is undefined.