Using the motion planning functionality

WARNING: Since V-REP release 3.3.0, a plugin wrapping the OMPL library is available. It is highly recommended to use the path planning API functions offered by that plugin, instead of using this old built-in path/motion planning functionality, since the plugin/OMPL approach is much more flexible and scalable. The following documentation is deprecated.

Before setting-up a motion planning task, make sure you have understood how the motion planning module operates. Following are the different items that should be prepared in order to create a functional motion planning task object:

  • Creating an empty motion planning task
  • Preparing and testing the manipulator kinematics
  • Selecting appropriate collision entities
  • Adjusting various parameters
  • Executing motion planning commands


  • Creating an empty motion planning task

    In order to create an empty motion planning task, you need to specify which joints are part of the manipulator. This typically does not include gripper joints or other auxiliary joint. The joints should be part of the same kinematic chain. Select the joints from base to tip, then in the motion planning dialog, click Add new object. Rename the added object appropriately in order to quickly recognize to which manipulator it relates.

    [Serial manipulator with 3 joints (DoF)]



    Preparing and testing the manipulator kinematics

    Each motion planning task requires an associated IK group in order to be operational. The IK group should contain exactly one IK element, which defines a kinematic chain involving the same joints as specified in previous step. Make sure to correctly define the base of the IK element (which should be the base of the manipulator), and its constraints (that are normally expressed relative to the specified base):

    [IK group definition for the motion planning task]


    The IK group should use an undamped resolution method (i.e. the pseudo inverse method), which means that the IK element should not be overconstrained. In order to test the created IK group, you should:

  • temporarily turn off dynamics (in the general dynamics properties)
  • temporarily set all involved joints into the inverse kinematics mode, with hybrid operation disabled (in the joint properties)
  • Test the created IK group by running the simulation and changing the position/orientation of the target: the manipulator's end-effector should follow the target in a stable and robust way (given that the target is reachable and not in a singular pose). If this is not the case, then you have not correctly defined your IK group/element. Revert the temporary changes, then set the IK group into explicit handling mode. Make sure the IK element is still active (Element is active).

    In the motion planning dialog, set the IK group you just created as the Associated IK group item. Watch out for warnings in red, in that dialog section.


    Selecting appropriate collision entities

    When motion planning needs to take into account potential collisions between the manipulator and the environment, or potential manipulator self-collisions, then you can specify collision entities:

  • self-collisions: define a collection that contains the collidable links of the manipulator. In the Self-collision check section of the motion planning dialog, specify twice that same collection: every time a path node is calculated, each collidable object of that collection will be tested against all other collidable objects of the same collection. If you do not wish consecutive links to be tested for collision, then you should give them a collection self-collision indicator that differs by exactly 1. Refer to the object common properties.
  • manipulator-environment collisions: In the Robot-obstacles collision check section of the motion planning dialog, you can specify the entities that should not collide. One would usually create a collection that contains the collidable links of the manipulator, and specify it as the robot-entity.
  • Collision detection will be performed only at the calculated path nodes, and what can happen is that the manipulator seems to jump over an obstacle:

    [Manipulator jumping over the obstacle]


    To avoid above problem, one can reduce the distance D (in configuration space) used in the simFindMpPath API function, or use a minimum distance calculation (with an appropriate threshold) instead of a simple collision detection.

    It is important that your manipulator collision entities are correctly defined, otherwise your manipulator could be in a state of constant collision, and no motion planning path will be calculated. For verification, you can always temporarily specify the same collision entities (or minimum distance entities) in the collision detection dialog or minimum distance calculation dialog.

    [Ill-defined self-collision: slight intersections between consecutive links (shown in green) put the manipulator in a state of constant collision]


    You should also be aware that if your manipulator grasps an object that was initially an obstacle (e.g. a cup), then the manipulator will be in a state of constant collision with that object, and any further motion planning calculation will fail. At that point you have two options:

  • you can temporarily make that object non-collidable (or non-measurable). It will then not be detected as an obstacle anymore, however, it will neither be seen as part of the manipulator (i.e. a motion planning task will not take into account possible collisions between the object and the environment).
  • you can also make that object temporarily part of the manipulator. This way, a motion planning task will take into account possible collisions between the object and the environment too. To do this, best is to define a manipulator collection as the tree including all objects of the manipulator starting at its base, then by attaching the object to the manipulator base (i.e. making the manipulator base parent of the object), the object will also be part of the manipulator collection.


  • Adjusting various parameters

    Following additional parameters can also be adjusted, in order to optimize the created motion planning object:

  • the individual joint subdivisions: the joint subdivisions define how many precalculated path nodes (phase 1 nodes) will be generates. The more phase 1 nodes, the slower the initial node generation will be, but the better (or more varied) the found trajectories will be.
  • the configuration space metric: this metric defines how distances are calculated in the configuration space of the manipulator, in order to identify a configuration node that is closest to what we are looking for. The metric corresponds to assigning a weight to each joint in the manipulator. Larger values will count for larger distances in the configuration space: if the first joint of a manipulator rotates by 1 degree, the end-effector displacement will usually be larger than if the last joint of the same manipulator rotates by the same amount. To take this into account, one would normally give larger weights to the base joints.
  • the Cartesian space metric: this metric defines how a distance is calculated in the Cartesian space, in order to identify an end-effector pose that is closest to what we are looking for.


  • Executing motion planning commands

    Once your motion planning object is fully defined, you can use it via appropriate API function calls.

    Following pseudo-code represents the logical sequence, but can be very slow (remember that the path simplification usually takes the longest to compute):

    1. Starting with a manipulator start configuration, we want the end-effector to
       move without collisions to a specific goal pose.
    
    2. Search for a goal configuration that matches the goal pose (simGetConfigForTipPose).
    
    3. Search for a path between the start and goal configurations (simFindMpPath).
    
    4. Simplify the found path (simSimplifyMpPath).
    
    5. Repeat above steps 2-4 n times.
    
    6. Select the shortest calculated path.

    Following pseudo-code represents the practical sequence, most often used:

    1. Starting with a manipulator start configuration, we want the end-effector to
       move without collisions to a specific goal pose.
    
    2. Search for a goal configuration that matches the goal pose (simGetConfigForTipPose).
    
    3. Search for a path between the start and goal configurations (simFindMpPath).
    
    4. Repeat above steps 2 & 3 n times.
    
    5. Select the shortest calculated path, and simplify it (simSimplifyMpPath).

    For a practical exemple, refer to the child scripts attached to objects Mico and Jaco, in the demo scene motionPlanningAndGraspingDemo.ttt.


    Recommended topics

  • Related API functions
  • Motion planning module
  • Motion planning dialog