KineoWorks step-by-step #1: path planning

KineoWorks is a software component that automatically computes collision-free motion, solving complex path-planning problems in applications such as robotic collision-free trajectory optimization. In this series of articles, Etienne Ferré, Development Director of Kineo components, describes some of the key steps that are easily implemented to develop a sophisticated robotics simulation.

Part 1: Path planning

This article explains how to solve the simplest and most common path planning scenario: finding a collision-free path that goes from point A to point B. 

Solving this point-to-point scenario involves two steps which we will explore in further detail: 

• Preparing the input 
• Running the path planner 

Preparing the Input

In this step you will obtain the inputs for the path planner which are: 

• the device 
• the start configuration 
• the goal configuration 

The first required input is a device which can be any moving system. For example, a free-flyer such as a building-inspection drone, or an articulated system, such as an industrial robot. The device defines the kinematic system and all its constraints: collision avoidance, joint limits and any other user-defined constraint. You can use our Kwik application to graphically construct your devices quickly and easily. This example will discuss path planning for a robotic system.

Next, you need to supply a start configuration and a goal configuration, which will be used to create a direct path. In the example code below, the start and goal configurations for the robot system are created, together with a path that goes directly from the start to the goal. 

// Build start configuration 
// Start DOF values: [0, 0, 1.57, 0, 0, 0] (positioned at home position) 
m_startCfg = CkwsConfig::create(m_device);
m_startCfg->dofValue(2, 1.57); 
// Build goal configuration 
m_goalCfg = CkwsConfig::create(m_device); 
// Goal DOF values: [3.14, 0, 1.57, 0, 0, 0] (positioned at J1=pi) 
m_goalCfg->dofValue(0, 3.14);
m_goalCfg->dofValue(2, 1.57);
// Build direct path 
m_path = CkwsPath::createWithDirectPath(*m_startCfg, *m_goalCfg); 
  

Most path planners don’t cope very well with colliding start/goal configurations. Therefore, it’s good practice is to check configuration validity. Here is an example: 

// Check configuration validity 
if(m_startCfg->validate()) 
{ 
  // m_startCfg is valid 
} 

You can also check that the direct input path is colliding to confirm that the path planner will have some work to do: 

// Check path validity 
if(m_path->validate()) 
{ 
  // m_path is valid 
} 

Running the Path Planner 

Creating the Path Planner 

In order to find a collision-free path, you will need to create a path planner object. Here, an all-purpose path planner called a smart planner is used. Set dynamic penetration to zero in order to compute a path that will be completely collision-free.

// Create the Smart Planner for solving the path planning problem:
CkwsSmartPlannerShPtr smartPlanner = CkwsSmartPlanner::create(); 
smartPlanner->penetration(0); 

Blocking Path Planning

This is the easiest method for finding a path. However, the call is blocking, so if there is no solution, the call will never return. It is not recommended for production code; however, it is useful in the prototyping stage:

// Find the path
smartPlanner->plan(*m_startCfg, *m_goalCfg, m_path);

Non-Blocking Path Planning

The atomic API allows you to run the path planning loop in your own code, thus keeping control. First, call startPlan() in order to initialize the path planner. Then, each call to continuePlan() performs an incremental computation. Finally, call finishPlan() to end path planning. This is the recommended API when integrating path planning into an application in order to keep the application responsive.

bool mustContinue = true;
ktStatus status = KD_ERROR;
// Start planner (STABLE_ENDS means modifying start or goal is not allowed)
status = smartPlanner->startPlan(m_path, CkwsPathPlanner::STABLE_ENDS);
if (KD_OK == status)
{
  // Run the loop
  do
  {
    status = smartPlanner->continuePlan(m_path, mustContinue);
  }
  while (mustContinue);
}
// Finish path planning
smartPlanner->finishPlan(status, m_path);

Instead of implementing this using a loop construct, you can call continuePlan() from the main event loop of your UI toolkit using a timer or idle events. This allows you to keep the UI responsive without using threads. For implementing a Cancel button or a timeout, you can interrupt path planning just by calling finishPlan() instead of continuePlan(). During path planning, you can also retrieve information like completion (for displaying a progress bar) or current estimated difficulty.

Retrieving the Result

In both examples above, m_path initially contains the problem to solve and finally contains the computed path in cases of success. During path planning, when using the atomic API, calls to continuePlan() can also modify m_path in order to give intermediary results which will be colliding.

In part 2 of our series, we take a closer look at how you can build robots quickly and easily using our graphical Kwik application.