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

VectorXd MyWindow::evalDeriv() {
    for (unsigned int i = 0; i < mSkels.size(); i++) {
        if (mSkels[i]->getImmobileState()) {
            mSkels[i]->setPose(mDofs[i], true, false);
        } else {
            mSkels[i]->setPose(mDofs[i], false, true);
            mSkels[i]->computeDynamics(mGravity, mDofVels[i], true);
        }
    }
    mConstraintHandle->computeConstraintForces();
    mController->setConstrForces(mConstraintHandle->getTotalConstraintForce(1));

    VectorXd deriv = VectorXd::Zero(mIndices.back() * 2);    
    for (unsigned int i = 0; i < mSkels.size(); i++) {
        if (mSkels[i]->getImmobileState())
            continue;
        int start = mIndices[i] * 2;
        int size = mDofs[i].size();

        VectorXd qddot = mSkels[i]->getInvMassMatrix() * (-mSkels[i]->getCombinedVector() + mSkels[i]->getExternalForces() + mConstraintHandle->getTotalConstraintForce(i) + mSkels[i]->getInternalForces());
        mSkels[i]->clampRotation(mDofs[i], mDofVels[i]);
        deriv.segment(start, size) = mDofVels[i] + (qddot * mTimeStep); // semi-implicit
        deriv.segment(start + size, size) = qddot;
    }
    return deriv;
}

VectorXd MyWindow::evalDeriv() {
    // compute dynamic equations
    for (unsigned int i = 0; i < mSkels.size(); i++) {
        if (mSkels[i]->getImmobileState()) {
            // need to update node transformation for collision
            mSkels[i]->setPose(mDofs[i], true, false);
        } else {
            // need to update first derivatives for collision
            mSkels[i]->setPose(mDofs[i], false, true);
            mSkels[i]->computeDynamics(mGravity, mDofVels[i], true);
        }
    }
    // compute contact forces
    mCollisionHandle->applyContactForces();

    // compute derivatives for integration
    VectorXd deriv = VectorXd::Zero(mIndices.back() * 2);    
    for (unsigned int i = 0; i < mSkels.size(); i++) {
        // skip immobile objects in forward simulation
        if (mSkels[i]->getImmobileState())
            continue;
        int start = mIndices[i] * 2;
        int size = mDofs[i].size();
        VectorXd qddot = mSkels[i]->getInvMassMatrix() * (-mSkels[i]->getCombinedVector() + mSkels[i]->getExternalForces() + mCollisionHandle->getConstraintForce(i));
        mSkels[i]->clampRotation(mDofs[i], mDofVels[i]);
        deriv.segment(start, size) = mDofVels[i] + (qddot * mTimeStep); // set velocities
        deriv.segment(start + size, size) = qddot; // set qddot (accelerations)
    }
    return deriv;
}

MatrixXd
PatchFit::parab (Matrix<double, Dynamic, 3> q, VectorXd a)
{   
  Vector3d k3, rv; 
  k3 << a(0), a(1), 0.0;
  rv << a.segment(2,3);
  
  MatrixXd rr; 
  rr = p_->rexp(rv);
  
  Vector3d c;
  if (ccon_)
    c = plane_c+a(5)*plane_n;
  else
    c = a.segment(5,3);
  MatrixXd qc(q.rows(),q.cols());

  for (int i=0; i<q.rows(); i++)
    qc.row(i) = q.row(i) - c.transpose();
  
  MatrixXd ql(q.rows(),3);
  ql = qc*rr;
  
  MatrixXd ql_sq(q.rows(),3);
  ql_sq = ql.array().square();

  return ((ql_sq * k3) - (2.0 * ql * zh));

  //TBD: enmo
}

Trajectory Trajectory::generatePolynomialTrajectoryThroughViapoint(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_yd_ydd_viapoint, double viapoint_time, const VectorXd& y_to)
{

    int n_dims = y_from.size();
    assert(n_dims==y_to.size());
    assert(3*n_dims==y_yd_ydd_viapoint.size()); // Contains y, yd and ydd, so *3

    int n_time_steps = ts.size();

    int viapoint_time_step = 0;
    while (viapoint_time_step<n_time_steps && ts[viapoint_time_step]<viapoint_time)
        viapoint_time_step++;

    if (viapoint_time_step>=n_time_steps)
    {
        cerr << __FILE__ << ":" << __LINE__ << ":";
        cerr << "ERROR: the time vector does not contain any time smaller than " << viapoint_time << ". Returning min-jerk trajectory WITHOUT viapoint." <<  endl;
        return Trajectory();
    }

    VectorXd yd_from        = VectorXd::Zero(n_dims);
    VectorXd ydd_from       = VectorXd::Zero(n_dims);

    VectorXd y_viapoint     = y_yd_ydd_viapoint.segment(0*n_dims,n_dims);
    VectorXd yd_viapoint    = y_yd_ydd_viapoint.segment(1*n_dims,n_dims);
    VectorXd ydd_viapoint   = y_yd_ydd_viapoint.segment(2*n_dims,n_dims);

    VectorXd yd_to          = VectorXd::Zero(n_dims);
    VectorXd ydd_to         = VectorXd::Zero(n_dims);

    Trajectory traj = Trajectory::generatePolynomialTrajectory(ts.segment(0, viapoint_time_step + 1), y_from, yd_from, ydd_from, y_viapoint, yd_viapoint, ydd_viapoint);
    traj.append(Trajectory::generatePolynomialTrajectory(ts.segment(viapoint_time_step, n_time_steps - viapoint_time_step), y_viapoint, yd_viapoint, ydd_viapoint, y_to, yd_to, ydd_to));

    return traj;
}

void MyWindow::setState(const VectorXd &newState) {
    for (unsigned int i = 0; i < mSkels.size(); i++) {
        int start = mIndices[i] * 2;
        int size = mDofs[i].size();
        mDofs[i] = newState.segment(start, size);
        mDofVels[i] = newState.segment(start + size, size);
    }
}

VectorXd MotionModel::Qprod(VectorXd Q, VectorXd P)
{
	double a, x;
	VectorXd QP(4), QP2;
	Vector3d u, v;
	a = Q(0); x = P(0);
	v = Q.segment(1,3);
	u = P.segment(1,3);
	QP2 = a * u + x * v + v.cross(u); // cross of v and u
	QP << a * x - v.transpose() * u, QP2;
	return QP; // QP should be a 4x1 vector
}

VectorXd vech(MatrixXd& M){
	int n = M.rows();
	VectorXd V;
	V.setZero(n*(n-1)/2+n);
	V.segment(0,n) = M.diagonal();
	int k = n;
	for(int i=0;i<n-1;i++){
		V.segment(k,n-i-1) = M.block(i,i+1,1,n-i-1).transpose();
		k += n-i-1;
	}
	return V;
}

double harmonic_potential(VectorXd x, MatrixXd W) {
	const int N = x.size()/3;

	double hpot = 0.5 * double(x.transpose() * W * x);
	double coul =0;
	for (int n = 0; n != N; n++) {
		for (int m =0; m!=n; m++) {
			// Recall: C[n]*x = x[3n:3(n+1)]
			coul += 1/((x.segment(3*n,3) - x.segment(3*m,3)).norm());
		}
	}
	return hpot + coul;
}

void pubOdometry()
{
    nav_msgs::Odometry odom;
    {
        odom.header.stamp = _last_imu_stamp;
        odom.header.frame_id = "map";

        odom.pose.pose.position.x = x(0);
        odom.pose.pose.position.y = x(1);
        odom.pose.pose.position.z = x(2);

        Quaterniond q;
        q = RPYtoR(x(3), x(4), x(5)).block<3,3>(0, 0);
        odom.pose.pose.orientation.x = q.x();
        odom.pose.pose.orientation.y = q.y();
        odom.pose.pose.orientation.z = q.z();
        odom.pose.pose.orientation.w = q.w();
    }

    //ROS_WARN("[update] publication done");
    ROS_WARN_STREAM("[final] b_g = " << x.segment(_B_G_BEG, _B_G_LEN).transpose());
    ROS_WARN_STREAM("[final] b_a = " << x.segment(_B_A_BEG, _B_A_LEN).transpose() << endl);
    ///ROS_WARN_STREAM("[final] cov_x = " << endl << cov_x << endl);
    odom_pub.publish(odom);
}

void MSCKF::addSlideState()
{
    SlideState newState;
    int nominalStateLength = (int)fullNominalState.size();
    int errorStateLength = (int)fullErrorCovariance.rows();
    
    VectorXd tmpNominal = VectorXd::Zero(nominalStateLength + NOMINAL_POSE_STATE_SIZE);
    MatrixXd tmpCovariance = MatrixXd::Zero(errorStateLength + ERROR_POSE_STATE_SIZE, errorStateLength + ERROR_POSE_STATE_SIZE);
    MatrixXd Jpi = MatrixXd::Zero(9, errorStateLength);
    
    tmpNominal.head(nominalStateLength) = fullNominalState;
    tmpNominal.segment(nominalStateLength, NOMINAL_POSE_STATE_SIZE) = fullNominalState.head(NOMINAL_POSE_STATE_SIZE);
    fullNominalState = tmpNominal;
    
    newState.q = fullNominalState.head(4);
    newState.p = fullNominalState.segment(4, 3);
    newState.v = fullNominalState.segment(7, 3);
    
    slidingWindow.push_back(newState);
    
    Jpi.block<3,3>(0, 0) = Matrix3d::Identity(3, 3);
    Jpi.block<3,3>(3, 3) = Matrix3d::Identity(3, 3);
    Jpi.block<3,3>(6, 6) = Matrix3d::Identity(3, 3);
    tmpCovariance.block(0, 0, errorStateLength, errorStateLength) = fullErrorCovariance;
    tmpCovariance.block(errorStateLength, 0, 9, errorStateLength) = Jpi * fullErrorCovariance;
    tmpCovariance.block(0, errorStateLength, errorStateLength, 9) = fullErrorCovariance * Jpi.transpose();
    tmpCovariance.block<ERROR_POSE_STATE_SIZE, ERROR_POSE_STATE_SIZE>(errorStateLength, errorStateLength) =
        Jpi * fullErrorCovariance * Jpi.transpose();
    
    fullErrorCovariance = tmpCovariance;
}

void nilss_solve(const std::vector<MatrixXd>& R,
                 const std::vector<MatrixXd>& D,
                 const std::vector<VectorXd>& b,
                 const std::vector<VectorXd>& c,
                 std::vector<VectorXd>& a)
{
    int n = R.size();
    assert(D.size() == n + 1);
    assert(b.size() == n);
    assert(c.size() == n + 1);

    std::unique_ptr<MatrixXd> kkt = assemble_kkt(R, D);
    std::unique_ptr<VectorXd> rhs = assemble_rhs(b, c);

    typedef SparseMatrix<double> SpMat;
    SpMat A(kkt->sparseView());

    SparseLU<SparseMatrix<double>> solver;
    solver.analyzePattern(A); 
    solver.factorize(A); 
    VectorXd sol = solver.solve(*rhs); 

    //VectorXd sol = kkt->partialPivLu().solve(*rhs);

    assert(sol.size() % (2 * n + 1) == 0);
    int m = sol.size() / (2 * n + 1);

    a.empty();
    a.reserve(n + 1);
    for (int i = 0; i <= n; ++ i) {
        a.push_back(sol.segment(i * m, m));
    }
}

void ContactDynamics::applySolution() {
    const int c = getNumContacts();

    // First compute the external forces
    VectorXd f_n = mX.head(c);
    VectorXd f_d = mX.segment(c, c * mNumDir);
    VectorXd lambda = mX.tail(c);
    VectorXd forces = mN * f_n;
    forces.noalias() += mB * f_d;

    // Next, apply the external forces skeleton by skeleton.
    int startRow = 0;
    for (int i = 0; i < getNumSkels(); i++) {
        if (mSkels[i]->getImmobileState())
            continue;
        int nDof = mSkels[i]->getNumDofs();
        mConstrForces[i] = forces.segment(startRow, nDof);
        startRow += nDof;
    }

    for (int i = 0; i < c; i++) {
        Contact& contact = mCollisionChecker->getContact(i);
        contact.force.noalias() = getTangentBasisMatrix(contact.point, contact.normal) * f_d.segment(i * mNumDir, mNumDir);
        contact.force += contact.normal * f_n[i];
    }
}

VectorXd PlanarClearance::Gamma()
{
	VectorXd tempVector = VectorXd::Zero(nC);
	tempVector.segment(0, 3) = Gammad3(bda->A0, bdb->A0, bda->omega, bdb->omega);//`````````````````````````
	tempVector.segment(3, 2) = Gammar2(bda->omega, bdb->omega, bdb->A0);
	return tempVector;
}

void Iterative_Control::thread_to_state(const Thread* thread, VectorXd& state)
{
  const int num_pieces = thread->num_pieces();
  state.resize(_size_each_state);
  for (int piece_ind=0; piece_ind < thread->num_pieces(); piece_ind++)
  {
    state.segment(piece_ind*3, 3) = thread->vertex_at_ind(piece_ind);
    if (piece_ind < thread->num_edges()) { 
      state.segment(piece_ind*3 + 3*num_pieces, 3) = thread->edge_at_ind(piece_ind);
    }
  }
  state(6*num_pieces - 3) = thread->end_angle();
  state(6*num_pieces - 2) = ((Thread*)thread)->calculate_energy();
  //state(3*num_pieces) = thread->end_angle();
  //state(3*num_pieces+1) = ((Thread*)thread)->calculate_energy();
}

Eigen::VectorXd ehanning(int len)
{
	VectorXd han(len);
	if (len%2 == 0)
	{
		int half = len/2;
		VectorXd h = ecalc_hanning(half,len);
		han.segment(0,half) = h;
		han.segment(half,len-half) = h.segment(0,half).reverse();
	} else {
		int half = (len+1)/2;
		VectorXd h = ecalc_hanning(half,len);
		han.segment(0,half) = h;
		han.segment(half,len-half) = h.segment(0,half-1).reverse();
	}
    return han;
}