Problems with Robot's Geometric Size and Collision Avoidance Failure

[ucas-zihaowang]

For the robot model double_integrator0, in the robot.cpp file at line 103: ` geom.emplace_back(new fcl::Spheref(0.15)); // Setting the radius to 0.15 In the visualization script visualize.py, at line 47: self.radius = 0.1 # Setting the radius to 0.1 ` (1) If I set the radius in visualize.py to 0.15, I find that the final solution fails to achieve collision avoidance. (2) If I set the radius in both robot.cpp and visualize.py to a larger value, the solution also fails to achieve collision avoidance.

My question is, can the geometric size of the robot be modified? Or what is the default size?

Thank you very much for your time and assistance. @akmaralAW

[akmaralAW]

Hello @ucas-zihaowang,

Yes, your understanding regarding the radius of the double integrator is correct. For the discrete search and optimization part we set it to 0.15. For the visualization it might be a typo, it also should be 0.15. That's the default value.

1) does the solution has collision with the environment, or inter-robot ? Is it discrete search solution or final optimized trajectory? 2) If you want to use another value for the radius, then make sure you changed them in robots.cpp file, and in joint_robots.hpp. Parameters from the last file are used for the joint optimization part.

if you want to share the visualization of the solution with collision, please feel free to email me: moldagalieva@tu-berlin.de

[ucas-zihaowang]

Thank you so much for your reply, I really appreciate it.

1. The solution results in collisions with the environment as well as between the robots. The discrete search results did not show collisions, collisions occurred after optimization.
2. I can find the location to modify parameters in robots.cpp, but can you help me find the location to modify parameters in joint_robot.cpp?
3. I have already sent the specific cases to your email. I directly downloaded the code from your GitHub repository, then added my test cases and modified line 47 in visualize.py:

   self.radius = 0.15 # default 0.1

   After running the program, I encountered this issue.

[ucas-zihaowang]

Hello, @akmaralAW. I may have resolved the issue.

1. The first step is to modify the robot's size in robots.cpp.
2. The second step, for double_integrator_0, you need to modify the robot kinematics configuration file double_integrator_0.yaml under the directory dynoplan/dynobench/models/ and change the corresponding radius to the desired value.
3. The third step is to modify the radius size in the visualization file visualize.py.

However, I discovered a bug. In dynoplan/dynobench/include/dynobench/integrator2_2d.hpp and dynoplan/dynobench/src/integrator2_2d.cpp, the Integrator2_2d_params constructor sets the default radius size to 0.1. But in the read_from_yaml() function, it does not update the radius size from the configuration file mentioned in step 2. Therefore, I added the following code in integrator2_2d.cpp within the read_from_yaml() function:

void Integrator2_2d_params::read_from_yaml(YAML::Node &node) {
    set_from_yaml(node, VAR_WITH_NAME(shape));
    set_from_yaml(node, VAR_WITH_NAME(dt));
    set_from_yaml(node, VAR_WITH_NAME(max_vel));
    set_from_yaml(node, VAR_WITH_NAME(max_acc));
    set_from_yaml(node, VAR_WITH_NAME(distance_weights));
    set_from_yaml(node, VAR_WITH_NAME(radius)); // add this line to update radius's value
}

Therefore, after fixing the bug, we only need to follow the first three steps to make the necessary modifications.

[akmaralAW]

Hello @ucas-zihaowang,

Correct, I just checked. If you are on the main branch, then adding set_from_yaml(node, VAR_WITH_NAME(radius)) should fic the problem.

I would highly recommend you to switch to benchmark_icra24 branch, which has been tested thoroughly.