Package for generating the arm movement for chair grasping with the simple_robot_control.
In this package we generate the graspmotion of the robot
The idea is that the robot first move his arms into a preGrasp_position.
From there it is possible to grasp the chair safely.
So we defined first the orientations of the Gripper and the preGrasp_positions.
After that we move the robot into a start_pose.
Now, we have to decide if we are able to grasp the chair correctly.
If not, we have to perform a correction.
For a 100% correction of the orientation of the chair we need some knowledge about the
pivot of the chair.
Because we haven't this knowledge, we could only use a simple try for correction.
We have to realize in which direction the chair is rotated (clockwise, or against clockwise)
so we can decide which arm should be used for the correction.
At this point we affect a hard assumption, because we presuppose that we can grasp the gripper_point
which is closer to the robot.
So, we grasp the closer gripper_point and move him to another new position.
