interactiveRigidBodyTree

Interact with rigid body tree robot models

Since R2020a

Description

The interactiveRigidBodyTree object creates a figure that displays a robot model using a rigidBodyTree object and enables you to directly modify the robot configuration using an interactive marker. The rigidBodyTree object defines the geometry of the different connected rigid bodies in the robot model and the joint limits for these bodies.

To compute new configurations using inverse kinematics, click and drag the interactive marker in the figure. The marker supports dragging of the center marker, linear motion along specific axes, and rotation about each axes. You can change the end effector by right-clicking a different body and choosing Set body as marker body.

To save the current configuration of the robot model, use the addConfiguration object function. The StoredConfigurations property contains the saved configurations.

By default, the joint limits of the robot model are the only constraint on the robot. To add additional constraints, use the addConstraint object function. For a list of available constraint objects, see Robot Constraints in Inverse Kinematics.

Creation

Syntax

viztree = interactiveRigidBodyTree(robot)

viztree = interactiveRigidBodyTree(robot,'Frames','off')

viztree = interactiveRigidBodyTree(___,Name,Value)

Description

viztree = interactiveRigidBodyTree(robot) creates an interactive rigid body tree object and associated figure for the specified robot model. The robot argument sets the RigidBodyTree property. To interact with the model, click and drag the interactive marker in the figure.

viztree = interactiveRigidBodyTree(robot,'Frames','off') turns off the frame axes plotted for each joint in the robot model.

viztree = interactiveRigidBodyTree(___,Name,Value) sets properties using one or more name-value pair arguments in addition to any of the input argument combinations in previous syntaxes. Enclose each property name in quotes. For example, 'RigidBodyTree',robot creates an interactive rigid body tree object with the specified robot model.

Properties

RigidBodyTree — Rigid body tree robot model

rigidBodyTree object

Rigid body tree robot model, specified as a rigidBodyTree object. The robot model defines the geometry of the rigid bodies and the joints connecting them. To access provided robot models, use the loadrobot function. To import models from URDF files or Simscape™ Multibody™ models, use the importrobot function.

You can set this property when you create the object. After you create the object, this property is read-only.

IKSolver — Inverse kinematics solver

generalizedInverseKinematics System object with default properties (default) | generalizedInverseKinematics object

Inverse kinematics solver, specified as a generalizedInverseKinematics System object. By default, the solver uses the Levenberg-Marquardt algorithm with a maximum number of iterations of 2. Increasing the maximum number of iterations can decrease the frame rate in the figure.

You can set this property when you create the object. After you create the object, this property is read-only.

MarkerBodyName — Name of rigid body associated with interactive marker

viztree.RigidBodyTree.BodyNames{end} (default) | string scalar | character vector

Name of rigid body associated with interactive marker, specified as a string scalar or character vector. By default, this property is set to the body with the highest index in the RigidBodyTree property. To change this property using the figure, right-click a rigid body and select Set body as marker body.

Example: "r_foot"

Data Types: char | string

MarkerPose — Current pose of interactive marker

4-by-4 homogeneous transformation matrix

This property is read-only.

Current pose of interactive marker, specified as a 4-by-4 homogeneous transformation matrix.

Data Types: double

MarkerBodyPose — Current pose of rigid body associated with interactive marker

4-by-4 homogeneous transformation matrix

This property is read-only.

Current pose of the rigid body associated with the interactive marker, specified as a 4-by-4 homogeneous transformation matrix.

Data Types: double

Constraints — Constraints on inverse kinematics solver

{} (default) | cell array of constraint objects

Constraints on inverse kinematics solver, specified as a cell array of one or more constraint objects:

• constraintAiming

• constraintCartesianBounds

• constraintJointBounds

• constraintOrientationTarget

• constraintPoseTarget

• constraintPositionTarget

By default, the inverse kinematics solver respects only the joint limits of the RigidBodyTree property. To add or remove the constraints on the robot model, use the addConstraint and removeConstraints object functions respectively. Alternatively, you can assign a new cell array of constraint objects to the property.

Example: {constraintAiming("head","ReferenceBody","right_hand")}

Data Types: cell

SolverPoseWeights — Weights on orientation and position of target pose

[1 1] (default) | two-element vector [orientation position]

Weights on orientation and position of target pose, respectively, specified as a two-element vector, [orientation position].

To increase priority for matching a desired orientation or position, increase the corresponding weight value.

Example: [1 4]

Data Types: double

ShowMarker — Visibility of interactive marker in figure

true or 1 (default) | false or 0

Visibility of interactive marker in figure, specified as logical 1 (true) or 0 or ( false). Set ShowMarker to false to hide the interactive marker in the figure.

Data Types: logical

MarkerControlMethod — Type of control for interactive marker

"InverseKinematics" (default) | "JointControl"

Type of control for interactive marker, specified as "InverseKinematics" or "JointControl". By default, the figure computes all the joint configurations of the robot by using inverse kinematics with the end effector set to MarkerBodyName property value. To control the position of a specific joint on the selected rigid body, set this property to "JointControl".

Data Types: char | string

MarkerScaleFactor — Relative scale of interactive marker

1 (default) | positive real number

Relative scale of interactive marker, specified as a positive real number. To increase or decrease the size of the marker in the figure, adjust this property.

Data Types: double

Configuration — Current configuration of rigid body tree robot model

homeConfiguration(viztree.RigidBodyTree) (default) | n-element vector

Current configuration of rigid body tree robot model, specified as an n-element vector. n is the number of nonfixed joints in the RigidBodyTree property.

Example: [1 pi 0 0.5 3.156]'

Data Types: double

StoredConfigurations — Stored robot configurations

[] (default) | n-by-p matrix

Stored robot configurations, specified as an n-by-p matrix. Each column of the matrix is a stored robot configuration. n is the number of nonfixed joints in the RigidBodyTree property. p is the number of stored robot configurations. To add or remove stored configurations, use the addConfiguration or removeConfigurations object functions, respectively.

Data Types: double

Object Functions

addConfiguration	Store current configuration
addConstraint	Add inverse kinematics constraint
removeConfigurations	Remove configurations from StoredConfigurations property
removeConstraints	Remove inverse kinematics constraints
showFigure	Show interactive rigid body tree figure

Examples

Interactively Build and Play Back Series of Robot Configurations

Use the interactiveRigidBodyTree object to manually move around a robot in a figure. The object uses interactive markers in the figure to track the desired poses of different rigid bodies in the rigidBodyTree object.

Load Robot Model

Use the loadrobot function to access provided robot models as rigidBodyTree objects.

robot = loadrobot("atlas");

Visualize Robot and Save Configurations

Create an interactive tree object and associated figure, specifying the loaded robot model and its left hand as the end effector.

viztree = interactiveRigidBodyTree(robot,"MarkerBodyName","l_hand");

Click and drag the interactive marker to change the robot configuration. You can click and drag any of the axes for linear motion, rotate the body about an axis using the red, green, and blue circles, and drag the center of the interactive marker to position it in 3-D space.

The interactiveRigidBodyTree object uses inverse kinematics to determine a configuration that achieves the desired end-effector pose. If the associated rigid body cannot reach the marker, the figure renders the best configuration from the inverse kinematics solver.

Programmatically set the current configuration. Assign a vector of length equal to the number of nonfixed joints in the RigidBodyTree to the Configuration property.

currConfig = homeConfiguration(viztree.RigidBodyTree);
currConfig(1:10) = [ 0.2201 -0.1319 0.2278 -0.3415 0.4996 ...
                     0.0747 0.0377 0.0718 -0.8117 -0.0427]';
viztree.Configuration = currConfig;

Save the current robot configuration in the StoredConfigurations property.

addConfiguration(viztree)

To switch the end effector to a different rigid body, right-click the desired body in the figure and select Set body as marker body. Use this process to select the right hand frame.

You can also set the MarkerBodyName property to the specific body name.

viztree.MarkerBodyName = "r_hand";

Move the right hand to a new position. Set the configuration programmatically. The marker moves to the new position of the end effector.

currConfig(1:18) = [-0.1350 -0.1498 -0.0167 -0.3415 0.4996 0.0747
                     0.0377 0.0718 -0.8117 -0.0427 0 0.4349 
                    -0.5738 0.0563 -0.0095 0.0518 0.8762 -0.0895]';
                
viztree.Configuration = currConfig;

Save the current configuration.

addConfiguration(viztree)

Add Constraints

By default, the robot model respects only the joint limits of the rigidBodyJoint objects associated with the RigidBodyTree property. To add constraints, generate Robot Constraint objects and specify them as a cell array in the Constraints property. To see a list of robotic constraints, see Inverse Kinematics. Specify a pose target for the pelvis to keep it fixed to the home position. Specify a position target for the right foot to be raised in front front and above its current position.

fixedWaist = constraintPoseTarget("pelvis");
raiseRightLeg = constraintPositionTarget("r_foot","TargetPosition",[1 0 0.5]);

Apply these constraints to the interactive rigid body tree object as a cell array. The right leg in the resulting figure changes position.

viztree.Constraints = {fixedWaist raiseRightLeg};                               

Notice the change in position of the right leg. Save this configuration as well.

addConfiguration(viztree)

Play Back Configurations

To play back configurations, iterate through the stored configurations index and set the current configuration equal to the stored configuration column vector at each iteration. Because configurations are stored as column vectors, use the second dimension of the matrix.

for i = 1:size(viztree.StoredConfigurations,2)
    viztree.Configuration = viztree.StoredConfigurations(:,i);
    pause(0.5)
end

Generate Robot Trajectory Using Interactive Rigid Body Tree Model

Use the interactiveRigidBodyTree object to visualize a robot model and interactively create waypoints and use them to generate a smooth trajectory using cubicpolytraj. For more information, see the interactiveRigidBodyTree object and cubicpolytraj function.

Load the Robot Model

Use the loadrobot function to access provided robot models as rigidBodyTree objects.

robot = loadrobot('abbIrb120'); 

Visualize Robot and Save Configurations

Create an interactive tree object using the interactiveRigidBodyTree function. By default, the interactive marker is set to the body with the highest index in the RigidBodyTree property. To change this property using the figure, right-click a rigid body and select Set body as marker body. Alternatively, MarkerBodyName property for the interactiveRigidBodyTree can be set using name-value pairs.

iRBT = interactiveRigidBodyTree(robot); 

Interactively Add Configurations

Click and drag the interactive marker to change the robot configuration. You can click and drag any of the axes for linear motion, rotate the body about an axis using the red, green, and blue circles, and drag the center of the interactive marker to position it in 3-D space.

The interactiveRigidBodyTree object uses inverse kinematics to determine a configuration that achieves the desired end-effector pose. If the associated rigid body cannot reach the marker, the figure renders the best configuration from the inverse kinematics solver.

When the robot is in a desired configuration use the addConfiguration object function to add the configuration to the StoredConfiguration property of the object.

In this example, 6 waypoints are created using the interactive marker and addConfiguration object function. They are saved in wayPoints.mat. Stored configurations can be accessed using iRBT.StoredConfigurations.

load("wayPts.mat");

Generate Smooth Trajectory Using the Waypoints

Use the cubicpolytraj function to generate smooth trajectory between the waypoints. Define time points that correspond to each waypoint. Define the time vector for generating the trajectory. The cubicpolyTraj function generates a configuration for each timestep in the time vector tvec.

iRBT.StoredConfigurations = wayPts ;          % Waypoints 
tpts = [0 2 4 6 8 10];                        % Time Points 
tvec = 0:0.1:10;                              % Time Vector 
[q,qd,qdd,pp] = cubicpolytraj(iRBT.StoredConfigurations,tpts,tvec); 

Visualize Robot Motion on the Trajectory

Define the simulation frequency using a rateControl object. Use the showFigure function to visualize the robot model and use a for loop to play all the configurations of the robot.

r = rateControl(10);
iRBT.ShowMarker = false;  % Hide the marker 


showFigure(iRBT)

for i = 1:size(q',1)
    iRBT.Configuration = q(:,i);
    waitfor(r);
end     

Limitations

• If the interactiveRigidBodyTree object is deleted while the figure is still open, the interactivity of the figure is disabled and the title of the figure is updated.

Tips

• To maximize performance when visualizing complex robot models with complex meshes, ensure you enable hardware-accelerated OpenGL. By default, MATLAB^® uses hardware-accelerated OpenGL if your graphics hardware supports it. For more information, see the opengl function.

Version History

Introduced in R2020a