Planning of contact-interaction trajectories using numerical optimization