In this lesson, we will learn how to use the PPL framework to create motion planning methods. Planning methods are implemented as MPStrategy objects, which call on other abstractions such as LocalPlanners and DistanceMetrics to perform various functions. Here we will start with a blank strategy called MyStrategy and fill in the content for a simple planner that creates four configurations connected in series and outputs both a .map file and the Euclidean length of the chain.

1. Set up MyStrategy in PPL

   The first step is to put the blank strategy into PPL and set it up to compile and run.

   1. Download MyStrategy.h and place it in your src/MPLibrary/MPStrategies directory.
   2. We want to be able to test our method as we go, so it's best to set up the compilation support right away. Go to src/Traits/ and open the file CfgTraits.h. This file defines the list of things that exist in PPL. We need to include our header file with the other MPStrategies. Use your editor's find command (for vim, type /MPStrategies) to locate the MPStrategies includes. Add a line for your file in alphabetical order:

      #include "MPLibrary/MPStrategies/MyStrategy.h"

   3. Now scroll down and look at the MPTraits struct. Find the list of strategies and add a line for MyStrategy:

      MyStrategy<MPTraits>,

      Now your file has been included in PPL and will be compiled with everything else, so go ahead and save and exit.
   4. Now, recompile PPL to incorporate MyStrategy. You should not have any compilation errors; if you did get some, look at the error message to find the file and line number with the error. Check that line to make sure that there are no typos or other simple mistakes. Be sure to follow the same formatting as you see in the file. Try to recompile; if you still get errors, ask for help.
   5. Next, we need to tell PPL to use MyStrategy. PPL uses an XML file as input to set all sorts of variables like which Environment file to use, which MPStrategy to map it with, which Sampler the MPStrategy should use, etc. This information is recorded in XML nodes, which look like this:

      <nodetag variable=stuff/> --or-- <nodetag> stuff </nodetag>

      Go to src/Examples and open the file CfgExamples.xml. This file includes an example node for most of the things in PPL. Look around to see how this is organized. Now go to the bottom of the file (type GG in vim) and add a node for MyStrategy above the closing tag of MPStrategies like so:

      <MyStrategy label="MyStrategy"/>

      This sets up MyStrategy as an MPStrategy in this XML file. Note that it is not necessary to recompile PPL after editing the XML file.
   6. CfgExamples includes nodes for many MPStrategies, but only one will be executed when we run the program. To set MyStrategy as the method to use, find the XML node called Solver - it should be the last node in the file. In the Solver node, change the label mpStrategyLabel to mpStrategyLabel="MyStrategy".
   7. Save and quit, and get ready for testing!
2. Test MyStrategy

   Thus far, we have added the file MyStrategy.h, included it in MPTraits, and created an XML node in order to run our method. We can now test our (blank) MyStrategy to see that PPL can execute it. Go to ppl/ and execute PPL using the file Examples/CfgExamples.xml with the following command:

   ./ppl_mp -f Examples/CfgExamples.xml

   You should see the message MPLibrary is solving with MPStrategyMethod labeled MyStrategy using seed 12345678. In this case, MyStrategy is a blank method and does nothing, so PPL essentially loads the files and then terminates. This is good news though, because we now have a blank, testable method that is ready to build on.

3. A look at MPStrategy

   Before we start adding things to MyStrategy, lets take a look at how it works.

   1. Open the file again in a text editor. The first thing to note is that MyStrategy is templated off of MPTraits. This is true for almost everything in PPL because MPTraits defines what objects are included in our world.
   2. Also notice that MyStrategy inherits MPStrategyMethod, which is the base class for all MPStrategies. This common base class enables PPL to interact with all MPStrategies through the same interface, provided by MPStrategyMethod.
   3. Next you will see several typdefs, which are included for convenience and code readability. Then we have two constructors, one each for XML and non-XML intialization, followed by MyStrategy's versions of the MPStrategyMethod functions ParseXML, Print, Initialize, Run, and Finalize.
   4. These MPStrategyMethod functions define the interface for MPStrategies. They need to be implemented for each MPStrategy so that PPL knows what to do with it:
      • Print is used by other methods that need to get the name and label information of MyStrategy.
      • ParseXML is intended to be used in conjunction with the XML constructor in order to extract parameter information from an XML node.
      • The functions Initialize, Run, and Finalize are called when PPL executes MyStrategy, in that order. This is where the planning will happen: we use Initialize to set up any supporting variables or objects, Run to execute the planning methods, and Finalize to export output files, such as maps and/or stats.
4. Set up your Infrastructure

   Now that we know roughly how an MPStrategy works, lets make a simple planner that creates four nodes (Cfgs) connected in series and outputs the Euclidean distance of the resulting path. Such a path for a real system would represent four configurations that the robot could reach in succession. To do this, we will use the following objects:

   • CfgType - to create the nodes
   • ValidityChecker - to check that the nodes are not in collision
   • DistanceMetric - to calculate the Euclidean distance
   • LocalPlanner - to check the connections between nodes
   • GraphType - to store the resulting Roadmap so we can check it in Vizmo later
   1. If we start typing a huge block of code in the Run method, our code will be very hard to read and debug, especially for other people. Since all code is shared in our lab, we need to write our methods so that other people can understand and maintain them. Therefore, we will organize our work into two auxiliary methods called helper functions to make reading and debugging easier:
      • GenerateNode - returns a valid CfgType
      • ConnectNode - tries to connect a valid CfgType to the end of the chain, returns true or false to indicate success or failure
   2. Lets begin by creating the declarations and definitions for these functions. Helper functions are only for internal use, so we want to declare them as protected. Find the declaration of Finalize in the class definition. Skip a line and add protected just like you see done at the top with public. Under protected, declare the helper functions:

      CfgType GenerateNode();
      bool ConnectNode(CfgType& _c);

   3. Now go to the bottom of the file and add empty definitions for each:

      template<class MPTraits>
      typename MPTraits::CfgType
      MyStrategy<MPTraits>::
      GenerateNode() {
       
      }

      The typename business with CfgType is needed because the typedef in the class definition is not in this scope. Write the ConnectNode definition similarly for its parameters (no typename... required).
   4. Next, we need to create member variables to store the labels of the ValidityChecker, DistanceMetric, and LocalPlanner we want MyStrategy to use. Go back to the class definition and add three private member variables to store these:

      string m_vcLabel;
      string m_dmLabel;
      string m_lpLabel;

   5. To populate those variables, we will need to read in data from the XML file. The purpose in doing things this way is that changing which ValidityChecker or which DistanceMetric you use is reduced to a one-word substitution in the XML file. To see how to set it up, look at ParseXML in src/MPLibrary/MPStrategies/VisibilityBasedPRM.h:
      • The entries there for m_vcLabel and m_lpLabel are the same as what we need for MyStrategy.
      • The four parameters for the string parser are: XML label, required, default, type. Compare what you see here to the VisibilityBasedPRM node in CfgExamples.xml to see what this is reading.
      • Duplicate the parsing for m_vcLabel and m_lpLabel in MyStrategy, and create the same for m_dmLabel based on those examples.
      • Change the default parameter for m_vcLabel from "" to "pqp_solid", and make m_dmLabel's default "euclidean". Now set the required parameter to false for all three of the m_ labels so that we can use the default values.
      • Finally, we need to tell the XML constructor to use this information. At the end of the XML constructor, add a call to our parsing method with

        ParseXML(_node);

      • The variables we created will now be read from the XML file if the corresponding XML label is present in the MyStrategy node. If it is not, the default value will be used. If you wanted to change the DistanceMetric to manhattan later on, all you would need to do is add dmLabel="manhattan" to the MyStrategy XML node.
5. Write GenerateNode

   Now that we have inputs configured and access to some abstractions, we can write the content for GenerateNode. This method will make a single node using the Cfg class (which is typedefed here as CfgType) and collision check it with the ValidityChecker stored in MPProblem (remember that MPProblem is the container that holds everything for this problem). We can get access to these objects through the MPBaseObject (from which MPStrategyMethod inherits) by using the this pointer.

   1. In the body of GenerateNode, get a pointer to the ValidityChecker and Environment, and create a new CfgType:

      auto vc = this->GetValidityChecker(m_vcLabel);
      auto env = this->GetEnvironment();
      CfgType newCfg(this->GetTask()->GetRobot());

   2. Now we want to generate a random CfgType until we find one that is inside the bounding box and not in collision. Use a basic loop to randomly generate a new Cfg until a good one is found:

      do {
        newCfg.GetRandomCfg(this->GetEnvironment()->GetBoundary());
      } while(!newCfg.InBounds(env) || !vc->IsValid(newCfg, this->GetNameAndLabel()));
      return newCfg;

      Take a close look at the while condition. At a high level, it basically says that if the node is either out of bounds or invalid, then resample the node.
   3. GenerateNode is now a simple, one-node sampler that we can use inside Run to keep things orderly and readable. Before moving on, add some comments to explain what you are doing so that others can work with your code.
6. Test your work

   With GenerateNode finished, now is a good time to check that your code compiles. Before we do, we need to give ConnectNode a return value. Just set it to return false so that we can test what we have done so far. Also, you might want to add a cout statement to Initialize so that you can see that your method is working:

   cout << "Some cheesy but safe for work message :)" << endl;

   Then recompile your code. Handling errors in small batches will help you improve debugging your time and maintain your sanity. Fix any compile errors before moving on. You can also test that it still executes if you want to see your message.

7. Write ConnectNode

   Now we can write the other core method, ConnectNode. We want this method to test if the input node can be connected to the last node in the chain. If it can, we want to add the node and corresponding edges to the Roadmap and return true. If not, it needs to ignore the node and return false. In order to do this, we will need to keep track of the last node that was added to the Roadmap. We will also need a way to store the total length of the chain.

   1. Make private member variables in the class definition: CfgType m_lastNode to hold a copy of the last node in the map, and double m_length to hold the total chain length. We also want to initialize those variables when the method is constructed. Include a default initialization for m_lastNode and m_length in both constructors:

      MyStrategy() : m_lastNode(), m_length(0.) {

      and

      MyStrategy(XMLNode& _node)
      : MPStrategyMethod(_node), m_lastNode(),
      m_length(0.) {

   2. In Initialize(), initialize m_lastNode to an empty CfgType object:

      m_lastNode = CfgType(this->GetTask()->GetRobot());

   3. Now go to ConnectNode. First we need to setup pointers to access the Environment, the Graph, the DistanceMetric, and the LocalPlanner. Go through the get-chain to acquire these:

      auto lp = this-> GetLocalPlanner(m_lpLabel);
      auto dm = this-> GetDistanceMetric(m_dmLabel);
      auto r = this->GetRoadmap();
      auto env = this->GetEnvironment();

      Again, remember that MPLibrary stores all of this stuff: we are going through our lowest-level base class (MPBaseObject) to access it.
   4. Now we want to test if this is the first node: if it is, it should be added to the Roadmap right away (it was already determined valid in GenerateNode, and there's nothing for it to connect to). Afterward, we need to update the information for the last node in the chain:

      CfgType blank(this->GetTask()->GetRobot());
      if(m_lastNode == blank) {
         r->AddVertex(_c);
         m_lastNode = _c;
         return true;
      }

      The if line checks if m_lastNode still holds the default value, which is a configuration with a 0 value for all degrees of freedom. If it has something other than the default value, then some other node has already been added to the chain.

      Note: This test could fail if the previous CfgType was sampled smack on the default values, but this is extremely unlikely in our case as we are working in a 6-DOF environment where each coordinate is represented as a double. Since the probability of sampling all six coordinates at exactly 0 is extremely small, we will ignore the issue in this tutorial.

   5. For subsequent nodes, we need to discern whether this node can be connected to m_lastNode. To do this, we need a few temporary variables:

      LPOutput<MPTraits> lpOutput;
      auto robot = this->GetTask()->GetRobot();
      CfgType collisionCfg(robot);

      These are needed by the LocalPlanner to store the output edge and the Cfg where collision occured in the event of failure.
   6. Now we can test the connection with the LocalPlanner, and add the new node and edge tp the Roadmap if it succeeds. We can also use this opportunity to add the distance between the nodes to the running total:

      if(lp->IsConnected(m_lastNode, _c, collisionCfg, &lpOutput,
      env->GetPositionRes(), env->GetOrientationRes())) {
         m_length += dm->Distance(m_lastNode, _c);
         VID newNode = r->AddVertex(_c);
         r->AddEdge(r->GetVID(m_lastNode), newNode, lpOutput.m_edge);
         m_lastNode = _c;
         return true;
      }

   7. The LocalPlanner call takes quite a few variables. Here is a short run-down:
      • The first two Cfgs are the Cfgs that lp is trying to connect.
      • The next, collisionCfg, is used to store the first configuration along the local path that is found to be in collision; we don't need this for our method, but IsConnected requires it, so we just use a dummy Cfg as a place holder.
      • The next parameter is an LPOutput object, which stores the calculated local path. We use this to add successful edges to the graph.
      • Finally, we have the position and orientation resolutions, which let the local planner know what step size to use when checking the connection.
   8. Assuming all goes well, _c is reachable from m_lastNode, and IsConnected returns true. We then enter into the if block, add the distance to the total, and add the new node and edge to our Roadmap.
   9. If ConnectNode hasn't returned after the if block, then _c is not the first node, and that it can't be connected to m_lastNode. In this case, we do nothing with _c and return false:

      return false;

   10. Great, now ConnectNode is complete! Take a moment to comment your code. Good comments here would describe each of the three cases and possible returns in 1-2 lines per case.
8. Test your work

   Once your code is commented, test that it compiles and fix any errors. We are now finished with the algorithmic methods, so all that remains is to call those methods, do some book keeping, and make output files!

9. Use GenerateNode and ConnectNode to write Run

   Lets put GenerateNode and ConnectNode together now to complete our algorithm.

   1. Go to the Run function and set up access to the RoadmapType pointer, just as you did in ConnectNode. Name the pointer r again for consistency.
   2. Next, let's create a temporary int variable to set the number of nodes we want:

      size_t numNodes = 4;

   3. Now we want to use our helper functions to continuously generate a new node and try to connect it to the chain until we have four nodes in our map. A while loop provides us with a simple and elegant means to express this:

      while(r->get_num_vertices() < numNodes) {
         CfgType newCfg = GenerateNode();
         ConnectNode(newCfg);
      }

   4. That's it: our helper functions have encapsulated all of the messy details into single-statement ideas, which makes our Run function clean and easy to understand. If someone else read our code, it would only take a few seconds for them to figure out the basic structure of our method. Add some brief comments if you think it would help readability.
10. Add Output Generation

    We're in the home stretch now: all that remains is to produce an output file for our Roadmap and to print the chain length.

    1. Since we want to produce output after Run completes, lets head to the Finalize method. Start by outputting the path length with a simple cout statement:

       cout << "MyStrategy Path Length: " << m_length << endl;

    2. Now lets handle the .map file. We always start with the base filename, which is owned by the base class MPStrategyMethod, and we export .map files as "filename.map":

       string fileName = this->GetBaseFilename() + ".map";

    3. Now we will call on the Roadmap class to write its data to complete the Finalize method:

       this->GetRoadmap()->Write(fileName, this->GetEnvironment());

    4. Hurrah! MyStrategy will now print the chain length and produce a .map file that we can look at in Vizmo to see our results. Before we recompile and test our work, we have one last task to perform: all classes in PPL are required to define their Print function to return their name and label. Go to the Print method and fill in the bare-bones minimum Print function:

       _os << this->GetNameAndLabel() << endl;

    5. The end is near. Recompile and debug your code, then you are ready to try it out!
11. Run MyStrategy
    1. It's finally time to see MyStrategy in action. Go to ppl/ and run PPL using Examples/CfgExamples.xml as before:

       ./ppl_mp -f Examples/CfgExamples.xml

    2. You should see some output messages, including your cout line in Initialize (if you made one) and the "MyStrategy Path Length" as created in Finalize. Type ls to verify that you have a .map file in this directory (it should be example.12345678.query.map).
       Note: The "example" part comes from the XML label baseFilename in the Solver node in CfgExamples.xml, while the 12345678 part is generated from the seed label in the same node.
    3. Now we can use Vizmo to see the fruits of our labor. From src/Examples, type vizmo++ to open Vizmo. Click open file and select your .map file.
    4. Once the files are loaded, use Show/Hide Roadmap on the Roadmap tool tab to see the Roadmap we made. You can also select Robot mode to view the actual robot positions instead of box-points. You should now see four valid configurations connected in series. If you see something else or an error occurs, double check for typos and simple mistakes before asking for help.

    Once you get the expected output, you are finished! Congratulations on completing your first MPStrategy!

12. Self-check

    Before you move on, make sure that you understand the main points of this lesson:

    1. MPStrategies define a way to execute PPL. During runtime, PPL will call Initialize, Run, and Finalize once each, in that order.
    2. MPStrategies use objects from MPLibrary like LocalPlanners and ValidityCheckers, and access those objects through the GetObject functions.
    3. Wrap up computational ideas like GenerateNode and ConnectNode in helper functions to make code easier to read and debug.
    4. To create a new MPStrategy, we need to make a class file, include the header and class in MPTraits, and create an XML node for the strategy.
    5. To run an MPStrategy, we set the Solver node in the XML file to refer to our desired strategy.
    6. Write small segments of code and then test them right away. This style of iterative development improves debugging speed and readability. It also makes it easier for others to help you when you get stuck.

    You can learn more about the individual components such as LocalPlanners through the PPL documentation, which is available under docs, and through looking at examples of their use in other code. For example, Connectors use LocalPlanners to connect nodes, so you might find an example of how to use a LocalPlanner in a Connector class. Keep both in mind as you will likely need both resources to find the functions and examples you require.