For both the grasp and the pre-grasp positions, I set the constraints such that the arm must be very close (within 0.0001) to a specific position and must have an angle that is very close to the angle of the rectangle to be grasped.
For the pre-grasp position, I had the arm move to a position away from the rectangle, such that the gripper would be aligned with the box so that it could be moved in a straight line (along which the rectangle is aligned) to the goal rectangle without having to worry about the two "fingers" of the gripper colliding.
I made sure that the position the gripper moved to for pre-grasp was far enough away that I wouldn't have to worry about the gripper fingers colliding with the rectangle (at least in the cases I saw when testing).
The criterion I had MATLAB minimize was a weighted sum of squares of the angles of the joints and the amount that the "elbow" had to move. The weight used for angle (in radians) was 100 times the one for torso translation, because I hoped to minimize the about that each joint would have to move.
In order to have the arm move, I had MATLAB calculate the pre-grasp solution, then had the arm adjust it's torso joint to match the torso position found in the pregrasp solution. Once the torso was in place, the joint angles were simultaenously adjusted to reach the values in the solution. I found that adjusting the torso first generally prevented collisions. To move to the grasp position from pregrasp, I had the code calculate the difference in angles and torso position between the pre-grasp and grasp positions found by fmincon, and then transition the torso and joints simultaneously.
In order to try to figure out if a point is reachable, I first ran fmincon with a criterion function that always returned zero, to get a guess for a the pre-grasp solution. If this failed, the script would try again a few times with random starting points, in case the algorithm got stuck in a local minimum. If a feasible pre-grasp solution was used, I repeated the same process for the grasp solution. These feasible solutions were used as starting points to the calls to fmincon that would be used. In the event it was not possible to find a feasible solution to both the grasp and pre-grasp problems, I had the script print a message and quit, as I assumed that if a solution could not be found after several iterations with random starting points that there actually was no solution, instead of the sovler just being caught in local miminima.
The possible positions and orientatons for the "hand" are all (x,y, \phi) that can be described by