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
Added a comment about the current state of the `lift' operation
Removed a left-over definition of an unused list
Removed the constraint-awareness for the actual grasp for handled objects

Since the grippers need to get real close to the object when executing the actual grasping motion (after the pregrasp), the constraint-awareness needs to be switched off here. Otherwise, the collision environment will signal an invalid IK solution and will break the whole grasp. As we are already in pregrasp when this happens, we assume that no obstacles are between the grippers and the handles to grasp.