A Generic, Affordance-Constrained, Screw-Based Motion Planner for Contact Task Manipulation

Performing autonomous contact tasks is still a challenge for robots. These require the robot to follow tight path constraints and handle largely unknown contact forces while doing so. These challenges are further increased for tasks that exceed the robot's dexterous workspace. In this work, we propose an enhanced, multi-path stepping algorithm that leverages the Sequential Path Stepping Screw Framework to generate long motion plans, and incorporates Cartesian task wrenches. We use a combination of methods to create continuous configuration space paths within the reachable workspace. We also explore task wrenches to generate joint torques for contact task plans and we validate our method with different inverse kinematics solvers and different task orientations. The results show a 90% success rate with BioIK and PRM when enforcing joint reconfiguration to the next way point pose in the trajectory, and a 100% percent success rate for various task orientations when enforcing reconfigurations to the motion start state. Our framework increases the likelihood of success for contact task execution by combining task kinematics and statics in motion plans.