/* static */ bool ocraWbiConversions::eigenDispdToWbiFrame(const Eigen::Displacementd &disp, wbi::Frame &frame)
    {
		double p_wbi[3];
		Eigen::Vector3d p;
		p = disp.getTranslation();

		p_wbi[0] = p[0];
		p_wbi[1] = p[1];
		p_wbi[2] = p[2];

		double _x, _y, _z, _w;
		_x = disp.getRotation().x();
		_y = disp.getRotation().y();
		_z = disp.getRotation().z();
		_w = disp.getRotation().w();

		wbi::Rotation R;	R.quaternion(_x, _y, _z, _w);
		wbi::Frame _frame(R, p_wbi);
		frame = _frame;

		// Check if y-translation value is the same between the two data containers
		if (frame.p[1]!=p(1,0)){
			std::cerr << "Displacementd could not be converted to Wbi Frame \n";
			return false;
		}

		return true;
    }
/* static */ bool ocraWbiConversions::wbiFrameToEigenDispd(const wbi::Frame &frame, Eigen::Displacementd &disp)
    {
		double _x, _y, _z, _w;
		frame.R.getQuaternion(_x, _y, _z, _w);

		double x, y, z;
		x = frame.p[0];
		y = frame.p[1];
		z = frame.p[2];

		Eigen::Vector3d trans;
		trans << x, y, z;

		Eigen::Displacementd _disp(x,y,z,_w,_x,_y,_z);

		disp = _disp;

		// Check if y-translation value is the same between the two data containers
		if (disp.getTranslation()(1,0)!=y){
			std::cerr << "Wbi Frame could not be converted to Displacementd\n";
			return false;
		}

		return true;
    }
// 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);
    }
// 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);
    }
Eigen::VectorXd TrajectoryThread::getCurrentTaskStateAsVector()
{
    Eigen::VectorXd startVector = Eigen::VectorXd::Zero(weightDimension);
    ocra::TaskState state = task->getTaskState();

    switch (taskType) {
        case ocra::Task::META_TASK_TYPE::UNKNOWN:
        {
            // do nothing
        }break;
        case ocra::Task::META_TASK_TYPE::POSITION:
        {
            startVector = state.getPosition().getTranslation();
        }break;
        case ocra::Task::META_TASK_TYPE::ORIENTATION:
        {
            Eigen::Rotation3d quat = state.getPosition().getRotation();
            startVector = Eigen::VectorXd(4);
            startVector << quat.w(), quat.x(), quat.y(), quat.z();
        }break;
        case ocra::Task::META_TASK_TYPE::POSE:
        {
            Eigen::Displacementd disp = state.getPosition();
            startVector = Eigen::VectorXd(7);
            startVector << disp.x(), disp.y(), disp.z(), disp.qw(), disp.qx(), disp.qy(), disp.qz();
        }break;
        case ocra::Task::META_TASK_TYPE::FORCE:
        {
            startVector = state.getWrench();
        }break;
        case ocra::Task::META_TASK_TYPE::COM:
        {
            startVector = state.getPosition().getTranslation();

        }break;
        case ocra::Task::META_TASK_TYPE::COM_MOMENTUM:
        {
            startVector = state.getPosition().getTranslation();

        }break;
        case ocra::Task::META_TASK_TYPE::PARTIAL_POSTURE:
        {
            startVector = state.getQ();

        }break;
        case ocra::Task::META_TASK_TYPE::FULL_POSTURE:
        {
            startVector = state.getQ();

        }break;
        case ocra::Task::META_TASK_TYPE::PARTIAL_TORQUE:
        {
            startVector = state.getTorque();

        }break;
        case ocra::Task::META_TASK_TYPE::FULL_TORQUE:
        {
            startVector = state.getTorque();

        }break;
        default:
        {

        }break;
    }
    return startVector;
}