externalForce
Compose external force matrix relative to base
Syntax
Description
composes the external force matrix, which you can use as inputs to fext
= externalForce(robot
,bodyname
,wrench
)inverseDynamics
and forwardDynamics
to apply an external force,
wrench
, to the body specified by
bodyname
. The
wrench
input is assumed to be in the base frame.
Examples
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 KUKA iiwa 14 robot model from the Robotics System Toolbox™ loadrobot
. The robot is specified as a rigidBodyTree
object.
Set the data format to "row"
. For all dynamics calculations, the data format must be either "row"
or "column"
.
Set the gravity. By default, gravity is assumed to be zero.
kukaIIWA14 = loadrobot("kukaIiwa14",DataFormat="row",Gravity=[0 0 -9.81]);
Get the home configuration for the kukaIIWA14
robot.
q = homeConfiguration(kukaIIWA14);
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 "iiwa_link_ee_kuka"
body frame, which requires you to specify the robot configuration, q
.
wrench = [0 0 0.5 0 0 0.3];
fext = externalForce(kukaIIWA14,"iiwa_link_ee_kuka",wrench,q);
Compute the resultant joint accelerations due to gravity, with the external force applied to the end-effector "iiwa_link_ee_kuka"
when kukaIIWA14
is at its home configuration. The joint velocities and joint torques are assumed to be zero (input as an empty vector []
).
qddot = forwardDynamics(kukaIIWA14,q,[],[],fext)
qddot = 1×7
-0.0023 -0.0112 0.0036 -0.0212 0.0067 -0.0075 499.9920
Compute Joint Torque to Counter External Forces
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 Universal Robots UR5e from the Robotics System Toolbox™ loadrobot
, specified as a rigidBodyTree
object. Update the gravity and set the data format to "row"
. For all dynamics calculations, the data format must be either "row"
or "column"
manipulator = loadrobot("universalUR5e", DataFormat="row", Gravity=[0 0 -9.81]); showdetails(manipulator)
-------------------- Robot: (10 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 base base_link-base_fixed_joint fixed base_link(0) 2 base_link_inertia base_link-base_link_inertia fixed base_link(0) shoulder_link(3) 3 shoulder_link shoulder_pan_joint revolute base_link_inertia(2) upper_arm_link(4) 4 upper_arm_link shoulder_lift_joint revolute shoulder_link(3) forearm_link(5) 5 forearm_link elbow_joint revolute upper_arm_link(4) wrist_1_link(6) 6 wrist_1_link wrist_1_joint revolute forearm_link(5) wrist_2_link(7) 7 wrist_2_link wrist_2_joint revolute wrist_1_link(6) wrist_3_link(8) 8 wrist_3_link wrist_3_joint revolute wrist_2_link(7) flange(9) 9 flange wrist_3-flange fixed wrist_3_link(8) tool0(10) 10 tool0 flange-tool0 fixed flange(9) --------------------
Get the home configuration for manipulator
.
q = homeConfiguration(manipulator);
Set external force on shoulder_link
. The input wrench vector is expressed in the base frame.
fext1 = externalForce(manipulator,"shoulder_link",[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(manipulator,"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(manipulator,q,[],[],fext1+fext2)
tau = 1×6
-0.0233 -52.4189 -14.4896 -0.0100 0.0100 -0.0000
Input Arguments
robot
— Robot model
rigidBodyTree
object
Robot model, specified as a rigidBodyTree
object. To use the
externalForce
function, set the DataFormat
property to either "row"
or "column"
.
bodyname
— Name of body to which external force is applied
string scalar | character vector
Name of body to which the external force is applied, specified as a string scalar or character
vector. This body name must match a body on the robot
object.
Data Types: char
| string
wrench
— Torques and forces applied to body
[Tx Ty Tz Fx Fy Fz]
vector
Torques and forces applied to the body, specified as a [Tx Ty Tz Fx Fy Fz]
vector. The first three elements of the wrench correspond to the moments around
xyz-axes. The last three elements are linear forces along the same
axes. Unless you specify the robot configuration
, the wrench is
assumed to be relative to the base frame.
configuration
— Robot configuration
vector
Robot configuration, specified as a vector with positions for all nonfixed joints in the robot
model. You can generate a configuration using
homeConfiguration(robot)
,
randomConfiguration(robot)
, or by specifying your own joint
positions. To use the vector form of configuration
, set the
DataFormat
property for the robot
to
either 'row'
or 'column'
.
Output Arguments
fext
— External force matrix
n-by-6 matrix | 6-by-n matrix
External force matrix, returned as either an n-by-6 or
6-by-n matrix, where n is the velocity number
(degrees of freedom) of the robot. The shape depends on the
DataFormat
property of robot
. The
"row"
data format uses an n-by-6 matrix. The
"column"
data format uses a 6-by-n.
The composed matrix lists only values other than zero at the locations relevant to the body
specified. You can add force matrices together to specify multiple forces on multiple
bodies. Use the external force matrix to specify external forces to dynamics functions
inverseDynamics
and forwardDynamics
.
More About
Dynamics Properties
When working with robot dynamics, specify the information for individual bodies of your manipulator robot using these properties of the rigidBody
objects:
Mass
— Mass of the rigid body in kilograms.CenterOfMass
— Center of mass position of the rigid body, specified as a vector of the form[x y z]
. The vector describes the location of the center of mass of the rigid body, relative to the body frame, in meters. ThecenterOfMass
object function uses these rigid body property values when computing the center of mass of a robot.Inertia
— Inertia of the rigid body, specified as a vector of the form[Ixx Iyy Izz Iyz Ixz Ixy]
. The vector is relative to the body frame in kilogram square meters. The inertia tensor is a positive definite matrix of the form:The first three elements of the
Inertia
vector are the moment of inertia, which are the diagonal elements of the inertia tensor. The last three elements are the product of inertia, which are the off-diagonal elements of the inertia tensor.
For information related to the entire manipulator robot model, specify these rigidBodyTree
object properties:
Gravity
— Gravitational acceleration experienced by the robot, specified as an[x y z]
vector in m/s2. By default, there is no gravitational acceleration.DataFormat
— The input and output data format for the kinematics and dynamics functions, specified as"struct"
,"row"
, or"column"
.
Dynamics Equations
Manipulator rigid body dynamics are governed by this equation:
also written as:
where:
— is a joint-space mass matrix based on the current robot configuration. Calculate this matrix by using the
massMatrix
object function.— are the Coriolis terms, which are multiplied by to calculate the velocity product. Calculate the velocity product by using by the
velocityProduct
object function.— is the gravity torques and forces required for all joints to maintain their positions in the specified gravity
Gravity
. Calculate the gravity torque by using thegravityTorque
object function.— is the geometric Jacobian for the specified joint configuration. Calculate the geometric Jacobian by using the
geometricJacobian
object function.— is a matrix of the external forces applied to the rigid body. Generate external forces by using the
externalForce
object function.— are the joint torques and forces applied directly as a vector to each joint.
— are the joint configuration, joint velocities, and joint accelerations, respectively, as individual vectors. For revolute joints, specify values in radians, rad/s, and rad/s2, respectively. For prismatic joints, specify in meters, m/s, and m/s2.
To compute the dynamics directly, use the forwardDynamics
object function. The function calculates the joint accelerations for the specified combinations of the above inputs.
To achieve a certain set of motions, use the inverseDynamics
object function. The function calculates the joint torques required to achieve the specified configuration, velocities, accelerations, and external forces.
References
[1] Featherstone, Roy. Rigid Body Dynamics Algorithms. Springer US, 2008. DOI.org (Crossref), doi:10.1007/978-1-4899-7560-7.
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Usage notes and limitations:
When creating the rigidBodyTree
object, use the syntax that specifies the
MaxNumBodies
as an upper bound for adding bodies to the robot model.
You must also specify the DataFormat
property as a name-value pair. For
example:
robot = rigidBodyTree("MaxNumBodies",15,"DataFormat","row")
To minimize data usage, limit the upper bound to a number close to the expected number of bodies in the model. All data formats are supported for code generation. To use the dynamics functions, the data format must be set to "row"
or "column"
.
The show
and showdetails
functions do not support code generation.
Version History
Introduced in R2017aR2024a: Static memory allocation support
externalForce
now supports code generation with disabled dynamic memory allocation. For more information about disabling dynamic memory allocation, see Set Dynamic Memory Allocation Threshold (MATLAB Coder).
MATLAB-Befehl
Sie haben auf einen Link geklickt, der diesem MATLAB-Befehl entspricht:
Führen Sie den Befehl durch Eingabe in das MATLAB-Befehlsfenster aus. Webbrowser unterstützen keine MATLAB-Befehle.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)