Made `?side' be a dynamic prolog parameter for put-down
Fixed some line lengths and added the `min-handles' property for grasping
Merge branch 'master' of https://github.com/moesenle/cram_pr2
Corrected indentation for key parameters
Merge pull request #29 from yazdani/master

 Extended put-down prolog pattern for multiple arms
Merge branch 'master' of https://github.com/moesenle/cram_pr2

Conflicts:
	pr2_manipulation_process_module/src/designator.lisp
Changed `graspable-handled' to `reachable-handled' for clarity

The new predicate and variable names better reflect what their meaning is.
Removed an old comment
Changed the `?grippers' variable names to `?arms' to make it more consistent

All over the code, the terms `grippers' and `arms' are intermixed, basically having the same meaning in those cases (i.e. the :left and :right side for the robot's grasping arms). The respective prolog patterns were now changed to use the `?arms' convention.
Added prolog predicates and lisp functions for multi handle/arm grasp evaluation

To decide which arm to use to grasp with which handle in multi-arm/-handle situations, functions for calculating the optimal gripper distance from the solution were implemented. The PR2 now determines which handle is grasped with which arm based on the respective distance between them. This way, objects can be grasped with an arbitrary number of arms in the most distance-optimal way. A future extension to this is the decision of arm-/handle-choice based on the stability of the handle-grasp.
Corrected line lengths
Removing the now obsolete `nearest-handle' predicate

Since the `optimal-handle-assignment' predicate is now working and taking over the multi-arm/handle-assignment, the `nearest-handle' predicate (which was used for single arm/handle evaluations) is not needed anymore.
Readded the original `ik->trajectory' function instead of workaround functions

Since the roslisp function `roslisp:with-fields' was fixed and now works fine with `(declare (type (or kinematics_msgs-srv:getpositionik-response kinematics_msgs-srv:getconstraintawarepositionik-response)))', the workaround functions for `ik->trajectory' were removed and the original method `ik->trajectory-ex' was renamed to `ik->trajectory' again.
Moved the decision of how to handle multiple grasps at the same time

The decision of how to deal with multiple arm/handle-grasps at the same time is now done in grab-handled-object-constraint-aware instead of the action handler grasp-handle. This will be refined further on as `(par' blocks need to be introduced here.
Corrected and added doc strings where necessary
Enabled the function `grab-handled-object' to grab multiple sides simultaneously

By using the `(par' macro, all given arm/handle-combinations are used during the grasp of a handled object. The individual functionalities like going into a pregrasp pose, opening the gripper(s) and the actual grasping are synchronized in between. The doc string has been updated appropriately and all calling functions that use `grab-handled-object' now use this logic.
Added `(par-synced' macro and made `grab-handled-object' use multiple arms

The macro `(par-synced' macro was added to execute a sequence of blocks of parallel actions. Meaning:

(par-synced (A B) (C D E))

A and B are executed in parallel. When they are done, C, D and E are executed in parallel. This is applied to the `grab-handled-object' method to make it grab multi-handled objects and use several grippers in parallel. The synchronized parallelization is necessary here due to the fact that each gripper has to be in pose / opened/closed before the next step can be started (otherwise, objects might slip, trajectories are not in sync anymore, etc.). Also, a `ros-info' was added to inform the user that the optimal grasp arm/handle combination is currently being calculated (this process can take a few seconds and led to confusion sometimes).
Made the holding-arms predicate only rely on the belief state

The belief state holds all information necessary to identify the arms that currently hold an object in question. Though, the information is still stored in the designator and should be used accordingly at appropriate places where a query to the belief state would be too `expensive'. Since at the moment, only single-arm grasps are stored in the designator, this solution is commented out and should be readded after refactoring the appropriate functions updating the designator.
Moved validity check for grippers after grasping a handled object out of `(par'

The check for validity of a gripper state does not need to be executed in parallel.
Corrected some line lengths