Robot manipulator Link class
A Link object holds all information related to a robot link such as kinematics parameters, rigid-body inertial parameters, motor and transmission parameters.
theta | kinematic | joint angle |
d | kinematic | link offset |
a | kinematic | link length |
alpha | kinematic | link twist |
sigma | kinematic | 0 if revolute, 1 if prismatic |
mdh | kinematic | 0 if standard D&H, else 1 |
offset | kinematic | joint variable offset |
qlim | kinematic | joint variable limits [min max] |
m | dynamic | link mass |
r | dynamic | link COG wrt link coordinate frame 3x1 |
I | dynamic | link inertia matrix, symmetric 3x3, about link COG |
B | dynamic | link viscous friction (motor referred) |
Tc | dynamic | link Coulomb friction |
G | actuator | gear ratio |
Jm | actuator | motor inertia (motor referred) |
L = Link() L = Link(L1) L = Link(L1,OPTIONS) L = Link(OPTIONS) Old syntax: L = Link(DH,OPTIONS) DH = [THETA D A ALPHA] Alternative syntax: L = Link(THETA, D, A, ALPHA, SIGMA, OFFSET, DH_TYPE)
L = Link() is a Link object with default parameters.
L = Link(L1) is a Link object that is a deep copy of the link object L1.
L = Link(L1,OPTIONS) is a Link object that is a deep copy of the link object L1 which kinematic and dynamic parameters are modified by means of key/value pairs specified in OPTIONS.
L = Link(OPTIONS) is a link object with the kinematic and dynamic parameters specified by the key/value pairs specified in OPTIONS.
joint angle, if not specified joint is revolute
joint extension, if not specified joint is prismatic
joint offset (default 0)
joint twist (default 0)
defined using standard D&H parameters (default).
defined using modified D&H parameters.
joint variable offset (default 0)
joint limit (default [])
link inertia matrix (3x1, 6x1 or 3x3)
link centre of gravity (3x1)
link mass (1x1)
motor gear ratio (default 0)
joint friction, motor referenced (default 0)
motor inertia, motor referenced (default 0)
Coulomb friction, motor referenced (1x1 or 2x1), (default [0 0])
for a revolute joint (default)
for a prismatic joint 'p'
for standard D&H parameters (default).
for modified D&H parameters.
Parameters 'theta' and 'd' cannot be both assigned for the same link
The link inertia matrix (3x3) is symmetric and can be specified by giving a 3x3 matrix, the diagonal elements [Ixx Iyy Izz], or the moments and products of inertia [Ixx Iyy Izz Ixy Iyz Ixz].
All friction quantities are referenced to the motor not the load.
Gear ratio is used only to convert motor referenced quantities such as friction and interia to the link frame.
Link objects can be used in vectors and arrays.
The parameter D is unused in a revolute joint, it is simply a placeholder in the vector and the value given is ignored.
The parameter THETA is unused in a prismatic joint, it is simply a placeholder in the vector and the value given is ignored.
The joint offset is a constant added to the joint angle variable before forward kinematics and subtracted after inverse kinematics. It is useful if you want the robot to adopt a 'sensible' pose for zero joint angle configuration.
The link dynamic (inertial and motor) parameters are all set to zero. These must be set by explicitly assigning the object properties: m, r, I, Jm, B, Tc, G.
L = Link(DH, OPTIONS)
L = Link(DH, OPTIONS) is a link object using the specified kinematic convention and with parameters:
- DH = [THETA D A ALPHA SIGMA OFFSET] where OFFSET is a constant displacement between the user joint angle vector and the true kinematic solution.
- DH = [THETA D A ALPHA SIGMA] where SIGMA=0 for a revolute and 1 for a prismatic joint, OFFSET is zero.
- DH = [THETA D A ALPHA], joint is assumed revolute and OFFSET is zero.
for standard D&H parameters, can be abbreviated to 'std' (default)
for modified D&H parameters, can be abbreviated to 'mod'
for a revolute joint, can be abbreviated to 'r' (default)
for a prismatic joint, can be abbreviated to 'p'
L = Link(THETA, D, A, ALPHA) L = Link(THETA, D, A, ALPHA, DH_TYPE) L = Link(THETA, D, A, ALPHA, SIGMA, DH_TYPE) L = Link(THETA, D, A, ALPHA, SIGMA, OFFSET, DH_TYPE)
L = Link(THETA, D, A, ALPHA, SIGMA, OFFSET, DH_TYPE) is a link object using the specified kinematic convention and with the parameters of the old syntax.
OFFSET is a constant displacement between the user joint angle vector and the true kinematic solution.
SIGMA=0 for a revolute and SIGMA=1 for a prismatic joint.
DH_TYPE is the type of Denavit-Hartenberg convencion used. It can take two values:
- "standard" or "std" for standard D&H parameters (default).
- "modified" or "mod" for modified D&H parameters.
SIGMA, OFFSET, DH_TYPE can be omitted; will be considered the default values: joint is assumed revolute, with no offset and using the standard Denavit-Hartenberg convencion.
Robotics, Vision & Control, Chap 7, P. Corke, Springer 2011.