Libsarathi usage guide

Author: Sagar Behere behere@kth.se

This document introduces some of the essentials of using libsarathi in your programs. The API documentation can be found here. It is suggested that in the first reading of this document, you skim quickly through the sections and focus on the typical usage scenario. Read the code mentioned in that section. This will give you an intuitive understanding of how to use the libsarathi API and then you can go through the detailed documentation as needed.

Of course, you need to get and install libsarathi. In addition, you also need the gSOAP C++ Webservices toolkit. Currently, a standalone install for libsarathi is not available. You need to install the complete Sarathi package, as described at Installing Sarathi. This document assumes that all software is properly installed and compiler paths are appropriately set.

Libsarathi is a C++ library to communicate with the Sarathi server. By using libsarathi, the user needn't understand the details of using SOAP XML and gets to use a simple API for robot motion control.

A typical usage of libsarath is is to create a server object, initialize it by telling it the hostname/ip-address and port number that the Sarathi server is running on and then executing functions to get/set the robot position, plan paths et cetera.

All sarathi functions are non-blocking. This means, that if you send a command to the Sarathi server, the libsarathi function will return immediately. You must then query the Sarathi server to check the status of the command and whether it is successfully executed or not. Malformed commands, however, will immediately return an error.

Libsarathi functions throw an exception object if an error occurs. Details on error handling in section (sec:Error-handling).

A special note when using configuration space. Libsarathi uses joint angle values which are normalized i.e the values MUST lie in [0,1] where 0 represents the minimum joint angle position and 1 represents the maximum joint angle possible. 0.5 will then be the middle of the span.

Include the following headers in your program:

#include <libsarathi.hpp>

Programs using libsarathi must link to the following libraries:

sarathi hyperpoint gsoap++

Libsarathi internally depends on a library called hyperpoint, which provides two custom datatype classes: hyperpoint and hyperarray. Some functions in the libsarathi API accept these data types as arguments. The API documentation for libhyperpoint can be found here. There are historic reasons for the use of hyperpoint and hyperarray instead of std::vector. We will not get into the reasons here :)

Think of a hyperpoint as a std::vector<float>. It is basically an array of floats. The size (dimension) of the array is defined when the hyperpoint is created and it cannot be changed without destroying its contents. Hyperpoints can be used to store a point in configuration or cartesian space. For example, a point in configuration space of a 4 DOF robot is represented using a hyperpoint of dimension 4 to store the 4 floats that contain the joint values. A point in cartesian space can be represented by a hyperpoint of dimension 6 to store the x-, y-, z- positions and 3 rotations. A hyperpoint is created as:

hyperpoint p1;    //creates a hyperpoint of size 0. It can't be used yet.
hyperpoint p2(6); //creates a hyperpoint of size 6. It can be used.
p1.initialize(4); //initializes p1 to dimension 4. Now it can be used.
 
p2.reset();       //reduces p2 to size 0. Now it can't be used.
p2.initialize(5); //initializes p2 to dimension 5. Now it can be used again.

A hyperpoint constructed without an integer constructor argument must always use the initialize(int size) method to initialize the hyperpoint before it is used. Read the hyperpoint documentation to understand what else can be done with a hyperpoint.

A hyperarray is an array of hyperpoints. The hyperpoints in a hyperarray need not be all of the same dimensions. However, in most usage scenarios, they are of the same dimensions. A hyperarray can be used, for example, to store the path the robot moves along. A path is represented by an ordered set of hyperpoints. Each hyperpoint typically represents a point in space for the robot and is therefore of the same size as all other points. A hyperarray is created as:

hyperarray a1;       // array of dimension 0. It can't be used yet.
hyperarray a2(2);    // array of 2 hyperpoints.
 
a2[0].initialize(2); // initialize the first hyperpoint of a2 to dimension 2
a2[0][1] = 2.2;      // set the second float in the first hyperpoint to value 2.2

After a hyperarray is created, each hyperpoint inside it must be initialized before the array as a whole can be used. Not doing so will lead to memory violations and segmentation faults.

In case of error, functions in the libsarathi API throw an exception object of type libsarathi::sException, derived from std::runtime_error. Unhandled exceptions lead to program termination, therefore libsarathi functions must always be enclosed in try{} catch{} blocks. The following code demonstrates how to catch an exception:

hyperpoint currpos;
try{
     libsarathi::get_position("cartesian", &currpos);
}
catch (libsarathi::sException e){
     std::cerr << e.what() << std::endl;
     std::cerr << e.detail() << std::endl;
     std::cerr << (strerror(e.errorcode())) << std::endl;
}

The what() method of the exception object returns a (const char *) string describing what went wrong. The detail() method returns a (const char *) string giving more details of what went wrong. The hint() method provides a (const char *) string giving a hint of what to do to fix the error. The hint might not always have meaningful information. In addition to these methods, the exception object also provides the errorcode() method returning an int which is set to a standard error code value which can be checked against the values provided by <errno.h> The strings are intended for humans, while the error code can be used by a program to recover from errors. For example:

if (e.errorcode() == EBUSY){
    // The Sarathi server was busy. Wait and resend the request.
}

A typical usage scenario is to move the robot from its current position to some other position in cartesian space. This activity can be broken down into the following steps:

1. Get the current position in the spacetype desired. In this case, it is cartesian
2. Create a hyperarray whose first hyperpoint is the current position, and last hyperpoint is the desired position. If there are any other positions which must be reached along the way, put them in order after the first hyperpoint.
3. Set the scene file
4. Ask the server to plan a path along the hyperarray
5. Check if a path could be successfully planned. If yes,
6. Issue a move command to move along the path
7. Check if the motion has been completed.

The complete code can be found in the file sarathi/tests/testlibsarathi.cpp. For convenience, it is reproduced here.

#include "../src/libsarathi/libsarathi.hpp"
#include <iostream>
#include <string>
#include <unistd.h>
 
int main (int argc, char **argv)
{
        libsarathi::serverProxy sarathiProxy;
 
        sarathiProxy.initialize("127.0.0.1", 18000);
 
        //set the planner scene file
        try{
                sarathiProxy.set_planner_file("kuka_hand.iv");
        }
        catch (libsarathi::sException e){
                std::cerr << e.what() << std::endl;
                std::cerr << e.detail() << std::endl;
                std::cerr << strerror(e.errorcode()) << std::endl;
                return -1;
        }
 
        hyperpoint currentPosition;
        hyperpoint targetPosition;
        hyperpoint intermediatePosition;
 
       //A good way to set target position is to read the current position
       //and change only the values that need to be changed
        try{
                sarathiProxy.get_position("cartesian", &targetPosition);
        }
        catch (libsarathi::sException e){
                std::cerr << e.what() << std::endl;
                std::cerr << e.detail() << std::endl;
                std::cerr << strerror(e.errorcode()) << std::endl;
                return -1;
        }
 
        //set desired target position
        targetPosition[0] = 443.841; targetPosition[1] = -476.206; targetPosition[2] = 381.195;
        // targetPosition[3] = 2.53276; targetPosition[4] = 0.074721; targetPosition[5] = -3.00883;
 
        //set some intermediate position we want to visit
        intermediatePosition = targetPosition; //this copies values of all the fields, then we'll change the first 6
        intermediatePosition[0] = 0.0342; intermediatePosition[1] = 536.552; intermediatePosition[2] = 733.458;
        intermediatePosition[3] = 2.68; intermediatePosition[4] = -0.58409; intermediatePosition[5] = 1.46275;        
 
        //get the current position
        try{
                sarathiProxy.get_position("cartesian", &currentPosition);
        }
        catch (libsarathi::sException e){
                std::cerr << e.what() << std::endl;
                std::cerr << e.detail() << std::endl;
                std::cerr << strerror(e.errorcode()) << std::endl;
                return -1;
        }
 
        std::cout << "\nCurrent position is :\n" << std::endl;
        currentPosition.print(std::cout);
 
        hyperarray keypoints(3);
        keypoints[0] = currentPosition;
        keypoints[1] = intermediatePosition;
        keypoints[2] = targetPosition;
 
        try{
                sarathiProxy.plan_along("cartesian", &keypoints);
        }
        catch (libsarathi::sException e){
                std::cerr << e.what() << std::endl;
                std::cerr << e.detail() << std::endl;
                std::cerr << strerror(e.errorcode()) << std::endl;
                return -1;
        }
 
        std::string planningStatus;
        bool planningDone = false;
        bool planningResult = false;
 
        while ( ! planningDone){
                try{
                        sarathiProxy.get_planning_status(planningStatus, planningDone, planningResult);
                }
                catch (libsarathi::sException e){
                        std::cerr << e.what() << std::endl;
                        std::cerr << e.detail() << std::endl;
                        std::cerr << strerror(e.errorcode()) << std::endl;
                        return -1;
                }
                sleep(1);
        }
 
        if (! planningResult){
                std::cout << "\nPlanning failed!" << std::endl;
                return 0;
        }
 
        //at this point, planning is done and a valid path was found
        // if necessary, the path can be obtained with
        //hyperarray path;
        //sarathiProxy.getPath("configuration", &path);
        //we can now issue a move command
 
        try{
                sarathiProxy.move();
        }
        catch (libsarathi::sException e){
                std::cerr << e.what() << std::endl;
                std::cerr << e.detail() << std::endl;
                std::cerr << strerror(e.errorcode()) << std::endl;
                return -1;
        }
        std::string motionStatus;
        bool motionDone = false;
        bool motionResult = false;
        while ( ! motionDone){
                try{
                        sarathiProxy.get_motion_status(motionStatus, motionDone, motionResult);
                }
                catch (libsarathi::sException e){
                        std::cerr << e.what() << std::endl;
                        std::cerr << e.detail() << std::endl;
                        std::cerr << strerror(e.errorcode()) << std::endl;
                        return -1;
                }
                sleep(1);
        }
 
        if (! motionResult){
                std::cout << "\nMotion failed!" << std::endl;
        }
        else {
                std::cout << "\nMotion complete!" << std::endl;
        }
        //get the current position
        currentPosition.reset();
        try{
                sarathiProxy.get_position("cartesian", &currentPosition);
        }
        catch (libsarathi::sException e){
                std::cerr << e.what() << std::endl;
                std::cerr << e.detail() << std::endl;
                std::cerr << strerror(e.errorcode()) << std::endl;
                return -1;
        }
        std::cout << "\nCurrent position is :\n" << std::endl;
        currentPosition.print(std::cout);
 
        return 0;
}