From f429619586ad252cef59949a11ebe8f37b018cfb Mon Sep 17 00:00:00 2001 From: Jack Rasiel Date: Sat, 6 Aug 2016 00:19:57 -0400 Subject: [PATCH 1/6] Added body name field to relevant msgs and srvs. --- msg/Body.msg | 3 +- msg/GraspableBody.msg | 3 +- msg/Robot.msg | 1 + src/graspit_interface.cpp | 164 +++++++++++++++++++++++++------------- srv/GetBodies.srv | 1 + 5 files changed, 115 insertions(+), 57 deletions(-) diff --git a/msg/Body.msg b/msg/Body.msg index aeab862..3cd831b 100644 --- a/msg/Body.msg +++ b/msg/Body.msg @@ -1,3 +1,4 @@ std_msgs/Header header -geometry_msgs/Pose pose \ No newline at end of file +string element_name #UID for the body's world element. +geometry_msgs/Pose pose diff --git a/msg/GraspableBody.msg b/msg/GraspableBody.msg index aeab862..3cd831b 100644 --- a/msg/GraspableBody.msg +++ b/msg/GraspableBody.msg @@ -1,3 +1,4 @@ std_msgs/Header header -geometry_msgs/Pose pose \ No newline at end of file +string element_name #UID for the body's world element. +geometry_msgs/Pose pose diff --git a/msg/Robot.msg b/msg/Robot.msg index 4dfa2f5..99c3ecd 100644 --- a/msg/Robot.msg +++ b/msg/Robot.msg @@ -1,5 +1,6 @@ std_msgs/Header header +string element_name #UID for the body's world element. geometry_msgs/Pose pose sensor_msgs/JointState[] joints float64[] dofs diff --git a/src/graspit_interface.cpp b/src/graspit_interface.cpp index f547889..f168fc7 100644 --- a/src/graspit_interface.cpp +++ b/src/graspit_interface.cpp @@ -126,11 +126,30 @@ bool GraspitInterface::getRobotCB(graspit_interface::GetRobot::Request &request, return true; } -bool GraspitInterface::getGraspableBodyCB(graspit_interface::GetGraspableBody::Request &request, - graspit_interface::GetGraspableBody::Response &response) -{ - if (graspitCore->getWorld()->getNumGB() <= request.id) { - response.result = response.RESULT_INVALID_ID; + bool GraspitInterface::getGraspableBodyCB(graspit_interface::GetGraspableBody::Request &request, + graspit_interface::GetGraspableBody::Response &response) + { + if (graspitCore->getWorld()->getNumGB() <= request.id) { + response.result = response.RESULT_INVALID_ID; + return true; + } else { + GraspableBody *b = graspitCore->getWorld()->getGB(request.id); + transf t = b->getTran(); + + geometry_msgs::Pose pose = geometry_msgs::Pose(); + + pose.position.x = t.translation().x() / 1000.0; + pose.position.y = t.translation().y() / 1000.0;; + pose.position.z = t.translation().z() / 1000.0;; + pose.orientation.w = t.rotation().w; + pose.orientation.x = t.rotation().x; + pose.orientation.y = t.rotation().y; + pose.orientation.z = t.rotation().z; + + response.graspable_body.pose = pose; + response.graspable_body.element_name = b->getName().toStdString(); + return true; + } return true; } else { GraspableBody *b = graspitCore->getWorld()->getGB(request.id); @@ -146,17 +165,41 @@ bool GraspitInterface::getGraspableBodyCB(graspit_interface::GetGraspableBody::R pose.orientation.y = t.rotation().y; pose.orientation.z = t.rotation().z; - response.graspable_body.pose = pose; + bool GraspitInterface::getBodyCB(graspit_interface::GetBody::Request &request, + graspit_interface::GetBody::Response &response) + { + if (graspitCore->getWorld()->getNumBodies() <= request.id) { + response.result = response.RESULT_INVALID_ID; + return true; + } else { + Body *b = graspitCore->getWorld()->getBody(request.id); + transf t = b->getTran(); + + geometry_msgs::Pose pose = geometry_msgs::Pose(); + + pose.position.x = t.translation().x() / 1000.0; + pose.position.y = t.translation().y() / 1000.0;; + pose.position.z = t.translation().z() / 1000.0;; + pose.orientation.w = t.rotation().w; + pose.orientation.x = t.rotation().x; + pose.orientation.y = t.rotation().y; + pose.orientation.z = t.rotation().z; + + response.body.pose = pose; + response.body.element_name = b->getName().toStdString(); + + return true; + } return true; } - return true; -} -bool GraspitInterface::getBodyCB(graspit_interface::GetBody::Request &request, - graspit_interface::GetBody::Response &response) -{ - if (graspitCore->getWorld()->getNumBodies() <= request.id) { - response.result = response.RESULT_INVALID_ID; + bool GraspitInterface::getRobotsCB(graspit_interface::GetRobots::Request &request, + graspit_interface::GetRobots::Response &response) + { + for (int i=0; i < graspitCore->getWorld()->getNumRobots(); i++) + { + response.ids.push_back(i); + } return true; } else { Body *b = graspitCore->getWorld()->getBody(request.id); @@ -174,63 +217,74 @@ bool GraspitInterface::getBodyCB(graspit_interface::GetBody::Request &request, response.body.pose = pose; + bool GraspitInterface::getGraspableBodiesCB(graspit_interface::GetGraspableBodies::Request &request, + graspit_interface::GetGraspableBodies::Response &response) + { + for (int i=0; i < graspitCore->getWorld()->getNumGB(); i++) + { + response.ids.push_back(i); + } return true; } - return true; -} -bool GraspitInterface::getRobotsCB(graspit_interface::GetRobots::Request &request, - graspit_interface::GetRobots::Response &response) -{ - for (int i=0; i < graspitCore->getWorld()->getNumRobots(); i++) + bool GraspitInterface::getBodiesCB(graspit_interface::GetBodies::Request &request, + graspit_interface::GetBodies::Response &response) { - response.ids.push_back(i); + for (int i=0; i < graspitCore->getWorld()->getNumBodies(); i++) + { + response.ids.push_back(i); + QString name = graspitCore->getWorld()->getBody(i)->getName(); + response.element_names.push_back(name.toStdString()); + } + return true; } - return true; -} -bool GraspitInterface::getGraspableBodiesCB(graspit_interface::GetGraspableBodies::Request &request, - graspit_interface::GetGraspableBodies::Response &response) -{ - for (int i=0; i < graspitCore->getWorld()->getNumGB(); i++) + bool GraspitInterface::setRobotPoseCB(graspit_interface::SetRobotPose::Request &request, + graspit_interface::SetRobotPose::Response &response) { - response.ids.push_back(i); + if (graspitCore->getWorld()->getNumRobots() <= request.id) { + response.result = response.RESULT_INVALID_ID; + return true; + } else { + vec3 newTranslation(request.pose.position.x * 1000.0, + request.pose.position.y * 1000.0, + request.pose.position.z * 1000.0); + + Quaternion newRotation(request.pose.orientation.w, + request.pose.orientation.x, + request.pose.orientation.y, + request.pose.orientation.z); + + transf newTransform(newRotation, newTranslation); + + graspitCore->getWorld()->getRobot(request.id)->setTran(newTransform); + return true; + } } - return true; -} -bool GraspitInterface::getBodiesCB(graspit_interface::GetBodies::Request &request, - graspit_interface::GetBodies::Response &response) -{ - for (int i=0; i < graspitCore->getWorld()->getNumBodies(); i++) + bool GraspitInterface::setGraspableBodyPoseCB(graspit_interface::SetGraspableBodyPose::Request &request, + graspit_interface::SetGraspableBodyPose::Response &response) { - response.ids.push_back(i); - } - return true; -} + if (graspitCore->getWorld()->getNumGB() <= request.id) { + response.result = response.RESULT_INVALID_ID; + return true; + } else { -bool GraspitInterface::setRobotPoseCB(graspit_interface::SetRobotPose::Request &request, - graspit_interface::SetRobotPose::Response &response) -{ - if (graspitCore->getWorld()->getNumRobots() <= request.id) { - response.result = response.RESULT_INVALID_ID; - return true; - } else { - vec3 newTranslation(request.pose.position.x * 1000.0, - request.pose.position.y * 1000.0, - request.pose.position.z * 1000.0); + vec3 newTranslation(request.pose.position.x * 1000.0, + request.pose.position.y * 1000.0, + request.pose.position.z * 1000.0); - Quaternion newRotation(request.pose.orientation.w, - request.pose.orientation.x, - request.pose.orientation.y, - request.pose.orientation.z); + Quaternion newRotation(request.pose.orientation.w, + request.pose.orientation.x, + request.pose.orientation.y, + request.pose.orientation.z); - transf newTransform(newRotation, newTranslation); + transf newTransform(newRotation, newTranslation); - graspitCore->getWorld()->getRobot(request.id)->setTran(newTransform); - return true; + graspitCore->getWorld()->getGB(request.id)->setTran(newTransform); + return true; + } } -} bool GraspitInterface::setGraspableBodyPoseCB(graspit_interface::SetGraspableBodyPose::Request &request, graspit_interface::SetGraspableBodyPose::Response &response) diff --git a/srv/GetBodies.srv b/srv/GetBodies.srv index 76134f7..62a4822 100644 --- a/srv/GetBodies.srv +++ b/srv/GetBodies.srv @@ -1,3 +1,4 @@ --- int32[] ids +string[] element_names From fd493545ed067bd414e821ec831e4e81c63fbc28 Mon Sep 17 00:00:00 2001 From: Jack Rasiel Date: Sat, 6 Aug 2016 00:37:32 -0400 Subject: [PATCH 2/6] Added functionality for generating "table grasps". --- CMakeLists.txt | 5 + include/graspit_interface.h | 26 ++ msg/PregraspParams.msg | 3 + msg/TableGraspPose.msg | 4 + msg/TableGraspPoseArray.msg | 2 + src/graspit_interface.cpp | 531 +++++++++++++++++++++++++----------- srv/FindTableGrasps.srv | 24 ++ srv/ImportGraspableBody.srv | 3 +- srv/ImportObstacle.srv | 3 +- srv/ImportRobot.srv | 3 +- 10 files changed, 435 insertions(+), 169 deletions(-) create mode 100644 msg/PregraspParams.msg create mode 100644 msg/TableGraspPose.msg create mode 100644 msg/TableGraspPoseArray.msg create mode 100644 srv/FindTableGrasps.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 07ed3ec..e1287d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,10 +57,14 @@ find_package(Qt4 COMPONENTS QtCore REQUIRED) Robot.msg Grasp.msg Planner.msg + PlannerResult.msg SearchEnergy.msg SearchSpace.msg SearchContact.msg Energy.msg + TableGraspPose.msg + TableGraspPoseArray.msg + PregraspParams.msg ) ## Generate services in the 'srv' folder @@ -95,6 +99,7 @@ find_package(Qt4 COMPONENTS QtCore REQUIRED) ApproachToContact.srv FindInitialContact.srv DynamicAutoGraspComplete.srv + FindTableGrasps.srv ) diff --git a/include/graspit_interface.h b/include/graspit_interface.h index 3984838..32ef1ab 100644 --- a/include/graspit_interface.h +++ b/include/graspit_interface.h @@ -12,6 +12,8 @@ //Message includes #include +#include +#include // Service includes #include @@ -43,6 +45,8 @@ #include #include #include +#include + // ActionServer includes #include @@ -98,6 +102,8 @@ class GraspitInterface : public QObject, public Plugin ros::ServiceServer dynamicAutoGraspComplete_srv; + ros::ServiceServer findTableGrasps_srv; + // ActionServer declarations actionlib::SimpleActionServer *plan_grasps_as; graspit_interface::PlanGraspsFeedback feedback_; @@ -194,6 +200,26 @@ class GraspitInterface : public QObject, public Plugin bool dynamicAutoGraspCompleteCB(graspit_interface::DynamicAutoGraspComplete::Request &request, graspit_interface::DynamicAutoGraspComplete::Response &response); + // Given grasps for an object, places the object in a bunch of random poses + // on a table, and finds the valid grasps for each pose: + bool findTableGraspsCB(graspit_interface::FindTableGrasps::Request &request, + graspit_interface::FindTableGrasps::Response &response); + // fn used by ^^^. Checks if pregrasp results in collision: + bool preGraspCheck(Hand *hand, Body *object, std::vector open_dofs_by, double retreat_dist); + // convenience function: + inline geometry_msgs::Pose graspitPoseToRosPose(transf pose) { + geometry_msgs::Pose ret; + ret.position.x = pose.translation().x() / 1000.0; + ret.position.y = pose.translation().y() / 1000.0;; + ret.position.z = pose.translation().z() / 1000.0;; + ret.orientation.w = pose.rotation().w; + ret.orientation.x = pose.rotation().x; + ret.orientation.y = pose.rotation().y; + ret.orientation.z = pose.rotation().z; + return ret; + } + + //ActionServer callbacks void PlanGraspsCB(const graspit_interface::PlanGraspsGoalConstPtr &goal); diff --git a/msg/PregraspParams.msg b/msg/PregraspParams.msg new file mode 100644 index 0000000..b667b03 --- /dev/null +++ b/msg/PregraspParams.msg @@ -0,0 +1,3 @@ +float64[] open_dofs_by # How much to open/close hand for pregrasp. +float64 retreat_by # Dist (in meters) to move hand backwards along approach vector. + diff --git a/msg/TableGraspPose.msg b/msg/TableGraspPose.msg new file mode 100644 index 0000000..8d5ccd1 --- /dev/null +++ b/msg/TableGraspPose.msg @@ -0,0 +1,4 @@ +# Just the pose of the hand, no DOFs or quality metrics needed: +geometry_msgs/Pose pose +# Whether pose puts hand in collision: +bool is_valid diff --git a/msg/TableGraspPoseArray.msg b/msg/TableGraspPoseArray.msg new file mode 100644 index 0000000..c5e24d3 --- /dev/null +++ b/msg/TableGraspPoseArray.msg @@ -0,0 +1,2 @@ +# TODO is this member name too inscrutable? +TableGraspPose[] tgp diff --git a/src/graspit_interface.cpp b/src/graspit_interface.cpp index f168fc7..2638bdd 100644 --- a/src/graspit_interface.cpp +++ b/src/graspit_interface.cpp @@ -38,7 +38,7 @@ int GraspitInterface::init(int argc, char** argv) autoGrasp_srv = nh->advertiseService("autoGrasp", &GraspitInterface::autoGraspCB, this); autoOpen_srv = nh->advertiseService("autoOpen", &GraspitInterface::autoOpenCB, this); - forceRobotDOF_srv = nh->advertiseService("forceRobotDof", &GraspitInterface::forceRobotDOFCB, this); + forceRobotDOF_srv = nh->advertiseService("forceRobotDOF", &GraspitInterface::forceRobotDOFCB, this); moveDOFToContacts_srv = nh->advertiseService("moveDOFToContacts", &GraspitInterface::moveDOFToContactsCB, this); setRobotDesiredDOF_srv = nh->advertiseService("setRobotDesiredDOF", &GraspitInterface::setRobotDesiredDOFCB, this); @@ -59,8 +59,11 @@ int GraspitInterface::init(int argc, char** argv) findInitialContact_srv = nh->advertiseService("findInitialContact", &GraspitInterface::findInitialContactCB, this); dynamicAutoGraspComplete_srv= nh->advertiseService("dynamicAutoGraspComplete", &GraspitInterface::dynamicAutoGraspCompleteCB, this); + findTableGrasps_srv= nh->advertiseService("findTableGrasps", &GraspitInterface::findTableGraspsCB, this); + + plan_grasps_as = new actionlib::SimpleActionServer(*nh, "planGrasps", - boost::bind(&GraspitInterface::PlanGraspsCB, this, _1), false); + boost::bind(&GraspitInterface::PlanGraspsCB, this, _1), false); plan_grasps_as->start(); firstTimeInMainLoop = true; @@ -91,7 +94,7 @@ int GraspitInterface::mainLoop() } bool GraspitInterface::getRobotCB(graspit_interface::GetRobot::Request &request, - graspit_interface::GetRobot::Response &response) + graspit_interface::GetRobot::Response &response) { if (graspitCore->getWorld()->getNumRobots() <= request.id) { response.result = response.RESULT_INVALID_ID; @@ -126,30 +129,11 @@ bool GraspitInterface::getRobotCB(graspit_interface::GetRobot::Request &request, return true; } - bool GraspitInterface::getGraspableBodyCB(graspit_interface::GetGraspableBody::Request &request, - graspit_interface::GetGraspableBody::Response &response) - { - if (graspitCore->getWorld()->getNumGB() <= request.id) { - response.result = response.RESULT_INVALID_ID; - return true; - } else { - GraspableBody *b = graspitCore->getWorld()->getGB(request.id); - transf t = b->getTran(); - - geometry_msgs::Pose pose = geometry_msgs::Pose(); - - pose.position.x = t.translation().x() / 1000.0; - pose.position.y = t.translation().y() / 1000.0;; - pose.position.z = t.translation().z() / 1000.0;; - pose.orientation.w = t.rotation().w; - pose.orientation.x = t.rotation().x; - pose.orientation.y = t.rotation().y; - pose.orientation.z = t.rotation().z; - - response.graspable_body.pose = pose; - response.graspable_body.element_name = b->getName().toStdString(); - return true; - } +bool GraspitInterface::getGraspableBodyCB(graspit_interface::GetGraspableBody::Request &request, + graspit_interface::GetGraspableBody::Response &response) +{ + if (graspitCore->getWorld()->getNumGB() <= request.id) { + response.result = response.RESULT_INVALID_ID; return true; } else { GraspableBody *b = graspitCore->getWorld()->getGB(request.id); @@ -165,41 +149,18 @@ bool GraspitInterface::getRobotCB(graspit_interface::GetRobot::Request &request, pose.orientation.y = t.rotation().y; pose.orientation.z = t.rotation().z; - bool GraspitInterface::getBodyCB(graspit_interface::GetBody::Request &request, - graspit_interface::GetBody::Response &response) - { - if (graspitCore->getWorld()->getNumBodies() <= request.id) { - response.result = response.RESULT_INVALID_ID; - return true; - } else { - Body *b = graspitCore->getWorld()->getBody(request.id); - transf t = b->getTran(); - - geometry_msgs::Pose pose = geometry_msgs::Pose(); - - pose.position.x = t.translation().x() / 1000.0; - pose.position.y = t.translation().y() / 1000.0;; - pose.position.z = t.translation().z() / 1000.0;; - pose.orientation.w = t.rotation().w; - pose.orientation.x = t.rotation().x; - pose.orientation.y = t.rotation().y; - pose.orientation.z = t.rotation().z; - - response.body.pose = pose; - response.body.element_name = b->getName().toStdString(); - - return true; - } + response.graspable_body.pose = pose; + response.graspable_body.element_name = b->getName().toStdString(); return true; } + return true; +} - bool GraspitInterface::getRobotsCB(graspit_interface::GetRobots::Request &request, - graspit_interface::GetRobots::Response &response) - { - for (int i=0; i < graspitCore->getWorld()->getNumRobots(); i++) - { - response.ids.push_back(i); - } +bool GraspitInterface::getBodyCB(graspit_interface::GetBody::Request &request, + graspit_interface::GetBody::Response &response) +{ + if (graspitCore->getWorld()->getNumBodies() <= request.id) { + response.result = response.RESULT_INVALID_ID; return true; } else { Body *b = graspitCore->getWorld()->getBody(request.id); @@ -216,78 +177,70 @@ bool GraspitInterface::getRobotCB(graspit_interface::GetRobot::Request &request, pose.orientation.z = t.rotation().z; response.body.pose = pose; + response.body.element_name = b->getName().toStdString(); - bool GraspitInterface::getGraspableBodiesCB(graspit_interface::GetGraspableBodies::Request &request, - graspit_interface::GetGraspableBodies::Response &response) - { - for (int i=0; i < graspitCore->getWorld()->getNumGB(); i++) - { - response.ids.push_back(i); - } return true; } + return true; +} - bool GraspitInterface::getBodiesCB(graspit_interface::GetBodies::Request &request, - graspit_interface::GetBodies::Response &response) +bool GraspitInterface::getRobotsCB(graspit_interface::GetRobots::Request &request, + graspit_interface::GetRobots::Response &response) +{ + for (int i=0; i < graspitCore->getWorld()->getNumRobots(); i++) { - for (int i=0; i < graspitCore->getWorld()->getNumBodies(); i++) - { - response.ids.push_back(i); - QString name = graspitCore->getWorld()->getBody(i)->getName(); - response.element_names.push_back(name.toStdString()); - } - return true; + response.ids.push_back(i); } + return true; +} - bool GraspitInterface::setRobotPoseCB(graspit_interface::SetRobotPose::Request &request, - graspit_interface::SetRobotPose::Response &response) +bool GraspitInterface::getGraspableBodiesCB(graspit_interface::GetGraspableBodies::Request &request, + graspit_interface::GetGraspableBodies::Response &response) +{ + for (int i=0; i < graspitCore->getWorld()->getNumGB(); i++) { - if (graspitCore->getWorld()->getNumRobots() <= request.id) { - response.result = response.RESULT_INVALID_ID; - return true; - } else { - vec3 newTranslation(request.pose.position.x * 1000.0, - request.pose.position.y * 1000.0, - request.pose.position.z * 1000.0); - - Quaternion newRotation(request.pose.orientation.w, - request.pose.orientation.x, - request.pose.orientation.y, - request.pose.orientation.z); - - transf newTransform(newRotation, newTranslation); - - graspitCore->getWorld()->getRobot(request.id)->setTran(newTransform); - return true; - } + response.ids.push_back(i); } + return true; +} - bool GraspitInterface::setGraspableBodyPoseCB(graspit_interface::SetGraspableBodyPose::Request &request, - graspit_interface::SetGraspableBodyPose::Response &response) +bool GraspitInterface::getBodiesCB(graspit_interface::GetBodies::Request &request, + graspit_interface::GetBodies::Response &response) +{ + for (int i=0; i < graspitCore->getWorld()->getNumBodies(); i++) { - if (graspitCore->getWorld()->getNumGB() <= request.id) { - response.result = response.RESULT_INVALID_ID; - return true; - } else { + response.ids.push_back(i); + QString name = graspitCore->getWorld()->getBody(i)->getName(); + response.element_names.push_back(name.toStdString()); + } + return true; +} - vec3 newTranslation(request.pose.position.x * 1000.0, - request.pose.position.y * 1000.0, - request.pose.position.z * 1000.0); +bool GraspitInterface::setRobotPoseCB(graspit_interface::SetRobotPose::Request &request, + graspit_interface::SetRobotPose::Response &response) +{ + if (graspitCore->getWorld()->getNumRobots() <= request.id) { + response.result = response.RESULT_INVALID_ID; + return true; + } else { + vec3 newTranslation(request.pose.position.x * 1000.0, + request.pose.position.y * 1000.0, + request.pose.position.z * 1000.0); - Quaternion newRotation(request.pose.orientation.w, - request.pose.orientation.x, - request.pose.orientation.y, - request.pose.orientation.z); + Quaternion newRotation(request.pose.orientation.w, + request.pose.orientation.x, + request.pose.orientation.y, + request.pose.orientation.z); - transf newTransform(newRotation, newTranslation); + transf newTransform(newRotation, newTranslation); - graspitCore->getWorld()->getGB(request.id)->setTran(newTransform); - return true; - } + graspitCore->getWorld()->getRobot(request.id)->setTran(newTransform); + return true; } +} bool GraspitInterface::setGraspableBodyPoseCB(graspit_interface::SetGraspableBodyPose::Request &request, - graspit_interface::SetGraspableBodyPose::Response &response) + graspit_interface::SetGraspableBodyPose::Response &response) { if (graspitCore->getWorld()->getNumGB() <= request.id) { response.result = response.RESULT_INVALID_ID; @@ -295,13 +248,13 @@ bool GraspitInterface::setGraspableBodyPoseCB(graspit_interface::SetGraspableBod } else { vec3 newTranslation(request.pose.position.x * 1000.0, - request.pose.position.y * 1000.0, - request.pose.position.z * 1000.0); + request.pose.position.y * 1000.0, + request.pose.position.z * 1000.0); Quaternion newRotation(request.pose.orientation.w, - request.pose.orientation.x, - request.pose.orientation.y, - request.pose.orientation.z); + request.pose.orientation.x, + request.pose.orientation.y, + request.pose.orientation.z); transf newTransform(newRotation, newTranslation); @@ -311,7 +264,7 @@ bool GraspitInterface::setGraspableBodyPoseCB(graspit_interface::SetGraspableBod } bool GraspitInterface::setBodyPoseCB(graspit_interface::SetBodyPose::Request &request, - graspit_interface::SetBodyPose::Response &response) + graspit_interface::SetBodyPose::Response &response) { if (graspitCore->getWorld()->getNumBodies() <= request.id) { response.result = response.RESULT_INVALID_ID; @@ -319,13 +272,13 @@ bool GraspitInterface::setBodyPoseCB(graspit_interface::SetBodyPose::Request &re } else { vec3 newTranslation(request.pose.position.x * 1000.0, - request.pose.position.y * 1000.0, - request.pose.position.z * 1000.0); + request.pose.position.y * 1000.0, + request.pose.position.z * 1000.0); Quaternion newRotation(request.pose.orientation.w, - request.pose.orientation.x, - request.pose.orientation.y, - request.pose.orientation.z); + request.pose.orientation.x, + request.pose.orientation.y, + request.pose.orientation.z); transf newTransform(newRotation, newTranslation); @@ -335,14 +288,14 @@ bool GraspitInterface::setBodyPoseCB(graspit_interface::SetBodyPose::Request &re } bool GraspitInterface::getDynamicsCB(graspit_interface::GetDynamics::Request &request, - graspit_interface::GetDynamics::Response &response) + graspit_interface::GetDynamics::Response &response) { response.dynamicsEnabled = graspitCore->getWorld()->dynamicsAreOn(); return true; } bool GraspitInterface::setDynamicsCB(graspit_interface::SetDynamics::Request &request, - graspit_interface::SetDynamics::Response &response) + graspit_interface::SetDynamics::Response &response) { if(request.enableDynamics && (!graspitCore->getWorld()->dynamicsAreOn())) { @@ -357,7 +310,7 @@ bool GraspitInterface::setDynamicsCB(graspit_interface::SetDynamics::Request &re } bool GraspitInterface::autoGraspCB(graspit_interface::AutoGrasp::Request &request, - graspit_interface::AutoGrasp::Response &response) + graspit_interface::AutoGrasp::Response &response) { if (graspitCore->getWorld()->getNumRobots() <= request.id) { response.result = response.RESULT_INVALID_ID; @@ -370,7 +323,7 @@ bool GraspitInterface::autoGraspCB(graspit_interface::AutoGrasp::Request &reques } bool GraspitInterface::autoOpenCB(graspit_interface::AutoOpen::Request &request, - graspit_interface::AutoOpen::Response &response) + graspit_interface::AutoOpen::Response &response) { if (graspitCore->getWorld()->getNumRobots() <= request.id) { response.result = response.RESULT_INVALID_ID; @@ -383,7 +336,7 @@ bool GraspitInterface::autoOpenCB(graspit_interface::AutoOpen::Request &request, } bool GraspitInterface::forceRobotDOFCB(graspit_interface::ForceRobotDOF::Request &request, - graspit_interface::ForceRobotDOF::Response &response) + graspit_interface::ForceRobotDOF::Response &response) { if (graspitCore->getWorld()->getNumRobots() <= request.id) { response.result = response.RESULT_INVALID_ID; @@ -392,14 +345,25 @@ bool GraspitInterface::forceRobotDOFCB(graspit_interface::ForceRobotDOF::Request response.result = response.RESULT_DYNAMICS_MODE_ENABLED; return true; } else { - graspitCore->getWorld()->getHand(request.id)->forceDOFVals(request.dofs.data()); + // Check that desired values are within range. + // If outside valid range, cap at min/max. + Hand *hand = graspitCore->getWorld()->getHand(request.id); + double * dof = request.dofs.data(); + for (int d=0; dgetNumDOF(); d++) { + if (dof[d] < hand->getDOF(d)->getMin() || + dof[d] > hand->getDOF(d)->getMax()) { + ROS_WARN("Desired value %f is out of range for DOF %d. (min: %f, max:%f).\n\tCapped value at max/min.", + dof[d], d, hand->getDOF(d)->getMin(),hand->getDOF(d)->getMax()); + } + } + hand->forceDOFVals(request.dofs.data()); response.result = response.RESULT_SUCCESS; return true; } } bool GraspitInterface::moveDOFToContactsCB(graspit_interface::MoveDOFToContacts::Request &request, - graspit_interface::MoveDOFToContacts::Response &response) + graspit_interface::MoveDOFToContacts::Response &response) { if (graspitCore->getWorld()->getNumRobots() <= request.id) { response.result = response.RESULT_INVALID_ID; @@ -415,7 +379,7 @@ bool GraspitInterface::moveDOFToContactsCB(graspit_interface::MoveDOFToContacts: } bool GraspitInterface::setRobotDesiredDOFCB(graspit_interface::SetRobotDesiredDOF::Request &request, - graspit_interface::SetRobotDesiredDOF::Response &response) + graspit_interface::SetRobotDesiredDOF::Response &response) { if (graspitCore->getWorld()->getNumRobots() <= request.id) { response.result = response.RESULT_INVALID_ID; @@ -424,24 +388,36 @@ bool GraspitInterface::setRobotDesiredDOFCB(graspit_interface::SetRobotDesiredDO response.result = response.RESULT_DYNAMICS_MODE_DISABLED; return true; } else { - for(int i=0; i < graspitCore->getWorld()->getHand(request.id)->getNumDOF(); i++) { - graspitCore->getWorld()->getHand(request.id)->getDOF(i)->setDesiredVelocity(request.dof_velocities.data()[i]); + Hand *hand = graspitCore->getWorld()->getHand(request.id); + for(int i=0; i < hand->getNumDOF(); i++) { + hand->getDOF(i)->setDesiredVelocity(request.dof_velocities.data()[i]); } - graspitCore->getWorld()->getHand(request.id)->setDesiredDOFVals(request.dofs.data()); + // Check that desired values are within range. + // If outside valid range, cap at min/max. + double * dof = request.dofs.data(); + for (int d=0; dgetNumDOF(); d++) { + if (dof[d] < hand->getDOF(d)->getMin() || + dof[d] > hand->getDOF(d)->getMax()) { + ROS_WARN("Desired value %f is out of range for DOF %d. (min: %f, max:%f).\n\tCapped value at max/min.", + dof[d], d, hand->getDOF(d)->getMin(),hand->getDOF(d)->getMax()); + } + } + + hand->setDesiredDOFVals(request.dofs.data()); response.result = response.RESULT_SUCCESS; return true; } } bool GraspitInterface::importRobotCB(graspit_interface::ImportRobot::Request &request, - graspit_interface::ImportRobot::Response &response) + graspit_interface::ImportRobot::Response &response) { QString filename = QString(getenv("GRASPIT"))+ - QString("/models/robots/") + - QString(request.filename.data()) + - QString("/") + - QString(request.filename.data()) + - QString(".xml"); + QString("/models/robots/") + + QString(request.filename.data()) + + QString("/") + + QString(request.filename.data()) + + QString(".xml"); ROS_INFO("Loading %s",filename.toStdString().c_str()); @@ -450,16 +426,18 @@ bool GraspitInterface::importRobotCB(graspit_interface::ImportRobot::Request &re response.result = response.RESULT_FAILURE; return true; } + // Identifier for this body. + response.element_name = r->getName().toStdString(); return true; } bool GraspitInterface::importObstacleCB(graspit_interface::ImportObstacle::Request &request, - graspit_interface::ImportObstacle::Response &response) + graspit_interface::ImportObstacle::Response &response) { QString filename = QString(getenv("GRASPIT"))+ - QString("/models/obstacles/") + - QString(request.filename.data()) + - QString(".xml"); + QString("/models/obstacles/") + + QString(request.filename.data()) + + QString(".xml"); ROS_INFO("Loading %s", filename.toStdString().c_str()); @@ -468,16 +446,18 @@ bool GraspitInterface::importObstacleCB(graspit_interface::ImportObstacle::Reque response.result = response.RESULT_FAILURE; return true; } + // Identifier for this body. + response.element_name = b->getName().toStdString(); return true; } bool GraspitInterface::importGraspableBodyCB(graspit_interface::ImportGraspableBody::Request &request, - graspit_interface::ImportGraspableBody::Response &response) + graspit_interface::ImportGraspableBody::Response &response) { QString filename = QString(getenv("GRASPIT"))+ - QString("/models/objects/") + - QString(request.filename.data()) + - QString(".xml"); + QString("/models/objects/") + + QString(request.filename.data()) + + QString(".xml"); ROS_INFO("Loading %s",filename.toStdString().c_str()); Body * b = graspitCore->getWorld()->importBody(QString("GraspableBody"),filename); @@ -485,17 +465,19 @@ bool GraspitInterface::importGraspableBodyCB(graspit_interface::ImportGraspableB response.result = response.RESULT_FAILURE; return true; } + + // Identifier for this body. + response.element_name = b->getName().toStdString(); return true; } - bool GraspitInterface::loadWorldCB(graspit_interface::LoadWorld::Request &request, - graspit_interface::LoadWorld::Response &response) + graspit_interface::LoadWorld::Response &response) { QString filename = QString(getenv("GRASPIT"))+ - QString("/worlds/") + - QString(request.filename.data()) + - QString(".xml"); + QString("/worlds/") + + QString(request.filename.data()) + + QString(".xml"); ROS_INFO("Loading World: %s",filename.toStdString().c_str()); int result = graspitCore->getWorld()->load(filename); @@ -507,12 +489,12 @@ bool GraspitInterface::loadWorldCB(graspit_interface::LoadWorld::Request &reques } bool GraspitInterface::saveWorldCB(graspit_interface::SaveWorld::Request &request, - graspit_interface::SaveWorld::Response &response) + graspit_interface::SaveWorld::Response &response) { QString filename = QString(getenv("GRASPIT"))+ - QString("/worlds/") + - QString(request.filename.data()) + - QString(".xml"); + QString("/worlds/") + + QString(request.filename.data()) + + QString(".xml"); ROS_INFO("Saving World: %s",filename.toStdString().c_str()); int result = graspitCore->getWorld()->save(filename); @@ -524,7 +506,7 @@ bool GraspitInterface::saveWorldCB(graspit_interface::SaveWorld::Request &reques } bool GraspitInterface::clearWorldCB(graspit_interface::ClearWorld::Request &request, - graspit_interface::ClearWorld::Response &response) + graspit_interface::ClearWorld::Response &response) { ROS_INFO("Emptying World"); graspitCore->emptyWorld(); @@ -532,12 +514,12 @@ bool GraspitInterface::clearWorldCB(graspit_interface::ClearWorld::Request &requ } bool GraspitInterface::saveImageCB(graspit_interface::SaveImage::Request &request, - graspit_interface::SaveImage::Response &response) + graspit_interface::SaveImage::Response &response) { QString filename = QString(getenv("GRASPIT"))+ - QString("/images/") + - QString(request.filename.data()) + - QString(".jpg"); + QString("/images/") + + QString(request.filename.data()) + + QString(".jpg"); ROS_INFO("Saving Image: %s",filename.toStdString().c_str()); graspitCore->getIVmgr()->saveImage(filename); @@ -545,7 +527,7 @@ bool GraspitInterface::saveImageCB(graspit_interface::SaveImage::Request &reques } bool GraspitInterface::toggleAllCollisionsCB(graspit_interface::ToggleAllCollisions::Request &request, - graspit_interface::ToggleAllCollisions::Response &response) + graspit_interface::ToggleAllCollisions::Response &response) { graspitCore->getWorld()->toggleAllCollisions(request.enableCollisions); if(request.enableCollisions) @@ -560,7 +542,7 @@ bool GraspitInterface::toggleAllCollisionsCB(graspit_interface::ToggleAllCollisi } bool GraspitInterface::computeQualityCB(graspit_interface::ComputeQuality::Request &request, - graspit_interface::ComputeQuality::Response &response) + graspit_interface::ComputeQuality::Response &response) { CollisionReport colReport; @@ -595,7 +577,7 @@ bool GraspitInterface::computeQualityCB(graspit_interface::ComputeQuality::Reque } bool GraspitInterface::approachToContactCB(graspit_interface::ApproachToContact::Request &request, - graspit_interface::ApproachToContact::Response &response) + graspit_interface::ApproachToContact::Response &response) { Hand *mHand =graspitCore->getWorld()->getHand(request.id); if (mHand==NULL) @@ -890,4 +872,221 @@ void GraspitInterface::runPlannerInMainThread() } } + +bool GraspitInterface::findTableGraspsCB(graspit_interface::FindTableGrasps::Request &request, + graspit_interface::FindTableGrasps::Response &response) +{ + /* Given grasps for an body, places the body in a bunch of random poses + * on a table, and finds the valid grasps for each pose. + */ + ROS_INFO("Emptying World"); + graspitCore->emptyWorld(); + World *world = graspitCore->getWorld(); + + /* Load robot. Robot must be a hand. */ + QString filename = QString(getenv("GRASPIT"))+ + QString("/models/robots/") + + QString(request.robot_name.data()) + + QString("/") + + QString(request.robot_name.data()) + + QString(".xml"); + + ROS_INFO("Loading %s",filename.toStdString().c_str()); + + Hand * hand; + Robot * robot = world->importRobot(filename); + if(robot == NULL){ + response.result = response.RESULT_FAILURE; + return true; + } + else if (!robot->inherits("Hand")) { + response.result = response.RESULT_FAILURE; + ROS_ERROR("Specified robot is not a Hand!"); + return true; + } + else { + hand = (Hand *)robot; + } + + /* Load body: */ + filename = QString(getenv("GRASPIT"))+ + QString("/models/objects/") + + QString(request.body_name.data()) + + QString(".xml"); + + ROS_INFO("Loading body from %s",filename.toStdString().c_str()); + Body * body = world->importBody(QString("GraspableBody"),filename); + if(body == NULL){ + response.result = response.RESULT_FAILURE; + return true; + } + + /* Load table: */ + filename = QString(getenv("GRASPIT"))+ + QString("/models/obstacles/table.xml"); + + ROS_INFO("Loading table from %s",filename.toStdString().c_str()); + Body * table = world->importBody(QString("Body"),filename); + if(table == NULL){ + response.result = response.RESULT_FAILURE; + return true; + } + + /* Main loop. Based on graspit's tableCheckTask.cpp: */ + ROS_INFO("Finshed loading bodies. Checking grasps for each pose:"); + for (int i=0; igetTran().translation()); + body->setTran(bodyTransform); + + /* Move table into position under the body. */ + // NOTE table's origin is at the midpoint of one of its edges. + // So, translate it to place its CENTER roughly over world origin. + // (These values are for graspit's included "table" model) + const double TABLE_TRANS_X = 2.6; const double TABLE_TRANS_Y = -570.; + // start way under the body + table->setTran( transf( Quaternion::IDENTITY, vec3(TABLE_TRANS_X, TABLE_TRANS_Y, -200.0) ) ); + //and move up until it touches the body + transf tr( Quaternion::IDENTITY, vec3(TABLE_TRANS_X, TABLE_TRANS_Y, 1000.0) ); + + world->toggleCollisions(false, hand, table); + table->moveTo( tr, 5.0, M_PI/36.0 ); + world->toggleCollisions(true, hand, table); + + + /* Check collision for each grasp. */ + ROS_INFO("\tChecking collisions for each grasp."); + // Result for this body pose: + graspit_interface::TableGraspPoseArray transformed_hand_poses; + int num_good_grasps = 0; + for (int j=0; jsetTran(handTransform); + graspit_interface::TableGraspPose tgp; + tgp.pose = graspitPoseToRosPose(handTransform); + + // Check for collision: + //Grasp valid IFF grasp AND pregrasp don't put hand in collision with table: + double distance = world->getDist(hand, table); + + if (distance <= 0) { + tgp.is_valid = false; + } + else { + // Check pregrasp hand DOFs: + // ERROR OUT if invalid # of DOFs for this hand + double retreat_by = 1000 * request.pregrasp.retreat_by; + ROS_WARN("%f", retreat_by); + std::vector dofs = request.pregrasp.open_dofs_by; + if (hand->getNumDOF() != dofs.size()) { + ROS_ERROR("Invalid # DOFs for this hand! (Expected %d, got %d)", + hand->getNumDOF(), int(dofs.size())); + response.result = response.RESULT_FAILURE; + return false; + } + else if (!preGraspCheck(hand, body, dofs, retreat_by)) { + tgp.is_valid = false; + } + else { + tgp.is_valid = true; + num_good_grasps++; + } + } + // Save result for this hand pose: + transformed_hand_poses.tgp.push_back(tgp); + } + ROS_INFO("\tFound %d valid grasps!", num_good_grasps ); + num_good_grasps=0; + response.hand_poses.push_back(transformed_hand_poses); + geometry_msgs::Pose body_pose = graspitPoseToRosPose(body->getTran()); + response.body_poses.push_back(body_pose); + geometry_msgs::Pose table_pose = graspitPoseToRosPose(table->getTran()); + response.table_poses.push_back(table_pose); + } + ROS_INFO("Finished checking grasps for all body poses!"); + response.result = response.RESULT_SUCCESS; + return true; +} + + +// From graspit's preGraspCheckTask.cpp: +// body is a ptr to the Body grasped by hand. +bool GraspitInterface::preGraspCheck(Hand *hand, Body *body, std::vector open_dofs_by, double retreat_dist) +{ + //TODO DOF check seems broken! Always fails to open gripper to specified amount. + // Mb a problem with the open_dofs_by params? + // Maybe DOFs should be passed as a parameter, rather than use open_dofs_by. + // Just skipping it for now-- it shouldn't be a problem in most cases: + /* + // -- Check pregrasp DOFS -- + // Increment DOFs and set step size. + double dofs[hand->getNumDOF()]; + std::vector stepSize(hand->getNumDOF(), 0.0); + hand->getDOFVals(dofs); + for (int d=0; d < hand->getNumDOF(); d++) { + dofs[d] += open_dofs_by[d]; + if (dofs[d] < hand->getDOF(d)->getMin()) { + dofs[d] = hand->getDOF(d)->getMin(); + } + if (dofs[d] > hand->getDOF(d)->getMax()) { + dofs[d] = hand->getDOF(d)->getMax(); + } + stepSize[d] = M_PI/36.0; // TODO what's this magic value? + } + ROS_INFO("In preGraspCheck:\n\tcalling moveDOFToContacts (to open fingers)."); + hand->moveDOFToContacts(&dofs[0], &stepSize[0], true, false); + ROS_INFO("\t...finished moveDOFToContacts.\n\tChecking success of move..."); + //check if move has succeeded: + for (int d=0; dgetNumDOF(); d++) { + if ( fabs( dofs[d] - hand->getDOF(d)->getVal() ) > 1.0e-5) { + ROS_INFO(" trying to open to %f", dofs[d]); + ROS_INFO(" only made it to %f", hand->getDOF(d)->getVal()); + ROS_INFO(" open gripper fails"); + return false; + } + } + ROS_INFO("\tfinished checking move.\n\tcalling approachToContact (to move hand backwards from grasp pose)."); + */ + + // -- Check pregrasp hand approach -- + // Disable collisions between hand and body-- only concerned w/ the table + graspitCore->getWorld()->toggleCollisions(false, hand, body); + //retreat along approach direction + if (hand->approachToContact(-1*retreat_dist, false)) { + //we have hit something + ROS_INFO(" retreat failed! Done!"); + graspitCore->getWorld()->toggleCollisions(true, hand, body); + return false; + } + else { + graspitCore->getWorld()->toggleCollisions(true, hand, body); + ROS_INFO(" retreat successful! Done!"); + return true; + } +} } diff --git a/srv/FindTableGrasps.srv b/srv/FindTableGrasps.srv new file mode 100644 index 0000000..a5ef14c --- /dev/null +++ b/srv/FindTableGrasps.srv @@ -0,0 +1,24 @@ +# Request: +Grasp[] grasps +string robot_name +PregraspParams pregrasp +string body_name + +uint8 num_poses +--- +# Result: + +#2D array-- indices (in grasps) of good grasps for each body pose: +#GraspIndices[] grasp_ids + +#Hand poses from input grasps, transformed to match each new body pose. +# Each Pose paired with a bool indicating validity of the pose: +TableGraspPoseArray[] hand_poses +geometry_msgs/Pose[] body_poses +geometry_msgs/Pose[] table_poses + +uint8 RESULT_SUCCESS = 0 +uint8 RESULT_FAILURE = 1 + +uint8 result + diff --git a/srv/ImportGraspableBody.srv b/srv/ImportGraspableBody.srv index 0fce9f9..1d16aa9 100644 --- a/srv/ImportGraspableBody.srv +++ b/srv/ImportGraspableBody.srv @@ -1,6 +1,7 @@ string filename --- +string element_name #UID for the body's world element. uint8 RESULT_SUCCESS = 0 uint8 RESULT_FAILURE = 1 -uint8 result \ No newline at end of file +uint8 result diff --git a/srv/ImportObstacle.srv b/srv/ImportObstacle.srv index 0fce9f9..1d16aa9 100644 --- a/srv/ImportObstacle.srv +++ b/srv/ImportObstacle.srv @@ -1,6 +1,7 @@ string filename --- +string element_name #UID for the body's world element. uint8 RESULT_SUCCESS = 0 uint8 RESULT_FAILURE = 1 -uint8 result \ No newline at end of file +uint8 result diff --git a/srv/ImportRobot.srv b/srv/ImportRobot.srv index 0fce9f9..1d16aa9 100644 --- a/srv/ImportRobot.srv +++ b/srv/ImportRobot.srv @@ -1,6 +1,7 @@ string filename --- +string element_name #UID for the body's world element. uint8 RESULT_SUCCESS = 0 uint8 RESULT_FAILURE = 1 -uint8 result \ No newline at end of file +uint8 result From 6f07deb8c4207591f9fcdec5d2f8e27c8fa606a0 Mon Sep 17 00:00:00 2001 From: Jack Rasiel Date: Mon, 15 Aug 2016 15:45:14 -0400 Subject: [PATCH 3/6] Added more fields to getRobot/Body, & moved duplicated code to function. --- CMakeLists.txt | 1 + include/graspit_interface.h | 13 ++- msg/Body.msg | 17 +++- msg/Robot.msg | 6 +- src/graspit_interface.cpp | 171 ++++++++++++++++-------------------- 5 files changed, 111 insertions(+), 97 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e1287d2..354d216 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,6 +65,7 @@ find_package(Qt4 COMPONENTS QtCore REQUIRED) TableGraspPose.msg TableGraspPoseArray.msg PregraspParams.msg + Contact.msg ) ## Generate services in the 'srv' folder diff --git a/include/graspit_interface.h b/include/graspit_interface.h index 32ef1ab..38d8b0d 100644 --- a/include/graspit_interface.h +++ b/include/graspit_interface.h @@ -14,6 +14,7 @@ #include #include #include +#include // Service includes #include @@ -206,8 +207,8 @@ class GraspitInterface : public QObject, public Plugin graspit_interface::FindTableGrasps::Response &response); // fn used by ^^^. Checks if pregrasp results in collision: bool preGraspCheck(Hand *hand, Body *object, std::vector open_dofs_by, double retreat_dist); - // convenience function: - inline geometry_msgs::Pose graspitPoseToRosPose(transf pose) { + // Convenience functions for converting between pose types: + inline geometry_msgs::Pose transfToRosMsg(transf pose) { geometry_msgs::Pose ret; ret.position.x = pose.translation().x() / 1000.0; ret.position.y = pose.translation().y() / 1000.0;; @@ -218,6 +219,14 @@ class GraspitInterface : public QObject, public Plugin ret.orientation.z = pose.rotation().z; return ret; } + inline transf rosMsgToTransf(geometry_msgs::Pose pose) { + Quaternion q(pose.orientation.w, pose.orientation.x, + pose.orientation.y, pose.orientation.z); + vec3 p(pose.position.x * 1000.0, pose.position.y * 1000.0, + pose.position.z * 1000.0); + transf ret(q, p); + return ret; + } //ActionServer callbacks diff --git a/msg/Body.msg b/msg/Body.msg index 3cd831b..21fa362 100644 --- a/msg/Body.msg +++ b/msg/Body.msg @@ -1,4 +1,19 @@ std_msgs/Header header -string element_name #UID for the body's world element. +string element_name #UID for the body's world element. geometry_msgs/Pose pose + +# Other properties +int32 material +float64 transparency +float64 youngs_modulus +bool is_dynamic +bool is_elastic + +# Dynamic body properties. -1/None if body is not dynamic: +float64 mass +geometry_msgs/Inertia inertia +#this is DynamicBody()->getCoG and DynamicBody->getInertia() +geometry_msgs/Accel accel +geometry_msgs/Twist velocity +float64 max_radius diff --git a/msg/Robot.msg b/msg/Robot.msg index 99c3ecd..eb46f42 100644 --- a/msg/Robot.msg +++ b/msg/Robot.msg @@ -1,6 +1,10 @@ +# Constants: std_msgs/Header header - string element_name #UID for the body's world element. +geometry_msgs/Pose approach_direction + +# Variables: geometry_msgs/Pose pose sensor_msgs/JointState[] joints float64[] dofs +Contact[] contacts diff --git a/src/graspit_interface.cpp b/src/graspit_interface.cpp index 3195667..d02b456 100644 --- a/src/graspit_interface.cpp +++ b/src/graspit_interface.cpp @@ -101,20 +101,27 @@ bool GraspitInterface::getRobotCB(graspit_interface::GetRobot::Request &request, return true; } else { Robot *r = graspitCore->getWorld()->getRobot(request.id); - transf t = r->getTran(); - geometry_msgs::Pose pose = geometry_msgs::Pose(); - - pose.position.x = t.translation().x() / 1000.0; - pose.position.y = t.translation().y() / 1000.0;; - pose.position.z = t.translation().z() / 1000.0;; - pose.orientation.w = t.rotation().w; - pose.orientation.x = t.rotation().x; - pose.orientation.y = t.rotation().y; - pose.orientation.z = t.rotation().z; - - response.robot.pose = pose; + response.robot.pose = transfToRosMsg(r->getTran()); + response.robot.approach_direction = transfToRosMsg(r->getApproachTran()); + + // Info for all contacts with this robot: + std::list contacts = r->getContacts(); + for (std::list::iterator it = contacts.begin(); + it != contacts.end(); it++) { + graspit_interface::Contact c; + c.body1 = (*it)->getBody1()->getName().toStdString(); + c.body2 = (*it)->getBody2()->getName().toStdString(); + + position p = (*it)->getPosition(); + c.position.x = p.x() * 0.001; + c.position.y = p.y() * 0.001; + c.position.z = p.z() * 0.001; + c.cof = (*it)->getCof(); + response.robot.contacts.push_back(c); + } + // Joint state and DOF information: for (int i=0; i < r->getNumJoints(); i++) { sensor_msgs::JointState robot_joint_state = sensor_msgs::JointState(); response.robot.joints.push_back(robot_joint_state); @@ -137,19 +144,9 @@ bool GraspitInterface::getGraspableBodyCB(graspit_interface::GetGraspableBody::R return true; } else { GraspableBody *b = graspitCore->getWorld()->getGB(request.id); - transf t = b->getTran(); - geometry_msgs::Pose pose = geometry_msgs::Pose(); + response.graspable_body.pose = transfToRosMsg(b->getTran()); - pose.position.x = t.translation().x() / 1000.0; - pose.position.y = t.translation().y() / 1000.0;; - pose.position.z = t.translation().z() / 1000.0;; - pose.orientation.w = t.rotation().w; - pose.orientation.x = t.rotation().x; - pose.orientation.y = t.rotation().y; - pose.orientation.z = t.rotation().z; - - response.graspable_body.pose = pose; response.graspable_body.element_name = b->getName().toStdString(); return true; } @@ -164,22 +161,56 @@ bool GraspitInterface::getBodyCB(graspit_interface::GetBody::Request &request, return true; } else { Body *b = graspitCore->getWorld()->getBody(request.id); - transf t = b->getTran(); - - geometry_msgs::Pose pose = geometry_msgs::Pose(); - - pose.position.x = t.translation().x() / 1000.0; - pose.position.y = t.translation().y() / 1000.0;; - pose.position.z = t.translation().z() / 1000.0;; - pose.orientation.w = t.rotation().w; - pose.orientation.x = t.rotation().x; - pose.orientation.y = t.rotation().y; - pose.orientation.z = t.rotation().z; - - response.body.pose = pose; + // Get Body properties: + response.body.pose = transfToRosMsg(b->getTran()); response.body.element_name = b->getName().toStdString(); - - return true; + response.body.material = b->getMaterial(); + response.body.transparency = b->getTransparency(); + response.body.youngs_modulus = b->getYoungs(); + response.body.is_dynamic = b->isDynamic(); + // Dynamic body properties, if applicable: + if (b->isDynamic()) { + DynamicBody * db = (DynamicBody*) b; + response.body.max_radius = db->getMaxRadius(); + + // ROS inertia msg includes mass, center mass, & inertia tensor: + geometry_msgs::Inertia inert; + // ros mass = kgs. graspit mass = grams: + inert.m = db->getMass() * 0.001; + position cog = db->getCoG(); + inert.com.x = cog.x(); inert.com.y = cog.y(); inert.com.z = cog.z(); + const double * in = db->getInertia(); + // TODO not sure if these line up right: + inert.ixx = in[0]; inert.ixy = in[1]; inert.ixz = in[2]; + inert.iyy = in[4]; inert.iyz = in[5]; inert.izz = in[8]; + response.body.inertia = inert; + + // ROS accel msg: + geometry_msgs::Accel accel; + geometry_msgs::Vector3 lin; + geometry_msgs::Vector3 ang; + const double * ac = db->getAccel(); + // TODO not sure if these line up right: + lin.x = ac[0]; lin.y = ac[1]; lin.z = ac[2]; + ang.x = ac[3]; ang.y = ac[4]; ang.z = ac[5]; + accel.linear = lin; accel.angular = ang; + response.body.accel = accel; + + // ROS vel msg: + geometry_msgs::Twist vel; + const double * v = db->getVelocity(); + // TODO not sure if these line up right: + lin.x = v[0]; lin.y = v[1]; lin.z = v[2]; + ang.x = v[3]; ang.y = v[4]; ang.z = v[5]; + vel.linear = lin; vel.angular = ang; + response.body.velocity = vel; + } + else { + response.body.inertia = geometry_msgs::Inertia(); + response.body.accel = geometry_msgs::Accel(); + response.body.velocity = geometry_msgs::Twist(); + response.body.max_radius = -1; + } } return true; } @@ -223,16 +254,7 @@ bool GraspitInterface::setRobotPoseCB(graspit_interface::SetRobotPose::Request & response.result = response.RESULT_INVALID_ID; return true; } else { - vec3 newTranslation(request.pose.position.x * 1000.0, - request.pose.position.y * 1000.0, - request.pose.position.z * 1000.0); - - Quaternion newRotation(request.pose.orientation.w, - request.pose.orientation.x, - request.pose.orientation.y, - request.pose.orientation.z); - - transf newTransform(newRotation, newTranslation); + transf newTransform = rosMsgToTransf(request.pose); graspitCore->getWorld()->getRobot(request.id)->setTran(newTransform); return true; @@ -246,17 +268,7 @@ bool GraspitInterface::setGraspableBodyPoseCB(graspit_interface::SetGraspableBod response.result = response.RESULT_INVALID_ID; return true; } else { - - vec3 newTranslation(request.pose.position.x * 1000.0, - request.pose.position.y * 1000.0, - request.pose.position.z * 1000.0); - - Quaternion newRotation(request.pose.orientation.w, - request.pose.orientation.x, - request.pose.orientation.y, - request.pose.orientation.z); - - transf newTransform(newRotation, newTranslation); + transf newTransform = rosMsgToTransf(request.pose); graspitCore->getWorld()->getGB(request.id)->setTran(newTransform); return true; @@ -270,17 +282,7 @@ bool GraspitInterface::setBodyPoseCB(graspit_interface::SetBodyPose::Request &re response.result = response.RESULT_INVALID_ID; return true; } else { - - vec3 newTranslation(request.pose.position.x * 1000.0, - request.pose.position.y * 1000.0, - request.pose.position.z * 1000.0); - - Quaternion newRotation(request.pose.orientation.w, - request.pose.orientation.x, - request.pose.orientation.y, - request.pose.orientation.z); - - transf newTransform(newRotation, newTranslation); + transf newTransform = rosMsgToTransf(request.pose); graspitCore->getWorld()->getBody(request.id)->setTran(newTransform); return true; @@ -820,17 +822,10 @@ void GraspitInterface::runPlannerInMainThread() mHand->autoGrasp(false,1.0,false); ROS_INFO("Building Pose"); - geometry_msgs::Pose pose; - transf t = mHand->getTran(); - pose.position.x = t.translation().x() / 1000.0; - pose.position.y = t.translation().y() / 1000.0;; - pose.position.z = t.translation().z() / 1000.0;; - pose.orientation.w = t.rotation().w; - pose.orientation.x = t.rotation().x; - pose.orientation.y = t.rotation().y; - pose.orientation.z = t.rotation().z; graspit_interface::Grasp g; + g.pose = transfToRosMsg(mHand->getTran()); + g.graspable_body_id = goal.graspable_body_id; double dof[mHand->getNumDOF()]; @@ -840,7 +835,6 @@ void GraspitInterface::runPlannerInMainThread() g.dofs.push_back(dof[i]); } - g.pose = pose; mHand->getGrasp()->update(); QualVolume mVolQual( mHand->getGrasp(), ("Volume"),"L1 Norm"); QualEpsilon mEpsQual( mHand->getGrasp(), ("Epsilon"),"L1 Norm"); @@ -978,22 +972,13 @@ bool GraspitInterface::findTableGraspsCB(graspit_interface::FindTableGrasps::Req int num_good_grasps = 0; for (int j=0; jsetTran(handTransform); graspit_interface::TableGraspPose tgp; - tgp.pose = graspitPoseToRosPose(handTransform); + tgp.pose = transfToRosMsg(handTransform); // Check for collision: //Grasp valid IFF grasp AND pregrasp don't put hand in collision with table: @@ -1028,9 +1013,9 @@ bool GraspitInterface::findTableGraspsCB(graspit_interface::FindTableGrasps::Req ROS_INFO("\tFound %d valid grasps!", num_good_grasps ); num_good_grasps=0; response.hand_poses.push_back(transformed_hand_poses); - geometry_msgs::Pose body_pose = graspitPoseToRosPose(body->getTran()); + geometry_msgs::Pose body_pose = transfToRosMsg(body->getTran()); response.body_poses.push_back(body_pose); - geometry_msgs::Pose table_pose = graspitPoseToRosPose(table->getTran()); + geometry_msgs::Pose table_pose = transfToRosMsg(table->getTran()); response.table_poses.push_back(table_pose); } ROS_INFO("Finished checking grasps for all body poses!"); From e283bbdc3010934c58c3911538a1f4325be2615d Mon Sep 17 00:00:00 2001 From: Jack Rasiel Date: Mon, 15 Aug 2016 15:57:07 -0400 Subject: [PATCH 4/6] Removed table grasp code. Also, forgot to add Contact msg. --- CMakeLists.txt | 3 - include/graspit_interface.h | 9 -- msg/Contact.msg | 8 ++ src/graspit_interface.cpp | 211 ------------------------------------ 4 files changed, 8 insertions(+), 223 deletions(-) create mode 100644 msg/Contact.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 354d216..251f3aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,8 +62,6 @@ find_package(Qt4 COMPONENTS QtCore REQUIRED) SearchSpace.msg SearchContact.msg Energy.msg - TableGraspPose.msg - TableGraspPoseArray.msg PregraspParams.msg Contact.msg ) @@ -100,7 +98,6 @@ find_package(Qt4 COMPONENTS QtCore REQUIRED) ApproachToContact.srv FindInitialContact.srv DynamicAutoGraspComplete.srv - FindTableGrasps.srv ) diff --git a/include/graspit_interface.h b/include/graspit_interface.h index 38d8b0d..7a412a8 100644 --- a/include/graspit_interface.h +++ b/include/graspit_interface.h @@ -46,7 +46,6 @@ #include #include #include -#include // ActionServer includes @@ -103,8 +102,6 @@ class GraspitInterface : public QObject, public Plugin ros::ServiceServer dynamicAutoGraspComplete_srv; - ros::ServiceServer findTableGrasps_srv; - // ActionServer declarations actionlib::SimpleActionServer *plan_grasps_as; graspit_interface::PlanGraspsFeedback feedback_; @@ -201,12 +198,6 @@ class GraspitInterface : public QObject, public Plugin bool dynamicAutoGraspCompleteCB(graspit_interface::DynamicAutoGraspComplete::Request &request, graspit_interface::DynamicAutoGraspComplete::Response &response); - // Given grasps for an object, places the object in a bunch of random poses - // on a table, and finds the valid grasps for each pose: - bool findTableGraspsCB(graspit_interface::FindTableGrasps::Request &request, - graspit_interface::FindTableGrasps::Response &response); - // fn used by ^^^. Checks if pregrasp results in collision: - bool preGraspCheck(Hand *hand, Body *object, std::vector open_dofs_by, double retreat_dist); // Convenience functions for converting between pose types: inline geometry_msgs::Pose transfToRosMsg(transf pose) { geometry_msgs::Pose ret; diff --git a/msg/Contact.msg b/msg/Contact.msg new file mode 100644 index 0000000..611b752 --- /dev/null +++ b/msg/Contact.msg @@ -0,0 +1,8 @@ +string body1 +string body2 + +geometry_msgs/Point position # in body1 frame. + +float64 cof # coefficient of friction. + + diff --git a/src/graspit_interface.cpp b/src/graspit_interface.cpp index f33aede..816a0a5 100644 --- a/src/graspit_interface.cpp +++ b/src/graspit_interface.cpp @@ -59,9 +59,6 @@ int GraspitInterface::init(int argc, char** argv) findInitialContact_srv = nh->advertiseService("findInitialContact", &GraspitInterface::findInitialContactCB, this); dynamicAutoGraspComplete_srv= nh->advertiseService("dynamicAutoGraspComplete", &GraspitInterface::dynamicAutoGraspCompleteCB, this); - findTableGrasps_srv= nh->advertiseService("findTableGrasps", &GraspitInterface::findTableGraspsCB, this); - - plan_grasps_as = new actionlib::SimpleActionServer(*nh, "planGrasps", boost::bind(&GraspitInterface::PlanGraspsCB, this, _1), false); plan_grasps_as->start(); @@ -874,212 +871,4 @@ void GraspitInterface::runPlannerInMainThread() } } - -bool GraspitInterface::findTableGraspsCB(graspit_interface::FindTableGrasps::Request &request, - graspit_interface::FindTableGrasps::Response &response) -{ - /* Given grasps for an body, places the body in a bunch of random poses - * on a table, and finds the valid grasps for each pose. - */ - ROS_INFO("Emptying World"); - graspitCore->emptyWorld(); - World *world = graspitCore->getWorld(); - - /* Load robot. Robot must be a hand. */ - QString filename = QString(getenv("GRASPIT"))+ - QString("/models/robots/") + - QString(request.robot_name.data()) + - QString("/") + - QString(request.robot_name.data()) + - QString(".xml"); - - ROS_INFO("Loading %s",filename.toStdString().c_str()); - - Hand * hand; - Robot * robot = world->importRobot(filename); - if(robot == NULL){ - response.result = response.RESULT_FAILURE; - return true; - } - else if (!robot->inherits("Hand")) { - response.result = response.RESULT_FAILURE; - ROS_ERROR("Specified robot is not a Hand!"); - return true; - } - else { - hand = (Hand *)robot; - } - - /* Load body: */ - filename = QString(getenv("GRASPIT"))+ - QString("/models/objects/") + - QString(request.body_name.data()) + - QString(".xml"); - - ROS_INFO("Loading body from %s",filename.toStdString().c_str()); - Body * body = world->importBody(QString("GraspableBody"),filename); - if(body == NULL){ - response.result = response.RESULT_FAILURE; - return true; - } - - /* Load table: */ - filename = QString(getenv("GRASPIT"))+ - QString("/models/obstacles/table.xml"); - - ROS_INFO("Loading table from %s",filename.toStdString().c_str()); - Body * table = world->importBody(QString("Body"),filename); - if(table == NULL){ - response.result = response.RESULT_FAILURE; - return true; - } - - /* Main loop. Based on graspit's tableCheckTask.cpp: */ - ROS_INFO("Finshed loading bodies. Checking grasps for each pose:"); - for (int i=0; igetTran().translation()); - body->setTran(bodyTransform); - - /* Move table into position under the body. */ - // NOTE table's origin is at the midpoint of one of its edges. - // So, translate it to place its CENTER roughly over world origin. - // (These values are for graspit's included "table" model) - const double TABLE_TRANS_X = 2.6; const double TABLE_TRANS_Y = -570.; - // start way under the body - table->setTran( transf( Quaternion::IDENTITY, vec3(TABLE_TRANS_X, TABLE_TRANS_Y, -200.0) ) ); - //and move up until it touches the body - transf tr( Quaternion::IDENTITY, vec3(TABLE_TRANS_X, TABLE_TRANS_Y, 1000.0) ); - - world->toggleCollisions(false, hand, table); - table->moveTo( tr, 5.0, M_PI/36.0 ); - world->toggleCollisions(true, hand, table); - - - /* Check collision for each grasp. */ - ROS_INFO("\tChecking collisions for each grasp."); - // Result for this body pose: - graspit_interface::TableGraspPoseArray transformed_hand_poses; - int num_good_grasps = 0; - for (int j=0; jsetTran(handTransform); - graspit_interface::TableGraspPose tgp; - tgp.pose = transfToRosMsg(handTransform); - - // Check for collision: - //Grasp valid IFF grasp AND pregrasp don't put hand in collision with table: - double distance = world->getDist(hand, table); - - if (distance <= 0) { - tgp.is_valid = false; - } - else { - // Check pregrasp hand DOFs: - // ERROR OUT if invalid # of DOFs for this hand - double retreat_by = 1000 * request.pregrasp.retreat_by; - ROS_WARN("%f", retreat_by); - std::vector dofs = request.pregrasp.open_dofs_by; - if (hand->getNumDOF() != dofs.size()) { - ROS_ERROR("Invalid # DOFs for this hand! (Expected %d, got %d)", - hand->getNumDOF(), int(dofs.size())); - response.result = response.RESULT_FAILURE; - return false; - } - else if (!preGraspCheck(hand, body, dofs, retreat_by)) { - tgp.is_valid = false; - } - else { - tgp.is_valid = true; - num_good_grasps++; - } - } - // Save result for this hand pose: - transformed_hand_poses.tgp.push_back(tgp); - } - ROS_INFO("\tFound %d valid grasps!", num_good_grasps ); - num_good_grasps=0; - response.hand_poses.push_back(transformed_hand_poses); - geometry_msgs::Pose body_pose = transfToRosMsg(body->getTran()); - response.body_poses.push_back(body_pose); - geometry_msgs::Pose table_pose = transfToRosMsg(table->getTran()); - response.table_poses.push_back(table_pose); - } - ROS_INFO("Finished checking grasps for all body poses!"); - response.result = response.RESULT_SUCCESS; - return true; -} - - -// From graspit's preGraspCheckTask.cpp: -// body is a ptr to the Body grasped by hand. -bool GraspitInterface::preGraspCheck(Hand *hand, Body *body, std::vector open_dofs_by, double retreat_dist) -{ - //TODO DOF check seems broken! Always fails to open gripper to specified amount. - // Mb a problem with the open_dofs_by params? - // Maybe DOFs should be passed as a parameter, rather than use open_dofs_by. - // Just skipping it for now-- it shouldn't be a problem in most cases: - /* - // -- Check pregrasp DOFS -- - // Increment DOFs and set step size. - double dofs[hand->getNumDOF()]; - std::vector stepSize(hand->getNumDOF(), 0.0); - hand->getDOFVals(dofs); - for (int d=0; d < hand->getNumDOF(); d++) { - dofs[d] += open_dofs_by[d]; - if (dofs[d] < hand->getDOF(d)->getMin()) { - dofs[d] = hand->getDOF(d)->getMin(); - } - if (dofs[d] > hand->getDOF(d)->getMax()) { - dofs[d] = hand->getDOF(d)->getMax(); - } - stepSize[d] = M_PI/36.0; // TODO what's this magic value? - } - ROS_INFO("In preGraspCheck:\n\tcalling moveDOFToContacts (to open fingers)."); - hand->moveDOFToContacts(&dofs[0], &stepSize[0], true, false); - ROS_INFO("\t...finished moveDOFToContacts.\n\tChecking success of move..."); - //check if move has succeeded: - for (int d=0; dgetNumDOF(); d++) { - if ( fabs( dofs[d] - hand->getDOF(d)->getVal() ) > 1.0e-5) { - ROS_INFO(" trying to open to %f", dofs[d]); - ROS_INFO(" only made it to %f", hand->getDOF(d)->getVal()); - ROS_INFO(" open gripper fails"); - return false; - } - } - ROS_INFO("\tfinished checking move.\n\tcalling approachToContact (to move hand backwards from grasp pose)."); - */ - - // -- Check pregrasp hand approach -- - // Disable collisions between hand and body-- only concerned w/ the table - graspitCore->getWorld()->toggleCollisions(false, hand, body); - //retreat along approach direction - if (hand->approachToContact(-1*retreat_dist, false)) { - //we have hit something - ROS_INFO(" retreat failed! Done!"); - graspitCore->getWorld()->toggleCollisions(true, hand, body); - return false; - } - else { - graspitCore->getWorld()->toggleCollisions(true, hand, body); - ROS_INFO(" retreat successful! Done!"); - return true; - } -} } From 6732158a44842701885e60d1f4de5e0e0c9dbbe3 Mon Sep 17 00:00:00 2001 From: Jack Rasiel Date: Mon, 15 Aug 2016 16:06:24 -0400 Subject: [PATCH 5/6] Removed msg and srv for table grasps. --- msg/TableGraspPose.msg | 4 ---- msg/TableGraspPoseArray.msg | 2 -- srv/FindTableGrasps.srv | 24 ------------------------ 3 files changed, 30 deletions(-) delete mode 100644 msg/TableGraspPose.msg delete mode 100644 msg/TableGraspPoseArray.msg delete mode 100644 srv/FindTableGrasps.srv diff --git a/msg/TableGraspPose.msg b/msg/TableGraspPose.msg deleted file mode 100644 index 8d5ccd1..0000000 --- a/msg/TableGraspPose.msg +++ /dev/null @@ -1,4 +0,0 @@ -# Just the pose of the hand, no DOFs or quality metrics needed: -geometry_msgs/Pose pose -# Whether pose puts hand in collision: -bool is_valid diff --git a/msg/TableGraspPoseArray.msg b/msg/TableGraspPoseArray.msg deleted file mode 100644 index c5e24d3..0000000 --- a/msg/TableGraspPoseArray.msg +++ /dev/null @@ -1,2 +0,0 @@ -# TODO is this member name too inscrutable? -TableGraspPose[] tgp diff --git a/srv/FindTableGrasps.srv b/srv/FindTableGrasps.srv deleted file mode 100644 index a5ef14c..0000000 --- a/srv/FindTableGrasps.srv +++ /dev/null @@ -1,24 +0,0 @@ -# Request: -Grasp[] grasps -string robot_name -PregraspParams pregrasp -string body_name - -uint8 num_poses ---- -# Result: - -#2D array-- indices (in grasps) of good grasps for each body pose: -#GraspIndices[] grasp_ids - -#Hand poses from input grasps, transformed to match each new body pose. -# Each Pose paired with a bool indicating validity of the pose: -TableGraspPoseArray[] hand_poses -geometry_msgs/Pose[] body_poses -geometry_msgs/Pose[] table_poses - -uint8 RESULT_SUCCESS = 0 -uint8 RESULT_FAILURE = 1 - -uint8 result - From 6c3760d02b98d14cca88d047ad0030c79aa2a077 Mon Sep 17 00:00:00 2001 From: Jack Rasiel Date: Wed, 17 Aug 2016 12:39:14 -0400 Subject: [PATCH 6/6] When setting DOFs, cap within valid range. --- src/graspit_interface.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/graspit_interface.cpp b/src/graspit_interface.cpp index 816a0a5..dc6043e 100644 --- a/src/graspit_interface.cpp +++ b/src/graspit_interface.cpp @@ -349,13 +349,18 @@ bool GraspitInterface::forceRobotDOFCB(graspit_interface::ForceRobotDOF::Request Hand *hand = graspitCore->getWorld()->getHand(request.id); double * dof = request.dofs.data(); for (int d=0; dgetNumDOF(); d++) { - if (dof[d] < hand->getDOF(d)->getMin() || - dof[d] > hand->getDOF(d)->getMax()) { - ROS_WARN("Desired value %f is out of range for DOF %d. (min: %f, max:%f).\n\tCapped value at max/min.", + if (dof[d] < hand->getDOF(d)->getMin()) { + ROS_WARN("Desired value %f is out of range for DOF %d. (min: %f, max:%f).\n\tCapped value at min.", dof[d], d, hand->getDOF(d)->getMin(),hand->getDOF(d)->getMax()); + dof[d] = hand->getDOF(d)->getMin(); + } + else if (dof[d] > hand->getDOF(d)->getMax()) { + ROS_WARN("Desired value %f is out of range for DOF %d. (min: %f, max:%f).\n\tCapped value at max.", + dof[d], d, hand->getDOF(d)->getMin(),hand->getDOF(d)->getMax()); + dof[d] = hand->getDOF(d)->getMax(); } } - hand->forceDOFVals(request.dofs.data()); + hand->forceDOFVals(dof); response.result = response.RESULT_SUCCESS; return true; } @@ -395,10 +400,15 @@ bool GraspitInterface::setRobotDesiredDOFCB(graspit_interface::SetRobotDesiredDO // If outside valid range, cap at min/max. double * dof = request.dofs.data(); for (int d=0; dgetNumDOF(); d++) { - if (dof[d] < hand->getDOF(d)->getMin() || - dof[d] > hand->getDOF(d)->getMax()) { - ROS_WARN("Desired value %f is out of range for DOF %d. (min: %f, max:%f).\n\tCapped value at max/min.", + if (dof[d] < hand->getDOF(d)->getMin()) { + ROS_WARN("Desired value %f is out of range for DOF %d. (min: %f, max:%f).\n\tCapped value at min.", + dof[d], d, hand->getDOF(d)->getMin(),hand->getDOF(d)->getMax()); + dof[d] = hand->getDOF(d)->getMin(); + } + else if (dof[d] > hand->getDOF(d)->getMax()) { + ROS_WARN("Desired value %f is out of range for DOF %d. (min: %f, max:%f).\n\tCapped value at max.", dof[d], d, hand->getDOF(d)->getMin(),hand->getDOF(d)->getMax()); + dof[d] = hand->getDOF(d)->getMax(); } }