Calculation of the mass of a portion of the kinematic chain

hrplib/hrpModel/Body.cpp

@@ -449,6 +449,10 @@ void Body::calcInverseDynamics(Link* ptr, Vector3& out_f, Vector3& out_tau)
             sw.setZero();
             sv.setZero();
         }
+
+	ptr->vo = parent->vo + sv * ptr->dq;  // Added by Rafa
+	ptr->w  = parent->w  + sw * ptr->dq;  // Added by Rafa
+
         dsv = parent->w.cross(sv) + parent->vo.cross(sw);
         dsw = parent->w.cross(sw);
 
@@ -458,9 +462,9 @@ void Body::calcInverseDynamics(Link* ptr, Vector3& out_f, Vector3& out_tau)
         ptr->sw = sw;
         ptr->sv = sv;
     }
-	
-    Vector3  c,P,L;
-    Matrix33 I,c_hat;
+
+    Vector3  c, P, L;
+    Matrix33 I, c_hat;
 
     c = ptr->R * ptr->c + ptr->p;
     I.noalias() = ptr->R * ptr->I * ptr->R.transpose();
@@ -925,9 +929,9 @@ void Body::calcCMJacobian(Link *base, dmatrix &J)
         }
 	case Link::SLIDE_JOINT:
 	{
-	  Vector3 dp((j->subm/totalMass_)*sgn[j->jointId]*j->R*j->d);
-	  J.col(j->jointId) = dp;
-	  break;
+	    Vector3 dp((j->subm/totalMass_)*sgn[j->jointId]*j->R*j->d);
+	    J.col(j->jointId) = dp;
+	    break;
 	}
         default:
             std::cerr << "calcCMJacobian() : unsupported jointType("

hrplib/hrpModel/JointPath.cpp

@@ -10,7 +10,7 @@
 /**
    \file
    \brief Implementations of the LinkPath class
-   \author Shin'ichiro Nakaoka
+   \author Shin'ichiro Nakaoka, Rafael Cisneros
 */
 
 #include "JointPath.h"
@@ -170,6 +170,58 @@ void JointPath::calcJacobian(dmatrix& out_J, const Vector3& local_p) const
 }
 
 
+void JointPath::calcJacobianDot(dmatrix& out_dJ, const Vector3& local_p) const
+{
+    const int n = joints.size();
+    out_dJ.resize(6, n);
+
+    if (n > 0) {
+
+        Link* targetLink = linkPath.endLink();
+
+        for (int i=0; i < n; ++i) {
+
+            Link* link = joints[i];
+
+            switch (link->jointType) {
+
+            case Link::ROTATIONAL_JOINT:
+            {
+                Vector3 omega(link->R * link->a);
+                Vector3 arm(targetLink->p + targetLink->R * local_p - link->p);
+
+                Vector3 omegaDot(hat(link->w) * link->R * link->a);
+                Vector3 armDot(targetLink->v + hat(targetLink->w) * targetLink->R * local_p - link->v);
+
+                if (!isJointDownward(i)) {
+                    omega *= -1.0;
+                    omegaDot *= -1.0;
+                }
+
+                Vector3 ddp(omegaDot.cross(arm) + omega.cross(armDot));
+
+                out_dJ.col(i) << ddp, omegaDot;
+            }
+            break;
+
+            case Link::SLIDE_JOINT:
+            {
+                Vector3 ddp(hat(link->w) * link->R * link->d);
+                if (!isJointDownward(i)) {
+                    ddp *= -1.0;
+                }
+                out_dJ.col(i) << ddp, Vector3::Zero();  
+            }
+            break;
+
+            default:
+                out_dJ.col(i).setZero();
+            }
+        }
+    }
+}
+
+
 void JointPath::setMaxIKError(double e)
 {
     maxIKErrorSqr = e * e;

hrplib/hrpModel/JointPath.h

@@ -9,7 +9,7 @@
 
 /** \file
     \brief The header file of the LinkPath and JointPath classes
-    \author Shin'ichiro Nakaoka
+    \author Shin'ichiro Nakaoka, Rafael Cisneros
 */
 
 #ifndef HRPMODEL_JOINT_PATH_H_INCLUDED
@@ -71,6 +71,8 @@ namespace hrp {
             return J;
         }
 
+        void calcJacobianDot(dmatrix& out_dJ, const Vector3& local_p = Vector3::Zero()) const;
+
         // InverseKinematics Interface
         virtual void setMaxIKError(double e);
         virtual void setBestEffortIKMode(bool on);

hrplib/hrpModel/LinkTraverse.cpp

@@ -11,7 +11,7 @@
 /** 
     \file
     \brief Implementations of the LinkTraverse class
-    \author Shin'ichiro Nakaoka
+    \author Shin'ichiro Nakaoka, Rafael Cisneros
 */
 
 
@@ -203,6 +203,18 @@ void LinkTraverse::calcForwardKinematics(bool calcVelocity, bool calcAcceleratio
 }
 
 
+double LinkTraverse::calcTotalMass()
+{
+  totalMass_ = 0.0;
+
+  for(int i = 0; i < numLinks(); ++i) {
+    totalMass_ += links[i]->m;
+  }
+
+  return totalMass_;
+}
+
+
 std::ostream& operator<<(std::ostream& os, LinkTraverse& traverse)
 {
     int n = traverse.numLinks();

hrplib/hrpModel/LinkTraverse.h

@@ -11,7 +11,7 @@
 /**
    \file
    \brief The header file of the LinkTraverse class
-   \author Shin'ichiro Nakaoka
+   \author Shin'ichiro Nakaoka, Rafael Cisneros
 */
 
 #ifndef HRPMODEL_LINK_TRAVERSE_H_INCLUDED
@@ -80,10 +80,17 @@ namespace hrp {
 
         void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false) const;
 
+        double calcTotalMass();
+
+        inline double totalMass() const {
+            return totalMass_;
+        }
+
       protected:
 
         std::vector<Link*> links;
         int numUpwardConnections;
+        double totalMass_;
 
       private: