diff --git a/README.md b/README.md
index 4c495307811198d87d673d0d633d7c2ef9c9e4fa..507bf9538303a30a19e1af4a4b02e02622632034 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,7 @@
# 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
@@ -9,10 +10,13 @@ Allows to pick and place objects. The current maximum height, where the panda ca
* catkin build & source the workspace
* that's it
-## How to use
+## How to use
* u can start the configurable pick and place with: ```roslaunch panda_grasping simulation.launch```
* 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 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
+ * 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 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
diff --git a/include/environment_util.h b/include/environment_util.h
index 01a6fe5fa06abb75e1f8018dfa131ae81be3e9b5..83918b7142b7572eaf8d19daed1c62bafc136600 100644
--- a/include/environment_util.h
+++ b/include/environment_util.h
@@ -18,42 +18,41 @@
/**
* Util for the construction of the scene.
*/
-class EnvironmentUtil {
-
+class EnvironmentUtil
+{
public:
-
- /**
- * Adds a table plate to the planning scene.
- * @param collision_objects current collision objects in the planning scene
- * @param x_dimension width of the table
- * @param y_dimension length of the table
- */
- void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension,
- double y_dimension);
-
- /**
- * Adds a support surface to the planning scene (with ID "bin").
- * @param collision_objects current collision objects in the planning scene
- * @param id id of the support surface to create
- * @param object_x_dimension x dimension object to be picked
- * @param object_y_dimension y dimension object to be picked
- * @param object_z_dimension z dimension object to be picked
- *@param object_to_pick_pose pose of the object to be picked
- */
- void constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
- double object_x_dimension, double object_y_dimension, double object_z_dimension,
- geometry_msgs::Pose &object_to_pick_pose);
-
- /**
- * Adds a object to be picked/placed to the planning scene.
- * @param collision_objects current collision objects in the planning scene
- * @param id id of the object to create
- * @param pose pose of the object to be picked
- * @param dimensions dimensions of the object to be picked
- */
- moveit_msgs::CollisionObject
- constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
- geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions);
+ /**
+ * Adds a table plate to the planning scene.
+ * @param collision_objects current collision objects in the planning scene
+ * @param x_dimension width of the table
+ * @param y_dimension length of the table
+ */
+ void constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects, double x_dimension,
+ double y_dimension);
+
+ /**
+ * Adds a support surface to the planning scene (with ID "bin").
+ * @param collision_objects current collision objects in the planning scene
+ * @param id id of the support surface to create
+ * @param object_x_dimension x dimension object to be picked
+ * @param object_y_dimension y dimension object to be picked
+ * @param object_z_dimension z dimension object to be picked
+ *@param object_to_pick_pose pose of the object to be picked
+ */
+ void constructSupport(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id,
+ double object_x_dimension, double object_y_dimension, double object_z_dimension,
+ geometry_msgs::Pose& object_to_pick_pose);
+
+ /**
+ * Adds a object to be picked/placed to the planning scene.
+ * @param collision_objects current collision objects in the planning scene
+ * @param id id of the object to create
+ * @param pose pose of the object to be picked
+ * @param dimensions dimensions of the object to be picked
+ */
+ moveit_msgs::CollisionObject constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects,
+ std::string id, geometry_msgs::Pose& pose,
+ geometry_msgs::Vector3& dimensions);
};
-#endif //PANDA_GRASPING_ENVIRONMENT_UTIL_H
+#endif // PANDA_GRASPING_ENVIRONMENT_UTIL_H
diff --git a/include/franka_gripper_util.h b/include/franka_gripper_util.h
index b0b8ecc2d2cc2cda51f8e2ed742541fab024108c..ef2e06e583951dcdff73defd9b7f53b72edcb5d8 100644
--- a/include/franka_gripper_util.h
+++ b/include/franka_gripper_util.h
@@ -5,16 +5,13 @@
#ifndef PUBLIC_FRANKA_GRIPPER_UTIL_H
#define PUBLIC_FRANKA_GRIPPER_UTIL_H
-
-class FrankaGripperUtil {
-
+class FrankaGripperUtil
+{
public:
- /**
- * resets the real robot, to allow correct future movements and grasps
- */
- void resetGripperForNextAction();
-
+ /**
+ * resets the real robot, to allow correct future movements and grasps
+ */
+ void resetGripperForNextAction();
};
-
-#endif //PUBLIC_FRANKA_GRIPPER_UTIL_H
+#endif // PUBLIC_FRANKA_GRIPPER_UTIL_H
diff --git a/include/grasp_util.h b/include/grasp_util.h
index 66bdb7f373f7a92aaa51e3f2490c0ea6aa3eda33..0e125b00436a6d9d4a853e573506ffec1d675726 100644
--- a/include/grasp_util.h
+++ b/include/grasp_util.h
@@ -18,85 +18,84 @@
/**
* Util for grasping.
*/
-class GraspUtil {
-
+class GraspUtil
+{
public:
-
- /**
- * distance of the center of the hand to the center of the finger tip
- */
- static constexpr tf2Scalar FRANKA_GRIPPER_DISTANCE{0.1034};
-
- /**
- * length of a finger
- */
- static constexpr tf2Scalar FRANKA_GRIPPER_FINGER_LENGTH{0.054};
-
- /**
- * size of the finger tip
- */
- static constexpr tf2Scalar FRANKA_GRIPPER_FINGERTIP_SIZE{0.018};
-
- /**
- * Sets up how far the gripper should be opened for the next action.
- * @param posture empty posture object defining grasp
- * @param amount total opening amount (space between "forks")
- */
- void setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount);
-
- /**
- * Sets up how much the gripper should be closed for the next action.
- * @param posture empty posture object defining grasp
- * @param amount total close amount (space between "forks")
- */
- void setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount);
-
- /**
- * Compute the "pre-pick"-pose
- * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported)
- * @param dimensions of the object
- * @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)
- * @return the "pre-pick"-pose
- */
- geometry_msgs::Pose
- getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
- geometry_msgs::Quaternion gripper_orientation,
- double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
-
- /**
- * Util method to pick an object from above.
- * @param move_group an initialized MoveGroupInterface instance
- * @param pick_pose via getPickFromAbovePose-method computed pre-pick-pose
- * @param close_amount total close amount (space between "forks")
- * @param open_amount total opening amount (space between "forks")
- * @param supporting_surface_id ID of surface where object is picked from (e.g. the table)
- * @param object_to_pick_id ID of the object to pick
- * @return true if successfully picked
- */
- bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
- geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
- std::string object_to_pick_id, bool plan_only = false);
-
- /**
- * Util method to place an object from above.
- * @param move_group an initialized MoveGroupInterface instance
- * @param place_pose target place pose for the object
- * @param close_amount total close amount (space between "forks")
- * @param open_amount total opening amount (space between "forks")
- * @param supporting_surface_id ID of surface where object is placed on (e.g. the table)
- * @param object_to_place_id ID of the object to place
- */
- 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);
-
- /**
- * Homes the gripper (in case it was abused before and is in an inconsistent state)
- * This method is blocking with a maximum runtime of ~30s
- * @return true if homing succeeded, false otherwise
- */
- static bool homeGripper();
-
+ /**
+ * distance of the center of the hand to the center of the finger tip
+ */
+ static constexpr tf2Scalar FRANKA_GRIPPER_DISTANCE{ 0.1034 };
+
+ /**
+ * length of a finger
+ */
+ static constexpr tf2Scalar FRANKA_GRIPPER_FINGER_LENGTH{ 0.054 };
+
+ /**
+ * size of the finger tip
+ */
+ static constexpr tf2Scalar FRANKA_GRIPPER_FINGERTIP_SIZE{ 0.018 };
+
+ /**
+ * Sets up how far the gripper should be opened for the next action.
+ * @param posture empty posture object defining grasp
+ * @param amount total opening amount (space between "forks")
+ */
+ void setupOpenGripper(trajectory_msgs::JointTrajectory& posture, double amount);
+
+ /**
+ * Sets up how much the gripper should be closed for the next action.
+ * @param posture empty posture object defining grasp
+ * @param amount total close amount (space between "forks")
+ */
+ void setupClosedGripper(trajectory_msgs::JointTrajectory& posture, double amount);
+
+ /**
+ * Compute the "pre-pick"-pose
+ * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported)
+ * @param dimensions of the object
+ * @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)
+ * @return the "pre-pick"-pose
+ */
+ geometry_msgs::Pose getPickFromAbovePose(geometry_msgs::Pose& object_to_pick_pose, geometry_msgs::Vector3 dimensions,
+ geometry_msgs::Quaternion gripper_orientation,
+ double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+
+ /**
+ * Util method to pick an object from above.
+ * @param move_group an initialized MoveGroupInterface instance
+ * @param pick_pose via getPickFromAbovePose-method computed pre-pick-pose
+ * @param close_amount total close amount (space between "forks")
+ * @param open_amount total opening amount (space between "forks")
+ * @param supporting_surface_id ID of surface where object is picked from (e.g. the table)
+ * @param object_to_pick_id ID of the object to pick
+ * @return true if successfully picked
+ */
+ bool pickFromAbove(moveit::planning_interface::MoveGroupInterface& move_group, geometry_msgs::Pose& pick_pose,
+ geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
+ std::string object_to_pick_id, bool plan_only = false);
+
+ /**
+ * Util method to place an object from above.
+ * @param move_group an initialized MoveGroupInterface instance
+ * @param place_pose target place pose for the object
+ * @param close_amount total close amount (space between "forks")
+ * @param open_amount total opening amount (space between "forks")
+ * @param supporting_surface_id ID of surface where object is placed on (e.g. the table)
+ * @param object_to_place_id ID of the object to place
+ */
+ 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);
+
+ /**
+ * Homes the gripper (in case it was abused before and is in an inconsistent state)
+ * This method is blocking with a maximum runtime of ~30s
+ * @return true if homing succeeded, false otherwise
+ */
+ static bool homeGripper();
};
-#endif //PANDA_GRASPING_GRASP_UTIL_H
+#endif // PANDA_GRASPING_GRASP_UTIL_H
diff --git a/launch/simulation.launch b/launch/simulation.launch
index 6ca38627f1a1b666f0ee7bc232a97d402cdeb0bf..f8f48409915161a59f4fc698d648ba22b3677cc2 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -1,10 +1,10 @@
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
- <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+ <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch"/>
<!--<node name="rqt_console" pkg="rqt_console" type="rqt_console" />-->
- <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" />
+ <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen"/>
<!-- sample commands -->
<!-- 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}}' -->
diff --git a/package.xml b/package.xml
index 94db6d09fcecffeab978784e6d6c1d115ae1e851..0ddc77e605dd980cf184db16b654a077bed4148b 100644
--- a/package.xml
+++ b/package.xml
@@ -1,44 +1,44 @@
<?xml version="1.0"?>
<package format="2">
- <name>panda_grasping</name>
- <version>0.1.0</version>
- <description>The panda_grasping package</description>
+ <name>panda_grasping</name>
+ <version>0.1.0</version>
+ <description>The panda_grasping package</description>
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
- <maintainer email="sebastian@todo.todo">sebastian</maintainer>
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="sebastian@todo.todo">sebastian</maintainer>
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
- <license>TODO</license>
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
- <buildtool_depend>catkin</buildtool_depend>
+ <buildtool_depend>catkin</buildtool_depend>
- <build_depend>message_generation</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>std_msgs</build_depend>
+ <build_depend>message_generation</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>std_msgs</build_depend>
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>std_msgs</build_export_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
- <depend>moveit_core</depend>
- <depend>moveit_ros_planning_interface</depend>
- <depend>moveit_visual_tools</depend>
- <depend>robot_state_publisher</depend>
- <depend>tf2</depend>
- <depend>franka_gripper</depend>
+ <depend>moveit_core</depend>
+ <depend>moveit_ros_planning_interface</depend>
+ <depend>moveit_visual_tools</depend>
+ <depend>robot_state_publisher</depend>
+ <depend>tf2</depend>
+ <depend>franka_gripper</depend>
- <exec_depend>message_runtime</exec_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>std_msgs</exec_depend>
- <exec_depend>pluginlib</exec_depend>
+ <exec_depend>message_runtime</exec_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+ <exec_depend>pluginlib</exec_depend>
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <!-- Other tools can request additional information be placed here -->
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
- </export>
+ </export>
</package>
diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp
index 54766834b5e4fb2a625ddabc44a907940babeea2..860f6160dc85e6a87a181f02e0786f100eab4d1e 100644
--- a/src/grasping/environment_util.cpp
+++ b/src/grasping/environment_util.cpp
@@ -5,80 +5,80 @@
#include "environment_util.h"
-void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension,
- double y_dimension) {
+void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects, double x_dimension,
+ double y_dimension)
+{
+ moveit_msgs::CollisionObject plate;
- moveit_msgs::CollisionObject plate;
+ plate.header.frame_id = "panda_link0";
+ plate.id = "plate";
- plate.header.frame_id = "panda_link0";
- plate.id = "plate";
+ plate.primitives.resize(1);
+ plate.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
+ plate.primitives[0].dimensions.resize(3);
+ plate.primitives[0].dimensions[0] = x_dimension;
+ plate.primitives[0].dimensions[1] = y_dimension;
+ plate.primitives[0].dimensions[2] = 0.1;
- plate.primitives.resize(1);
- plate.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
- plate.primitives[0].dimensions.resize(3);
- plate.primitives[0].dimensions[0] = x_dimension;
- plate.primitives[0].dimensions[1] = y_dimension;
- plate.primitives[0].dimensions[2] = 0.1;
+ plate.primitive_poses.resize(1);
+ plate.primitive_poses[0].position.x = 0;
+ plate.primitive_poses[0].position.y = 0;
+ plate.primitive_poses[0].position.z = -0.1;
- plate.primitive_poses.resize(1);
- plate.primitive_poses[0].position.x = 0;
- plate.primitive_poses[0].position.y = 0;
- plate.primitive_poses[0].position.z = -0.1;
+ plate.operation = plate.ADD;
- plate.operation = plate.ADD;
-
- collision_objects.push_back(plate);
+ collision_objects.push_back(plate);
}
-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,
- geometry_msgs::Pose &object_to_pick_pose) {
-
- moveit_msgs::CollisionObject pick_support;
+ geometry_msgs::Pose& object_to_pick_pose)
+{
+ moveit_msgs::CollisionObject pick_support;
- pick_support.header.frame_id = "panda_link0";
- pick_support.id = id;
+ pick_support.header.frame_id = "panda_link0";
+ pick_support.id = id;
- pick_support.primitives.resize(1);
- pick_support.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
- pick_support.primitives[0].dimensions.resize(3);
- pick_support.primitives[0].dimensions[0] = object_x_dimension;
- pick_support.primitives[0].dimensions[1] = object_y_dimension;
- pick_support.primitives[0].dimensions[2] = 0.04;
+ pick_support.primitives.resize(1);
+ pick_support.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
+ pick_support.primitives[0].dimensions.resize(3);
+ pick_support.primitives[0].dimensions[0] = object_x_dimension;
+ pick_support.primitives[0].dimensions[1] = object_y_dimension;
+ pick_support.primitives[0].dimensions[2] = 0.04;
- pick_support.primitive_poses.resize(1);
- pick_support.primitive_poses[0].position.x = object_to_pick_pose.position.x;
- pick_support.primitive_poses[0].position.y = object_to_pick_pose.position.y;
- pick_support.primitive_poses[0].position.z = (object_to_pick_pose.position.z - (object_z_dimension / 2) - 0.02);
+ pick_support.primitive_poses.resize(1);
+ pick_support.primitive_poses[0].position.x = object_to_pick_pose.position.x;
+ pick_support.primitive_poses[0].position.y = object_to_pick_pose.position.y;
+ pick_support.primitive_poses[0].position.z = (object_to_pick_pose.position.z - (object_z_dimension / 2) - 0.02);
- pick_support.operation = pick_support.ADD;
+ pick_support.operation = pick_support.ADD;
- collision_objects.push_back(pick_support);
+ collision_objects.push_back(pick_support);
}
moveit_msgs::CollisionObject
-EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects,
- std::string id, geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions) {
-
- moveit_msgs::CollisionObject object_to_pick;
+EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id,
+ geometry_msgs::Pose& pose, geometry_msgs::Vector3& dimensions)
+{
+ moveit_msgs::CollisionObject object_to_pick;
- object_to_pick.header.frame_id = "panda_link0";
- object_to_pick.id = id;
+ object_to_pick.header.frame_id = "panda_link0";
+ object_to_pick.id = id;
- object_to_pick.primitives.resize(1);
- object_to_pick.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
- object_to_pick.primitives[0].dimensions.resize(3);
- object_to_pick.primitives[0].dimensions[0] = dimensions.x;
- object_to_pick.primitives[0].dimensions[1] = dimensions.y;
- object_to_pick.primitives[0].dimensions[2] = dimensions.z;
+ object_to_pick.primitives.resize(1);
+ object_to_pick.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
+ object_to_pick.primitives[0].dimensions.resize(3);
+ object_to_pick.primitives[0].dimensions[0] = dimensions.x;
+ object_to_pick.primitives[0].dimensions[1] = dimensions.y;
+ object_to_pick.primitives[0].dimensions[2] = dimensions.z;
- object_to_pick.primitive_poses.resize(1);
- object_to_pick.primitive_poses[0].position = pose.position;
- object_to_pick.primitive_poses[0].orientation = pose.orientation;
+ object_to_pick.primitive_poses.resize(1);
+ object_to_pick.primitive_poses[0].position = pose.position;
+ object_to_pick.primitive_poses[0].orientation = pose.orientation;
- object_to_pick.operation = object_to_pick.ADD;
+ object_to_pick.operation = object_to_pick.ADD;
- collision_objects.push_back(object_to_pick);
+ collision_objects.push_back(object_to_pick);
- return object_to_pick;
+ return object_to_pick;
}
\ No newline at end of file
diff --git a/src/grasping/franka_gripper_util.cpp b/src/grasping/franka_gripper_util.cpp
index 66f1e2240705126dfb0d50cd821f4a0539e12559..46484f2e31bef3219b2d2ccc4d36993b6b594e90 100644
--- a/src/grasping/franka_gripper_util.cpp
+++ b/src/grasping/franka_gripper_util.cpp
@@ -10,26 +10,29 @@
// Franka
#include <franka_gripper/MoveAction.h>
-void FrankaGripperUtil::resetGripperForNextAction() {
-
- actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true);
-
- ROS_INFO("Waiting for reset action server to start.");
- ac.waitForServer();
-
- ROS_INFO("Reset action server started, sending goal.");
- franka_gripper::MoveGoal goal;
- goal.speed = 0.08;
- goal.width = 0.0;
- ac.sendGoal(goal);
-
- //wait for the action to return
- bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0));
-
- if (finished_before_timeout) {
- actionlib::SimpleClientGoalState state = ac.getState();
- ROS_INFO("Reset action finished: %s", state.toString().c_str());
- } else {
- ROS_INFO("Reset action did not finish before the time out.");
- }
+void FrankaGripperUtil::resetGripperForNextAction()
+{
+ actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true);
+
+ ROS_INFO("Waiting for reset action server to start.");
+ ac.waitForServer();
+
+ ROS_INFO("Reset action server started, sending goal.");
+ franka_gripper::MoveGoal goal;
+ goal.speed = 0.08;
+ goal.width = 0.0;
+ ac.sendGoal(goal);
+
+ // wait for the action to return
+ bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0));
+
+ if (finished_before_timeout)
+ {
+ actionlib::SimpleClientGoalState state = ac.getState();
+ ROS_INFO("Reset action finished: %s", state.toString().c_str());
+ }
+ else
+ {
+ ROS_INFO("Reset action did not finish before the time out.");
+ }
}
diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp
index 7e27a2a4de28d4004599d4bd1a48a57efdc080fe..62b2eaae96bbe83536146bc8971c5c5171c0e806 100644
--- a/src/grasping/grasp_service.cpp
+++ b/src/grasping/grasp_service.cpp
@@ -16,8 +16,9 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
-namespace grasping_state {
- bool object_picked = false;
+namespace grasping_state
+{
+bool object_picked = false;
}
/**
@@ -33,59 +34,60 @@ tf2_ros::Buffer tfBuffer;
* @param group a initialized MoveGroupInterface instance
* @param n a valid nodehandle
*/
-bool pickObject(panda_grasping::PickObject::Request &req,
- panda_grasping::PickObject::Response &res, ros::NodeHandle &n,
- moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
- moveit::planning_interface::MoveGroupInterface &group) {
-
- if (!grasping_state::object_picked) {
-
- grasping_state::object_picked = true;
-
- EnvironmentUtil env_util;
- GraspUtil grasp_util;
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
-
- ROS_INFO("Adding plate.");
- env_util.constructPlate(collision_objects, 3.0, 3.0);
-
- ROS_INFO("Adding pick support.");
- env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y,
- req.dimensions.z,
- req.pose);
-
- ROS_INFO("Adding object.");
- moveit_msgs::CollisionObject po = env_util.constructObjectToPick(collision_objects, "pick_object", req.pose,
- req.dimensions);
-
- ROS_INFO("Applying to scene.");
- if (!planning_scene_interface.applyCollisionObjects(collision_objects)) {
- res.success = false;
- return false;
- }
-
- ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand.");
- geometry_msgs::Transform gripper_transform = tfBuffer.lookupTransform("panda_hand", "panda_link8",
- ros::Time(0)).transform;
+bool pickObject(panda_grasping::PickObject::Request& req, panda_grasping::PickObject::Response& res, ros::NodeHandle& n,
+ moveit::planning_interface::PlanningSceneInterface& planning_scene_interface,
+ moveit::planning_interface::MoveGroupInterface& group)
+{
+ if (!grasping_state::object_picked)
+ {
+ grasping_state::object_picked = true;
+
+ EnvironmentUtil env_util;
+ GraspUtil grasp_util;
+
+ std::vector<moveit_msgs::CollisionObject> collision_objects;
+
+ ROS_INFO("Adding plate.");
+ env_util.constructPlate(collision_objects, 3.0, 3.0);
+
+ ROS_INFO("Adding pick support.");
+ env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
+ req.pose);
+
+ ROS_INFO("Adding object.");
+ moveit_msgs::CollisionObject po =
+ env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, req.dimensions);
+
+ ROS_INFO("Applying to scene.");
+ if (!planning_scene_interface.applyCollisionObjects(collision_objects))
+ {
+ res.success = false;
+ return false;
+ }
- ROS_INFO("Computing pre-grasp-pose.");
- geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
+ ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand.");
+ geometry_msgs::Transform gripper_transform =
+ tfBuffer.lookupTransform("panda_hand", "panda_link8", ros::Time(0)).transform;
- ROS_INFO("Picking up ...");
- // 0.08 = 8cm = maximum gripper opening
- if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only)) {
- res.success = false;
- return false;
- }
+ ROS_INFO("Computing pre-grasp-pose.");
+ geometry_msgs::Pose pick_pose =
+ grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
- res.success = true;
- return true;
+ ROS_INFO("Picking up ...");
+ // 0.08 = 8cm = maximum gripper opening
+ if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only))
+ {
+ res.success = false;
+ return false;
}
- ROS_ERROR("Error: Cannot pick object. An other object is already picked.");
- res.success = false;
- return false;
+ res.success = true;
+ return true;
+ }
+
+ ROS_ERROR("Error: Cannot pick object. An other object is already picked.");
+ res.success = false;
+ return false;
}
/**
@@ -96,70 +98,70 @@ bool pickObject(panda_grasping::PickObject::Request &req,
* @param group a initialized MoveGroupInterface instance
* @param n a valid nodehandle
*/
-bool
-placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObject::Response &res, ros::NodeHandle &n,
- moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
- moveit::planning_interface::MoveGroupInterface &group) {
-
- ROS_INFO_STREAM("received new place-request: " << req);
-
- if (grasping_state::object_picked) {
- EnvironmentUtil env_util;
- GraspUtil grasp_util;
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
-
- ROS_INFO("Adding place support.");
- env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y,
- req.dimensions.z,
- req.pose);
-
- ROS_INFO("Applying to scene.");
- if (!planning_scene_interface.applyCollisionObjects(collision_objects)) {
- res.success = false;
- return false;
- }
-
- ROS_INFO("Placing object.");
- if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only)) {
- res.success = false;
- return false;
- }
-
- grasping_state::object_picked = false;
- res.success = true;
- return true;
+bool placeObject(panda_grasping::PlaceObject::Request& req, panda_grasping::PlaceObject::Response& res,
+ ros::NodeHandle& n, moveit::planning_interface::PlanningSceneInterface& planning_scene_interface,
+ moveit::planning_interface::MoveGroupInterface& group)
+{
+ ROS_INFO_STREAM("received new place-request: " << req);
+
+ if (grasping_state::object_picked)
+ {
+ EnvironmentUtil env_util;
+ GraspUtil grasp_util;
+
+ std::vector<moveit_msgs::CollisionObject> collision_objects;
+
+ ROS_INFO("Adding place support.");
+ env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
+ req.pose);
+
+ ROS_INFO("Applying to scene.");
+ if (!planning_scene_interface.applyCollisionObjects(collision_objects))
+ {
+ res.success = false;
+ return false;
}
- ROS_ERROR("Error: Could not place object. Reason: There is currently no object attached to the gripper.");
- res.success = false;
- return false;
-}
-
+ ROS_INFO("Placing object.");
+ if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only))
+ {
+ res.success = false;
+ return false;
+ }
-int main(int argc, char **argv) {
+ grasping_state::object_picked = false;
+ res.success = true;
+ return true;
+ }
- ros::init(argc, argv, "grasp_service");
- ros::NodeHandle n("panda_grasping");
- ros::AsyncSpinner spinner(2);
- spinner.start();
+ ROS_ERROR("Error: Could not place object. Reason: There is currently no object attached to the gripper.");
+ res.success = false;
+ return false;
+}
- tf2_ros::TransformListener tfListener(tfBuffer);
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "grasp_service");
+ ros::NodeHandle n("panda_grasping");
+ ros::AsyncSpinner spinner(2);
+ spinner.start();
- moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
- moveit::planning_interface::MoveGroupInterface group("panda_arm");
+ tf2_ros::TransformListener tfListener(tfBuffer);
- ros::ServiceServer pick_object_service = n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>(
- "PickObjectService",
- boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
+ moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+ moveit::planning_interface::MoveGroupInterface group("panda_arm");
+ ros::ServiceServer pick_object_service =
+ n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>(
+ "PickObjectService",
+ boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
- ros::ServiceServer place_object_service = n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>(
- "PlaceObjectService",
- boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
+ ros::ServiceServer place_object_service =
+ n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>(
+ "PlaceObjectService",
+ boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
- ros::waitForShutdown();
+ ros::waitForShutdown();
- return 0;
+ return 0;
}
-
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index b89d05d4c7a49a20168f8ec3e5da4becd9aab221..d3e5bbcc6a2286fff38944f39753f6705462104e 100644
--- a/src/grasping/grasp_util.cpp
+++ b/src/grasping/grasp_util.cpp
@@ -7,182 +7,198 @@
#include <franka_gripper/MoveAction.h>
#include <franka_gripper/HomingGoal.h>
-void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
- posture.joint_names.resize(2);
- posture.joint_names[0] = "panda_finger_joint1";
- posture.joint_names[1] = "panda_finger_joint2";
-
- posture.points.resize(1);
- posture.points[0].positions.resize(2);
- posture.points[0].positions[0] = amount / 2;
- posture.points[0].positions[1] = amount / 2;
- posture.points[0].time_from_start = ros::Duration(0.5);
+void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory& posture, double amount)
+{
+ posture.joint_names.resize(2);
+ posture.joint_names[0] = "panda_finger_joint1";
+ posture.joint_names[1] = "panda_finger_joint2";
+
+ posture.points.resize(1);
+ posture.points[0].positions.resize(2);
+ posture.points[0].positions[0] = amount / 2;
+ posture.points[0].positions[1] = amount / 2;
+ posture.points[0].time_from_start = ros::Duration(0.5);
}
-void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
- posture.joint_names.resize(2);
- posture.joint_names[0] = "panda_finger_joint1";
- posture.joint_names[1] = "panda_finger_joint2";
-
- posture.points.resize(1);
- posture.points[0].positions.resize(2);
- posture.points[0].positions[0] = amount / 2;
- posture.points[0].positions[1] = amount / 2;
- posture.points[0].time_from_start = ros::Duration(0.5);
+void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory& posture, double amount)
+{
+ posture.joint_names.resize(2);
+ posture.joint_names[0] = "panda_finger_joint1";
+ posture.joint_names[1] = "panda_finger_joint2";
+
+ posture.points.resize(1);
+ posture.points[0].positions.resize(2);
+ posture.points[0].positions[0] = amount / 2;
+ posture.points[0].positions[1] = amount / 2;
+ posture.points[0].time_from_start = ros::Duration(0.5);
}
-geometry_msgs::Pose
-GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions,
- geometry_msgs::Quaternion gripper_orientation_msg, double reach) {
-
- // object orientation
- tf2::Quaternion object_orientation;
- tf2::fromMsg(object_pose_msg.orientation, object_orientation);
-
- // gripper orientation
- tf2::Quaternion gripper_orientation;
- tf2::fromMsg(gripper_orientation_msg, gripper_orientation);
-
- // approach orientation
- double approach_roll, approach_pitch, approach_yaw;
- approach_roll = M_PI; // turn upside down
- approach_pitch = 0;
- approach_yaw = 0;
- tf2::Quaternion approach_orientation;
- approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
-
- tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
-
- // 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.
- 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 << ".");
- 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 "
- << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << ".");
- reach = 0;
- }
-
- // The pose is 10 cm straight above the object to pick. This only works on upright boxes.
- geometry_msgs::Pose target_pose;
- target_pose.orientation = tf2::toMsg(orientation);
- target_pose.position.x = object_pose_msg.position.x;
- target_pose.position.y = object_pose_msg.position.y;
- target_pose.position.z = object_pose_msg.position.z + (0.5 * dimensions.z) + FRANKA_GRIPPER_DISTANCE - reach;
-
- return target_pose;
+geometry_msgs::Pose GraspUtil::getPickFromAbovePose(geometry_msgs::Pose& object_pose_msg,
+ geometry_msgs::Vector3 dimensions,
+ geometry_msgs::Quaternion gripper_orientation_msg, double reach)
+{
+ // object orientation
+ tf2::Quaternion object_orientation;
+ tf2::fromMsg(object_pose_msg.orientation, object_orientation);
+
+ // gripper orientation
+ tf2::Quaternion gripper_orientation;
+ tf2::fromMsg(gripper_orientation_msg, gripper_orientation);
+
+ // approach orientation
+ double approach_roll, approach_pitch, approach_yaw;
+ approach_roll = M_PI; // turn upside down
+ approach_pitch = 0;
+ approach_yaw = 0;
+ tf2::Quaternion approach_orientation;
+ approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+ tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
+
+ // 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.
+ 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 << ".");
+ 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 "
+ << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << ".");
+ reach = 0;
+ }
+
+ // The pose is 10 cm straight above the object to pick. This only works on upright boxes.
+ geometry_msgs::Pose target_pose;
+ target_pose.orientation = tf2::toMsg(orientation);
+ target_pose.position.x = object_pose_msg.position.x;
+ target_pose.position.y = object_pose_msg.position.y;
+ target_pose.position.z = object_pose_msg.position.z + (0.5 * dimensions.z) + FRANKA_GRIPPER_DISTANCE - reach;
+
+ return target_pose;
}
-bool
-GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
- geometry_msgs::Vector3 dimensions,
- double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) {
-
- std::vector<moveit_msgs::Grasp> grasps;
- grasps.resize(1);
- grasps[0].grasp_pose.header.frame_id = "panda_link0";
- grasps[0].grasp_pose.pose = pick_pose;
-
- // Setup pre-grasp approach
- grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
- grasps[0].pre_grasp_approach.direction.vector.z = -1.0;
- grasps[0].pre_grasp_approach.min_distance = 0.095;
- grasps[0].pre_grasp_approach.desired_distance = 0.115;
-
- // Setup post-grasp retreat
- grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
- grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
- grasps[0].post_grasp_retreat.min_distance = 0.1;
- grasps[0].post_grasp_retreat.desired_distance = 0.25;
-
- // Setup the opening / closing - mechanism of the gripper
-
- setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
-
- if (dimensions.x <= 0.08) {
- setupClosedGripper(grasps[0].grasp_posture, dimensions.x);
- }
-
- if (dimensions.x > 0.08 && dimensions.y <= 0.08) {
- setupClosedGripper(grasps[0].grasp_posture, dimensions.y);
- }
-
- if (dimensions.x > 0.08 && dimensions.y > 0.08) {
- ROS_ERROR("Could not grasp: Object to big (in any dimension).");
- return false;
- }
-
- move_group.setSupportSurfaceName(supporting_surface_id);
-
- moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
-
- if (miec.val == miec.SUCCESS) {
- return true;
- }
-
- ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
+bool GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface& move_group,
+ geometry_msgs::Pose& pick_pose, geometry_msgs::Vector3 dimensions, double open_amount,
+ std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only)
+{
+ std::vector<moveit_msgs::Grasp> grasps;
+ grasps.resize(1);
+ grasps[0].grasp_pose.header.frame_id = "panda_link0";
+ grasps[0].grasp_pose.pose = pick_pose;
+
+ // Setup pre-grasp approach
+ grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
+ grasps[0].pre_grasp_approach.direction.vector.z = -1.0;
+ grasps[0].pre_grasp_approach.min_distance = 0.095;
+ grasps[0].pre_grasp_approach.desired_distance = 0.115;
+
+ // Setup post-grasp retreat
+ grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
+ grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
+ grasps[0].post_grasp_retreat.min_distance = 0.1;
+ grasps[0].post_grasp_retreat.desired_distance = 0.25;
+
+ // Setup the opening / closing - mechanism of the gripper
+
+ setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
+
+ if (dimensions.x <= 0.08)
+ {
+ setupClosedGripper(grasps[0].grasp_posture, dimensions.x);
+ }
+
+ if (dimensions.x > 0.08 && dimensions.y <= 0.08)
+ {
+ setupClosedGripper(grasps[0].grasp_posture, dimensions.y);
+ }
+
+ if (dimensions.x > 0.08 && dimensions.y > 0.08)
+ {
+ ROS_ERROR("Could not grasp: Object to big (in any dimension).");
return false;
-}
-
-bool
-GraspUtil::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) {
-
- std::vector<moveit_msgs::PlaceLocation> place_location;
- place_location.resize(1);
-
- place_location[0].place_pose.header.frame_id = "panda_link0";
- // tf2::Quaternion orientation;
- // orientation.setRPY(0, 0, M_PI / 2);
- place_location[0].place_pose.pose.orientation.w = 1;// = tf2::toMsg(orientation);
- place_location[0].place_pose.pose.position = place_pose.position;
+ }
- place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
- place_location[0].pre_place_approach.direction.vector.z = -1.0;
- place_location[0].pre_place_approach.min_distance = 0.095;
- place_location[0].pre_place_approach.desired_distance = 0.115;
+ move_group.setSupportSurfaceName(supporting_surface_id);
- place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
- place_location[0].post_place_retreat.direction.vector.z = 1.0;
- place_location[0].post_place_retreat.min_distance = 0.1;
- place_location[0].post_place_retreat.desired_distance = 0.25;
+ moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
- setupOpenGripper(place_location[0].post_place_posture, open_amount);
- move_group.setSupportSurfaceName(supporting_surface_id);
+ if (miec.val == miec.SUCCESS)
+ {
+ return true;
+ }
- moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only);
-
- if (miec.val == miec.SUCCESS) {
- return true;
- }
+ ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
+ return false;
+}
- ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
- return false;
+bool GraspUtil::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)
+{
+ std::vector<moveit_msgs::PlaceLocation> place_location;
+ place_location.resize(1);
+
+ place_location[0].place_pose.header.frame_id = "panda_link0";
+ // tf2::Quaternion orientation;
+ // orientation.setRPY(0, 0, M_PI / 2);
+ place_location[0].place_pose.pose.orientation.w = 1; // = tf2::toMsg(orientation);
+ place_location[0].place_pose.pose.position = place_pose.position;
+
+ place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
+ place_location[0].pre_place_approach.direction.vector.z = -1.0;
+ place_location[0].pre_place_approach.min_distance = 0.095;
+ place_location[0].pre_place_approach.desired_distance = 0.115;
+
+ place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
+ place_location[0].post_place_retreat.direction.vector.z = 1.0;
+ place_location[0].post_place_retreat.min_distance = 0.1;
+ place_location[0].post_place_retreat.desired_distance = 0.25;
+
+ setupOpenGripper(place_location[0].post_place_posture, open_amount);
+ move_group.setSupportSurfaceName(supporting_surface_id);
+
+ moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only);
+
+ if (miec.val == miec.SUCCESS)
+ {
+ return true;
+ }
+
+ ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
+ return false;
}
-bool GraspUtil::homeGripper() {
- actionlib::SimpleActionClient<franka_gripper::MoveAction> moveActionClient("franka_gripper/move", true);
- if (moveActionClient.waitForServer(ros::Duration(1.0))) {
- moveActionClient.cancelAllGoals();
- }
-
- actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true);
- if (!ac.waitForServer(ros::Duration(10.0))) {
- ROS_ERROR("Gripper homing failed (action server not available).");
- return false;
- }
-
- ac.cancelAllGoals();
- franka_gripper::HomingGoal goal;
- ac.sendGoal(goal);
- if (ac.waitForResult(ros::Duration(15.0))) {
- actionlib::SimpleClientGoalState state = ac.getState();
- ROS_INFO_STREAM("Homing finished: " << state.toString());
- return state == actionlib::SimpleClientGoalState::SUCCEEDED;
- } else {
- ROS_ERROR("Gripper homing failed (with timeout).");
- return false;
- }
+bool GraspUtil::homeGripper()
+{
+ actionlib::SimpleActionClient<franka_gripper::MoveAction> moveActionClient("franka_gripper/move", true);
+ if (moveActionClient.waitForServer(ros::Duration(1.0)))
+ {
+ moveActionClient.cancelAllGoals();
+ }
+
+ actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true);
+ if (!ac.waitForServer(ros::Duration(10.0)))
+ {
+ ROS_ERROR("Gripper homing failed (action server not available).");
+ return false;
+ }
+
+ ac.cancelAllGoals();
+ franka_gripper::HomingGoal goal;
+ ac.sendGoal(goal);
+ if (ac.waitForResult(ros::Duration(15.0)))
+ {
+ actionlib::SimpleClientGoalState state = ac.getState();
+ ROS_INFO_STREAM("Homing finished: " << state.toString());
+ return state == actionlib::SimpleClientGoalState::SUCCEEDED;
+ }
+ else
+ {
+ ROS_ERROR("Gripper homing failed (with timeout).");
+ return false;
+ }
}