C++ code to accompany mjregrasping, which is written in python