Interactively Build A 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
'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
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
Define Waypoints for a Trajectory
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];
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.