Planning with legged robots

derektan95 commented 3 years ago

Good evening,

Thank you very much for your work. I've read both your papers and the youtube video detailing the entire exploration algorithm. It is stated that this autonomous exploration algorithm can also worked for legged robots, even though the demonstrations were mainly for quadcopters. I wonder if there are specific parameters for me to set if I were to want the RRT local planner to generate paths only tangential to the ground (which the legged robot can walk on), instead of paths that are midair that are aimed more for quadcopters? Thanks!

MihirDharmadhikari commented 3 years ago

Hi @derektan95, Thank you for your interest in our work. As you correctly observed the planner is capable of running on both aerial and legged robots. Currently, the planner plans paths in 2D for legged robot and you will need to have your path tracking controller to follow the path provided. There are a few changes that are needed in the config files for running on legged robot. You will need to change the following parameters:

The robot type here should be changed to kLeggedRobot
The z axis sampling parameters need to be changed to sample in 2D. Change this set of parameters to:
```
Z:
pdf_type:       kConst
sample_mode:    kManual
const_val:      0.3 # e.g. off from the ground to satisfy collision check for legged robot.
```
The collision checking happens for the body of the robot which is considered to be at a height above current robot z coordinate specified by the const_val

I hope this helps you in your work. Let me know how it goes and if you have any further doubts.

Thanks, Mihir

derektan95 commented 3 years ago

Hi Mihir,

Thank you very much for you detailed explanation; this should be enough for me to get started with my robotic dog. There are a few follow up questions:

You mentioned that this set of configuration will allow us to plan in 2d. Are you referring to 2d from a top-down view (planar to the ground), or 2d with respect to the tangent of the ground? In other words, will the nodes on the chosen path be defined by simply (x, y, theta), or will it still be in (x, y, z, r, p, y)? This will affect how the command pose I will issue to my robot.
After the local planner determined the best path based on highest exploration gain, and let's say a dynamic obstacle appears the chosen path while the robot is following the path, will the local planner replans a path around the obstacle? I don't remember seeing this feature both in the video as well as the paper.

Looking forward to hearing from you, thanks!

Stay safe, Derek

MihirDharmadhikari commented 3 years ago

Hey Derek,

The sampling happens in 2D but the path finally given by the robot will be in the same format as that of aerial robot, i.e. (x,y,z,quaternion). Also, as the sampling takes place inside a local window around the current position of the robot, the const_val is above current robot z coordinate. I made a mistake in the above message saying that it is value above ground, but it is above current robot z.
This is a good question. Currently, the planner is meant to serve as a higher level planner and does not handle dynamic obstacles, hence requiring a lower level obstacle checking mechanism. This also relates to the fact that gbplanner performs all the collision checking on the occupancy map built. This map is has a fairly low resolution (20 to 30cm voxel size) to keep it computationally tractable to run online. Hence, certain points in the path may pass through regions untraversable by the ground robot. So a traversability check also needs to run along with the obstacle checking if you plan to operate is extremely unstructured terrain.

Hope this helps.

Thanks, Mihir

derektan95 commented 3 years ago

Hi Mihir,

Thanks for the quick comments. Some thoughts here:

I took a deep dive into the codebase and realized what you meant. Am I right to say that the rrg data structure calls for the generate function, which samples the z coordinates as a const_val value above the robot's current z position? If that is the case, I can forsee this working well on a flat even ground. However, wouldn't there be issues if the robot is, for example, moving up a slope? Random points with z = current_pose.z + const_val will be generated, and it might fail the collision check esp for a sloped path in front of the robot (i.e. the path is not traversible)? This issue may also extend to staircases in urban settings, where points may not be sampled above its steps.
Great thing you pointed out. Perhaps it will be worthwhile for me to explore a sub-local planner that samples obstacles within a certain radius of the robot using point cloud that is not downsized to maintain high resolution. That will give a more accurate picture as to whether the robot can traverse through a certain path.

I would like to take this chance to clarify a few more concepts:

It says on the 2019 Graph-Based Path Planning for Autonomous Robotic Exploration in Subterranean Environments paper that your team published (Pg 4): In the second step, the global planner attempts to add other shortest paths from the local graph with high volumetric gain. I have tried looking for this in the best path selection algorithm. Is it done by the addFrontiers function?
Once the robot hits a deadend (i.e. no more solution found), the robot would use the global RRG and evaluate unexplored frontiers to decide where to explore next. Am I right to say that the addition of unexplored frontiers are handled by the expandGlobalGraphTimerCallback function, which updates the global RRG by converting unexplored high volumetric gain paths to frontiers to explore? From what I understand, this callback function is called on a separate thread after every defined time period. How does the robot rank the frontiers to explore next?

On a sidenote, I love how code in this repository is written. It's written in a manner that is very easy to understand! Thank you so much again, Mihir. Looking forward to hearing from you.

Take Care, Derek

MihirDharmadhikari commented 3 years ago

Hi Derek,

As you have correctly identified, there will be problems on steep slopes. However, as the robot box is check sufficiently higher than the current robot z (of course it is in the hands of the user to set the height), it will be able to handle small slopes. We had tested this on slopes that are not too steep and it works. But it is a known drawback of sampling in 2D that there is a limitation on sampling in terrain that is not flat and has steep inclinations. This is part of our future work and we are planning to work on the same.
Yes, as the low level planner will need only local yet dense map a sliding window map can be built for the same.
This step is handled in the addFrontiers function in rrg.cpp. Which is then called here.
The expandGlobalGraphTimerCallback function is called periodically as you mentioned. It re-evaluates the volumetric gain of each frontier vertex in the global graph and samples points near the frontiers to expand the global graph. The vertices to visit are sorted based on their volumetric gain.

I am glad to hear that you liked the package and the code structure. This has happened through the hard work of the main developer of the planner and this software stack @tungdanganh .

Feel free to reach out for any discussion related to the package.

Thanks, Mihir

derektan95 commented 3 years ago

Hi Mihir,

Sounds great. What are your thoughts on sampling points to form the local RRG as it is right now with a const_val. However, we will run another loop through all points in the local RRG to correct for their z coordinates. More specifically, we will sample points in the z coordinates, from some negative z threshold, to the robot z pose, up till a positive z threshold. Here, we reject all points below the ground, and accept the first point that is above the ground plane. Of course, we would need to firstly know where the ground point is in the octmap based on the X and y. This will ensure that all points are above the ground. Thereafter, we run another loop through all edges to eliminate all edges that are too steep (relative to ground plane based on where the robot is currently standing). This angle threshold would depend on the stability of the robot, and can be set in th config file. Will this be computationally feasible?

Really appreciate the work you are doing here. Many people can benefit from such an open source project. Thanks again!

-Derek

MihirDharmadhikari commented 3 years ago

Hi Derek,

Yes this is a possible approach to have the points on the ground but this will increase the number of collision checks on the map. How this is implemented will be critical in the increased computational cost.

-Mihir

derektan95 commented 3 years ago

Hi Mihir,

That sounds great. I guess that's all the questions I have for now. Good luck with your endeavours in academia! Thanks again.

-Derek

ntnu-arl / gbplanner_ros

Planning with legged robots #6