Follow this link to skip to the main content

src » project_2dof_planar_arm

2dof_planar_arm.cc

Go to the documentation of this file.
00001 // -*-c++-*-
00002 //---------------------------< /-/ CLARAty /-/ >------------------------------
00003 /**
00004  * @file  2dof_planar_arm.cc
00005  *
00006  * A specialization of mechanism_model for a 2 dof planar arm. This is
00007  * primarily to implement the custom inverse kinematics for the arm.
00008  *
00009  * <br>@b Designer(s):  Hari Das Nayar
00010  * <br>@b Author(s):    Hari Das Nayar
00011  * <br>@b Date:         September 27, 2006
00012  *
00013  * <b>Software License:</b><br>
00014  * <code> http://claraty.jpl.nasa.gov/license/open_src/  or 
00015  *        file: license/open_src.txt </code> 
00016  *
00017  * &copy; 2006, Jet Propulsion Laboratory, California Institute of Technology<br>
00018  *
00019  * $Revision: 1.6 $
00020  */
00021 //-----------------------------------------------------------------------------
00022 
00023 #include "claraty/share.h"
00024 #include "claraty/parameter_parser.h"
00025 #include "claraty/mechanism_model_io.h"
00026 #include "claraty/2dof_planar_arm.h"
00027 
00028 namespace claraty {
00029 
00030 //-----------------------------------------------------------------------------
00031 /**
00032  * Constructor of a N_2dof_Planar_Arm
00033  *
00034  * @param[in] filename  XML file/path name
00035  */
00036 
00037 N_2DOF_Planar_Arm::N_2DOF_Planar_Arm(std::string filename)
00038 {
00039   Mechanism_Model_IO::load(filename, *this);
00040 
00041   initialize();
00042 
00043 }
00044 
00045 
00046 //-----------------------------------------------------------------------------
00047 /**
00048  * Destructor of a N_2dof_Planar_Arm
00049  *
00050  */
00051 
00052 N_2DOF_Planar_Arm::~N_2DOF_Planar_Arm()
00053 {
00054 }
00055 
00056 //-----------------------------------------------------------------------------
00057 /**
00058  * Retrieve the link lengths from the model.
00059  *
00060  */
00061 void N_2DOF_Planar_Arm::initialize()
00062 {
00063   Transform link1_joint_loc = get_body(1).get_frame("ref1").get_transform();
00064   Transform link2_joint_loc = get_body(2).get_frame("ref2").get_transform();
00065   Transform link2_tip_loc   = get_body(2).get_frame("tip").get_transform();
00066 
00067   _link1_length = link1_joint_loc.distance_from(link2_joint_loc);
00068   _link2_length = link2_tip_loc.get_translation().magnitude();
00069 }
00070 
00071 //-----------------------------------------------------------------------------
00072 /**
00073  * Compute the inverse kinematics for the 2-link planar arm. There are
00074  * in general, two solutions. If given tip pos is outside the
00075  * workspace, the inverse kinematics returns false. It returns true
00076  * otherwise.
00077  *
00078  * @param[in]  top_pos  Point_2d<double> to specify the tip position
00079  * @param[out] joint_values  Vector <Vector<double> > a vector of the possible
00080  *                  2-angle solutions.
00081  * @return       bool
00082  */
00083 
00084 bool N_2DOF_Planar_Arm::
00085 inverse_kinematics(N_2D_Point_d tip_pos,
00086                    Vector < Vector<double> > & joint_values) 
00087 {
00088   joint_values.resize(2);
00089   joint_values[0].resize(2);
00090   joint_values[1].resize(2);
00091 
00092   double c    = tip_pos.length();
00093   // The tip is outside the workspace of the robot
00094   if (c > _link1_length + _link2_length) {
00095     std::cout << "The tip is outside the workspace" << std::endl;
00096     joint_values[0][0] = 0.0;
00097     joint_values[1][0] = 0.0;
00098     joint_values[0][1] = 0.0;
00099     joint_values[1][1] = 0.0;
00100     return false;
00101   }
00102   // If the tip is at the base joint, there are an
00103   // infinite set of solutions with joint 2 angle equal to
00104   // PI. Arbitrarily choose a couple to return.
00105   if (c == 0.0) {
00106     std::cout << "The tip is at the base" << std::endl;
00107     joint_values[0][0] = 0.0;
00108     joint_values[1][0] = M_PI_2;
00109     joint_values[0][1] = M_PI;
00110     joint_values[1][1] = M_PI;
00111     return true;
00112   }
00113 
00114   // Use the cosine rule to compute the joint angles
00115   double l1_2 = cl_sqr(_link1_length);
00116   double l2_2 = cl_sqr(_link2_length);
00117   double c_2  = cl_sqr(c);
00118   double inc_angle = acos( (l1_2 + l2_2 - c_2)  /
00119                            (2.0 * _link1_length * _link2_length) );
00120 
00121   joint_values[0][1] = M_PI - inc_angle;
00122   joint_values[1][1] = - joint_values[0][1];
00123 
00124   double tip_angle = tip_pos.angle();
00125 
00126   double base_inc_angle = acos((c_2 + l1_2 - l2_2)/(2.0 * _link1_length * c));
00127   
00128   joint_values[0][0] = tip_angle - base_inc_angle;
00129   joint_values[1][0] = tip_angle + base_inc_angle;
00130 
00131   return true;
00132 }
00133 
00134 } // namespace claraty