From 4ad44e52ba61a5b1bb59da011e61bd4fbffc3ae3 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 4 Mar 2026 15:08:07 +0100 Subject: [PATCH 1/3] [pyhpp.pinocchio] Bind method Device::removeJoints --- src/pyhpp/pinocchio/device.cc | 3 ++- src/pyhpp/pinocchio/device.hh | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 6191c8f..64a04ea 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -332,7 +332,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 diff --git a/src/pyhpp/pinocchio/device.hh b/src/pyhpp/pinocchio/device.hh index 1b59d28..3627b79 100644 --- a/src/pyhpp/pinocchio/device.hh +++ b/src/pyhpp/pinocchio/device.hh @@ -54,7 +54,8 @@ struct Device { obj->computeFramesForwardKinematics(); } void updateGeometryPlacements() { obj->updateGeometryPlacements(); } - + void removeJoints(const std::vector& 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) { From 20f22d4c312f4a684bb7be76c9ab25a8ad669e95 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 4 Mar 2026 15:07:34 +0100 Subject: [PATCH 2/3] [manipulation.Device] Add methods to add handles and grippers. Bind approachingDirection setter and getter. --- src/pyhpp/manipulation/device.cc | 99 ++++++++++++++++++++++++++++++-- src/pyhpp/manipulation/device.hh | 7 +++ src/pyhpp/pinocchio/device.cc | 3 +- 3 files changed, 103 insertions(+), 6 deletions(-) diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 9c80201..9291d51 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/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& 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,17 @@ 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); +} + void exposeHandle() { // DocClass(Handle) class_("Handle", no_init) - .def("create", &Handle::create, DocClassMethod(create)) - .staticmethod("create") .add_property("name", &getHandleName, &setHandleName) .add_property("localPosition", &getHandleLocalPosition, &setHandleLocalPosition) @@ -195,6 +268,7 @@ void exposeHandle() { "clearance", static_cast(&Handle::clearance), static_cast(&Handle::clearance)) + .add_property("approachingDirection", &getApproachingDirection, &setApproachingDirection) .def("createGrasp", &Handle::createGrasp, DocClassMethod(createGrasp)) .def("createPreGrasp", &Handle::createPreGrasp, DocClassMethod(createPreGrasp)) @@ -228,7 +302,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 diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index 23cabda..596140f 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/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,6 +67,7 @@ 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::JointPtr_t JointPtr_t; @@ -85,6 +88,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& mask); + void addGripper(const std::string& linkName, const std::string& gripperName, + const Transform3s& pose, value_type clearance); }; // struct Device } // namespace manipulation } // namespace pyhpp diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 64a04ea..4d1f13d 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/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; @@ -215,8 +216,6 @@ static boost::python::list getJointsPosition( void exposeGripper() { // DocClass(Gripper) class_("Gripper", no_init) - .def("create", &Gripper::create, DocClassMethod(create)) - .staticmethod("create") .add_property("localPosition", &getObjectPositionInJoint) .add_property( "clearance", From f5e2bf1daaa1b8278fcd9bb8f0b47f4c4cc969f7 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 6 Mar 2026 15:23:07 +0100 Subject: [PATCH 3/3] [Gripper, Handle] Add a method to get the parent joint index in pinocchio model This is particularly useful to define RelativeTransformation constraints. Also add missing accessor to gripper name. --- src/pyhpp/manipulation/device.cc | 8 ++++++++ src/pyhpp/manipulation/device.hh | 1 + src/pyhpp/pinocchio/device.cc | 14 +++++++++++++- src/pyhpp/pinocchio/device.hh | 1 + 4 files changed, 23 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 9291d51..2c9b4b0 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -256,6 +256,12 @@ 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", no_init) @@ -270,6 +276,8 @@ void exposeHandle() { static_cast(&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, diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index 596140f..76edb15 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/src/pyhpp/manipulation/device.hh @@ -69,6 +69,7 @@ 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 diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 4d1f13d..e32c872 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -213,6 +213,15 @@ 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", no_init) @@ -221,7 +230,10 @@ void exposeGripper() { "clearance", static_cast(&Gripper::clearance), static_cast( - &Gripper::clearance)); + &Gripper::clearance)) + .def("name", &getGripperName) + .def("getParentJointId", &getParentJointId, "Get index of the joint the handle is attached to" + " in pinocchio Model"); class_ >("GripperMap") .def( boost::python::map_indexing_suite, diff --git a/src/pyhpp/pinocchio/device.hh b/src/pyhpp/pinocchio/device.hh index 3627b79..cefb377 100644 --- a/src/pyhpp/pinocchio/device.hh +++ b/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;