WAM-Plan: Contact-Rich Global Planning for Whole-Arm Manipulation and Control

I am currently researching contact-rich global planning for whole-arm manipulation (first-author), focusing on ways to combine Model Predictive Control with offline planning to enable robot arms to move around humans while safely making contact across the entire robot arm and regulating contact forces. I am specifically using a robot arm equipped with distributed tactile sensing which enable MPC to regulate forces in real time. My key insight is that globally reasoning about manipulability at contact locations as part of a trajectory optimization cost function should create trajectories that ensure the robot has enough flexibility of movement at contact points for MPC to effectively regulate contact forces, avoid local minima, and safely react to unexpected human movements with minimal disruption to the overall goal. Guiding the MPC along the least dangerous path should allow for more effective and safe robotics for pHRI.

My current approach relies on specifically maximizing √det(JJ’), a metric of the volume of the manipulability ellipsoid, using the contact-Jacobian for each contact location. I am weighting this manipulability cost for each taxel as a function of its distance to the obstacle, ensuring that the closest taxels have more importance. This cost is incorporated into a trajectory optimization stack (CasADi solved with IPOPT) along with distance and smoothness costs. For initialization, I am using multiple seeds of a task-space RRT algorithm running collision-aware inverse kinematics (Mink) so as to avoid optimization local minima and ensure a solution is found. Some penetration is allowed to simulate contact, but in reality contact is likely to be made due to inaccuracy in modeling and movement of obstacles such as humans. Planning with such whole-arm contacts in mind ensures that the robot can always regulate contact forces to perform its tasks safely when around humans, which allows a much greater workspace area than enforcing a safety clearance. I am utilizing PyBullet and MuJoCo as part of this project.

This experience has been absolutely crucial with regards to understanding and experiencing the full cycle of research from a first author perspective. Over the course of a year I worked on my own research project with complete independence other than weekly advisor meetings: performing literature review, narrowing down key exploration topics and strategies, building code infrastructure, deriving and implementing various original algorithms, testing my methods, adjusting my focus, iterating, and writing reports on my findings.

(ours) RRT Baseline