interactively build trajectory for abb yumi robot -凯发k8网页登录

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

the markercontrolmethod property of the object is set to "jointcontrol".

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.

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