Geometry-Based Two-Contact Inverse Kinematic Solution for Whole Arm Manipulation