rlPlanDemo use .urdf model file

[george66s]

Hi, I am working on path planning section and trying to fig out how RL deals with this issue. It appears that rlPlanDemo uses a quite complicated .xml file that describes detail kinematics information of a robot and even defines the path planning algorithm within the .xml file. Do you have any demo that takes a .urdf robot model file and completes the path planning job? If not, and if the .urdf file format is to be loaded, how can path planning be completed?

Lots of thanks!
George

[rickertm]

The rlPrmDemo and rlRrtDemo applications are a good introduction into the path planning components of RL. They both allow you to use a URDF file for scene and kinematics definition and setup the planners without a rl::plan XML file, e.g., for a 2-DOF robot with 2 rotational joints, file 2dof.urdf, start = [0°, 0°], and goal = [45°, 90° ]

rlRrtDemo 2dof.urdf 2dof.urdf 0 0 45 90
rlPrmDemo 2dof.urdf 2dof.urdf 0 0 45 90

rlPlanDemo currently does not check for URDF files for scenes and kinematics, but I can add this.

[george66s]

Hi, I tried above demo. I modified my 7 joints robot to 2 joints robot by simply remove any link and join greater than 2. It loads the scene and model successfully and verified successfully as well. But it seems the solver can not finish its job. Below is the screen copy:

joint: 0
parent: base
child: arm1
origin.translation: 0 0 0
origin.rotation: 180 -180 180
type: revolute
max: inf
min: -inf
speed: 0
axis: 0 1 0
name: joarm1
joint: 1
parent: arm1
child: arm2
origin.translation: 0 0 0
origin.rotation: -6.63671e-13 2.77244e-13 2.37862e-14
type: revolute
max: inf
min: -inf
speed: 0
axis: -1 0 0
name: joarm2
root: base
start: 0 0
goal: 45 30
verify() ...
verify() true
solve() ...

The demo stopped there forever. I am trying to debug and will get back.

Lots of thanks!

[george66s]

The demo stopped at this following sentence:

bool solved = planner.solve();

Finally, after I waited long enough, Clion debug jumped to following place and marked as an error:

/usr/include/eigen3/Eigen/src/Core/Block.h

EIGEN_DEVICE_FUNC
inline BlockImpl_dense(XprType& xpr,
Index startRow, Index startCol,
Index blockRows, Index blockCols)
: Base(xpr.data()+xpr.innerStride()(XprTypeIsRowMajor?startCol:startRow) + xpr.outerStride()(XprTypeIsRowMajor?startRow:startCol), blockRows, blockCols),
m_xpr(xpr), m_startRow(startRow), m_startCol(startCol)
{
init();
}

The demo then exit with following messages:

Signal: SIGSEGV (Segmentation fault)

Process finished with exit code 1

Not sure what went wrong.

[george66s]

The rlPrmDemo works fine. It's the rlRrtDemo stops forever.

[george66s]

A further question. The rlPrmDemo took only 28 ms to complete the solve. Just wondering, what is the result from the solve. How can I watch the result of the solve.

Thanks!

[rickertm]

The rlRrtDemo issue is difficult to narrow down without additional information. Could you provide a backtrace of a debug build with this error and a link to your URDF model?

Regarding the result from a planning request: you can access the solution path of a successful query via Planner::getPath(). This gives you a list of vectors that represent the waypoints of the solution.

rl::plan::VectorList path = planner.getPath();

for (rl::plan::VectorList::iterator i = path.begin(); i != path.end(); ++i)
{
	std::cout << i->transpose() << std::endl;
}

[george66s]

Hi,

I tried above codes and got following in return:

start: 0 0
goal: 45 30
verify() ...
verify() true
construct() ...
construct() 2.30562 ms
solve() ...
solve() true 27.3159 ms
0 0
0.785398 0.523599

It printed tow points; 0 0 and 0.785398 0.523599, which is the radian expression of the start and the goal. After checking the documents, it seems to me that if I want a path with more points, I must setup something, like setDuration or something else to define the attributes of the path, but not sure what to do exactly. Could you please help on this issue? Thanks

[george66s]

Also, attached below is the full content of my 2dof.urdf

Since Github does not support uploading .xml file, the file name was changed to 2dof.txt. You may dowaload and rename it to 2dof.urdf and check out if you get the same result as mine, i.e. the Prm solve works fine but Rrt solve stops forever.

2dof.txt

Lots of thanks!

[rickertm]

Thank you for the URDF file of your robot. From a first look the kinematic model seems fine, in order to test the planning part however I would also require the STL files referenced in the model.

The planner will only provide a solution path with waypoints, not a full trajectory. You can generate a trajectory with these points using the interpolation functions in rl::math. If you prefer to do blending around the waypoints to avoid stopping and deviate from the path, this trajectory again would need to be tested for collision.

[george66s]

Attached please find the .stl files.
base.txt
arm1.txt
arm2.txt

After downloading, please simply rename them to base.stl, arm1.stl and arm2.stl respectively.

Further to your reply about the planner. You said that the planner could provide a solution path with waypoints. If I want, for example, 4 points in a path, not two(i.e. the start and the end), what should I do? Also, if I want to generate a direct line trajectory or a certain curve trajectory, what should I do?

Lots of thanks
George

[rickertm]

Thank you for providing the STL files. With this I could identify the issue: the URDF model does not define upper and lower limits for the joints. The UrdfFactory then defaults to +/- DBL_MAX for these values and the distance between these is equal to DBL_INF. As the URDF specification mentions zero as default values for these parameters, I modified the UrdfFactory accordingly. You can either define upper and lower limits for the joints in your model or you can use the URDF joint type="continuous" instead of type="revolute".

Regarding the number of waypoints: the planners in rl::plan will create collision-free paths for your query of start and goal configuration. For simply interpolating between different configurations without collision checks, you can use the interpolation functions in rl::math, e.g., rl::math::Spline::TrapezoidalAccelerationAtRest for a seven-segment trajectory between two configurations with limited velocity, acceleration, and jerk or rl::math::Spline::CubicFirst for a cubic spline between multiple configurations and specified waypoint times. rlAxisControllerDemo is a small example on how to use cubic and quintic polynomials to interpolate between two configurations and send these to a robot controller (by default, a running rlCoachKin or rlCoachMdl simulator). rlInterpolatorDemo includes examples of other interpolation functions.

[RodrigoFBernardo]

I was reading the topic, and you mentioned that we could configure the planner to get more points of a trajectory. But I didn't understand how we can get more points without using the functions in rl::math. Where collisions are not taken into account.