search

humanoid-path-planner / hpp_tutorial

Tutorial for humanoid path planner platform
BSD 2-Clause "Simplified" License
7 stars 12 forks source link

Questions about how to go beyond the tutorial #45

Closed DrawZeroPoint closed 2 years ago

DrawZeroPoint commented 2 years ago

Dear developers,

Regarding converting the tutorial to a real-world functional script for a tailored dual-arm mobile manipulation robot:

  1. I have successfully run the manipulation tutorial and also managed to build a similar script that I tested with no major flaw. However, in some situations where the scenario parameters are altered (maybe in an infeasible way but the consequence is hard to foresee), the program can stuck (able to reach solve but gives no result after hours) at: https://github.com/humanoid-path-planner/hpp_tutorial/blob/0e7416efc1e3911d83f0dc94839646f7d39d8850/script/tutorial_3.py#L112

    or

    https://github.com/humanoid-path-planner/hpp_tutorial/blob/0e7416efc1e3911d83f0dc94839646f7d39d8850/script/tutorial_3.py#L115

    I wonder what is the practical way to handle this. Should we set some parameters before calling solve (as the paper goes "After initializing the roadmap with the initial and goal configurations, the algorithm iteratively calls method ONESTEP until a solution path is found or the maximum number of iterations is reached"), or use an external timer to monitor the run time and cancel the run?

  2. When one arm performs the grasp, other parts of the robot, especially another arm, could wave around. The motion is acceptable since it is collision-free, yet it seems weird from an ordinary perspective. Could we circumvent this in some ways?

florent-lamiraux commented 2 years ago
  1. You can set a maximal number of iterations or a timeout with the following commands: 
    ps.setMaxIterPathPlanning
ps.setTimeOutPathPlanning
  2. It is usually desirable to run a path optimizer after random path planning. To get the list of path optimizers, you can type 
    ps.getAvailable('pathoptimizer')

    More generally, the command ps.getAvailable can be called with various keywords. To the the list of those keywords, type

    ps.getAvailable('type')

    To optimizer the manipulation path, you can try

    ps.loadPlugin('manipulation-spline-gradient-based.so')
ps.addPathOptimizer('SplineGradientBased_bezier1')
ps.optimizePath(pid)

    You can also try 'RandomShortcut' or 'Graph-RandomShortcut'. The latter methods pick random configurations on the initial path and try to connect them with the steering method. The former is described in this paper.

DrawZeroPoint commented 2 years ago

Thank you @florent-lamiraux, I have followed your suggestions on Q1 and the problem is solved.

After trying out the optimizers, I found that they can limit the detours of the robot body, which solved the issue in the original Q2.

Yet, extending Q2, I wonder if it is possible to completely constrain the robot's locomotion during certain planning? Do these optimizers can be tweaked to meet this requirement, or some other techniques within the HPP's capacity can be adopted?

The concern behind this question is that when working with a mobile manipulator with omni-wheels, if one naively applies the plan to the base, its inevitable slip on the ground can induce a huge error in the end-effector's pose, which could lead to a failed grasp.

The RandomShortcut optimizer provided in the paper is very impressive in stabilizing the robot base's locomotion and inspired me to ask this question. I noticed that it can do so "without any assumption about the group of DOF to activate (i.e. no DOF is locked)". So, the former question can also be put in this way: how could we lock the DoF of the robot's root joint for the plan?

florent-lamiraux commented 2 years ago

This is indeed a good practice to stop the base when moving the arms. To do so, you can have a look at the following script that plans motions for Tiago robot. After building the graph with the factory, the steps to follow are the following:

  1. Block the base along each edge of the graph:
  2. Create a state in which the arm is folded, and an edge from this state to itself
  3. connect this state to the graph:
DrawZeroPoint commented 2 years ago

The provided gold mine helped me a lot! By closely following L349 and L403, I was able to lock the base when performing grasping, and I will dig deeper into the rest parts in the future.

BTW, maybe not directly related to the former discussions but relevant to the title, I wonder what is the best way to find out the timing for physically close/open the gripper from the solved plan. With the PR2 tutorial, I noticed that the left gripper performing the grasp does not close in the whole process, and the box is just attached to the gripper magically. Besides, the API configAtParam somehow indicates the same thing as the visual cues, as the configurations for the gripper's joints are unchanged.

For now, I come up with some trivial ideas like constantly monitoring the box's configuration and regarding that the grasp happens/releases when the pose is changed. However, there must be better ways to do this, thanks in advance for any suggestions that lead me out.

florent-lamiraux commented 2 years ago

With the python API, it is not easy to finely manipulate paths. In our experiments, we use a dedicated project called agimus but this is not an easy thing to deploy.

You can try the following:

wp, times = ps.getWaypoints(pid)

where pid is the id of the path. You will get the list of waypoints the path goes through and the times (or parameter value). You can then run graph.getNode(wp[i]) to know in which state each waypoint lies. From that, you can deduce precisely when a grasp starts and ends.

DrawZeroPoint commented 2 years ago

Dear @florent-lamiraux,

Thank you so much for the instructions. I have implemented the gripper control using waypoints. When testing the code, regarding:

This is indeed a good practice to stop the base when moving the arms. ...

I realized and testified that if this constraint is added to the edge, then in subsequent planning it will still be preserved. I went through the python API trying to find a way to release (remove) the constraint but with no luck. Could you please provide some hints about removing the constraint on the go, such that the robot could move to new locations after a stationary grasping?

florent-lamiraux commented 2 years ago

It is not a good idea to add and remove constraints from an edge to obtain different behaviors. If you want to follow different behaviors, it is preferable to use several graphs.

DrawZeroPoint commented 2 years ago

@florent-lamiraux Using multiple graphs does solve my problem, thank you a lot!

By fixing the base during grasping, the final grasp accuracy is significantly improved (for now, I forwarded the plan from HPP to a MuJoCo-based simulator via ROS and I am working on directing that to a real robot) when a valid plan was found. However, a drop in the success rate of finding such a plan is also witnessed. To improve that, I tried to tweak setErrorThreshold's param, which can raise the success rate but has a negative impact on the accuracy. This contradiction makes setting the error threshold seems not a good idea.

To mitigate this, what I have tried are: 1) Altering the path planner from M-RRT to other candidates. 2) Enlarge the time-out for path planning (from 20s to 60s). Unfortunately, these methods did not help in my circumstance. I am willing to provide more details if they are needed.

FYI, among all the trials sharing the same initial and goal configurations, in a few cases, the valid plan was generated successfully even though the threshold is relatively small (1e-2) and the time cost is around ~10s, indicating that such a plan exists. I learned from the TRO paper that the planner starts from a random configuration, so the variance in success rate is reasonable to me.

To summarize, I wonder what could be the right way for increasing the planning success rate without a trade-off in accuracy, given that the fixing base trick is still preferred?

florent-lamiraux commented 2 years ago

Dear @DrawZeroPoint , The error threshold should be kept small and should not be changed for a better path planning efficiency. The large variance in the time of computation for a given problem is a well known drawback of random sampling methods. It is sometimes more efficient to set a relatively small timeout and to restart in case of failure. To better help you, I would need to know what you are exactly trying to do. Can you provide me a script ?

DrawZeroPoint commented 2 years ago

Dear @florent-lamiraux, Thank you for your generous support and invaluable time spent on this issue. Here is the minimal script simplified from my working code that may help identify the problem. Both links lead to the public available codebase that I am working with.

To run the minimal script, two asset repos are needed: clover_lab and curi_description, which are basically ROS descriptions for the environment and the robot, respectively.

The minimal script has been made self-contained such that only HPP and some pre-installed python packages are needed. For HPP installation, I followed the official guide and reorganized it like this. My working system is Ubuntu 20.04 with ROS noetic and python 3.8.

Regarding the problem itself, I surprisingly found out it could be caused by different configuration sets which numerically seem to have no big? difference from each other. Please note that by using the initial and goal configurations in

https://github.com/DrawZeroPoint/RoTools/blob/f6fc02a835591a67f81b634553e40f9ebbd97418/src/rotools/hpp/manipulation/minimal_test.py#L336-L345,

we can successfully plan the grasp in a limited time and with a low error threshold, yet alter to

https://github.com/DrawZeroPoint/RoTools/blob/f6fc02a835591a67f81b634553e40f9ebbd97418/src/rotools/hpp/manipulation/minimal_test.py#L348-L357

we cannot. The other parts of the script were not changed during the comparison. Both sets come from recording the corresponding ROS messages published by the MuJoCo simulator to the configurations and are hardcoded.

Some thoughts about the issue: 1) Is intrinsically not stable to perturbations also a character of sampling-based methods? 2) If that is the case, since both the sets pass the constraint test (valid from a prehensile manipulation perspective), maybe we need to check other properties beforehand?

florent-lamiraux commented 2 years ago

Dear @DrawZeroPoint In package curi_description, the directories robots and scripts are missing. I could not install the package.

florent-lamiraux commented 2 years ago

In curi_description, you also need to install directory srdf And in clover_lab, nothing is installed.

DrawZeroPoint commented 2 years ago

Dear @florent-lamiraux My bad. Just fixed the issue in both repos. Sorry for the inconvenience.

florent-lamiraux commented 2 years ago

Dear @DrawZeroPoint I have been able to run your script. The problem is that at lines https://github.com/DrawZeroPoint/RoTools/blob/f6fc02a835591a67f81b634553e40f9ebbd97418/src/rotools/hpp/manipulation/minimal_test.py#L348-L357, in the initial and goal configurations, the mobile base is not at the same place and the distance is above the numerical threshold. As the base is not allowed to move, the initial and goal configurations will never be connected together.

To better understand and diagnose possible issues, you may use some methods from ProblemSolver and ConstraintGraph python classes documented here: https://github.com/humanoid-path-planner/hpp-practicals/blob/master/instructions/exercise-2.asciidoc Others methods of class ConstraintGraph can be useful, for example methods starting with getConfigErrorFor. Type help(interface._constraint_graph) to see their documentations.

Finally, note that calling ProblemSolver.solve calls the selected path optimizers after solving the problem. No need to call ProblemSolver.optimizePath.

DrawZeroPoint commented 2 years ago

Dear @florent-lamiraux, Clear and informative as always! I truly appreciate your timely help!