Solving planning problems in C++

This part of the tutorial assumes that you have completed the previous step. We will work off the same code and get on with solving some motion plans.

We will continue to use the exotica example found in manual.cpp as a reference, starting after we sent the problem to the solver:

...

// Continued from initialization
// Create the initial configuration
Eigen::VectorXd q = Eigen::VectorXd::Zero(any_problem->GetScene()->GetNumControlledJoints());
Eigen::MatrixXd solution;

ROS_INFO_STREAM("Calling solve() in an infinite loop");

double t = 0.0;
ros::Rate loop_rate(500.0);
ros::WallTime init_time = ros::WallTime::now();

while (ros::ok())
{
    ros::WallTime start_time = ros::WallTime::now();

    // Update the goal if necessary
    // e.g. figure eight
    t = ros::Duration((ros::WallTime::now() - init_time).toSec()).toSec();
    my_problem->y << 0.6,
                    -0.1 + sin(t * 2.0 * M_PI * 0.5) * 0.1,
                        0.5 + sin(t * M_PI * 0.5) * 0.2;

    // Solve the problem using the IK solver
    any_solver->Solve(solution);

    double time = ros::Duration((ros::WallTime::now() - start_time).toSec()).toSec();
    ROS_INFO_STREAM_THROTTLE(0.5, "Finished solving in "<<time<<"s. Solution ["<<solution<<"]");
    q = solution.row(solution.rows() - 1);

    my_problem->Update(q);
    my_problem->GetScene()->GetKinematicTree().PublishFrames();

    ros::spinOnce();
    loop_rate.sleep();
}

// All classes will be destroyed at this point.
...

CODE EXPLAINED

.._setting-start-position-cpp: Setting Start Position ======================

Firstly, we must set up a vector of initial joint variables to be fed into the solver. Here we initialize the joints to zero angles:

Eigen::VectorXd q = Eigen::VectorXd::Zero(any_problem->getScene()->getNumControlledJoints());

Using the robot description to determine the size of the vector (any_problem->getScene()->getNumControlledJoints()). This vector can naturally be changed to whatever joint configuration is required for your motion planning purposes. for example, after instantiating q:

q << joint_start[0],joint_start[1],joint_start[2],joint_start[3],joint_start[4],joint_start[5],joint_start[6];

and so on. This init joint configuration vector will be sent to the solver later.

Solution Container

When we do call the solver, we will get the motion plan back as an n*DOF matrix, where n is the number of steps along the trajectory. A dynamic matrix container will need to be created to hold this. This is created in the next line:

Eigen::MatrixXd solution;

Now we have a starting point and something to hold the trajectory, we need somewhere to go. It’s time to set the goal.

Goal Setting

Methods of goal setting vary according to the problem and your requirements. Two methods exist for the IK_solver. The method used in the tutorial specifies the goal for all IK_solver task maps by setting the y value to the desired goal:

my_problem->y << 0.6,  // X Position
                -0.1 + sin(t * 2.0 * M_PI * 0.5) * 0.1, // Y Position
                 0.5 + sin(t * M_PI * 0.5) * 0.2; // Z Position

NOTE: To set the goal for an individual map, use the SetGoal() function. This requires the name of the task map and a Eigen vector containing the Cartesian coordinates of the goal:

Eigen::VectorXd goal(3);

goal << 0.6,0.8,0.5;

my_problem->SetGoal("Position",goal);

Now the initial joint positions have been set, we have the solution container and have set the goal, we are ready to solve the problem using the any_solver container in which we stored the generic solver in earlier, passing the initial joint states q and the trajectory holder:

Solving Problems

any_solver->Solve(solution);

Solution Format

Now we have a solution to our problem. But what does it look like?

[ INFO] [1501240815.111167097]: Finished solving in 3.085e-05s. Solution [  -0.109557   -0.653855  -0.0687444     1.28515 1.06079e-17           0           0]

When using the IK_solver as in this tutorial and we set the MaxIterations to a low number, we get single step solution to the IK problem, as shown above - this is what you would expect to see if you run this code; it shows a vector of angles, one column for each joint in our robot. Each entry a joint configuration in radians, which will result in the end effector reaching the desired target. The rows of the output represent the positional steps each joint must pass through to reach the end effector goal. When using a higher MaxIterations setting, the number of rows in your motion plan would likely increase.

When using other problems or a different configuration of the UnconstrainedEndPoseProblem, trajectories will start to look a little more substantial. The output below shows the format of a solution after being solved by the OMPLSolver. Note that this solution was computed for a 6DOF robot and thus contains 6 columns. The first row represents the initial joint configuration, which here we set to zeros. The final row shows the configuration of the robot which allows the end-effector to reach the goal. The intermediate rows are the positional configurations that transfer the arm from start to end.

Solution:
   [       0            0            0            0              0     1.63042e-322
    0.134729   0.00623148  -0.00439002     -0.0770144   -6.41669e-18   1.63042e-322
    0.269458     0.012463  -0.00878005     -0.154029    -1.28334e-17   1.63042e-322
    0.404187    0.0186944   -0.0131701     -0.231043    -1.92501e-17   1.63042e-322
    0.538915    0.0249259   -0.0175601     -0.308058    -2.56668e-17   1.63042e-322
    0.673644    0.0311574   -0.0219501     -0.385072    -3.20835e-17   1.63042e-322
    0.808373    0.0373889   -0.0263401     -0.462086    -3.85001e-17   1.63042e-322
    0.943102    0.0436204   -0.0307302     -0.539101    -4.49168e-17   1.63042e-322
     1.07783    0.0498519   -0.0351202     -0.616115    -5.13335e-17   1.63042e-322
     1.21256    0.0560833   -0.0395102     -0.69313     -5.77502e-17   1.63042e-322
     1.34729    0.0623148   -0.0439002     -0.770144    -6.41669e-17   1.63042e-322
     1.48202    0.0685463   -0.0482903     -0.847158    -7.05836e-17   1.63042e-322
     1.61675    0.0747778   -0.0526803     -0.924173    -7.70003e-17   1.63042e-322
     1.75148    0.0810093   -0.0570703     -1.00119     -8.3417e-17    1.63042e-322
      1.8862    0.0872407   -0.0614603     -1.0782      -8.98337e-17   1.63042e-322
     2.02093    0.0934722   -0.0658504     -1.15522     -9.62504e-17   1.63042e-322
     2.15566    0.0997037   -0.0702404     -1.23223     -1.02667e-16   1.63042e-322
     2.29039     0.105935   -0.0746304     -1.30924     -1.09084e-16   1.63042e-322
     2.42512     0.112167   -0.0790204     -1.38626     -1.155e-16     1.63042e-322
     2.55985     0.118398   -0.0834105     -1.46327     -1.21917e-16   1.63042e-322
     2.69458      0.12463   -0.0878005     -1.54029     -1.28334e-16   1.63042e-322 ]

Publishing to RVIZ

We’ve set up or problem, solver and the rest and got our motion plan. EXOTica has the functionality to visualize this in RVIZ, so you can see your plan in action. The parts of the example code we are yet to mention deals with this and we’ll go through it now.

As we cycle through our motion plan, we can update the joint states:

q = solution.row(solution.rows() - 1);

and we send them to the problem:

my_problem->Update(q);

Now we can publish those frames to the /joint_states topic to be read by RVIZ:

my_problem->GetScene()->GetKinematicTree().PublishFrames();

RVIZ can either be set-up manually or via a roslaunch file.