Add Jia's grasp planning work