Merge pull request orocos#58 from ahoarau/fk_solvers_all_seg_frames_ret

Add possibility to return all the FK frames in a vector

Author: Ruben Smits

orocos_kdl/src/chainfksolver.hpp

@@ -31,14 +31,12 @@
 #include "solveri.hpp"
 
 namespace KDL {
-
     /**
 	  * \brief This <strong>abstract</strong> class encapsulates a
 	  * solver for the forward position kinematics for a KDL::Chain.
      *
      * @ingroup KinematicFamily
      */
-
     //Forward definition
     class ChainFkSolverPos : public KDL::SolverI {
     public:
@@ -52,6 +50,16 @@ namespace KDL {
          * @return if < 0 something went wrong
          */
         virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0;
+        /**
+         * Calculate forward position kinematics for a KDL::Chain,
+         * from joint coordinates to cartesian pose.
+         *
+         * @param q_in input joint coordinates
+         * @param p_out reference to a vector of output cartesian poses for all segments
+         *
+         * @return if < 0 something went wrong
+         */
+        virtual int JntToCart(const JntArray& q_in, std::vector<KDL::Frame>& p_out,int segmentNr=-1)=0;
         virtual ~ChainFkSolverPos(){};
     };
 
@@ -73,7 +81,16 @@ namespace KDL {
          * @return if < 0 something went wrong
          */
         virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0;
-
+        /**
+         * Calculate forward position and velocity kinematics, from
+         * joint coordinates to cartesian coordinates.
+         *
+         * @param q_in input joint coordinates (position and velocity)
+         * @param out output cartesian coordinates for all segments (position and velocity)
+         *
+         * @return if < 0 something went wrong
+         */
+        virtual int JntToCart(const JntArrayVel& q_in, std::vector<KDL::FrameVel>& out,int segmentNr=-1)=0;
         virtual ~ChainFkSolverVel(){};
     };
 
@@ -98,7 +115,19 @@ namespace KDL {
          * @return if < 0 something went wrong
          */
     virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0;
-
+        /**
+         * Calculate forward position, velocity and accelaration
+         * kinematics, from joint coordinates to cartesian coordinates
+         *
+         * @param q_in input joint coordinates (position, velocity and
+         * acceleration
+         @param out output cartesian coordinates (position, velocity
+         * and acceleration for all segments
+         *
+         * @return if < 0 something went wrong
+         */
+    virtual int JntToCart(const JntArrayAcc& q_in, std::vector<FrameAcc>& out,int segmentNr=-1)=0;
+    
         virtual ~ChainFkSolverAcc()=0;
     };
 

orocos_kdl/src/chainfksolverpos_recursive.cpp

@@ -56,7 +56,33 @@ namespace KDL {
             return 0;
         }
     }
+    int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, std::vector<Frame>& p_out, int seg_nr)    {
+        unsigned int segmentNr;
+        if(seg_nr<0)
+            segmentNr=chain.getNrOfSegments();
+        else
+            segmentNr = seg_nr;
 
+        if(q_in.rows()!=chain.getNrOfJoints())
+            return -1;
+        else if(segmentNr>chain.getNrOfSegments())
+            return -1;
+        else if(p_out.size() != segmentNr)
+            return -1;
+        else{
+            std::fill(p_out.begin(),p_out.end(),Frame::Identity());
+            int j=0;
+            for(unsigned int i=0;i<segmentNr;i++){
+                if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+                    p_out[i] = p_out[i]*chain.getSegment(i).pose(q_in(j));
+                    j++;
+                }else{
+                    p_out[i] = p_out[i]*chain.getSegment(i).pose(0.0);
+                }
+            }
+            return 0;
+        }
+    }
 
     ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive()
     {

orocos_kdl/src/chainfksolverpos_recursive.hpp

@@ -40,6 +40,7 @@ namespace KDL {
         ~ChainFkSolverPos_recursive();
 
         virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1);
+        virtual int JntToCart(const JntArray& q_in, std::vector<Frame>& p_out, int segmentNr=-1);
 
     private:
         const Chain chain;

orocos_kdl/src/chainfksolvervel_recursive.cpp

@@ -63,5 +63,38 @@ namespace KDL
             return 0;
         }
     }
-}
 
+    int ChainFkSolverVel_recursive::JntToCart(const JntArrayVel& in,std::vector<FrameVel>& out,int seg_nr)
+    {
+        unsigned int segmentNr;
+        if(seg_nr<0)
+            segmentNr=chain.getNrOfSegments();
+        else
+            segmentNr = seg_nr;
+
+
+
+        if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
+            return -1;
+        else if(segmentNr>chain.getNrOfSegments())
+            return -1;
+        else if(out.size()!=chain.getNrOfSegments())
+            return -1;
+        else{
+          std::fill(out.begin(),out.end(),FrameVel::Identity());
+            int j=0;
+            for (unsigned int i=0;i<segmentNr;i++) {
+                //Calculate new Frame_base_ee
+                if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+                    out[i]=out[i]*FrameVel(chain.getSegment(i).pose(in.q(j)),
+                                     chain.getSegment(i).twist(in.q(j),in.qdot(j)));
+                    j++;//Only increase jointnr if the segment has a joint
+                }else{
+                    out[i]=out[i]*FrameVel(chain.getSegment(i).pose(0.0),
+                                     chain.getSegment(i).twist(0.0,0.0));
+                }
+            }
+            return 0;
+        }
+    }
+}

orocos_kdl/src/chainfksolvervel_recursive.hpp

@@ -41,6 +41,7 @@ namespace KDL
         ~ChainFkSolverVel_recursive();
 
         virtual int JntToCart(const JntArrayVel& q_in,FrameVel& out,int segmentNr=-1);
+        virtual int JntToCart(const JntArrayVel& q_in,std::vector<FrameVel>& out,int segmentNr=-1);
     private:
         const Chain chain;
     };