Skip to content
Snippets Groups Projects
Commit d82db6b7 authored by Johannes Mey's avatar Johannes Mey
Browse files

autoformat code

parent 7bc09198
No related branches found
No related tags found
No related merge requests found
# Panda Grasping Package # Panda Grasping Package
Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm. This package uses the old pick-and-place pipeline of MoveIt. Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm. This
package uses the old pick-and-place pipeline of MoveIt.
## How to install ## How to install
...@@ -13,6 +14,9 @@ Allows to pick and place objects. The current maximum height, where the panda ca ...@@ -13,6 +14,9 @@ Allows to pick and place objects. The current maximum height, where the panda ca
* u can start the configurable pick and place with: ```roslaunch panda_grasping simulation.launch``` * u can start the configurable pick and place with: ```roslaunch panda_grasping simulation.launch```
* to pick or place open a second console * to pick or place open a second console
* to pick type for example: ```rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'``` * to pick type for
* to place type for example ```rosservice call panda_grasping/PlaceObjectService '{pose: {position: {x: 0.0, y: 0.5, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'``` example: ```rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
* inside of the message u can update orientation (currenly only z is supported), position and size of the object to pick or place * to place type for
example ```rosservice call panda_grasping/PlaceObjectService '{pose: {position: {x: 0.0, y: 0.5, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
* inside of the message u can update orientation (currenly only z is supported), position and size of the object to
pick or place
...@@ -18,10 +18,9 @@ ...@@ -18,10 +18,9 @@
/** /**
* Util for the construction of the scene. * Util for the construction of the scene.
*/ */
class EnvironmentUtil { class EnvironmentUtil
{
public: public:
/** /**
* Adds a table plate to the planning scene. * Adds a table plate to the planning scene.
* @param collision_objects current collision objects in the planning scene * @param collision_objects current collision objects in the planning scene
...@@ -51,9 +50,9 @@ public: ...@@ -51,9 +50,9 @@ public:
* @param pose pose of the object to be picked * @param pose pose of the object to be picked
* @param dimensions dimensions of the object to be picked * @param dimensions dimensions of the object to be picked
*/ */
moveit_msgs::CollisionObject moveit_msgs::CollisionObject constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects,
constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id, std::string id, geometry_msgs::Pose& pose,
geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions); geometry_msgs::Vector3& dimensions);
}; };
#endif // PANDA_GRASPING_ENVIRONMENT_UTIL_H #endif // PANDA_GRASPING_ENVIRONMENT_UTIL_H
...@@ -5,16 +5,13 @@ ...@@ -5,16 +5,13 @@
#ifndef PUBLIC_FRANKA_GRIPPER_UTIL_H #ifndef PUBLIC_FRANKA_GRIPPER_UTIL_H
#define PUBLIC_FRANKA_GRIPPER_UTIL_H #define PUBLIC_FRANKA_GRIPPER_UTIL_H
class FrankaGripperUtil
class FrankaGripperUtil { {
public: public:
/** /**
* resets the real robot, to allow correct future movements and grasps * resets the real robot, to allow correct future movements and grasps
*/ */
void resetGripperForNextAction(); void resetGripperForNextAction();
}; };
#endif // PUBLIC_FRANKA_GRIPPER_UTIL_H #endif // PUBLIC_FRANKA_GRIPPER_UTIL_H
...@@ -18,10 +18,9 @@ ...@@ -18,10 +18,9 @@
/** /**
* Util for grasping. * Util for grasping.
*/ */
class GraspUtil { class GraspUtil
{
public: public:
/** /**
* distance of the center of the hand to the center of the finger tip * distance of the center of the hand to the center of the finger tip
*/ */
...@@ -56,11 +55,11 @@ public: ...@@ -56,11 +55,11 @@ public:
* @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported) * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported)
* @param dimensions of the object * @param dimensions of the object
* @param gripper_orientation current gripper orientation relative to the link it is attached to * @param gripper_orientation current gripper orientation relative to the link it is attached to
* @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner) * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at
* the object's corner)
* @return the "pre-pick"-pose * @return the "pre-pick"-pose
*/ */
geometry_msgs::Pose geometry_msgs::Pose getPickFromAbovePose(geometry_msgs::Pose& object_to_pick_pose, geometry_msgs::Vector3 dimensions,
getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
geometry_msgs::Quaternion gripper_orientation, geometry_msgs::Quaternion gripper_orientation,
double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
...@@ -88,7 +87,8 @@ public: ...@@ -88,7 +87,8 @@ public:
* @param object_to_place_id ID of the object to place * @param object_to_place_id ID of the object to place
*/ */
bool placeFromAbove(moveit::planning_interface::MoveGroupInterface& move_group, geometry_msgs::Pose& place_pose, bool placeFromAbove(moveit::planning_interface::MoveGroupInterface& move_group, geometry_msgs::Pose& place_pose,
double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false); double open_amount, std::string supporting_surface_id, std::string object_to_place_id,
bool plan_only = false);
/** /**
* Homes the gripper (in case it was abused before and is in an inconsistent state) * Homes the gripper (in case it was abused before and is in an inconsistent state)
...@@ -96,7 +96,6 @@ public: ...@@ -96,7 +96,6 @@ public:
* @return true if homing succeeded, false otherwise * @return true if homing succeeded, false otherwise
*/ */
static bool homeGripper(); static bool homeGripper();
}; };
#endif // PANDA_GRASPING_GRASP_UTIL_H #endif // PANDA_GRASPING_GRASP_UTIL_H
...@@ -6,8 +6,8 @@ ...@@ -6,8 +6,8 @@
#include "environment_util.h" #include "environment_util.h"
void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects, double x_dimension, void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects, double x_dimension,
double y_dimension) { double y_dimension)
{
moveit_msgs::CollisionObject plate; moveit_msgs::CollisionObject plate;
plate.header.frame_id = "panda_link0"; plate.header.frame_id = "panda_link0";
...@@ -32,8 +32,8 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> & ...@@ -32,8 +32,8 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &
void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id, void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id,
double object_x_dimension, double object_y_dimension, double object_z_dimension, double object_x_dimension, double object_y_dimension, double object_z_dimension,
geometry_msgs::Pose &object_to_pick_pose) { geometry_msgs::Pose& object_to_pick_pose)
{
moveit_msgs::CollisionObject pick_support; moveit_msgs::CollisionObject pick_support;
pick_support.header.frame_id = "panda_link0"; pick_support.header.frame_id = "panda_link0";
...@@ -57,9 +57,9 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> ...@@ -57,9 +57,9 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>
} }
moveit_msgs::CollisionObject moveit_msgs::CollisionObject
EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects, EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id,
std::string id, geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions) { geometry_msgs::Pose& pose, geometry_msgs::Vector3& dimensions)
{
moveit_msgs::CollisionObject object_to_pick; moveit_msgs::CollisionObject object_to_pick;
object_to_pick.header.frame_id = "panda_link0"; object_to_pick.header.frame_id = "panda_link0";
......
...@@ -10,8 +10,8 @@ ...@@ -10,8 +10,8 @@
// Franka // Franka
#include <franka_gripper/MoveAction.h> #include <franka_gripper/MoveAction.h>
void FrankaGripperUtil::resetGripperForNextAction() { void FrankaGripperUtil::resetGripperForNextAction()
{
actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true); actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true);
ROS_INFO("Waiting for reset action server to start."); ROS_INFO("Waiting for reset action server to start.");
...@@ -26,10 +26,13 @@ void FrankaGripperUtil::resetGripperForNextAction() { ...@@ -26,10 +26,13 @@ void FrankaGripperUtil::resetGripperForNextAction() {
// wait for the action to return // wait for the action to return
bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0)); bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0));
if (finished_before_timeout) { if (finished_before_timeout)
{
actionlib::SimpleClientGoalState state = ac.getState(); actionlib::SimpleClientGoalState state = ac.getState();
ROS_INFO("Reset action finished: %s", state.toString().c_str()); ROS_INFO("Reset action finished: %s", state.toString().c_str());
} else { }
else
{
ROS_INFO("Reset action did not finish before the time out."); ROS_INFO("Reset action did not finish before the time out.");
} }
} }
...@@ -16,7 +16,8 @@ ...@@ -16,7 +16,8 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
namespace grasping_state { namespace grasping_state
{
bool object_picked = false; bool object_picked = false;
} }
...@@ -33,13 +34,12 @@ tf2_ros::Buffer tfBuffer; ...@@ -33,13 +34,12 @@ tf2_ros::Buffer tfBuffer;
* @param group a initialized MoveGroupInterface instance * @param group a initialized MoveGroupInterface instance
* @param n a valid nodehandle * @param n a valid nodehandle
*/ */
bool pickObject(panda_grasping::PickObject::Request &req, bool pickObject(panda_grasping::PickObject::Request& req, panda_grasping::PickObject::Response& res, ros::NodeHandle& n,
panda_grasping::PickObject::Response &res, ros::NodeHandle &n,
moveit::planning_interface::PlanningSceneInterface& planning_scene_interface, moveit::planning_interface::PlanningSceneInterface& planning_scene_interface,
moveit::planning_interface::MoveGroupInterface &group) { moveit::planning_interface::MoveGroupInterface& group)
{
if (!grasping_state::object_picked) { if (!grasping_state::object_picked)
{
grasping_state::object_picked = true; grasping_state::object_picked = true;
EnvironmentUtil env_util; EnvironmentUtil env_util;
...@@ -51,30 +51,32 @@ bool pickObject(panda_grasping::PickObject::Request &req, ...@@ -51,30 +51,32 @@ bool pickObject(panda_grasping::PickObject::Request &req,
env_util.constructPlate(collision_objects, 3.0, 3.0); env_util.constructPlate(collision_objects, 3.0, 3.0);
ROS_INFO("Adding pick support."); ROS_INFO("Adding pick support.");
env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
req.dimensions.z,
req.pose); req.pose);
ROS_INFO("Adding object."); ROS_INFO("Adding object.");
moveit_msgs::CollisionObject po = env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, moveit_msgs::CollisionObject po =
req.dimensions); env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, req.dimensions);
ROS_INFO("Applying to scene."); ROS_INFO("Applying to scene.");
if (!planning_scene_interface.applyCollisionObjects(collision_objects)) { if (!planning_scene_interface.applyCollisionObjects(collision_objects))
{
res.success = false; res.success = false;
return false; return false;
} }
ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand."); ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand.");
geometry_msgs::Transform gripper_transform = tfBuffer.lookupTransform("panda_hand", "panda_link8", geometry_msgs::Transform gripper_transform =
ros::Time(0)).transform; tfBuffer.lookupTransform("panda_hand", "panda_link8", ros::Time(0)).transform;
ROS_INFO("Computing pre-grasp-pose."); ROS_INFO("Computing pre-grasp-pose.");
geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation); geometry_msgs::Pose pick_pose =
grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
ROS_INFO("Picking up ..."); ROS_INFO("Picking up ...");
// 0.08 = 8cm = maximum gripper opening // 0.08 = 8cm = maximum gripper opening
if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only)) { if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only))
{
res.success = false; res.success = false;
return false; return false;
} }
...@@ -96,32 +98,33 @@ bool pickObject(panda_grasping::PickObject::Request &req, ...@@ -96,32 +98,33 @@ bool pickObject(panda_grasping::PickObject::Request &req,
* @param group a initialized MoveGroupInterface instance * @param group a initialized MoveGroupInterface instance
* @param n a valid nodehandle * @param n a valid nodehandle
*/ */
bool bool placeObject(panda_grasping::PlaceObject::Request& req, panda_grasping::PlaceObject::Response& res,
placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObject::Response &res, ros::NodeHandle &n, ros::NodeHandle& n, moveit::planning_interface::PlanningSceneInterface& planning_scene_interface,
moveit::planning_interface::PlanningSceneInterface &planning_scene_interface, moveit::planning_interface::MoveGroupInterface& group)
moveit::planning_interface::MoveGroupInterface &group) { {
ROS_INFO_STREAM("received new place-request: " << req); ROS_INFO_STREAM("received new place-request: " << req);
if (grasping_state::object_picked) { if (grasping_state::object_picked)
{
EnvironmentUtil env_util; EnvironmentUtil env_util;
GraspUtil grasp_util; GraspUtil grasp_util;
std::vector<moveit_msgs::CollisionObject> collision_objects; std::vector<moveit_msgs::CollisionObject> collision_objects;
ROS_INFO("Adding place support."); ROS_INFO("Adding place support.");
env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y, env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
req.dimensions.z,
req.pose); req.pose);
ROS_INFO("Applying to scene."); ROS_INFO("Applying to scene.");
if (!planning_scene_interface.applyCollisionObjects(collision_objects)) { if (!planning_scene_interface.applyCollisionObjects(collision_objects))
{
res.success = false; res.success = false;
return false; return false;
} }
ROS_INFO("Placing object."); ROS_INFO("Placing object.");
if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only)) { if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only))
{
res.success = false; res.success = false;
return false; return false;
} }
...@@ -136,9 +139,8 @@ placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObje ...@@ -136,9 +139,8 @@ placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObje
return false; return false;
} }
int main(int argc, char** argv)
int main(int argc, char **argv) { {
ros::init(argc, argv, "grasp_service"); ros::init(argc, argv, "grasp_service");
ros::NodeHandle n("panda_grasping"); ros::NodeHandle n("panda_grasping");
ros::AsyncSpinner spinner(2); ros::AsyncSpinner spinner(2);
...@@ -149,12 +151,13 @@ int main(int argc, char **argv) { ...@@ -149,12 +151,13 @@ int main(int argc, char **argv) {
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroupInterface group("panda_arm"); moveit::planning_interface::MoveGroupInterface group("panda_arm");
ros::ServiceServer pick_object_service = n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>( ros::ServiceServer pick_object_service =
n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>(
"PickObjectService", "PickObjectService",
boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group))); boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
ros::ServiceServer place_object_service =
ros::ServiceServer place_object_service = n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>( n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>(
"PlaceObjectService", "PlaceObjectService",
boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group))); boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
...@@ -162,4 +165,3 @@ int main(int argc, char **argv) { ...@@ -162,4 +165,3 @@ int main(int argc, char **argv) {
return 0; return 0;
} }
...@@ -7,7 +7,8 @@ ...@@ -7,7 +7,8 @@
#include <franka_gripper/MoveAction.h> #include <franka_gripper/MoveAction.h>
#include <franka_gripper/HomingGoal.h> #include <franka_gripper/HomingGoal.h>
void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) { void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory& posture, double amount)
{
posture.joint_names.resize(2); posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1"; posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2"; posture.joint_names[1] = "panda_finger_joint2";
...@@ -19,7 +20,8 @@ void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, doub ...@@ -19,7 +20,8 @@ void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, doub
posture.points[0].time_from_start = ros::Duration(0.5); posture.points[0].time_from_start = ros::Duration(0.5);
} }
void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount) { void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory& posture, double amount)
{
posture.joint_names.resize(2); posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1"; posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2"; posture.joint_names[1] = "panda_finger_joint2";
...@@ -31,10 +33,10 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do ...@@ -31,10 +33,10 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do
posture.points[0].time_from_start = ros::Duration(0.5); posture.points[0].time_from_start = ros::Duration(0.5);
} }
geometry_msgs::Pose geometry_msgs::Pose GraspUtil::getPickFromAbovePose(geometry_msgs::Pose& object_pose_msg,
GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Vector3 dimensions,
geometry_msgs::Quaternion gripper_orientation_msg, double reach) { geometry_msgs::Quaternion gripper_orientation_msg, double reach)
{
// object orientation // object orientation
tf2::Quaternion object_orientation; tf2::Quaternion object_orientation;
tf2::fromMsg(object_pose_msg.orientation, object_orientation); tf2::fromMsg(object_pose_msg.orientation, object_orientation);
...@@ -55,12 +57,16 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m ...@@ -55,12 +57,16 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
// Actually, the maximum reach should be FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE/2, but half a // Actually, the maximum reach should be FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE/2, but half a
// finger length is considered a safety distance. // finger length is considered a safety distance.
if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE) { if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE)
ROS_WARN_STREAM("Reach around object reduced from " << reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH - {
FRANKA_GRIPPER_FINGERTIP_SIZE << "."); ROS_WARN_STREAM("Reach around object reduced from "
<< reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE << ".");
reach = FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE; reach = FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE;
} else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2) { }
ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0." << ". Minimum allowed reach is " else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2)
{
ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0."
<< ". Minimum allowed reach is "
<< (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << "."); << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << ".");
reach = 0; reach = 0;
} }
...@@ -75,11 +81,10 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m ...@@ -75,11 +81,10 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
return target_pose; return target_pose;
} }
bool bool GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface& move_group,
GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, geometry_msgs::Pose& pick_pose, geometry_msgs::Vector3 dimensions, double open_amount,
geometry_msgs::Vector3 dimensions, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only)
double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) { {
std::vector<moveit_msgs::Grasp> grasps; std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1); grasps.resize(1);
grasps[0].grasp_pose.header.frame_id = "panda_link0"; grasps[0].grasp_pose.header.frame_id = "panda_link0";
...@@ -101,15 +106,18 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr ...@@ -101,15 +106,18 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
setupOpenGripper(grasps[0].pre_grasp_posture, open_amount); setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
if (dimensions.x <= 0.08) { if (dimensions.x <= 0.08)
{
setupClosedGripper(grasps[0].grasp_posture, dimensions.x); setupClosedGripper(grasps[0].grasp_posture, dimensions.x);
} }
if (dimensions.x > 0.08 && dimensions.y <= 0.08) { if (dimensions.x > 0.08 && dimensions.y <= 0.08)
{
setupClosedGripper(grasps[0].grasp_posture, dimensions.y); setupClosedGripper(grasps[0].grasp_posture, dimensions.y);
} }
if (dimensions.x > 0.08 && dimensions.y > 0.08) { if (dimensions.x > 0.08 && dimensions.y > 0.08)
{
ROS_ERROR("Could not grasp: Object to big (in any dimension)."); ROS_ERROR("Could not grasp: Object to big (in any dimension).");
return false; return false;
} }
...@@ -118,7 +126,8 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr ...@@ -118,7 +126,8 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only); moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
if (miec.val == miec.SUCCESS) { if (miec.val == miec.SUCCESS)
{
return true; return true;
} }
...@@ -126,10 +135,10 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr ...@@ -126,10 +135,10 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
return false; return false;
} }
bool bool GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface& move_group,
GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, geometry_msgs::Pose& place_pose, double open_amount, std::string supporting_surface_id,
double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only) { std::string object_to_place_id, bool plan_only)
{
std::vector<moveit_msgs::PlaceLocation> place_location; std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1); place_location.resize(1);
...@@ -154,7 +163,8 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g ...@@ -154,7 +163,8 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only); moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only);
if (miec.val == miec.SUCCESS) { if (miec.val == miec.SUCCESS)
{
return true; return true;
} }
...@@ -162,14 +172,17 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g ...@@ -162,14 +172,17 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
return false; return false;
} }
bool GraspUtil::homeGripper() { bool GraspUtil::homeGripper()
{
actionlib::SimpleActionClient<franka_gripper::MoveAction> moveActionClient("franka_gripper/move", true); actionlib::SimpleActionClient<franka_gripper::MoveAction> moveActionClient("franka_gripper/move", true);
if (moveActionClient.waitForServer(ros::Duration(1.0))) { if (moveActionClient.waitForServer(ros::Duration(1.0)))
{
moveActionClient.cancelAllGoals(); moveActionClient.cancelAllGoals();
} }
actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true); actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true);
if (!ac.waitForServer(ros::Duration(10.0))) { if (!ac.waitForServer(ros::Duration(10.0)))
{
ROS_ERROR("Gripper homing failed (action server not available)."); ROS_ERROR("Gripper homing failed (action server not available).");
return false; return false;
} }
...@@ -177,11 +190,14 @@ bool GraspUtil::homeGripper() { ...@@ -177,11 +190,14 @@ bool GraspUtil::homeGripper() {
ac.cancelAllGoals(); ac.cancelAllGoals();
franka_gripper::HomingGoal goal; franka_gripper::HomingGoal goal;
ac.sendGoal(goal); ac.sendGoal(goal);
if (ac.waitForResult(ros::Duration(15.0))) { if (ac.waitForResult(ros::Duration(15.0)))
{
actionlib::SimpleClientGoalState state = ac.getState(); actionlib::SimpleClientGoalState state = ac.getState();
ROS_INFO_STREAM("Homing finished: " << state.toString()); ROS_INFO_STREAM("Homing finished: " << state.toString());
return state == actionlib::SimpleClientGoalState::SUCCEEDED; return state == actionlib::SimpleClientGoalState::SUCCEEDED;
} else { }
else
{
ROS_ERROR("Gripper homing failed (with timeout)."); ROS_ERROR("Gripper homing failed (with timeout).");
return false; return false;
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment