Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

snp_automate_2022: generate motion plan failure #40

Open
mrunaljsarvaiya opened this issue Jul 10, 2023 · 11 comments
Open

snp_automate_2022: generate motion plan failure #40

mrunaljsarvaiya opened this issue Jul 10, 2023 · 11 comments

Comments

@mrunaljsarvaiya
Copy link

I'm running the stack on the foxy-desktop docker image

after launching snp_automate_2022 through ros2 launch snp_automate_2022 start.launch.xml, I went through the steps in the rviz plugin:

  1. get detailed scan
  2. generate tool path plan (I see the scanned object show up in rviz along with the rasterized markers)
  3. generate motion plan

(1) and (2) pass but I get a failure on (3), logs_A attached below. I haven't changed any of the mesh/tool planning settings through the other gui that opens up.

Next, I played around with the raster planner parameters and changed the following values

  • Line Spacing: 0.030 -> 0.02
  • Point Spacing: 0.015 -> 0.010
  • Min Hole Size: 0.1 -> 0.05

the motion plan status now says CONVERGED but there's still a failure at the end (logs_B)

logs_A

[snp_motion_planning_node-7] Optimization results:
[snp_motion_planning_node-7] status: PENALTY_ITERATION_LIMIT
[snp_motion_planning_node-7] cost values: (0.0243942, 0.0182881, 0.0223221, 0.0134271, 0, 0, 0, 0, 0, 0, 0.0456933, 0.64492, 11.8282, 0.000378859, 0, 50.6274, 160.889, 958.131)
[snp_motion_planning_node-7] constraint violations: (1.7055e-07, 2.45747e-07, 2.94625e-07, 2.12127e-07, 4.25655e-10, 3.74105e-08, 1.83988e-09, 3.88354e-07, 2.1322e-06, 2.26175e-06, 2.95489e-06, 3.41135e-06, 0.000338777, 1.94892e-05, 9.78981e-06, 6.72639e-06)
[snp_motion_planning_node-7] n func evals: 74
[snp_motion_planning_node-7] n qp solves: 76
[snp_motion_planning_node-7] ==================
[snp_motion_planning_node-7] Info:    TRAJOPT motion planning failed (Failed to find valid solution) for process input: Raster #1
[snp_motion_planning_node-7] Debug:   worker 1 finished running TRAJOPT
[snp_motion_planning_node-7] Debug:   worker 1 ready to run Error Callback
[snp_motion_planning_node-7] Debug:   worker 1 finished running Error Callback
[snp_motion_planning_node-7] Debug:   worker 1 finished running Raster #1: Raster #1
[snp_motion_planning_node-7] Debug:   worker 1 ready to run From Start: from_start
[snp_motion_planning_node-7] Debug:   worker 1 ready to run CHECK_TASK_INPUT
[snp_motion_planning_node-7] Debug:   worker 6 ready to run Transition #1: Transition #1
[snp_motion_planning_node-7] Debug:   worker 7 ready to run CHECK_TASK_INPUT
[snp_motion_planning_node-7] Debug:   worker 1 finished running CHECK_TASK_INPUT
[snp_motion_planning_node-7] Debug:   worker 1 ready to run HAS_SEED_CHECK
[snp_motion_planning_node-7] Debug:   worker 7 finished running CHECK_TASK_INPUT
[snp_motion_planning_node-7] Debug:   worker 7 ready to run HAS_SEED_CHECK
[snp_motion_planning_node-7] Debug:   worker 7 finished running HAS_SEED_CHECK
[snp_motion_planning_node-7] Debug:   worker 7 ready to run SEED_MIN_LENGTH_CHECK
[snp_motion_planning_node-7] Debug:   worker 1 finished running HAS_SEED_CHECK
[snp_motion_planning_node-7] Debug:   worker 7 finished running SEED_MIN_LENGTH_CHECK
[snp_motion_planning_node-7] Debug:   worker 7 ready to run Error Callback
[snp_motion_planning_node-7] Debug:   worker 1 ready to run SEED_MIN_LENGTH_CHECK
[snp_motion_planning_node-7] Debug:   worker 7 finished running Error Callback
[snp_motion_planning_node-7] Debug:   worker 6 finished running Transition #1: Transition #1
[snp_motion_planning_node-7] Debug:   worker 1 finished running SEED_MIN_LENGTH_CHECK
[snp_motion_planning_node-7] Debug:   worker 1 ready to run Error Callback
[snp_motion_planning_node-7] Debug:   worker 1 finished running Error Callback
[snp_motion_planning_node-7] Debug:   worker 1 finished running From Start: from_start
[snp_motion_planning_node-7] Error:   GraphTaskflow Failure: 
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   RasterGlobalTaskflow Failure: Raster #1
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   GraphTaskflow Failure: 
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   RasterGlobalTaskflow Failure: Transition #1
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   GraphTaskflow Failure: 
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   RasterGlobalTaskflow Failure: from_start
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] [INFO] [1689018977.779919614] [snp_planning_server]: Failed to create motion plan

logs_B

[snp_motion_planning_node-7] Optimization results:
[snp_motion_planning_node-7] status: CONVERGED
[snp_motion_planning_node-7] cost values: (0.173069, 0.15701, 0.130848, 0.0981664, 0.0759429, 0.0481714, 0.0152581, 0, 0, 0, 0, 0.0436864, 0.373675, 9.64582, 0, 0, 0, 40.9813, 159.249, 936.242)
[snp_motion_planning_node-7] constraint violations: (2.03267e-06, 1.75855e-06, 1.56929e-06, 1.01447e-06, 4.51107e-07, 5.40044e-08, 1.10441e-07, 3.6829e-07, 2.34141e-08, 4.90062e-07, 4.51434e-06, 5.31683e-06, 8.98953e-06, 1.89281e-05, 2.10981e-05, 1.36696e-05, 1.25661e-05, 1.47266e-05)
[snp_motion_planning_node-7] n func evals: 53
[snp_motion_planning_node-7] n qp solves: 53
[snp_motion_planning_node-7] ==================
[snp_motion_planning_node-7] Debug:   Motion Planner process succeeded
[snp_motion_planning_node-7] Debug:   worker 5 finished running TRAJOPT
[snp_motion_planning_node-7] Debug:   worker 5 ready to run DISCRETE_CONTACT_CHECK
[snp_motion_planning_node-7] Debug:   worker 5 finished running DISCRETE_CONTACT_CHECK
[snp_motion_planning_node-7] Debug:   worker 5 ready to run Error Callback
[snp_motion_planning_node-7] Debug:   worker 5 finished running Error Callback
[snp_motion_planning_node-7] Debug:   worker 5 finished running Raster #1: Raster #1
[snp_motion_planning_node-7] Debug:   worker 5 ready to run From Start: from_start
[snp_motion_planning_node-7] Debug:   worker 5 ready to run CHECK_TASK_INPUT
[snp_motion_planning_node-7] Debug:   worker 3 ready to run Transition #1: Transition #1
[snp_motion_planning_node-7] Debug:   worker 1 ready to run CHECK_TASK_INPUT
[snp_motion_planning_node-7] Debug:   worker 5 finished running CHECK_TASK_INPUT
[snp_motion_planning_node-7] Debug:   worker 5 ready to run HAS_SEED_CHECK
[snp_motion_planning_node-7] Debug:   worker 1 finished running CHECK_TASK_INPUT
[snp_motion_planning_node-7] Debug:   worker 1 ready to run HAS_SEED_CHECK
[snp_motion_planning_node-7] Debug:   worker 5 finished running HAS_SEED_CHECK
[snp_motion_planning_node-7] Debug:   worker 1 finished running HAS_SEED_CHECK
[snp_motion_planning_node-7] Debug:   worker 1 ready to run SEED_MIN_LENGTH_CHECK
[snp_motion_planning_node-7] Debug:   worker 1 finished running SEED_MIN_LENGTH_CHECK
[snp_motion_planning_node-7] Debug:   worker 1 ready to run Error Callback
[snp_motion_planning_node-7] Debug:   worker 1 finished running Error Callback
[snp_motion_planning_node-7] Debug:   worker 5 ready to run SEED_MIN_LENGTH_CHECK
[snp_motion_planning_node-7] Debug:   worker 3 finished running Transition #1: Transition #1
[snp_motion_planning_node-7] Debug:   worker 5 finished running SEED_MIN_LENGTH_CHECK
[snp_motion_planning_node-7] Debug:   worker 5 ready to run Error Callback
[snp_motion_planning_node-7] Debug:   worker 5 finished running Error Callback
[snp_motion_planning_node-7] Debug:   worker 5 finished running From Start: from_start
[snp_motion_planning_node-7] Error:   GraphTaskflow Failure: 
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   RasterGlobalTaskflow Failure: Raster #1
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   GraphTaskflow Failure: 
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   RasterGlobalTaskflow Failure: Transition #1
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   GraphTaskflow Failure: 
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] Error:   RasterGlobalTaskflow Failure: from_start
[snp_motion_planning_node-7]          at line 51 in /snpd_ws/tesseract_planning/tesseract_process_managers/src/core/utils.cpp
[snp_motion_planning_node-7] [INFO] [1689019305.250033936] [snp_planning_server]: Failed to create motion plan
@marrts
Copy link
Member

marrts commented Jul 10, 2023

Is this running on the master branch of this repo? I'd personally recommend running on my PR here as the debugging is greatly improved. If you're running on this branch already I'd recommend running it with verbose on to assist with debugging further: ros2 launch snp_automate_2022 start.launch.xml verbose:=true

One useful feature is that it generates a graph of the planning pipeline with the taken path highlighted. With the verbose tag set to true it'll automatically save this file to /tmp/ScanNPlanPipelineResults.dot. With a software like XDot installed you'll be able to see where exactly in the process things went wrong like the following:

Overview of Full Graph:

image

Top Level Pipeline:

image

Raster Taskflow Pipeline:

image

A Single Motion Plan Pipeline

image

From this I can see that the raster planning taskflow failed ultimately because the from_start motion failed. (I intentionally set it up so that the start position was in a self-collision which is a common problem encountered)

@mrunaljsarvaiya
Copy link
Author

Did you run into any issues with setting up the stack on the branch you linked? I'm running into an error with tesseract_geometry similar to the fcl post here, make[2]: *** No rule to make target 'octomap-NOTFOUND', needed by 'lib/libfcl.so.0.6.1'. Stop.

I fixed the fcl build issue above by following the post. I've also built the octomap package using the build instructions on their github page and set the correct OCTOMAP_DIR. However, ros can't seem to find the package (ros2 pkg prefix octomap returns empty). Installing octomap through apt install ros-foxy-octomap doesn't fix the issue either

@marip8
Copy link
Member

marip8 commented Jul 11, 2023

Have you tried installing liboctomap-dev?

@mrunaljsarvaiya
Copy link
Author

mrunaljsarvaiya commented Jul 11, 2023

Yup I did, I got the same error on tesseract_geometry and ros2 pkg prefix still returned empty

note that this didn't happen on the master branch, it occurs on #22

@marip8
Copy link
Member

marip8 commented Jul 11, 2023

make[2]: *** No rule to make target 'octomap-NOTFOUND', needed by 'lib/libfcl.so.0.6.1'. Stop.

The error you're reporting above is CMake not being able to find octomap, not ROS not being able to find octomap. The installation of liboctomap-dev should install the CMake package config file that let's CMake find octomap. Did you try removing the build directory after installing liboctomap-dev and then rebuilding? CMake caches a lot of things during a build, so a build may continue to fail if these build artifacts aren't cleared before trying again. FWIW, we got a successful build on Jammy with tesseract in CI here, so the build has worked on a ubuntu:jammy docker image

@mrunaljsarvaiya
Copy link
Author

Okay so I'm running on the master branch and was able to run the example demo in automate_2022.

I swapped out the default robot for our custom robot. After some debugging, looks like the motion plans are now failing due to IK failures. I updated the IKFast plugin configuration with the updated frame names, etc. We have the analytical solution for our robot's IK. Could someone point me to how I can create an IK plugin that allows me to use my analytical solution? Or alternatively is it better to simply add my IK solver directly into the descartes_robot_sampler's sample method replacing the call to manip_->calcInvKin?

@marrts
Copy link
Member

marrts commented Jul 19, 2023

This file shows making an IK plugin, in this case for IKfast. At the end of the file is where the plugin is added. Then you need to include it in your CMakeLists like here. And you reference it in the plugins yaml here

@mrunaljsarvaiya
Copy link
Author

mrunaljsarvaiya commented Jul 21, 2023

This file shows making an IK plugin, in this case for IKfast. At the end of the file is where the plugin is added. Then you need to include it in your CMakeLists like here. And you reference it in the plugins yaml here

Thank you! How can I go about editing the relative pose of the mesh object with respect to its reference frame? (I see that the marker is publisheed in open3d_sim_node.cpp, but I'm assuming there's purely visualization and there's some other code that stores its pose)

@marrts
Copy link
Member

marrts commented Jul 21, 2023

The pose is relative to the reference_frame parameter. If you want to keep the same reference frame but move the mesh you can use meshlab to modify the mesh directly.
image

@mrunaljsarvaiya
Copy link
Author

mrunaljsarvaiya commented Jul 25, 2023

Are there any explicit formatting options for the mesh file? I have a mesh file for a table in the correct format (.ply) but the tool can't seem to generate the raster points. I tried different outlines using the polygon selection tool. The error in the rviz panel is either "no points found within polygon boundary" or "no clusters found". The mesh file looks okay when opened in meshlab

I see this warning in the terminal:

[snp_tpp_app-6] Warning: Invalid frame ID "mobile_base_link" passed to canTransform argument target_frame - frame does not exist
[snp_tpp_app-6]          at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp

mobile_base_link is a valid frame in my system though (verified in the rviz tf tree) and I don't get this error when using the default part_scan.ply mesh file

Screenshot from 2023-07-24 17-50-16

@marip8
Copy link
Member

marip8 commented Jul 25, 2023

Are there any explicit formatting options for the mesh file? I have a mesh file for a table in the correct format (.ply) but the tool can't seem to generate the raster points.

Not particularly. I have noticed that .ply files exported from Solidworks sometimes can't be displayed (possible error in formatting from Windows to Linux). But if it shows up in Rviz, it loaded the mesh correctly.

The error in the rviz panel is either "no points found within polygon boundary" or "no clusters found".

This is the real error. One step in segmenting the region of interest submesh with the boundary is clustering the vertices inside of the boundary. There is a parameter that controls the minimum cluster size (100, by default; see image below). If there are not at least that many mesh vertices inside the selection boundary, then it won't identify the sub-mesh. I would try reducing that value and see if it generates tool paths

image

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants