%% Compute Forward Dynamics Due to External Forces on Rigid Body Tree Model
% Calculate the resultant joint accelerations for a given robot configuration with applied external forces and forces due to gravity. A wrench is applied to a specific body with the gravity being specified for the whole robot.
% Load a predefined KUKA LBR robot model, which is specified as a  RigidBodyTree object.

load exampleRobots.mat lbr

%
% Set the data format to 'row'. For all dynamics calculations, the data format must be either 'row' or 'column'.

lbr.DataFormat = 'row';

%
% Set the gravity. By default, gravity is assumed to be zero.

lbr.Gravity = [0 0 -9.81];

%
% Get the home configuration for the lbr robot.

q = homeConfiguration(lbr);

%
% Specify the wrench vector that represents the external forces experienced by the robot. Use the externalForce function to generate the external force matrix. Specify the robot model, the end effector that experiences the wrench, the wrench vector, and the current robot configuration. wrench is given relative to the 'tool0' body frame, which requires you to specify the robot configuration, q.

wrench = [0 0 0.5 0 0 0.3];
fext = externalForce(lbr,'tool0',wrench,q);

%
% Compute the resultant joint accelerations due to gravity, with the external force applied to the end-effector 'tool0' when lbr is at its home configuration. The joint velocities and joint torques are assumed to be zero (input as an empty vector []).

qddot = forwardDynamics(lbr,q,[],[],fext);

% Copyright 2012 The MathWorks, Inc.