C++ VectorXd::head方法代码示例

//! Convert Cartesian to cylindrical state.
Eigen::VectorXd convertCartesianToCylindrical( const Eigen::VectorXd& cartesianState )
{
    // Create cylindrical state vector, initialized with zero entries.
    Eigen::VectorXd cylindricalState = Eigen::VectorXd::Zero( 6 );

    // Compute and set cylindrical coordinates.
    cylindricalState.head( 3 ) = convertCartesianToCylindrical(
                Eigen::Vector3d( cartesianState.head( 3 ) ) );

    // Compute and set cylindrical velocities.
    /* If radius = 0, then Vr = sqrt(xdot^2+ydot^2) and Vtheta = 0,
       else Vr = (x*xdot+y*ydot)/radius and Vtheta = (x*ydot-y*xdot)/radius.
    */
    if ( cylindricalState( 0 ) <= std::numeric_limits< double >::epsilon( ) )
    {
        cylindricalState.tail( 3 ) <<
            std::sqrt( pow( cartesianState( 3 ), 2 ) + pow( cartesianState( 4 ), 2 ) ), // Vr
            0.0,                                                                        // Vtheta
            cartesianState( 5 );                                                        // Vz
    }

    else
    {
        cylindricalState.tail( 3 ) <<
            ( cartesianState( 0 ) * cartesianState( 3 )
              + cartesianState( 1 ) * cartesianState( 4 ) ) / cylindricalState( 0 ),    // Vr
            ( cartesianState( 0 ) * cartesianState( 4 )
              - cartesianState( 1 ) * cartesianState( 3 ) ) / cylindricalState( 0 ),    // Vtheta
                cartesianState( 5 );                                                    // Vz
    }

    return cylindricalState;
}

void ExperimentalTrajectory::removeWaypoint(int index)
{
    std::cout << "Removing waypoint index: " << index << "..." << std::endl;
    if ( (index>=0) && (index<nWaypoints) )
    {
        Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints-1);

        newWaypointsMat.leftCols(index) = waypoints.leftCols(index);
        newWaypointsMat.rightCols(nWaypoints-1-index) = waypoints.rightCols(nWaypoints-1-index);

        waypoints.resize(nDoF, nWaypoints-1);
        waypoints = newWaypointsMat;

        Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints-2);

        int durVecIndex = index;
        durationVectorTmp.head(durVecIndex) = pointToPointDurationVector.head(durVecIndex);
        durationVectorTmp.tail(nWaypoints -1 -durVecIndex) = pointToPointDurationVector.tail(nWaypoints -1 -durVecIndex);

        pointToPointDurationVector.resize(durationVectorTmp.size());
        pointToPointDurationVector = durationVectorTmp;


        reinitialize();
    }
    else{
        std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of index bounds. Should have fall between 0 and " << nWaypoints-1 << "." << std::endl;
    }

}

bool RectangleHandler::getFeaturePoseInWorldFrame(long int id,
		Eigen::VectorXd& c) const {

	const string &sensor = getFeatureSensor(id);

	ParameterWrapper_Ptr f_par = _filter->getParameterByName(sensor + "_F"); // anchor frame

	if (!f_par) {
		return false;
	}

	const Eigen::VectorXd &fw = f_par->getEstimate();

	Eigen::VectorXd dim(2);
	getFeatureDimensions(id, dim);

	Eigen::Quaterniond R_WO(fw(3),fw(4),fw(5),fw(6));
	Eigen::Vector3d t_WO(fw(0),fw(1),fw(2));
	Eigen::Vector3d t_OC(dim(0)/2.0, dim(1)/2.0, 0.0);

	c.head(3) = t_WO+R_WO.toRotationMatrix()*t_OC;
	c.tail(4) =  fw.tail(4);

	return true;
}

/**
 * Returns the expected return of the given portfolio structure.
 *
 * @param portfolio  the portfolio structure
 *
 * @return the expected portfolio return
 */
double EfficientPortfolioWithRisklessAsset::portfolioReturn(const Eigen::VectorXd& portfolio)
{
	Eigen::VectorXd riskyAssets = portfolio.head(dim - 1);

	double riskReturn = riskyAssets.dot(mean);
	double risklessReturn = portfolio(dim - 1) * _risklessRate;

	return riskyAssets.dot(mean) + portfolio(dim - 1) * _risklessRate;
}

// receive FT Sensor data
void
recvFT(const geometry_msgs::WrenchStampedConstPtr& msg){
    mutex_force.lock();
//    std::cout <<"Fx"<<"\t"<<"Fy"<<"\t"<<"Fz"<<"\t"<<"Tx"<<"\t"<<"Ty"<<"\t"<<"Tz"<<std::endl;
//     if (counter >50){
//    std::cout << msg->wrench.force.x<<","<<msg->wrench.force.y<<","<<msg->wrench.force.z<<","<<msg->wrench.torque.x<<","<<msg->wrench.torque.y<<","<<msg->wrench.torque.z<<std::endl;
//         counter = 0;
//     }
//    else{
//        counter ++;
// }
    Eigen::Vector3d grav_tmp;
    grav_tmp.setZero();
    grav_tmp(2) = Grav_Schunk_Acc+tool_grav;
    ft_gama->raw_ft_f(0) = msg->wrench.force.x;
    ft_gama->raw_ft_f(1) = msg->wrench.force.y;
    ft_gama->raw_ft_f(2) = msg->wrench.force.z-Grav_Schunk_Acc;
    ft_gama->raw_ft_t(0) = msg->wrench.torque.x;
    ft_gama->raw_ft_t(1) = msg->wrench.torque.y;
    ft_gama->raw_ft_t(2) = msg->wrench.torque.z;
    //get rid of tool/pokingstick gravity--calib
    ft_gama->calib_ft_f = right_rs->robot_orien["eef"] * ft_gama->raw_ft_f + grav_tmp;
    ft_gama->calib_ft_t = ft_gama->raw_ft_t;
    Eigen::Vector3d f_offset;
    f_offset.setZero();
    f_offset(0) = -0.5;
    f_offset(1) = -0.5;
    f_offset(2) = -0.6;
    //use smooth filter to get rid of noise.
    ft.head(3) = ft_gama->filtered_gama_f = gama_f_filter->push(ft_gama->calib_ft_f) - f_offset;
    //get rid of noise of no contacting
    if(ft_gama->filtered_gama_f.norm() <1.5)
	    ft.head(3).setZero();
    ft.tail(3) = ft_gama->filtered_gama_t = gama_t_filter->push(ft_gama->calib_ft_t);
    
    counter_t ++;
    if(counter_t%50==0){
        counter_t = 0;
//        std::cout<<"estimated contact force: "<<ft_gama->filtered_gama_f(0)<<","<<ft_gama->filtered_gama_f(1)<<","
//                <<ft_gama->filtered_gama_f(2)<<","<<ft_gama->filtered_gama_t(0)<<","<<ft_gama->filtered_gama_t(1)<<","<<ft_gama->filtered_gama_t(2)<<std::endl;
}
    
    mutex_force.unlock();
}

            void set_h_params(const Eigen::VectorXd& p)
            {
                _h_params = p.head(_tr.rows() * _tr.cols());
                for (int c = 0; c < _tr.cols(); c++)
                    for (int r = 0; r < _tr.rows(); r++)
                        _tr(r, c) = p[r * _tr.cols() + c];

                if (_mean_function.h_params_size() > 0)
                    _mean_function.set_h_params(p.tail(_mean_function.h_params_size()));
            }

void ExperimentalTrajectory::addNewWaypoint(Eigen::VectorXd newWaypoint, double waypointTime)
{
    std::cout << "Adding waypoint at: " << waypointTime << " seconds..." << std::endl;

    if ( (newWaypoint.rows() == nDoF) && (waypointTime>=0.0) && (waypointTime<=kernelCenters.maxCoeff()) )
    {
        //Find out where the new T falls...
        for (int i=0; i<kernelCenters.size(); i++)
        {
            std::cout << "i = " << i << std::endl;
            if (kernelCenters(i)>waypointTime) {
                youngestWaypointIndex=i;
                std::cout << "youngestWaypointIndex" << youngestWaypointIndex << std::endl;
                i = kernelCenters.size();
            }
        }


        Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints+1);
        newWaypointsMat.leftCols(youngestWaypointIndex) = waypoints.leftCols(youngestWaypointIndex);
        newWaypointsMat.col(youngestWaypointIndex) = newWaypoint;
        newWaypointsMat.rightCols(nWaypoints - youngestWaypointIndex) = waypoints.rightCols(nWaypoints - youngestWaypointIndex);

        waypoints.resize(nDoF, nWaypoints+1);
        waypoints = newWaypointsMat;

        Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints);

        std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;

        durationVectorTmp.head(youngestWaypointIndex-1) = pointToPointDurationVector.head(youngestWaypointIndex-1);
        durationVectorTmp.row(youngestWaypointIndex-1) << waypointTime - kernelCenters(youngestWaypointIndex-1);
        durationVectorTmp.tail(nWaypoints - youngestWaypointIndex) = pointToPointDurationVector.tail(nWaypoints - youngestWaypointIndex);

        pointToPointDurationVector.resize(durationVectorTmp.size());
        pointToPointDurationVector = durationVectorTmp;

        std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;


        reinitialize();


    }
    else{
        if (newWaypoint.rows() != nDoF){
            std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint is not the right size. Should have dim = " <<nDoF << "x1." << std::endl;
        }
        if ((waypointTime<=0.0) || (waypointTime>=kernelCenters.maxCoeff())){
            std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of time bounds. Should have fall between 0.0 and " << kernelCenters.maxCoeff() << " seconds." << std::endl;
        }
    }


}

//! Convert cylindrical to Cartesian state.
Eigen::VectorXd convertCylindricalToCartesian( const Eigen::VectorXd& cylindricalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    Eigen::VectorXd cartesianState = Eigen::VectorXd::Zero( 6 );

    // Get azimuth angle, theta.
    double azimuthAngle = cylindricalState( 1 );

    // Compute and set Cartesian coordinates.
    cartesianState.head( 3 ) = convertCylindricalToCartesian(
                Eigen::Vector3d( cylindricalState.head( 3 ) ) );

    // If r = 0 AND Vtheta > 0, then give warning and assume Vtheta=0.
    if ( std::fabs(cylindricalState( 0 )) <= std::numeric_limits< double >::epsilon( )
         && std::fabs(cylindricalState( 4 )) > std::numeric_limits< double >::epsilon( ) )
    {
        std::cerr << "Warning: cylindrical velocity Vtheta (r*thetadot) does not equal zero while "
                  << "the radius (r) is zero! Vtheta is taken equal to zero!\n";

        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                              // zdot
    }

    else
    {
        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle )
                   - cylindricalState( 4 ) * std::sin( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle )
                   + cylindricalState( 4 ) * std::cos( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                                // zdot
    }

    return cartesianState;
}

void ODELCPSolver::transferSolFromODEFormulation(const Eigen::VectorXd& _x,
                                              Eigen::VectorXd* _xOut,
                                              int _numDir,
                                              int _numContacts) {
  int numOtherConstrs = _x.size() - _numContacts * 3;
  *_xOut = Eigen::VectorXd::Zero(_numContacts * (2 + _numDir)
                                 + numOtherConstrs);

  _xOut->head(_numContacts) = _x.head(_numContacts);

  int offset = _numDir / 4;
  for (int i = 0; i < _numContacts; ++i) {
    (*_xOut)[_numContacts + i * _numDir + 0] = _x[_numContacts + i * 2 + 0];
    (*_xOut)[_numContacts + i * _numDir + offset] = _x[_numContacts + i * 2
                                                       + 1];
  }
  for (int i = 0; i < numOtherConstrs; i++)
    (*_xOut)[_numContacts * (2 + _numDir) + i] = _x[_numContacts * 3 + i];
}

bool AnchoredRectangleHandler::getFeaturePoseInWorldFrame(long int id,
    Eigen::VectorXd& c) const {

  const string &sensor = getFeatureSensor(id);

  ParameterWrapper_Ptr f_par = _filter->getParameterByName(sensor + "_F"); // anchor frame
  ParameterWrapper_Ptr fohp_par = _filter->getParameterByName(sensor + "_FOhp"); // anchor frame
  ParameterWrapper_Ptr foq_par = _filter->getParameterByName(sensor + "_FOq"); // anchor frame

  if (!f_par || !fohp_par || !foq_par) {
    return false;
  }

  const Eigen::VectorXd &fw = f_par->getEstimate();

  const Eigen::VectorXd &FOhp = fohp_par->getEstimate();
  const Eigen::VectorXd &FOq = foq_par->getEstimate();

  Eigen::Quaterniond Fq_e(fw(3), fw(4), fw(5), fw(6)); // anchor frame orientation wrt world

  // compute the position of the lower left corner of the object starting from anchor frame and homogeneous point

  Eigen::Vector3d FOhp_e;
  FOhp_e << FOhp(0), FOhp(1), 1.0; // construct a proper homogeneous point from the first two components of the parameter

  Eigen::Vector3d Ow;
  Ow = fw.head(3) + 1 / FOhp(2) * (Fq_e.toRotationMatrix() * FOhp_e);

  // orientation of the object
  Eigen::Quaterniond FOq_e(FOq(0), FOq(1), FOq(2), FOq(3));
  Eigen::Quaterniond R_WO = Fq_e * FOq_e;

  Eigen::VectorXd dim(2);
  getFeatureDimensions(id, dim);

  Eigen::Vector3d t_OC(dim(0) / 2.0, dim(1) / 2.0, 0.0);

  c.head(3) = Ow + R_WO.toRotationMatrix() * t_OC;
  c.tail(4) << R_WO.w(), R_WO.x(), R_WO.y(), R_WO.z();

  return true;
}

void IRLTest::generateData()
{
  int num_active_features;
  int num_data;
  int num_samples_per_data;
  double cost_noise_stddev;

  num_features_ = 10;
  num_active_features = 2;
  num_data = 10;
  num_samples_per_data = 10;
  cost_noise_stddev = 0.1;

  // generate random weights
  real_weights_ = Eigen::VectorXd::Zero(num_features_);
  real_weights_.head(num_active_features).setRandom();

  data_.resize(num_data);
  for (int i=0; i<num_data; ++i)
  {
    IRLData* d = new IRLData(num_features_);
    Eigen::MatrixXd features = Eigen::MatrixXd::Zero(num_samples_per_data, num_features_);
    Eigen::VectorXd target = Eigen::VectorXd::Zero(num_samples_per_data);
    features.setRandom();

    // compute w^t * phi
    Eigen::VectorXd costs = features*real_weights_;

    // add random noise to costs
    costs += cost_noise_stddev * Eigen::VectorXd::Random(num_samples_per_data);

    // set min cost target to 1
    int min_index;
    double min_cost = costs.minCoeff(&min_index);
    target(min_index) = 1.0;
    d->addSamples(features, target);
    data_[i].reset(d);
  }
  real_weights_exist_ = true;
}

void ExperimentalTrajectory::initializeTrajectory()
{
    originalWaypoints = waypoints;
    kernelLengthParameter = pointToPointDurationVector.mean();
    // Add a waypoint to the end.
    int extraWaypoints = 1;
    int nStartWp = extraWaypoints;
    int nEndWp = extraWaypoints;

    int nWpNew = nWaypoints + nStartWp + nEndWp;

    Eigen::MatrixXd waypointsTmp = Eigen::MatrixXd::Zero(nDoF, nWpNew);

    waypointsTmp.leftCols(nStartWp) = waypoints.leftCols(1).replicate(1,nStartWp);
    waypointsTmp.middleCols(nStartWp, nWaypoints) = waypoints;
    waypointsTmp.rightCols(nEndWp) = waypoints.rightCols(1).replicate(1,nEndWp);

    waypoints.resize(nDoF, nWaypoints);
    waypoints = waypointsTmp;
    // std::cout << pointToPointDurationVector << std::endl;


    Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWpNew-1);

    double extraWpDt = 0.01 * kernelLengthParameter;
    // double extraWpDt = 0.01 * pointToPointDurationVector.sum();
    // std::cout << "extraWpDt: " << extraWpDt << std::endl;

    durationVectorTmp.head(nStartWp) = Eigen::VectorXd::Constant(nStartWp, extraWpDt);
    durationVectorTmp.segment(nStartWp, nWaypoints-1) = pointToPointDurationVector;
    durationVectorTmp.tail(nEndWp) = Eigen::VectorXd::Constant(nEndWp, extraWpDt);

    pointToPointDurationVector.resize(durationVectorTmp.size());
    pointToPointDurationVector = durationVectorTmp;

    nWaypoints = nWpNew;

    std::cout << pointToPointDurationVector << std::endl;
    calculateVarianceParameters();
}

//==============================================================================
MyWindow::MyWindow(bioloidgp::robot::HumanoidController* _controller)
    : SimWindow(),
      mController(_controller),
	  mController2(NULL),
	  mTime(0)
	  
{

	mDisplayTimeout = 10;
	DecoConfig::GetSingleton()->GetDouble("Server", "timeout", mDisplayTimeout);
    // 0.166622 0.548365 0.118241 0.810896
	//Eigen::Quaterniond q(0.15743952233047084, 0.53507160411429699, 0.10749289301287825, 0.82301687360383402);
 //   mTrackBall.setQuaternion(q);
    //mTrans = Eigen::Vector3d(-32.242,  212.85, 21.7107);
	
	mTrans = Eigen::Vector3d(0, -212.85, 0); 
	mFrameCount = 0;
	
	string fileName;
	mOffsetTransform = Eigen::Isometry3d::Identity();
	DecoConfig::GetSingleton()->GetString("Player", "FileName", fileName);
	readMovieFile(fileName);
	mController->robot()->setPositions(mMovie[0].mPose);

	int isRobot2 = 0;
	DecoConfig::GetSingleton()->GetInt("Player", "IsRobot2", isRobot2);
	if (isRobot2)
	{
		DecoConfig::GetSingleton()->GetString("Player", "FileName2", fileName);
		readMovieFile2(fileName);
		Eigen::VectorXd rootPos = mMovie[0].mPose.head(6);
		Eigen::VectorXd rootPos2 = mMovie2[0].mPose.head(6);
		Eigen::Matrix3d rootR = dart::math::expMapRot(rootPos.head(3));
		Eigen::Matrix3d rootR2 = dart::math::expMapRot(rootPos2.head(3));
		mOffsetTransform.linear() = rootR * rootR2.inverse();
	}
	glutTimerFunc(mDisplayTimeout, refreshTimer, 0);

}

//.........这里部分代码省略.........
                if (vertex_flag[index_j[p]]) {
                    for (int k=0; k<3; ++k) 
                        for (int dim=0; dim < 3; ++dim)
                            Y[dim](row + k) +=  weight_smooth*Q_j_hat(p, k)*dst.vertex_coord[vertex_real_col[index_j[p]]][dim];
                } else {
                    for (int k=0; k<3; ++k) 
                        A.coeffRef(row+k, src.poly_num + vertex_real_col[index_j[p]]) += -weight_smooth*Q_j_hat(p, k);
                }
            }
            row += 3;
        }
    }
    energy_size[0] = row;

    double weight_identity = weights[1];
    for (int i=0; i<src.poly_num; ++i) {
        Eigen::Matrix3d& Q_i_hat = Q_hat_inverse[i];
        unsigned int* index_i = src.polyIndex[i].vert_index;
        Y[0](row) = weight_identity;    Y[0](row+1) = 0.0;              Y[0](row+2) = 0.0; 
        Y[1](row) = 0.0;                Y[1](row+1) = weight_identity;  Y[1](row+2) = 0.0; 
        Y[2](row) = 0.0;                Y[2](row+1) = 0.0;              Y[2](row+2) = weight_identity; 

        for (int k=0; k<3; ++k)    
            A.coeffRef(row+k, i) = weight_identity*Q_i_hat(0, k);   //n

        if (vertex_flag[index_i[0]]) {
            for (int k=0; k<3; ++k) 
                for (int dim = 0; dim < 3; ++dim)
                    Y[dim](row+k) += weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k))*dst.vertex_coord[vertex_real_col[index_i[0]]][dim];
        } else {
            for (int k=0; k<3; ++k) 
                A.coeffRef(row+k, src.poly_num+vertex_real_col[index_i[0]]) = -weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k));
        }

        for (int p=1; p<3; ++p) {
            if (vertex_flag[index_i[p]]) {
                for (int k=0; k<3; ++k)
                    for (int dim=0; dim<3; ++dim)
                        Y[dim](row + k) += -weight_identity*Q_i_hat(p, k)*dst.vertex_coord[vertex_real_col[index_i[p]]][dim];
            } else {
                for (int k=0; k<3; ++k) 
                    A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[p]]) = weight_identity*Q_i_hat(p, k);
            }
        }

        row += 3;
    }
    energy_size[1] = row;

    double weight_soft_constraint = weights[2];
    for (int i=0; i<soft_corres.size(); ++i) {
        if (vertex_flag[soft_corres[i].first]) continue;
        A.coeffRef(row, src.poly_num + vertex_real_col[soft_corres[i].first]) = weight_soft_constraint;
        for (int dim=0; dim<3; ++dim)
            Y[dim](row) += weight_soft_constraint*dst.vertex_coord[soft_corres[i].second][dim];
        ++row;
    }
    energy_size[2] = row;

    assert (row == rows);
    
       //start solving the least-square problem
    fprintf(stdout, "finished filling matrix\n");
    Eigen::SparseMatrix<double> At = A.transpose();
    Eigen::SparseMatrix<double> AtA = At*A;

    Eigen::SimplicialCholesky<SparseMatrix<double>> solver;
    solver.compute(AtA);
    if (solver.info() != Eigen::Success) {
        fprintf(stdout, "unable to defactorize AtA\n");
        exit(-1);
    }

	VectorXd X[3];
	for (int i=0; i<3; ++i) {
		VectorXd AtY = At*Y[i];
		X[i] = solver.solve(AtY);
		Eigen::VectorXd Energy = A*X[i] - Y[i];
		Eigen::VectorXd smoothEnergy = Energy.head(energy_size[0]);
		Eigen::VectorXd identityEnergy = Energy.segment(energy_size[0], energy_size[1]-energy_size[0]);
		Eigen::VectorXd softRegularEnergy = Energy.tail(energy_size[2]-energy_size[1]);
		fprintf(stdout, "\t%lf = %lf + %lf + %lf\n", 
			Energy.dot(Energy), smoothEnergy.dot(smoothEnergy), 
			identityEnergy.dot(identityEnergy), softRegularEnergy.dot(softRegularEnergy));
	}
    
    //fill data back to src
    for (int i=0; i<src.poly_num; ++i)
        for (int d=0; d<3; ++d)
            src.face_norm[i][d] = X[d](i);
    for (size_t i=0; i<corres.size(); ++i) src.vertex_coord[corres[i].first] = dst.vertex_coord[corres[i].second];
    int p = 0;
    for (int i=0; i<src.vert_num; ++i) {
        if (vertex_flag[i]) continue;
        for (int d=0; d<3; ++d)
            src.vertex_coord[i][d] = X[d](src.poly_num+p);
        ++p;
    }
    return;
}

void run_rightarm(){
    //only call for this function, the ->jnt_position_act is updated
    if((com_okc_right->data_available == true)&&(com_okc_right->controller_update == false)){
		
		
//        kuka_right_arm->update_robot_stiffness();
        kuka_right_arm->get_joint_position_act();
        kuka_right_arm->update_robot_state();
        right_rs->updated(kuka_right_arm);
        if(toolposition_record == true)
	        ToolOriginposition<<right_rs->robot_position["eef"](0)<<","<<right_rs->robot_position["eef"](1)<<","<<right_rs->robot_position["eef"](2)<<std::endl;
        
        //using all kinds of controllers to update the reference
        for(unsigned int i = 0; i < right_ac_vec.size();i++){
            if(right_task_vec[i]->mt == JOINTS)
                right_ac_vec[i]->update_robot_reference(kuka_right_arm,right_task_vec[i],(right_rs->robot_orien["eef"] * local_hinged_axis_vec).normalized(),ft.head(3),right_rs);
            if(right_task_vec[i]->mt == FORCE){
                mutex_force.lock();
                right_ac_vec[i]->update_robot_reference(kuka_right_arm,right_task_vec[i],ft,right_rs);
                mutex_force.unlock();
            }
        }
        //update with act_vec
        right_ac_vec[0]->llv.setZero();
        right_ac_vec[0]->lov.setZero();

        //use CBF to compute the desired joint angle rate
        kuka_right_arm->update_cbf_controller();
        kuka_right_arm->set_joint_command(right_rmt);
        com_okc_right->controller_update = true;
        com_okc_right->data_available = false;
    }
}