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 <moveit/macros/class_forward.h> 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 MOVEIT_CLASS_FORWARD(ManipulationPlanSharedData); 00055 00056 struct ManipulationPlanSharedData 00057 { 00058 ManipulationPlanSharedData() 00059 : planning_group_(NULL) 00060 , end_effector_group_(NULL) 00061 , ik_link_(NULL) 00062 , max_goal_sampling_attempts_(0) 00063 , minimize_object_distance_(false) 00064 { 00065 } 00066 00067 const robot_model::JointModelGroup* planning_group_; 00068 const robot_model::JointModelGroup* end_effector_group_; 00069 const robot_model::LinkModel* ik_link_; 00070 00071 unsigned int max_goal_sampling_attempts_; 00072 00073 std::string planner_id_; 00074 00075 bool minimize_object_distance_; 00076 00077 moveit_msgs::Constraints path_constraints_; 00078 00079 moveit_msgs::AttachedCollisionObject diff_attached_object_; 00080 00081 ros::WallTime timeout_; 00082 }; 00083 00084 MOVEIT_CLASS_FORWARD(ManipulationPlan); 00085 00086 struct ManipulationPlan 00087 { 00088 ManipulationPlan(const ManipulationPlanSharedDataConstPtr& shared_data) 00089 : shared_data_(shared_data), processing_stage_(0) 00090 { 00091 } 00092 00094 void clear() 00095 { 00096 goal_sampler_.reset(); 00097 trajectories_.clear(); 00098 approach_state_.reset(); 00099 possible_goal_states_.clear(); 00100 processing_stage_ = 0; 00101 } 00102 00103 // Shared data between manipulation plans (set at initialization) 00104 ManipulationPlanSharedDataConstPtr shared_data_; 00105 00106 // the approach motion towards the goal 00107 moveit_msgs::GripperTranslation approach_; 00108 00109 // the retreat motion away from the goal 00110 moveit_msgs::GripperTranslation retreat_; 00111 00112 // the kinematic configuration of the end effector when approaching the goal (an open gripper) 00113 trajectory_msgs::JointTrajectory approach_posture_; 00114 00115 // the kinematic configuration of the end effector when retreating from the goal (a closed gripper) 00116 trajectory_msgs::JointTrajectory retreat_posture_; 00117 00118 // -------------- computed data -------------------------- 00119 geometry_msgs::PoseStamped goal_pose_; 00120 Eigen::Affine3d transformed_goal_pose_; 00121 00122 moveit_msgs::Constraints goal_constraints_; 00123 00124 // Allows for the sampling of a kineamtic state for a particular group of a robot 00125 constraint_samplers::ConstraintSamplerPtr goal_sampler_; 00126 00127 std::vector<robot_state::RobotStatePtr> possible_goal_states_; 00128 00129 robot_state::RobotStatePtr approach_state_; 00130 00131 // The sequence of trajectories produced for execution 00132 std::vector<plan_execution::ExecutableTrajectory> trajectories_; 00133 00134 // An error code reflecting what went wrong (if anything) 00135 moveit_msgs::MoveItErrorCodes error_code_; 00136 00137 // The processing stage that was last working on this plan, or was about to work on this plan 00138 std::size_t processing_stage_; 00139 00140 // An id for this plan; this is usually the index of the Grasp / PlaceLocation in the input request 00141 std::size_t id_; 00142 }; 00143 } 00144 00145 #endif