This example shows how to use the
interactiveRigidBodyTree object to move a robot, design a trajectory, and replay it.
'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];
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 to select it.
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.
MarkerControlMethod property of the object is set to
These steps can also be accomplished by changing properties on the object directly.
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.
Iteractively move the robot around until you have a desired configuration. Save configurations using
addConfiguration. Each call adds the current configuration to the
For the purpose of this example, a set of configurations are provided in a
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];
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 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.