void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, ChainIkSolverPos& iksolverpos)
{
    JntArray q(chain.getNrOfJoints());
    for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
    {
        random(q(i));
    }
    JntArray q_init(chain.getNrOfJoints());
    double tmp;
    for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
    {
        random(tmp);
        q_init(i)=q(i)+0.1*tmp;
    }
    JntArray q_solved(q);

    Frame F1,F2;

    CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F1));
    CPPUNIT_ASSERT(0 <= iksolverpos.CartToJnt(q_init,F1,q_solved));
    CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q_solved,F2));

    CPPUNIT_ASSERT_EQUAL(F1,F2);
    //CPPUNIT_ASSERT_EQUAL(q,q_solved);

}
void SolverTest::FkVelAndIkVelLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainIkSolverVel& iksolvervel)
{

    JntArray q(chain.getNrOfJoints());
    JntArray qdot(chain.getNrOfJoints());

    for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
    {
        random(q(i));
        random(qdot(i));
    }
    JntArrayVel qvel(q,qdot);
    JntArray qdot_solved(chain.getNrOfJoints());

    FrameVel cart;

    CPPUNIT_ASSERT(0==fksolvervel.JntToCart(qvel,cart));

    int ret = iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved);
    CPPUNIT_ASSERT(0<=ret);

    qvel.deriv()=qdot_solved;

    if(chain.getNrOfJoints()<=6)
        CPPUNIT_ASSERT(Equal(qvel.qdot,qdot_solved,1e-5));
    else
    {
        FrameVel cart_solved;
        CPPUNIT_ASSERT(0==fksolvervel.JntToCart(qvel,cart_solved));
        CPPUNIT_ASSERT(Equal(cart.deriv(),cart_solved.deriv(),1e-5));
    }
}
void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
{
    double deltaq = 1E-4;

    Frame F1,F2;

    JntArray q(chain.getNrOfJoints());
    Jacobian jac(chain.getNrOfJoints());

    for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
    {
        random(q(i));
    }

    jacsolver.JntToJac(q,jac);

    for (unsigned int i=0; i< q.rows() ; i++)
    {
        // test the derivative of J towards qi
        double oldqi = q(i);
        q(i) = oldqi+deltaq;
        CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F2));
        q(i) = oldqi-deltaq;
        CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F1));
        q(i) = oldqi;
        // check Jacobian :
        Twist Jcol1 = diff(F1,F2,2*deltaq);
        Twist Jcol2(Vector(jac(0,i),jac(1,i),jac(2,i)),
                    Vector(jac(3,i),jac(4,i),jac(5,i)));

        //CPPUNIT_ASSERT_EQUAL(true,Equal(Jcol1,Jcol2,epsJ));
        CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
    }
}
ChainIkSolverPos_LMA_JL_Mimic::ChainIkSolverPos_LMA_JL_Mimic(const Chain& _chain,
                                                           const JntArray& _q_min,
                                                           const JntArray& _q_max,
                                                           ChainFkSolverPos& _fksolver,
                                                           ChainIkSolverPos_LMA& _iksolver,
                                                           unsigned int _maxiter,
                                                           double _eps,
                                                           bool _position_ik)
  : chain(_chain),
    q_min(_q_min),
    q_min_mimic(chain.getNrOfJoints()),
    q_max(_q_max),
    q_max_mimic(chain.getNrOfJoints()),
    q_temp(chain.getNrOfJoints()),
    fksolver(_fksolver),
    iksolver(_iksolver),
    delta_q(_chain.getNrOfJoints()),
    maxiter(_maxiter),
    eps(_eps),
    position_ik(_position_ik)
{
  mimic_joints.resize(chain.getNrOfJoints());
  for(std::size_t i=0; i < mimic_joints.size(); ++i)
  { 
    mimic_joints[i].reset(i);
  }
}
 ChainIkSolverPos_NR::ChainIkSolverPos_NR(const Chain& _chain,ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver,
                                          unsigned int _maxiter, double _eps):
     chain(_chain),nj (chain.getNrOfJoints()),
     iksolver(_iksolver),fksolver(_fksolver),
     delta_q(_chain.getNrOfJoints()),
     maxiter(_maxiter),eps(_eps)
 {
 }
示例#6
0
int VerifyJacobian::ComputeMaxError(const std::string& model_file, const std::string& joint_params_file, const std::string& jacobians_file, double& max_error)
{
  int result ;
  
  result = model_getter_.OpenFile(model_file) ;
  if (result < 0)
    return -1 ;

  result = joint_params_getter_.OpenFile(joint_params_file) ;
  if (result < 0)
    return -1 ;

  result = jacobians_getter_.OpenFile(jacobians_file) ;
  if (result < 0)
    return -1 ;
  
  Chain chain = model_getter_.GetModel() ;

  const unsigned int J = chain.getNrOfJoints() ;
  JntArray joint_array(J) ;
  result = joint_params_getter_.GetNextJointArray(joint_array) ;
  if (result < 0)
    return -1 ;
  
  LinkParamJacobian jac_actual ;
  jac_actual.twists_.resize(J) ;
  result = jacobians_getter_.GetNextJacobian(jac_actual) ;

  // Compute the jacobian using KDL functions
  
  LinkParamJacobian jac_computed ;
  jac_computed.twists_.resize(J) ;
  
  LinkParamJacobianSolver jac_solver ;
  jac_solver.JointsToCartesian(chain, joint_array, jac_computed) ;
  
  max_error = 0.0 ;
  for (unsigned int i=0; i < jac_computed.twists_.size(); i++)
  {
    for (unsigned int j=0; j<3; j++)
    {
      for (unsigned int k=0; k<3; k++)
      {
        double cur_error ;
        cur_error = fabs( jac_computed.twists_[i].trans_[j].vel(k) - jac_actual.twists_[i].trans_[j].vel(k) ) ;
        if (cur_error > max_error)
          max_error = cur_error ;
        
        cur_error = fabs( jac_computed.twists_[i].rot_[j].vel(k) - jac_actual.twists_[i].rot_[j].vel(k) ) ;
        if (cur_error > max_error)
          max_error = cur_error ;
      }
    }
  }    
  
  return 0 ;
}
void SolverTest::FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver)
{
    JntArray q(chain.getNrOfJoints());
    JntArray qdot(chain.getNrOfJoints());

    for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
    {
        random(q(i));
        random(qdot(i));
    }
    JntArrayVel qvel(q,qdot);
    Jacobian jac(chain.getNrOfJoints());

    FrameVel cart;
    Twist t;

    jacsolver.JntToJac(qvel.q,jac);
    CPPUNIT_ASSERT(fksolvervel.JntToCart(qvel,cart)==0);
    MultiplyJacobian(jac,qvel.qdot,t);
    CPPUNIT_ASSERT_EQUAL(cart.deriv(),t);
}
double compare_Jdot_Diff_vs_Solver(const Chain& chain,const double& dt,const int& representation,bool verbose)
{
    // This test verifies if the solvers gives approx. the same result as [ J(q+qdot*dot) - J(q) ]/dot
    JntArray q(chain.getNrOfJoints());
    JntArray qdot(chain.getNrOfJoints());
    JntArray q_dqdt(chain.getNrOfJoints());


    random(q);
    random(qdot);
    q_dqdt = diff(q,qdot,dt);
    
    ChainJntToJacDotSolver jdot_solver(chain);
    ChainJntToJacSolver j_solver(chain);
    ChainFkSolverPos_recursive fk_solver(chain);

    Frame F_bs_ee_q,F_bs_ee_q_dqdt;
    Jacobian jac_q(chain.getNrOfJoints()),
                jac_q_dqdt(chain.getNrOfJoints()),
                jdot_by_diff(chain.getNrOfJoints());
                
    j_solver.JntToJac(q,jac_q);
    j_solver.JntToJac(q_dqdt,jac_q_dqdt);  
    
    fk_solver.JntToCart(q,F_bs_ee_q);
    fk_solver.JntToCart(q_dqdt,F_bs_ee_q_dqdt);
    
    changeRepresentation(jac_q,F_bs_ee_q,representation);
    changeRepresentation(jac_q_dqdt,F_bs_ee_q_dqdt,representation);
    
    Jdot_diff(jac_q,jac_q_dqdt,dt,jdot_by_diff);

    Jacobian jdot_by_solver(chain.getNrOfJoints());
    jdot_solver.setRepresentation(representation);
    jdot_solver.JntToJacDot(JntArrayVel(q_dqdt,qdot),jdot_by_solver);

    Twist jdot_qdot_by_solver;
    MultiplyJacobian(jdot_by_solver,qdot,jdot_qdot_by_solver);

    Twist jdot_qdot_by_diff;
    MultiplyJacobian(jdot_by_diff,qdot,jdot_qdot_by_diff);
    
    if(verbose){
        std::cout << "Jdot diff : \n" << jdot_by_diff<<std::endl;
        std::cout << "Jdot solver:\n"<<jdot_by_solver<<std::endl;
        
        std::cout << "Error : " <<jdot_qdot_by_diff-jdot_qdot_by_solver<<q<<qdot<<std::endl;
    }
    double err = jdot_qdot_by_diff.vel.Norm() - jdot_qdot_by_solver.vel.Norm()
                  + jdot_qdot_by_diff.rot.Norm() - jdot_qdot_by_solver.rot.Norm();
    return std::abs(err);
}
Expression_Chain::Expression_Chain( const Chain& _chain, int _index_of_first_joint ):
    FunctionType("kinematic_chain"),
    chain(_chain),
    jointndx_to_segmentndx( _chain.getNrOfJoints()),
    jval( _chain.getNrOfJoints() ),
    T_base_jointroot( _chain.getNrOfJoints()),
    T_base_jointtip(  _chain.getNrOfJoints()),
    jacobian( _chain.getNrOfJoints() ),
    cached_deriv( _chain.getNrOfJoints() ),
    cached(false),
    index_of_first_joint(_index_of_first_joint)
{
    using namespace std;
    fill(jval.begin(),jval.end(),0.0);
    fill(cached_deriv.begin(),cached_deriv.end(),false);
    _number_of_derivatives=index_of_first_joint+_chain.getNrOfJoints();
}
//#include <chainidsolver_constraint_vereshchagin.hpp>
int main()
{

    using namespace KDL;


    //Definition of kinematic chain
    //-----------------------------------------------------------------------------------------------//
    //Joint (const JointType &type=None, const double &scale=1, const double &offset=0,
    //       const double &inertia=0, const double &damping=0, const double &stiffness=0)
    Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
    Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);
    //Joint rotJoint1 = Joint(Joint::None,1,0,0.01);

    Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
    Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
    Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
    //Segment (const Joint &joint=Joint(Joint::None),
    //         const Frame &f_tip=Frame::Identity(), const RigidBodyInertia &I=RigidBodyInertia::Zero())
    Segment segment1 = Segment(rotJoint0, frame1);
    Segment segment2 = Segment(rotJoint1, frame2);

    // 	RotationalInertia (double Ixx=0, double Iyy=0, double Izz=0, double Ixy=0, double Ixz=0, double Iyz=0)
    RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
    //RigidBodyInertia (double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
    RigidBodyInertia inerSegment1(1.0, Vector(0.0, -0.2, 0.0), rotInerSeg1);
    RigidBodyInertia inerSegment2(1.0, Vector(0.0, -0.2, 0.0), rotInerSeg1);
    segment1.setInertia(inerSegment1);
    segment2.setInertia(inerSegment2);

    Chain chain;
    chain.addSegment(segment1);
    //chain.addSegment(segment2);
    //----------------------------------------------------------------------------------------------//

    //Definition of constraints and external disturbances
    //--------------------------------------------------------------------------------------//
    JntArray arrayOfJoints(chain.getNrOfJoints());
    //Constraint force matrix at the end-effector
    //What is the convention for the spatial force matrix; is it the same as in thesis?
    Vector constrainXLinear(0.0, 1.0, 0.0);
    Vector constrainXAngular(0.0, 0.0, 0.0);
    Vector constrainYLinear(0.0, 0.0, 0.0);
    Vector constrainYAngular(0.0, 0.0, 0.0);
    Vector constrainZLinear(0.0, 0.0, 0.0);
    Vector constrainZAngular(0.0, 0.0, 0.0);
    const Twist constraintForcesX(constrainXLinear, constrainXAngular);
    const Twist constraintForcesY(constrainYLinear, constrainYAngular);
    const Twist constraintForcesZ(constrainZLinear, constrainZAngular);
    Jacobian alpha(1);
    alpha.setColumn(0, constraintForcesX);
    //alpha.setColumn(1,constraintForcesY);
    //alpha.setColumn(2,constraintForcesZ);

    //Acceleration energy at  the end-effector
    JntArray betha(1); //set to zero
    betha(0) = 0.0;
    //betha(1) = 0.0;
    //betha(2) = 0.0;

    //arm root acceleration
    Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
    Vector angularAcc(0.0, 0.0, 0.0);
    Twist twist1(linearAcc, angularAcc);

    //external forces on the arm
    Vector externalForce1(0.0, 0.0, 0.0);
    Vector externalTorque1(0.0, 0.0, 0.0);
    Vector externalForce2(0.0, 0.0, 0.0);
    Vector externalTorque2(0.0, 0.0, 0.0);
    Wrench externalNetForce1(externalForce1, externalTorque1);
    Wrench externalNetForce2(externalForce2, externalTorque2);
    Wrenches externalNetForce;
    externalNetForce.push_back(externalNetForce1);
    //externalNetForce.push_back(externalNetForce2);
    //-------------------------------------------------------------------------------------//

    //solvers
    ChainFkSolverPos_recursive fksolver(chain);
    //ChainFkSolverVel_recursive fksolverVel(chain);
    int numberOfConstraints = 1;
    ChainIdSolver_Vereshchagin constraintSolver(chain, twist1, numberOfConstraints);

    // Initial arm position configuration/constraint
    Frame frameInitialPose;
    JntArray jointInitialPose(chain.getNrOfJoints());
    jointInitialPose(0) = M_PI/6.0; // initial joint0 pose
    //jointInitialPose(1) = M_PI/6.0;                          //initial joint1 pose, negative in clockwise

    //cartesian space/link accelerations
    Twist accLink0;
    Twist accLink1;
    Twists cartAcc;
    cartAcc.push_back(accLink0);
    //cartAcc.push_back(accLink1);

    //arm joint rate configuration and initial values are zero
    JntArray qDot(chain.getNrOfJoints());
    //arm joint accelerations returned by the solver
    JntArray qDotDot(chain.getNrOfJoints());
    
    //arm joint torques returned by the solver
    JntArray qTorque(chain.getNrOfJoints());

    arrayOfJoints(0) = jointInitialPose(0);
    //arrayOfJoints(1) = jointInitialPose(1);

    //Calculate joint values for each cartesian position
    Frame frameEEPose;
    Twist frameEEVel;
    double TimeConstant = 1; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
    double timeDelta = 0.002;
    bool status;

    Frame cartX1;
    Frame cartX2;
    Frames cartX;
    cartX.push_back(cartX1);
    cartX.push_back(cartX2);
    Twists cartXDot;
    cartXDot.push_back(accLink0);
    cartXDot.push_back(accLink1);

    Twists cartXDotDot;
    cartXDotDot.push_back(accLink0);
    cartXDotDot.push_back(accLink1);


    for (double t = 0.0; t <= 5*TimeConstant; t = t + timeDelta)
    {
        //at time t0, inputs are arrayOfjoints(0,pi/2.0), qdot(0,0)
        //qDot(0) = 0.5;
        // qDot(1) = 0.5;

        //printf("Inside the main loop %f  %f\n",qTorque(0), qTorque(1));
        

        //NOTE AZAMAT:what is qTorque? in implementation returned qTorque is constraint torque generated by the virtual constraint forces.
        // these torques are required to satify a virtual constraint and basically are real torques applied to joints to achieve
        // these constraint. But if additionally to the constraint generated torques we also had internal motor torques, then should not they
        //be added together?
        
        //qTorque(1) = qTorque(1)+internalJointTorque1;

        // In what frame of reference are the results expressed?

        status = constraintSolver.CartToJnt(arrayOfJoints, qDot, qDotDot, alpha, betha, externalNetForce, qTorque);
        if (status >= 0)
        {
            //For 2-dof arm
            //printf("%f          %f %f       %f %f       %f %f           %f %f           %f %f\n",t , arrayOfJoints(0), arrayOfJoints(1), qDot(0), qDot(1), qDotDot(0), qDotDot(1), qTorque(0), qTorque(1), frameEEPose.p.x(), frameEEPose.p.y());
            //printf("%f          %f %f       %f %f       %f %f\n", t, cartAcc[0].vel.x(), cartAcc[0].vel.y(), cartAcc[0].rot.z(), cartAcc[1].vel.x(), cartAcc[1].vel.y(), cartAcc[1].rot.z());

            //For 1-dof
            
            printf("Joint actual %f          %f       %f        %f            %f\n", t, arrayOfJoints(0), qDot(0), qDotDot(0), qTorque(0));
            std::cout<<std::endl;
        }
        constraintSolver.getLinkCartesianPose(cartX);
        constraintSolver.getLinkCartesianVelocity(cartXDot);
        constraintSolver.getLinkCartesianAcceleration(cartXDotDot);
        constraintSolver.getLinkAcceleration(cartAcc);
        printf("Cartesian acc local %f          %f       %f        %f\n", t, cartAcc[0].vel.x(), cartAcc[0].vel.y(), cartAcc[0].rot.z());
        printf("Cartesian actual %f          %f      %f     %f      %f\n", t, cartX[0].p.x(), cartX[0].p.y(), cartXDot[0].vel[0], cartXDot[0].vel[1]);
        std::cout<<std::endl;
        //printf("External torque %f\n", externalNetForce[0].torque.z());
        //Integration at the given "instanteneous" interval for joint position and velocity.
        //These will be inputs for the next time instant
        //actually here it should be a time interval and not time from the beginning, that is why timeDelta;
        arrayOfJoints(0) = arrayOfJoints(0) + (qDot(0) + qDotDot(0) * timeDelta / 2.0) * timeDelta;
        qDot(0) = qDot(0) + qDotDot(0) * timeDelta;
        //arrayOfJoints(1) = arrayOfJoints(1) + (qDot(1) + qDotDot(1)*timeDelta/2.0)*timeDelta;
        //qDot(1) = qDot(1)+qDotDot(1)*timeDelta;



        //For cartesian space control one needs to calculate from the obtained joint space value, new cartesian space poses.
        //Then based on the difference of the signal (desired-actual) we define a regulation function (controller)
        // this difference should be compensated either by constraint forces or by joint torques.
        // The current version of the algorithm assumes that we don't have control over torques, thus change in constraint forces
        //should be a valid option.
    }

    //torque1=[(m1+m2)a1^2+m2a2^2+2m2a1a2cos2]qDotDot1+[m2a2^2+m2a1a2cos2]qDotDot2-m2a1a2(2qDot1qDot2+qDot2^2)sin2+(m1+m2)ga1cos2+m2ga2cos(1+2)

    //torque2=[m2a2^2+m2a1a2cos2]qDotDot1+m2a2^2qDotDot2+m2a1a2qDot1^2sin2+m2ga2cos(1+2)

    return 0;
}
int main()
{

    using namespace KDL;


    //Definition of kinematic chain
    //-----------------------------------------------------------------------------------------------//
    //Joint (const JointType &type=None, const double &scale=1, const double &offset=0, const double &inertia=0, const double &damping=0, const double &stiffness=0)
    Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
    Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);

    Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
    Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
    Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
    //Segment (const Joint &joint=Joint(Joint::None), const Frame &f_tip=Frame::Identity(), const RigidBodyInertia &I=RigidBodyInertia::Zero())
    Segment segment1 = Segment(rotJoint0, frame1);
    Segment segment2 = Segment(rotJoint1, frame2);

    // 	RotationalInertia (double Ixx=0, double Iyy=0, double Izz=0, double Ixy=0, double Ixz=0, double Iyz=0)
    RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
    //RigidBodyInertia (double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
    RigidBodyInertia inerSegment1(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
    RigidBodyInertia inerSegment2(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
    segment1.setInertia(inerSegment1);
    segment2.setInertia(inerSegment2);

    Chain chain;
    chain.addSegment(segment1);
    chain.addSegment(segment2);
    //~Definition of kinematic chain
    //----------------------------------------------------------------------------------------------//



    //Definition of constraints and external disturbances
    //--------------------------------------------------------------------------------------//

    //Constraint force matrix at the end-effector
    //What is the convention for the spatial force matrix; is it the same as in thesis?
    //NOTE AZAMAT: Constraint are also defined in local reference frame?!
    Vector constrainXLinear(1.0, 0.0, 0.0);
    Vector constrainXAngular(0.0, 0.0, 0.0);
    Vector constrainYLinear(0.0, 0.0, 0.0);
    Vector constrainYAngular(0.0, 0.0, 0.0);
    Vector constrainZLinear(0.0, 0.0, 0.0);
    Vector constrainZAngular(0.0, 0.0, 0.0);
    Twist constraintForcesX(constrainXLinear, constrainXAngular);
    Twist constraintForcesY(constrainYLinear, constrainYAngular);
    Twist constraintForcesZ(constrainZLinear, constrainZAngular);
    Jacobian alpha(3);
    alpha.setColumn(0, constraintForcesX);
    alpha.setColumn(1, constraintForcesY);
    alpha.setColumn(2, constraintForcesZ);

    //Acceleration energy at  the end-effector
    JntArray betha(3); //set to zero
    betha(0) = 0.0;
    betha(1) = 0.0;
    betha(2) = 0.0;

    //arm root acceleration
    Vector linearAcc(0.0, 9.8, 0.0); //gravitational acceleration along Y
    Vector angularAcc(0.0, 0.0, 0.0);
    Twist twist1(linearAcc, angularAcc);

    //external forces on the arm
    Vector externalForce1(0.0, 0.0, 0.0);
    Vector externalTorque1(0.0, 0.0, 0.0);
    Vector externalForce2(0.0, 0.0, 0.0);
    Vector externalTorque2(0.0, 0.0, 0.0);
    Wrench externalNetForce1(externalForce1, externalTorque1);
    Wrench externalNetForce2(externalForce2, externalTorque2);
    Wrenches externalNetForce;
    externalNetForce.push_back(externalNetForce1);
    externalNetForce.push_back(externalNetForce2);
    //~Definition of constraints and external disturbances
    //-------------------------------------------------------------------------------------//


    //Definition of solver and initial configuration
    //-------------------------------------------------------------------------------------//
    int numberOfConstraints = 3;
    ChainIdSolver_Vereshchagin constraintSolver(chain, twist1, numberOfConstraints);

    //These arrays of joint values contains values for 3
    //consequetive steps
    int k = 3;
    JntArray jointPoses[k];
    JntArray jointRates[k];
    JntArray jointAccelerations[k];
    JntArray jointTorques[k];
    JntArray biasqDotDot(chain.getNrOfJoints());
    for (int i = 0; i < k; i++)
    {
        JntArray jointValues(chain.getNrOfJoints());
        jointPoses[i] = jointValues;
        jointRates[i] = jointValues;
        jointAccelerations[i] = jointValues;
        jointTorques[i] = jointValues;
    }


    //cartesian space/link values
    Frames cartX[k];
    Twists cartXDot[k];
    Twists cartXDotDot[k];
    Twist accLink;
    for (int i = 0; i < k; i++) //i is number of variables (actual, desired, error)
    {
        for (int j = 0; j < k - 1; j++) //j is number of links
        {
            cartX[i].push_back(frame1);
            cartXDot[i].push_back(accLink);
            cartXDotDot[i].push_back(accLink);
        }

    }

    //at time=0.0 positions, rates and accelerations
    JntArray jointInitialPose(chain.getNrOfJoints());
    jointInitialPose(0) = 0.0; // initial joint0 pose
    jointInitialPose(1) = M_PI / 6.0; //initial joint1 pose, negative in clockwise
    //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
    //j0=pi/6.0, j1=pi/6.0 correspond to x = 0.5464, y = -0.5464
    //~Definition of solver and initial configuration
    //-------------------------------------------------------------------------------------//

    //First iteration to kick start integration process, because we need 0,1 iterations for the integration process
    //We use Euler method for the iteration 0
    double timeDelta = 0.001; //sampling interval/step size
    double TimeConstant = 60; //Time required to complete the task
    double Kp = 150.0;
    double Kv = 10.0;

    //Interpolator parameters:
    double b0_y = -0.7464102; //should come from initial joint configuration
    double b1_y = 0.0;
    double b2_y = 0.0; //((0.5 + 0.7464)*3.0 / TimeConstant * TimeConstant);
    double b3_y = 0.0; //-((0.5 + 0.7464)*2.0 / TimeConstant * TimeConstant * TimeConstant);

    double b0_x = 0.2; //should come from initial joint configuration
    double b1_x = 0.0;
    double b2_x = 0.0; //((0.5 + 0.7464)*3.0 / TimeConstant * TimeConstant);
    double b3_x = 0.0; //-((0.5 + 0.7464)*2.0 / TimeConstant * TimeConstant * TimeConstant);

    //Desired
    cartX[1][1].p[0] = b0_x;
    cartX[1][1].p[1] = b0_y;
    cartXDot[1][1].vel[0] = 0.0;
    cartXDot[1][1].vel[1] = 0.0;
    cartXDotDot[1][1].vel[0] = 0.0;
    cartXDotDot[1][1].vel[1] = 0.0;
    
    //step 0
    jointPoses[0](0) = jointInitialPose(0);
    jointPoses[0](1) = jointInitialPose(1);

    constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], cartXDotDot[0], alpha, betha, externalNetForce, jointTorques[0]);
    //Actual
    constraintSolver.getLinkCartesianPose(cartX[0]);
    constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
    
    //Error
    cartX[2][1].p[0] = cartX[1][1].p[0] - cartX[0][1].p[0];
    cartX[2][1].p[1] = cartX[1][1].p[1] - cartX[0][1].p[1];
    cartXDot[2][1].vel[0] = cartXDot[1][1].vel[0] - cartXDot[0][1].vel[0];
    cartXDot[2][1].vel[1] = cartXDot[1][1].vel[1] - cartXDot[0][1].vel[1];
    cartXDotDot[2][1].vel[0] = cartXDotDot[1][1].vel[0] - cartXDotDot[0][1].vel[0];
    cartXDotDot[2][1].vel[1] = cartXDotDot[1][1].vel[1] - cartXDotDot[0][1].vel[1];
    printf("Iter 0:   %f      %f     %f         %f        %f      %f\n", cartX[2][1].p[0], cartX[2][1].p[1], cartXDot[2][1].vel[0], cartXDot[2][1].vel[1], cartXDotDot[2][1].vel[1], cartXDotDot[2][1].vel[1]);

    //Regulator or controller
    betha(0) = alpha(0, 0)*((Kp / 10000.0) * cartXDotDot[2][1].vel[0] + (Kv / 200.0) * cartXDot[2][1].vel[0] + (2 * Kp) * cartX[2][1].p[0]);
    betha(1) = alpha(1, 1)*((Kp / 10000.0) * cartXDotDot[2][1].vel[1] + (Kv / 250.0) * cartXDot[2][1].vel[1] + (1.5 * Kp) * cartX[2][1].p[1]);



    //step 1
    jointRates[1](0) = jointRates[0](0) + timeDelta * jointAccelerations[0](0);
    jointRates[1](1) = jointRates[0](1) + timeDelta * jointAccelerations[0](1);
    jointPoses[1](0) = jointPoses[0](0) + timeDelta * jointRates[0](0);
    jointPoses[1](1) = jointPoses[0](1) + timeDelta * jointRates[0](1);

    constraintSolver.CartToJnt(jointPoses[1], jointRates[1], jointAccelerations[1], cartXDotDot[0], alpha, betha, externalNetForce, jointTorques[0]);
    //Actual
    constraintSolver.getLinkCartesianPose(cartX[0]);
    constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
    
    //Error
    cartX[2][1].p[0] = cartX[1][1].p[0] - cartX[0][1].p[0];
    cartX[2][1].p[1] = cartX[1][1].p[1] - cartX[0][1].p[1];
    cartXDot[2][1].vel[0] = cartXDot[1][1].vel[0] - cartXDot[0][1].vel[0];
    cartXDot[2][1].vel[1] = cartXDot[1][1].vel[1] - cartXDot[0][1].vel[1];
    cartXDotDot[2][1].vel[0] = cartXDotDot[1][1].vel[0] - cartXDotDot[0][1].vel[0];
    cartXDotDot[2][1].vel[1] = cartXDotDot[1][1].vel[1] - cartXDotDot[0][1].vel[1];
    printf("Iter 0:   %f      %f     %f         %f        %f      %f\n", cartX[2][1].p[0], cartX[2][1].p[1], cartXDot[2][1].vel[0], cartXDot[2][1].vel[1], cartXDotDot[2][1].vel[1], cartXDotDot[2][1].vel[1]);

    //Regulator or controller
    betha(0) = alpha(0, 0)*((Kp / 10000.0) * cartXDotDot[2][1].vel[0] + (Kv / 200.0) * cartXDot[2][1].vel[0] + (2 * Kp) * cartX[2][1].p[0]);
    betha(1) = alpha(1, 1)*((Kp / 10000.0) * cartXDotDot[2][1].vel[1] + (Kv / 250.0) * cartXDot[2][1].vel[1] + (1.5 * Kp) * cartX[2][1].p[1]);

    //Definition of process main loop
    //-------------------------------------------------------------------------------------//
    for (double t = 2 * timeDelta; t <= TimeConstant; t = t + timeDelta)
    {
        //Interpolation q = a0+a1t+a2t^2+a3t^3
        //Desired
        cartX[1][1].p[0] = b0_x + b1_x * t + b2_x * t * t + b3_x * t * t*t;
        cartX[1][1].p[1] = b0_y + b1_y * t + b2_y * t * t + b3_y * t * t*t;
        cartXDot[1][1].vel[0] = b1_x + 2 * b2_x * t + 3 * b3_x * t*t;
        cartXDot[1][1].vel[1] = b1_y + 2 * b2_y * t + 3 * b3_y * t*t;
        cartXDotDot[1][1].vel[0] = 2 * b2_x + 6 * b3_x*t;
        cartXDotDot[1][1].vel[1] = 2 * b2_y + 6 * b3_y*t;

        // AB 2 order: predictor
        jointRates[2](0) = jointRates[1](0) + timeDelta * (1.5 * jointAccelerations[1](0) - 0.5 * jointAccelerations[0](0));
        jointRates[2](1) = jointRates[1](1) + timeDelta * (1.5 * jointAccelerations[1](1) - 0.5 * jointAccelerations[0](1));
        jointPoses[2](0) = jointPoses[1](0) + timeDelta * (1.5 * jointRates[1](0) - 0.5 * jointRates[0](0));
        jointPoses[2](1) = jointPoses[1](1) + timeDelta * (1.5 * jointRates[1](1) - 0.5 * jointRates[0](1));
        jointTorques[1] = jointTorques[0]; //correction is done on pose,vel and acc, so we need old torques with corrected pose, vel to get corrected acc
        // printf("Prediction: %f          %f %f       %f %f       %f %f           %f %f\n", t, jointPoses[2](0), jointPoses[2](1), jointRates[2](0), jointRates[2](1), jointAccelerations[1](0), jointAccelerations[1](1), jointTorques[0](0), jointTorques[0](1));

        //Function evaluation
        constraintSolver.CartToJnt(jointPoses[2], jointRates[2], jointAccelerations[2], cartXDotDot[0], alpha, betha, externalNetForce, jointTorques[0]);
        //     printf("Evaluation 1: %f          %f %f       %f %f       %f %f           %f %f\n", t, jointPoses[2](0), jointPoses[2](1), jointRates[2](0), jointRates[2](1), jointAccelerations[2](0), jointAccelerations[2](1), jointTorques[0](0), jointTorques[0](1));

        //AM 2 order: corrector
        jointPoses[2](0) = jointPoses[1](0) + timeDelta * ((5 / 12.0) * jointRates[2](0) + (2 / 3.0) * jointRates[1](0) - (1 / 12.0) * jointRates[0](0));
        jointPoses[2](1) = jointPoses[1](1) + timeDelta * ((5 / 12.0) * jointRates[2](1) + (2 / 3.0) * jointRates[1](1) - (1 / 12.0) * jointRates[0](1));
        jointPoses[0] = jointPoses[1];
        jointPoses[1] = jointPoses[2];
        jointRates[2](0) = jointRates[1](0) + timeDelta * ((5 / 12.0) * jointAccelerations[2](0) + (2 / 3.0) * jointAccelerations[1](0) - (1 / 12.0) * jointAccelerations[0](0));
        jointRates[2](1) = jointRates[1](1) + timeDelta * ((5 / 12.0) * jointAccelerations[2](1) + (2 / 3.0) * jointAccelerations[1](1) - (1 / 12.0) * jointAccelerations[0](1));
        jointRates[0] = jointRates[1]; //memorize n+1
        jointRates[1] = jointRates[2]; //memorize n+2
        // printf("Correction : %f          %f %f       %f %f       %f %f           %f %f\n", t, jointPoses[2](0), jointPoses[2](1), jointRates[2](0), jointRates[2](1), jointAccelerations[2](0), jointAccelerations[2](1), jointTorques[0](0), jointTorques[0](1));

        //Function evaluation give final corrected acc n+2
        constraintSolver.CartToJnt(jointPoses[2], jointRates[2], jointAccelerations[2], cartXDotDot[0], alpha, betha, externalNetForce, jointTorques[1]);
        //printf("%f          %f      %f       %f     %f       %f     %f           %f     %f\n", t, jointPoses[2](0), jointPoses[2](1), jointRates[2](0), jointRates[2](1), jointAccelerations[2](0), jointAccelerations[2](1), jointTorques[1](0), jointTorques[1](1));

        jointAccelerations[0] = jointAccelerations[1]; //memorize n+1
        jointAccelerations[1] = jointAccelerations[2]; //memoze n+2
        jointTorques[0] = jointTorques[1];

        constraintSolver.initial_upwards_sweep(jointPoses[1], jointRates[1], jointAccelerations[1], externalNetForce);
        //Actual
        constraintSolver.getLinkCartesianPose(cartX[0]);
        constraintSolver.getLinkCartesianVelocity(cartXDot[0]);

        //Error
        cartX[2][1].p[0] = cartX[1][1].p[0] - cartX[0][1].p[0];
        cartX[2][1].p[1] = cartX[1][1].p[1] - cartX[0][1].p[1];
        cartXDot[2][1].vel[0] = cartXDot[1][1].vel[0] - cartXDot[0][1].vel[0];
        cartXDot[2][1].vel[1] = cartXDot[1][1].vel[1] - cartXDot[0][1].vel[1];
        cartXDotDot[2][1].vel[0] = cartXDotDot[1][1].vel[0] - cartXDotDot[0][1].vel[0];
        cartXDotDot[2][1].vel[1] = cartXDotDot[1][1].vel[1] - cartXDotDot[0][1].vel[1];
        printf("%f          %f      %f     %f         %f        %f      %f\n", t, cartX[2][1].p[0], cartX[2][1].p[1], cartXDot[2][1].vel[0], cartXDot[2][1].vel[1], cartXDotDot[2][1].vel[1], cartXDotDot[2][1].vel[1]);

        //Regulator or controller
        betha(0) = alpha(0, 0)*((Kp / 40000.0) * cartXDotDot[2][1].vel[0] + (Kv / 4000.0) * cartXDot[2][1].vel[0] + (Kp) * cartX[2][1].p[0]);
        betha(1) = alpha(1, 1)*((Kp / 20000.0) * cartXDotDot[2][1].vel[1] + (Kv / 500.0) * cartXDot[2][1].vel[1] + (0.75 * Kp) * cartX[2][1].p[1]);

    }

    return 0;
}
int main()
{

    using namespace KDL;


    //Definition of kinematic chain
    //-----------------------------------------------------------------------------------------------//
    //Joint (const JointType &type=None, const double &scale=1, const double &offset=0, const double &inertia=0, const double &damping=0, const double &stiffness=0)
    Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
    Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);

    Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
    Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
    Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
    //Segment (const Joint &joint=Joint(Joint::None), const Frame &f_tip=Frame::Identity(), const RigidBodyInertia &I=RigidBodyInertia::Zero())
    Segment segment1 = Segment(rotJoint0, frame1);
    Segment segment2 = Segment(rotJoint1, frame2);

    // 	RotationalInertia (double Ixx=0, double Iyy=0, double Izz=0, double Ixy=0, double Ixz=0, double Iyz=0)
    RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
    //RigidBodyInertia (double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
    RigidBodyInertia inerSegment1(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
    RigidBodyInertia inerSegment2(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
    segment1.setInertia(inerSegment1);
    segment2.setInertia(inerSegment2);

    Chain chain;
    chain.addSegment(segment1);
    chain.addSegment(segment2);
    //~Definition of kinematic chain
    //----------------------------------------------------------------------------------------------//



    //Definition of constraints and external disturbances
    //--------------------------------------------------------------------------------------//
    //Constraint force matrix at the end-effector
    //What is the convention for the spatial force matrix; is it the same as in thesis?
    //NOTE AZAMAT: Constraint are also defined in local reference frame?!
    Vector constrainXLinear(0.0, 0.0, 0.0);
    Vector constrainXAngular(0.0, 0.0, 0.0);
    Vector constrainYLinear(0.0, 1.0, 0.0);
    Vector constrainYAngular(0.0, 0.0, 0.0);
    Vector constrainZLinear(0.0, 0.0, 0.0);
    Vector constrainZAngular(0.0, 0.0, 0.0);
    Twist constraintForcesX(constrainXLinear, constrainXAngular);
    Twist constraintForcesY(constrainYLinear, constrainYAngular);
    //Twist constraintForcesZ(constrainZLinear, constrainZAngular);
    Jacobian alpha(1);
    //alpha.setColumn(0, constraintForcesX);
    alpha.setColumn(0, constraintForcesY);


    //Acceleration energy at  the end-effector
    JntArray betha(1); //set to zero
    betha(0) = 0.0;
    //betha(1) = 0.0;
    //betha(2) = 0.0;

    //arm root acceleration
    Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
    Vector angularAcc(0.0, 0.0, 0.0);
    Twist twist1(linearAcc, angularAcc);

    //external forces on the arm
    Vector externalForce1(0.0, 0.0, 0.0);
    Vector externalTorque1(0.0, 0.0, 0.0);
    Vector externalForce2(0.0, 0.0, 0.0);
    Vector externalTorque2(0.0, 0.0, 0.0);
    Wrench externalNetForce1(externalForce1, externalTorque1);
    Wrench externalNetForce2(externalForce2, externalTorque2);
    Wrenches externalNetForce;
    externalNetForce.push_back(externalNetForce1);
    externalNetForce.push_back(externalNetForce2);
    //~Definition of constraints and external disturbances
    //-------------------------------------------------------------------------------------//


    //Definition of solver and initial configuration
    //-------------------------------------------------------------------------------------//
    int numberOfConstraints = 1;
    ChainIdSolver_Vereshchagin constraintSolver(chain, twist1, numberOfConstraints);
    
    //These arrays of joint values contain actual and desired values
    //actual is generated by a solver and integrator
    //desired is given by an interpolator
    //error is the difference between desired-actual
    int k = 3;
    JntArray jointPoses[k];
    JntArray jointRates[k];
    JntArray jointAccelerations[k];
    JntArray jointTorques[k];
    JntArray biasqDotDot(chain.getNrOfJoints());
    for (int i = 0; i < k; i++)
    {
        JntArray jointValues(chain.getNrOfJoints());
        jointPoses[i] = jointValues;
        jointRates[i] = jointValues;
        jointAccelerations[i] = jointValues;
        jointTorques[i] = jointValues;
    }

    //cartesian space/link values
    k = 4;
    Frames cartX[k];
    Twists cartXDot[k];
    Twists cartXDotDot[k];
    Twist accLink;
    for (int i = 0; i < k; i++) //i is number of variables (actual, desired, error)
    {
        for (int j = 0; j < k-1; j++) //j is number of links
        {
            cartX[i].push_back(frame1);
            cartXDot[i].push_back(accLink);
            cartXDotDot[i].push_back(accLink);
        }

    }


    // Initial arm position configuration/constraint
    JntArray jointInitialPose(chain.getNrOfJoints());
    jointInitialPose(0) = 0.0; // initial joint0 pose
    jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise
    //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
    //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636

    //actual
    jointPoses[0](0) = jointInitialPose(0);
    jointPoses[0](1) = jointInitialPose(1);
    //desired
    jointPoses[1](0) = jointInitialPose(0);
    jointPoses[1](1) = jointInitialPose(1);

   
    //~Definition of solver and initial configuration
    //-------------------------------------------------------------------------------------//



    //Definition of process main loop
    //-------------------------------------------------------------------------------------//
    double taskTimeConstant = 2; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
    double simulationTime = 2.5*taskTimeConstant;
    double timeDelta = 0.001;
    bool status;
    
    double ksi = 1; //damping factor
    double Kp = 0.02/(taskTimeConstant*taskTimeConstant);
    double Kv = 1*ksi/taskTimeConstant;
    double Ki = 1.0;
    double Ka = 0.0;


    //Interpolator parameters:
    double b0_y = -0.7464102; //should come from initial joint configuration
    double b1_y = 0.0;
    double b2_y = 0.0;//((0.5 + 0.7464)*3.0 / TimeConstant * TimeConstant);
    double b3_y = 0.0;//-((0.5 + 0.7464)*2.0 / TimeConstant * TimeConstant * TimeConstant);

    double b0_x = 0.2; //should come from initial joint configuration
    double b1_x = 0.0;
    double b2_x = 0.0;//((0.5 + 0.7464)*3.0 / TimeConstant * TimeConstant);
    double b3_x = 0.0;//-((0.5 + 0.7464)*2.0 / TimeConstant * TimeConstant * TimeConstant);

    /*
    double a0_q0 = jointPoses[1](0);
    double a1_q0 = 0.0;
    double a2_q0 = ((qFinalPose(0) - jointPoses[1](0))*3.0 / TimeConstant * TimeConstant);
    double a3_q0 = -((qFinalPose(0) - jointPoses[1](0))*2.0 / TimeConstant * TimeConstant * TimeConstant);

    double a0_q1 = jointPoses[1](1);
    double a1_q1 = 0.0;
    double a2_q1 = ((qFinalPose(1) - jointPoses[1](1))*3.0 / TimeConstant * TimeConstant);
    double a3_q1 = -((qFinalPose(1) - jointPoses[1](1))*2.0 / TimeConstant * TimeConstant * TimeConstant);
     */

    
    for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
    {

        //Interpolation (Desired) q = a0+a1t+a2t^2+a3t^3
        //Do we feed these values? then         jointPoses[0] = jointPoses[1];         jointRates[0] = jointRates[1];
        // But I don't think so, in control desired trajectory plays role of the reference that the controller should strive for from its actual (previous, current) state.

        cartX[1][1].p[0] = b0_x + b1_x * t + b2_x * t * t + b3_x * t * t*t;
        cartX[1][1].p[1] = b0_y + b1_y * t + b2_y * t * t + b3_y * t * t*t;
        cartXDot[1][1].vel[0] = b1_x + 2 * b2_x * t + 3 * b3_x * t*t;
        cartXDot[1][1].vel[1] = b1_y + 2 * b2_y * t + 3 * b3_y * t*t;
        cartXDotDot[1][1].vel[0] = 2 * b2_x + 6 * b3_x*t;
        cartXDotDot[1][1].vel[1] = 2 * b2_y + 6 * b3_y*t;
       // printf("Desired Cartesian values: %f          %f      %f     %f         %f        %f      %f\n", t, cartX[1][1].p[0], cartX[1][1].p[1], cartXDot[1][1].vel[0], cartXDot[1][1].vel[1], cartXDotDot[1][1].vel[0], cartXDotDot[1][1].vel[1]);


        status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0]);
        /*
        constraintSolver.initial_upwards_sweep(jointPoses[0], jointRates[0], jointAccelerations[0], externalNetForce);
        constraintSolver.downwards_sweep(alpha, jointTorques[0]);
        constraintSolver.constraint_calculation(betha);
        constraintSolver.final_upwards_sweep(jointAccelerations[0], cartXDotDot[0], jointTorques[0]);
       */
        //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
        jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
        jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
        jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
        jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
        //printf("Joint %f          %f      %f       %f     %f       %f      %f     %f      %f\n", t, jointPoses[0](0), jointPoses[0](1), jointRates[0](0), jointRates[0](1), jointAccelerations[0](0), jointAccelerations[0](1), jointTorques[0](0), jointTorques[0](1));
        
        constraintSolver.initial_upwards_sweep(jointPoses[0], jointRates[0], jointAccelerations[0], externalNetForce);
        constraintSolver.getLinkCartesianPose(cartX[0]);
        constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
        constraintSolver.getLinkCartesianAcceleration(cartXDotDot[0]);
        //printf("Cartesian actual %f          %f      %f     %f         %f        %f      %f     %f\n", t, cartX[0][1].p.x(), cartX[0][1].p.y(), cartXDot[0][1].vel[0], cartXDot[0][1].vel[1], cartXDotDot[0][1].vel[0], cartXDotDot[0][1].vel[1], cartXDotDot[0][1].rot[2]);

        //Error
        /*
        jointPoses[2](0) = jointPoses[1](0) - jointPoses[0](0);
        jointPoses[2](1) = jointPoses[1](1) - jointPoses[0](1);
        jointRates[2](0) = jointRates[1](0) - jointRates[0](0);
        jointRates[2](1) = jointRates[1](1) - jointRates[0](1);
        jointAccelerations[2](0) = jointAccelerations[1](0) - jointAccelerations[0](0);
        jointAccelerations[2](1) = jointAccelerations[1](1) - jointAccelerations[0](1);
        printf("Errors: %f          %f      %f          %f     %f       %f      %f\n", t, jointPoses[2](0), jointPoses[2](1), jointRates[2](0), jointRates[2](1), jointAccelerations[2](0), jointAccelerations[2](1));
        */
        //e = d - a
        cartX[2][1].p[0] = cartX[1][1].p[0] - cartX[0][1].p[0];
        cartX[2][1].p[1] = cartX[1][1].p[1] - cartX[0][1].p[1];
        cartXDot[2][1].vel[0] = cartXDot[1][1].vel[0] - cartXDot[0][1].vel[0];
        cartXDot[2][1].vel[1] = cartXDot[1][1].vel[1] - cartXDot[0][1].vel[1];
        cartXDotDot[2][1].vel[0] = cartXDotDot[1][1].vel[0] - cartXDotDot[0][1].vel[0];
        cartXDotDot[2][1].vel[1] = cartXDotDot[1][1].vel[1] - cartXDotDot[0][1].vel[1];
        cartXDotDot[2][1].rot[2] = cartXDotDot[1][1].rot[2] - cartXDotDot[0][1].rot[2];
        cartX[3][1].p[0] += timeDelta*cartX[2][1].p[0]; //for integral term;
        cartX[3][1].p[1] += timeDelta*cartX[2][1].p[1];
        printf("Cartesian error  %f          %f      %f     %f         %f        %f      %f     %f\n", t, cartX[2][1].p[0], cartX[2][1].p[1], cartXDot[2][1].vel[0], cartXDot[2][1].vel[1], cartXDotDot[2][1].vel[1], cartXDotDot[2][1].vel[1], cartXDotDot[2][1].rot[2]);
        //Regulator or controller
        //externalNetForce[1].force[0] =  ( (Kv)*cartXDot[2][1].vel[0] + (Kp)*cartX[2][1].p[0] + Ki*cartX[3][1].p[0]);

        
        betha(0) = ( (Kv)*cartXDot[2][1].vel[1] - (Ka)*cartXDot[2][1].rot[2] + (Kp)*cartX[2][1].p[1]);// + Ki*cartX[3][1].p[0]);
        //betha(1) = betha(1) +(  (Kv)*cartXDot[2][1].vel[1] + (Kp)*cartX[2][1].p[1]);// + Ki*cartX[3][1].p[0]);
        //printf("betha   %f      %f\n", betha(0), betha(1));
    }
    //~Definition of process main loop
    //-------------------------------------------------------------------------------------//





    return 0;
}
/*
 * To compute the jacobian, we start at the base, and iterate up the chain. We assume that the link we are currently on
 * is the last link in the chain, so the tip of our current link is effectively the end-effector of our chain. Once we compute
 * the jacobian for this pseudo-end effector, we move to the next link, and transform the previously computed jacobian to be for
 * the new end-effector.
 */
int LinkParamJacobianSolver::JointsToCartesian(const Chain& chain, const JntArray& joint_states, LinkParamJacobian& jac)
{
  const unsigned int N = chain.getNrOfJoints() ;

  if ( joint_states.rows() != N )                                 // Sanity check on input data
    return -1 ;

  jac.links_.resize(N) ;

  Frame T_start_to_eef ;                                          // Defines transform from the start of the chain to end-effector [of the part of the chain we've seen so far]
  Frame T_start_to_prev_eef = Frame::Identity() ;

  unsigned int joint_num = 0 ;                                    // Keep track of which joint we're currently on. Since some joints are fixed,
                                                                  //    the joint # could be different than the segment #

  for (unsigned int i=0; i<chain.getNrOfSegments(); i++)                            // Walk up the chain
  {
    const bool movable = chain.getSegment(i).getJoint().getType() != Joint::None ;  // Not sure what the right answer is for dealing with fixed joints
    const bool process_segment = movable ;                                          //    For now, compute Jacobians only for movable joints.

    if(process_segment)
      T_start_to_eef = T_start_to_prev_eef * chain.getSegment(i).pose(joint_states(joint_num)) ;
    else
      T_start_to_eef = T_start_to_prev_eef * chain.getSegment(i).pose(0.0) ;

    Vector cur_segment_length = T_start_to_eef.p - T_start_to_prev_eef.p ;  // Defines vector-length of the current segment in the global frame
    jac.changeRefPoint(cur_segment_length) ;                                // Walk the previously computed jacobians up the chain

    if (process_segment)
    {
      // Build Twists for the translation elements x,y,z.  These translations occur in the base frame of the current segment
      Twist trans_twist[3] ;
      trans_twist[0].vel[0] = 1.0 ;
      trans_twist[0].vel[1] = 0.0 ;
      trans_twist[0].vel[2] = 0.0 ;
      trans_twist[1].vel[0] = 0.0 ;
      trans_twist[1].vel[1] = 1.0 ;
      trans_twist[1].vel[2] = 0.0 ;
      trans_twist[2].vel[0] = 0.0 ;
      trans_twist[2].vel[1] = 0.0 ;
      trans_twist[2].vel[2] = 1.0 ;

      // Build rotation from start to base frame of current link. This also includes the rotation from the current link's joint.
      Rotation R_start_to_cur_base = T_start_to_prev_eef.M * chain.getSegment(i).getJoint().pose(joint_states(joint_num)).M ;
      for (int j=0; j<3; j++)
      {
        trans_twist[j] = R_start_to_cur_base*trans_twist[j] ;                 // Rotate translations from the base frame of the current link to the start frame
        jac.links_[joint_num].trans_[j] = trans_twist[j] ;                   // Copy data into the output datatype
      }

      // Build Twists for incremental rotations at the end of the segment.  These translations occur after the segment transform.
      Twist rot_twist[3] ;

      // First build twists in the [current] end-effector frame
      rot_twist[0].rot[0] = 1.0 ;
      rot_twist[0].rot[1] = 0.0 ;
      rot_twist[0].rot[2] = 0.0 ;
      rot_twist[1].rot[0] = 0.0 ;
      rot_twist[1].rot[1] = 1.0 ;
      rot_twist[1].rot[2] = 0.0 ;
      rot_twist[2].rot[0] = 0.0 ;
      rot_twist[2].rot[1] = 0.0 ;
      rot_twist[2].rot[2] = 1.0 ;

      // Now we need to transform these rotation twists to the base (beginning-of-segment) frame
      for (int j=0; j<3; j++)
      {
        rot_twist[j] = T_start_to_eef.M*rot_twist[j] ;
        jac.links_[joint_num].rot_[j] = rot_twist[j] ;                                   // Copy data into the output datatype
      }
    }
    if (movable)
      joint_num++ ;

    T_start_to_prev_eef = T_start_to_eef ;
  }
  return 0 ;
}
int main()
{

    using namespace KDL;


    //Definition of kinematic chain
    //-----------------------------------------------------------------------------------------------//
    //Joint (const JointType &type=None, const double &scale=1, const double &offset=0, const double &inertia=0, const double &damping=0, const double &stiffness=0)
    Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
    Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);
    Joint rotJoint2 = Joint(Joint::RotZ, 1, 0, 0.01);
    Joint rotJoint3 = Joint(Joint::RotZ, 1, 0, 0.01);

    Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
    Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
    Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
    Frame frame3(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
    Frame frame4(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
    //Segment (const Joint &joint=Joint(Joint::None), const Frame &f_tip=Frame::Identity(), const RigidBodyInertia &I=RigidBodyInertia::Zero())
    Segment segment1 = Segment(rotJoint0, frame1);
    Segment segment2 = Segment(rotJoint1, frame2);
    Segment segment3 = Segment(rotJoint2, frame3);
    Segment segment4 = Segment(rotJoint3, frame4);

    // 	RotationalInertia (double Ixx=0, double Iyy=0, double Izz=0, double Ixy=0, double Ixz=0, double Iyz=0)
    RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
    //RigidBodyInertia (double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
    RigidBodyInertia inerSegment1(0.5, Vector(0.0, -0.2, 0.0), rotInerSeg1);
    RigidBodyInertia inerSegment2(0.5, Vector(0.0, -0.2, 0.0), rotInerSeg1);
    RigidBodyInertia inerSegment3(0.5, Vector(0.0, -0.2, 0.0), rotInerSeg1);
    RigidBodyInertia inerSegment4(0.5, Vector(0.0, -0.2, 0.0), rotInerSeg1);
    segment1.setInertia(inerSegment1);
    segment2.setInertia(inerSegment2);
    segment3.setInertia(inerSegment3);
    segment4.setInertia(inerSegment4);

    Chain chain;
    chain.addSegment(segment1);
    chain.addSegment(segment2);
    chain.addSegment(segment3);
    chain.addSegment(segment4);
    //~Definition of kinematic chain




    //Definition of constraints and external disturbances
    //--------------------------------------------------------------------------------------//
    //Constraint force matrix at the end-effector
    //What is the convention for the spatial force matrix; is it the same as in thesis?
    //NOTE AZAMAT: Constraint are also defined in local reference frame?!
    Vector constrainXLinear(1.0, 0.0, 0.0);
    Vector constrainXAngular(0.0, 0.0, 0.0);
    Vector constrainYLinear(0.0, 0.0, 0.0);
    Vector constrainYAngular(0.0, 0.0, 0.0);
    Vector constrainZLinear(0.0, 0.0, 0.0);
    Vector constrainZAngular(0.0, 0.0, 0.0);
    Twist constraintForcesX(constrainXLinear, constrainXAngular);
    Twist constraintForcesY(constrainYLinear, constrainYAngular);
    Twist constraintForcesZ(constrainZLinear, constrainZAngular);
    Jacobian alpha(1);
    alpha.setColumn(0, constraintForcesX);
    // alpha.setColumn(1, constraintForcesY);
    //Acceleration energy at  the end-effector
    JntArray betha(1); //set to zero
    betha(0) = 0.0;
    // betha(1) = 0.0;
    //betha(2) = 0.0;
    //arm root acceleration
    Vector linearAcc(0.0, 9.8, 0.0); //gravitational acceleration along Y
    Vector angularAcc(0.0, 0.0, 0.0);
    Twist twist1(linearAcc, angularAcc);
    //external forces on the arm
    Vector externalForce1(0.0, 0.0, 0.0);
    Vector externalTorque1(0.0, 0.0, 0.0);
    Vector externalForce2(0.0, 0.0, 0.0);
    Vector externalTorque2(0.0, 0.0, 0.0);
    Wrench externalNetForce1(externalForce1, externalTorque1);
    Wrench externalNetForce2(externalForce2, externalTorque2);
    Wrenches externalNetForce;
    externalNetForce.push_back(externalNetForce1);
    externalNetForce.push_back(externalNetForce2);
    externalNetForce.push_back(externalNetForce1);
    externalNetForce.push_back(externalNetForce2);
    //~Definition of constraints and external disturbances



    //Definition of solver and initial configuration
    //-------------------------------------------------------------------------------------//
    int numberOfConstraints = 1;
    ChainIdSolver_Vereshchagin constraintSolver(chain, twist1, numberOfConstraints);

    //These arrays of joint values contain actual and desired values
    //actual is generated by a solver and integrator
    //desired is given by an interpolator
    //error is the difference between desired-actual
    //0-actual, 1-desired, 2-error
    int k = 4;
    JntArray jointPoses[k];
    JntArray jointRates[k];
    JntArray jointAccelerations[k];
    JntArray jointTorques[k];
    JntArray biasqDotDot(chain.getNrOfJoints());
    for (int i = 0; i < k; i++)
    {
        JntArray jointValues(chain.getNrOfJoints());
        jointPoses[i] = jointValues;
        jointRates[i] = jointValues;
        jointAccelerations[i] = jointValues;
        jointTorques[i] = jointValues;
    }


    //cartesian space/link values
    //0-actual, 1-desire, 2-error, 3-errorsum
    k = 4;
    Frames cartX[k];
    Twists cartXDot[k];
    Twists cartXDotDot[k];
    Twist accLink;
    for (int i = 0; i < k; i++) //i is number of variables (actual, desired, error)
    {
        for (int j = 0; j < 4; j++) //j is number of links, in this case 4
        {
            cartX[i].push_back(frame1);
            cartXDot[i].push_back(accLink);
            cartXDotDot[i].push_back(accLink);
        }

    }


    // Initial arm position configuration/constraint, negative in clockwise
    JntArray jointFinalPose(chain.getNrOfJoints());
    jointFinalPose(0) = M_PI/2.0;
    jointFinalPose(1) = M_PI/24.0;
    jointFinalPose(2) = -M_PI/12.0;
    jointFinalPose(3) = M_PI/12.0;
    //correspond to x = 0.398289   y = 0.026105

    JntArray jointInitialPose(chain.getNrOfJoints());
    jointInitialPose(0) = 5*M_PI/6.0;
    jointInitialPose(1) = M_PI/12.0;
    jointInitialPose(2) = -M_PI/6.0;
    jointInitialPose(3) = -M_PI/3.0;
    //correspond to x = 0.151764      y= 0.366390

    //actual initial
    jointPoses[0](0) = jointInitialPose(0);
    jointPoses[0](1) = jointInitialPose(1);
    jointPoses[0](2) = jointInitialPose(2);
    jointPoses[0](3) = jointInitialPose(3);

    //desired initial
    // jointPoses[1](0) = jointInitialPose(0);
    // jointPoses[1](1) = jointInitialPose(1);
    //~Definition of solver and initial configuration
    //-------------------------------------------------------------------------------------//



    //Definition of process main loop
    //-------------------------------------------------------------------------------------//
    //-------------------------------------------------------------------------------------//
    double taskTimeConstant = 2.5; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
    double simulationTime = 2*taskTimeConstant;
    double timeDelta = 0.001;
    double timeToSettle = 0.015;
    int status;

    double ksi[2] = {1.0, 1.0}; //damping factor
    double Kp[2];
    Kp[0] = 5.49996/(timeToSettle*timeToSettle); 
    Kp[1] = 1.352/(timeToSettle*timeToSettle);
    double Kv[2];
    Kv[0] = 18.4997*ksi[0]/timeToSettle; 
    Kv[1] = 7.5189*ksi[1]/timeToSettle;
    double Ki[2] = {0.0, 0.0};
    double K = 0.005;
    /*
    //For cartesian space control with constraints and using external forces
    double ksi[2] = {1.0, 1.0}; //damping factor
    double Kp[2];
    Kp[0] = 6000.0/(taskTimeConstant*taskTimeConstant);
    Kp[1] = 11000.0/(taskTimeConstant*taskTimeConstant);
    double Kv[2];
    Kv[0] = 130*ksi[0]/taskTimeConstant;
    Kv[1] = 160*ksi[1]/taskTimeConstant;
    double Ki[2] = {-5.01, -5.1};

    //For cartesian space control without constraints using external forces
    double ksi[2] = {1.0, 1.0}; //damping factor
    double Kp[2];
    Kp[0] = 7000.0/(taskTimeConstant*taskTimeConstant);
    Kp[1] = 12000.0/(taskTimeConstant*taskTimeConstant);
    double Kv[2];
    Kv[0] = 130*ksi[0]/taskTimeConstant;
    Kv[1] = 160*ksi[1]/taskTimeConstant;
    double Ki[2] = {-5.01, -5.1};

    //For joint space control without constraints using computed torque
    double ksi[2] = {1.0, 1.0}; //damping factor
    double Kp[2];
    Kp[0] = 4000.0/(taskTimeConstant*taskTimeConstant);
    Kp[1] = 3200.0/(taskTimeConstant*taskTimeConstant);
    double Kv[2];
    Kv[0] = 20*ksi[0]/taskTimeConstant;
    Kv[1] = 30*ksi[1]/taskTimeConstant;
    double Ki[2] = {33.5, 10.5};
    double Ka[2] = {0.0, 0.0};
    */
    //Interpolator parameters:
    
    double b0_y = 0.366390; //should come from initial joint configuration
    double b1_y = 0.0;
    double b2_y =  ((0.026105 - b0_y)*3.0 / (simulationTime * simulationTime)); //yFinal = 0.026105 x= 0.398289      
    double b3_y = -((0.026105 - b0_y)*2.0 / (simulationTime * simulationTime * simulationTime));

    double b0_x = 0.151764; //should come from initial joint configuration
    double b1_x = 0.0;
    double b2_x = ((0.151764 - b0_x)*3.0 / (simulationTime * simulationTime)); // Xfinal= xInitial = 0.151764
    double b3_x = -((0.151764 - b0_x)*2.0 / (simulationTime * simulationTime * simulationTime));
     
    /*
    double a0_q0 = jointInitialPose(0);
    double a1_q0 = 0.0;
    double a2_q0 = ((jointFinalPose(0)  - jointInitialPose(0))*3.0 / (simulationTime * simulationTime));
    double a3_q0 = -((jointFinalPose(0) - jointInitialPose(0))*2.0 / (simulationTime * simulationTime * simulationTime));

    double a0_q1 = jointInitialPose(1);
    double a1_q1 = 0.0;
    double a2_q1 = ((jointFinalPose(1) - jointInitialPose(1))*3.0 / (simulationTime * simulationTime));
    double a3_q1 = -((jointFinalPose(1) - jointInitialPose(1))*2.0 / (simulationTime * simulationTime * simulationTime));
    */

    double feedforwardJointTorque0 = 0.0;
    double feedforwardJointTorque1 = 0.0;


    for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
    {
        /*
         * double j1 = 0;
        double j2 = 0;
        double j3 = 0;
        double j4 = 0;
        Eigen::MatrixXd chainJacobian(6, 2);
        chainJacobian.setZero();
        chainJacobian(5, 0) = 1;
        chainJacobian(5, 1) = 1;
        // Jacobian inverse
        j1 = -0.4 * sin(jointPoses[0](0)) - 0.4 * sin(jointPoses[0](1));
        j2 = -0.4 * sin(jointPoses[0](0) + jointPoses[0](0));
        j3 = 0.4 * cos(jointPoses[0](0)) + 0.4 * cos(jointPoses[0](0) + jointPoses[0](1));
        j4 = 0.4 * cos(jointPoses[0](0) + jointPoses[0](1));

        //printf("t, j1, j2, j3, j4   %f      %f          %f      %f      %f\n", t, j1, j2, j3, j4);

        chainJacobian(0, 0) = j1;
        chainJacobian(0, 1) = j2;
        chainJacobian(1, 0) = j3;
        chainJacobian(1, 1) = j4;

         */


        //Interpolation (Desired) q = a0+a1t+a2t^2+a3t^3
        //Do we feed these values? then         jointPoses[0] = jointPoses[1];         jointRates[0] = jointRates[1];
        // But I don't think so, in control desired trajectory plays role of the reference that the controller should strive for from its actual (previous, current) state.
        
        cartX[1][1].p[0] = b0_x + b1_x * t + b2_x * t * t + b3_x * t * t*t;
        cartX[1][1].p[1] = b0_y + b1_y * t + b2_y * t * t + b3_y * t * t*t;
        cartXDot[1][1].vel[0] = b1_x + 2 * b2_x * t + 3 * b3_x * t*t;
        cartXDot[1][1].vel[1] = b1_y + 2 * b2_y * t + 3 * b3_y * t*t;
        cartXDotDot[1][1].vel[0] = 2 * b2_x + 6 * b3_x*t;
        cartXDotDot[1][1].vel[1] = 2 * b2_y + 6 * b3_y*t;
        // printf("%f          %f      %f     %f         %f        %f      %f\n", t, cartX[1][1].p[0], cartX[1][1].p[1], cartXDot[1][1].vel[0], cartXDot[1][1].vel[1], cartXDotDot[1][1].vel[0], cartXDotDot[1][1].vel[1]);
        //printf("Desired Cartesian values: %f          %f      %f     %f         %f        %f      %f\n", t, cartX[1][1].p[0], cartX[1][1].p[1], cartXDot[1][1].vel[0], cartXDot[1][1].vel[1], cartXDotDot[1][1].vel[0], cartXDotDot[1][1].vel[1]);
        
        /*
        jointPoses[1](0) = a0_q0 + a1_q0 * t + a2_q0 * t * t + a3_q0 * t * t*t;
        jointPoses[1](1) = a0_q1 + a1_q1 * t + a2_q1 * t * t + a3_q1 * t * t*t;
        jointRates[1](0) = a1_q0 + 2 * a2_q0 * t + 3 * a3_q0 * t*t;
        jointRates[1](1) = a1_q1 + 2 * a2_q1 * t + 3 * a3_q1 * t*t;
        jointAccelerations[1](0) = 2 * a2_q0 + 6 * a3_q0*t;
        jointAccelerations[1](1) = 2 * a2_q1 + 6 * a3_q1*t;
        //printf("Desired joint values: %f         %f          %f      %f       %f     %f       %f\n", t, jointPoses[1](0), jointPoses[1](1), jointRates[1](0), jointRates[1](1), jointAccelerations[1](0), jointAccelerations[1](1));
        //printf("%f         %f          %f      %f       %f     %f       %f\n", t, jointPoses[1](0), jointPoses[1](1), jointRates[1](0), jointRates[1](1), jointAccelerations[1](0), jointAccelerations[1](1));
        */

        status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0],alpha, betha, externalNetForce, jointTorques[0]);  

        constraintSolver.getLinkCartesianPose(cartX[0]);
        constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
        constraintSolver.getLinkCartesianAcceleration(cartXDotDot[0]);
        printf("%f          %f      %f     %f         %f        %f      %f\n", t, cartX[0][1].p.x(), cartX[0][1].p.y(), cartXDot[0][1].vel[0], cartXDot[0][1].vel[1], cartXDotDot[0][1].vel[0], cartXDotDot[0][1].vel[1]);
        // printf("Actual cartesian values: %f          %f      %f     %f         %f        %f      %f\n", t, cartX[0][1].p.x(), cartX[0][1].p.y(), cartXDot[0][1].vel[0], cartXDot[0][1].vel[1], cartXDotDot[0][1].vel[0], cartXDotDot[0][1].vel[1]);

        //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
        jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
        jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
        jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
        jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;

        jointRates[0](2) = jointRates[0](2) + jointAccelerations[0](2) * timeDelta; //Euler Forward
        jointPoses[0](2) = jointPoses[0](2) + (jointRates[0](2) - jointAccelerations[0](2) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
        jointRates[0](3) = jointRates[0](3) + jointAccelerations[0](3) * timeDelta; //Euler Forward
        jointPoses[0](3) = jointPoses[0](3) + (jointRates[0](3) - jointAccelerations[0](3) * timeDelta / 2.0) * timeDelta;
        // printf("%f          %f      %f       %f     %f       %f      %f     %f      %f\n", t, jointPoses[0](0), jointPoses[0](1), jointPoses[0](2), jointPoses[0](3), jointAccelerations[0](0), jointAccelerations[0](1), jointAccelerations[0](2), jointAccelerations[0](3));
        //printf("Actual joint values: %f          %f      %f       %f     %f       %f      %f     %f      %f\n", t, jointPoses[0](0), jointPoses[0](1), jointRates[0](0), jointRates[0](1), jointAccelerations[0](0), jointAccelerations[0](1), jointTorques[0](0), jointTorques[0](1));
        //Error
        /*
        jointPoses[2](0) = jointPoses[1](0) - jointPoses[0](0);
        jointPoses[2](1) = jointPoses[1](1) - jointPoses[0](1);
        jointRates[2](0) = jointRates[1](0) - jointRates[0](0);
        jointRates[2](1) = jointRates[1](1) - jointRates[0](1);
        jointAccelerations[2](0) = jointAccelerations[1](0) - jointAccelerations[0](0);
        jointAccelerations[2](1) = jointAccelerations[1](1) - jointAccelerations[0](1);
        jointPoses[3](0) += timeDelta*jointPoses[2](0);
        jointPoses[3](1) += timeDelta*jointPoses[2](1);
        //printf("%f          %f      %f          %f     %f       %f      %f\n", t, jointPoses[2](0), jointPoses[2](1), jointRates[2](0), jointRates[2](1), jointAccelerations[2](0), jointAccelerations[2](1));
        */

        cartX[2][1].p[0] = cartX[1][1].p[0] - cartX[0][1].p[0];
        cartX[2][1].p[1] = cartX[1][1].p[1] - cartX[0][1].p[1];
        cartXDot[2][1].vel[0] = cartXDot[1][1].vel[0] - cartXDot[0][1].vel[0];
        cartXDot[2][1].vel[1] = cartXDot[1][1].vel[1] - cartXDot[0][1].vel[1];
        cartXDotDot[2][1].vel[0] = cartXDotDot[1][1].vel[0] - cartXDotDot[0][1].vel[0];
        cartXDotDot[2][1].vel[1] = cartXDotDot[1][1].vel[1] - cartXDotDot[0][1].vel[1];
        cartX[3][1].p[0] += timeDelta * cartX[2][1].p[0]; //for integral term;
        cartX[3][1].p[1] += timeDelta * cartX[2][1].p[1];
        //printf("%f          %f      %f     %f         %f        %f      %f\n", t, cartX[2][1].p[0], cartX[2][1].p[1], cartXDot[2][1].vel[0], cartXDot[2][1].vel[1], cartXDotDot[2][1].vel[1], cartXDotDot[2][1].vel[1]);

        //Regulator or controller
        //acceleration energy control
         // betha(0) = alpha(0, 0)*(K * cartXDotDot[2][1].vel[0] + (Kv[0]) * cartXDot[2][1].vel[0] + (Kp[0]) * cartX[2][1].p[0]+ Ki[1] * cartX[3][1].p[0]); // 0.5xgain
        // betha(1) = alpha(1, 1)*(K * cartXDotDot[2][1].vel[1] + (Kv[0]) * cartXDot[2][1].vel[1] + (Kp[1]) * cartX[2][1].p[1] + Ki[1] * cartX[3][1].p[0]);
        /*
        //computed joint torque control
        feedforwardJointTorque0 = jointAccelerations[1](0) + (Kp[0])*jointPoses[2](0) + (Kv[0])*jointRates[2](0) + (Ki[0])*jointPoses[3](0);
        feedforwardJointTorque1 = jointAccelerations[1](1) + (Kp[1])*jointPoses[2](1) + (Kv[1])*jointRates[2](1)+ (Ki[1])*jointPoses[3](1);
        jointTorques[0](0) = jointTorques[0](0) + feedforwardJointTorque0;
        jointTorques[0](1) = jointTorques[0](1) + feedforwardJointTorque1;

        //For cartesian space control one needs to calculate from the obtained joint space value, new cartesian space poses.
        //Then based on the difference of the signal (desired-actual) we define a regulation function (controller)
        // this difference should be compensated either by joint torques.
        externalNetForce[1].force[0] = cartXDotDot[1][1].vel[0] + ((Kv[0]) * cartXDot[2][1].vel[0] + (Kp[0]) * cartX[2][1].p[0] + (Ki[0]) * cartX[3][1].p[0]);
        externalNetForce[1].force[1] = cartXDotDot[1][1].vel[1] + ((Kv[1]) * cartXDot[2][1].vel[1] + (Kp[1]) * cartX[2][1].p[1] + (Ki[1]) * cartX[3][1].p[1]);
        */
        externalNetForce[1].force[0] = ((Kv[0]) * cartXDot[2][1].vel[0] + (Kp[0]) * cartX[2][1].p[0] );
        externalNetForce[1].force[1] = ((Kv[1]) * cartXDot[2][1].vel[1] + (Kp[1]) * cartX[2][1].p[1] ); //controlling y seems to help a little

    }
    //~Definition of process main loop
    //-------------------------------------------------------------------------------------//





    return 0;
}