Plan Manipulator Path for Dispensing Task Using Inverse Kinematics Designer

This example shows how to design a robotic manipulator path for a dispensing task. A successful path consists of a sequence of collision-free waypoints that are designed and verified in the Inverse Kinematics Designer app. Create waypoints for an adhesive dispensing task, in which the robot picks up two adhesive strips, applies glue, and then applies the strips to a box.

Create Robot and Environment Elements for Scene

This example has three main steps:

1. The robot picks up adhesive strips from a loading station using a custom tool.

2. The robot places the tool under a dispensing stand, where glue is applied to both strips.

3. The robot attaches the two strips to the object in the desired location.

The example uses a Universal Robots UR5e manipulator, equipped with a custom end effector. The result of this application is a series of waypoints that can be connected by using a planner, trajectory tooling, or both. The path planner attempts to generate a single collision-free path that reaches all the waypoints.

Create UR5e with Custom End Effector

This example uses the UR5e manipulator robot model, which is available in the robot library. Load the robot model from the library.

ur5e_robot = loadrobot("universalUR5e");

Create a custom gripper by using the customEEBuilder helper function, and then attach the custom gripper to the robot. Display the robot to verify that the gripper is attached to the robot.

customEE = customEEBuilder.build(true);
addSubtree(ur5e_robot,"tool0",customEE);
show(ur5e_robot);

Create Environment Representation

Add collision objects to the scene to represent scene objects. Create the collision objects primitives by using the constructToolStation helper function.

Add the feeder station at the position [0 0.4 0].

stationPose = trvec2tform([0 0.4 0]);
[toolStationBase,partFeeder] = constructToolStation(stationPose);

Next, add the dispensing station, modeled by a small cylinder next to the feeder at [.25 .45 .65].

dispensingStation = collisionCylinder(.01,.1);
dispensingStation.Pose = trvec2tform([.25 .45 .65]);

Create a platform underneath the robot, located at [0 0 -.011].

platform = collisionBox(1,1,0.02);
platform.Pose = trvec2tform([0 0 -.011]);

Add the box on which the robot places adhesive strips in front of the robot at [0.3 -.3 .05].

box = collisionBox(0.2,0.3,0.1,Pose=trvec2tform([0.3 -.3 .05]));

Use Inverse Kinematics Designer to Create Waypoints

Use Inverse Kinematics Designer to find the waypoint configurations that satisfy specific goals. Once the waypoints are created, export these configurations from the app to the base workspace.

To skip to the final state, load the included save session dispensingSessionData and its associated exported configurations by using these commands.

inverseKinematicsDesigner("dispensingSessionData.mat")
load pathWaypointData.mat

Otherwise, continue to the next section.

Start New Session

Now that the scene elements have been defined, start a new app session and populate it with the robot and environment pieces.

To start the app, open it from the Apps tab, or by entering this command:

inverseKinematicsDesigner

Next, start a new session. On the toolstrip click New Session to bring up the New Session dialog box. Set Rigid body tree to Load robot from workspace, and, in the Rigid body tree objects in workspace table, select ur5e_robot. Click OK.

Add Environment to Scene

To load the environment model, on the Inverse Kinematics tab, in the Scene section, select Add Collision Object. In the Add Collision Object dialog box, which shows all the available collision objects in the MATLAB® workspace, hold Shift and click to select the box, dispensing station, tool station base, part feeder, and platform.

Click Add to add these objects to the scene. In the Scene Canvas, to reposition the scene so it fits. Fore more information, see the Use Scene Canvas and Move Robot example on the Inverse Kinematics Designer page.

Add Waypoints for Each Task

Find the configurations that correspond to the waypoints for each task. Use constraints to ensure that each configuration satisfies the target pose.

Pick Up Adhesive Strips from Part Feeders

The two flat panels that simulate adhesive grip attachments on the gripper must align with the pads at the feeding station. If you are having trouble seeing these parts, you can temporarily disable the marker visuals. To disable the marker visuals, in the Constraints Browser, right-click Marker Pose Constraint, select Toggle marker display.

In the Scene Browser, select partFeeder. To ensure the part picker picks the parts properly, set a goal to place the attached strip modeled on the gripper so that it aligns with the part feeder. Read the pose of the object from the scene inspector. The object is located at [-0.05 0.4 0.525], and rotated 45 degrees about the world X-axis.

Move the marker pose target to this pose so the solver finds a configuration in which the robot is aligned in the picking task pose. While you can position the pose target using hand-tuning, the most accurate way is to directly set the end-effector pose. Select Marker Pose Constraint from the app toolstrip, or right-click the Marker Pose Target in the Constraints Browser and select Edit constraint. Once the Constraint tab opens, enter the pose of the object in the Pose Constraint section using these values:

• X (m) — -0.05

• Y (m) — 0.4

• Z (m) — 0.475

• Euler X (deg) — 45

As you add each value, the pose axis visual preview in the scene updates to indicate the new target pose.

To set this value as the target, Click Apply. This updates the pose, but the robot is backwards from the intended direction. Set Euler Y to 180 to add a rotation of 180 degrees about the Y-axis to flip the target pose.

You can determine the required orientation of the end effector ahead of time by examining the gripper.

show(customEE);

Select the adhesive strip to highlight the orientation of its origin and reference frame.

As indicated by the red, green, and blue highlighting on the tips of the violet reference joint marker, the Z-axis is orthogonal to the part, while the Y-axis points away from the L bracket that attaches to the gripper.

Therefore, you must satisfy these rules regarding part orientation:

• When picking up the part, the Z-axis must point at the feeder, and the Y-axis should be in-line with the global Y-axis.

• When applying glue, the Z-axis must point at the dispenser.

• When applying to a part, the Z-axis should be pointed at the part, with the Y-axis pointed down, so that the L bracket is above the part.

Adjust Configuration to Avoid Collisions

Determine which bodies are in collision by, on the Inverse Kinematics tab, selecting Check Collisions. Bodies highlighted red in the scene, and icons with a red x next to part names in the Scene Browser, indicate objects in collision. Select an object to display a list of the objects it is in collision with in the Scene Inspector.

In the Scene Browser, select toolStationBase, and note that the upper_arm_link and gripperBase links are in collision with it. Select partFeeder, and note that the adhesiveStrip object is in collision with it.

Collision between the adhesive strip and part feeder is expected, because the original marker pose target coincides with the position of the part feeder. However, assuming the part you are picking lays flat on the feeder, you can retract the end effector such that the end effector has enough clearance to pick up the part.

To retract the end effector, select the lateral Z-axis grip of the marker, and drag the marker along its local Z-axis.

Click Check Collision again to verify that the adhesive and gripper bodies are no longer in collision.

The other collision is between the tool station base and two links in the robot. To avoid these in-collision configurations, use constraints such as the Cartesian bounds constraint to adjust the solver so that it no longer finds configurations in which the forearm and upper arm can collide with the base.

You can use a bounds constraint to keep bodies inside a particular bounded range. In this case, the bound should keep the distal end of the upper arm from contacting the base. Since that end of the link is near coincident with the original (and reference point) of the forearm link, a Cartesian bounds constraint that prevents the forearm from reaching the base along Y-axis should prevent this collision. To add the constraint, on the Inverse Kinematics tab, click Add Constraint > Cartesian Bounds Constraint.

Create a target bounded region for the manipulator that avoids the tooling station base. Set the default upper Y-bound to 0.3 meters and the default upper Z-bound to 0.7 meters to create a region for this body that avoids the base. Additionally, set the weight on the X-direction to 0, since the range in that direction is unbounded.

Click Apply to apply the constraint, then exit the Constraints tab. The robot adjusts its position, but a and collision-check confirms that while the arm links are no longer in collision with the base, the wrist is still in collision. Create another Cartesian bounds constraint with a Y range of -0.5 meters to 0.2 meters to prevent this collision. Set the weights on the other bounds to 0, so that only Y is considered.

Apply the changes and exit the Constraints tab. Then click Check Collisions again verify that the configuration is collision-free.

While the solution is now collision-free and visually close to the target, the marker pose target constraint is not met. Click Refresh Solver to attempt to see if the solver can find another solution. Try this a few times. If the solver cannot find a solution where the marker pose target constraint is not met, modify the solver parameters so it can find a solution that satisfies all constraints during its execution time.

The default solver parameters are set to be fairly fast with just 50 iterations, which can struggle with a higher number of constraints. In the toolstrip, click Solver Settings or select the Solver tab. Then set the value of the number of Maximum Iterations to 500. Click Apply to run the solver and return to the Inverse Kinematics tab.

The robot should now be in a configuration that is collision-free and meets all constraints.

Once satisfied with the configuration, store it by clicking Store Configuration in the Configurations Panel. Rename the configuration to Pick up part.

The method used above to find a solution, i.e. adding constraints and extending solver run-time, is one way to ensure a result can be reached. Another method is to give the solver different starting configurations. Since each call to inverse kinematics uses the current configuration as the initial guess, you can "help" the solver by putting the robot in a pose that appears near satisfaction. This may be done by setting selecting links and modifying their joint values directly in the scene inspector, or by using the marker pose target to move the robot around. You can also continue to click Refresh Solver or modify the parameters further to search for different configurations if unsatisfied with the current one.

Apply Glue to Part at Dispensing Station

The next pose is below the dispensing station. For this constraint to be reachable, the second Cartesian constraint must be removed. De-select the constraint by clicking the check box next to Constraint2 [Cartesian]. Alternatively, right-click on it and delete it since it is no longer needed.

Next, click on the dispensing station to determine its global pose and dimensions, then modify the marker target pose so it lies at the base of the dispensing station. Since the dispensing station is located at [0.25 0.45 0.65] meters and has length 0.1 meters, set the target pose to 0.05 meters below the origin of the dispensing station at [0.25 0.45 0.6] meters. With the orientation set to [0 0 0] degrees so the Z axis of the adhesive strip is pointing at the dispensing station.

Apply the changes and exit the Constraints tab. Then drag the Z axis of the marker to provide some clearance between the part and the dispensing station. Check collisions and verify that the pose is collision-free.

Lastly, store the configuration and rename it to Dispense glue.

Apply Strips to Product

For the final configuration, the part will be applied to the box. As with the previous two configurations, the pose will be based off of the part location.

Start by determining the target pose. Click on the box, and choose a target that will orient the gripper orthogonally to the box with the L bracket above the box. Based on the position of the box, [0.3 -0.3 -.05], the gripper should be placed in the center of the box pointing outward, but with clearance for the 0.15 meter long strip to be placed without colliding with the platform. An example of such a position is [0.2 -0.3 0] meters. Using the orientation rules described, the Z axis of the gripper must point towards the box, along the global X axis, and the Y axis of the gripper must point down, towards the platform.

Apply the constraint, then exit the tab. Check collisions and observe that the robot has reached the configuration, but is still in collision with the box. Again, this is expected due to the positions coinciding. Provide a bit of clearance by dragging the Z axis of the marker to move the gripper. You can see the pose of the selected link in the Scene Inspector whenever the marker is released.

After verifying that the last configuration is collision-free, save it to the Configurations Browser.

Check that there are no collisions in any of the configurations and iterate through the waypoints.

Export Waypoints and Plan Path

Now that all waypoint configurations have been designed, click Export Path to export the waypoints to the workspace. Specify the name of the waypoint matrix, shift-click to select all waypoints, and click Export. The set of ordered waypoints now exist as a matrix of configurations in the workspace.

Plan Path Between Waypoints

Since the path between these waypoints is direct and clear of obstacles, a trapezoidal velocity profile trajectory could be used to connect these waypoints with smooth trajectories that stop at each waypoint.

However, because the robot is very close to the collision objects, it is more prudent to use a path planner. Use the manipulatorRRT function to plan a path between the three waypoints created in Inverse Kinematics Designer.

load pathWaypointData.mat
env = {dispensingStation partFeeder toolStationBase platform box};
planner = manipulatorRRT(ur5e_robot,env);
planner.MaxConnectionDistance = 0.45;
planner.ValidationDistance = 0.1;
planner.SkippedSelfCollisions = "parent";

rng(10);
numPts = 25;
numWaypoints = size(pathWaypoints,1);
paths = cell(1,numWaypoints);
for segIdx = 1:numWaypoints
    tic;
    plannedPath = plan(planner, pathWaypoints(segIdx,:), pathWaypoints(mod(segIdx,numWaypoints)+1,:));
    shortenedPath = shorten(planner, plannedPath, 10);
    paths{segIdx} = interpolate(planner, shortenedPath, 10);

    segTime = toc;
    disp(['Done planning for segment ',num2str(segIdx),' in ',num2str(segTime),' seconds']) %i in %f seconds\n])
end

Done planning for segment 1 in 3.1736 seconds
Done planning for segment 2 in 9.0467 seconds
Done planning for segment 3 in 4.9664 seconds

A complete path can be made by combining the segments.

totalSegs = vertcat(paths{:});

Visualize Completed Trajectory

Now that the waypoints have been planned, play back the path to make sure it works.

ax = show(ur5e_robot);
hold all
for i = 1:numel(env)
    env{i}.show(Parent=ax);
end

% Display the figure window the animation
pathFig = ancestor(ax,"figure");
set(pathFig,Visible="on")

% Set up timing and configure robot
r = rateControl(10);
tvec = linspace(1,numWaypoints,numWaypoints*numPts);
ur5e_robot.DataFormat = "row";

% Animate all path segments
for pathSegIdx = 1:numel(paths)
    path = paths{pathSegIdx};

    % For each path segment, step through all the configurations
    for configIdx = 1:size(path,1)
        show(ur5e_robot,path(configIdx,:),FastUpdate=true,PreservePlot=false,Parent=ax);
        waitfor(r);
    end

    % Hold the pose and update the title each time a waypoint is reached
    title(sprintf('Segment %i completed',pathSegIdx),Parent=ax)
    pause(1);
end

Next Steps

Once satisfied with the path, this workflow can be completed by smoothing the paths using trajectories and incorporating them into a higher-level workflow. For example, these paths may be deployed to a robot using hardware tools, integrating with hardware tools to actuate the gripper. See Robotics System Toolbox Supported Hardware for more information.