How to deploy code to a physical quadruped robot

[xin111222]

Hello,think you for your work. Now I want to deploy it on a physical robot, and I want to know how to modify it？ Can you give me some advise?

[MihirDharmadhikari]

Hey @xin111222 ,

If you want to use the planner for exploration, you don't need to modify it. However, you will need to set up a few things to be able to run on your robot:

1. In your launch file of your software stack you will need to add the gbplanner and pci_general nodes as shown in the demo launch file
2. Provide the correct inputs to the planner:
   • Point cloud with the tf from the sensor frame to the fixed frame. Topic to be set here. This tf is usually provided by your localization solution. Hence, the robot needs to have an odometry/SLAM solution.
   • Odometry. Topic to be set here.
3. The planner (the pci_general node to be precise) will publish the path on the topic command/trajectory, which is remapped here, which is of the message type trajectory_msgs/MultiDOFJointTrajectory. You will need a path follower that will subscribe to this and follow the path.

I hope this helps. Feel free to let me know if you have any more questions.

Best, Mihir

[xin111222]

Thank you for your reply. I have another question. Does your code include Traversability‐aware for quadruped robots?

[MihirDharmadhikari]

Hi @xin111222,

We do include a basic level of traversability check on the volumetric map. The planner projects the sampled graph down on the volumetric map and checks if there exists mapped ground under each vertex and edge. Subsequently, it interpolates each edge and projects the interpolated points down on the map to check if the inclination of each segment in the projected edge is within a limit.

Let me know if this helps.

Best, Mihir

[xin111222]

I would like to ask if you have attempted to incorporate the cost of accessibility into the planner?

[MihirDharmadhikari]

Hi @xin111222,

Currently, we have not incorporated any other traversability cost than what I mentioned above.

Best, Mihir

[xin111222]

Hello,I am trying modify launch file.But i have some questions,as you say ,i need add two nodes in my launch,Is there anything else that is not needed？for example: "All settings" and "Config files "

[MihirDharmadhikari]

Hi @xin111222,

As our research is not on trajectory tracking for quadrupeds, we don't have that. However, there are some open-sourced local planners (for example this) and trajectory trackers that you can refer to.

As for the trajectory_msgs/MultiDOFJointTrajectory message type, this is not very hard to convert to your desired data type. The documentation for it explains how it is structured: http://docs.ros.org/en/noetic/api/trajectory_msgs/html/msg/MultiDOFJointTrajectory.html If you don't want to do that we also provide two other options for subscribing to the path.

1. If you have set the PCI in the kTopic mode here, it will publish the path as a geometry_msgs/PoseArray as well on the topic pci_command_path.
2. If you have set the PCI in kAction mode (as done in the ground robot example), you can create a ROS action server in your path tracker similar to what we provide in the demo path tracker.

Let me know if this helps.

Best, Mihir

[xin111222]

So What is the difference between the command/trajectory and pci_command_path.

[MihirDharmadhikari]

It is the same path published in two message types for convenience.

[xin111222]

The paper mentions the use of elevation maps for perform traversability analysis for quadruped. Is this part also included in the code? I don't seem to have found it. Another question, how can i choose octomap？Which is the better for low performance platform

[MihirDharmadhikari]

If you are referring to the CERBERUS Field Robotics paper, the ANYmal robot uses a local planner from the Robotics Systems Lab (open-sourced here) for quadrupeds that uses an elevation map. This is not part of the GBPlanner software.

As for the question of Octomap vs Voxblox, we have found that voxblox runs faster, has a constant voxel query time, and provides TSDF/ESDF which are useful for planning. Hence, we prefer voxblox.

[xin111222]

Forgive me for bothering you again. I tried to run planner by play bag data, but it encountered the following effor.

[MihirDharmadhikari]

Hi @xin111222 ,

The error saying "Input point cloud queue getting too long ... " is not a problem. This happens when the point cloud starts getting too big for voxblox to process. You can ignore this error. If you see the error printing all the time throughout the bag, then you might want to downsample your point cloud before feeding it to the planner.

Always happy to help.

Best, Mihir

[xin111222]

Ok,I have already solved the problem by downsampling, but it still doesn't seem to work.

[MihirDharmadhikari]

Can you send the launch file you are using?

[xin111222]

here

[MihirDharmadhikari]

Since you are playing a rosbag you should set the rosparameter use_sim_time to True by adding the line <param name="use_sim_time" value="true"/> to the launch file and playing your bag with the argument --clock. Let me know if this works.

[xin111222]

Thank you,i find it. By the way,can i deploy it on ubuntu16? And how can I minimize performance consumption as much as possible?

[xin111222]

Hi,i wonder why you set this parameter to infinity，this will consume a lot of performance.

[MihirDharmadhikari]

Hi @xin111222 ,

The full stack (including simulations and demo) is tested on Ubuntu 18 and 20 but the planner will work on Ubuntu 16 as well.

Regarding your third question about the voxblox parameter. Since we wanted to maintain a whole map of the environment, we leaver that parameter to default which is infinity. This will not cost additional computational cost but definitely increase the memory footprint as now it has to store the whole map.

Finally, for your question regarding reducing the computation cost you can do the following changes (some of them might be there in the config files by default):

• Increase the voxel size here.
• Sparsify the ray casting resolution by setting these to a higher value (upto rad(15.0*pi.180) should be ok for open environments).
• Increase the clustering radius for volumetric gain calculation here.
• The two parameters leafs_only_for_volumetric_gain and cluster_vertices_for_gain should be set to true. They are set to True by default in the rmf_obelix config file but not for the smb so you might want to check which one you are using as they make a big impact.
• You can set this parameter to a higher value (DON'T go above 4.0). This parameter skips voxels during raycasting at further distances for volumetric gain calculations.

Please note that all these changes will reduce the computational load but at the cost of making the exploration performance of the planner slightly worse. Hence, if you are using the config files provided for the smb (these ones: files) I would advise that you first check these parameters against the corresponding files for rmf_obelix (here) as they are more optimized for low computational cost. If you want to reduce the computation cost further, apply the changes I suggested in the order I mention, one by one until you reach your desired cost. DON'T apply all as there is a trade-off between computation cost vs exploration performance.

Let me know if this helps.

Best, Mihir

[xin111222]

hello, i have depoly it .But i encountered a problem. It Stop planning at a certain location (should be the endpoint of the planned path).The terminal information is as follows.

[MihirDharmadhikari]

Hi @xin111222 ,

Sorry for the late reply. Does this happen at one specific location or in general every time you plan? It seems like this is an error from the adaptive bounding box calculation where it is receiving a pointcloud with less than 3 points so the PCA is failing. I would check the following:

• Is the voxblox map being populated properly?
• Does the planner work fine if you disable the adaptive bounding box calculation? For this, you need to set this parameter to kBasicExploration.

Let me know this.

Best, Mihir

[xin111222]

Yes,it is OK.When I change places, it can work. But there is an important issue that when I test in the corridor, it often collides with the door on the side of the corridor.

[MihirDharmadhikari]

When it collides with the door do you see the path visualized in rviz colliding with the map built? I am trying to understand if it is the planner's issue or the controller's.

[xin111222]

hi, I have encountered a collision situation. Below is a screenshot of rviz.

[MihirDharmadhikari]

Hi @xin111222 ,

Sorry for the late reply. Can you visualize the path in the previous iteration as well? Here it is showing the planning iteration after the collision. Also, you can turn off the other visualizations in the PlannerViz group in rviz, just keep the vis/ref_path.

Best, Mihir

[xin111222]

hello @MihirDharmadhikari I seem to have discovered the problem why robot hit walls : there is a significant difference between the actual pose of the robot and the pose in the planner.I want to know if the problem lies here.

[AngelRodriguez8008]

Hello @MihirDharmadhikari

Thanks for Sharing your work and valuable Feedbacks.

I am working on my Master's Thesis with Boston Dynamics' Spot, focusing on Autonomous Exploration in multi-level environments. After reviewing your project, I see that it is theoretically sound. Congratulations on your results in the SubT competition. However, after several weeks of attempting to configure and execute the planner on Spot, despite reviewing the documentation, papers, and related issues, and trying various configurations and simulations, I have not achieved satisfactory results.

1. The system is completely unstable. Voxels appear randomly and persist indefinitely, regardless of the robot's movements.

2. However, Spot's odometry is very accurate; the robot's boding box moves erratically up and down, keeping it locked on the way.

3. The planner do not complete the Path's generation, even after a long time. The planner stops frequently and requires manual restarting.

4. Despite setting pci::trigger_mode = kAuto, the planner needs multiple start attempts. If the robot moves a few steps, it stops again and requires manual intervention, leading to repeated issues without proper exploration.

These are not path follower issues (I implemented an ActionClient); it worked properly during initialization and always when some small paths were provided.

I would greatly appreciate any specific guidance or suggestions you may have to resolve these issues.

Best regards. Angel A.

Thanks! If you prefer, I can create a new Issue

Same moment, Point Cloud & Voxel

[MihirDharmadhikari]

Hi @AngelRodriguez8008,

Looking at the point cloud it seems like there are some points very close to the robot that might be due to the lidar rays hitting the robot body. This could be creating some voxels at the robot location. As there are voxels at the robot location, the planner moves the robot bounding box up when trying to plan a path and might fail. Can you try to filter out the points within a certain radius of the robot before feeding it to the planner? Also, make sure that the point cloud given to the planner is in the sensor frame and not the inertial frame (world / map / odom).

Additionally, I see that there are some walls on the right side of the robot in the point cloud but they are not there in the voxblox map. Have you reduced the max range of voxblox?

Best, Mihir

[AngelRodriguez8008]

Hi @MihirDharmadhikari,

Thank you for your prompt response. I tried your suggestions.

Looking at the point cloud, it seems like there are some points very close to the robot that might be due to the lidar rays hitting the robot body. This could be creating some voxels at the robot location. As there are voxels at the robot location, the planner moves the robot bounding box up when trying to plan a path and might fail. Can you try to filter out the points within a certain radius of the robot before feeding it to the planner?

Yes, I did this. However, the problem persists with "phantom" voxels that were present before the robot arrived or the planner started.

Also, make sure that the point cloud given to the planner is in the sensor frame and not the inertial frame (world / map / odom).

I fixed this, now they are in the "body" frame instead of "world". This looks much better! I generate the point cloud be fusing Deep Cameras’ & Lidar VLP16’s points. Is this approach correct, or is it better to define both sensors separately? Can the "world" frame be used by setting use_tf_transforms: false; voxels disappear when I did this.

Additionally, I see that there are some walls on the right side of the robot in the point cloud but they are not there in the voxblox map. Have you reduced the max range of voxblox?

Yes, I set max_ray_length_m to 5. I have now changed it to 75.0.

I still have the issue where the planner unintentionally stops. Is there a parameter to increase the number of retries?

Additionally, I need to redo the Path follower. Spot is not moving smoothly. Adjusting the RobotDynamics: v_max: 6.0 v_init_max: 3.0 v_homing_max: 6.0 yaw_rate_max: rad(pi/2) dt: 0.3 The robot does not move, and the message "Goal arrived" is shown.

I tried removing the Path interpolation & I got a time-out in some cases. I was also trying to change the edge_length_min: 1.0 edge_length_max: 5.0 edge_overshoot: 1.0 #m nearest_range: 2.5 nearest_range_min: 2.0 # >= edge_length_min nearest_range_max: 4.0 # <= edge_length_ma If I don't care about moving backward or forwards, Should I empty the no_gain_zones_list.

Do you have any experience with Stairs? When Spot downstairs, he need to do it backwards. Is there a callback or mechanism to turn the robot depending on the slop or any related semantic?

Please let me know if you have any solutions to these issues. I will continue to update you on my progress.

Thank you very much. Best regards Angel A.

[AngelRodriguez8008]

Hi @MihirDharmadhikari,

I would appreciate your help to fix the following two issues I have:

1. Path Follower and Planner Trigger:

   • I am publishing all required feedback to the PCI Action Server, including remaining time, distance, and point count. However, despite configuring planner_trigger_lead_time and v_max in pci_config.yaml, the triggerPlanner method doesn't seem to be called as expected. The planner stops and requires manual restarting after each path is completed.
   • I managed a workaround by simplifying PlannerControlInterface::run(), which works partially.

     void PCIGeneral::executionTimerCallback(const ros::TimerEvent& event) {
     if (pci_status_ == PCIStatus::kRunning) {
     double kGoalThres = 0.5;
     double remaining_dist = getEndPointDistanceAlongPath(executing_path_);
     if (planner_trigger_lead_time_ > 0.0) {
     if (remaining_dist / v_max_ <= planner_trigger_lead_time_) {
     triggerPlanner();
     }
     } else {
     if (remaining_dist <= kGoalThres) {
     triggerPlanner();
     }
     }
     }
     }

     Could the issue be due to a low rate?

2. Chaotic Navigation:

   • The planner does not navigate properly, moving erratically between rooms and ignoring exits, even when doors are wide open. After a long time, it eventually exits, but follows the same inefficient logic, barely making progress. Stairs are also ignored. Could this behavior be related to the fact that we have a lot of clear glass windows, and the Planner identifies the exterior as a potential gain area?

Do you have any suggestions to fix these issues?

Thank you!