Load Robot Model

Use theloadrobotfunction to access provided robot models asrigidBodyTreeobjects.

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.

TheinteractiveRigidBodyTreeobject 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 theRigidBodyTreeto theConfigurationproperty.

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 theStoredConfigurationsproperty.

addConfiguration(viztree)

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

You can also set theMarkerBodyNameproperty 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 therigidBodyJointobjects associated with theRigidBodyTreeproperty. To add constraints, generateRobot Constraintobjects and specify them as a cell array in theConstraintsproperty. To see a list of robotic constraints, seeInverse 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.

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

Load the Robot Model

Use theloadrobotfunction to access provided robot models asrigidBodyTreeobjects.

robot = loadrobot('abbIrb120');

Visualize Robot and Save Configurations

Create an interactive tree object using theinteractiveRigidBodyTreefunction. By default, the interactive marker is set to the body with the highest index in theRigidBodyTreeproperty. To change this property using the figure, right-click a rigid body and selectSet body as marker body．Alternatively,MarkerBodyNameproperty for theinteractiveRigidBodyTreecan 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.

TheinteractiveRigidBodyTreeobject 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 theaddConfigurationobject function to add the configuration to theStoredConfigurationproperty of the object.

In this example, 6 waypoints are created using the interactive marker andaddConfigurationobject function. They are saved inwayPoints.mat．Stored configurations can be accessed usingiRBT.StoredConfigurations．

load("wayPts.mat");

Generate Smooth Trajectory Using the Waypoints

Use thecubicpolytrajfunction to generate smooth trajectory between the waypoints. Define time points that correspond to each waypoint. Define the time vector for generating the trajectory. ThecubicpolyTrajfunction generates a configuration for each timestep in the timevectortvec.

iRBT.StoredConfigurations = wayPts ;% Waypointstpts = [0 2 4 6 8 10];% Time Pointstvec = 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 arateControlobject. Use theshowFigurefunction to visualize the robot model and use aforloop to play all the configurations of the robot.

r = rateControl(10); iRBT.ShowMarker = false;% Hide the markershowFigure(iRBT)fori = 1:size(q',1) iRBT.Configuration = q(:,i); waitfor(r);end