00001 /* 00002 * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu> 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 /* 00031 #include <ros/ros.h> 00032 #include <actionlib/client/simple_action_client.h> 00033 #include <move_arm_msgs/MoveArmAction.h> 00034 00035 int main(int argc, char **argv){ 00036 ros::init (argc, argv, "move_arm_joint_goal_test"); 00037 ros::NodeHandle nh; 00038 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_right_arm",true); 00039 00040 move_arm.waitForServer(); 00041 ROS_INFO("Connected to server"); 00042 00043 move_arm_msgs::MoveArmGoal goalA; 00044 00045 goalA.motion_plan_request.group_name = "right_arm"; 00046 goalA.motion_plan_request.num_planning_attempts = 1; 00047 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0); 00048 00049 nh.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string("")); 00050 nh.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("ompl_planning/plan_kinematic_path")); 00051 goalA.motion_plan_request.goal_constraints.set_position_constraints_size(2); 00052 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now(); 00053 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link"; 00054 00055 goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link"; 00056 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.527; 00057 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.258; 00058 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0.687; 00059 00060 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX; 00061 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.05); 00062 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.05); 00063 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.05); 00064 00065 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0; 00066 goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 0.5; 00067 00068 goalA.motion_plan_request.goal_constraints.position_constraints[1].header.stamp = ros::Time::now(); 00069 goalA.motion_plan_request.goal_constraints.position_constraints[1].header.frame_id = "base_footprint"; 00070 00071 goalA.motion_plan_request.goal_constraints.position_constraints[1].link_name = "r_elbow_flex_link"; 00072 goalA.motion_plan_request.goal_constraints.position_constraints[1].position.x = 0; 00073 goalA.motion_plan_request.goal_constraints.position_constraints[1].position.y = 0; 00074 goalA.motion_plan_request.goal_constraints.position_constraints[1].position.z = 5.82; 00075 00076 goalA.motion_plan_request.goal_constraints.position_constraints[1].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX; 00077 goalA.motion_plan_request.goal_constraints.position_constraints[1].constraint_region_shape.dimensions.push_back(10.0f); 00078 goalA.motion_plan_request.goal_constraints.position_constraints[1].constraint_region_shape.dimensions.push_back(10.0f); 00079 goalA.motion_plan_request.goal_constraints.position_constraints[1].constraint_region_shape.dimensions.push_back(10.0f); 00080 00081 goalA.motion_plan_request.goal_constraints.position_constraints[1].constraint_region_orientation.w = 1.0; 00082 goalA.motion_plan_request.goal_constraints.position_constraints[1].weight = 0.5; 00083 00084 goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1); 00085 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now(); 00086 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link"; 00087 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link"; 00088 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0; 00089 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0; 00090 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0; 00091 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0; 00092 00093 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04; 00094 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04; 00095 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04; 00096 00097 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0; 00098 00099 if (nh.ok()) 00100 { 00101 bool finished_within_time = false; 00102 move_arm.sendGoal(goalA); 00103 finished_within_time = move_arm.waitForResult(ros::Duration(200.0)); 00104 if (!finished_within_time) 00105 { 00106 move_arm.cancelGoal(); 00107 ROS_INFO("Timed out achieving goal A"); 00108 } 00109 else 00110 { 00111 actionlib::SimpleClientGoalState state = move_arm.getState(); 00112 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); 00113 if(success) 00114 ROS_INFO("Action finished: %s",state.toString().c_str()); 00115 else 00116 ROS_INFO("Action failed: %s",state.toString().c_str()); 00117 } 00118 } 00119 ros::shutdown(); 00120 }*/ 00121 00122 /********************************************************************* 00123 * 00124 * Software License Agreement (BSD License) 00125 * 00126 * Copyright (c) 2008, Willow Garage, Inc. 00127 * All rights reserved. 00128 * 00129 * Redistribution and use in source and binary forms, with or without 00130 * modification, are permitted provided that the following conditions 00131 * are met: 00132 * 00133 * * Redistributions of source code must retain the above copyright 00134 * notice, this list of conditions and the following disclaimer. 00135 * * Redistributions in binary form must reproduce the above 00136 * copyright notice, this list of conditions and the following 00137 * disclaimer in the documentation and/or other materials provided 00138 * with the distribution. 00139 * * Neither the name of the Willow Garage nor the names of its 00140 * contributors may be used to endorse or promote products derived 00141 * from this software without specific prior written permission. 00142 * 00143 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00144 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00145 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00146 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00147 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00148 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00149 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00150 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00151 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00152 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00153 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00154 * POSSIBILITY OF SUCH DAMAGE. 00155 * 00156 * \author Sachin Chitta 00157 *********************************************************************/ 00158 00159 #include <ros/ros.h> 00160 #include <actionlib/client/simple_action_client.h> 00161 #include <move_arm_msgs/MoveArmAction.h> 00162 #include <tf/transform_listener.h> 00163 00164 int main(int argc, char **argv){ 00165 ros::init (argc, argv, "move_arm_joint_goal_test"); 00166 ros::NodeHandle nh; 00167 00168 int side = atoi(argv[1]); 00169 00170 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm(side ? "move_left_arm" : "move_right_arm",true); 00171 00172 move_arm.waitForServer(); 00173 ROS_INFO("Connected to server"); 00174 00175 move_arm_msgs::MoveArmGoal goalA; 00176 00177 goalA.motion_plan_request.group_name = side ? "left_arm" : "right_arm"; 00178 goalA.motion_plan_request.num_planning_attempts = 1; 00179 goalA.motion_plan_request.allowed_planning_time = ros::Duration(atof(argv[9])); 00180 00181 nh.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string("")); 00182 //nh.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("ompl_planning/plan_kinematic_path")); 00183 nh.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("stomp_motion_planner/plan_path")); 00184 goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1); 00185 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now(); 00186 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link"; 00187 00188 goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = side ? "l_wrist_roll_link" : "r_wrist_roll_link"; 00189 //goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.75; 00190 //goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.188; 00191 //goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0; 00192 00193 btVector3 pos(atof(argv[2]),atof(argv[3]),atof(argv[4])); 00194 btQuaternion ori(atof(argv[5]),atof(argv[6]),atof(argv[7]),atof(argv[8])); 00195 //btVector3 pos(0.624, -0.483, 1.285); 00196 //btQuaternion ori(0.494, 0.869, -0.030, -0.011); 00197 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = pos.x(); 00198 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = pos.y(); 00199 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = pos.z(); 00200 00201 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX; 00202 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02); 00203 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02); 00204 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02); 00205 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0; 00206 goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0; 00207 00208 goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1); 00209 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now(); 00210 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link"; 00211 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = side ? "l_wrist_roll_link" : "r_wrist_roll_link"; 00212 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = ori.x(); 00213 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = ori.y(); 00214 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = ori.z(); 00215 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = ori.w(); 00216 00217 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04; 00218 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04; 00219 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04; 00220 00221 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0; 00222 00223 /*float minimumHeight = .82 + .10; // minimum height for ellbow 00224 00225 motion_planning_msgs::PositionConstraint pc; 00226 00227 pc.header.stamp = ros::Time::now(); 00228 pc.header.frame_id = "torso_lift_link"; 00229 pc.link_name = "r_elbow_flex_link"; 00230 pc.position.x = 0; 00231 pc.position.y = 0; 00232 pc.position.z = 2.5 + (minimumHeight - 1.2); // assuming torso all the way up 00233 //pc.position.z = 2.5 + (minimumHeight - .808); // assuming torso all the way down 00234 pc.constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX; 00235 pc.constraint_region_shape.dimensions.push_back(5.0); 00236 pc.constraint_region_shape.dimensions.push_back(5.0); 00237 pc.constraint_region_shape.dimensions.push_back(5.0); 00238 pc.constraint_region_orientation.w = 1.0; 00239 pc.weight = 1.0; 00240 00241 goalA.motion_plan_request.goal_constraints.position_constraints.push_back(pc);*/ 00242 00243 00244 if (nh.ok()) 00245 { 00246 bool finished_within_time = false; 00247 move_arm.sendGoal(goalA); 00248 finished_within_time = move_arm.waitForResult(ros::Duration(150.0)); 00249 if (!finished_within_time) 00250 { 00251 move_arm.cancelGoal(); 00252 ROS_INFO("Timed out achieving goal A"); 00253 } 00254 else 00255 { 00256 actionlib::SimpleClientGoalState state = move_arm.getState(); 00257 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); 00258 if(success) 00259 ROS_INFO("Action finished: %s",state.toString().c_str()); 00260 else 00261 ROS_INFO("Action failed: %s",state.toString().c_str()); 00262 } 00263 } 00264 ros::shutdown(); 00265 }