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

Use the externalForce function to generate force matrices to apply to a rigid body tree model. The force matrix is an m-by-6 vector that has a row for each joint on the robot to apply a six-element wrench. Use the externalForce function and specify the end effector to properly assign the wrench to the correct row of the matrix. You can add multiple force matrices together to apply multiple forces to one robot.

To calculate the joint torques that counter these external forces, use the inverseDynamics function.

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 property to give a specific gravitational acceleration.

lbr.Gravity = [0 0 -9.81];

Get the home configuration for lbr.

q = homeConfiguration(lbr);

Set external force on link1. The input wrench vector is expressed in the base frame.

fext1 = externalForce(lbr,'link_1',[0 0 0.0 0.1 0 0]);

Set external force on the end effector, tool0. The input wrench vector is expressed in the tool0 frame.

fext2 = externalForce(lbr,'tool0',[0 0 0.0 0.1 0 0],q);

Compute the joint torques required to balance the external forces. To combine the forces, add the force matrices together. Joint velocities and accelerations are assumed to be zero (input as []).

tau = inverseDynamics(lbr,q,[],[],fext1+fext2);