From a691bffd584c3a40ba70a0c6b9f3d90a6bd18bcc Mon Sep 17 00:00:00 2001
From: Stephen Roderick <kiwi.net@mac.com>
Date: Wed, 12 Jan 2011 10:46:43 -0500
Subject: [PATCH] modified JntToJac in class ChainJntToJacSolver to allow for calculation of Jacobian in intermediate frames

---
 src/chainjnttojacsolver.cpp |   71 +++++++++++++++++++++++++------------------
 src/chainjnttojacsolver.hpp |    2 +-
 2 files changed, 42 insertions(+), 31 deletions(-)

diff --git a/src/chainjnttojacsolver.cpp b/src/chainjnttojacsolver.cpp
index b36b8ef..8af2087 100644
--- a/src/chainjnttojacsolver.cpp
+++ b/src/chainjnttojacsolver.cpp
@@ -45,43 +45,54 @@ namespace KDL
         }
     }
 
-    int ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac)
+    int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNr)
     {
+        if(segmentNr<0)
+			segmentNr=chain.getNrOfSegments();
+		
+		//Initialize Jacobian to zero since only segmentNr colunns are computed
+		SetToZero(jac) ;
+
         if(q_in.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jac.columns())
             return -1;
-        T_tmp = Frame::Identity();
-        SetToZero(t_tmp);
-        int j=0;
-        int k=0;
-        Frame total;
-        for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
-            //Calculate new Frame_base_ee
-            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
-            	//pose of the new end-point expressed in the base
-                total = T_tmp*chain.getSegment(i).pose(q_in(j));
-                //changing base of new segment's twist to base frame if it is not locked
-                //t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0);
-                if(!locked_joints_[i])
-                    t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
-            }else{
-                total = T_tmp*chain.getSegment(i).pose(0.0);
+        else if(segmentNr>chain.getNrOfSegments())
+            return -1;
+        else{
+			T_tmp = Frame::Identity();
+			SetToZero(t_tmp);
+            int j=0;
+            int k=0;
+            Frame total;
+            for (unsigned int i=0;i<segmentNr;i++) {
+                //Calculate new Frame_base_ee
+                if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+            	    //pose of the new end-point expressed in the base
+                    total = T_tmp*chain.getSegment(i).pose(q_in(j));
+                    //changing base of new segment's twist to base frame if it is not locked
+                    //t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0);
+                    if(!locked_joints_[i])
+                        t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
+                }else{
+                    total = T_tmp*chain.getSegment(i).pose(0.0);
 
-            }
+                }
 
-            //Changing Refpoint of all columns to new ee
-            changeRefPoint(jac,total.p-T_tmp.p,jac);
+                //Changing Refpoint of all columns to new ee
+                changeRefPoint(jac,total.p-T_tmp.p,jac);
 
-            //Only increase jointnr if the segment has a joint
-            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
-                //Only put the twist inside if it is not locked
-                if(!locked_joints_[i])
-                    jac.setColumn(k++,t_tmp);
-                j++;
-            }
+                //Only increase jointnr if the segment has a joint
+                if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+                    //Only put the twist inside if it is not locked
+                     if(!locked_joints_[i])
+                        jac.setColumn(k++,t_tmp);
+                    j++;
+                }
 
-            T_tmp = total;
-        }
-        return 0;
+                T_tmp = total;
+            }
+            return 0;
+			
+		}
     }
 }
 
diff --git a/src/chainjnttojacsolver.hpp b/src/chainjnttojacsolver.hpp
index cf60f42..73ca9ef 100644
--- a/src/chainjnttojacsolver.hpp
+++ b/src/chainjnttojacsolver.hpp
@@ -53,7 +53,7 @@ namespace KDL
          *
          * @return always returns 0
          */
-        int JntToJac(const JntArray& q_in,Jacobian& jac);
+		virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNR=-1);
         
         int setLockedJoints(const std::vector<bool> locked_joints);
     private:
-- 
1.7.3.5