Index: src/segment.cpp
===================================================================
--- src/segment.cpp	(revision 29874)
+++ src/segment.cpp	(working copy)
@@ -21,15 +21,16 @@

 namespace KDL {

-    Segment::Segment(const Joint& _joint, const Frame& _f_tip, const Inertia& _M):
+    Segment::Segment(const Joint& _joint, const Frame& _f_tip, const Inertia& _M, const Vector& _cog):
         joint(_joint),M(_M),
-        f_tip(_f_tip)
+        f_tip(_joint.pose(0).Inverse() * _f_tip),
+	cog(_cog)
     {
     }

     Segment::Segment(const Segment& in):
         joint(in.joint),M(in.M),
-        f_tip(in.f_tip)
+        f_tip(in.f_tip), cog(in.cog)
     {
     }

@@ -38,6 +39,7 @@
         joint=arg.joint;
         M=arg.M;
         f_tip=arg.f_tip;
+	cog=arg.cog;
         return *this;
     }

@@ -52,7 +54,7 @@

     Twist Segment::twist(const double& q, const double& qdot)const
     {
-        return joint.twist(qdot).RefPoint(pose(q).p);
+        return joint.twist(qdot).RefPoint(joint.pose(q).M * f_tip.p);
     }

 }//end of namespace KDL
Index: src/segment.hpp
===================================================================
--- src/segment.hpp	(revision 29874)
+++ src/segment.hpp	(working copy)
@@ -49,6 +49,7 @@
         Joint joint;
         Inertia M;
         Frame f_tip;
+        Vector cog;  // Vector of center of mass from frame origin in local coordinates.

     public:
         /**
@@ -59,8 +60,10 @@
          * @param f_tip frame from the end of the joint to the tip of
          * the segment, default: Frame::Identity()
          * @param M rigid body inertia of the segment, default: Inertia::Zero()
-         */
-        Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const Inertia& M = Inertia::Zero());
+         * @param cog Vector from segment frame to center of gravity
+        */
+        Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),
+		const Inertia& M = Inertia::Zero(), const Vector& cog = Vector::Zero());
         Segment(const Segment& in);
         Segment& operator=(const Segment& arg);

@@ -108,6 +111,17 @@
             return M;
         }

+        /**
+         * Request the location of center of mass of the segment
+         *
+         *
+         * @return const reference to the center of mass (vector) of the segment
+         */
+        const Vector& getCOG() const
+        {
+	  return cog;
+	}
+
         /**
          * Request the pose from the joint end to the tip of the
          *segment.