Pick and Place - Path Planning

What you will learn

How to use Actin to build a pick and place using global path planning with a Universal Robots model UR5e and a Festo EHPS-20 gripper.

Model Tuning

In this demo, we provide a tuned UR5e robot model. You will learn how to tune the other converted actin models in the Actin Viewer

Renaming Model

Rename UR5e.ecz to URInterfacePickandPlacePP.ecz. A converted and tuned model of the UR5e is provided in the installer actin_ur/data directory or here

Using Actin Viewer, open URInterfacePickandPlacePP.ecz and notice in the lower left corner of the window, the manipulator index is 0 for the UR5e.

Bounding Volumes

The CAD converter, in our case the SolidWorks add-in, has automatically created bounding volumes for each manipulator/link. Sometimes the auto-generated shapes are not optimal and should be tuned. Bounding volumes are configured with primitive shapes for collision detection and collision avoidance. For information about Bounding Volumes, visit here.

Download the file Table.ecz here.

Open file Table.ecz and tune the bounding volumes. Select EditBounding Volumes and Shapes…

Click the Bounding Volumes radio button.

Adjust the Table bounding volumes.

Download the file EHPS-20.ecz here

Repeat the bounding volume adjustments for the EHPS-20 gripper.

Download the file 25x25x500extrusionbar.ecz here.

Repeat the bounding volume adjustments for the 25x25x500extrusionbar.

Merge

In this step, we merge all the individual components: the table, gripper and extrusion bar into a single model file URInterfacePickandPlacePP.ecz. This allows all manipulators/converted files to be in the same simulation environment.

Open the file URInterfacePickandPlacePP.ecz in Actin Viewer.

Then, select FileMerge

The external files to be merged are:

  1. Table.ecz
  2. EHPS-20.ecz
  3. 25x25x500extrusionbar.ecz

Note: Make sure to merge the files in that exact order. Don’t worry about the placements of the merged objects. That will be corrected later with attachments.

The full tuned merged model can also be downloaded here.

Collision Exclusions

A collision exclusion map is created to indicate which manipulators in the scene can collide with each other. Information about Collision Exclusions can be found here.

Click on EditCollision Exclusions…

Create the Collision Exclusions map. Adjacent links should be excluded from collision in the self-exclusion map. Manipulators that are constant contact with each other should be excluded from the external collision map.

Position Control System

The position controller for the UR5e robot must be enabled. To verify, select EditPosition Control SystemControl System Parameters…

Disable the position controller for the Table, EHPS-20, and 25x25x500extrusionbar. The boxes must be unchecked to allow attachments in EcScript during runtime.

Uncheck Table Position Controller.

Uncheck EHPS-20 Position Controller.

Uncheck 25x25x500extrusionbar Position Controller.

EcScript

EcScript is A LISP-like language to create motion and tasking sequences in Actin. During run-time, the EcScript is parsed and converted into an EcMotionScriptObject. It is this object that is executed at run time. The design of the motion planning library is such as to not dynamically allocate memory at runtime of the EcScript.

Download the EcScript file here.

Or copy the code and name the file URInterfacePickAndPlace.ecs:

(motion_seq
   ########## Setup ##########
   # Inspection table attachment
   (attach_manip 1 0 BASE ((0 0 -0.0113m)(0 0 0)))

   # Tool attachment
   (attach_manip 2 0 5 ((0 0 0)(0 0 0)))

   # Extrusion Bar attachment
   (attach_manip 3 "BAR_PICK" 1 BASE "TABLE_PICK_PLACE_1_1")

   # Getting pose of wrist 3 in system frame
   (def_pose wrist3_in_sys (get_frame 0 5 "Primary Frame"))   
   #Getting TCP of gripper in system frame
   (def_pose tcp_in_sys (get_frame 2 BASE "GRIPPER_ACTUATION"))
   # Calculating the offset from wrist 3 to gripper TCP
   (def_pose tcp_in_wrist (* (inv wrist3_in_sys) tcp_in_sys))
   # Setting the TCP relative to wrist3
   (set_tcp 0 0 tcp_in_wrist)

   # Home joints vector
   (def_real_vec home (-1.5707963267948966 -1.5707963267948966 -1.5707963267948966 0 1.5707963267948966 .7853981633974483))

   ##### Functions #####
   # Inspection NamedFrame using RRT
   (def_fun RRT ((def_string named_frame))

      # Define pose of named_frame on Table in system frame
      (def_pose pose_in_sys (get_frame 1 BASE named_frame))

      # Define Vector of doubles and find joint solution of target frame
      (def_real_vec rv0 ())
      (find_solution 0 0 rv0 (pose_in_sys) (CollisionThreshold 1e-4))

      # RRT Path Planning and Execution  
      (rrt_plan 0 rv0
            (MaxTime 15.0) (CollisionThreshold 1e-4) 
            (Tolerance 1e-4) (SpeedFactor 1.0) (AccelerationFactor 1.0)
      )
      (wait 0.06)
   )

   # Sets the digital output mask to binary 3, 0000 0011
   # This sets which binary locations will be used
   #(set_data_store_u8 "STANDARD_DIGITAL_OUTPUT_MASK" "/transfer_layers/rtde" 3)

   # Close Gripper
   (def_fun CloseGripper ()
   (wait 1.0)
   (set_position_state 2 (7.5mm 7.5mm))
   #Digital Output 0 - Gripper1 Close, binary = 1, 0000 0001
   #(set_data_store_u8 "STANDARD_DIGITAL_OUTPUT" "/transfer_layers/rtde" 1)
   (wait 1.0)
   )

   #Open Gripper
   (def_fun OpenGripper ()
   (wait 1.0)
   (set_position_state 2 (0 0))
   #Digital Output 1 - Gripper1 Open, binary = 2, 0000 0010
   #(set_data_store_u8 "STANDARD_DIGITAL_OUTPUT" "/transfer_layers/rtde" 2)
   (wait 1.0)
   )

   ########## main ##########
   # Excluding collision with the table and extrusion bar 
   (exclude_collision 1 3)
   # Opening gripper at Start
   (OpenGripper)
   # RRT to home
   (rrt_plan 0 home 
            (MaxTime 15.0) (CollisionThreshold 1e-4) 
            (Tolerance 1e-4) (SpeedFactor 1.0) (AccelerationFactor 1.0)
   )
   # RRT to first pre-pick location
   (RRT "TABLE_PICK_PLACE_1_2")
   # Excluding collision with the table and gripper
   (exclude_collision 1 2)
   # Exclusing collision with gripper and table
   (exclude_collision 2 3)
   # Moving locally to pick location
   (move_pose 0 0 0 (get_frame 1 BASE "TABLE_PICK_PLACE_1_1") (0.1 0.1) 1e-4)
   # Grasping the bar
   (CloseGripper)
   # Detach from the bar from the table
   (detach_manip 3)
   # Attach the bar to the gripper 
   (attach_manip 3 "BAR_PICK" 2 BASE "GRIPPER_ACTUATION")
   # Retract the manipulator to pre-pick location 
   (move_pose 0 0 0 (get_frame 1 BASE "TABLE_PICK_PLACE_1_2") (0.1 0.1) 1e-4)
   # Including collision with the table and gripper
   (include_collision 1 2)
   # Including collision with the table and bar
   (include_collision 1 3)
   # RRT to pre-place location
   (RRT "TABLE_PICK_PLACE_2_2")
   # Excluding collision with the table and gripper
   (exclude_collision 1 2)
   # Excluding collision with the table and bar
   (exclude_collision 1 3)
   # Excluding collision with the gripper and bar
   (exclude_collision 2 3)
   # Moving locally to place location
   (move_pose 0 0 0 (get_frame 1 BASE "TABLE_PICK_PLACE_2_1") (0.1 0.1) 1e-4)
   # Opening gripper
   (OpenGripper)
   # Detaching bar from gripper
   (detach_manip 3)
   # Moving locally to pre-pick location
   (move_pose 0 0 0 (get_frame 1 BASE "TABLE_PICK_PLACE_2_2") (0.1 0.1) 1e-4)
   # Including collision with table and gripper
   (include_collision 1 2)
   # RRT to home
   (rrt_plan 0 home 
            (MaxTime 15.0) (CollisionThreshold 1e-4) 
            (Tolerance 1e-4) (SpeedFactor 1.0) (AccelerationFactor 1.0)
   )
   # RRT to second pre-pick location
   (RRT "TABLE_PICK_PLACE_2_2")
   # Excluding collision with the table and gripper
   (exclude_collision 1 2)
   # Excluding collision with the gripper and bar
   (exclude_collision 2 3)
   # Moving locally to pick location
   (move_pose 0 0 0 (get_frame 1 BASE "TABLE_PICK_PLACE_2_1") (0.1 0.1) 1e-4)
   # Grasping bar
   (CloseGripper)
   # Attach the bar to the gripper 
   (attach_manip 3 "BAR_PICK" 2 BASE "GRIPPER_ACTUATION")
   # Retract the manipulator to pre-pick location 
   (move_pose 0 0 0 (get_frame 1 BASE "TABLE_PICK_PLACE_2_2") (0.1 0.1) 1e-4)
   # Including collision with the table and gripper
   (include_collision 1 2)
   # Including collision with the table and bar
   (include_collision 1 3)
   # RRT to pre-place location
   (RRT "TABLE_PICK_PLACE_1_2")
   # Excluding collision with the table and gripper
   (exclude_collision 1 2)
   # Excluding collision with the table and bar
   (exclude_collision 1 3)
   # Excluding collision with the gripper and bar
   (exclude_collision 2 3)
   # Moving locally to place location
   (move_pose 0 0 0 (get_frame 1 BASE "TABLE_PICK_PLACE_1_1") (0.1 0.1) 1e-4)
   # Release bar
   (OpenGripper)
   # Detach from the bar from the gripper
   (detach_manip 3)
   # Retract the manipulator to pre-pick location 
   (move_pose 0 0 0 (get_frame 1 BASE "TABLE_PICK_PLACE_1_2") (0.1 0.1) 1e-4)
   # Including collision with the table and gripper
   (include_collision 1 2)
)

Select EditMotion Script GUI to display the panel with the EcScript code editor.

In the editor panel, Open the URInterfacePickAndPlace.ecs file that you previously downloaded or created.

Save the URInterfacePickAndPlace.ecs inside your Actin SDK installation directory:

<actin_installation>\share\data\actin_ur\data\ur_ref_design\scripts\URInterfacePickAndPlace.ecs

The Motion Script Gui is a tool to create robotic motion using EcScript and is used to perform analysis and validation during simulation.

Run the script to see the robot perform its task.

When running the scipt for the first time, you will notice the objects in the scene snap into place. This snapping is the result of the manipulator attachments in EcScript.

For more information about EcScript, visit here.

Up until now, everything has been done in the Actin Viewer. Motions were tested on the tuned model using the Actin Viewer and the Motion Script Gui plugin. The same script will be used in the Robot Control Framework (RCF) to drive the UR5e in real time.

Actin Viewer will no longer be used in the following sections.

Hardware/UR Simulator Communication

The Pick and Place - Path Planning reference can be used to drive actual hardware or drive the URSim. URSim is a simulation software in a virtual machine used for offline programming and simulating robot programs. Essentially, it emulates the UR controller and emulates the teach pendant. You will be guided through using the URSim to control the UR5e simulation in real-time.

To control the hardware, simply use the physical robot IP Address, which can be found using the real teach pendant instead of the URSim’s interface.

Export System Control Executive

The System Control Executive is required from the Actin Model. It contains all the relevant components of the model required for controlling the robot.

Open Export…

Save the file as a (.xml) in the Actin SDK installation directory:

<actin_installation>\share\data\actin_ur\data\ur_ref_design\ system_control_executive\URInterfacePickandPlacePP_SCE.xml

URInterfacePickandPlacePP_SCE.xml can also be downloaded here.

URInterfacePickandPlacePP_SCE.xml is generated.

Robot IP Address (URSim)

The robot IP Address is required to have Actin control the UR5e. Make sure the Program and the robot/URSim is connected locally (Ethernet, Virtual Machine).

Start the Virtual Machine containing the URSim image and run URSim UR5.

Universal Robots has instructions on running URSim in a Virtual Machine. It is assumed that you know how to do this or already have it running.

Click About, located in the top right corner drop down menu.

Copy the displayed IP address. It will be used later. The IP Address here is 192.168.177.128.

Click the Installation button on the top tool bar. This is where you can view the UR5e simulation motion.

Edit the System Controller

The system controller xml configuration file, used by Actin's RT System controller, points to the system control executive. That was the file that you saved in Export System Control Executive. You'll need to edit the system controller to point to this new file.

In the system_controller.xml (Actin installation directory):

Open:

<actin_installation>\share\data\actin_ur\data\ur_ref_design\system_controller.xml

Find:

<controlExecutiveFileName>./system_control_executive/UR10eTable.ecz</controlExecutiveFileName>

Replace:

<controlExecutiveFileName>./system_control_executive/URInterfacePickandPlacePP_SCE.xml</controlExecutiveFileName>

Edit the Manipulator Controller

In the ur_manipulator_controller.xml (Actin installation directory):

Open:

<actin_installation>\share\data\actin_ur\data\ manipulator_controllers\ ur_manipulator_controller.xml

Find All:

UR10e

Replace All:

UR5e

RTSystemController and ActinViz Startup

Open two command prompts. You need one to start the real time system controller and the other to run ActinViz.. In Windows, you will use the command prompt (cmd.exe). In linux, open a shell window (terminal). These commands will be slightly different depending on your OS.

Shell 1 - RTSystemController Startup
cd <actin_installation>\build\

#Set up the environment variables
SETUP.bat

#Set the IP address for the robot, the one you saved earlier.
set EC_UR_ROBOT_IP=192.168.177.128

cd ..\bin
rtSystemControllerApp.exe ..\share\data\actin_ur\data\ur_ref_design\system_controller.xml

The startup sequence should look like this:

Shell 2
cd <actin_installation>\build\

# Set up the environment variables by entering in both Shell1 and Shell2:
SETUP.bat

cd ..\bin

# Start ActinViz and connect to the local host:
actinViz.exe -c localhost

ActinViz is now loaded and should render the tuned model.

ActinViz does not have the full functionality of the Actin Viewer. It provides visualization and an EcScript interface.

Execution

In ActinViz, click open the clicking right slider tab on the right with the arrow and select the Control Panel tab.

Change the Robot Connection, Robot Power, Actin Control all to YES.

You should see that in the URSim and ActinViz, both displayed robots have matching poses.

In the Actin Viz Control Panel, change the Actin Mode to SERVO. This will allow the EcScript execution to control the URSim.

Click Script and Open the motion script file URInterfacePickAndPlace.ecs.

The URInterfacePickAndPlace.ecs is located:

<actin_installation>\share\data\actin_ur\data\ur_ref_design\scripts\

Uncomment the following lines in URInterfacePickAndPlace.ecs:

From:

47  #(set_data_store_u8 "STANDARD_DIGITAL_OUTPUT_MASK" "/transfer_layers/rtde" 3)
...
54  #(set_data_store_u8 "STANDARD_DIGITAL_OUTPUT" "/transfer_layers/rtde" 1)
...
63  #(set_data_store_u8 "STANDARD_DIGITAL_OUTPUT" "/transfer_layers/rtde" 2)

To:

47  (set_data_store_u8 "STANDARD_DIGITAL_OUTPUT_MASK" "/transfer_layers/rtde" 3)
...
54  (set_data_store_u8 "STANDARD_DIGITAL_OUTPUT" "/transfer_layers/rtde" 1)
...
63  (set_data_store_u8 "STANDARD_DIGITAL_OUTPUT" "/transfer_layers/rtde" 2)

Uncommenting this turns on the digital IO and allows you to read/write to the data store.

For more information about RCF EcScript visit here.

Execute the script by clicking on the play button.

Notice Digital Output 0 and 1 toggle in the URSim as the gripper is opens and closes.

Congratulations! You have completed the Pick and Place - Path Planning with the UR5e!