Time taken for path planning is very long

[sandeepmattepu]

I have followed the documentation present in rl website. When I try to execute a path planning algorithm it is taking for ever to solve.

[rickertm]

Could you please provide some more info on the version of RL and the setup you're using? Is this with the current master branch or a release version? What system are you using (Linux x86, x64, ...), which compiler version, etc.?

Please provide the output when running the following commands in a terminal:

lsb_release -a
uname -a
gcc --version

[sandeepmattepu]

@rickertm Thanks for the reply. I have downloaded the source code from this https://www.roboticslibrary.org/download link. There is no git initialized in this source code. I guess the version is 0.7.0. My operating system is ubuntu 16.04 and x64. g++ version is 5.4.0, cmake version 3.5.1, gcc version 5.4.0

[rickertm]

Thank you for providing the additional information, this should indeed be version 0.7.0. The rlPrmDemo in this version requires the command line input to be in radians (the current master has already been updated to use degrees). Can you please try the following command instead?

./demos/rlPrmDemo/rlPrmDemo /usr/local/share/rl-0.7.0/examples/rlsg/unimation-puma560_boxes.convex.xml /usr/local/share/rl-0.7.0/examples/rlkin/unimation-puma560.xml 1.57 -3.14 1.57 0 0 0 1.57 -1.57 0 0 0 0

[sandeepmattepu]

@rickertm I have ran the command as you said and it still takes forever to solve. Other things I have tried 1) I have ran rlPlanDemo using ./demos/rlPlanDemo/rlPlanDemo /usr/local/share/rl-0.7.0/examples/rlplan/unimation-puma560_boxes_prm.xml and specified start and end angles as in the above command. It took less than a second to solve and it worked. 2) I have written debug statements to see what lines of code are being called in Main.cpp file of rlPlanDemo. I have also noted down how planner variable of type rl::plan::Planner is used and also it's dependent variables. 3) I have written temporary code where I have duplicated the logic from my notes which I have noted down in previous step. Unfortunately it has same problem as rlPrmDemo where it takes forever to solve.

If it is possible for you, could you please checkout 0.7.0 version and try to run the rlPrmDemo by yourself. We will see if the problem is caused only to me or if it is there in 0.7.0 version.

[rickertm]

I think I've found the issue: the scenario definition in rlplan/unimation-puma560_boxes_prm.xml sets a different world frame for the robot, with a rotation of 90° around the z axis:

<rotation>
    <x>0</x>
    <y>0</y>
    <z>90</z>
</rotation>
<translation>
    <x>0</x>
    <y>0</y>
    <z>0</z>
</translation>

You can replicate this in rlPrmDemo by either modifying the world frame accordingly in rlkin/unimation-puma560.xml or by changing rlPrmDemo similar to rlPrmTest, where the first command line parameters use X Y Z A B C to modify the world frame.

With this, the demo should run fine (tested on Ubuntu 16.04.6 x64 with gcc 5.4.0):

$ demos/rlPrmDemo/rlPrmDemo ../rl-0.7.0/examples/rlsg/unimation-puma560_boxes.convex.xml ../rl-0.7.0/examples/rlkin/unimation-puma560.xml 1.57 -3.14 1.57 0 0 0 1.57 -1.57 0 0 0 0
construct() ... 
construct() 170.281 ms
solve() ... 
solve() true 15.9445 ms

[sandeepmattepu]

@rickertm Yes, it works. I got stuck with new problem in path planning(reason for the late reply). I have followed the instructions described in the website to build kinematics and scene file for my robot. When I use rlPrmDemo to test my files, it takes forever to solve the path planning problem. I have added following lines of code to further debug my problem

std::cout << "Is model's starting pose valid : " << model.isValid(start) << std::endl;
std::cout << "Is model's ending pose valid : " << model.isValid(goal) << std::endl;
model.setPosition(start);
model.updateFrames();
std::cout << "Is model's starting pose in collision " << model.isColliding() << std::endl;
model.setPosition(goal);
model.updateFrames();
std::cout << "Is model's ending pose in collision " << model.isColliding() << std::endl;
model.setPosition(start);
model.updateFrames();
std::cout << "Is valid question : " << (planner.verify() ? "true" : "false") << std::endl;
std::cout << "solve() ... " << std::endl;;
bool solved = planner.solve();

It prints following output

Is model's starting pose valid : 1
Is model's ending pose valid : 1
Is model's starting pose in collision 1
Is model's ending pose in collision 1
Is valid question : false
solve() ... 

I have even used robot scene file(just robot in empty world) for testing. It prints exact same output as above. I have used rlCollisionDemo to see if any joints of the robot are in collision. It shows my joints are in collision even though there is clear gap between the joints The model I am using is a low poly version of actual model. Is there anything that I am missing to solve my problem?

[rickertm]

I'm glad to hear the original scenario works for you now.

Regarding the new scenario: please note that in order to optimize performance, a collision engine will create a convex hull for a VRML shape if its CONVEX property is set to TRUE (see the path planning scenario tutorial). While you can set this to FALSE to force a concave shape, I would recommend to disable self collision checking between connected links in the kinematics file

<body id="body1">
    <ignore idref="body0"/>
    <ignore idref="body2"/>
</body>

and between the first link and the environment (e.g., the floor it is mounted on)

<body id="body0">
    <ignore/>
    <ignore idref="body1"/>
</body>

[sandeepmattepu]

@rickertm Thanks a lot. You have solved my problem. Following are my suggestions in the documentation that you can make. 1) Include the purpose of ignore tag in "Create a Robot model". 2) In the API section under rl::plan subsection, the following code :

rl::plan::SimpleModel model;
if (rlkin) model.kin = kinematics; // for rl::kin kinematics
if (rlmdl) model.mdl = kinematics; // for rl::mdl kinematics
model.model = simpleScene->getModel(0); // ID of robot model
model.scene = simpleScene;

If we assign rl::mdl::kinematics it doesn't compile and complains that cannot convert rl::mdl::Kinematic to rl::mdl::Dynamic. Following code will make it possible to compile:

rl::mdl::Dynamic* dynamicDescription = dynamic_cast<rl::mdl::Dynamic*>(kinematicDescription.get());
dynamicDescription->world() = rl::math::AngleAxis(0 * rl::math::DEG2RAD, rl::math::Vector3::UnitZ()) * 
                              rl::math::AngleAxis(0 * rl::math::DEG2RAD, rl::math::Vector3::UnitY()) * 
                              rl::math::AngleAxis(0 * rl::math::DEG2RAD, rl::math::Vector3::UnitX());
dynamicDescription->world().translation().x() = 0;
dynamicDescription->world().translation().y() = 0;
dynamicDescription->world().translation().z() = 0;
model.mdl = dynamicDescription; // for rl::mdl kinematics