Trouble creating initial database

[AndyZe]

I'm having a bit of trouble running this for the first time. My two launch commands are:

roslaunch reachability reachability_analysis.launch

This loads everything robot-related (joints, moveit stuff, fake trajectory execution, etc). I'm pretty sure this is set up correctly because I can plan/execute with the MoveIt GUI. It also loads the mesh that I want to study (as part of the URDF) and creates the tf frame for the mesh (the tf frame is called reach_object).

The second launch command is:

roslaunch reach_core start.launch config_file:=rospack find reachability/config/reachability_params.yaml config_name:=reach_study

What's the significance of config_name? Right now it's arbitrary, it doesn't match anything else.

The stack trace at this second launch command is:

ros.moveit_reach_plugins: Successfully initialized MoveItIKSolver plugin
ros.moveit_reach_plugins: Successfully initialized MoveItReachDisplay plugin
ros.reach_core: ------------------------------
ros.reach_core: No reach study database loaded
ros.reach_core: ------------------------------
================================================================================
REQUIRED process [robot_reach_study_node-1] has died!

^I'm confused here because I'm trying to create the initial database.

My reachability_params.yaml config file is below. I'm sure I have the wrong file name or something.

`fixed_frame: "base_link" object_frame: "reach_object" results_directory: "$(find reachability)/results" # I created a new package to hold reachability stuff pcd_filename: "$(find reachability)/data/plane_pointcloud.pcd" get_avg_neighbor_count: false compare_dbs: [] visualize_results: true

optimization: radius: 0.2 max_steps: 10 step_improvement_threshold: 0.01

ik_solver_config: name: "moveit_reach_plugins/ik/MoveItIKSolver" distance_threshold: 0.0 planning_group: "left_arm" collision_mesh_filename: "package://reachability/data/plane.ply" collision_mesh_frame: "reach_object" touch_links: [] evaluation_plugin: name: "reach_core/plugins/MultiplicativeFactory" plugins:

• name: "moveit_reach_plugins/evaluation/ManipulabilityMoveIt" planning_group: "left_arm"
• name: "moveit_reach_plugins/evaluation/DistancePenaltyMoveIt" planning_group: "left_arm" distance_threshold: 0.025 exponent: 2 collision_mesh_filename: "package://reach_demo/config/part.ply" collision_mesh_frame: "reach_object" touch_links: []

display_config: name: "moveit_reach_plugins/display/MoveItReachDisplay" planning_group: "left_arm" collision_mesh_filename: "package://reachability/data/plane.ply" collision_mesh_frame: reach_object fixed_frame: "base_link" marker_scale: 0.05`

[AndyZe]

If I create an empty reach.db file in my results folder, it usually crashes but once got as far as 6% without crashing

[AndyZe]

Here's what gdb says. Crashing because x[i] doesn't exist, I suppose.

[AndyZe]

It looks like mersenne_twister is a random number generator. Do you think that's in the IK solver? Maybe I could switch to a different IK solver

[marip8]

roslaunch reachability reachability_analysis.launch

This loads everything robot-related (joints, moveit stuff, fake trajectory execution, etc). I'm pretty sure this is set up correctly because I can plan/execute with the MoveIt GUI. It also loads the mesh that I want to study (as part of the URDF) and creates the tf frame for the mesh (the tf frame is called reach_object).

This seems reasonable.

What's the significance of config_name? Right now it's arbitrary, it doesn't match anything else.

config_name is the name of the sub-folder (within the results_directory, specified in parameters YAML) in which to store the results of the reach study. It's a totally arbitrary name, but allows you to more easily run the same reach study with different parameters without overwriting the results.

The reach study first tries to load a database from this sub-folder. If it doesn't exist, it prints the message you showed ("No reach study database loaded") and starts running a reach study from scratch

ros.moveit_reach_plugins: Successfully initialized MoveItIKSolver plugin
ros.moveit_reach_plugins: Successfully initialized MoveItReachDisplay plugin
ros.reach_core: ------------------------------
ros.reach_core: No reach study database loaded
ros.reach_core: ------------------------------
================================================================================
REQUIRED process [robot_reach_study_node-1] has died!

^I'm confused here because I'm trying to create the initial database.

I can't say that I've seen this issue before. To test, I just removed the reach_study sub-folder from the demo package and was able to successfully re-generate the reach study results with no failure. Can you try to do the same thing and let me know if you encounter the same issue?

My reachability_params.yaml config file is below. I'm sure I have the wrong file name or something.

I don't see any specific issues in your parameters file that would cause the crash. I did notice that your collision_mesh_filename for the IK solver plugin is different than for the other plugins. Not sure if that was intentional or not.

collision_mesh_filename: "package://reach_demo/config/part.ply"

[marip8]

It's strange that the segfault comes from the random engine. It might have something to do with the IK solver you're using. It would be a good test to try a different solver. I've generally used TRAC-IK for previous projects and haven't experienced this issue. Also the demo is using the "default" MoveIt KDL kinematics plugin, and I haven't had issues running that

[marip8]

Another thing to try would be to set kinematics_solver_attempts equal to 1 in your MoveIt kinematics.yaml. My understanding is that if an IK solution cannot be obtained in the first solve, all subsequent attempts use a random joint pose as the IK seed. Setting this to 1 might eliminate some calls to the random number generator

[AndyZe]

I switched to this other IK solver (because it was convenient) and no issues yet (it's still running): moveit_reach_plugins/ik/DiscretizedMoveItIKSolver

Thanks for noticing my mesh name typo

[Levi-Armstrong]

Recommend switching out the boost generator for the one in std.

#if __cplusplus <= 201402
/** @brief Random number generator */
static std::mt19937 mersenne{ static_cast<std::mt19937::result_type>(std::time(nullptr)) };
#else
/** @brief Random number generator */
inline std::mt19937 mersenne{ static_cast<std::mt19937::result_type>(std::time(nullptr)) };
#endif

[marip8]

I switched to this other IK solver (because it was convenient) and no issues yet (it's still running): moveit_reach_plugins/ik/DiscretizedMoveItIKSolver

I'm surprised this didn't also fail for you. It inherits from MoveItIKSolver and calls the same functions that seem to be causing issues

FYI, the intent behind this plugin is that it solves IK for a specified number of poses that have been rotated around the Z-axis of the nominal target pose. This would allow someone to simulate a tool-Z free constraint and still use a closed-form IK solver (i.e. IKFast, OPW kinematics). This plugin will make the reach study take longer to finish than the regular solver plugin, especially if you're using a gradient based solver. Some gradient-based solvers like TRAC-IK let you specify relaxed orientation and position constraints within the solver itself. If you are looking to represent a tool Z-free constraint, this is definitely the better route to go.

[AndyZe]

Hmm. Well, I just double-checked. MoveItIKSolver crashes, DiscretizedMoveItIKSolver doesn't.

[marip8]

What solver are you using? I can try to replicate the failure locally

[AndyZe]

right_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 left_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005

[marip8]

Okay. That's what I used for the demo. Does the demo also fail for you if you delete the folder containing the results and re-run the study?

[AndyZe]

i'll try it tonight. In the middle of a run right now. Thanks!

[AndyZe]

The demo runs just fine. :+1:

Another strange thing about my particular robot is that Percent Reached and Total Points Reached and the other statistics are all zero, even with the DiscretizedMoveItIKSolver. I'll look into it a bit more.

[AndyZe]

Ah, I think it's because I scaled my mesh down by a factor of 1000. d'oh

[AndyZe]

Still having some issues but at least I see the normals on the mesh now. All unreachable.

Does the algorithm seek to align end-effector Z with the pointcloud normals? Or X?

[marip8]

The assumption is that the TCP Z-axis aligns with a surface normal whose direction has been reversed. Effectively the normals should point away from the surface (as they do for your mesh) and the robot TCP Z-axis should point towards the surface.

Also, the IK solver tries to put the robot TCP at the same position as the defined points, so you may need to adjust your TCP to have standoff from the geometry of your robot to avoid collision. You could also add the TCP (and/or other links) to the touch_links parameter in the configuration YAML to disable collision checking between them and the reach object.

These assumptions are probably not documented well enough, so I should at least add some clarification on the README about this

Here is the code that modifies the target positions if you need to look at it in more detail.

[AndyZe]

I finally found some reachable poses. Cool. Well I think this is a great package. I'll make a PR to suggest README improvements (won't be offended if you rewrite it heavily).