Go to the documentation of this file.
```00001 //-*-c++-*-
00004
00005 #ifdef TGT_IS_HANDEYE
00006
00007 #include "Motion/IKSolver.h"
00008 #include "Shared/fmatCore.h"
00009 #include "Motion/Kinematics.h"
00010 #include "Shared/RobotInfo.h"
00011
00012 //! Kinematics solver for an arm composed of three consecutive joints in the same plane
00013 class PlanarThreeLinkArm : public IKSolver {
00014 public:
00015   //! Constructor: use the last three links in the chain
00017
00018   //! Constructor: @offset specifies the first joint in the chain
00020
00021   using IKSolver::solve;
00022   using IKSolver::step;
00023
00024   //! Solve to get an 'effector' (@a pEff, @a oriEff, relative to link following @a j) to a solution of @a pTgt, @a oriTgt (or at least a local minimum)
00025   /*! @a posPri and @a oriPri specify relative weighting of each solution in case they are mutually exclusive */
00026   virtual bool solve(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float posPri, const Orientation& oriTgt, float oriPri) const;
00027   virtual StepResult_t step(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float pDist, float posPri, const Orientation& oriTgt, float oriDist, float oriPri) const;
00028
00029   struct Solutions {
00030     Solutions() : angles(),noSols(),valid() {}
00031     fmat::Matrix<2,3> angles; // double check all references to Solutions.angles
00032     //fmat::Matrix<3,2> angles;  // This is what is should be!
00033     int noSols;
00034     bool valid;
00035   };
00036
00037   void PrintVariables() const;
00038   PlanarThreeLinkArm::Solutions solveWithOffset(float x, float y, float pref_phi, fmat::Column<3> baseOffset = fmat::pack(0.0f, 0.0f,1.0f)) const;
00041   PlanarThreeLinkArm::Solutions punt(float x, float y) const;
00046   float angDist(float a1,float a2) const;
00047   float angNorm(float a) const;
00048   fmat::Matrix<2,2> rot(float t) const;
00049   //get Joint lengths
00050   float getL1() const;
00051   float getL2() const;
00052   float getL3() const;
00053   //set Joint lengths
00054   void setL1(float length1, float jointMax, float jointMin);
00055   void setL2(float length2, float jointMax, float jointMin);
00056   void setL3(float length3, float jointMax, float jointMin);
00057
00058 private:
00059   //! holds the class name, set via registration with the DeviceDriver registry