00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2012, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan, Sachin Chitta */ 00036 00037 #ifndef MOVEIT_PICK_PLACE_MANIPULATION_PLAN_ 00038 #define MOVEIT_PICK_PLACE_MANIPULATION_PLAN_ 00039 00040 #include <boost/shared_ptr.hpp> 00041 #include <moveit/robot_state/robot_state.h> 00042 #include <moveit/constraint_samplers/constraint_sampler.h> 00043 #include <moveit/plan_execution/plan_representation.h> 00044 #include <moveit_msgs/GripperTranslation.h> 00045 #include <moveit_msgs/RobotState.h> 00046 #include <moveit_msgs/RobotTrajectory.h> 00047 #include <moveit_msgs/MoveItErrorCodes.h> 00048 #include <moveit_msgs/Constraints.h> 00049 #include <string> 00050 #include <vector> 00051 00052 namespace pick_place 00053 { 00054 00055 struct ManipulationPlanSharedData 00056 { 00057 ManipulationPlanSharedData() 00058 : planning_group_(NULL) 00059 , end_effector_group_(NULL) 00060 , ik_link_(NULL) 00061 , max_goal_sampling_attempts_(0) 00062 , minimize_object_distance_(false) 00063 { 00064 } 00065 00066 const robot_model::JointModelGroup *planning_group_; 00067 const robot_model::JointModelGroup *end_effector_group_; 00068 const robot_model::LinkModel *ik_link_; 00069 00070 unsigned int max_goal_sampling_attempts_; 00071 00072 std::string planner_id_; 00073 00074 bool minimize_object_distance_; 00075 00076 moveit_msgs::Constraints path_constraints_; 00077 00078 moveit_msgs::AttachedCollisionObject diff_attached_object_; 00079 00080 ros::WallTime timeout_; 00081 }; 00082 00083 typedef boost::shared_ptr<ManipulationPlanSharedData> ManipulationPlanSharedDataPtr; 00084 typedef boost::shared_ptr<const ManipulationPlanSharedData> ManipulationPlanSharedDataConstPtr; 00085 00086 struct ManipulationPlan 00087 { 00088 ManipulationPlan(const ManipulationPlanSharedDataConstPtr &shared_data) : 00089 shared_data_(shared_data), 00090 processing_stage_(0) 00091 { 00092 } 00093 00095 void clear() 00096 { 00097 goal_sampler_.reset(); 00098 trajectories_.clear(); 00099 approach_state_.reset(); 00100 possible_goal_states_.clear(); 00101 processing_stage_ = 0; 00102 } 00103 00104 // Shared data between manipulation plans (set at initialization) 00105 ManipulationPlanSharedDataConstPtr shared_data_; 00106 00107 // the approach motion towards the goal 00108 moveit_msgs::GripperTranslation approach_; 00109 00110 // the retreat motion away from the goal 00111 moveit_msgs::GripperTranslation retreat_; 00112 00113 // the kinematic configuration of the end effector when approaching the goal (an open gripper) 00114 trajectory_msgs::JointTrajectory approach_posture_; 00115 00116 // the kinematic configuration of the end effector when retreating from the goal (a closed gripper) 00117 trajectory_msgs::JointTrajectory retreat_posture_; 00118 00119 // -------------- computed data -------------------------- 00120 geometry_msgs::PoseStamped goal_pose_; 00121 Eigen::Affine3d transformed_goal_pose_; 00122 00123 moveit_msgs::Constraints goal_constraints_; 00124 00125 // Allows for the sampling of a kineamtic state for a particular group of a robot 00126 constraint_samplers::ConstraintSamplerPtr goal_sampler_; 00127 00128 std::vector<robot_state::RobotStatePtr> possible_goal_states_; 00129 00130 robot_state::RobotStatePtr approach_state_; 00131 00132 // The sequence of trajectories produced for execution 00133 std::vector<plan_execution::ExecutableTrajectory> trajectories_; 00134 00135 // An error code reflecting what went wrong (if anything) 00136 moveit_msgs::MoveItErrorCodes error_code_; 00137 00138 // The processing stage that was last working on this plan, or was about to work on this plan 00139 std::size_t processing_stage_; 00140 00141 // An id for this plan; this is usually the index of the Grasp / PlaceLocation in the input request 00142 std::size_t id_; 00143 00144 }; 00145 00146 typedef boost::shared_ptr<ManipulationPlan> ManipulationPlanPtr; 00147 typedef boost::shared_ptr<const ManipulationPlan> ManipulationPlanConstPtr; 00148 00149 } 00150 00151 #endif