Energy optimization robot paths
3 views (last 30 days)
Show older comments
% 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
% 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?
0 Comments
Answers (1)
Torsten
on 24 Apr 2024
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.
0 Comments
See Also
Categories
Find more on Robotics in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!