Interactively Build Robot Trajectory

Build a trajectory using interactive markers on the ABB YuMi robot model. The interactiveRigidBodyTree object displays the rigid body tree robot model in an interactive figure. This example shows how to move different parts of the robot, design a trajectory, and save configurations.

Load Robot Visualization and Build Environment

Load the 'abbYumi' robot model. Initialize the interactive figure using interactiveRigidBodyTree. Save the current axes.

robot = loadrobot('abbYumi', 'Gravity', [0 0 -9.81]);
iviz = interactiveRigidBodyTree(robot);
ax = gca;

Build an environment consisting of a collision boxes that represent a floor, two shelves with objects, and a center table.

plane = collisionBox(1.5,1.5,0.05);
plane.Pose = trvec2tform([0.25 0 -0.025]);
show(plane,'Parent', ax);

leftShelf = collisionBox(0.25,0.1,0.2);
leftShelf.Pose = trvec2tform([0.3 -.65 0.1]);
[~, patchObj] = show(leftShelf,'Parent',ax);
patchObj.FaceColor = [0 0 1];

rightShelf = collisionBox(0.25,0.1,0.2);
rightShelf.Pose = trvec2tform([0.3 .65 0.1]);
[~, patchObj] = show(rightShelf,'Parent',ax);
patchObj.FaceColor = [0 0 1];

leftWidget = collisionCylinder(0.01, 0.07);
leftWidget.Pose = trvec2tform([0.3 -0.65 0.225]);
[~, patchObj] = show(leftWidget,'Parent',ax);
patchObj.FaceColor = [1 0 0];

rightWidget = collisionBox(0.03, 0.02, 0.07);
rightWidget.Pose = trvec2tform([0.3 0.65 0.225]);
[~, patchObj] = show(rightWidget,'Parent',ax);
patchObj.FaceColor = [1 0 0];

centerTable = collisionBox(0.5,0.3,0.05);
centerTable.Pose = trvec2tform([0.75 0 0.025]);
[~, patchObj] = show(centerTable,'Parent',ax);
patchObj.FaceColor = [0 1 0];

Interactively Generate Configurations

Use the interactive visualization to move the robot around and set configurations. When the figure is initialized, the robot is in its home configuration with the arms crossed. Zoom in and click on an end effector to get more information.

To select the body as the end effector, right-click on the body and choose Set body as marker body.

The marker body can also be assigned from the command line:

iviz.MarkerBodyName = "gripper_r_base";

Once the body has been set, use the provided marker elements to move the marker around, and the selected body follows. Dragging the central gray marker moves the marker in Cartesian space. The red, green, and blue axes move the marker along the xyz-axes. The circles rotate the marker about the axes of equivalent color.

You can also move individual joints by right-clicking the joint and click Toggle marker control method.

These steps can also be accomplished by changing properties on the object directly. The MarkerControlMethod property of the object is set to "JointControl".

iviz.MarkerBodyName = "yumi_link_2_r";
iviz.MarkerControlMethod = "JointControl";

Changing to joint control produces a yellow marker that allows the joint position to be set directly.

Interactively move the robot around until you have a desired configuration. Save configurations using addConfiguration. Each call adds the current configuration to the StoredConfigurations property.

addConfiguration(iviz)

Define Waypoints for a Trajectory

For the purpose of this example, a set of configurations are provided in a .mat file.

Load the configurations, and specify them as the set of stored configurations. The first configuration is added by updating the Configuration property and calling addConfiguration, which you could do interactively, but the rest are simply added by assigning the StoredConfigurations property directly.

load abbYumiSaveTrajectoryWaypts.mat

removeConfigurations(iviz) % Clear stored configurations

% Start at a valid starting configuration
iviz.Configuration = startingConfig;

addConfiguration(iviz)

% Specify the entire set of waypoints
iviz.StoredConfigurations = [startingConfig, ...
                             graspApproachConfig, ...
                             graspPoseConfig, ...
                             graspDepartConfig, ...
                             placeApproachConfig, ...
                             placeConfig, ...
                             placeDepartConfig, ...
                             startingConfig];

Generate the Trajectory and Play It Back

Once all the waypoints have been stored, construct a trajectory that the robot follows. For this example, a trapezoidal velocity profile is generated using trapveltraj. A trapezoidal velocity profile means the robot stops smoothly at each waypoint, but achieves a set max speed while in motion.

numSamples = 100*size(iviz.StoredConfigurations, 2) + 1;
[q,~,~,tvec] = trapveltraj(iviz.StoredConfigurations,numSamples,'EndTime',2);

Replay the generated trajectory by iterating through the generated q matrix, which represents a series of joint configurations that move between each waypoint. In this case, a rate control object is used to ensure that the play back speed is reflective of the actual execution speed.

iviz.ShowMarker = false;
showFigure(iviz)
rateCtrlObj = rateControl(numSamples/(max(tvec) + tvec(2)));
for i = 1:numSamples
    iviz.Configuration = q(:,i);
    waitfor(rateCtrlObj);
end

The figure shows the robot executes a smooth trajectory between all the defined waypoints.

×

Ouvrir l'exemple

Vous possédez une version modifiée de cet exemple. Souhaitez-vous ouvrir cet exemple avec vos modifications ?

Non, écraser la version modifiée Oui