Plan grasp points, grip matrix and initial grasp force