Energy optimization robot paths

1 次查看（过去 30 天）
Tim De Witte
Tim De Witte 2024-4-24
回答： Torsten 2024-4-24
Ran in:
% 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?

请先登录，再回答此问题。

回答（1 个）

Torsten
Torsten 2024-4-24
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.

类别

在 Help Center 和 File Exchange 中查找有关 Robotics 的更多信息

标签

产品


版本

R2023b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by