Please, go to branch HOLM-primitives to find the T-RO relevant code.
The HOLM-primitives technical details are described in the paper entitled: "Online Hybrid Motion Planning for Dyadic collaborative Manipulation via Bilevel Optimization", which is published at Transactions of Robotics (T-RO).