Added type checking for ik responses to ik->trajectory again

As the former version didn't work for constraint aware iks, the type check is now extended to check for both types.
Fixed declarations of types and ignores
Merge branch 'master' of https://github.com/moesenle/cram_pr2
Moved functions to the right places, corrected line lengths, extended distance

Functions in the pr2_manipulation_process_module are now moved into the right files (according to their use). Also, corrected line lengths and indentations at several places. Extended the euclidean-distance function to return a distance for each link that is given (before, it just returned the first distance it could get). So for each target-link given, there is a cons cell containing the link name and the resulting distance. Additionally, extended the get-fk function to return the right values as well. Also, overall cleanup.
Implemented optional use of the euclidean-distance measure while grasp eval

The distance for the grippers during grasps can now be measured by either a quadratic joint-space integral or the euclidean distance for one of the robot-links from the current pose to the final grasping pose. The `reaching-length' function now takes an optional parameter `calc-euclidean-distance' with which the euclidean distance measure can be turned on.
Switched on euclidean measure calculation for nearest handle evaluation

The nearest handle is now evaluated based on the euclidean distance, not on the joint-space distance anymore.
Corrected line length
Increased the pregrasp distance as the old one was too small.

The gripper almost collided with the object handle. Now the distance is set to 0.25 instead of 0.15 (the actual grasp distance is 0.12, so the extra 0.03 from pregrasp to grasp didn't make such a big difference).
Merge pull request #17 from fairlight1337/master

Changed grasp evaluation to euclidean distance calculation and cleaned up code
Added simplifying function to get euclidean distance for only one link

As the full call to `euclidean-distance' and the interpretation of it's output is pretty cumbersome when only one link is requested (which is the usual case), the simplyfied function `euclidean-distance-for-link' now handles all the data preparation and returns the distance for only one given link. The `reaching-length' function has been adapted appropriately.
Accidentally left copy-paste error in function `euclidean-distance-for-link'

After moving existing code to a separate function, the variable names weren't changed. Fixed this.
Added adapter functions for `ik->trajectory' to overcome type problems

The "(declare (type (or" formulation does not work
here as it does not play well with
`roslisp:with-fields'. `roslisp:with-fields' internally wants to
transform `ik-result' into a list and reads the `type' from
`ik-result', resulting in an error saying "OR
KINEMATICS_MSGS-SRC:GETPOSITIONIK-RESPONSE
KINEMATICS_MSGS-SRV:GETCONSTRAINTAWAREPOSITIONIK-RESPONSE is not
of type SYMBOL". Due to this, the function holding the actual
functionality is renamed to `ik->trajectory-ex' and two separate
adapter functions `ik->trajectory' are created, each stating the
type of message they accept in the parameters list. They in turn call the actual `ik->trajectory-ex' function, relieving it from it's type checking duties.
Fixed a bug where an object designator was checked instead of a location desig

During evaluation whether an object is in a gripper or not, the `gripper' property was checked on the object desig instead of the `at' location desig. This kept lifting from working correctly. Fixed this.
Fixed side evaluation in `grasp-object-with-handles'

The side parameter which can be passed as `nil' was still used as the same value even after handle evaluation. The correct side is normally inferred by `nearest-handle' when the `side' parameter is `nil'. The inferred value is used now.
Removed an obsolete check for nil and removed an unnecessary `declare' line

A value was checked for nil which was checked two lines before already. Also, removed a `(declare (ignore ...))' line which is not necessary anymore (concerns the now used nearest-side inferred handle side in grasp-object-with-handles).
Corrected indentation
Added another call to the simpler function `euclidean-distance-for-link'
Changed calls to grasp-object-with-handles to the constraint aware version
Corrected indentations
Corrected indentation