Create a joint
The rigidBodyJoint
objects defines how a rigid body moves relative to
an attachment point. In a tree-structured robot, a joint always belongs to a specific
rigid body, and each rigid body has one joint.
The rigidBodyJoint
object can describe joints of various types. When
building a rigid body tree structure with rigidBodyTree
, you must assign the Joint
object to a
rigid body using the rigidBody
class.
The different joint types supported are:
fixed
— Fixed joint that prevents relative
motion between two bodies.
revolute
— Single degree of freedom (DOF) joint
that rotates around a given axis. Also called a pin or hinge joint.
prismatic
— Single DOF joint that slides along a
given axis. Also called a sliding joint.
Each joint type has different properties with different dimensions, depending on its defined geometry.
jname
— Joint nameJoint name, specified as a string scalar or character vector. The joint name must be unique to access it off the rigid body tree.
Example: "elbow_right"
Data Types: char
| string
jtype
— Joint type'fixed'
(default) | string scalar | character vectorJoint type, specified as a string scalar or character vector. The joint type predefines certain properties when creating the joint.
The different joint types supported are:
fixed
— Fixed joint that
prevents relative motion between two bodies.
revolute
— Single degree of
freedom (DOF) joint that rotates around a given axis. Also
called a pin or hinge joint.
prismatic
— Single DOF joint
that slides along a given axis. Also called a sliding
joint.
Example:
Data Types: char
| string
Type
— Joint type'fixed'
(default) | string scalar | character vectorThis property is read-only.
Joint type, returned as a string scalar or character vector. The joint type predefines certain properties when creating the joint.
The different joint types supported are:
fixed
— Fixed joint that prevents
relative motion between two bodies.
revolute
— Single degree of freedom
(DOF) joint that rotates around a given axis. Also called a pin
or hinge joint.
prismatic
— Single DOF joint that
slides along a given axis. Also called a sliding joint.
If the rigid body that contains this joint is added to a robot model, the
joint type must be changed by replacing the joint using replaceJoint
.
Example:
Data Types: char
| string
Name
— Joint nameJoint name, returned as a string scalar or character vector. The joint
name must be unique to access it off the rigid body tree. If the rigid body
that contains this joint is added to a robot model, the joint name must be
changed by replacing the joint using replaceJoint
.
Example: "elbow_right"
Data Types: char
| string
PositionLimits
— Position limits of jointPosition limits of the joint, specified as a vector of [min
max]
values. Depending on the type of joint, these values have
different definitions.
fixed
— [NaN NaN]
(default). A fixed joint has no joint limits. Bodies remain fixed
between each other.
revolute
— [-pi pi]
(default). The limits define the angle of rotation around the axis
in radians.
prismatic
— [-0.5
0.5]
(default). The limits define the linear motion
along the axis in meters.
Example:
HomePosition
— Home position of jointHome position of joint, specified as a scalar that depends on your joint
type. The home position must fall in the range set by
PositionLimits
. This property is used by homeConfiguration
to
generate the predefined home configuration for an entire rigid body
tree.
Depending on the joint type, the home position has a different definition.
fixed
— 0
(default).
A fixed joint has no relevant home position.
revolute
— 0
(default). A revolute joint has a home position defined by the angle
of rotation around the joint axis in radians.
prismatic
— 0
(default). A prismatic joint has a home position defined by the
linear motion along the joint axis in meters.
Example:
JointAxis
— Axis of motion for jointNaN
NaN
NaN
] (default) | three-element unit vectorAxis of motion for joint, specified as a three-element unit vector. The vector can be any direction in 3-D space in local coordinates.
Depending on the joint type, the joint axis has a different definition.
fixed
— A fixed joint has no relevant
axis of motion.
revolute
— A revolute joint rotates the
body in the plane perpendicular to the joint axis.
prismatic
— A prismatic joint moves the
body in a linear motion along the joint axis direction.
Example:
JointToParentTransform
— Fixed transform from joint to parent frameeye(4)
(default) | 4-by-4 homogeneous transform matrixThis property is read-only.
Fixed transform from joint to parent frame, returned as a 4-by-4 homogeneous transform matrix. The transform converts the coordinates of points in the joint predecessor frame to the parent body frame.
Example:
ChildToJointTransform
— Fixed transform from child body to joint frameeye(4)
(default) | 4-by-4 homogeneous transform matrixThis property is read-only.
Fixed transform from child body to joint frame, returned as a 4-by-4 homogeneous transform matrix. The transform converts the coordinates of points in the child body frame to the joint successor frame.
Example:
copy | Create copy of joint |
setFixedTransform | Set fixed transform properties of joint |
Add a rigid body and corresponding joint to a rigid body tree. Each rigidBody
object contains a rigidBodyJoint
object and must be added to the rigidBodyTree
using addBody
.
Create a rigid body tree.
rbtree = rigidBodyTree;
Create a rigid body with a unique name.
body1 = rigidBody('b1');
Create a revolute joint. By default, the rigidBody
object comes with a fixed joint. Replace the joint by assigning a new rigidBodyJoint
object to the body1.Joint
property.
jnt1 = rigidBodyJoint('jnt1','revolute'); body1.Joint = jnt1;
Add the rigid body to the tree. Specify the body name that you are attaching the rigid body to. Because this is the first body, use the base name of the tree.
basename = rbtree.BaseName; addBody(rbtree,body1,basename)
Use showdetails
on the tree to confirm the rigid body and joint were added properly.
showdetails(rbtree)
-------------------- Robot: (1 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 b1 jnt1 revolute base(0) --------------------
Use the Denavit-Hartenberg (DH) parameters of the Puma560® robot to build a robot. Each rigid body is added one at a time, with the child-to-parent transform specified by the joint object.
The DH parameters define the geometry of the robot with relation to how each rigid body is attached to its parent. For convenience, setup the parameters for the Puma560 robot in a matrix. The Puma robot is a serial chain manipulator. The DH parameters are relative to the previous line in the matrix, corresponding to the previous joint attachment.
dhparams = [0 pi/2 0 0; 0.4318 0 0 0 0.0203 -pi/2 0.15005 0; 0 pi/2 0.4318 0; 0 -pi/2 0 0; 0 0 0 0];
Create a rigid body tree object to build the robot.
robot = rigidBodyTree;
Create the first rigid body and add it to the robot. To add a rigid body:
Create a rigidBody
object and give it a unique name.
Create a rigidBodyJoint
object and give it a unique name.
Use setFixedTransform
to specify the body-to-body transformation using DH parameters. The last element of the DH parameters, theta
, is ignored because the angle is dependent on the joint position.
Call addBody
to attach the first body joint to the base frame of the robot.
body1 = rigidBody('body1'); jnt1 = rigidBodyJoint('jnt1','revolute'); setFixedTransform(jnt1,dhparams(1,:),'dh'); body1.Joint = jnt1; addBody(robot,body1,'base')
Create and add other rigid bodies to the robot. Specify the previous body name when calling addBody
to attach it. Each fixed transform is relative to the previous joint coordinate frame.
body2 = rigidBody('body2'); jnt2 = rigidBodyJoint('jnt2','revolute'); body3 = rigidBody('body3'); jnt3 = rigidBodyJoint('jnt3','revolute'); body4 = rigidBody('body4'); jnt4 = rigidBodyJoint('jnt4','revolute'); body5 = rigidBody('body5'); jnt5 = rigidBodyJoint('jnt5','revolute'); body6 = rigidBody('body6'); jnt6 = rigidBodyJoint('jnt6','revolute'); setFixedTransform(jnt2,dhparams(2,:),'dh'); setFixedTransform(jnt3,dhparams(3,:),'dh'); setFixedTransform(jnt4,dhparams(4,:),'dh'); setFixedTransform(jnt5,dhparams(5,:),'dh'); setFixedTransform(jnt6,dhparams(6,:),'dh'); body2.Joint = jnt2; body3.Joint = jnt3; body4.Joint = jnt4; body5.Joint = jnt5; body6.Joint = jnt6; addBody(robot,body2,'body1') addBody(robot,body3,'body2') addBody(robot,body4,'body3') addBody(robot,body5,'body4') addBody(robot,body6,'body5')
Verify that your robot was built properly by using the showdetails
or show
function. showdetails
lists all the bodies in the MATLAB® command window. show
displays the robot with a given configuration (home by default). Calls to axis
modify the axis limits and hide the axis labels.
showdetails(robot)
-------------------- Robot: (6 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 body1 jnt1 revolute base(0) body2(2) 2 body2 jnt2 revolute body1(1) body3(3) 3 body3 jnt3 revolute body2(2) body4(4) 4 body4 jnt4 revolute body3(3) body5(5) 5 body5 jnt5 revolute body4(4) body6(6) 6 body6 jnt6 revolute body5(5) --------------------
show(robot);
axis([-0.5,0.5,-0.5,0.5,-0.5,0.5])
axis off
Make changes to an existing rigidBodyTree
object. You can get replace joints, bodies and subtrees in the rigid body tree.
Load example robots as rigidBodyTree
objects.
load exampleRobots.mat
View the details of the Puma robot using showdetails
.
showdetails(puma1)
-------------------- Robot: (6 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 L1 jnt1 revolute base(0) L2(2) 2 L2 jnt2 revolute L1(1) L3(3) 3 L3 jnt3 revolute L2(2) L4(4) 4 L4 jnt4 revolute L3(3) L5(5) 5 L5 jnt5 revolute L4(4) L6(6) 6 L6 jnt6 revolute L5(5) --------------------
Get a specific body to inspect the properties. The only child of the L3
body is the L4
body. You can copy a specific body as well.
body3 = getBody(puma1,'L3');
childBody = body3.Children{1}
childBody = rigidBody with properties: Name: 'L4' Joint: [1x1 rigidBodyJoint] Mass: 1 CenterOfMass: [0 0 0] Inertia: [1 1 1 0 0 0] Parent: [1x1 rigidBody] Children: {[1x1 rigidBody]} Visuals: {} Collisions: {}
body3Copy = copy(body3);
Replace the joint on the L3
body. You must create a new Joint
object and use replaceJoint
to ensure the downstream body geometry is unaffected. Call setFixedTransform
if necessary to define a transform between the bodies instead of with the default identity matrices.
newJoint = rigidBodyJoint('prismatic'); replaceJoint(puma1,'L3',newJoint); showdetails(puma1)
-------------------- Robot: (6 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 L1 jnt1 revolute base(0) L2(2) 2 L2 jnt2 revolute L1(1) L3(3) 3 L3 prismatic fixed L2(2) L4(4) 4 L4 jnt4 revolute L3(3) L5(5) 5 L5 jnt5 revolute L4(4) L6(6) 6 L6 jnt6 revolute L5(5) --------------------
Remove an entire body and get the resulting subtree using removeBody
. The removed body is included in the subtree.
subtree = removeBody(puma1,'L4')
subtree = rigidBodyTree with properties: NumBodies: 3 Bodies: {[1x1 rigidBody] [1x1 rigidBody] [1x1 rigidBody]} Base: [1x1 rigidBody] BodyNames: {'L4' 'L5' 'L6'} BaseName: 'L3' Gravity: [0 0 0] DataFormat: 'struct'
Remove the modified L3
body. Add the original copied L3
body to the L2
body, followed by the returned subtree. The robot model remains the same. See a detailed comparison through showdetails
.
removeBody(puma1,'L3'); addBody(puma1,body3Copy,'L2') addSubtree(puma1,'L3',subtree) showdetails(puma1)
-------------------- Robot: (6 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 L1 jnt1 revolute base(0) L2(2) 2 L2 jnt2 revolute L1(1) L3(3) 3 L3 jnt3 revolute L2(2) L4(4) 4 L4 jnt4 revolute L3(3) L5(5) 5 L5 jnt5 revolute L4(4) L6(6) 6 L6 jnt6 revolute L5(5) --------------------
rigidBodyJoint
was renamedBehavior change in future release
The rigidBodyJoint
object was renamed from
robotics.Joint
. Use rigidBodyJoint
for all
object creation.
[1] Craig, John J. Introduction to Robotics: Mechanics and Control. Reading, MA: Addison-Wesley, 1989.
[2] Siciliano, Bruno. Robotics: Modelling, Planning and Control. London: Springer, 2009.
You have a modified version of this example. Do you want to open this example with your edits?