diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6675a3cb9f25c6cf711063f3777f0428f944c1a1..c08734b6b4340b67a04d8f2b539712b55dddb0ef 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,11 +49,15 @@ catkin_package(CATKIN_DEPENDS
include_directories(${catkin_INCLUDE_DIRS})
add_library(EnvironmentUtil src/grasping/environment_util.cpp src/grasping/environment_util.h)
+add_library(GraspUtil src/grasping/grasp_util.cpp src/grasping/grasp_util.h)
add_executable(grasp_service src/grasping/grasp_service.cpp)
+add_executable(pick_and_place_sample src/pick_and_place_sample.cpp)
target_link_libraries(EnvironmentUtil ${catkin_LIBRARIES})
-target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil)
+target_link_libraries(GraspUtil ${catkin_LIBRARIES})
+target_link_libraries(pick_and_place_sample ${catkin_LIBRARIES})
+target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil GraspUtil)
add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
diff --git a/launch/simulation.launch b/launch/simulation.launch
index 58ed7ef440b396ab07e7bf6f5a607f3c812f3e3b..17f2e8a594d33f6b2867d5574d23b78a124358c2 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -61,5 +61,5 @@
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
<node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" />
- <!--<node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" />-->
+ <!--<node pkg="panda_grasping" type="pick_and_place_sample" name="pick_and_place_sample" output="screen" />-->
</launch>
diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp
index 2d22d60ee513487efff30066b4840994446640bc..3cb59e6dbc547eebfe087da50495f252fcf8a705 100644
--- a/src/grasping/environment_util.cpp
+++ b/src/grasping/environment_util.cpp
@@ -53,4 +53,28 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>
pick_support.operation = pick_support.ADD;
collision_objects.push_back(pick_support);
+}
+
+void 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.primitives.resize(1);
+ object_to_pick.primitives[0].type = object_to_pick.primitives[0].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.operation = object_to_pick.ADD;
+
+ collision_objects.push_back(object_to_pick);
}
\ No newline at end of file
diff --git a/src/grasping/environment_util.h b/src/grasping/environment_util.h
index b24d9df220cdb27170d2f4e5aaa862ec3815f68d..dd1d7675130fb0fbfca058b153fce26d384ead24 100644
--- a/src/grasping/environment_util.h
+++ b/src/grasping/environment_util.h
@@ -5,9 +5,6 @@
#ifndef PANDA_GRASPING_ENVIRONMENT_UTIL_H
#define PANDA_GRASPING_ENVIRONMENT_UTIL_H
-//
-// Created by sebastian on 19.05.20.
-//
#include <ros/ros.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
@@ -15,6 +12,9 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Vector3.h>
+
class EnvironmentUtil {
public:
@@ -24,5 +24,7 @@ public:
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);
+
+ void 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
diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp
index ad4b2c8d4a2d35ff97a8eb66dc17131dcc5f9ee0..20d25446e284f5ffd99d09b4f2e8ce392809d10e 100644
--- a/src/grasping/grasp_service.cpp
+++ b/src/grasping/grasp_service.cpp
@@ -11,8 +11,9 @@
#include "panda_grasping/PickObject.h"
#include "panda_grasping/PlaceObject.h"
-#include "environment_util.h"
+#include "environment_util.h"
+#include "grasp_util.h"
bool pickObject(panda_grasping::PickObject::Request &req,
panda_grasping::PickObject::Response &res, ros::NodeHandle &n,
@@ -24,17 +25,28 @@ bool pickObject(panda_grasping::PickObject::Request &req,
group.setPlanningTime(45.0);
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 support.");
+ 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.");
+ env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, req.dimensions);
+
ROS_INFO("Applying to scene.");
planning_scene_interface.applyCollisionObjects(collision_objects);
+ ROS_INFO("Computing pre-grasp-pose.");
+ geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, 0.2);
+
+ ROS_INFO("Picking up ...");
+ grasp_util.pickFromAbove(group, pick_pose, 0.00, 0.08, "pick_support", "pick_object");
+
res.success = true;
return true;
}
@@ -42,7 +54,7 @@ bool pickObject(panda_grasping::PickObject::Request &req,
bool placeObject(panda_grasping::PlaceObject::Request &req,
panda_grasping::PlaceObject::Response &res) {
- std::cout << "received: " << req << std::endl;
+ std::cout << "received new place-request: " << req << std::endl;
return true;
}
@@ -60,7 +72,7 @@ int main(int argc, char **argv) {
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("PlaceObjectService", placeObject);
+ ros::ServiceServer place_object_service = n.advertiseService("PlaceObjectService", placeObject);
ROS_INFO("Ready to grasp.");
ros::spin();
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index 16ca57c052c4cd88e551543b485519bb28d5c0be..12237b809944b46c519c100eb08d936596bf8469 100644
--- a/src/grasping/grasp_util.cpp
+++ b/src/grasping/grasp_util.cpp
@@ -1,89 +1,77 @@
//
// Created by sebastian on 19.05.20.
//
-#include <ros/ros.h>
-
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-
-class GraspUtil {
-
-public:
-
- GraspUtil() {};
-
- void 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 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 getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, double distance_to_eef) {
-
- geometry_msgs::Pose target_pose;
-
- tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
- tf2::Quaternion orientation_object(object_to_pick_pose.orientation.x, object_to_pick_pose.orientation.y,
- object_to_pick_pose.orientation.z, object_to_pick_pose.orientation.w);
-
- tf2::Quaternion orientation = orientation_gripper * orientation_object;
- orientation.setZ(orientation.getZ() * -1);
- orientation.setY(orientation.getY() * -1);
- orientation.setX(orientation.getX() * -1);
-
- target_pose.orientation = tf2::toMsg(orientation);
- target_pose.position.x = object_to_pick_pose.position.x;
- target_pose.position.y = object_to_pick_pose.position.y;
- target_pose.position.z = object_to_pick_pose.position.z + distance_to_eef;
-
- return target_pose;
- }
-
- void pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
- double close_amount,
- double open_amount, std::string supporting_surface_id, std::string object_to_pick_id) {
- 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.x = 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);
- setupClosedGripper(grasps[0].grasp_posture, close_amount);
-
- move_group.setSupportSurfaceName(supporting_surface_id);
- move_group.pick(object_to_pick_id, grasps);
- }
-};
\ No newline at end of file
+#include "grasp_util.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::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_to_pick_pose, double distance_to_eef) {
+
+ geometry_msgs::Pose target_pose;
+
+ tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
+ tf2::Quaternion orientation_object(object_to_pick_pose.orientation.x, object_to_pick_pose.orientation.y,
+ object_to_pick_pose.orientation.z, object_to_pick_pose.orientation.w);
+
+ tf2::Quaternion orientation = orientation_gripper * orientation_object;
+ orientation.setZ(orientation.getZ() * -1);
+ orientation.setY(orientation.getY() * -1);
+ orientation.setX(orientation.getX() * -1);
+
+ target_pose.orientation = tf2::toMsg(orientation);
+ target_pose.position.x = object_to_pick_pose.position.x;
+ target_pose.position.y = object_to_pick_pose.position.y;
+ target_pose.position.z = object_to_pick_pose.position.z + distance_to_eef;
+
+ return target_pose;
+}
+
+void GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
+ double close_amount,
+ double open_amount, std::string supporting_surface_id, std::string object_to_pick_id) {
+ 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.x = 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);
+ setupClosedGripper(grasps[0].grasp_posture, close_amount);
+
+ move_group.setSupportSurfaceName(supporting_surface_id);
+ move_group.pick(object_to_pick_id, grasps);
+}
diff --git a/src/grasping/grasp_util.h b/src/grasping/grasp_util.h
new file mode 100644
index 0000000000000000000000000000000000000000..c958e6d8ddf8a359a101d04576f3d151c63f6344
--- /dev/null
+++ b/src/grasping/grasp_util.h
@@ -0,0 +1,32 @@
+//
+// Created by sebastian on 27.05.20.
+//
+
+#ifndef PANDA_GRASPING_GRASP_UTIL_H
+#define PANDA_GRASPING_GRASP_UTIL_H
+
+#include <ros/ros.h>
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include <trajectory_msgs/JointTrajectory.h>
+#include <geometry_msgs/Pose.h>
+
+class GraspUtil {
+
+public:
+
+ void setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount);
+
+ void setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount);
+
+ geometry_msgs::Pose getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, double distance_to_eef);
+
+ void pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
+ double close_amount, double open_amount, std::string supporting_surface_id,
+ std::string object_to_pick_id);
+};
+#endif //PANDA_GRASPING_GRASP_UTIL_H
diff --git a/src/pick_and_place_sample.cpp b/src/pick_and_place_sample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39b0ce94d41bf91db72db06fad8e4821b69103ba
--- /dev/null
+++ b/src/pick_and_place_sample.cpp
@@ -0,0 +1,327 @@
+// ROS
+#include <ros/ros.h>
+
+// MoveIt!
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+// TF2
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+geometry_msgs::Quaternion q_pick_object;
+
+geometry_msgs::Pose getTargetPickPose(){
+
+ std::vector<double> co_dim{0.02, 0.02, 0.2};
+ std::vector<double> co_pose{0.5, 0.0, 0.2};
+
+ geometry_msgs::Pose target_pose;
+
+ tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
+ tf2::Quaternion orientation_object(q_pick_object.x, q_pick_object.y, q_pick_object.z, q_pick_object.w);
+
+ tf2::Quaternion orientation = orientation_gripper * orientation_object;
+ orientation.setZ(orientation.getZ() * -1);
+ orientation.setY(orientation.getY() * -1);
+ orientation.setX(orientation.getX() * -1);
+
+ target_pose.orientation = tf2::toMsg(orientation);
+ target_pose.position.x = co_pose[0];
+ target_pose.position.y = co_pose[1];
+ target_pose.position.z = co_pose[2] + 0.2;
+
+ return target_pose;
+}
+
+void openGripper(trajectory_msgs::JointTrajectory& posture)
+{
+ // BEGIN_SUB_TUTORIAL open_gripper
+ /* Add both finger joints of panda robot. */
+ posture.joint_names.resize(2);
+ posture.joint_names[0] = "panda_finger_joint1";
+ posture.joint_names[1] = "panda_finger_joint2";
+
+ /* Set them as open, wide enough for the object to fit. */
+ posture.points.resize(1);
+ posture.points[0].positions.resize(2);
+ posture.points[0].positions[0] = 0.04;
+ posture.points[0].positions[1] = 0.04;
+ posture.points[0].time_from_start = ros::Duration(0.5);
+ // END_SUB_TUTORIAL
+}
+
+void closedGripper(trajectory_msgs::JointTrajectory& posture)
+{
+ // BEGIN_SUB_TUTORIAL closed_gripper
+ /* Add both finger joints of panda robot. */
+ posture.joint_names.resize(2);
+ posture.joint_names[0] = "panda_finger_joint1";
+ posture.joint_names[1] = "panda_finger_joint2";
+
+ /* Set them as closed. */
+ posture.points.resize(1);
+ posture.points[0].positions.resize(2);
+ posture.points[0].positions[0] = 0.00;
+ posture.points[0].positions[1] = 0.00;
+ posture.points[0].time_from_start = ros::Duration(0.5);
+ // END_SUB_TUTORIAL
+}
+
+void pick(moveit::planning_interface::MoveGroupInterface& move_group)
+{
+ // BEGIN_SUB_TUTORIAL pick1
+ // Create a vector of grasps to be attempted, currently only creating single grasp.
+ // This is essentially useful when using a grasp generator to generate and test multiple grasps.
+ std::vector<moveit_msgs::Grasp> grasps;
+ grasps.resize(1);
+
+ // Setting grasp pose
+ // ++++++++++++++++++++++
+ // This is the pose of panda_link8. |br|
+ // From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length
+ // of the cube). |br|
+ // Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some
+ // extra padding)
+ grasps[0].grasp_pose.header.frame_id = "panda_link0";
+ /*tf2::Quaternion orientation;
+ orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
+ grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
+ grasps[0].grasp_pose.pose.position.x = 0.415;
+ grasps[0].grasp_pose.pose.position.y = 0;
+ grasps[0].grasp_pose.pose.position.z = 0.5;*/
+
+ grasps[0].grasp_pose.pose = getTargetPickPose();
+ // Setting pre-grasp approach
+ // ++++++++++++++++++++++++++
+ /* Defined with respect to frame_id */
+ grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
+ /* Direction is set as positive x axis */
+ grasps[0].pre_grasp_approach.direction.vector.x = 1.0;
+ grasps[0].pre_grasp_approach.min_distance = 0.095;
+ grasps[0].pre_grasp_approach.desired_distance = 0.115;
+
+ // Setting post-grasp retreat
+ // ++++++++++++++++++++++++++
+ /* Defined with respect to frame_id */
+ grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
+ /* Direction is set as positive z axis */
+ 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;
+
+ // Setting posture of eef before grasp
+ // +++++++++++++++++++++++++++++++++++
+ openGripper(grasps[0].pre_grasp_posture);
+ // END_SUB_TUTORIAL
+
+ // BEGIN_SUB_TUTORIAL pick2
+ // Setting posture of eef during grasp
+ // +++++++++++++++++++++++++++++++++++
+ closedGripper(grasps[0].grasp_posture);
+ // END_SUB_TUTORIAL
+
+ // BEGIN_SUB_TUTORIAL pick3
+ // Set support surface as table1.
+ move_group.setSupportSurfaceName("table1");
+ // Call pick to pick up the object using the grasps given
+ move_group.pick("object", grasps);
+ // END_SUB_TUTORIAL
+}
+
+void place(moveit::planning_interface::MoveGroupInterface& group)
+{
+ // BEGIN_SUB_TUTORIAL place
+ // TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last
+ // location in
+ // verbose mode." This is a known issue and we are working on fixing it. |br|
+ // Create a vector of placings to be attempted, currently only creating single place location.
+ std::vector<moveit_msgs::PlaceLocation> place_location;
+ place_location.resize(1);
+
+ // Setting place location pose
+ // +++++++++++++++++++++++++++
+ 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 = tf2::toMsg(orientation);
+
+ /* While placing it is the exact location of the center of the object. */
+ place_location[0].place_pose.pose.position.x = 0;
+ place_location[0].place_pose.pose.position.y = 0.5;
+ place_location[0].place_pose.pose.position.z = 0.2;
+
+ // Setting pre-place approach
+ // ++++++++++++++++++++++++++
+ /* Defined with respect to frame_id */
+ place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
+ /* Direction is set as negative z axis */
+ 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;
+
+ // Setting post-grasp retreat
+ // ++++++++++++++++++++++++++
+ /* Defined with respect to frame_id */
+ place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
+ /* Direction is set as negative y axis */
+ 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;
+
+ // Setting posture of eef after placing object
+ // +++++++++++++++++++++++++++++++++++++++++++
+ /* Similar to the pick case */
+ openGripper(place_location[0].post_place_posture);
+
+ // Set support surface as table2.
+ group.setSupportSurfaceName("table2");
+ // Call place to place the object using the place locations given.
+ group.place("object", place_location);
+ // END_SUB_TUTORIAL
+}
+
+void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)
+{
+ // BEGIN_SUB_TUTORIAL table1
+ //
+ // Creating Environment
+ // ^^^^^^^^^^^^^^^^^^^^
+ // Create vector to hold 4 collision objects.
+
+ std::vector<moveit_msgs::CollisionObject> collision_objects;
+ collision_objects.resize(4);
+
+
+ // Add the first table where the cube will originally be kept.
+ collision_objects[0].id = "table1";
+ collision_objects[0].header.frame_id = "panda_link0";
+
+ // Define the primitive and its dimensions.
+ collision_objects[0].primitives.resize(1);
+ collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
+ collision_objects[0].primitives[0].dimensions.resize(3);
+ collision_objects[0].primitives[0].dimensions[0] = 0.2;
+ collision_objects[0].primitives[0].dimensions[1] = 0.4;
+ collision_objects[0].primitives[0].dimensions[2] = 0.4;
+
+ // Define the pose of the table.
+ collision_objects[0].primitive_poses.resize(1);
+ collision_objects[0].primitive_poses[0].position.x = 0.5;
+ collision_objects[0].primitive_poses[0].position.y = 0;
+ collision_objects[0].primitive_poses[0].position.z = -0.1;
+ // END_SUB_TUTORIAL
+
+ collision_objects[0].operation = collision_objects[0].ADD;
+
+ // BEGIN_SUB_TUTORIAL table2
+ // Add the second table where we will be placing the cube.
+ collision_objects[1].id = "table2";
+ collision_objects[1].header.frame_id = "panda_link0";
+
+ // Define the primitive and its dimensions.
+ collision_objects[1].primitives.resize(1);
+ collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
+ collision_objects[1].primitives[0].dimensions.resize(3);
+ collision_objects[1].primitives[0].dimensions[0] = 0.4;
+ collision_objects[1].primitives[0].dimensions[1] = 0.2;
+ collision_objects[1].primitives[0].dimensions[2] = 0.4;
+
+ // Define the pose of the table.
+ collision_objects[1].primitive_poses.resize(1);
+ collision_objects[1].primitive_poses[0].position.x = 0;
+ collision_objects[1].primitive_poses[0].position.y = 0.5;
+ collision_objects[1].primitive_poses[0].position.z = -0.1;
+ // END_SUB_TUTORIAL
+
+ collision_objects[1].operation = collision_objects[1].ADD;
+
+ // BEGIN_SUB_TUTORIAL object
+ // Define the object that we will be manipulating
+ collision_objects[2].header.frame_id = "panda_link0";
+ collision_objects[2].id = "object";
+
+ //Define the primitive and its dimensions.
+ collision_objects[2].primitives.resize(1);
+ collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
+ collision_objects[2].primitives[0].dimensions.resize(3);
+ collision_objects[2].primitives[0].dimensions[0] = 0.02;
+ collision_objects[2].primitives[0].dimensions[1] = 0.02;
+ collision_objects[2].primitives[0].dimensions[2] = 0.2;
+
+ //Define the pose of the object.
+ collision_objects[2].primitive_poses.resize(1);
+ collision_objects[2].primitive_poses[0].position.x = 0.5;
+ collision_objects[2].primitive_poses[0].position.y = 0;
+ collision_objects[2].primitive_poses[0].position.z = 0.2;
+
+ geometry_msgs::Pose target_pose;
+ tf2::Quaternion orientation;
+ orientation.setRPY(0, 0, -M_PI / 4);
+
+ geometry_msgs::Pose object_pose;
+ collision_objects[2].primitive_poses[0].orientation = tf2::toMsg(orientation);
+ q_pick_object.w = tf2::toMsg(orientation).w;
+ q_pick_object.x = tf2::toMsg(orientation).x;
+ q_pick_object.y = tf2::toMsg(orientation).y;
+ q_pick_object.z = tf2::toMsg(orientation).z;
+ // END_SUB_TUTORIAL
+
+ // the surface
+ collision_objects[2].operation = collision_objects[2].ADD;
+
+ collision_objects[3].header.frame_id = "panda_link0";
+ collision_objects[3].id = "surface";
+
+ // Define the primitive and its dimensions.
+ collision_objects[3].primitives.resize(1);
+ collision_objects[3].primitives[0].type = collision_objects[1].primitives[0].BOX;
+ collision_objects[3].primitives[0].dimensions.resize(3);
+ collision_objects[3].primitives[0].dimensions[0] = 3.0;
+ collision_objects[3].primitives[0].dimensions[1] = 3.0;
+ collision_objects[3].primitives[0].dimensions[2] = 0.1;
+
+ // Define the pose of the object.
+ collision_objects[3].primitive_poses.resize(1);
+ collision_objects[3].primitive_poses[0].position.x = 0;
+ collision_objects[3].primitive_poses[0].position.y = 0;
+ collision_objects[3].primitive_poses[0].position.z = -0.1;
+ // END_SUB_TUTORIAL
+
+ collision_objects[3].operation = collision_objects[3].ADD;
+
+ planning_scene_interface.applyCollisionObjects(collision_objects);
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "panda_arm_pick_place");
+ ros::NodeHandle nh;
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+
+ ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
+ ros::Duration(5.0).sleep();
+ ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
+
+ ros::WallDuration(1.0).sleep();
+ moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+ moveit::planning_interface::MoveGroupInterface group("panda_arm");
+ group.setPlanningTime(45.0);
+
+ addCollisionObjects(planning_scene_interface);
+
+ // Wait a bit for ROS things to initialize
+ ros::WallDuration(1.0).sleep();
+
+ pick(group);
+
+ ros::WallDuration(1.0).sleep();
+
+ place(group);
+
+ //std::vector<std::string>objectsToRemove{"object"};
+ //planning_scene_interface.removeCollisionObjects(objectsToRemove);
+
+ ros::waitForShutdown();
+ return 0;
+}
\ No newline at end of file