diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 9c80201..2c9b4b0 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,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", 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(&Handle::clearance), 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, @@ -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 diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index 23cabda..76edb15 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,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& 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 6191c8f..e32c872 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; @@ -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", no_init) - .def("create", &Gripper::create, DocClassMethod(create)) - .staticmethod("create") .add_property("localPosition", &getObjectPositionInJoint) .add_property( "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, @@ -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 diff --git a/src/pyhpp/pinocchio/device.hh b/src/pyhpp/pinocchio/device.hh index 1b59d28..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; @@ -54,7 +55,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) {