Continued working on subproblem1 for inverse kinematics