Constraints.cpp
Go to the documentation of this file.
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 09:44:37