search

SoMa-Project / ec_grasp_planner

Grasp Planner Based on Environmental Constraint Exploitation
BSD 3-Clause "New" or "Revised" License
7 stars 3 forks source link

Planner cleanup and recipe splitting per hand #47

Closed ptriantd-ocado closed 5 years ago

ptriantd-ocado commented 5 years ago

This PR is mainly restructuring the planner in the following ways:

  1. The final recipe is split into 3 parts: Grasping, Transport and Placement (Recipes for the WAM do not have a Placement part since the recipe in the master branch was finishing above the tote and the hand was opened manually)
  2. For the RBO hand there are separate Grasping recipes for the WAM and the IIWA so that if clauses and do-nothing controllers inside the recipe are avoided. For the rest of the hands, there is only one Grasping recipe.
  3. We have also a different Transport recipe file for the WAM and the IIWA to allow different people to use different strategies that fit better their robot workspace.

The most important changes in the planner are:

  1. A naming convention for grasp names has been established (SurfaceGrasp instead of surface_grasp, etc.)
  2. Dead/not-used-at-the-moment code in planner.py has been removed to reduce the file size (e.g. the find_a_path function)
  3. Generation of pregrasp poses is no longer performed inside the recipes. This is in preparation for a next PR where the planner will make an effort to no longer propose unreachable pregrasp poses for the robot.
  4. Redundant params in the handarm_params file have been removed.
  5. The name of the ee_frame is now passed as an argument to the grasp_success_estimator

We have tried to keep the WAM recipes as close to the ones in master BUT there is an issue with the way the InterpolatedHTransformControlMode was being used.

More specifically, there seems to exist a misconception regarding how the InterpolatedHTransformControlMode works. When goal_is_relative=0 the underlying controller is an Operation Space Controller and the goal is an absolute pose (wrt to base_frame) in the 6D robot space. However, when goal_is_relative=1 the underlying controller is actually trik_controller which is a velocity controller and the goal is actually cartesian velocities (which are extracted from the translation part of the goal matrix). This has obviously caused quite a lot of confusion to all of us, so that's why we decided to:

  1. Keep the InterpolatedHTransformController with the 6D pose functionality and when goal_is_relative=1 this would mean that we send a 6D pose which is relative to the passed refernce frame.
  2. Create the CartesianVelocityController which will actually implement the previous goal_is_relative=1 functionality of the InterpolatedHSpaceController. The CartesianVelocityControlMode takes a 6x1 np.array as input which corresponds to the 6D velocity of the robot.

This change would need some refactoring from your side, but we believe it is essential for the sake of clarity.

JannisW commented 5 years ago

I'm working on merging this (and the corresponding others) PR right now. The changes mentioned in the last statement of the PR's description are currently keeping me from getting it done and thus require some further discussion:

We have tried to keep the WAM recipes as close to the ones in master BUT there is an issue with the way the InterpolatedHTransformControlMode was being used.

More specifically, there seems to exist a misconception regarding how the InterpolatedHTransformControlMode works. When goal_is_relative=0 the underlying controller is an Operation Space Controller and the goal is an absolute pose (wrt to base_frame) in the 6D robot space. However, when goal_is_relative=1 the underlying controller is actually trik_controller which is a velocity controller and the goal is actually cartesian velocities (which are extracted from the translation part of the goal matrix). This has obviously caused quite a lot of confusion to all of us, so that's why we decided to:

1. Keep the InterpolatedHTransformController with the 6D pose functionality and when `goal_is_relative=1` this would mean that we send a 6D pose which is relative to the passed refernce frame.

2. Create the CartesianVelocityController which will actually implement the previous `goal_is_relative=1` functionality of the InterpolatedHSpaceController. The CartesianVelocityControlMode takes a 6x1 np.array as input which corresponds to the 6D velocity of the robot.

This change would need some refactoring from your side, but we believe it is essential for the sake of clarity.

As far as I know, for us, the semantics of the relative InterpolatedHTransformController has always been defined the way you're describing it in 1. The HybridAutomatonManager on our WAM was implemented respectively. Thus, it was never intended to use the second behavior (CartesianVelocityController) in the planner. I suspect the main confusion resulted from our badly named parameters. This is the reason why we already changed all parameter names from speed to dist in all our current working branches.

Am I right, that you don't have the option of doing relative position control on your robot (right now)?

Unfortunately, we can't move to velocity control on our side either. Also note that our feasibility checker works based on displacement, rather than velocity. It would be still possible for you to use it, but (at least in the current state) you might have to define some of the distance-based parameters again, until I fix that. The validity of the check would't be affected by that anyway. It would only introduce some unwated redundancy.

If you do have relative position control, I'd suggest to throw out CartesianVelocityController and only use InterpolatedHTransformController as defined in 1. I already removed velocity dependencies from all WAM related parts, so I could do that for the KUKA ones as well.

Since I suspect it is not the case, I absolutely agree that it makes sense to use your clear and distinct definitions. In that case, I think separating it at the recipe level makes sense. This would mean the recipes for WAM and KUKA will differ in what controller (InterpolatedHTransformController vs. CartesianVelocityController) is used. I could adjust that while merging our features.

Please let me know if I misunderstood something and if you think that the suggest solution would work for you.

ptriantd-ocado commented 5 years ago

We can do relative position control on our robot. You are right that this is currently not implemented in the respective hybrid_automaton_manager_kuka controller, but it could be easily implemented. But this is not the problem...

Our current implementation of InterpolatedHTransformController is a cartesian space controller that uses MoveIt! to account for collisions with the environment. The reason we don't use that when we approach or lift the object is that it does not always lead to predictable/straight-line motions due to the kinematic constraints of the robot and the environmental constraints (IFCO walls, etc.)

Instead, we use a CartesianVelocityController to do straight-line motions. Of course, we are not sending velocities to the robot. We are tracking a point, etc.

The reason I thought you were using a velocity controller as well is that you had a FT switch after the InterpolatedHTransformControlMode. Are you trying to go under the IFCO and that's why the FT switch will eventually trigger? If this is the case, then it is fine by us, but we would prefer to keep the predictability of our CartesianVelocityControlMode. So yes, we can have different recipes for WAM and KUKA (this is a feature that this PR allows us to have after all)

Regarding the feasibility check, our reachability check is also reasoning in terms of displacements and not velocities, so I think there will be a way to integrate when we reach that point. If we need some extra params, then so be it...

JannisW commented 5 years ago

Thank you for the clarification and the quick response.

We indeed place the goal position slightly below the ifco / behind the ifco wall to ensure triggering the FT-Switch.

I understand that you prefer to keep the CartesianVelocityControlMode on your side. Therefore, I will only adjust our WAM recipes accordingly. Being able to do so is definitely a benefit of having that new feature. It comes with the price of potentially maintaining redundant code. However, separating the recipes into phases already mitigates that effect, so I think it is an improvement over the current state. I have to note though that I wont be able to merge new features into recipes, which aren't for the WAM. Especially since I can't test them on a real platform here. Thus, we will provide all new features, which require changes in the recipes, only for those used by the WAM. From there it should be straight forward for you to propagate the changes to the KUKA recipes as well. If it is not, we are happy to answer your questions.

ptriantd-ocado commented 5 years ago

Great! Yes, we will propagate the recipe changes to the KUKA recipes...