Efficient Constrained Motion Planning Using Direct Sampling of Screw-Constraint Manifolds

Manipulating articulated objects is especially difficult if the robot is operating autonomously or far from any human operator. Object articulation imposes strict constraints on robot motion, making it a challenge to generate valid trajectories to complete the task. Problems compound when the robot is mobile and operates in an uncontrolled environment, where the location or articulation model is unknown a priori. In this work, we leverage screw theory to model constraints imposed on a generic manipulator by simple articulated objects and present two novel, fast, and robust methods–Sequential Path Stepping (SPS) and Direct Screw Sampling (DSS)–for planning trajectories by directly sampling these constraints. We show that these methods are hardware-agnostic and work in cluttered environments using long, complex paths modeled by multiple screw-axis constraints. We demonstrate that modeling constraints using multiple screw axes handles objects with multiple DoF, or multi-step tasks (e.g., turning a knob before opening the door). In addition, the direct sampling component of the proposed approaches is implemented as a module that used with existing well-known probabilistic planning methods, allowing customization across different hardware, domains, or planning problems. We validate our methods across many planning and inverse kinematic elements, with three different mobile and stationary manipulators, and on a set of challenging planning problems that include single- and multiple-screw constraints. Results demonstrate a 97.6% success rate planning in cluttered environments, in less than 0.2 seconds.Note to Practitioners—This paper was motivated by the articulated manipulation problem for mobile manipulators. The solutions also apply to non-articulated object manipulation and task planning. Existing approaches do not include helical constraints (e.g., turning a threaded bolt), require operator oversight, or lack integration with MoveIt: the de facto kinematic manipulation standard. We address these issues with two methods that utilize screw theory to enable helical (inclusive of revolute and prismatic) constraints, require little input from operators, and align with standard plan-execute, task definition, and robot configuration capabilities offered by MoveIt and ROS. Through experimentation, we show that our methods easily plan manipulations of arbitrary articulated objects-including those with multiple DoF-are relatively quick, successful, and hardware agnostic.