// namespace sequence{
    void JointTest::doInit(wocra::wOcraController& ctrl, wocra::wOcraModel& model)
    {
        ocraWbiModel& wbiModel = dynamic_cast<ocraWbiModel&>(model);
        // Full posture task
        nDoF = model.nbInternalDofs();
        // Eigen::VectorXd nominal_q = Eigen::VectorXd::Zero(nDoF);
        // getNominalPosture(model, nominal_q);


        q_init = model.getJointPositions();

        q_des = q_init;

        jointMin = model.getJointLowerLimits();

        jointMax = model.getJointUpperLimits();

        taskManagers["tmFull"] = new wocra::wOcraFullPostureTaskManager(ctrl, model, "fullPostureTask", ocra::FullState::INTERNAL, 20.0, 2.0*sqrt(20), 1.0, q_init);
        for (int i=0; i<nDoF; i++){
            jointNames[i] = wbiModel.getJointName(i);
        }
        taskErr = 0.0;
        jIndex = 0;
        goToMin = true;
        goToMax = false;
        counter = 401;



    }
    void TrajectoryTrackingTest::doUpdate(double time, wocra::wOcraModel& state, void** args)
    {
        if (isDisplacementd)
        {
            wocra::wOcraSegPoseTaskManager*   tmp_tmLeftHandPose = dynamic_cast<wocra::wOcraSegPoseTaskManager*>(taskManagers["tmLeftHandPose"]);

            Eigen::Displacementd desiredPose;
            Eigen::Twistd desiredVelocity;
            Eigen::Twistd desiredAcceleration;
            leftHandTrajectory->getDesiredValues(time, desiredPose, desiredVelocity, desiredAcceleration);

            tmp_tmLeftHandPose->setState(desiredPose, desiredVelocity, desiredAcceleration);

            // tmp_tmLeftHandPose->setState(desiredPose, desiredVelocity, Eigen::Twistd::Zero());

            std::cout << "\nFinal desired pose: " << endingDispd << std::endl;
            std::cout << "Desired pose: " << desiredPose << std::endl;
            std::cout << "Desired vel: " << desiredVelocity.transpose() << std::endl;
            std::cout << "Desired acc: " << desiredAcceleration.transpose() << std::endl;
            std::cout << "Current pose: " << state.getSegmentPosition(lHandIndex) << std::endl;
            std::cout << "Error: " << tmp_tmLeftHandPose->getTaskError().transpose() << "   norm: " << tmp_tmLeftHandPose->getTaskErrorNorm() << std::endl;

        }
        else if (isRotation3d)
        {

            wocra::wOcraSegOrientationTaskManager*   tmp_tmLeftHandOrient = dynamic_cast<wocra::wOcraSegOrientationTaskManager*>(taskManagers["tmLeftHandOrient"]);


            Eigen::Rotation3d desiredOrientation;
            leftHandTrajectory->getDesiredValues(time, desiredOrientation);
            tmp_tmLeftHandOrient->setOrientation(desiredOrientation);

            std::cout << "\nFinal desired orientation: " << endingRotd << std::endl;
            std::cout << "Desired orientation: " << desiredOrientation << std::endl;
            std::cout << "Current orientation: " << state.getSegmentPosition(lHandIndex).getRotation() << std::endl;
            std::cout << "Error: " << tmp_tmLeftHandOrient->getTaskError().transpose() << "   norm: " << tmp_tmLeftHandOrient->getTaskErrorNorm() << std::endl;
        }
        else if (isCartesion || isCartesionWaypoints)
        {
            wocra::wOcraSegCartesianTaskManager*   tmp_tmLeftHandCart = dynamic_cast<wocra::wOcraSegCartesianTaskManager*>(taskManagers["tmLeftHandCart"]);


            Eigen::MatrixXd desiredPosVelAcc = leftHandTrajectory->getDesiredValues(time);
            // Eigen::MatrixXd H_adj = state.getSegmentPosition(lHandIndex).getRotation().inverse().adjoint();
            // H_adj*
            tmp_tmLeftHandCart->setState(desiredPosVelAcc.col(0));//,  desiredPosVelAcc.col(1), desiredPosVelAcc.col(2));

            // if(isCartesion){std::cout << "\nFinal desired position: " << desiredPos.transpose() << std::endl;}
            // std::cout << "\nDesired position: " << desiredPosVelAcc.col(0).transpose() << std::endl;
            // std::cout << "Current position: " << state.getSegmentPosition(lHandIndex).getTranslation().transpose()<< std::endl;
            // std::cout << "Error: " << tmp_tmLeftHandCart->getTaskError().transpose() << "   norm: " << tmp_tmLeftHandCart->getTaskErrorNorm() << std::endl;
        }



    }
 void CartesianTest::doUpdate(double time, wocra::wOcraModel& state, void** args)
 {
     wocra::wOcraSegCartesianTaskManager*   tmp_tmLeftHandCart = dynamic_cast<wocra::wOcraSegCartesianTaskManager*>(taskManagers["tmLeftHandCart"]);
     // tmLeftHandCart->setPosition(desiredPos);
     std::cout << "\n---\nDesired position: " << desiredPos.transpose() << std::endl;
     std::cout << "Current pose: " << state.getSegmentPosition(lHandIndex).getTranslation().transpose()<< std::endl;
     std::cout << "Error: " << tmp_tmLeftHandCart->getTaskError().transpose() << "\n" << std::endl;
 }
    void PoseTest::doUpdate(double time, wocra::wOcraModel& state, void** args)
    {
        wocra::wOcraSegPoseTaskManager*   tmp_tmLeftHandPose = dynamic_cast<wocra::wOcraSegPoseTaskManager*>(taskManagers["tmLeftHandPose"]);
        // tmLeftHandCart->setPosition(desiredPos);
        std::cout << "\n---\nDesired pose: " << endingDispd << std::endl;
        std::cout << "Current pose: " << state.getSegmentPosition(lHandIndex)<< std::endl;
        std::cout << "Error: " << tmp_tmLeftHandPose->getTaskError().transpose() << "\n" << std::endl;
        // tmLeftHandPose->printTaskErrors();
}
// namespace sequence{
    void PoseTest::doInit(wocra::wOcraController& ctrl, wocra::wOcraModel& model)
    {
        ocraWbiModel& wbiModel = dynamic_cast<ocraWbiModel&>(model);

        // Task Coeffs
        double Kp = 10.0;
        double Kd = 2.0 * sqrt(Kp);
        double wFullPosture = 0.001;
        double wPartialPosture = 0.01;
        double wLeftHandTask = 1.0;

        // Full posture task
        Eigen::VectorXd nominal_q = Eigen::VectorXd::Zero(model.nbInternalDofs());
        getNominalPosture(model, nominal_q);
        taskManagers["tmFull"] = new wocra::wOcraFullPostureTaskManager(ctrl, model, "fullPostureTask", ocra::FullState::INTERNAL, Kp, Kd, wFullPosture, nominal_q);

        // Partial (torso) posture task
        Eigen::VectorXi torso_indices(3);
        Eigen::VectorXd torsoTaskPosDes(3);
        torso_indices << wbiModel.getDofIndex("torso_pitch"), wbiModel.getDofIndex("torso_roll"), wbiModel.getDofIndex("torso_yaw");
        torsoTaskPosDes << M_PI / 18, 0, 0;
        taskManagers["tmPartialTorso"] = new wocra::wOcraPartialPostureTaskManager(ctrl, model, "partialPostureTorsoTask", ocra::FullState::INTERNAL, torso_indices, Kp, Kd, wPartialPosture, torsoTaskPosDes);

        lHandIndex = model.getSegmentIndex("l_hand");

        Eigen::Vector3d YZ_disp;
        YZ_disp << 0.0, 0.2, 0.2;


        Eigen::Displacementd startingDispd  = model.getSegmentPosition(lHandIndex);
        std::cout << startingDispd << std::endl;
        // start and end positions
        Eigen::Vector3d desiredPos = startingDispd.getTranslation() + YZ_disp;
        // Eigen::Rotation3d desiredOrientation = Eigen::Rotation3d(1.0, 0.0, 0.0, 0.0);
        Eigen::Rotation3d desiredOrientation = startingDispd.getRotation();

        endingDispd    = Eigen::Displacementd(desiredPos, desiredOrientation);//, startingDispd.getRotation());//.inverse());

        // endingDispd    = startingDispd;
        std::cout << endingDispd << std::endl;
        // Left hand cartesian task
        taskManagers["tmLeftHandPose"]      = new wocra::wOcraSegPoseTaskManager(ctrl, model, "leftHandPoseTask", "l_hand", ocra::XYZ, Kp, Kd, wLeftHandTask, endingDispd);
    }
void Sequence_iCub_01_Standing::doInit(wocra::wOcraController& ctrl, wocra::wOcraModel& model)
{
    // Initialise full posture task
    Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model.nbInternalDofs());
    q_full[model.getDofIndex("l_elbow_pitch")] = PI/8.0;
    q_full[model.getDofIndex("r_elbow_pitch")] = PI/8.0;
    q_full[model.getDofIndex("l_knee")] = -0.05;
    q_full[model.getDofIndex("r_knee")] = -0.05;
    q_full[model.getDofIndex("l_ankle_pitch")] = -0.05;
    q_full[model.getDofIndex("r_ankle_pitch")] = -0.05;
    q_full[model.getDofIndex("l_shoulder_roll")] = PI/8.0;
    q_full[model.getDofIndex("r_shoulder_roll")] = PI/8.0;

    taskManagers["tmFull"] = new wocra::wOcraFullPostureTaskManager(ctrl, model, "fullPostureTask", ocra::FullState::INTERNAL, 9.0, 2*sqrt(9.0), 0.0001, q_full, false);

    // Initialise waist pose
    taskManagers["tmSegPoseWaist"] = new wocra::wOcraSegPoseTaskManager(ctrl, model, "waistPoseTask", "waist", ocra::XYZ, 36.0, 2*sqrt(36.0), 1.0, Eigen::Displacementd(0.0,0.0,0.58,-M_SQRT1_2,0.0,0.0,M_SQRT1_2), false);

    // Initialise partial posture task
    Eigen::VectorXi sdofs(3);
    sdofs << model.getDofIndex("torso_pitch"), model.getDofIndex("torso_roll"), model.getDofIndex("torso_yaw");
    Eigen::VectorXd zero = Eigen::VectorXd::Zero(3);

    taskManagers["tmPartialBack"] = new wocra::wOcraPartialPostureTaskManager(ctrl, model, "partialPostureBackTask", ocra::FullState::INTERNAL, sdofs, 16.0, 2*sqrt(16.0), 0.001, zero, false);

    double mu_sys = 0.5;
    double margin = 0.0;

    double sqrt2on2 = sqrt(2.0)/2.0;
    Eigen::Rotation3d rotLZdown = Eigen::Rotation3d(-sqrt2on2, 0.0, -sqrt2on2, 0.0) * Eigen::Rotation3d(0.0, 1.0, 0.0, 0.0);
    Eigen::Rotation3d rotRZdown = Eigen::Rotation3d(0.0, sqrt2on2, 0.0, sqrt2on2) * Eigen::Rotation3d(0.0, 1.0, 0.0, 0.0);

    // Initialise left foot contacts
    std::vector<Eigen::Displacementd> LFContacts;
    LFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039,-.027,-.031), rotLZdown));
    LFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039, .027,-.031), rotLZdown));
    LFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039, .027, .099), rotLZdown));
    LFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039,-.027, .099), rotLZdown));

    taskManagers["tmFootContactLeft"] = new wocra::wOcraContactSetTaskManager(ctrl, model, "leftFootContactTask", "l_foot", LFContacts, mu_sys, margin, false);

    // Initailise right foot contacts
    std::vector<Eigen::Displacementd> RFContacts;
    RFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039,-.027, .031), rotRZdown));
    RFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039, .027, .031), rotRZdown));
    RFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039, .027,-.099), rotRZdown));
    RFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039,-.027,-.099), rotRZdown));

    taskManagers["tmFootContactRight"] = new wocra::wOcraContactSetTaskManager(ctrl, model, "RightFootContactTask", "r_foot", RFContacts, mu_sys, margin, false);
}
// namespace sequence{
    void TrajectoryTrackingTest::doInit(wocra::wOcraController& ctrl, wocra::wOcraModel& model)
    {
        ocraWbiModel& wbiModel = dynamic_cast<ocraWbiModel&>(model);

        // Task Coeffs
        double Kp = 20.0;
        double Kd = 4.0 * sqrt(Kp);

        double Kp_hand = 40.0;
        double Kd_hand = 2.0 *sqrt(Kp_hand);
        double wFullPosture = 0.0001;
        double wPartialPosture = 0.1;
        double wLeftHandTask = 1.0;

        // Full posture task
        Eigen::VectorXd nominal_q = Eigen::VectorXd::Zero(model.nbInternalDofs());
        getNominalPosture(model, nominal_q);
        taskManagers["tmFull"] = new wocra::wOcraFullPostureTaskManager(ctrl, model, "fullPostureTask", ocra::FullState::INTERNAL, Kp, Kd, wFullPosture, nominal_q);

        // Partial (torso) posture task
        Eigen::VectorXi torso_indices(3);
        Eigen::VectorXd torsoTaskPosDes(3);
        torso_indices << wbiModel.getDofIndex("torso_pitch"), wbiModel.getDofIndex("torso_roll"), wbiModel.getDofIndex("torso_yaw");
        torsoTaskPosDes << 0, -10.0*(M_PI / 180.0), 40.0*(M_PI / 180.0);
        // torsoTaskPosDes << 0.0, 0.0, 0.0;
        taskManagers["tmPartialTorso"] = new wocra::wOcraPartialPostureTaskManager(ctrl, model, "partialPostureTorsoTask", ocra::FullState::INTERNAL, torso_indices, 6., 2.0 * sqrt(6.), wPartialPosture, torsoTaskPosDes);

        // Right hand cartesian task
        Eigen::Vector3d posRHandDesDelta(0.1, 0.08, 0.15);

        Eigen::Vector3d posRHandDes = model.getSegmentPosition(model.getSegmentIndex("r_hand")).getTranslation();
        posRHandDes = posRHandDes + posRHandDesDelta;


        taskManagers["tmSegCartHandRight"] = new wocra::wOcraSegCartesianTaskManager(ctrl, model, "rightHandCartesianTask", "r_hand", ocra::XYZ, Kp_hand, Kd_hand, 1.0, posRHandDes);

        /**
        * Left hand task. Pick one of these booleans to test the different constructors.
        */
        //*************** Type of Trajectory ******************//
        bool isLinInterp = false;
        bool isMinJerk = true;
        //*****************************************************//

        //***************** Type of Reference ******************//
        isDisplacementd         = false;
        isRotation3d            = false;
        isCartesion             = false;
        isCartesionWaypoints    = true;
        //*****************************************************//
        int boolSum = isCartesion + isCartesionWaypoints + isDisplacementd + isRotation3d;
        if (boolSum>1){std::cout << "\nYou picked too many reference types." << std::endl;}
        else if (boolSum<1){std::cout << "\nYou picked too few reference types." << std::endl;}

        lHandIndex = model.getSegmentIndex("l_hand");

        Eigen::Vector3d YZ_disp;
        YZ_disp << -0.15, 0.1, 0.2;

        // start and end displacements & rotations
        Eigen::Displacementd startingDispd  = model.getSegmentPosition(lHandIndex);
        Eigen::Rotation3d startingRotd      = startingDispd.getRotation();

        endingRotd  = Eigen::Rotation3d(0.105135,   0.0828095,   0.253438,    -0.958049);
        endingDispd = Eigen::Displacementd(startingDispd.getTranslation() + YZ_disp, endingRotd);

        // start and end positions
        Eigen::Vector3d startingPos = startingDispd.getTranslation();
        desiredPos  = startingPos + YZ_disp;

        // multiple position waypoints
        Eigen::MatrixXd waypoints(3,5);
        Eigen::MatrixXd squareDisplacement(3,5);
        waypoints << startingPos, startingPos, startingPos, startingPos, startingPos;
        squareDisplacement << 0.0, 0.0, 0.0, 0.0, 0.0,
                              0.0, 0.15, 0.15, 0.0, 0.0,
                              0.0, 0.0, 0.25, 0.25, 0.0;
        waypoints += squareDisplacement;

        if (isLinInterp)
        {
            /**
            * Linear interpolation trajectory constructor tests:
            */
            leftHandTrajectory = new wocra::wOcraLinearInterpolationTrajectory();

            if      (isDisplacementd)       {leftHandTrajectory->setWaypoints(startingDispd, endingDispd);}
            else if (isRotation3d)          {leftHandTrajectory->setWaypoints(startingRotd, endingRotd);}
            else if (isCartesion)           {leftHandTrajectory->setWaypoints(startingPos, desiredPos);}
            else if (isCartesionWaypoints)  {leftHandTrajectory->setWaypoints(waypoints);}
            else                            {std::cout << "\nGotta pick a reference type m**********r!" << std::endl;}
        }
        else if (isMinJerk)
        {
            /**
            * Minimum jerk trajectory constructor tests:
            */
            leftHandTrajectory = new wocra::wOcraMinimumJerkTrajectory();

            if      (isDisplacementd)       {leftHandTrajectory->setWaypoints(startingDispd, endingDispd);}
            else if (isRotation3d)          {leftHandTrajectory->setWaypoints(startingRotd, endingRotd);}
            else if (isCartesion)           {leftHandTrajectory->setWaypoints(startingPos, desiredPos);}
            else if (isCartesionWaypoints)  {leftHandTrajectory->setWaypoints(waypoints);}
            else                            {std::cout << "\nGotta pick a reference type m**********r!" << std::endl;}
        }
        else{std::cout << "\nGotta pick a trajectory type m**********r!" << std::endl;}



        if      (isDisplacementd)      {taskManagers["tmLeftHandPose"]      = new wocra::wOcraSegPoseTaskManager(ctrl, model, "leftHandPoseTask", "l_hand", ocra::XYZ, Kp_hand, Kd_hand, wLeftHandTask, startingDispd);}
        else if (isRotation3d)         {taskManagers["tmLeftHandOrient"]    = new wocra::wOcraSegOrientationTaskManager(ctrl, model, "leftHandOrientationTask", "l_hand", Kp_hand, Kd_hand, wLeftHandTask, startingRotd);}
        else if (isCartesion)          {taskManagers["tmLeftHandCart"]      = new wocra::wOcraSegCartesianTaskManager(ctrl, model, "leftHandCartesianTask", "l_hand", ocra::XYZ, Kp_hand, Kd_hand, wLeftHandTask, startingPos);}
        else if (isCartesionWaypoints) {taskManagers["tmLeftHandCart"]      = new wocra::wOcraSegCartesianTaskManager(ctrl, model, "leftHandCartesianTask", "l_hand", ocra::XYZ, Kp_hand, Kd_hand, wLeftHandTask, startingPos);}


    // tmSegCartHandRight = new wocra::wOcraSegCartesianTaskManager(ctrl, model, "rightHandCartesianTask", "r_hand", ocra::XYZ, Kp_hand, Kd_hand, wLeftHandTask, posRHandDes);
    }