C++ MatrixXd::setIdentity方法代码示例

 TestState() :
     x(0.0),
     v(0.0)
 {
     Covariance.resize(ndim(), ndim());
     Covariance.setIdentity();
 };

void FixedAxisOneDoFJoint::v2qdot(double* q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
  v_to_qdot.setIdentity(getNumPositions(), getNumVelocities());
  if (dv_to_qdot) {
    dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
  }
}

void FixedAxisOneDoFJoint::qdot2v(double* q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.setIdentity(getNumVelocities(), getNumPositions());
  if (dqdot_to_v) {
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
  }
}

void RollPitchYawFloatingJoint::v2qdot(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
  v_to_qdot.setIdentity(getNumPositions(), getNumVelocities());

  if (dv_to_qdot) {
    dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
  }
}

void RollPitchYawFloatingJoint::qdot2v(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.setIdentity(getNumVelocities(), getNumPositions());

  if (dqdot_to_v) {
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
  }
}

 PitchRollState()
 {
     Omega.setZero();
     GyroBias.setZero();
     AccelBias.setZero();
     Covariance.resize(ndim(), ndim());
     Covariance.setIdentity();
 };

void FingerKinematics::initialise(KDL::Chain &chain,const std::string name){
    this->name = name;

    for(std::size_t i = 0; i < chain.segments.size();i++){
         joint_names.push_back(chain.segments[i].getJoint().getName());
    }

    ik_solver_vel_wdls = new KDL::ChainIkSolverVel_wdls(chain, 0.001, 5);
    ik_solver_vel_wdls->setLambda(0.01);
    Eigen::MatrixXd TS;  TS.resize(6,6);   TS.setIdentity();   TS(3,3) = 0;     TS(4,4) = 0;     TS(5,5) = 0;
    ik_solver_vel_wdls->setWeightTS(TS);

    fk_solver_pos        = new KDL::ChainFkSolverPos_recursive(chain);
    ik_solver_pos_NR     = new KDL::ChainIkSolverPos_NR(chain,*fk_solver_pos,*ik_solver_vel_wdls);

    num_joints = chain.getNrOfJoints();
    q.resize(num_joints);
    q.data.setZero();
    q_out = q;

}

/**
 * Take as input the full constraint matrix CI for a dimension ny, and
 * the associated ci vector with the 2-norm of each row scaled by delta.
 * Apply the quadratic program and test against all the constraints.
 * Return true if all constraints match, false otherwise.
 */
int rational_fitter_parsec_multi::solve_wrapper( const gesvdm2_args_t *args, subproblem_t *pb, int M, int ny,
						 double *CIptr, double *ciptr )
{
    const vertical_segment *d    = (const vertical_segment*)(args->dataptr);
    rational_function      *rf   = (rational_function*)(pb->rfptr);
    rational_function_1d   *rf1d = rf->get(ny);
    const int np = pb->np;
    const int nq = pb->nq;
    const int N  = np + nq;

    // Compute the solution
    Eigen::MatrixXd CE(N, 0);
    Eigen::VectorXd ce(0);
    Eigen::MatrixXd G (N, N); G.setIdentity();
    Eigen::VectorXd g (N); g.setZero();
    Eigen::VectorXd x (0.0, N);

    Eigen::MatrixXd CI = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Map(CIptr, N, M);
    Eigen::Map<Eigen::VectorXd> ci(ciptr, M);

    // Select the size of the result vector to
    // be equal to the dimension of p + q
    double cost = QuadProgPP::solve_quadprog(G, g, CE, ce, CI, ci, x);
    bool solves_qp = !(cost == std::numeric_limits<double>::infinity());

    if(solves_qp) {
	std::cout << "<<INFO>> got solution for pb with np=" << pb->np << ", nq=" << pb->nq << std::endl;

	// Recopy the vector d
	vec p(np), q(nq);
	double norm = 0.0;

	for(int i=0; (i<N) & solves_qp; ++i) {
	    const double v = x[i];

	    solves_qp = solves_qp && !isnan(v)
		&& (v != std::numeric_limits<double>::infinity());

	    norm += v*v;
	    if(i < np) {
		p[i] = v;
	    }
	    else {
		q[i - np] = v;
	    }
	}

	if (solves_qp) {
	    std::cout << "<<INFO>> got solution to second step for pb with np=" << pb->np << ", nq=" << pb->nq << std::endl;

	    // Rq: doesn't need protection in // since it should be working on independant vectors
	    rf1d->update(p, q);
	    solves_qp = (test_all_constraint( d, rf1d, ny ) == 1);
	}
    }
    else {
	std::cerr << "<<DEBUG>> Didn't get solution to the pb with np=" << pb->np << ", nq=" << pb->nq << std::endl;
    }

    return solves_qp;
}

bool WholeBodyController::initialize(const double Ts)
{
    ros::NodeHandle n("~");
    std::string ns = n.getNamespace();
    //ROS_INFO("Nodehandle %s",n.getNamespace().c_str());
    ROS_INFO("Initializing whole body controller");

    KDL::JntArray q_min, q_max;

    if ( !ChainParser::parse(robot_state_.tree_, joint_name_to_index_, index_to_joint_name_, q_min, q_max) ) {
        return false;
    }

    num_joints_ = joint_name_to_index_.size();
    robot_state_.tree_.getJointNames(joint_name_to_index_,robot_state_.tree_.joint_name_to_index_);
    robot_state_.tree_.getTreeJointIndex(robot_state_.tree_.kdl_tree_, robot_state_.tree_.tree_joint_index_);

    q_current_.resize(num_joints_);
    q_current_.data.setZero();

    // Read parameters
    std::vector<double> JLA_gain(num_joints_);      // Gain for the joint limit avoidance
    std::vector<double> JLA_workspace(num_joints_); // Workspace: joint gets 'pushed' back when it's outside of this part of the center of the workspace

    std::vector<double> posture_q0(num_joints_);
    std::vector<double> posture_gain(num_joints_);

    std::vector<double> admittance_mass(num_joints_);
    std::vector<double> admittance_damping(num_joints_);

    for (std::map<std::string, unsigned int>::iterator iter = joint_name_to_index_.begin(); iter != joint_name_to_index_.end(); ++iter)
    {
        n.param<double> ("/whole_body_controller/joint_limit_avoidance/gain/"+iter->first, JLA_gain[iter->second], 1.0);
        n.param<double> ("/whole_body_controller/joint_limit_avoidance/workspace/"+iter->first, JLA_workspace[iter->second], 0.9);
        n.param<double> ("/whole_body_controller/posture_control/home_position/"+iter->first, posture_q0[iter->second], 0);
        n.param<double> ("/whole_body_controller/posture_control/gain/"+iter->first, posture_gain[iter->second], 1.0);
        n.param<double> ("/whole_body_controller/admittance_control/mass/"+iter->first, admittance_mass[iter->second], 10);
        n.param<double> ("/whole_body_controller/admittance_control/damping/"+iter->first, admittance_damping[iter->second], 10);
    }

    loadParameterFiles();

    // Construct the FK and Jacobian solvers
    robot_state_.fk_solver_ = new KDL::TreeFkSolverPos_recursive(robot_state_.tree_.kdl_tree_);
    robot_state_.tree_.jac_solver_ = new KDL::TreeJntToJacSolver(robot_state_.tree_.kdl_tree_);

    // Initialize admittance controller
    AdmitCont_.initialize(Ts,q_min, q_max, admittance_mass, admittance_damping);

    // Initialize nullspace calculator
    // ToDo: Make this variable
    Eigen::MatrixXd A;
    A.setIdentity(num_joints_,num_joints_);

    ComputeNullspace_.initialize(num_joints_, A);

    // Initialize Joint Limit Avoidance
    //for (uint i = 0; i < num_joints_; i++) ROS_INFO("JLA gain of joint %i is %f",i,JLA_gain[i]);
    JointLimitAvoidance_.initialize(q_min, q_max, JLA_gain, JLA_workspace);
    ROS_INFO("Joint limit avoidance initialized");

    // Initialize Posture Controller
    PostureControl_.initialize(q_min, q_max, posture_q0, posture_gain, joint_name_to_index_);
    ROS_INFO("Posture Control initialized");

    /// Resizing variables (are set to zero in updatehook)
    qdot_reference_.resize(num_joints_);
    q_reference_.resize(num_joints_);
    tau_.resize(num_joints_);
    Jacobians_.resize(4);
    Ns_.resize(4);
    taus_.resize(4);
    for (unsigned int i = 0; i < 4; i++) {
        Jacobians_[i].resize(100, num_joints_);
        Ns_[i].resize(num_joints_, num_joints_);
        taus_[i].resize(num_joints_);
    }

    /// Initialize tracer
    std::vector<std::string> column_names = index_to_joint_name_;
    for (unsigned int i = 0; i < index_to_joint_name_.size(); i++) column_names.push_back("tau_cart"+index_to_joint_name_[i]);
    for (unsigned int i = 0; i < index_to_joint_name_.size(); i++) column_names.push_back("tau_null"+index_to_joint_name_[i]);
    for (unsigned int i = 0; i < index_to_joint_name_.size(); i++) column_names.push_back("tau_total"+index_to_joint_name_[i]);
    column_names.push_back("cartesian_impedance_cost");
    column_names.push_back("collision_avoidance_cost");
    column_names.push_back("joint_limit_cost");
    column_names.push_back("posture_cost");
    int buffersize;
    std::string foldername;
    n.param<int> ("/whole_body_controller/tracing_buffersize", buffersize, 0);
    n.param<std::string> ("/whole_body_controller/tracing_folder", foldername, "/tmp/");
    std::string filename = "joints";
    //std::string folderfilename = foldername + filename; // ToDo: make nice
    tracer_.Initialize(foldername, filename, column_names, buffersize);
    statsPublisher_.initialize();

    ROS_INFO("Whole Body Controller Initialized");

    return true;

//.........这里部分代码省略.........

Eigen::Isometry3d pose_estimate(FrameMatchPtr match,
                                std::vector<char> & inliers,
                                Eigen::Isometry3d & motion,
                                Eigen::MatrixXd & motion_covariance,
                                Eigen::Matrix<double, 3, 4> & proj_matrix) {
    using namespace pose_estimator;

    PoseEstimator pe(proj_matrix);

    if ((match->featuresA_indices.size()!=match->featuresB_indices.size()))
        cout <<    "Number of features doesn't match\n";

    size_t num_matches = match->featuresA_indices.size();

    if(num_matches < 3)
        cout << "Need at least three matches to estimate pose";

    motion.setIdentity();
    motion_covariance.setIdentity();

    Eigen::Matrix<double, 4, Eigen::Dynamic, Eigen::ColMajor> src_xyzw(4, num_matches);
    Eigen::Matrix<double, 4, Eigen::Dynamic, Eigen::ColMajor>dst_xyzw(4, num_matches);
    for (size_t i=0; i < num_matches; ++i) {

        //    const ImageFeature& featureA(int i) const {
        //      assert (frameA);
        //    int ix = featuresA_indices.at(i);
        //      return frameA->features().at(ix);
        //    }
        //src_xyzw.col(i) = match->featureA(i).xyzw;
        //dst_xyzw.col(i) = match->featureB(i).xyzw;

        int ixA = match->featuresA_indices.at(i);
        int ixB = match->featuresB_indices.at(i);
        //cout << ixA << " | " << ixB << "\n";
        //cout << match->featuresA.size() << " fA size\n";
        //cout <<  match->featuresA[ixA].xyzw[0] << "\n";
        //cout <<  match->featuresA[ixA].xyzw[0] << "\n";

        src_xyzw.col(i) = match->featuresA[ixA].xyzw;
        dst_xyzw.col(i) = match->featuresB[ixB].xyzw;
    }

    // PoseEstimateStatus status = pe.estimate(src_xyzw, dst_xyzw, &inliers, &motion, &motion_covariance);
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> motion_covariance_col_major;

    pose_estimator::PoseEstimateStatus status = pe.estimate(src_xyzw, dst_xyzw, &inliers, &motion, &motion_covariance_col_major); //&motion_covariance);
    motion_covariance = motion_covariance_col_major;

    match->status = status;
    match->delta = motion;

    /*
    int num_inliers = std::accumulate(inliers.begin(), inliers.end(), 0);
    const char* str_status = PoseEstimateStatusStrings[status];
    std::cerr << "Motion: " << str_status << " feats: " << match->featuresA_indices.size()
              << " inliers: " << num_inliers
              << " Pose: " << motion
              << " Delta: " << match->delta
              //<< " Cov:\n" << motion_covariance << "\n"
              << " " << match->timeA << " " << match->timeB << std::endl;
     */

    return motion;
}

void Kinematics::test_ik(){
    using namespace KDL;
    KDL::Chain chain1;
    chain1.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
    chain1.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
    chain1.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
    chain1.addSegment(Segment(Joint(Joint::RotZ)));
    chain1.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
    chain1.addSegment(Segment(Joint(Joint::RotZ)));

    print_chain(chain1);

    //Creation of the solvers:
    ChainFkSolverPos_recursive fksolver1(chain1);//Forward position solver
    ChainIkSolverVel_wdls ik_solver_wdls(chain1);
    Eigen::MatrixXd Mx;
    Mx.resize(6,6);
    Mx.setIdentity();
    Mx(3,3) = 0;Mx(4,4) = 0;Mx(5,5) = 0;
    std::cout<< Mx << std::endl;
    // Mx.Identity();
  //  ik_solver_wdls.setWeightTS(Mx);



    ChainIkSolverPos_NR iksolver1(chain1,fksolver1,ik_solver_wdls,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6

    //Creation of jntarrays:
    JntArray q(chain1.getNrOfJoints());
    JntArray q_init(chain1.getNrOfJoints());

    q_init(0) = 0.1;
    q_init(1) = 0.2;
    q_init(2) = 0.3;
    q_init(3) = 0.2;
    q_init(4) = 0.1;
    q_init(5) = 0.0;

    // Create the frame that will contain the results
    KDL::Frame cartpos;

    // Calculate forward position kinematics
    bool kinematics_status;
    kinematics_status = fksolver1.JntToCart(q_init,cartpos);


    if(kinematics_status>=0){
        std::cout << cartpos <<std::endl;
        printf("%s \n","Succes, thanks KDL!");
    }else{
        printf("%s \n","Error: could not calculate forward kinematics :(");
    }

    Frame current,target;

    fksolver1.JntToCart(q_init,current);

    std::cout<< "current: " << current.p(0) << " " << current.p(1) << " " << current.p(2) << std::endl;
    target = current;
    target.p(0) = target.p(0) + 0.1;
    std::cout<< "target: " << target.p(0) << " " << target.p(1) << " " << target.p(2) << std::endl;


    int ret = iksolver1.CartToJnt(q_init,target,q);

    std::cout<< "ret: " << ret << std::endl;
    std::cout<< "q_init: " << q_init.data << std::endl;
    std::cout<< "q: "      << q.data << std::endl;

}

/**
 * Simulate
 * 
 * @param		double input
 * @return		double u - control signal
 */
double RegulatorGPC::simulate(double input)
{
	if(m_proces)
	{
		m_w = m_proces->simulate();
		const double y = input;

		m_e = m_w - y;
		m_history_Y.push_front(y);
		m_identify.add_sample(input, m_history_U.front());

		//std::deque<double> A,B;
		m_poly_A.clear();
		m_poly_B.clear();

		//A.push_back(-0.6);
		//B.push_back(0.4);

		m_identify.get_polynomial_a(m_poly_A);
		m_identify.get_polynomial_b(m_poly_B);

		// Return disruption
		if (m_initial_steps_left > 0)
		{
			const double u = m_w - y;
			m_history_U.push_front(u);
			m_initial_steps_left--;
			return u;
		}
    
		// Control algorithm
		// ------------------------------------------------------------------------------------------------------
	
		// 1. Calculating h initial conditions equal zero, delay = 0
		// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
		// page 27
		Eigen::VectorXd h(m_H);
		{
			std::map<std::string, double> others;
			others["k"] = 0;
			others["stationary"] = 0;
			others["noise"] = 0;

			ARX ob;
			ob.set_parameters(m_poly_A,m_poly_B,others);
			for(int i=0; i<m_H; i++)
			{
				h[i] = ob.simulate(1.0);
			}
		}

		// 2. Calculating Q:
		// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
		// page 28
		Eigen::MatrixXd Q;
		Q.setZero(m_H, m_L);
		for(int i=0; i<m_H; i++)
		{
			for(int j=0; j<m_L; j++)
			{
				if(i-j<0)
				{
					Q(i,j) = 0;
				}
				else
				{
					Q(i,j) = h[i-j];
				}
			}
		}

		// 3. Calculating w0
		// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
		// page 8
		Eigen::VectorXd w0(m_H);
		w0[0] = (1-m_alpha)*m_w + m_alpha*y;
		for(int i=1; i<m_H; i++)
		{
			w0[i] = (1-m_alpha)*m_w + m_alpha*w0[i-1];
		}
		

		// 4. Calculating q
		// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
		// page 20
		Eigen::VectorXd tmp;
		tmp.setZero(m_L);
		tmp[0] = 1;
		Eigen::MatrixXd mIdentity;
		mIdentity.setIdentity(m_L,m_L);
		Eigen::VectorXd q = (tmp.transpose()
							*((Q.transpose()*Q + m_ro*mIdentity).inverse())
							*Q.transpose()
							).transpose();
//.........这里部分代码省略.........