pr2_interactive_manipulation: arm_locations.h Source File

00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
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 the 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 namespace pr2_interactive_manipulation {
00031 
00032 inline std::vector<double> getSidePosition(std::string arm_name)
00033 {
00034   std::vector<double> sp(7,0.0);
00035   if (arm_name=="right_arm") 
00036   {
00037     //sp[0] = -2.135; sp[1] = 0.803; sp[2] = -1.732; sp[3] = -1.905; sp[4] = -2.369; sp[5] = -1.680; sp[6] = 1.398;
00038     //sp[0] = -2.135; sp[1] = -0.18; sp[2] = -1.732; sp[3] = -1.905; sp[4] = -2.369; sp[5] = -1.680; sp[6] = 1.398;
00039     sp[0] = -2.135; sp[1] = -0.02; sp[2] = -1.64; sp[3] = -2.07; sp[4] = -1.64; sp[5] = -1.680; sp[6] = 1.398;
00040   } 
00041   else 
00042   {
00043     //sp[0] = 2.135; sp[1] = 0.803; sp[2] = 1.732; sp[3] = -1.905; sp[4] = 2.369; sp[5] = -1.680; sp[6] = 1.398;
00044     //sp[0] = 2.135; sp[1] = -0.18; sp[2] = 1.732; sp[3] = -1.905; sp[4] = 2.369; sp[5] = -1.680; sp[6] = 1.398;
00045     sp[0] = 2.135; sp[1] = -0.02; sp[2] = 1.64; sp[3] = -2.07; sp[4] = 1.64; sp[5] = -1.680; sp[6] = 1.398;
00046   }
00047   return sp;
00048 }
00049 
00050 inline std::vector<double> getFrontPosition(std::string arm_name)
00051 {
00052   std::vector<double> fp(7, 0.0);
00053   if (arm_name=="right_arm") 
00054   {
00055     fp[0] = 0.0; fp[1] = 1.203; fp[2] = 0.0; fp[3] = -2.105; fp[4] = -3.13; fp[5] = -1.680; fp[6] = 1.398;
00056   } 
00057   else 
00058   {
00059     fp[0] = 0.0; fp[1] = 1.203; fp[2] = 0.0; fp[3] = -2.105; fp[4] =  3.13; fp[5] = -1.680; fp[6] = 1.398;
00060   }
00061   return fp;
00062 }
00063 
00064 inline std::vector< std::vector<double> > getSideTrajectory(std::string arm_name)
00065 {
00066   std::vector< std::vector<double> > traj;
00067   std::vector<double> p1(7, 0.0);
00068   std::vector<double> p2(7, 0.0);
00069   if (arm_name=="right_arm")
00070   {
00071     //p1[0]=-0.968; p1[1]=0.729; p1[2]=-0.554; p1[3]=-1.891; p1[4]=-1.786; p1[5]=-1.127; p1[6]=0.501;
00072     //p2[0]=-2.135; p2[1]=0.803; p2[2]=-1.732; p2[3]=-1.905; p2[4]=-2.369; p2[5]=-1.680; p2[6]=1.398;
00073     //p1[0]=-0.968; p1[1]=0.129; p1[2]=-0.554; p1[3]=-1.891; p1[4]=-1.786; p1[5]=-1.127; p1[6]=0.501;
00074     //p2[0]=-2.135; p2[1]=-0.18; p2[2]=-1.732; p2[3]=-1.905; p2[4]=-2.369; p2[5]=-1.680; p2[6]=1.398;
00075     p1[0]=-0.968; p1[1]=0.129; p1[2]=-0.554; p1[3]=-1.891; p1[4]=-1.786; p1[5]=-1.127; p1[6]=0.501;
00076     p2[0] = -2.135; p2[1] = -0.02; p2[2] = -1.64; p2[3] = -2.07; p2[4] = -1.64; p2[5] = -1.680; p2[6] = 1.398;
00077   }
00078   else
00079   {
00080     //p1[0]=0.968; p1[1]=0.729; p1[2]=0.554; p1[3]=-1.891; p1[4]=1.786; p1[5]=-1.127; p1[6]=0.501;
00081     //p2[0]=2.135; p2[1]=0.803; p2[2]=1.732; p2[3]=-1.905; p2[4]=2.369; p2[5]=-1.680; p2[6]=1.398;
00082     //p1[0]=0.968; p1[1]=0.129; p1[2]=0.554; p1[3]=-1.891; p1[4]=1.786; p1[5]=-1.127; p1[6]=0.501;
00083     //p2[0]=2.135; p2[1]=-0.18; p2[2]=1.732; p2[3]=-1.905; p2[4]=2.369; p2[5]=-1.680; p2[6]=1.398;
00084     p1[0]=0.968; p1[1]=0.129; p1[2]=0.554; p1[3]=-1.891; p1[4]=1.786; p1[5]=-1.127; p1[6]=0.501;
00085     p2[0] = 2.135; p2[1] = -0.02; p2[2] = 1.64; p2[3] = -2.07; p2[4] = 1.64; p2[5] = -1.680; p2[6] = 1.398;
00086   }
00087   traj.push_back(p1);
00088   traj.push_back(p2);
00089   return traj;
00090 }
00091 
00092 inline std::vector< std::vector<double> > getFrontTrajectory(std::string arm_name)
00093 {
00094   std::vector< std::vector<double> > traj;
00095   std::vector<double> p1(7, 0.0);
00096   std::vector<double> p2(7, 0.0);
00097   if (arm_name=="right_arm")
00098   {
00099     p1[0]=-0.968; p1[1]=0.729; p1[2]=-0.554; p1[3]=-1.891; p1[4]=-1.786; p1[5]=-1.127; p1[6]=0.501;
00100     p2[0] = 0.0; p2[1] = 1.203; p2[2] = 0.0; p2[3] = -2.105; p2[4] = -3.13; p2[5] = -1.680; p2[6] = 1.398;
00101   }
00102   else
00103   {
00104     p1[0]=0.968; p1[1]=0.729; p1[2]=0.554; p1[3]=-1.891; p1[4]=1.786; p1[5]=-1.127; p1[6]=0.501;
00105     p2[0] = 0.0; p2[1] = 1.203; p2[2] = 0.0; p2[3] = -2.105; p2[4] = 3.13; p2[5] = -1.680; p2[6] = 1.398;
00106   }
00107   traj.push_back(p1);
00108   traj.push_back(p2);
00109   return traj;
00110 }
00111 
00112 inline std::vector<double> getHandoffPosition(std::string arm_name)
00113 {
00114   std::vector<double> sp(7,0.0);
00115   if (arm_name=="right_arm")
00116   {
00117     //sp[0] = -2.135; sp[1] = 0.803; sp[2] = -1.732; sp[3] = -1.905; sp[4] = -2.369; sp[5] = -1.680; sp[6] = 1.398;
00118     sp[0] = -2.135; sp[1] = -0.18; sp[2] = -1.732; sp[3] = -1.905; sp[4] = -2.369; sp[5] = -1.680; sp[6] = 1.398;
00119   }
00120   else
00121   {
00122     //sp[0] = 2.135; sp[1] = 0.803; sp[2] = 1.732; sp[3] = -1.905; sp[4] = 2.369; sp[5] = -1.680; sp[6] = 1.398;
00123     sp[0] = 2.135; sp[1] = -0.18; sp[2] = 1.732; sp[3] = -1.905; sp[4] = 2.369; sp[5] = -1.680; sp[6] = 1.398;
00124   }
00125   return sp;
00126 }
00127 
00128 inline std::vector< std::vector<double> > getHandoffTrajectory(std::string arm_name)
00129 {
00130   std::vector< std::vector<double> > traj;
00131   std::vector<double> p1(7, 0.0);
00132   std::vector<double> p2(7, 0.0);
00133   if (arm_name=="right_arm")
00134   {
00135     //p1[0]=-0.968; p1[1]=0.729; p1[2]=-0.554; p1[3]=-1.891; p1[4]=-1.786; p1[5]=-1.127; p1[6]=0.501;
00136     //p2[0]=-2.135; p2[1]=0.803; p2[2]=-1.732; p2[3]=-1.905; p2[4]=-2.369; p2[5]=-1.680; p2[6]=1.398;
00137     p1[0]=-0.968; p1[1]=0.129; p1[2]=-0.554; p1[3]=-1.891; p1[4]=-1.786; p1[5]=-1.127; p1[6]=0.501;
00138     p2[0]=-2.135; p2[1]=-0.18; p2[2]=-1.732; p2[3]=-1.905; p2[4]=-2.369; p2[5]=-1.680; p2[6]=1.398;
00139   }
00140   else
00141   {
00142     //p1[0]=0.968; p1[1]=0.729; p1[2]=0.554; p1[3]=-1.891; p1[4]=1.786; p1[5]=-1.127; p1[6]=0.501;
00143     //p2[0]=2.135; p2[1]=0.803; p2[2]=1.732; p2[3]=-1.905; p2[4]=2.369; p2[5]=-1.680; p2[6]=1.398;
00144     p1[0]=0.968; p1[1]=0.129; p1[2]=0.554; p1[3]=-1.891; p1[4]=1.786; p1[5]=-1.127; p1[6]=0.501;
00145     p2[0]=2.135; p2[1]=-0.18; p2[2]=1.732; p2[3]=-1.905; p2[4]=2.369; p2[5]=-1.680; p2[6]=1.398;
00146   }
00147   traj.push_back(p1);
00148   traj.push_back(p2);
00149   return traj;
00150 }
00151 
00152 inline std::vector<double> getSideHandoffPosition(std::string arm_name)
00153 {
00154   std::vector<double> sp(7,0.0);
00155   if (arm_name=="right_arm")
00156   {
00157     sp[0] = -2.099; sp[1] = 0.041; sp[2] = -1.488; sp[3] = -0.694; sp[4] = -1.693; sp[5] = -1.596; sp[6] = 0.128;
00158   }
00159   else
00160   {
00161     sp[0] = 2.099; sp[1] = -0.041; sp[2] = 1.488; sp[3] = -0.694; sp[4] = 1.693; sp[5] = -1.596; sp[6] = 0.128;
00162   }
00163   return sp;
00164 }
00165 
00166 inline std::vector< std::vector<double> > getSideHandoffTrajectory(std::string arm_name)
00167 {
00168   std::vector< std::vector<double> > traj;
00169   std::vector<double> p1(7, 0.0);
00170   std::vector<double> p2(7, 0.0);
00171   if (arm_name=="right_arm")
00172   {
00173     p1[0]=-0.968; p1[1]=0.129; p1[2]=-0.554; p1[3]=-1.891; p1[4]=-1.786; p1[5]=-1.127; p1[6]=0.501;
00174     p2[0] = -2.099; p2[1] = 0.041; p2[2] = -1.488; p2[3] = -0.694; p2[4] = -1.693; p2[5] = -1.596; p2[6] = 0.128;
00175   }
00176   else
00177   {
00178     p1[0]=0.968; p1[1]=0.129; p1[2]=0.554; p1[3]=-1.891; p1[4]=1.786; p1[5]=-1.127; p1[6]=0.501;
00179     p2[0] = 2.099; p2[1] = -0.041; p2[2] = 1.488; p2[3] = -0.694; p2[4] = 1.693; p2[5] = -1.596; p2[6] = 0.128;
00180   }
00181   traj.push_back(p1);
00182   traj.push_back(p2);
00183   return traj;
00184 }
00185 
00186 }