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

pick and place working in simulation

parent 7ef30876
Branches
No related tags found
No related merge requests found
......@@ -24,7 +24,8 @@ find_package(catkin REQUIRED
moveit_ros_planning
moveit_ros_planning_interface
controller_interface
hardware_interface)
hardware_interface
simulation_util)
# System dependencies are found with CMake's conventions
find_package(PkgConfig REQUIRED)
......@@ -40,6 +41,7 @@ catkin_package(CATKIN_DEPENDS
controller_interface
hardware_interface
pluginlib
simulation_util
DEPENDS
# system_lib
)
......@@ -55,9 +57,13 @@ add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
add_executable(SampleTimedCartesianPlanner src/SampleTimedCartesianPlanner.cpp)
add_executable(SampleSimpleMotion src/SampleSimpleMotion.cpp)
add_executable(MinimalSimpleMotion src/MinimalSimpleMotion.cpp)
add_executable(${PROJECT_NAME}_pick_place src/pick_place_tutorial.cpp)
set_target_properties(${PROJECT_NAME}_pick_place PROPERTIES OUTPUT_NAME pick_place PREFIX "")
# Specify libraries to link a library or executable target against
target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES})
target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES})
target_link_libraries(SampleSimpleMotion ${catkin_LIBRARIES})
target_link_libraries(MinimalSimpleMotion ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_pick_place ${catkin_LIBRARIES})
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<!-- <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />-->
<!-- <rosparam file="$(find sample_applications)/config/config.yaml" command="load" />-->
<!-- <node name="safety_zone_spawner" pkg="sample_applications" type="safety_zone_spawner" respawn="false" output="screen"/>-->
<node pkg="sample_applications" type="pick_place" name="pick_place" />
<!-- <node name="rqt_console" pkg="rqt_console" type="rqt_console" />-->
</launch>
......@@ -40,6 +40,7 @@
<depend>moveit_visual_tools</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>simulation_util</depend>
<exec_depend>pluginlib</exec_depend>
......
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Ioan Sucan, Ridhwan Luthra*/
// 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>
// Zone Spawner
#include <GazeboZoneSpawner.h>
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|
// Make sure that when you set the grasp_pose, you are setting it to be the pose of the last link in
// your manipulator which in this case would be `"panda_link8"` You will have to compensate for the
// transform from `"panda_link8"` to the palm of the end effector.
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.4;
grasps[0].grasp_pose.pose.position.y = 0;
grasps[0].grasp_pose.pose.position.z = 0.5;
// 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. |br|
// |br|
// Ideally, you would create a vector of place locations to be attempted although in this example, we only create
// a 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);
/* For place location, we set the value to 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.5;
// 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.y = -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 3 collision objects.
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.resize(3);
// 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.2;
collision_objects[0].primitive_poses[0].orientation.w = 1.0;
shape_msgs::SolidPrimitive table1;
table1.type = shape_msgs::SolidPrimitive::BOX;
table1.dimensions = collision_objects[0].primitives[0].dimensions;
geometry_msgs::Pose table1pose;
table1pose.position.x = collision_objects[0].primitive_poses[0].position.x;
table1pose.position.y = collision_objects[0].primitive_poses[0].position.y;
table1pose.position.z = collision_objects[0].primitive_poses[0].position.z;
// the orientation is always the default
table1pose.orientation.w = 1;
GazeboZoneSpawner::spawnPrimitive("table1", table1, table1pose, 20, true);
// 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.2;
collision_objects[1].primitive_poses[0].orientation.w = 1.0;
shape_msgs::SolidPrimitive table2;
table2.type = shape_msgs::SolidPrimitive::BOX;
table2.dimensions = collision_objects[1].primitives[0].dimensions;
geometry_msgs::Pose table2pose;
table2pose.position.x = collision_objects[1].primitive_poses[0].position.x;
table2pose.position.y = collision_objects[1].primitive_poses[0].position.y;
table2pose.position.z = collision_objects[1].primitive_poses[0].position.z;
// the orientation is always the default
table2pose.orientation.w = 1;
GazeboZoneSpawner::spawnPrimitive("table2", table2, table2pose, 20, true);
// 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.5;
collision_objects[2].primitive_poses[0].orientation.w = 1.0;
shape_msgs::SolidPrimitive object;
object.type = shape_msgs::SolidPrimitive::BOX;
object.dimensions = collision_objects[2].primitives[0].dimensions;
geometry_msgs::Pose objectpose;
objectpose.position.x = collision_objects[2].primitive_poses[0].position.x;
objectpose.position.y = collision_objects[2].primitive_poses[0].position.y;
objectpose.position.z = collision_objects[2].primitive_poses[0].position.z;
// the orientation is always the default
objectpose.orientation.w = 1;
GazeboZoneSpawner::spawnPrimitive("pickObject", object, objectpose, 1);
// END_SUB_TUTORIAL
collision_objects[2].operation = collision_objects[2].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);
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
spinner.start();
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);
ros::waitForShutdown();
return 0;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment