LAB4 : Motion Planning and Obstacle Avoidance

ROBOT MODELING AND CONTROL
ECE470S
LAB4 : Motion Planning and Obstacle Avoidance
with the KUKA Robotic Arm
1 Purpose
The purpose of this lab is to adapt the results of Lab 3 for implementation in a real robot. The objective is
to use the artificial potential algorithm to make the KUKA robotic arm pick up an object and drop it in a
cup while avoiding obstacles that are placed in the workspace.
2 Introduction
In lab 3 you experimented with the artificial potential motion planning algorithm. This algorithm generates
attractive and repulsive potentials, which are used in conjunction with a gradient descent algorithm to
generate a trajectory through obstacles from one point to another.
In this lab, you will implement this algorithm with the KUKA robotic arm. During the laboratory, you will
experiment with various parameters of the algorithm and investigate its strengths and weaknesses. Further,
you will see how a simulated experiment translates to real mechanical hardware. You will also have the
chance to improve the algorithm as much as you can, utilizing the Manipulator API.
3 Preparation
Please submit a complete preparation at the beginning of the lab session. Prior to the lab, you should
make sure that your repulsive and attractive force functions return the expected values. In this lab, instead
of having a pencil attached to the gripper, we will be using the gripper to grasp objects. The offset a6 now
corresponds to the length of the gripper which is approximately a6 = 156mm.
1. Modify your motion planning algorithm from Lab 3 (developed for the PUMA 560 manipulator) to
work for the KUKA manipulator. The main difference is the DH table. For better results, set a6 = 0
when deriving the attractive and repulsive force functions. You can create a second DH table called
DH forces for this purpose. This shifts the point on the gripper where the attractive and repulsive
forces act. However you must still use a6 = 156 in your inverse kinematics computations. Otherwise
the gripper will collide with the ground.
2. Write neatly on paper the expression of repulsive forces for the following two objects: 1) repulsion
upward from the Workspace plane, which we assume to be parallel to the x0 − y0 plane, and have a z0
value of 32 mm; 2) Repulsion from a cylinder of finite length. The bottom of the cylinder lies on the
x0 − y0 plane and the height of the cylinder is a parameter h.
3. Modify your repulsive function to account for the two new objects described in point 2 above: the
horizontal plane z = 32 mm and the finite length cylinder. Set η = 1 in the repulsive function.
1
4. Create a Matlab script setupobstacle lab4prep.m containing the statements:
prepobs{1}.R = 100;
prepobs{1}.c = [250; 0];
prepobs{1}.rho0 = 500;
prepobs{1}.h = 300;
prepobs{1}.type = ’cyl’;
Test your repulsive function; you should get:
>>setupobstacle_lab4rep;
>>tau = rep(myrobot,[pi/10,pi/12,pi/6,pi/2,pi/2,-pi/6],obs);
tau = [0.1795 0.9540 0.2353 -0.0344 -0.0344 0.0000]
In particular, the repulsive forces should be
Frep =
1.0e-06 *
-0.1269 0.0059 0.0073 0.0168 0.0168 0.0189
0.0043 0.0096 0.0092 0.0096 0.0096 0.0018
0.1005 0.1037 0.0615 0 0 0
5. Test that the motion planning algorithm works. Set the attractive and repulsive force coefficients as
αatt = 0.013 and αrep = 0.01 respectively. The following trajectory must be demonstrated for the
preparation (i.e., the arm should plan a trajectory over the cylinder successfully). Observe the effect
of reducing rho0.
>> kuka = mykuka(DH);
>> kuka_forces = mykuka(DH_forces);
>> p1 = [620 375 50];
>> p2 = [620 -375 50];
>> R=[0 0 1;0 -1 0;1 0 0];
>> H1=[R P1’;zeros(1,3) 1];
>> H2=[R P2’;zeros(1,3) 1];
>> q1 = inverse_kuka(kuka, H1);
>> q2 = inverse_kuka(kuka, H2);
>> motionplan(q1, q2, kuka_forces, obs);
6. Read Section 4.3 of this document, and discuss with your partner ideas for creative motion planning.
Robot Manipulator API
For your reference, below are the Matlab commands you will use to control the Kuka robot arm. You have
practiced these commands in Lab 0.
• startConnection establishes connection between Matlab and KUKA.
2
• stopConnection terminates connection between Matlab and KUKA.
• getAngles() returns the vector of current joint angles q = (θ1, θ2, θ3, θ4, θ5, θ6).
• stop() stops KUKA motion.
• moveAxis(axis,vel) moves a single KUKA axis. axis takes a value between 1 and 6 that corresponds
to the joint angle to be commanded. vel is a signed value that determines the commanded angular
speed of the joint. In this lab vel should be set to no greater than 0.01.
• setAngles(q,vel) sets KUKA arm angles to those defined in q to within a small tolerance. vel
corresponds to the speed of motion. For this lab, set vel to 0.04. To cancel the command during
execution press ctrl c and run stop() in the Matlab command window.
• setHome(vel) sets KUKA arm to the HOME configuration in Figure ??. vel corresponds to the speed
of motion. For this lab, set vel to 0.04. To cancel the command during execution press ctrl c and
run stop() in the Matlab command window.
• setGripper(state) sets the gripper state. setGripper(0) closes the gripper; setgripper(1) opens
the gripper.
Warning: Never use the clear all command in Matlab. Instead use clearvars -except udpObj
to avoid errors from occurring.
The steps to connect KUKA to the external PC:
• Run startConnection in Matlab.
• On the KUKA SmartPad run RSI Ethernet.src until the line RSI MOVECORR().
• To run the program, hold down half-way one of the enable buttons on the back of the KUKA SmartPAD.
The enable buttons are labelled 3 and 5 in Figure 1. If you press too hard, an emergency stop will
be activated. While holding down the enable button, press and hold the run button labelled 10 in
Figure 2. Note that when running the line RSI ON a warning will appear saying ‘Caution - sensor
correction is activated”. Simply confirm this warning to continue.
The steps to command the KUKA arm from Matlab are as follows:
• Run the desired command in the Matlab command window. The robot will not yet move.
• Hold down half-way one of the enable buttons on the back of the KUKA SmartPAD.
• While holding down the enable button, press and hold the run button. Now the robot will carry out
the desired command as long as both the enable and run buttons are being held down.
• To make the robot stop moving, simply release the enable button. To cancel the current command,
press ctrl c and run stop().
• If the robot ever hits an object while moving (such as the ground), immediately release the enable
button and the robot will stop. Then press ctrl c in the Matlab control window to cancel the
command and run stop(). Immediately call a TA to resolve the collision.
• If the robot does not move when commanded, it means an error has occurred. Call a TA to resolve
the issue.
• For safety, students must never open the safety gate enclosing the robot unless instructed to do so.
The enable and start button must never be pressed while the safety gate is open.
3
Figure 1: 3 and 5 are Enable buttons on the KUKA SmartPAD.
Warning: Make sure to FIRST connect KUKA to Matlab using startConnection.m. THEN
run the RSI from the Kuka SmartPad
The steps to disconnect KUKA from the external PC:
• When finished running the lab, return to the program RSI Ethernet.src on the SmartPAD, click on
line 32 and then click Block selection to set the program cursor to the command ret=RSI OFF().
Then continue running the program to the end. The program status indicator will turn black to
indicate the program is complete.
• Run stopConnection in Matlab.
The steps to deal with an RSI connection problem:
• Type CTRL+C within the Matlab environment.
• There are two options available on the top portion of the SmartPAD screen: cancel or reset. Use
reset, not cancel, to reinitialize the RSI program from the Kuka SmartPAD.
• After resetting the RSI program, push the play button multiple times until the program reaches the
RSI MOVECORR() line.
• In Matlab, run getAngles() to verify that the RSI connection has been correctly re-established.
4 Experiment
The following steps should guide you in investigating the motion planning algorithm with the KUKA manipulator. You will be varying parameters to see how the actual implementation works and commanding
4
Figure 2: 10 is the Run button on the KUKA SmartPAD.
the robot to move while avoiding real obstacles in the workspace. You will also witness some challenges in
working with hardware in contrast to simulations.
In this lab, you will not use the Matlab function FrameTransformation.m because all specifications will be
directly expressed in the base frame of the robot.
4.1 Initial Motion Planning in Simulation
Now that you have calibrated the robot, you will perform motion planning with your algorithm and the
manipulator.
1. Define the points:
p0 = [370 -440 150];
p1 = [370 -440 z_grid];
p2 = [750 -220 225];
p3 = [620 350 225];
where z grid = 45 mm. You might tweak this value later to make sure that the end effector correctly
grabs the object.
2. Create a new function setupobstacle.m that has three obstacles. The ground plane z = 32 mm, with
ρ0 = 150, and two cylinders of radius 100 mm and height 572 mm with ρ0 = 150. One cylinder is
centred at [620; 0] and the other cylinder is centred at [620; −440]
3. Set the coefficient of the repulsive force η = 1, and both repulsive and attractive force coefficients
α = 0.01.
5
4. Perform motion planning from the HOME configuration to p0, from p0 to p1, from p1 to p2, and from
p2 to p3 ensuring that the gripper is pointing towards the ground at each end point. That is,
R
0
6 =


0 0 1
0 − 1 0
1 0 0

 .
As in Lab 3, the outputs of att and rep do not affect q(6). Simply use the command linspace to
make q(6) transition from its initial value to the desired final value for each segment connecting pi to
pi+1. Test your algorithm in simulation. You may have to tune the parameters to make this succeed.
Show these results to your lab TA.
4.2 Initial Motion Planning with Kuka
Now you will use the motion planning algorithm tested in simulation to command the actual robot arm.
1. Send the robot to the HOME configuration.
2. Place the provided target object at p1. Place the provided target basket at p3. Align the target object
so that the gripper will be able to grasp it. Finally, place the obstacles at the appropriate positions.
3. Send the angles generated by the motion planning to the robot arm. Once again, you may have to vary
parameters so that the trajectory succeeds without hitting the obstacles. Your software should start
with the gripper open and move to point p0 which is just above the target object. Once at the point
p0, use setAngles to move directly to point p1 without obstacle avoidance. Then close the gripper
to grasp the target object at p1, move through the trajectory, and open the gripper at p3 to drop the
object into the target basket.
4. Without changing the force parameters, change the cylinder position within a 5-10 cm radius of the
original position. To do this, update the appropriate cylinder parameters in setupobstacle.m and
move the physical obstacle to the corresponding position. Then perform motion planning and send the
angles to the robot arm.
5. What effect does varying the algorithm parameters have? Discuss repulsive and attractive α, repulsive
η, and the ρ0 associated with each obstacle.
Show these results to your lab TA.
4.3 Creative Motion Planning with Kuka
1. This section is to challenge you to come up with your own creative obstacle avoidance algorithm. You
should utilize the gripper function to pick up a provided object and drop it at some target place. Some
ideas are below:
• Picking up multiple objects for multiple trajectories.
• Stacking some items one on top of each other.
• Add more cylinders, or bring new obstacles and develop functions for them.
• The potential force method is only one algorithm for obstacle avoidance. Develop a different
motion planning algorithm that does not make use of potential forces. For instance, you could
exploit the specific geometry of the KUKA robot to come up with an intuitive algorithm that
plans a trajectory that avoids the obstacles.
Show your results to the lab TA. Shoot a movie of the trajectory.
6
5 Submission
No later than one week after your lab session, submit on Quercus a zipped folder containing:
1. ALL of your Matlab functions. Thoroughly comment your code. Your folder may contain intermediate
Matlab functions, but calls to the main functions must work without any modification required.
2. The main Matlab file which should be named Lab4.m.
3. Any movies captured throughout the lab.
7