Collision Avoidance - Path Planning

What you will learn

In this section, you will gain experience working with various forms of path planning to move a manipulator end effector around obstacles.

Prequisites

Background

The motion concepts explored by this example are explained below.

Joint Control

The most basic operation offered by Actin is joint control. The EcScript command move_joint allows the user to define target positions for all joints in a manipulator, which will then move to achieve those positions. This motion is performed with no path planning whatsoever and may be halted prematurely due to impending collisions between manipulator links or with other manipulators in the environment.

Joint control is typically employed by users to get a manipulator back into a home position, where the manipulator is already relatively close to the home position and/or the chance of collisions is small. It is also used internally by Actin as a building block for more complex EcScript commands like rrt_plan.

For convenience, the EcScript command set_position_state may be also used to snap a manipulator's joints instantaneously to the desired positions. This command should only be used within simulations: if used while Actin is controlling physical hardware, it will lead to large divergence between the expected and actual state of joints.

End Effector Placement

The most commonly used Action operation is end effector placement. This allows the user to define a target pose for a manipulator's TCP with a given set of constrained axes. Actin will then move the TCP to the target pose, while taking advantage of any unconstrained axes. This motion can be performed with local path planning, global path planning, or none whatsoever.

No Path Planning

The EcScript command move_pose, when used with a hard motion constraint, will command the TCP to move along the shortest straight path to the target pose, regardless of any obstacles in the way. Should a manipulator collision or joint limit become imminent during the motion, the motion will be halted prematurely.

Moving the end effector without path planning is typically used when the TCP is already relatively close to the target pose and/or the change of collisions is small. It is computationally cheaper than moving the TCP with path planning. It is also desirable for scenarios where any divergence of the TCP from the selected path or target pose cannot be tolerated.

Local Path Planning

The EcScript command move_pose, when used with a soft motion constraint, will also command the TCP to move along the shortest straight path to the target pose. However, unlike a hard motion constraint, should a manipulator collision or joint limit become imminent during the motion, the manipulator will attempt to deviate from the selected path to avoid the collision/limit and still reach the target pose. This behavior is known as local path planning.

The force and extent of path deviations during local path planning are configurable within the control system parameters of the Actin model (see "Editing Optimizations" section of Actin Viewer: Control System Tuning training). The type of motion constraint used is also significant: those with fewer constrained axes (e.g. FreeSpinZ) provide Actin with more flexibility in finding paths around obstacles.

Local path planning is not guaranteed to always find a way for the TCP to reach the target pose. It is a reactive operation that only accounts for obstacles in the immediate vicinity of the TCP, rather than considering all obstacles between the TCP and target pose. Large obstacles or those with complex bounding volumes are more challenging for it to circumvent. Should it fail to find a path around an obstacle, the TCP will be become stuck in place: motion will continue indefinitely, but very little change will occur to the TCP position/orientation.

Moving the end effector with local path planning is typically used when one or more small obstacles are known to be between the initial and target TCP pose. It can also be used in a dynamic manner where sensed obstacles need to be avoided in real-time. It is computationally much cheaper than moving the TCP with global path planning.

Global Path Planning

The EcScript command find_solution finds a set of manipulator joint angles that achieve a given target TCP pose. The rrt_plan command finds a sequence of move_joint commands that will achieve these joint angles while also ensuring the manipulator avoid collisions and joint limits during movement. The eval command executes this move_joint sequence. Together these commands implement a feature known as global path planning.

Like local path planning, global path planning is not guaranteed to always find a way for the TCP to reach the target pose: the target pose may be out of reach or too close to an obstacle. Unlike local path planning however, global path planning will consider all obstacles between the TCP and target pose. If possible, it will find a way around all of these obstacles. If this is not possible, it will either return an error (from find_solution) or time out (within rrt_plan). Also, unlike local path planning, global planning does not typically require configuration of control system parameters or usage of soft motion constraint to achieve a solution.

Moving the end effector with global path planning is typically used when one or more large obstacles are known to be between the initial and target TCP pose. It can also be used when the number and sizes of obstacles between the initial and target TCP poses is not known beforehand. It is computationally much more expensive than moving the TCP with local path planning and cannot be performed in real-time like the latter method can.

Setup

Here are the steps needed to properly set up this example.

Load System Model

Open the model file <ACTIN_UR_HOME\>/data/UR/rcf/system_control_executive/URInterfacePickandPlacePP_SCE.xml in Actin Viewer.

Add Soft Motion Constraints

Open the Motion Constraints panel in Actin Viewer (from the Edit --> Advanced --> Motion Contraints... menu).

Copy and paste Frame End-Effector Set [0] and rename it to Soft Frame End-Effector Set.

Within the Soft Frame End-Effector Set, change the Is Hard Constraint property for Wrist3 [0] from 1 to 0 as shown below.

Copy and paste FreeSpinZ End-Effector Set [1] and rename it to Soft FreeSpinZ End-Effector Set.

Within the Soft FreeSpinZ End-Effector Set, change the Is Hard Constraint property for Wrist3 [0] from 1 to 0 as shown below.

Load Motion Script

If the Motion Script GUI icon is not visible on the Actin Viewer tool bar (see Motion Script GUI section of Actin Viewer: Basics training), load the "motionScriptGuiPlugin.ecp" plugin.

Click on the Motion Script GUI icon to display the Motion Script GUI pane on the righthand side of Actin Viewer.

Copy the following script (referred to hereafter as path_test.ecs) and paste it into the Script text area in the Motion Script GUI pane.

(motion_seq
  # Constants
  # ---------

  # Manipulator indices in stated system
  (def_u32 ROBOT 0)

  # Motion constraint set indices for `ROBOT` manipulator
  (def_u32 HARD_FRAME 0) #Hard Frame constraint set
  (def_u32 HARD_FSZ 1)   #Hard 'Free Spin in Z' constraint set
  (def_u32 SOFT_FRAME 5) #Soft Frame constraint set
  (def_u32 SOFT_FSZ 6)   #Soft 'Free Spin in Z' constraint set

  # Initial joint vectors for `ROBOT` manipulator
  (def_real_vec HOME1 (-1.537550 -1.644889 -1.604207
                       -1.463190  1.570693  1.573507)) #TCP just above table divider
  (def_real_vec HOME2 (-1.537550 -1.683891 -1.768266
                       -1.260129  1.570693  1.573507)) #TCP just below table divider
  (def_real_vec HOME3 (-1.537550 -1.862974 -1.982535
                       -0.866777  1.570693  1.573507)) #TCP way below table divider

  # Goal end effector poses for `ROBOT` manipulator
  (def_pose GOAL1 ((-0.539556 -0.126747 0.338840)
                   (2.9e-05 0.917929 0.396743 -6.7e-05))) #TCP just above table divider
  (def_pose GOAL2 ((-0.539556 -0.126747 0.258840)
                   (2.9e-05 0.917929 0.396743 -6.7e-05))) #TCP just below table divider
  (def_pose GOAL3 ((-0.541093 -0.124919 0.101730)
                   (2.9e-05 0.917929 0.396743 -6.7e-05))) #TCP way below table divider
  (def_pose GOAL4 ((-0.541093 -0.124919 -0.100000)
                   (2.9e-05 0.917929 0.396743 -6.7e-05))) #TCP below table

  # Functions
  # ---------

  # Snap joints of `ROBOT` manipulator to given positions
  (def_fun ResetJoints ((def_real_vec jointVec))
    (set_position_state ROBOT jointVec)
  )

  # Move end effector to `ROBOT` manipulator to given pose using local path planning
  (def_fun MovePoseLocal ((def_u32 constraintSet) (def_pose goalPose))
    (move_pose ROBOT constraintSet 0 goalPose 0.3 0.1)
  )

  # Move end effector to `ROBOT` manipulator to given pose using global path planning
  (def_fun MovePoseGlobal ((def_u32 constraintSet) (def_pose goalPose))
    (def_real_vec goalJoints ())
    (find_solution ROBOT constraintSet goalJoints (goalPose))

    (def_string plannedPath "")
    (rrt_plan ROBOT goalJoints plannedPath
      (SpeedFactor 1.0) (AccelerationFactor 0.1)
      (CollisionThreshold 0.001) (Tolerance 0.01) (MaxTime 20.0) 
    )

    (if (!= (size_of plannedPath) 0)
      (motion_seq
        (print "Moving along planned path.")
        (eval plannedPath)
      )
      (motion_seq
        (print "Failed to find path.")
        (return)
      )
    )
  )

  # Main Logic
  # ----------

  (ResetJoints HOME1)
  (MovePoseLocal HARD_FRAME GOAL1)
)

Exercises

Each of the sections below uses small edits to path_test.ecs to illustrate the concepts explained in the Background section of this page.

TCP Motion with No Plan Planning: Success

This exercise uses move_pose with a hard motion constraint to move the robot gripper from one side of the table divider to the other. Because the initial and target TCP poses are above the table divider, no collision should occur and the motion should complete as expected.

To perform the exercise, run path_test.ecs in Actin Viewer.

The robot gripper should glide in a straight line from one side of the table separator to the other as illustrated below.

TCP Motion with No Plan Planning: Collision

This exercise also uses move_pose with a hard motion constraint to move the robot gripper from one side of the table divider to the other. However, because the initial and target TCP poses are below the table divider, an imminent collision with the divider should be detected and the motion halted prematurely.

To perform the exercise, replace the last three lines in path_test.ecs with the following lines and then run the script in Actin Viewer.

  (ResetJoints HOME2)
  (MovePoseLocal HARD_FRAME GOAL2)
)

The robot gripper should glide in a straight line towards the table divider and stop before touching the divider as illustrated below.

TCP Motion with Local Plan Planning: Resolvable Failure

This exercise uses move_pose with a soft frame motion constraint (see Frame EE section of Motion Control training) to move the robot gripper from one side of the table divider to the other. The gripper should approach the divider but fail to move past it because of the following problems:

  1. The initial and target TCP poses are below the table divider.
  2. There are no unconstrained axes in the target pose that would give the path planner more flexibility in finding an alternative path around the divider.
  3. The control system parameters are not configured to allow the path planner to diverge from the original path enough to get over the divider.

Resolving any one of these problems would allow the motion to be completed as expected.

To perform the exercise, replace the last three lines in path_test.ecs with the following lines and then run the script in Actin Viewer.

  (ResetJoints HOME2)
  (MovePoseLocal SOFT_FRAME GOAL2)
)

The robot gripper should glide in a straight line towards the table divider, rise a little bit, and then remain stuck in place as illustrated below. (Because the motion will not halt, you will need to manually stop the motion script.)

TCP Motion with Local Plan Planning: Solution A

This exercise uses move_pose with a soft free-spin-in-z motion constraint (see Free Spin in Z section of Motion Control training) to move the robot gripper from one side of the table divider to the other. Although the initial and target TCP poses are below the table divider, the unconstrained z orientation in the target pose should allow the motion planner to get the gripper over the divider to the target pose.

To perform the exercise, replace the last three lines in path_test.ecs with the following lines and then run the script in Actin Viewer.

  (ResetJoints HOME2)
  (MovePoseLocal SOFT_FSZ GOAL2)
)

The robot gripper should glide in a straight line towards the table divider, rise over the divider while rotating, and then glide in a straight line the remaining way to the target pose as illustrated below.

TCP Motion with Local Plan Planning: Solution B

This exercise uses move_pose with a soft frame motion constraint (see "Frame EE" section of Motion Control training) with modified control system parameters to move the robot gripper from one side of the table divider to the other. Although the initial and target TCP poses are below the table divider, the updated control system parameters should allow the motion planner to get the gripper over the divider to the target pose.

To perform the exercise, first open the Control System Parameters dialog in Actin Viewer (from the Edit --> Position Control System --> Control System Parameters... menu).

Then double-click on the Avoid Collisions and Joint Limits item under the Control Descriptions pane.

In the "Velocity Control Description" dialog that appears, first click on the "Collision Avoidance" item under the "Vector" pane. Then change the "avoidanceDistance" property under the "Collision Avoidance" pane from "0.005" to "0.025".

Click the "OK" button to close the "Velocity Control Description" dialog, and then click the "OK" button to close the "Control System Parameters" dialog.

Finally, replace the last three lines in path_test.ecs with the following lines and then run the script in Actin Viewer.

  (ResetJoints HOME2)
  (MovePoseLocal SOFT_FRAME GOAL2)
)

The robot gripper should glide in a straight line towards the table divider, rise over the divider without rotating, and then glide in a straight line the remaining way to the target pose as illustrated below.

TCP Motion with Local Plan Planning: Irresolvable Failure

This exercise uses move_pose with a soft frame motion constraint (see "Frame EE" section of Motion Control training) to move the robot gripper from one side of the table divider to the other. Although the control system parameters have been modified (during the previous exercise), the gripper should fail to clear the divider because of the initial and target TCP poses are too far below the divider (causing it to be treated as an excessively large obstacle).

To perform the exercise, replace the last three lines in "path_test.ecs" with the following lines and then run the script in Actin Viewer.

  (ResetJoints HOME3)
  (MovePoseLocal SOFT_FRAME GOAL3)
)

The robot gripper should glide in a straight line horizontally towards the table divider, rise a little bit, and then remain stuck in place as illustrated below. (Because the motion will not halt, you will need to manually stop the motion script.)

TCP Motion with Global Path Planning: Success

This exercise uses find_solution and rrt_plan to move the robot gripper from one side of the table divider to the other. Despite the initial and target TCP poses being way below the divider, the gripper should be able to clear the divider with ease.

To perform the exercise, replace the last three lines in "path_test.ecs" with the following lines and then run the script in Actin Viewer.

  (ResetJoints HOME3)
  (MovePoseGlobal HARD_FRAME GOAL3)
)

The robot gripper should move to a pose centered over the table divider and then move down to the target pose in manner similar to that illustrated below. (Because rrt_plan uses a randomized algorithm, the exact path taken by the gripper may be different from what is depicated here).

TCP Motion with Global Path Planning: Failure

This exercise uses find_solution and rrt_plan to move the robot gripper from one side of the table divider to the other. Because the target TCP poses is below the table however, the path planning should fail and no motion occur.

To perform the exercise, replace the last three lines in "path_test.ecs" with the following lines and then run the script in Actin Viewer.

(ResetJoints HOME3)
(MovePoseGlobal HARD_FRAME GOAL4)

The robot gripper should remain motionless and message similar to the following should be printed to the "Output" area in the "Motion Script GUI" pane in Actin Viewer.

EcMotionScriptFindSolution: solution not found.
rrt_plan: Target joint size error. Should be 6
Failed to find path.