URDF files dont work

robot = importrobot('irb6640.urdf');

by using a irb6640.urdf file from the internet the code I used for generating a robot traject doesnt work.

I tried this for different urdf files but the code can't run the inverse dynamics for some reason. Normally this should be possible. I need this for my thesis :(. Does someone know what I did wrong . this is the urdf file <?xml version="1.0" ?>

<!-- =================================================================================== -->

<!-- | This document was autogenerated by xacro from irb6640.xacro | -->

<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->

<!-- =================================================================================== -->

<robot name="abb_irb6640" xmlns:xacro="http://ros.org/wiki/xacro">

<link name="base_link">

<collision name="collision">

<geometry>

<mesh filename="package://collision/base_link.stl"/>

</geometry>

<material name="yellow">

<color rgba="0 1 1 1"/>

</material>

</collision>

<visual name="visual">

<geometry>

<mesh filename="package://visual/base_link.stl"/>

</geometry>

<material name="orange">

<color rgba="1 0.43 0 1"/>

</material>

</visual>

</link>

<link name="link_1">

<collision name="collision">

<geometry>

<mesh filename="package://collision/link_1.stl"/>

</geometry>

<material name="yellow"/>

</collision>

<visual name="visual">

<geometry>

<mesh filename="package://visual/link_1.stl"/>

</geometry>

<material name="orange"/>

</visual>

</link>

<link name="link_2">

<collision name="collision">

<geometry>

<mesh filename="package://collision/link_2.stl"/>

</geometry>

<material name="yellow"/>

</collision>

<visual name="visual">

<geometry>

<mesh filename="package://visual/link_2.stl"/>

</geometry>

<material name="orange"/>

</visual>

</link>

<link name="link_3">

<collision name="collision">

<geometry>

<mesh filename="package://collision/link_3.stl"/>

</geometry>

<material name="yellow"/>

</collision>

<visual name="visual">

<geometry>

<mesh filename="package://visual/link_3.stl"/>

</geometry>

<material name="orange"/>

</visual>

</link>

<link name="link_4">

<collision name="collision">

<geometry>

<mesh filename="package://collision/link_4.stl"/>

</geometry>

<material name="yellow"/>

</collision>

<visual name="visual">

<geometry>

<mesh filename="package://visual/link_4.stl"/>

</geometry>

<material name="orange"/>

</visual>

</link>

<link name="link_5">

<collision name="collision">

<geometry>

<mesh filename="package://collision/link_5.stl"/>

</geometry>

<material name="yellow"/>

</collision>

<visual name="visual">

<geometry>

<mesh filename="package://visual/link_5.stl"/>

</geometry>

<material name="orange"/>

</visual>

</link>

<link name="link_6">

<collision name="collision">

<geometry>

<mesh filename="package://collision/link_6.stl"/>

</geometry>

<material name="yellow"/>

</collision>

<visual name="visual">

<geometry>

<mesh filename="package://visual/link_6.stl"/>

</geometry>

<material name="orange"/>

</visual>

</link>

<link name="tool0"/>

<link name="link_cylinder">

<collision name="collision">

<geometry>

<mesh filename="package://collision/cylinder.stl"/>

</geometry>

<material name="yellow"/>

</collision>

<visual name="visual">

<geometry>

<mesh filename="package://visual/cylinder.stl"/>

</geometry>

<material name="orange"/>

</visual>

</link>

<link name="link_piston">

<collision name="collision">

<geometry>

<mesh filename="package://collision/piston.stl"/>

</geometry>

<material name="yellow"/>

</collision>

<visual name="visual">

<geometry>

<mesh filename="package://visual/piston.stl"/>

</geometry>

<material name="orange"/>

</visual>

</link>

<joint name="joint_1" type="revolute">

<origin rpy="0 0 0" xyz="0 0 0.227"/>

<axis xyz="0 0 1"/>

<parent link="base_link"/>

<child link="link_1"/>

<limit effort="0" lower="-2.967" upper="2.967" velocity="1.7453"/>

</joint>

<joint name="joint_2" type="revolute">

<origin rpy="0 0 0" xyz="0.322 0.03 0.551"/>

<axis xyz="0 1 0"/>

<parent link="link_1"/>

<child link="link_2"/>

<limit effort="0" lower="-1.134" upper="1.4855" velocity="1.5707"/>

</joint>

<joint name="joint_3" type="revolute">

<origin rpy="0 0 0" xyz="0 -0.2 1.07"/>

<axis xyz="0 1 0"/>

<parent link="link_2"/>

<child link="link_3"/>

<limit effort="0" lower="-3.142" upper="1.222" velocity="1.5707"/>

</joint>

<joint name="joint_4" type="revolute">

<origin rpy="0 0 0" xyz="-0.275 0.181 0.2"/>

<axis xyz="1 0 0"/>

<parent link="link_3"/>

<child link="link_4"/>

<limit effort="0" lower="-5.236" upper="5.236" velocity="2.9671"/>

</joint>

<joint name="joint_5" type="revolute">

<origin rpy="0 0 0" xyz="1.67 0 0 "/>

<axis xyz="0 1 0"/>

<parent link="link_4"/>

<child link="link_5"/>

<limit effort="0" lower="-2.094" upper="2.094" velocity="2.4435"/>

</joint>

<joint name="joint_6" type="revolute">

<origin rpy="0 0 0" xyz="0.153 0 0 "/>

<axis xyz="1 0 0"/>

<parent link="link_5"/>

<child link="link_6"/>

<limit effort="0" lower="-6.283" upper="6.283" velocity="3.3161"/>

</joint>

<joint name="joint_6-tool0" type="fixed">

<parent link="link_6"/>

<child link="tool0"/>

<origin rpy="0 1.57079632679 0" xyz=".055 0 0"/>

</joint>

<joint name="joint_cylinder" type="fixed">

<origin rpy="0 -0.170 0" xyz="-0.365 -0.1895 0.405"/>

<axis xyz="0 1 0"/>

<parent link="link_1"/>

<child link="link_cylinder"/>

<limit effort="0" lower="-2.96705973" upper="2.96705973" velocity="0"/>

</joint>

<joint name="joint_piston" type="fixed">

<origin rpy="0 0 0" xyz="0.475 0 0"/>

<axis xyz="1 0 0"/>

<parent link="link_cylinder"/>

<child link="link_piston"/>

<limit effort="0" lower="0" upper="0.6" velocity="0"/>

</joint>

<link name="base"/>

<joint name="base_link-base" type="fixed">

<origin rpy="0 0 0" xyz="0 0 0"/>

<parent link="base_link"/>

<child link="base"/>

</joint>

</robot>

Answer 1

[Gaurav Bhosale]

Hi Tim De Witte,

I am able to run shared URDF without any issue.

Can you please share me more details such as MATLAB version, error/warning message details or reproduction steps ?

Thanks

[Tim De Witte]

Could you explain how you dit that? I do not get an error but it just wont calculate the inverse dynamics. I am using version 2023b of matlab.

this is my code now:

% Adapted from:

% https://nl.mathworks.com/help/supportpkg/robotmanipulator/ug/

% generate-trajectory-endeffector.html

clear all

clc

close all

% Create the Robot Model

% (*** List of other built-in robot models:)

% *** https://nl.mathworks.com/help/robotics/ref/loadrobot.html)

%

% Create a Rigid Body Tree for the robot, set the home configuration and

% calculate the transformation at the home configuration:

robot = importrobot('irb6640.urdf');

% 7-axis manipulator

% Continuous payload: 4 kg / 8.8 lbs

% Maximum reach: 902 mm

% Weight: 8.2 kg / 18 lbs

% Power consumption: 36 W

% loadrobot(robotname) loads a robot model from the robot library as a

% rigidBodyTree object.

% To import other robot models in Unified Robot Description Format (URDF),

% XML Macros (Xacro), or Simulation Description Format (SDF) file

% or Simscape Multibody model, the 'importrobot' function can be used.

robot.Gravity = [0 0 -9.81];

robot.DataFormat = 'column';

% To see robot details:

% showdetails(robot)

% Define arbitrary home configuration

q_home = [0 0 0 0 0 0]'*pi/180;

% Another option would be to create a random home configuration:

% q_home = randomConfiguration(robot);

% To interact with the robot model:

% interactiveGUI = interactiveRigidBodyTree(robot);

show(robot,q_home);

axis auto;

view([60,10]); % camera line of sight view(az,el)

% To see the names of links:

% robot.BodyNames

% (or "showdetails(robot)")

% The end-effector is the last body. Its name is 'EndEffector_Link'

% So we can define:

% eeName = 'EndEffector_Link';

% A more automated way to identify the end-effector is:

eeName = string(robot.BodyNames(length(robot.BodyNames)));

% 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 ; 1 2 5 ; 3 4 6 ; 1 1 1 ; 0 0 0]';

% EXERCISE: how to have the robot stopping for a few seconds at a

% specified point?

% Let's define the desired orientation of the end-effector in the

% waypoints.

% The orientation is represented in Euler angles and the convention used

% is the Tait-Bryan, extrinsic ZYX. Extrinsic rotations apply to axis

% in world coordinate system.

orientations = [-2.845 -0.971 -0.569;

0.26 -0.85 2.82;

0.175 0.625 2.89;

0.421 -0.744 -2.81;

-2.845 -0.971 -0.569;

]';

waypointTimes = [0 4 8 12 16];

% The trajectory will be divided into "Number_of_steps" steps:

Number_of_steps = 100;

% Time is discretized in steps of duration ts:

ts = waypointTimes(length(waypointTimes))/Number_of_steps;

% Vector of discretized times:

trajTimes = 0:ts:waypointTimes(end);

% End effector velocity at waypoints:

waypointVels = 5 *[ 0 1 0;

-1 0 0;

0 -1 0;

1 0 0;

0 1 0]';

% Acceleration at waypoints is computed from velocity. But first we must

% allocate memory for accelerations:

waypointAccels = zeros(size(waypointVels));

waypointAccelTimes = diff(waypointTimes)/4;

% Create Inverse Kinematics Solver and set solver parameters. The

% algorithm for solving inverse kinematics can be specified: either

% 'BFGSGradientProjection' (default) or 'LevenbergMarquardt'.

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;

plotMode = 2; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames

show(robot,q_home,'Frames','off','PreservePlot',false);

hold on

if plotMode == 1

hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-');

end

plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2);

axis auto;

view([30 15]);

% 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".

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

[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 = 'bspline';

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',zeros(numJoints,numWaypoints));

case 'quintic'

[q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, ...

'VelocityBoundaryCondition',zeros(numJoints,numWaypoints), ...

'AccelerationBoundaryCondition',zeros(numJoints,numWaypoints));

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 (6,1);

otherwise

error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline''');

end

for idx = 1:numel(trajTimes)

config = q(:,idx)';

% Find Cartesian points for visualization

eeTform = getTransform(robot,config',eeName);

% plotMode == 1 means "show trajectory"; "0" was: nothing;

% "2" was: show axes

if plotMode == 1

eePos = tform2trvec(eeTform);

set(hTraj,'xdata',[hTraj.XData eePos(1)], ...

'ydata',[hTraj.YData eePos(2)], ...

'zdata',[hTraj.ZData eePos(3)]);

elseif plotMode == 2

plotTransforms(tform2trvec(eeTform),tform2quat(eeTform),'FrameSize',0.05);

end

% Show the robot

show(robot,config','Frames','off','PreservePlot',false);

title(['Trajectory at t = ' num2str(trajTimes(idx))])

drawnow

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

figure

plot(trajTimes,Power,'b-o')

hold on

plot(trajTimes,TotalPower,'r-*')

title('Power')

xlabel('Time [s]')

ylabel('Power [W]')

hold off

legend('Total power (algebraic)','Total power (Absolute values)')

figure

plot(trajTimes,jointTorques)

title('Joint torques')

xlabel('Time [s]')

ylabel('Joint torques [Nm]')

% EXERCISE: adapt the script so to interpolate waypoints in the task

% space

[Gaurav Bhosale]

The above shared code is also running fine in R2023b MATLAB. Can you check, is there any pending MATLAB update ?

https://www.mathworks.com/help/matlab/matlab_env/check-for-software-updates.html

[Tim De Witte]

No there ius not a pending update available but I am not used to urdf files, do I have to do something in simulink like you did cause I just took a urdf file and put it in my code?

[Gaurav Bhosale]

Your shared code looks fine. I don't see any issue. Can you provide me error message (you see at your end) ?

[Tim De Witte]

I dont get an error but the power calculations are zero when i run the code which is weird. Do you have a working power calculations.

clear all

clc

close all

% Create the Robot Model

% (*** List of other built-in robot models:)

% *** https://nl.mathworks.com/help/robotics/ref/loadrobot.html)

%

% Create a Rigid Body Tree for the robot, set the home configuration and

% calculate the transformation at the home configuration:

robot = importrobot('irb6640.urdf');

% 7-axis manipulator

% Continuous payload: 4 kg / 8.8 lbs

% Maximum reach: 902 mm

% Weight: 8.2 kg / 18 lbs

% Power consumption: 36 W

% loadrobot(robotname) loads a robot model from the robot library as a

% rigidBodyTree object.

% To import other robot models in Unified Robot Description Format (URDF),

% XML Macros (Xacro), or Simulation Description Format (SDF) file

% or Simscape Multibody model, the 'importrobot' function can be used.

robot.Gravity = [0 0 -9.81];

robot.DataFormat = 'column';

% To see robot details:

% showdetails(robot)

% Define arbitrary home configuration

q_home = [0 0 0 0 0 0]'*pi/180;

% Another option would be to create a random home configuration:

% q_home = randomConfiguration(robot);

% To interact with the robot model:

% interactiveGUI = interactiveRigidBodyTree(robot);

show(robot,q_home);

axis auto;

view([60,10]); % camera line of sight view(az,el)

% To see the names of links:

% robot.BodyNames

% (or "showdetails(robot)")

% The end-effector is the last body. Its name is 'EndEffector_Link'

% So we can define:

% eeName = 'EndEffector_Link';

% A more automated way to identify the end-effector is:

eeName = string(robot.BodyNames(length(robot.BodyNames)));

% 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 ; 1 2 5 ; 3 4 6 ; 1 1 1 ; 0 0 0]';

% EXERCISE: how to have the robot stopping for a few seconds at a

% specified point?

% Let's define the desired orientation of the end-effector in the

% waypoints.

% The orientation is represented in Euler angles and the convention used

% is the Tait-Bryan, extrinsic ZYX. Extrinsic rotations apply to axis

% in world coordinate system.

orientations = [-2.845 -0.971 -0.569;

0.26 -0.85 2.82;

0.175 0.625 2.89;

0.421 -0.744 -2.81;

-2.845 -0.971 -0.569;

]';

waypointTimes = [0 4 8 12 16];

% The trajectory will be divided into "Number_of_steps" steps:

Number_of_steps = 100;

% Time is discretized in steps of duration ts:

ts = waypointTimes(length(waypointTimes))/Number_of_steps;

% Vector of discretized times:

trajTimes = 0:ts:waypointTimes(end);

% End effector velocity at waypoints:

waypointVels = 5 *[ 0 1 0;

-1 0 0;

0 -1 0;

1 0 0;

0 1 0]';

% Acceleration at waypoints is computed from velocity. But first we must

% allocate memory for accelerations:

waypointAccels = zeros(size(waypointVels));

waypointAccelTimes = diff(waypointTimes)/4;

% Create Inverse Kinematics Solver and set solver parameters. The

% algorithm for solving inverse kinematics can be specified: either

% 'BFGSGradientProjection' (default) or 'LevenbergMarquardt'.

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;

plotMode = 2; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames

show(robot,q_home,'Frames','off','PreservePlot',false);

hold on

if plotMode == 1

hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-');

end

plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2);

axis auto;

view([30 15]);

% 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".

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

[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 = 'bspline';

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',zeros(numJoints,numWaypoints));

case 'quintic'

[q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, ...

'VelocityBoundaryCondition',zeros(numJoints,numWaypoints), ...

'AccelerationBoundaryCondition',zeros(numJoints,numWaypoints));

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 (6,1);

otherwise

error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline''');

end

for idx = 1:numel(trajTimes)

config = q(:,idx)';

% Find Cartesian points for visualization

eeTform = getTransform(robot,config',eeName);

% plotMode == 1 means "show trajectory"; "0" was: nothing;

% "2" was: show axes

if plotMode == 1

eePos = tform2trvec(eeTform);

set(hTraj,'xdata',[hTraj.XData eePos(1)], ...

'ydata',[hTraj.YData eePos(2)], ...

'zdata',[hTraj.ZData eePos(3)]);

elseif plotMode == 2

plotTransforms(tform2trvec(eeTform),tform2quat(eeTform),'FrameSize',0.05);

end

% Show the robot

show(robot,config','Frames','off','PreservePlot',false);

title(['Trajectory at t = ' num2str(trajTimes(idx))])

drawnow

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

figure

plot(trajTimes,Power,'b-o')

hold on

plot(trajTimes,TotalPower,'r-*')

title('Power')

xlabel('Time [s]')

ylabel('Power [W]')

hold off

legend('Total power (algebraic)','Total power (Absolute values)')

figure

plot(trajTimes,jointTorques)

title('Joint torques')

xlabel('Time [s]')

ylabel('Joint torques [Nm]')

[Tim De Witte]

maybe you can share your output?