Bind some more methods related to grippers and handles

src/pyhpp/manipulation/device.cc

@@ -124,13 +124,80 @@ boost::python::dict Device::contactSurfaces() {
   }
   return result;
 }
+
+void Device::addHandle(const std::string& linkName, const std::string& handleName,
+                       const Transform3s& pose, value_type clearance, const std::vector<bool>& mask){
+  hpp::manipulation::DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj);
+  if (!robot) {
+    throw std::logic_error("Device.addHandle expects a manipulation device (imported from pyhpp."
+                           "manipulation)");
+  }
+  if (mask.size () != 6) {
+    throw std::logic_error("mask should be of size 6");
+  }
+  JointPtr_t joint = robot->getJointByBodyName(linkName);
+
+  const ::pinocchio::Frame& linkFrame =
+    robot->model().frames[robot->model().getFrameId(std::string(linkName))];
+  assert(linkFrame.type == ::pinocchio::BODY);
+
+  hpp::pinocchio::JointIndex index(0);
+  std::string jointName("universe");
+  if (joint) {
+    index = joint->index();
+    jointName = joint->name();
+  }
+  HandlePtr_t handle =
+    Handle::create(handleName, linkFrame.placement * pose, robot, joint);
+  handle->clearance(clearance);
+  handle->mask(mask);
+  robot->handles.add(handleName, handle);
+  assert(robot->model().existFrame(jointName));
+  FrameIndex previousFrame(robot->model().getFrameId(jointName));
+  robot->model().addFrame(::pinocchio::Frame(handleName, index, previousFrame,
+                                             linkFrame.placement * pose,
+                                             ::pinocchio::OP_FRAME));
+  // Recreate pinocchio data after modifying model
+  robot->createData();
+}
+
+void Device::addGripper(const std::string& linkName, const std::string& gripperName,
+                        const Transform3s& pose, value_type clearance) {
+  hpp::manipulation::DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj);
+  if (!robot) {
+    throw std::logic_error("Device.addHandle expects a manipulation device (imported from pyhpp."
+                           "manipulation)");
+  }
+  JointPtr_t joint = robot->getJointByBodyName(linkName);
+
+  const ::pinocchio::Frame& linkFrame =
+    robot->model().frames[robot->model().getFrameId(std::string(linkName))];
+  assert(linkFrame.type == ::pinocchio::BODY);
+
+  hpp::pinocchio::JointIndex index(0);
+  std::string jointName("universe");
+  if (joint) {
+    index = joint->index();
+    jointName = joint->name();
+  }
+  assert(robot->model().existFrame(jointName));
+  FrameIndex previousFrame(robot->model().getFrameId(jointName));
+  robot->model().addFrame(::pinocchio::Frame(gripperName, index, previousFrame,
+      linkFrame.placement * pose, ::pinocchio::OP_FRAME));
+  // Recreate pinocchio data after modifying model
+  robot->createData();
+  GripperPtr_t gripper = Gripper::create(gripperName, robot);
+  gripper->clearance(clearance);
+  robot->grippers.add(gripperName, gripper);
+}
+
 boost::python::list Device::getJointConfig(const char* jointName) {
   try {
     Frame frame = obj->getFrameByName(jointName);
     if (frame.isFixed()) return boost::python::list();
     JointPtr_t joint = frame.joint();
     if (!joint) return boost::python::list();
-    hpp::core::vector_t config = obj->currentConfiguration();
+    vector_t config = obj->currentConfiguration();
     size_type ric = joint->rankInConfiguration();
     size_type dim = joint->configSize();
     auto segment = config.segment(ric, dim);
@@ -181,11 +248,23 @@ void setHandleMaskComp(const HandlePtr_t& handle,
   handle->maskComp(mask);
 }
 
+vector3_t getApproachingDirection(const HandlePtr_t& handle) {
+  return handle->approachingDirection();
+}
+
+void setApproachingDirection(const HandlePtr_t& handle, const vector3_t& dir) {
+  handle->approachingDirection(dir);
+}
+
+static JointIndex getParentJointId(const HandlePtr_t& handle) {
+  assert(handle->robot());
+  Model model(handle->robot()->model());
+  return model.frames[model.getFrameId(handle->name())].parentJoint;
+}
+
 void exposeHandle() {
   // DocClass(Handle)
   class_<Handle, HandlePtr_t>("Handle", no_init)
-      .def("create", &Handle::create, DocClassMethod(create))
-      .staticmethod("create")
       .add_property("name", &getHandleName, &setHandleName)
       .add_property("localPosition", &getHandleLocalPosition,
                     &setHandleLocalPosition)
@@ -195,7 +274,10 @@ void exposeHandle() {
           "clearance",
           static_cast<value_type (Handle::*)() const>(&Handle::clearance),
           static_cast<void (Handle::*)(const value_type&)>(&Handle::clearance))
+      .add_property("approachingDirection", &getApproachingDirection, &setApproachingDirection)
       .def("createGrasp", &Handle::createGrasp, DocClassMethod(createGrasp))
+      .def("getParentJointId", &getParentJointId, "Get index of the joint the handle is attached to"
+         " in pinocchio Model")
       .def("createPreGrasp", &Handle::createPreGrasp,
            DocClassMethod(createPreGrasp))
       .def("createGraspComplement", &Handle::createGraspComplement,
@@ -228,7 +310,24 @@ void exposeDevice() {
       .def("contactSurfaceNames", &Device::contactSurfaceNames,
            "Return list of contact surface names registered on device")
       .def("contactSurfaces", &Device::contactSurfaces,
-           "Return dict mapping surface names to list of {joint, points}");
+           "Return dict mapping surface names to list of {joint, points}")
+      .def("addHandle", &Device::addHandle,
+           "Add a handle to the kinematic chain\n\n"
+           "  input\n"
+           "    linkName: name of the link the handle is attached to,\n"
+           "    handleName: name of the handle,\n"
+           "    pose: pose of the handle in the link frame (SE3),\n"
+           "    clearance: clearance of the handle, the sum of handle and gripper clearances\n"
+           "               defines the distance between pregrasp and grasp,\n"
+           "    mask: list of 6 Boolean use to define symmetries in the grasp constraint.")
+      .def("addGripper", &Device::addGripper,
+           "Add a gripper to the kinematic chain\n\n"
+           "  input\n"
+           "    linkName: name of the link the handle is attached to,\n"
+           "    gripperName: name of the gripper,\n"
+           "    pose: pose of the gripper in the link frame (SE3),\n"
+           "    clearance: clearance of the gripper, the sum of handle and gripper clearances\n"
+           "               defines the distance between pregrasp and grasp.");
 }
 }  // namespace manipulation
 }  // namespace pyhpp

src/pyhpp/manipulation/device.hh

@@ -54,6 +54,8 @@ typedef hpp::pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
 typedef hpp::pinocchio::GeomModel GeomModel;
 typedef hpp::pinocchio::Configuration_t Configuration_t;
 typedef hpp::pinocchio::ConfigurationIn_t ConfigurationIn_t;
+typedef hpp::pinocchio::vector_t vector_t;
+typedef hpp::pinocchio::vector3_t vector3_t;
 typedef hpp::pinocchio::size_type size_type;
 typedef hpp::pinocchio::value_type value_type;
 typedef hpp::pinocchio::Transform3s Transform3s;
@@ -65,7 +67,9 @@ typedef hpp::manipulation::Handle Handle;
 typedef hpp::manipulation::DevicePtr_t DevicePtr_t;
 typedef hpp::pinocchio::DevicePtr_t PinDevicePtr_t;
 typedef hpp::pinocchio::Frame Frame;
+typedef hpp::pinocchio::FrameIndex FrameIndex;
 typedef hpp::pinocchio::Joint Joint;
+typedef hpp::pinocchio::JointIndex JointIndex;
 typedef hpp::pinocchio::JointPtr_t JointPtr_t;
 
 // Wrapper class to hpp::manipulation::Device
@@ -85,6 +89,10 @@ struct Device : public pyhpp::pinocchio::Device {
   void setJointBounds(const char* jointName, boost::python::list jointBounds);
   boost::python::list contactSurfaceNames();
   boost::python::dict contactSurfaces();
+  void addHandle(const std::string& linkName, const std::string& handleName,
+                 const Transform3s& pose, value_type clearance, const std::vector<bool>& mask);
+  void addGripper(const std::string& linkName, const std::string& gripperName,
+                  const Transform3s& pose, value_type clearance);
 };  // struct Device
 }  // namespace manipulation
 }  // namespace pyhpp

src/pyhpp/pinocchio/device.cc

@@ -59,6 +59,7 @@ namespace pyhpp {
 namespace pinocchio {
 
 namespace bp = boost::python;
+typedef hpp::pinocchio::DevicePtr_t DevicePtr_t;
 typedef hpp::pinocchio::GripperPtr_t GripperPtr_t;
 typedef hpp::pinocchio::Gripper Gripper;
 typedef hpp::pinocchio::FrameIndex FrameIndex;
@@ -212,17 +213,27 @@ static boost::python::list getJointsPosition(
   }
 }
 
+static std::string getGripperName(const GripperPtr_t& gripper) { return gripper->name(); }
+
+static JointIndex getParentJointId(const GripperPtr_t& gripper) {
+  assert(gripper->joint());
+  assert(gripper->joint()->robot());
+  Model model(gripper->joint()->robot()->model());
+  return model.frames[model.getFrameId(gripper->name())].parentJoint;
+}
+
 void exposeGripper() {
   // DocClass(Gripper)
   class_<Gripper, GripperPtr_t>("Gripper", no_init)
-      .def("create", &Gripper::create, DocClassMethod(create))
-      .staticmethod("create")
       .add_property("localPosition", &getObjectPositionInJoint)
       .add_property(
           "clearance",
           static_cast<value_type (Gripper::*)() const>(&Gripper::clearance),
           static_cast<void (Gripper::*)(const value_type&)>(
-              &Gripper::clearance));
+              &Gripper::clearance))
+      .def("name", &getGripperName)
+      .def("getParentJointId", &getParentJointId, "Get index of the joint the handle is attached to"
+           " in pinocchio Model");
   class_<std::map<std::string, GripperPtr_t> >("GripperMap")
       .def(
           boost::python::map_indexing_suite<std::map<std::string, GripperPtr_t>,
@@ -332,7 +343,8 @@ void exposeDevice() {
       .def("setJointBounds", &setJointBounds)
       .def("getCenterOfMass", &getCenterOfMass)
       .def("getJointPosition", &getJointPosition)
-      .def("getJointsPosition", &getJointsPosition);
+      .def("getJointsPosition", &getJointsPosition)
+      .def("removeJoints", &Device::removeJoints);
   register_device_converters();
 }
 }  // namespace pinocchio

src/pyhpp/pinocchio/device.hh

@@ -21,6 +21,7 @@ typedef hpp::pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
 typedef hpp::pinocchio::GeomModel GeomModel;
 typedef hpp::pinocchio::Configuration_t Configuration_t;
 typedef hpp::pinocchio::ConfigurationIn_t ConfigurationIn_t;
+typedef hpp::pinocchio::JointIndex JointIndex;
 typedef hpp::pinocchio::size_type size_type;
 typedef hpp::pinocchio::Transform3s Transform3s;
 typedef hpp::pinocchio::DevicePtr_t DevicePtr_t;
@@ -54,7 +55,8 @@ struct Device {
     obj->computeFramesForwardKinematics();
   }
   void updateGeometryPlacements() { obj->updateGeometryPlacements(); }
-
+  void removeJoints(const std::vector<std::string>& jointNames,
+                    Configuration_t q) { obj->removeJoints(jointNames, q); }
   hpp::manipulation::DevicePtr_t asManipulationDevice() const {
     auto manipDevice = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, obj);
     if (!manipDevice) {