Skip to content
Snippets Groups Projects
Commit 7c6983b6 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

implemented pick up of objects

parent 52f9d78a
No related branches found
No related tags found
No related merge requests found
...@@ -49,11 +49,15 @@ catkin_package(CATKIN_DEPENDS ...@@ -49,11 +49,15 @@ catkin_package(CATKIN_DEPENDS
include_directories(${catkin_INCLUDE_DIRS}) include_directories(${catkin_INCLUDE_DIRS})
add_library(EnvironmentUtil src/grasping/environment_util.cpp src/grasping/environment_util.h) 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(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(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) add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
......
...@@ -61,5 +61,5 @@ ...@@ -61,5 +61,5 @@
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" /> <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="grasp_service" name="grasp_service" output="screen" />--> <!--<node pkg="panda_grasping" type="pick_and_place_sample" name="pick_and_place_sample" output="screen" />-->
</launch> </launch>
...@@ -54,3 +54,27 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> ...@@ -54,3 +54,27 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>
collision_objects.push_back(pick_support); 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
...@@ -5,9 +5,6 @@ ...@@ -5,9 +5,6 @@
#ifndef PANDA_GRASPING_ENVIRONMENT_UTIL_H #ifndef PANDA_GRASPING_ENVIRONMENT_UTIL_H
#define PANDA_GRASPING_ENVIRONMENT_UTIL_H #define PANDA_GRASPING_ENVIRONMENT_UTIL_H
//
// Created by sebastian on 19.05.20.
//
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
...@@ -15,6 +12,9 @@ ...@@ -15,6 +12,9 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Vector3.h>
class EnvironmentUtil { class EnvironmentUtil {
public: public:
...@@ -24,5 +24,7 @@ public: ...@@ -24,5 +24,7 @@ public:
void constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id, void 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);
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 #endif //PANDA_GRASPING_ENVIRONMENT_UTIL_H
...@@ -11,8 +11,9 @@ ...@@ -11,8 +11,9 @@
#include "panda_grasping/PickObject.h" #include "panda_grasping/PickObject.h"
#include "panda_grasping/PlaceObject.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, bool pickObject(panda_grasping::PickObject::Request &req,
panda_grasping::PickObject::Response &res, ros::NodeHandle &n, panda_grasping::PickObject::Response &res, ros::NodeHandle &n,
...@@ -24,17 +25,28 @@ bool pickObject(panda_grasping::PickObject::Request &req, ...@@ -24,17 +25,28 @@ bool pickObject(panda_grasping::PickObject::Request &req,
group.setPlanningTime(45.0); group.setPlanningTime(45.0);
EnvironmentUtil env_util; EnvironmentUtil env_util;
GraspUtil grasp_util;
std::vector<moveit_msgs::CollisionObject> collision_objects; std::vector<moveit_msgs::CollisionObject> collision_objects;
ROS_INFO("Adding plate."); ROS_INFO("Adding plate.");
env_util.constructPlate(collision_objects, 3.0, 3.0); 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); 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."); ROS_INFO("Applying to scene.");
planning_scene_interface.applyCollisionObjects(collision_objects); 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; res.success = true;
return true; return true;
} }
...@@ -42,7 +54,7 @@ bool pickObject(panda_grasping::PickObject::Request &req, ...@@ -42,7 +54,7 @@ bool pickObject(panda_grasping::PickObject::Request &req,
bool placeObject(panda_grasping::PlaceObject::Request &req, bool placeObject(panda_grasping::PlaceObject::Request &req,
panda_grasping::PlaceObject::Response &res) { panda_grasping::PlaceObject::Response &res) {
std::cout << "received: " << req << std::endl; std::cout << "received new place-request: " << req << std::endl;
return true; return true;
} }
...@@ -60,7 +72,7 @@ int main(int argc, char **argv) { ...@@ -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", 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))); 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_INFO("Ready to grasp.");
ros::spin(); ros::spin();
......
// //
// Created by sebastian on 19.05.20. // Created by sebastian on 19.05.20.
// //
#include <ros/ros.h> #include "grasp_util.h"
#include <moveit/planning_scene_interface/planning_scene_interface.h> void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
#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.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";
...@@ -26,7 +15,7 @@ public: ...@@ -26,7 +15,7 @@ public:
posture.points[0].time_from_start = ros::Duration(0.5); posture.points[0].time_from_start = ros::Duration(0.5);
} }
void 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";
...@@ -38,7 +27,7 @@ public: ...@@ -38,7 +27,7 @@ public:
posture.points[0].time_from_start = ros::Duration(0.5); 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 GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, double distance_to_eef) {
geometry_msgs::Pose target_pose; geometry_msgs::Pose target_pose;
...@@ -59,7 +48,7 @@ public: ...@@ -59,7 +48,7 @@ public:
return target_pose; return target_pose;
} }
void pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, void GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
double close_amount, double close_amount,
double open_amount, std::string supporting_surface_id, std::string object_to_pick_id) { double open_amount, std::string supporting_surface_id, std::string object_to_pick_id) {
std::vector<moveit_msgs::Grasp> grasps; std::vector<moveit_msgs::Grasp> grasps;
...@@ -86,4 +75,3 @@ public: ...@@ -86,4 +75,3 @@ public:
move_group.setSupportSurfaceName(supporting_surface_id); move_group.setSupportSurfaceName(supporting_surface_id);
move_group.pick(object_to_pick_id, grasps); move_group.pick(object_to_pick_id, grasps);
} }
};
\ No newline at end of file
//
// 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
// 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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment