From c23c5b7c79b033f0d8f25960ae9195dd6231f42b Mon Sep 17 00:00:00 2001
From: Azamat Shakhimardanov <azamat.shakhimardanov@fh-brs.de>
Date: Fri, 6 Jan 2012 13:29:42 +0100
Subject: [PATCH 2/2] added vereshchagin ID solver with corrections to original from Ruben

---
 orocos_kdl/CMakeLists.txt                     |    9 +-
 orocos_kdl/src/chainidsolver_vereshchagin.cpp |  515 +++++++++++++++++++++++++
 orocos_kdl/src/chainidsolver_vereshchagin.hpp |  171 ++++++++
 3 files changed, 688 insertions(+), 7 deletions(-)
 create mode 100644 orocos_kdl/src/chainidsolver_vereshchagin.cpp
 create mode 100644 orocos_kdl/src/chainidsolver_vereshchagin.hpp

diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt
index ea478d6..256c105 100644
--- a/orocos_kdl/CMakeLists.txt
+++ b/orocos_kdl/CMakeLists.txt
@@ -44,14 +44,9 @@ ENDIF ( NOT CMAKE_BUILD_TYPE )
 
 SET( KDL_CFLAGS "" CACHE INTERNAL "")
 
-#find_package(Eigen 3.0 QUIET)
-#if(NOT Eigen_DIR)
-#include(${PROJ_SOURCE_DIR}/config/FindEigen3.cmake)
-#include_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
-#endif()
-find_package(Eigen 2.0 QUIET)
+find_package(Eigen 3.0 QUIET)
 if(NOT Eigen_DIR)
-include(${PROJ_SOURCE_DIR}/config/FindEigen2.cmake)
+include(${PROJ_SOURCE_DIR}/config/FindEigen3.cmake)
 include_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
 endif()
 INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.cpp b/orocos_kdl/src/chainidsolver_vereshchagin.cpp
new file mode 100644
index 0000000..a7829cb
--- /dev/null
+++ b/orocos_kdl/src/chainidsolver_vereshchagin.cpp
@@ -0,0 +1,515 @@
+// Copyright  (C)  2009
+
+// Version: 1.0
+// Author:
+// Maintainer:
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "chainidsolver_vereshchagin.hpp"
+#include "frames_io.hpp"
+#include "utilities/svd_eigen_HH.hpp"
+
+
+namespace KDL
+{
+using namespace Eigen;
+
+ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin(const Chain& chain_, Twist root_acc, unsigned int _nc) :
+chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(_nc),
+results(ns + 1, segment_info(nc))
+{
+    acc_root = root_acc;
+
+    //Provide the necessary memory for computing the inverse of M0
+    nu_sum.resize(nc);
+    M_0_inverse.resize(nc, nc);
+    Um = MatrixXd::Identity(nc, nc);
+    Vm = MatrixXd::Identity(nc, nc);
+    Sm = VectorXd::Ones(nc);
+    tmpm = VectorXd::Ones(nc);
+}
+
+int ChainIdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques)
+{
+    //Check sizes always
+    if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || f_ext.size() != ns)
+        return -1;
+    if (alfa.columns() != nc || beta.rows() != nc)
+        return -2;
+    //do an upward recursion for position and velocities
+    this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext);
+    //do an inward recursion for inertia, forces and constraints
+    this->downwards_sweep(alfa, torques);
+    //Solve for the constraint forces
+    this->constraint_calculation(beta);
+    //do an upward recursion to propagate the result
+    this->final_upwards_sweep(q_dotdot, torques);
+    return 0;
+}
+
+void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext)
+{
+    //if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns)
+    //        return -1;
+
+    unsigned int j = 0;
+    F_total = Frame::Identity();
+    for (unsigned int i = 0; i < ns; i++)
+    {
+        //Express everything in the segments reference frame (body coordinates)
+        //which is at the segments tip, i.e. where the next joint is attached.
+
+        //Calculate segment properties: X,S,vj,cj
+        const Segment& segment = chain.getSegment(i);
+        segment_info& s = results[i + 1];
+        //The pose between the joint root and the segment tip (tip expressed in joint root coordinates)
+        s.F = segment.pose(q(j)); //X pose of each link in link coord system
+
+        F_total = F_total * s.F; //X pose of the each link in root coord system
+        s.F_base = F_total; //X pose of the each link in root coord system for getter functions
+
+        //The velocity due to the joint motion of the segment expressed in the segments reference frame (tip)
+        Twist vj = s.F.M.Inverse(segment.twist(q(j), qdot(j))); //XDot of each link
+        Twist aj = s.F.M.Inverse(segment.twist(q(j), qdotdot(j))); //XDotDot of each link
+
+        //The unit velocity due to the joint motion of the segment expressed in the segments reference frame (tip)
+        s.Z = s.F.M.Inverse(segment.twist(q(j), 1.0));
+        //Put Z in the joint root reference frame:
+        s.Z = s.F * s.Z;
+      
+        //The total velocity of the segment expressed in the the segments reference frame (tip)
+        if (i != 0)
+        {
+            s.v = s.F.Inverse(results[i].v) + vj; // recursive velocity of each link in segment frame
+            //s.A=s.F.Inverse(results[i].A)+aj;
+            s.A = s.F.M.Inverse(results[i].A);
+        }
+        else
+        {
+            s.v = vj;
+            s.A = s.F.M.Inverse(acc_root);
+        }
+        //c[i] = cj + v[i]xvj (remark: cj=0, since our S is not time dependent in local coordinates)
+        //The velocity product acceleration
+        //std::cout << i << " Initial upward" << s.v << std::endl;
+        s.C = s.v*vj; //This is a cross product: cartesian space BIAS acceleration in local link coord.
+        //Put C in the joint root reference frame
+        s.C = s.F * s.C; //+F_total.M.Inverse(acc_root));
+        //The rigid body inertia of the segment, expressed in the segments reference frame (tip)
+        s.H = segment.getInertia();
+
+        //wrench of the rigid body bias forces and the external forces on the segment (in body coordinates, tip)
+        //external forces are taken into account through s.U. Check why s.U is given as below
+        Wrench FextLocal = F_total.M.Inverse() * f_ext[i];
+        s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i];
+        if (segment.getJoint().getType() != Joint::None)
+            j++;
+    }
+
+}
+
+void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const JntArray &torques)
+{
+    unsigned int j = nj - 1;
+    for (int i = ns; i >= 0; i--)
+    {
+        //Get a handle for the segment we are working on.
+        segment_info& s = results[i];
+        //For segment N,
+        //tilde is in the segment refframe (tip, not joint root)
+        //without tilde is at the joint root (the childs tip!!!)
+        //P_tilde is the articulated body inertia
+        //R_tilde is the sum of external and coriolis/centrifugal forces
+        //M is the (unit) acceleration energy already generated at link i
+        //G is the (unit) magnitude of the constraint forces at link i
+        //E are the (unit) constraint forces due to the constraints
+        if (i == ns)
+        {
+            s.P_tilde = s.H;
+            s.R_tilde = s.U;
+            s.M.setZero();
+            s.G.setZero();
+            //changeBase(alfa_N,F_total.M.Inverse(),alfa_N2);
+            for (unsigned int r = 0; r < 3; r++)
+                for (unsigned int c = 0; c < nc; c++)
+                {
+                    //copy alfa constrain force matrix in E~
+                    s.E_tilde(r, c) = alfa(r + 3, c);
+                    s.E_tilde(r + 3, c) = alfa(r, c);
+                }
+            //Change the reference frame of alfa to the segmentN tip frame
+            //F_Total holds end effector frame, if done per segment bases then constraints could be extended to all segments
+            Rotation base_to_end = F_total.M.Inverse();
+            for (unsigned int c = 0; c < nc; c++)
+            {
+                Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)),
+                           Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c)));
+                col = base_to_end*col;
+                s.E_tilde.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data);
+            }
+        }
+        else
+        {
+            //For all others:
+            //Everything should expressed in the body coordinates of segment i
+            segment_info& child = results[i + 1];
+            //Copy PZ into a vector so we can do matrix manipulations, put torques above forces
+            Vector6d vPZ;
+            vPZ << Vector3d::Map(child.PZ.torque.data), Vector3d::Map(child.PZ.force.data);
+            Matrix6d PZDPZt = (vPZ * vPZ.transpose()).lazy();
+            PZDPZt /= child.D;
+
+            //equation a) (see Vereshchagin89) PZDPZt=[I,H;H',M]
+            //Azamat:articulated body inertia as in Featherstone (7.19)
+            s.P_tilde = s.H + child.P - ArticulatedBodyInertia(PZDPZt.corner < 3, 3 > (BottomRight), PZDPZt.corner < 3, 3 > (TopRight), PZDPZt.corner < 3, 3 > (TopLeft));
+            //equation b) (see Vereshchagin89)
+            //Azamat: bias force as in Featherstone (7.20)
+            s.R_tilde = s.U + child.R + child.PC + (child.PZ / child.D) * child.u;
+            //equation c) (see Vereshchagin89)
+            s.E_tilde = child.E;
+
+            //Azamat: equation (c) right side term
+            s.E_tilde -= (vPZ * child.EZ.transpose()).lazy() / child.D;
+
+            //equation d) (see Vereshchagin89)
+            s.M = child.M;
+            //Azamat: equation (d) right side term
+            s.M -= (child.EZ * child.EZ.transpose()).lazy() / child.D;
+
+            //equation e) (see Vereshchagin89)
+            s.G = child.G;
+            Twist CiZDu = child.C + (child.Z / child.D) * child.u;
+            Vector6d vCiZDu;
+            vCiZDu << Vector3d::Map(CiZDu.rot.data), Vector3d::Map(CiZDu.vel.data);
+            s.G += (child.E.transpose() * vCiZDu).lazy();
+        }
+        if (i != 0)
+        {
+            //Transform all results to joint root coordinates of segment i (== body coordinates segment i-1)
+            //equation a)
+            s.P = s.F * s.P_tilde;
+            //equation b)
+            s.R = s.F * s.R_tilde;
+            //equation c), in matrix: torques above forces, so switch and switch back
+            for (unsigned int c = 0; c < nc; c++)
+            {
+                Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)),
+                           Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c)));
+                col = s.F*col;
+                s.E.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data);
+            }
+
+            //needed for next recursion
+            s.PZ = s.P * s.Z;
+            s.D = dot(s.Z, s.PZ);
+            s.PC = s.P * s.C;
+
+            //u=(Q-Z(R+PC)=sum of external forces along the joint axes,
+            //R are the forces comming from the children,
+            //Q is taken zero (do we need to take the previous calculated torques?
+
+            //projection of coriolis and centrepital forces into joint subspace (0 0 Z)
+            s.totalBias = -dot(s.Z, s.R + s.PC);
+            s.u = torques(j) + s.totalBias;
+
+            //Matrix form of Z, put rotations above translations
+            Vector6d vZ;
+            vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data);
+            s.EZ = (s.E.transpose() * vZ).lazy();
+
+            if (chain.getSegment(i - 1).getJoint().getType() != Joint::None)
+                j--;
+        }
+
+        /*
+        std::cout<<"E~ "<<i<<": "<<s.E_tilde<<std::endl;
+        std::cout<<"R~ "<<i<<": "<<s.R_tilde<<std::endl;
+        std::cout<<"M  "<<i<<": "<<s.M<<std::endl;
+        std::cout<<"G  "<<i<<": "<<s.G<<std::endl;
+         *
+        std::cout<<"E"<<i<<": "<<s.E<<std::endl;
+        std::cout<<"Z: "<<s.Z<<std::endl;
+        std::cout<<"D: "<<s.D<<std::endl;
+        std::cout<<"PZ: "<<s.PZ<<std::endl;
+        std::cout<<"E'Z: "<<s.EZ<<std::endl;
+        std::cout<<"G: "<<s.G<<std::endl;
+        std::cout<<"M: "<<s.M<<std::endl;
+         */
+        /*
+        std::cout<<"For segment "<<i<<std::endl;
+        std::cout<<"D: "<<s.D<<std::endl;
+        std::cout<<"E~: "<<s.E_tilde<<std::endl;
+        std::cout<<"E: "<<s.E<<std::endl;
+
+        std::cout<<"E: "<<s.E<<std::endl;
+        std::cout<<"Z: "<<s.Z.rot<<s.Z.vel<<std::endl;
+        Matrix6d tmp;
+        tmp<<s.P_tilde.I,s.P_tilde.H,s.P_tilde.H.transpose(),s.P_tilde.M;
+        std::cout<<"P~: \n"<<tmp<<std::endl;
+        tmp<<s.P.I,s.P.H,s.P.H.transpose(),s.P.M;
+        std::cout<<"P: \n"<<tmp<<std::endl;
+         */
+    }
+}
+
+void ChainIdSolver_Vereshchagin::constraint_calculation(const JntArray& beta)
+{
+    //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0)
+    //M_0_inverse, always nc*nc symmetric matrix
+    //std::cout<<"M0: "<<results[0].M<<std::endl;
+    //results[0].M-=MatrixXd::Identity(nc,nc);
+    //std::cout<<"augmented M0: "<<results[0].M<<std::endl;
+
+
+    //ToDo: Need to check ill conditions
+
+    //M_0_inverse=results[0].M.inverse();
+    svd_eigen_HH(results[0].M, Um, Sm, Vm, tmpm);
+    //truncated svd, what would sdls, dls physically mean?
+    for (unsigned int i = 0; i < nc; i++)
+        if (Sm(i) < 1e-14)
+            Sm(i) = 0.0;
+        else
+            Sm(i) = 1 / Sm(i);
+
+    results[0].M = (Vm * Sm.asDiagonal()).lazy();
+    M_0_inverse = (results[0].M * Um.transpose()).lazy();
+    //results[0].M.ldlt().solve(MatrixXd::Identity(nc,nc),&M_0_inverse);
+    //results[0].M.computeInverse(&M_0_inverse);
+    Vector6d acc;
+    acc << Vector3d::Map(acc_root.rot.data), Vector3d::Map(acc_root.vel.data);
+    nu_sum = -(results[0].E_tilde.transpose() * acc).lazy();
+    //nu_sum.setZero();
+    nu_sum += beta.data;
+    nu_sum -= results[0].G;
+
+    //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0)
+    nu = (M_0_inverse * nu_sum).lazy();
+}
+
+void ChainIdSolver_Vereshchagin::final_upwards_sweep(JntArray &q_dotdot, JntArray &torques)
+{
+    unsigned int j = 0;
+
+    for (unsigned int i = 1; i <= ns; i++)
+    {
+        segment_info& s = results[i];
+        //Calculation of joint and segment accelerations
+        //equation g) qdotdot[i] = D^-1*(Q - Z'(R + P(C + acc[i-1]) + E*nu))
+        // = D^-1(u - Z'(P*acc[i-1] + E*nu)
+        Twist a_g;
+        Twist a_p;
+        if (i == 1)
+        {
+            a_p = acc_root;
+        }
+        else
+        {
+            a_p = results[i - 1].acc;
+        }
+
+        //The contribution of the constraint forces at segment i
+        Vector6d tmp = s.E*nu;
+        Wrench constraint_force = Wrench(Vector(tmp(3), tmp(4), tmp(5)),
+                                         Vector(tmp(0), tmp(1), tmp(2)));
+
+        //acceleration components are also computed
+        //Contribution of the acceleration of the parent (i-1)
+        Wrench parent_force = s.P*a_p;
+        double parent_forceProjection = -dot(s.Z, parent_force);
+        double parentAccComp = parent_forceProjection / s.D;
+
+        //The constraint force and acceleration force projected on the joint axes -> axis torque/force
+        double constraint_torque = -dot(s.Z, constraint_force);
+        //The result should be the torque at this joint
+
+        torques(j) = constraint_torque;
+        //s.constAccComp = torques(j) / s.D;
+        s.constAccComp = constraint_torque / s.D;
+        s.nullspaceAccComp = s.u / s.D;
+
+        q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp);
+        s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed
+        if (chain.getSegment(i - 1).getJoint().getType() != Joint::None)
+            j++;
+    }
+}
+
+/*
+void ChainIdSolver_Vereshchagin::getLinkCartesianPose(Frames& x_base)
+{
+    for (int i = 0; i < ns; i++)
+    {
+        x_base[i] = results[i + 1].F_base;
+    }
+    return;
+}
+
+void ChainIdSolver_Vereshchagin::getLinkCartesianVelocity(Twists& xDot_base)
+{
+
+    for (int i = 0; i < ns; i++)
+    {
+        xDot_base[i] = results[i + 1].F_base.M * results[i + 1].v;
+    }
+    return;
+}
+
+void ChainIdSolver_Vereshchagin::getLinkCartesianAcceleration(Twists& xDotDot_base)
+{
+
+    for (int i = 0; i < ns; i++)
+    {
+        xDotDot_base[i] = results[i + 1].F_base.M * results[i + 1].acc;
+        //std::cout << "XDotDot_base[i] " << xDotDot_base[i] << std::endl;
+    }
+    return;
+}
+
+void ChainIdSolver_Vereshchagin::getLinkPose(Frames& x_local)
+{
+    for (int i = 0; i < ns; i++)
+    {
+        x_local[i] = results[i + 1].F;
+    }
+    return;
+}
+
+void ChainIdSolver_Vereshchagin::getLinkVelocity(Twists& xDot_local)
+{
+    for (int i = 0; i < ns; i++)
+    {
+        xDot_local[i] = results[i + 1].v;
+    }
+    return;
+
+}
+
+void ChainIdSolver_Vereshchagin::getLinkAcceleration(Twists& xDotdot_local)
+{
+     for (int i = 0; i < ns; i++)
+    {
+        xDotdot_local[i] = results[i + 1].acc;
+    }
+    return;
+
+}
+
+void ChainIdSolver_Vereshchagin::getJointBiasAcceleration(JntArray& bias_q_dotdot)
+{
+    for (int i = 0; i < ns; i++)
+    {
+        //this is only force
+        double tmp = results[i + 1].totalBias;
+        //this is accelleration
+        bias_q_dotdot(i) = tmp / results[i + 1].D;
+
+        //s.totalBias = - dot(s.Z, s.R + s.PC);
+        //std::cout << "totalBiasAccComponent" << i << ": " << bias_q_dotdot(i) << std::endl;
+        //bias_q_dotdot(i) = s.totalBias/s.D
+
+    }
+    return;
+
+}
+
+void ChainIdSolver_Vereshchagin::getJointConstraintAcceleration(JntArray& constraint_q_dotdot)
+{
+    for (int i = 0; i < ns; i++)
+    {
+        constraint_q_dotdot(i) = results[i + 1].constAccComp;
+        //double tmp = results[i + 1].u;
+        //s.u = torques(j) + s.totalBias;
+        // std::cout << "s.constraintAccComp" << i << ": " << results[i+1].constAccComp << std::endl;
+        //nullspace_q_dotdot(i) = s.u/s.D
+
+    }
+    return;
+
+
+}
+
+//Check the name it does not seem to be appropriate
+
+void ChainIdSolver_Vereshchagin::getJointNullSpaceAcceleration(JntArray& nullspace_q_dotdot)
+{
+    for (int i = 0; i < ns; i++)
+    {
+        nullspace_q_dotdot(i) = results[i + 1].nullspaceAccComp;
+        //double tmp = results[i + 1].u;
+        //s.u = torques(j) + s.totalBias;
+        //std::cout << "s.nullSpaceAccComp" << i << ": " << results[i+1].nullspaceAccComp << std::endl;
+        //nullspace_q_dotdot(i) = s.u/s.D
+
+    }
+    return;
+
+
+}
+
+//This is not only a bias force energy but also includes generalized forces
+//change type of parameter G
+//this method should retur array of G's
+
+void ChainIdSolver_Vereshchagin::getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G)
+{
+    for (int i = 0; i < ns; i++)
+    {
+        G = results[i + 1].G;
+        //double tmp = results[i + 1].u;
+        //s.u = torques(j) + s.totalBias;
+        //std::cout << "s.G " << i << ":  " << results[i+1].G << std::endl;
+        //nullspace_q_dotdot(i) = s.u/s.D
+
+    }
+    return;
+
+}
+
+//this method should retur array of R's
+
+void ChainIdSolver_Vereshchagin::getLinkBiasForceMatrix(Wrenches& R_tilde)
+{
+    for (int i = 0; i < ns; i++)
+    {
+        R_tilde[i] = results[i + 1].R_tilde;
+        //Azamat: bias force as in Featherstone (7.20)
+        //s.R_tilde = s.U + child.R + child.PC + child.PZ / child.D * child.u;
+        std::cout << "s.R_tilde " << i << ":  " << results[i + 1].R_tilde << std::endl;
+    }
+    return;
+}
+
+//this method should retur array of M's
+
+void ChainIdSolver_Vereshchagin::getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M)
+{
+
+
+}
+
+//this method should retur array of E_tilde's
+
+void ChainIdSolver_Vereshchagin::getLinkUnitForceMatrix(Matrix6Xd& E_tilde)
+{
+
+
+}
+
+*/
+
+}//namespace
diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.hpp b/orocos_kdl/src/chainidsolver_vereshchagin.hpp
new file mode 100644
index 0000000..732f346
--- /dev/null
+++ b/orocos_kdl/src/chainidsolver_vereshchagin.hpp
@@ -0,0 +1,171 @@
+// Copyright  (C)  2009
+
+// Version: 1.0
+// Author:
+// Maintainer:
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
+#define KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
+
+#include "chainidsolver.hpp"
+#include "frames.hpp"
+#include "articulatedbodyinertia.hpp"
+
+namespace KDL {
+    /**
+     * \brief Dynamics calculations by constraints based on Vereshchagin 1989.
+     *
+     * For a chain
+     */
+    typedef std::vector<Twist> Twists;
+    typedef std::vector<Frame> Frames;
+
+    typedef Eigen::Matrix<double, 6, 1 > Vector6d;
+    typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
+    typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;
+
+    class ChainIdSolver_Vereshchagin {
+    public:
+        /**
+         * Constructor for the solver, it will allocate all the necessary memory
+         * \param chain The kinematic chain to calculate the inverse dynamics for, an internal copy will be made.
+         * \param root_acc The acceleration vector of the root to use during the calculation.(most likely contains gravity)
+         *
+         */
+        ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc);
+
+        ~ChainIdSolver_Vereshchagin() {
+        };
+      int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques);
+
+        /**
+         * Function to calculate from Cartesian forces to joint torques.
+         * Input parameters;
+         * \param q The current joint positions
+         * \param q_dot The current joint velocities
+         * \param f_ext The external forces (no gravity) on the segments
+         * Output parameters:
+         * \param q_dotdot The joint accelerations
+         * \param torques the resulting torques for the joints
+         */
+        /*
+
+        //Returns cartesian positions of links in base coordinates
+        void getLinkCartesianPose(Frames& x_base);
+        //Returns cartesian velocities of links in base coordinates
+        void getLinkCartesianVelocity(Twists& xDot_base);
+        //Returns cartesian acceleration of links in base coordinates
+        void getLinkCartesianAcceleration(Twists& xDotDot_base);
+        //Returns cartesian postions of links in link tip coordinates
+        void getLinkPose(Frames& x_local);
+        //Returns cartesian velocities of links in link tip coordinates
+        void getLinkVelocity(Twists& xDot_local);
+        //Returns cartesian acceleration of links in link tip coordinates
+        void getLinkAcceleration(Twists& xDotdot_local);
+        //Acceleration energy due to unit constraint forces at the end-effector
+        void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M);
+        //Acceleration energy due to arm configuration: bias force plus input joint torques
+        void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G);
+
+        void getLinkUnitForceMatrix(Matrix6Xd& E_tilde);
+
+        void getLinkBiasForceMatrix(Wrenches& R_tilde);
+
+        void getJointBiasAcceleration(JntArray &bias_q_dotdot);
+
+        void getJointConstraintAcceleration(JntArray &constraint_q_dotdot);
+
+        void getJointNullSpaceAcceleration(JntArray &nullspace_q_dotdot);
+        */
+private:
+        //Functions to calculate velocity, propagated inertia, propagated bias forces, constraint forces and accelerations
+        void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);
+        void downwards_sweep(const Jacobian& alfa, const JntArray& torques);
+        void constraint_calculation(const JntArray& beta);
+        void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques);
+
+    private:
+        Chain chain;
+        unsigned int nj;
+        unsigned int ns;
+        unsigned int nc;
+        Twist acc_root;
+        Jacobian alfa_N;
+        Jacobian alfa_N2;
+        Eigen::MatrixXd M_0_inverse;
+        Eigen::MatrixXd Um;
+        Eigen::MatrixXd Vm;
+        JntArray beta_N;
+        Eigen::VectorXd nu;
+        Eigen::VectorXd nu_sum;
+        Eigen::VectorXd Sm;
+        Eigen::VectorXd tmpm;
+        Wrench qdotdot_sum;
+        Frame F_total;
+
+        struct segment_info {
+            Frame F; //local pose with respect to previous link in segments coordinates
+            Frame F_base; // pose of a segment in root coordinates
+            Twist Z; //Unit twist
+            Twist v; //twist
+            Twist acc; //acceleration twist
+            Wrench U; //wrench p of the bias forces (in cartesian space)
+            Wrench R; //wrench p of the bias forces
+            Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form
+            Twist C; //constraint
+            Twist A; //constraint
+            ArticulatedBodyInertia H; //I (expressed in 6*6 matrix)
+            ArticulatedBodyInertia P; //I (expressed in 6*6 matrix)
+            ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix)
+            Wrench PZ; //vector U[i] = I_A[i]*S[i]
+            Wrench PC; //vector E[i] = I_A[i]*c[i]
+            double D; //vector D[i] = S[i]^T*U[i]
+            Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints
+            Matrix6Xd E_tilde;
+            Eigen::MatrixXd M; //acceleration energy already generated at link i
+            Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i
+            Eigen::VectorXd EZ; //K[i] = Etiltde'*Z
+
+
+            double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration
+            double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration
+            double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration
+
+            double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace
+            double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias
+
+            segment_info(unsigned int nc) {
+                E.resize(6, nc);
+                E_tilde.resize(6, nc);
+                G.resize(nc);
+                M.resize(nc, nc);
+                EZ.resize(nc);
+                E.setZero();
+                E_tilde.setZero();
+                M.setZero();
+                G.setZero();
+                EZ.setZero();
+            };
+        };
+
+        std::vector<segment_info> results;
+
+    };
+}
+
+#endif
-- 
1.7.2.5

