Trajectory planning with task constraints in densely filled environments

In this paper the problem of computing a rigid object trajectory in an environment populated with deformable objects is addressed. The problem arises in Minimally Invasive Robotic Surgery (MIRS) from the needs of reaching a point of interest inside the anatomy with rigid laparoscopic instruments. We address the case of abdominal surgery. The abdomen is a densely populated soft environment and it is not possible to apply classical techniques for obstacle avoidance because a collision free solution is, most of the time, not feasible. In order to have a convergent algorithm with, at least, one possible solution we have to relax the constraints and allow collision under a specific contact threshold to avoid tissue damaging. In this work a new approach for trajectory planning under these peculiar conditions is implemented. The method computes offline the path which is then tested in a surgical simulator as part of a pre-operative surgical plan.