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 */ 00036 00037 #include <move_group_interface/move_group.h> 00038 #include <ros/ros.h> 00039 00040 int main(int argc, char **argv) 00041 { 00042 ros::init(argc, argv, "rw_move_group_interface_test", ros::init_options::AnonymousName); 00043 00044 ros::AsyncSpinner spinner(1); 00045 spinner.start(); 00046 00047 move_group_interface::MoveGroup group("arm"); 00048 // move_group_interface::MoveGroup group2("left_arm"); 00049 /* 00050 std::vector<double> v0(7,0.0); 00051 group.setJointValueTarget(v0); 00052 00053 group.move(); 00054 sleep(1); 00055 return 0; 00056 */ 00057 /* 00058 std::vector<double> v; 00059 v.push_back(0.4455); 00060 v.push_back(-0.1734); 00061 v.push_back(1.2177); 00062 v.push_back(-0.18); 00063 v.push_back(-1.37); 00064 v.push_back(0); 00065 v.push_back(0); 00066 00067 group.setJointValueTarget(v); 00068 00069 group.move(); 00070 00071 sleep(3); 00072 00073 // std::vector<double> v; 00074 00075 v.clear(); 00076 v.push_back(-1.1); 00077 v.push_back(0.5); 00078 v.push_back(-0.373); 00079 v.push_back(0.126); 00080 v.push_back(-1.196); 00081 v.push_back(0); 00082 v.push_back(0); 00083 */ 00084 00085 00086 /* 00087 // safe plan 00088 std::vector<double> v; 00089 00090 v.push_back(0.27); 00091 v.push_back(-0.68); 00092 v.push_back(-0.89); 00093 v.push_back(-1.09); 00094 v.push_back(-0.87); 00095 v.push_back(0); 00096 v.push_back(0); 00097 00098 group.setJointValueTarget(v); 00099 group.move(); 00100 sleep(2); 00101 00102 std::vector<double> q; 00103 q.push_back(0.47); 00104 q.push_back(0.94); 00105 q.push_back(-0.04); 00106 q.push_back(0.95); 00107 q.push_back(-1.67); 00108 q.push_back(0); 00109 q.push_back(0); 00110 00111 group.setJointValueTarget(q); 00112 group.move(); 00113 sleep(1); 00114 */ 00115 00116 00117 // evil plan 00118 std::vector<double> v; 00119 00120 v.push_back(1.15); 00121 v.push_back(-0.105); 00122 v.push_back(0.38); 00123 v.push_back(0.055); 00124 v.push_back(-1.47); 00125 v.push_back(0); 00126 v.push_back(0); 00127 00128 group.setJointValueTarget(v); 00129 group.move(); 00130 sleep(2); 00131 00132 std::vector<double> q; 00133 q.push_back(-0.69); 00134 q.push_back(0.52); 00135 q.push_back(1.09); 00136 q.push_back(0.8); 00137 q.push_back(0.23); 00138 q.push_back(0); 00139 q.push_back(0); 00140 00141 group.setJointValueTarget(q); 00142 group.move(); 00143 sleep(1); 00144 00145 00146 00147 return 0; 00148 }