コード例 #1
0
bool Localizer::setIntrinsicsMatrix(const string &type, const Matrix &M)
{
    if (type=="left")
    {
        if (PrjL!=NULL)
        {
            *PrjL=M;
            *invPrjL=pinv(M.transposed()).transposed();
        }
        else
        {
            PrjL=new Matrix(M);
            invPrjL=new Matrix(pinv(M.transposed()).transposed());
        }

        return true;
    }
    else if (type=="right")
    {
        if (PrjR!=NULL)
        {
            *PrjR=M;
            *invPrjR=pinv(M.transposed()).transposed();
        }
        else
        {
            PrjR=new Matrix(M);
            invPrjR=new Matrix(pinv(M.transposed()).transposed());
        }

        return true;
    }
    else
        return false;
}
コード例 #2
0
Vector Controller::computeNeckVelFromdxFP(const Vector &fp, const Vector &dfp)
{
    // convert fp from root to the neck reference frame
    Vector fpR=fp;
    fpR.push_back(1.0);
    Vector fpE=SE3inv(chainNeck->getH())*fpR;

    // compute the Jacobian of the head joints alone 
    // (by adding the new fixation point beforehand)
    Matrix HN=eye(4);
    HN(0,3)=fpE[0];
    HN(1,3)=fpE[1];
    HN(2,3)=fpE[2];

    mutexChain.lock();
    chainNeck->setHN(HN);
    Matrix JN=chainNeck->GeoJacobian();
    chainNeck->setHN(eye(4,4));
    mutexChain.unlock();

    // take only the last three rows of the Jacobian
    // belonging to the head joints
    Matrix JNp=JN.submatrix(3,5,3,5);

    return pinv(JNp)*dfp.subVector(3,5);
}
コード例 #3
0
ファイル: solver.cpp プロジェクト: AbuMussabRaja/icub-main
Vector EyePinvRefGen::getEyesCounterVelocity(const Matrix &eyesJ, const Vector &fp)
{
    // ********** implement VOR
    Matrix H=imu->getH(cat(fbTorso,fbHead.subVector(0,2)));

    H(0,3)=fp[0]-H(0,3);
    H(1,3)=fp[1]-H(1,3);
    H(2,3)=fp[2]-H(2,3);

    // gyro rate [rad/s]
    Vector gyro=CTRL_DEG2RAD*commData->get_imu().subVector(6,8);

    // filter out the noise on the gyro readouts
    if (norm(gyro)<commData->gyro_noise_threshold)
        gyro=0.0;

    Vector vor_fprelv=(gyro[0]*cross(H,0,H,3)+
                       gyro[1]*cross(H,1,H,3)+
                       gyro[2]*cross(H,2,H,3));

    // ********** implement OCR
    H=chainNeck->getH();
    Matrix HN=eye(4,4);
    HN(0,3)=fp[0]-H(0,3);
    HN(1,3)=fp[1]-H(1,3);
    HN(2,3)=fp[2]-H(2,3);

    chainNeck->setHN(HN);
    Vector ocr_fprelv=chainNeck->GeoJacobian()*commData->get_v().subVector(0,2);
    ocr_fprelv=ocr_fprelv.subVector(0,2);
    chainNeck->setHN(eye(4,4));

    // ********** blend the contributions
    return -1.0*(pinv(eyesJ)*(counterRotGain[0]*vor_fprelv+counterRotGain[1]*ocr_fprelv));
}
コード例 #4
0
ファイル: cenv_algs.cpp プロジェクト: joseparnau/iocjac
// Algorithms -------------------------------------------------------------------------------------------------------------------------------------
Eigen::VectorXd controlEnv::mobile_manip_inverse_kinematics_simple(void){
  return pinv(jacobian(mobile_manip_), 0.001)*scaled_pose_error_.v();
  
//   scaled_pose_error_.plot();
//   std::cout << "scaled err:  " << scaled_pose_error_.v().transpose() << std::endl;
//   std::cout << "jac:  "  << std::endl << jacobian(mobile_manip_) << std::endl;
//   std::cout << "pinv jac:  "  << std::endl << pinv(jacobian(mobile_manip_), 0.001) << std::endl;
}
コード例 #5
0
Vector Controller::computeEyesVelFromdxFP(const Vector &dfp)
{
    Matrix eyesJ; Vector tmp;
    if ((fbEyes[2]>CTRL_DEG2RAD*GAZECTRL_CRITICVER_STABILIZATION) &&
        CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,tmp,eyesJ))
        return pinv(eyesJ)*dfp.subVector(0,2);
    else
        return zeros(vEyes.length());
}
コード例 #6
0
ファイル: gmath.cpp プロジェクト: elen4/GURLS
GURLS_EXPORT void pinv(const gMat2D<float>& A, gMat2D<float>& Ainv, float RCOND)
{
    int r, c;
    float* inv = pinv(A.getData(), A.rows(), A.cols(), r, c, &RCOND);

    Ainv.resize(r, c);
    gurls::copy(Ainv.getData(), inv, r*c);

    delete[] inv;
}
コード例 #7
0
/*********************************************************//**
    Allocate and initialize skeleton decomposition 
    with a set of pivots and a given approximation

    \param[in] f      - function to approximate
    \param[in] args   - function arguments
    \param[in] bounds - bounds on function
    \param[in] cargs  - cross2d args
    \param[in] pivx   - x pivots
    \param[in] pivy   - y pivots

    \return skeleton decomposition
*************************************************************/
struct SkeletonDecomp * 
skeleton_decomp_init2d_from_pivots(
    double (*f)(double,double,void *),
    void * args, const struct BoundingBox * bounds,
    const struct Cross2dargs * cargs,
    const double * pivx, const double * pivy)

{

    size_t r = cargs->r;
    struct SkeletonDecomp * skd = skeleton_decomp_alloc(r);

    struct FiberCut ** fx;  
    struct FiberCut ** fy;
    
    double * lb = bounding_box_get_lb(bounds);
    double * ub = bounding_box_get_ub(bounds);

    fx = fiber_cut_2darray(f,args,0,r, pivy);
    quasimatrix_free(skd->xqm);
    skd->xqm = quasimatrix_approx_from_fiber_cuts(
            r, fiber_cut_eval2d, fx, cargs->fclass[0], 
            cargs->sub_type[0],
            lb[0],ub[0], cargs->approx_args[0]);
    fiber_cut_array_free(r, fx);

    fy = fiber_cut_2darray(f,args,1,r, pivx);
    quasimatrix_free(skd->yqm);
    skd->yqm = quasimatrix_approx_from_fiber_cuts(
            r, fiber_cut_eval2d, fy, cargs->fclass[1], 
            cargs->sub_type[1],
            lb[1],ub[1], cargs->approx_args[1]);
    fiber_cut_array_free(r, fy);

    
    size_t ii,jj;
    double * cmat = calloc_double(r*r);
    for (ii = 0; ii <r; ii++){
        for (jj = 0; jj <r; jj++){
            cmat[ii * r + jj] = f(pivx[jj],pivy[ii], args);
        }
    }

    /*
    printf("cmat = ");
    dprint2d_col(r,r,cmat);
    */
    pinv(r,r,r,cmat,skd->skeleton,1e-15);

    free(cmat);cmat=NULL;
    return skd;
}
コード例 #8
0
ファイル: cenv_algs.cpp プロジェクト: joseparnau/iocjac
Eigen::VectorXd controlEnv::manipulator_inverse_kinematics_simple(void){
  
  unsigned int n_manip_joints = mobile_manip_.n_manip_joints();
  unsigned int n_joints = mobile_manip_.dim();
  
  VectorXd manip_joint_inc(n_manip_joints);
  manip_joint_inc = pinv(jacobian(mobile_manip_), 0.001)*scaled_pose_error_.v();
  
  VectorXd joint_inc(n_joints);
  for (unsigned int i=0; i<n_joints-n_manip_joints; i++)	joint_inc(i) = 0.0;
  for (unsigned int i=0; i<n_manip_joints; i++)			joint_inc(i+n_joints-n_manip_joints) = manip_joint_inc(i);
  
  return joint_inc;
}
コード例 #9
0
  /**
   * The update function that actually updates the H matrix. The function takes
   * in all the matrices and only changes the value of the H matrix.
   *
   * @param V Input matrix to be factorized.
   * @param W Basis matrix.
   * @param H Encoding matrix to be updated.
   */
  inline static void Update(const arma::mat& V,
                            const arma::mat& W,
                            arma::mat& H)
  {
    H = pinv(W.t() * W) * W.t() * V;

    // Set all negative numbers to 0.
    for (size_t i = 0; i < H.n_elem; i++)
    {
      if (H(i) < 0.0)
      {
        H(i) = 0.0;
      }
    }
  }
コード例 #10
0
/* Test matrix invertion routine */
void testPinv(void)
{
  double **A,val;
  int nRows,nCols, i,j, n;
  nRows=3;
  nCols=4;

  n = MAX(nRows,nCols);
printf("n=%d\n",n);
  /* Allocate matrix */
  A = (double **) calloc(n,sizeof(double *));
  for (i=0;i<n;i++){
    A[i] = (double *) calloc(n,sizeof(double));
  }

  /* Set up test matrix values */
  val = 1.0;
  for (j=0;j<nCols;j++){
    if (j<nRows) A[j][j] = 2.0;
    for (i=0;i<nRows;i++){
      A[i][j]+=val;
      val += 1.0;
    }
  }

  /* Display matrix */
  for (i=0;i<nRows;i++){
    for (j=0;j<nCols;j++){
      printf(" %16.8f",A[i][j]);
    }
    printf("\n");
  }
  printf("\n");

  /* Invert matrix */
  pinv(A,nRows,nCols);

  /* Display matrix */
  for (i=0;i<nCols;i++){
    for (j=0;j<nRows;j++){
     printf(" %16.8f",A[i][j]);
    }
    printf("\n");
  }

  printf("testPinv halting program\n");
  _EXIT_;
}
コード例 #11
0
  /**
   * The update function that actually updates the W matrix. The function takes
   * in all the matrices and only changes the value of the W matrix.
   *
   * @param V Input matrix to be factorized.
   * @param W Basis matrix to be updated.
   * @param H Encoding matrix.
   */
  inline static void Update(const arma::mat& V,
                            arma::mat& W,
                            const arma::mat& H)
  {
    // The call to inv() sometimes fails; so we are using the psuedoinverse.
    // W = (inv(H * H.t()) * H * V.t()).t();
    W = V * H.t() * pinv(H * H.t());

    // Set all negative numbers to machine epsilon
    for (size_t i = 0; i < W.n_elem; i++)
    {
      if (W(i) < 0.0)
      {
        W(i) = 0.0;
      }
    }
  }
コード例 #12
0
ファイル: fingerDetector.cpp プロジェクト: xufango/contrib_bk
void fingerDetector::run()
{
    //fprintf(stderr, "Entering the main thread\n");

    //fprintf(stderr, "Reading: %s\n", analogs.toString().c_str());
   
    int n = q1.size(); 
    Vector q(n);
    Vector qStar(n);
    Vector dq(n);
    Vector tStar(1);
    Matrix Q1(n,1);

    for (int i = 0; i < n ; i++)
        {
            Q1(i,0) = q1(i);
            q(i) = fingerAnalog(i);
        }
    tStar = pinv(Q1)*(q-q0);
    qStar = q0 + q1 * tStar(0);
    dq = q - qStar;

    double res = 0;
    double q1N = 0;
    for (int i = 0; i < n ; i++)
        {
            res += sqrt(dq(i)*dq(i));
            q1N += sqrt(q1(i)*q1(i));
        }


    if (tStar(0) > maxT && res < max)
        status = (tStar(0) - maxT) * q1N;
    if (tStar(0) > maxT && res > max)
        status = (tStar(0) - maxT) * q1N + (res - max);

    if (tStar(0) < minT && res < max)
        status = (minT - tStar(0)) * q1N;
    if (tStar(0) < minT && res > max)
        status = (minT - tStar(0)) * q1N + (res - max);

    if ( tStar(0) > minT && tStar(0) < maxT && res < max)
        status = 0.0;
    if ( tStar(0) > minT && tStar(0) < maxT && res > max)
        status = res - max;
}  
コード例 #13
0
ファイル: RtDesignMatrix.cpp プロジェクト: cccbauer/murfi2
// get the covariance of a contrast given a contrast vector
// if this matrix is X, this function computes c' pinv(X'X) c
double RtDesignMatrix::computeContrastCovariance(
    vnl_vector<double> &contrastVector) {
  if (contrastVector.size() != columns()) {
    cerr << "ERROR: number of elements in contrast vector does not match the "
         << "number of columns in the design matrix" << endl;
    return std::numeric_limits<double>::quiet_NaN();
  }

  // compute the contrast covariance based on the currently known regressors
  // NOTE: this will not be the same as computing it at the end of the
  // experiment when all regressors are known. it would be nice to compute
  // final values using the known design.
  vnl_matrix<double> convec(contrastVector.data_block(),
                            contrastVector.size(), 1);
  vnl_svd<double> pinv(transpose() * (*this));
  vnl_matrix<double> result = convec.transpose() * pinv.pinverse() * convec;
  return result.get(0, 0);
}
コード例 #14
0
double * solve(size_t nfield, double * perm, double h, double lb, double ub)
{
    double * op = buildOP(nfield,perm,h);
    double * rhs = buildRHS(nfield-1,lb+h,ub);
    
    double * inv = calloc_double((nfield-1) * (nfield-1));
    pinv(nfield-1,nfield-1,nfield-1,op,inv,0.0);
    
    double * sol = calloc_double(nfield);
    cblas_dgemv(CblasColMajor,CblasNoTrans,nfield-1,nfield-1,1.0,inv,nfield-1,
                rhs,1,0.0,sol+1,1);

    free(op); op = NULL;
    free(rhs); rhs = NULL;
    free(inv); inv = NULL;

    return sol;
}
コード例 #15
0
ファイル: robot.cpp プロジェクト: stephentu/cs184-ikrobot
vec LinkedTreeRobot::computeDeltaThetas(const vec& desiredPositions, vec& axes) const {

  assert(_numEffectors * 3 == desiredPositions.n_elem);

  vec s(3 * _numEffectors);
  getEffectorPositions(s);

  vec e = clamp(desiredPositions - s, 0.5);

  mat J;
  computeJacobian(desiredPositions, J, axes);

  if (_method == PINV) { 
    //cout << "PINV" << endl;
    mat pInvJ = pinv(J); // magic. uses SVD
    vec soln = pInvJ * e;
    assert(soln.n_elem == _numJoints);
    return soln;
  } else if (_method == DLS) {
    //cout << "DLS" << endl;
    // delta theta = J^T(JJ^T + lambda^2I)^-1 * e
    
    mat JT = trans(J); 
    mat JJT = J * JT;

    mat I(J.n_rows, J.n_rows); 
    I.eye();

    vec soln = (JT * inv(JJT + lambda_squared * I)) * e;
    assert(soln.n_elem == _numJoints);
    return soln;

  } else
    throw runtime_error("invalid method");

}
コード例 #16
0
bool Localizer::triangulatePoint(const Vector &pxl, const Vector &pxr, Vector &x)
{
    if ((pxl.length()<2) || (pxr.length()<2))
    {
        yError("Not enough values given for the pixels!");
        return false;
    }

    if (PrjL && PrjR)
    {
        Vector torso=commData->get_torso();
        Vector head=commData->get_q();

        Vector qL(8);
        qL[0]=torso[0];
        qL[1]=torso[1];
        qL[2]=torso[2];
        qL[3]=head[0];
        qL[4]=head[1];
        qL[5]=head[2];
        qL[6]=head[3];
        qL[7]=head[4]+head[5]/2.0;

        Vector qR=qL;
        qR[7]-=head[5];

        mutex.lock();
        Matrix HL=SE3inv(eyeL->getH(qL));
        Matrix HR=SE3inv(eyeR->getH(qR));
        mutex.unlock();

        Matrix tmp=zeros(3,4); tmp(2,2)=1.0;
        tmp(0,2)=pxl[0]; tmp(1,2)=pxl[1];
        Matrix AL=(*PrjL-tmp)*HL;

        tmp(0,2)=pxr[0]; tmp(1,2)=pxr[1];
        Matrix AR=(*PrjR-tmp)*HR;

        Matrix A(4,3);
        Vector b(4);
        for (int i=0; i<2; i++)
        {
            b[i]=-AL(i,3);
            b[i+2]=-AR(i,3);

            for (int j=0; j<3; j++)
            {
                A(i,j)=AL(i,j);
                A(i+2,j)=AR(i,j);
            }
        }

        // solve the least-squares problem
        x=pinv(A)*b;

        return true;
    }
    else
    {
        yError("Unspecified projection matrix for at least one camera!");
        return false;
    }
}
コード例 #17
0
ファイル: utils.cpp プロジェクト: willard-yuan/bag-of-words
/******************************************************************
 * 函数功能:几何校正
 * 
 * 待写:H_final的值也应该返回去
 */
arma::uvec geometricVerification(const arma::mat &frames1, const arma::mat &frames2, 
	const arma::mat &matches, const superluOpts &opts){
	// 测试载入是否准确
    /*std::cout<< "element测试: " << " x: " << frames1(0,1) << " y: " << frames1(1,1) << std::endl;
    std::cout << " 行数: " << frames1.n_rows << " 列数:" << frames1.n_cols << std::endl;
    std::cout << "==========================================================" << std::endl;*/

	int numMatches = matches.n_cols;
	// 测试匹配数目是否准确
    /*std::cout << "没有RANSAC前匹配数目: " << numMatches << std::endl;
    std::cout << "==========================================================" << std::endl;*/

	arma::field<arma::uvec> inliers(1, numMatches);
	arma::field<arma::mat> H(1, numMatches);

	arma::uvec v = arma::linspace<arma::uvec>(0,1,2);
    arma::mat onesVector = arma::ones(1, matches.n_cols);
	arma::uvec matchedIndex_Query = arma::conv_to<arma::uvec>::from(matches.row(0)-1);
	arma::uvec matchedIndex_Object = arma::conv_to<arma::uvec>::from(matches.row(1)-1);

	arma::mat x1 = frames1(v, matchedIndex_Query) ;
	arma::mat x2 = frames2(v, matchedIndex_Object);
    /*std::cout << " x1查询图像匹配行数: " << x1.n_rows << " 查询图像匹配列数:" << x1.n_cols << std::endl;
    std::cout << " x2目标图像匹配行数: " << x2.n_rows << " 目标图像匹配列数:" << x2.n_cols << std::endl;
    std::cout<< "x1 element测试: " << " x: " << x1(0,1) << " y: " << x1(1,1) << std::endl;
    std::cout<< "x2 element测试: " << " x: " << x2(0,1) << " y: " << x2(1,1) << std::endl;
    std::cout << "==========================================================" << std::endl;*/

	arma::mat x1hom = arma::join_cols(x1, arma::ones<arma::mat>(1, numMatches));  //在下面添加一行,注意和join_rows的区别
	arma::mat x2hom = arma::join_cols(x2, arma::ones<arma::mat>(1, numMatches));
    /*std::cout << " x1hom查询图像匹配行数: " << x1hom.n_rows << " 查询图像匹配列数:" << x1hom.n_cols << std::endl;
    std::cout<< "x1hom element测试: " << " x: " << x1hom(0,1) << " y: " << x1hom(1,1) << " z: " << x1hom(2,1) << std::endl;
    std::cout << "==========================================================" << std::endl;*/

	arma::mat x1p, H21;  //作用域
	double tol;
	for(int m = 0; m < numMatches; ++m){
		//cout << "m: " << m << endl;
		for(unsigned int t = 0; t < opts.numRefinementIterations; ++t){
			//cout << "t: " << t << endl;
			if (t == 0){
				arma::mat tmp1 = frames1.col(matches(0, m)-1);
				arma::mat A1 = toAffinity(tmp1);
				//A1.print("A1 =");
				arma::mat tmp2 = frames2.col(matches(1, m)-1);
				arma::mat A2 = toAffinity(tmp2);
				//A2.print("A2 =");
				H21 = A2 * inv(A1);
				//H21.print("H21 =");
				x1p = H21.rows(0, 1) * x1hom ;
				//x1p.print("x1p =");
				tol = opts.tolerance1;
			}else if(t !=0 && t <= 3){
				arma::mat A1 = x1hom.cols(inliers(0, m));
				arma::mat A2 = x2.cols(inliers(0, m));
				//A1.print("A1 =");
				//A2.print("A2 =");
		        H21 = A2*pinv(A1);
				//H21.print("H21 =");
				x1p = H21.rows(0, 1) * x1hom ;
				//x1p.print("x1p =");
				arma::mat v;
				v << 0 << 0 << 1 << arma::endr;
				H21 = join_vert(H21, v);
				//H21.print("H21 =");
				//x1p.print("x1p =");
				tol = opts.tolerance2;
			}else{
				arma::mat x1in = x1hom.cols(inliers(0, m));
				arma::mat x2in = x2hom.cols(inliers(0, m));
				arma::mat S1 = centering(x1in);
				arma::mat S2 = centering(x2in);
				arma::mat x1c = S1 * x1in;
				//x1c.print("x1c =");
				arma::mat x2c = S2 * x2in;
				//x2c.print("x2c =");
				arma::mat A1 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
				A1.zeros();
				arma::mat A2 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
				A2.zeros();
				arma::mat A3 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
				A3.zeros();
				for(unsigned int i = 0; i < x1c.n_cols; ++i){
					A2.col(i) = x1c.col(i)*(-x2c.row(0).col(i));
					A3.col(i) = x1c.col(i)*(-x2c.row(1).col(i));
				}
				arma::mat T1 = join_cols(join_horiz(x1c, A1), join_horiz(A1, x1c));
				arma::mat T2 = join_cols(T1, join_horiz(A2, A3));
				//T2.print("T2 =");
				arma::mat U;
				arma::vec s;
				arma::mat V;
				svd_econ(U, s, V, T2);
				//U.print("U =");
				//V.print("V =");
				arma::vec tmm = U.col(U.n_cols-1);
				H21 = reshape(tmm, 3, 3).t();
				H21 = inv(S2) * H21 * S1;
				H21 = H21 / H21(H21.n_rows-1, H21.n_cols-1) ;
				//H21.print("H21 =");
				arma::mat x1phom = H21 * x1hom ;
				arma::mat cc1 = x1phom.row(0) / x1phom.row(2);
				arma::mat cc2 = x1phom.row(1) / x1phom.row(2);
				arma::mat x1p = join_cols(cc1, cc2);
				//x1p.print("x1p =");
				tol = opts.tolerance3;
			}
			arma::mat tmp = arma::square(x2 - x1p); //精度跟matlab相比更高?
			//tmp.print("tmp =");
			arma::mat dist2 = tmp.row(0) + tmp.row(1);
			//dist2.print("dist2 =");
			inliers(0, m) = arma::find(dist2 < pow(tol, 2));
			H(0, m) = H21;
			//H(0, m).print("H(0, m) =");
			//inliers(0, m).print("inliers(0, m) =");
			//cout << inliers(0, m).size() << endl;
			//cout << "==========================================================" << endl;
			if (inliers(0, m).size() < opts.minInliers) break;
			if (inliers(0, m).size() > 0.7 * numMatches) break;
		}
	}
	arma::uvec scores(numMatches);
	for(int i = 0; i < numMatches; ++i){
		scores.at(i) = inliers(0, i).n_rows;
	}
	//scores.print("scores = ");
	arma::uword index;
	scores.max(index);
	//cout << index << endl;
	arma::mat H_final = inv(H(0, index));
	//H_final.print("H_final = ");
	arma::uvec inliers_final = inliers(0, index);
	//inliers_final.print("inliers_final = ");
	return inliers_final;
}
コード例 #18
0
ファイル: pinv.c プロジェクト: qiuyesuifeng/gpdb
Datum
pseudoinverse(PG_FUNCTION_ARGS)
{
    /*
     * A note on types: PostgreSQL array dimensions are of type int. See, e.g.,
     * the macro definition ARR_DIMS
     */
    int         rows, columns;
    float8     *A, *Aplus;
    ArrayType  *A_PG, *Aplus_PG;
    int lbs[2], dims[2];

    /*
     * Perform all the error checking needed to ensure that no one is
     * trying to call this in some sort of crazy way.
     */
    if (PG_NARGS() != 1)
    {
        ereport(ERROR,
                (errcode(ERRCODE_INVALID_PARAMETER_VALUE),
                 errmsg("pseudoinverse called with %d arguments",
                        PG_NARGS())));
    }
    if (PG_ARGISNULL(0))
        PG_RETURN_NULL();

    A_PG = PG_GETARG_ARRAYTYPE_P(0);

    if (ARR_ELEMTYPE(A_PG) != FLOAT8OID)
        ereport(ERROR,
                (errcode(ERRCODE_INVALID_PARAMETER_VALUE),
                 errmsg("pseudoinverse only defined over float8[]")));
    if (ARR_NDIM(A_PG) != 2)
        ereport(ERROR,
                (errcode(ERRCODE_INVALID_PARAMETER_VALUE),
                 errmsg("pseudoinverse only defined over 2 dimensional arrays"))
               );
    if (ARR_NULLBITMAP(A_PG))
        ereport(ERROR,
                (errcode(ERRCODE_NULL_VALUE_NOT_ALLOWED),
                 errmsg("null array element not allowed in this context")));

    /* Extract rows, columns, and data */
    rows = ARR_DIMS(A_PG)[0];
    columns = ARR_DIMS(A_PG)[1];
    A = (float8 *) ARR_DATA_PTR(A_PG);


    /*  Allocate a PG array for the result, "Aplus" is A+, the pseudo inverse of A */
    lbs[0] = 1;
    lbs[1] = 1;
    dims[0] = columns;
    dims[1] = rows;
    Aplus_PG = construct_md_array(NULL, NULL, 2, dims, lbs, FLOAT8OID,
                                  sizeof(float8), true, 'd');

    Aplus = (float8 *) ARR_DATA_PTR(Aplus_PG);

    pinv(rows,columns,A,Aplus);

    PG_RETURN_ARRAYTYPE_P(Aplus_PG);
}
コード例 #19
0
ファイル: regress.c プロジェクト: BenjaminYu/gpdb
/*----------
 * Do the computations requested from final functions.
 *
 * Compute regression coefficients, coefficient of determination (R^2),
 * t-statistics, and p-values whenever the respective argument is non-NULL.
 * Since these functions share a lot of computation, they have been distilled
 * into this function.
 *
 * First, we compute the regression coefficients as:
 *   
 *   c = (X^T X)^+ * X^T * y = X^+ * y
 *
 * where:
 * 
 *   X^T = the transpose of X
 *   X^+ = the pseudo-inverse of X
 *
 * The identity X^+ = (X^T X)^+ X^T holds for all matrices X, a proof
 * can be found here:
 * http://en.wikipedia.org/wiki/Proofs_involving_the_Moore%2DPenrose_pseudoinverse
 *
 * Note that when the system X c = y is satisfiable (because (X|c) has rank at
 * most inState->len), then setting c = X^+ y means that |c|_2 <= |d|_2 for all
 * solutions d satisfying X d = y.
 * (See http://en.wikipedia.org/wiki/Moore%2DPenrose_pseudoinverse)
 *
 * Caveat: Explicitly computing (X^T X)^+ can become a significant source of
 * numerical rounding erros (see, e.g., 
 * http://en.wikipedia.org/wiki/Moore%2DPenrose_pseudoinverse#Construction
 * or http://www.mathworks.com/moler/leastsquares.pdf p.16).
 *----------
 */
static void
float8_mregr_compute(MRegrState	*inState,
					 ArrayType	**outCoef,
					 float8		*outR2,
					 ArrayType	**outTStats,
					 ArrayType	**outPValues)
{
	ArrayType   *coef_array, *stdErr_array, *tStats_array = NULL, *pValues_array = NULL;
	float8      ess = 0., /* explained sum of squares (regression sum of squares) */
	            tss = 0., /* total sum of squares */
	            rss, /* residual sum of squares */
	            r2,
	            variance;
	float8      *XtX_inv, *coef, *stdErr, *tStats = NULL, *pValues = NULL;
	uint32      i;
	
	/*
	 * Precondition: inState->len * inState->len * sizeof(float8) < STATE_LEN(inState->len)
	 *           and IS_FEASIBLE_STATE_LEN(STATE_LEN(inState->len))
	 */
	XtX_inv = palloc((uint64) inState->len * inState->len * sizeof(float8));
	pinv(inState->len, inState->len, inState->XtX, XtX_inv);
	
	/*
	 * FIXME: Decide on whether we want to display an info message or rather
	 * provide a second function that tells how well the data is conditioned.
	 *
	 * Check if we should expect multicollineratiy. [MPP-13582]
	 *
	 * See:
	 * Lichtblau, Daniel and Weisstein, Eric W. "Condition Number."
	 * From MathWorld--A Wolfram Web Resource.
	 * http://mathworld.wolfram.com/ConditionNumber.html
	 *
	if (condNoOfXtX > 1000)
		ereport(INFO, 
		        (errmsg("matrix X^T X is ill-conditioned"),
		         errdetail("condition number = %f", condNoOfXtX),
		         errhint("This can indicate strong multicollinearity.")));
	 */
	
	/*
	 * We want to return a one-dimensional array (as opposed to a
	 * two-dimensional array).
	 *
	 * Note: Calling construct_array with NULL as first arguments is a Greenplum
	 * extension
	 */
	coef_array = construct_array(NULL, inState->len,
								 FLOAT8OID, sizeof(float8), true, 'd');
	coef = (float8 *) ARR_DATA_PTR(coef_array);
	symmetricMatrixTimesVector(inState->len, XtX_inv, inState->Xty, coef);

	if (outCoef)
		*outCoef = coef_array;
	
	if (outR2 || outTStats || outPValues)
	{	
		/*----------
		 * Computing the total sum of squares (tss) and the explained sum of squares (ess)
		 *
		 *   ess = y'X * c - sum(y)^2/n
		 *   tss = sum(y^2) - sum(y)^2/n
		 *   R^2 = ess/tss
		 *----------
		 */
		ess = dotProduct(inState->len, inState->Xty, coef)
			  - inState->sumy*inState->sumy/inState->count;
		tss = inState->sumy2 - inState->sumy * inState->sumy / inState->count;
		
		/*
		 * With infinite precision, the following checks are pointless. But due to
		 * floating-point arithmetic, this need not hold at this point.
		 * Without a formal proof convincing us of the contrary, we should
		 * anticipate that numerical peculiarities might occur.
		 */
		if (tss < 0)
			tss = 0;
		if (ess < 0)
			ess = 0;
		/*
		 * Since we know tss with greater accuracy than ess, we do the following
		 * sanity adjustment to ess:
		 */
		if (ess > tss)
			ess = tss;
	}
	
	if (outR2)
	{
		/*
		 * coefficient of determination
		 * If tss == 0, then the regression perfectly fits the data, so the
		 * coefficient of determination is 1.
		 */
		r2 = (tss == 0 ? 1 : ess / tss);
		*outR2 = r2;
	}
	
	if (outTStats || outPValues)
	{
		stdErr_array = construct_array(NULL, inState->len,
		                               FLOAT8OID, sizeof(float8), true, 'd');
		stdErr = (float8 *) ARR_DATA_PTR(stdErr_array);
		tStats_array = construct_array(NULL, inState->len,
		                               FLOAT8OID, sizeof(float8), true, 'd');
		tStats = (float8 *) ARR_DATA_PTR(tStats_array);
		pValues_array = construct_array(NULL, inState->len,
		                                FLOAT8OID, sizeof(float8), true, 'd');
		pValues = (float8 *) ARR_DATA_PTR(pValues_array);

		/*
		 * Computing t-statistics and p-values
		 *
		 * Total sum of squares (tss) = Residual Sum of sqaures (rss) +
		 * Explained Sum of Squares (ess) for linear regression.
		 * Proof: http://en.wikipedia.org/wiki/Sum_of_squares
		 */
		rss = tss - ess;
		
		/* Variance is also called the Mean Square Error */
		variance = rss / (inState->count - inState->len);
		
		/*
		 * The t-statistic for each coef[i] is coef[i] / stdErr[i]
		 * where stdErr[i] is the standard error of coef[ii], i.e.,
		 * the square root of the i'th diagonoal element of
		 * variance * (X^T X)^{-1}.
		 */
		for (i = 0; i < inState->len; i++) {
			/*
			 * In an abundance of caution, we see a tiny possibility that numerical
			 * instabilities in the pinv operation can lead to negative values on
			 * the main diagonal of even a SPD matrix
			 */
			if (XtX_inv[i * (inState->len + 1)] < 0) {
				stdErr[i] = 0;
			} else {
				stdErr[i] = sqrt( variance * XtX_inv[i * (inState->len + 1)] );
			}
			
			if (coef[i] == 0 && stdErr[i] == 0) {
				/*
				 * In this special case, 0/0 should be interpreted as 0:
				 * We know that 0 is the exact value for the coefficient, so
				 * the t-value should be 0 (corresponding to a p-value of 1)
				 */
				tStats[i] = 0;
			} else {
				/*
				 * If stdErr[i] == 0 then abs(tStats[i]) will be infinity, which is
				 * what we need.
				 */
				tStats[i] = coef[i] / stdErr[i];
			}
		}
	}
	
	if (outTStats)
		*outTStats = tStats_array;
	
	if (outPValues) {
		for (i = 0; i < inState->len; i++)
			if (inState->count <= inState->len) {
				/*
				 * This test is purely for detecting long int overflows because
				 * studentT_cdf expects an unsigned long int as first argument.
				 */
				pValues[i] = NAN;
			} else {
				pValues[i] = 2. * (1. - studentT_cdf(
						(uint64) (inState->count - inState->len), fabs( tStats[i] )
					));
			}
		
		*outPValues = pValues_array;
	}
}
コード例 #20
0
Eigen::MatrixXd cinv(const Eigen::MatrixXd M, const Eigen::VectorXi TS, const Eigen::VectorXd H, const double tol){

    // ******************************************************************************************************
    // Function that implements the continuous inverse as in:
    //   Mansard, N., et al.; A Unified Approach to Integrate Unilateral Constraints in the Stack of Tasks
    //   IEEE, Transactions on Robotics, 2009
    //
    // INPUT
    //  - M: matrix from which we want to compute the continuous inverse
    //  - TS: task size vector with 'ts(i)' being the size of task 'i'. The sum of the elements of 'TS' == number of rows of 'M'.
    //  - H: task activation vector whose elements \in [0,1]
    //
    // OUTPUT
    //  Given B(m) the set of all permutations without repetitions of the first 'm' elements,
    //  that is, B(m==3) = {{1},{2},{3},{1,2},{1,3},{2,3},{1,2,3}} then:
    //  - cinv = sum_{P \in B(m)}( prod_{i \in P}(h_i) * prod_{i !\in P}(1.0 - h_i) * pinv(M_P) )
    //    with:
    //     - m = TS.size()
    //     - M_P = H0_P * M and H0_P as the diagonal matrix with HO_P(j,j) = 1 if j \in P, otherwise HO_P(j,j) = 0.

//    std::cout << "M" << std::endl << M << std::endl;
//    std::cout << "TS: " << TS.transpose() << std::endl;
//    std::cout << "H: " << H.transpose() << std::endl;

    unsigned int m_rows = M.rows();
    unsigned int m_cols = M.cols();

    // Gather task data
    unsigned int n_tasks = TS.size();
    std::vector< std::pair<unsigned int, unsigned int> > task_rows;
    std::vector<Eigen::MatrixXd> task_M;
    for (unsigned int i=0, curr_row=0; i<n_tasks; ++i){
        task_rows.push_back( std::pair<unsigned int, unsigned int>(curr_row, curr_row+static_cast<unsigned int>(TS(i))) );

        task_M.push_back(M.block(curr_row,0,TS(i),m_cols));

        curr_row += static_cast<unsigned int>(TS(i));
    }
//    // Plot data by task
//    std::cout << "curr rows" << std::endl;
//    for (auto i=0; i<n_tasks; ++i){
//        std::cout << task_rows[i].first << " " << task_rows[i].second << " - " << H[i] << std::endl;
//        std::cout << "task_M_pinv[i]:" << std::endl << task_M[i] << std::endl;
//    }


    // Get number of active tasks
    std::vector<unsigned int> active_tasks;
    for (unsigned int i=0; i<n_tasks; ++i)  if (H[i])   active_tasks.push_back(i);
    unsigned int n_active_tasks = active_tasks.size();
//    std::cout << "active_tasks: ";    for (auto i=0; i<active_tasks.size(); ++i)    std::cout << active_tasks[i] << " ";     std::cout << std::endl;
  
    Eigen::MatrixXd cinv(Eigen::MatrixXd::Zero(m_cols,m_rows));
    for (unsigned int n_permutation_elem = 1; n_permutation_elem <= n_active_tasks; ++n_permutation_elem){

        std::vector<bool> active_tasks_in_permutation(n_active_tasks);
        std::fill(active_tasks_in_permutation.begin(), active_tasks_in_permutation.begin() + n_permutation_elem, true);

        do {
            Eigen::MatrixXd subset_task_m(Eigen::MatrixXd::Zero(m_rows,m_cols));
            double h_subset_task_activation_prod = 1.0;
            for (unsigned int j_task = 0; j_task < n_active_tasks; ++j_task) {
                if (active_tasks_in_permutation[j_task]) {
                    subset_task_m.block(task_rows[active_tasks[j_task]].first,0,TS(active_tasks[j_task]),m_cols) = task_M[active_tasks[j_task]];
                    h_subset_task_activation_prod *= H(active_tasks[j_task]);
                }
                else
                    h_subset_task_activation_prod *= 1.0 - H(active_tasks[j_task]);
            }
//            std::cout << "permutation: "; for (auto k=0; k<active_tasks_in_permutation.size(); ++k) std::cout<< active_tasks_in_permutation[k] << " ";    std::cout << std::endl;
//            std::cout << "h_subset_task_activation_prod: " << h_subset_task_activation_prod << std::endl;
//            std::cout << "subset_task_m" << std::endl << subset_task_m << std::endl;

            cinv = cinv + h_subset_task_activation_prod * pinv(subset_task_m,tol);

        } while (std::prev_permutation(active_tasks_in_permutation.begin(), active_tasks_in_permutation.end()));
    }
  
    return cinv;
}
コード例 #21
0
int main(void){
  
  std::cout  << " ---- Programa de Tests de la pseudoinverse de Mansard ----" << std::endl;

  unsigned int nq = 10;  
  unsigned int nx1 = 7;

  Eigen::MatrixXd H1(Eigen::MatrixXd::Zero(nx1,nx1));
  H1(0,0) = 0;
  H1(1,1) = 0.2;
  H1(2,2) = 0;  
  H1(3,3) = 0.5;
  H1(4,4) = 0;
  H1(5,5) = 0;
  H1(6,6) = 0.7;
  
  std::vector<unsigned int> active_joints;
  for (unsigned int i=0; i<nx1; ++i)  if (H1(i,i))    active_joints.push_back(i);
  unsigned int n_active_joints = active_joints.size();
  std::cout << "n_active_joints: " << n_active_joints << std::endl;
  
  // Initialize pseudoinverse to zero
  Eigen::MatrixXd pJ1(Eigen::MatrixXd::Zero(nq,nx1));
  
//  // Per cada grup de 'i' elements
//  for (unsigned int i=1; i<=n_active_joints; ++i){
  
//    std::vector<bool> v(n_active_joints);
//    std::fill(v.begin(), v.begin() + i, true);

//    do {

//      Eigen::MatrixXd J_sum(Eigen::MatrixXd::Zero(nx1,nq));
      
//      double h_prod = 1.0;
      
//      for (int j = 0; j < n_active_joints; ++j) {
//	unsigned int qi = active_joints[j];
	
//	if (v[j]) {
//// 	  std::cout << j << " ";
//	  std::cout << qi << " ";
	  
// 	  J_sum(qi,qi+3) = 1.0;
//	  h_prod *= H1(qi,qi);
//	}
//	else	h_prod *= 1.0 - H1(qi,qi);
//      }
//      std::cout << "\n";
////       std::cout << "  -  P h: " << h_prod << "\n";
      
//      std::cout << " J_sum: " << "\n" << J_sum << "\n";

//      Eigen::MatrixXd pJ_sum(pinv(J_sum,0.00001));
//      std::cout << " h_prod * pJ_sum: " << "\n" << h_prod * pJ_sum << "\n";
      
//      pJ1 = pJ1 + h_prod * pJ_sum;
//      std::cout << " pJ1: " << "\n" << pJ1 << "\n";
      
//    } while (std::prev_permutation(v.begin(), v.end()));
    
//     std::cout << "-----\n";
//  }
//  std::cout << " pJ1: " << "\n" << pJ1 << "\n";
  
  
 // Per cada grup de 'i' elements
  pJ1 = Eigen::MatrixXd::Zero(nq,nx1);
  for (unsigned int i=1; i<=n_active_joints; ++i){
  
    std::vector<bool> v(n_active_joints);
    std::fill(v.begin(), v.begin() + i, true);

    do {
      Eigen::MatrixXd J_sum(Eigen::MatrixXd::Zero(nx1,nq));
      double h_prod = 1.0;
      for (unsigned int j = 0; j < n_active_joints; ++j) {
    unsigned int qi = active_joints[j];
	
    if (v[j]) {
      J_sum(qi,qi+3) = 1.0;
      h_prod *= H1(qi,qi);
    }
    else
      h_prod *= 1.0 - H1(qi,qi);
      }
      pJ1 = pJ1 + h_prod * pinv(J_sum,0.00001);
      
    } while (std::prev_permutation(v.begin(), v.end()));

  }
  std::cout << " pJ1: " << "\n" << pJ1 << "\n";
  


//    Eigen::MatrixXd m_id(Eigen::MatrixXd::Identity(6,6));
//    m_id(1,1) = 0.2;
//    m_id(4,4) = -3;

//    Eigen::VectorXi TS(3);
//    TS(0) = 2;
//    TS(1) = 3;
//    TS(2) = 1;

//    Eigen::VectorXd H(3);
//    H(0) = 0.1;
//    H(1) = 0.6;
//    H(2) = 0.4;

//    std::cout << " cinv: " << "\n" << cinv(m_id,TS,H) << "\n";

    double tol = 0.0000001;

    Eigen::MatrixXd m_id(Eigen::MatrixXd::Identity(nx1,nx1));

    Eigen::VectorXi TS(nx1);
    for (unsigned int i=0; i<nx1; ++i)  TS(i) = 1.0;

    Eigen::VectorXd H(nx1);
    H(0) = 0;
    H(1) = 0.2;
    H(2) = 0;
    H(3) = 0.5;
    H(4) = 0;
    H(5) = 0;
    H(6) = 0.7;

    std::cout << "cinv: " << "\n" << cinv(m_id,TS,H) << "\n";

    Eigen::MatrixXd lH(nx1,nx1);
    lH(0,0) = 0;
    lH(1,1) = 0.2;
    lH(2,2) = 0;
    lH(3,3) = 0.5;
    lH(4,4) = 0;
    lH(5,5) = 0;
    lH(6,6) = 0.7;

    std::cout << "cinv: " << "\n" << cinv(m_id,TS,lH) << "\n";
    std::cout << "left cinv: " << "\n" << left_cinv(m_id,TS,lH) << "\n";
    std::cout << "right cinv: " << "\n" << right_cinv(m_id,TS,lH) << "\n";

    return 0;
}
コード例 #22
0
Localizer::Localizer(exchangeData *_commData, const unsigned int _period) :
                     RateThread(_period), commData(_commData), period(_period)
{
    iCubHeadCenter eyeC(commData->head_version>1.0?"right_v2":"right");
    eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
    eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");

    // remove constraints on the links
    // we use the chains for logging purpose
    eyeL->setAllConstraints(false);
    eyeR->setAllConstraints(false);

    // release links
    eyeL->releaseLink(0); eyeC.releaseLink(0); eyeR->releaseLink(0);
    eyeL->releaseLink(1); eyeC.releaseLink(1); eyeR->releaseLink(1);
    eyeL->releaseLink(2); eyeC.releaseLink(2); eyeR->releaseLink(2);

    // add aligning matrices read from configuration file
    getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
    getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);

    // overwrite aligning matrices iff specified through tweak values
    if (commData->tweakOverwrite)
    {
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
    }

    // get the absolute reference frame of the head
    Vector q(eyeC.getDOF(),0.0);
    eyeCAbsFrame=eyeC.getH(q);
    // ... and its inverse
    invEyeCAbsFrame=SE3inv(eyeCAbsFrame);

    // get the length of the half of the eyes baseline
    eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));

    bool ret;

    // get camera projection matrix
    ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_LEFT",&PrjL,true);
    if (commData->tweakOverwrite)
    {
        Matrix *Prj;
        if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_LEFT",&Prj,true))
        {
            delete PrjL;
            PrjL=Prj;
        }
    }

    if (ret)
    {
        cxl=(*PrjL)(0,2);
        cyl=(*PrjL)(1,2);
        invPrjL=new Matrix(pinv(PrjL->transposed()).transposed());
    }
    else
        PrjL=invPrjL=NULL;

    // get camera projection matrix
    ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_RIGHT",&PrjR,true);
    if (commData->tweakOverwrite)
    {
        Matrix *Prj;
        if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_RIGHT",&Prj,true))
        {
            delete PrjR;
            PrjR=Prj;
        }
    }

    if (ret)
    {
        cxr=(*PrjR)(0,2);
        cyr=(*PrjR)(1,2);
        invPrjR=new Matrix(pinv(PrjR->transposed()).transposed());
    }
    else
        PrjR=invPrjR=NULL;

    Vector Kp(1,0.001), Ki(1,0.001), Kd(1,0.0);
    Vector Wp(1,1.0),   Wi(1,1.0),   Wd(1,1.0);
    Vector N(1,10.0),   Tt(1,1.0);
    Matrix satLim(1,2);

    satLim(0,0)=0.05;
    satLim(0,1)=10.0;

    pid=new parallelPID(0.05,Kp,Ki,Kd,Wp,Wi,Wd,N,Tt,satLim);

    Vector z0(1,0.5);
    pid->reset(z0);
    dominantEye="left";

    port_xd=NULL;
}
コード例 #23
0
ファイル: cenv_algs.cpp プロジェクト: joseparnau/iocjac
Eigen::VectorXd controlEnv::mobile_manip_inverse_kinematics_siciliano(void){
  // Siciliano; modelling, planning and control; pag 139
  // --------------------------------------------------------------------------------
  
  
//   std::cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA:  " << n_iteration_ << std::endl;
  
  // Position
  VectorXd p_err(3);
  for (unsigned int i=0; i<3; i++)	p_err(i) = pose_error_.p()(i);
  
  MatrixXd Kp(3,3);
  Kp = MatrixXd::Zero(3,3);
  Kp(0,0) = 1.0;
  Kp(1,1) = 1.0;
  Kp(2,2) = 1.0;
  
  VectorXd vd(3);
  desired_pose_velocity_ = compute_pose_velocity(desired_pose_, past_desired_pose_, time_increment_);
  vd = desired_pose_velocity_.p();
  
  VectorXd v_p(3);
  v_p = vd + Kp * p_err;
  
  
  // Orientation
  Quaternion<double> rot_current, rot_desired;
  rot_current = mobile_manip_.tcp_pose().o();
  rot_desired = desired_pose_.o();
  
  MatrixXd L(3,3);
  L = Lmat(rot_current, rot_desired);
  
  MatrixXd Sne(3,3), Sse(3,3), Sae(3,3);
  Sne = cross_product_matrix_S(rot_current.toRotationMatrix().col(0));
  Sse = cross_product_matrix_S(rot_current.toRotationMatrix().col(1));
  Sae = cross_product_matrix_S(rot_current.toRotationMatrix().col(2));
  
  VectorXd nd(3), sd(3), ad(3);
  nd = rot_desired.toRotationMatrix().col(0);
  sd = rot_desired.toRotationMatrix().col(1);
  ad = rot_desired.toRotationMatrix().col(2);
  
  VectorXd o_err(3);
  o_err = 0.5 * ( Sne*nd + Sse*sd + Sae*ad );
  
  MatrixXd Ko(3,3);
  Ko = MatrixXd::Zero(3,3);
  Ko(0,0) = 1.0;
  Ko(1,1) = 1.0;
  Ko(2,2) = 1.0;
  
  VectorXd wd(3);
  wd = desired_pose_velocity_.o_angaxis_r3();
  
  VectorXd v_o(3);
  v_o = L.inverse() * (L.transpose() * wd + Ko * o_err );
  
  
  // Put all together
  VectorXd v(6);
  for (unsigned int i=0; i<3; i++){
    v(i) = v_p(i);
    v(i+3) = v_o(i);
  }
 
  return pinv(jacobian(mobile_manip_), 0.001)*v;
}
コード例 #24
0
    //Class constructor
	TimeSegmentation(Tobj &G, Col <T1> map_in, Col <T1> timeVec_in, uword a, uword b, uword c, uword interptype = 1,
					 uword shots = 1)
    {
		cout << "Entering Class constructor" << endl;
	    n1 = a; //Data size
	    n2 = b;//Image size
	    L = c; //number of time segments
	    type = interptype; // type of time segmentation performed
	    Nshots = shots; // number of shots
	    obj = &G;
	    fieldMap = map_in;
    	cout << "N1 = " << n1 << endl;
		cout << "N2 = " << n2 << endl;
		cout << "L = " << L << endl;


		AA.set_size(n1, L); //time segments weights
	    timeVec = timeVec_in;
	    T_min =timeVec.min();
	    T1 rangt = timeVec.max()-T_min;
		tau = (rangt + datum::eps) / (L - 1); // it was L-1 before
	    timeVec = timeVec-T_min;

	    uword NOneShot = n1/Nshots;
	    if (L==1) {
		    tau = 0;
		    AA.ones();
	    }
	    else {
			Mat <CxT1> tempAA(NOneShot, L);
		    if (type==1) {// Hanning interpolator
			    cout << "Hanning interpolation" << endl;
			    for (unsigned int ii = 0; ii<L; ii++) {
				    for (unsigned int jj = 0; jj<NOneShot; jj++) {
					    if ((std::abs(timeVec(jj)-((ii)*tau)))<=tau) {
						    tempAA(jj, ii) = 0.5+0.5*std::cos((datum::pi)*(timeVec(jj)-((ii)*tau))/tau);
					    }
					    else {
						    tempAA(jj, ii) = 0.0;
					    }
				    }
			    }
			    AA = repmat(tempAA, Nshots, 1);
		    }
		    else if (type==2) { // Min-max interpolator: Exact LS interpolator

			    cout << "Min Max time segmentation" << endl;

			    Mat <CxT1> Ltp;
			    Ltp.ones(1, L);
			    Col <CxT1> ggtp;
			    ggtp.ones(n2, 1);
			    Mat <CxT1> gg;
			    gg = exp(i*fieldMap*tau)*Ltp;
			    Mat <CxT1> iGTGGT;
			    iGTGGT.set_size(L+1, n2);
			    Mat <CxT1> gl;
			    gl.zeros(n2, L);


			    for (unsigned int ii = 0; ii<L; ii++) {
				    for (unsigned int jj = 0; jj<n2; jj++) {
					    gl(jj, ii) = pow(gg(jj, ii), (T1) (ii+1));
				    }
			    }

			    Mat <CxT1> G;
			    G.set_size(n2, L);

			    for (unsigned int jj = 0; jj<L; jj++) {
				    if (jj==0) {
					    G.col(jj) = ggtp;
				    }
				    else {
					    G.col(jj) = gl.col(jj-1);
				    }
			    }

			    Col <CxT1> glsum;
			    Mat <CxT1> GTG;
			    GTG.zeros(L, L);
			    GTG.diag(0) += n2;
			    glsum = sum(gl.t(), 1);
				Mat <CxT1> GTGtp(L, L);
				for (unsigned int ii = 0; ii < (L - 1); ii++) {
					GTGtp.zeros();
				    GTGtp.diag(-(T1) (ii+1)) += glsum(ii);
				    GTGtp.diag((T1) (ii+1)) += std::conj(glsum(ii));
				    GTG = GTG+GTGtp;
			    }

			    T1 rcn = 1/cond(GTG);
			    if (rcn>10*2e-16) { //condition number of GTG
				    iGTGGT = inv(GTG)*G.t();

			    }
			    else {
				    iGTGGT = pinv(GTG)*G.t(); // pseudo inverse
			    }


			    Mat <CxT1> iGTGGTtp;
			    Mat <CxT1> ftp;
			    Col <CxT1> res, temp;

			    for (unsigned int ii = 0; ii<NOneShot; ii++) {
				    ftp = exp(i*fieldMap*timeVec(ii));
				    res = iGTGGT*ftp;
				    tempAA.row(ii) = res.t();
			    }
			    AA = repmat(tempAA, Nshots, 1);
		    }
	    }
		savemat("aamat.mat", "AA", vectorise(AA));
		cout << "Exiting class constructor." << endl;
    }
コード例 #25
0
ファイル: ICA.cpp プロジェクト: guilhermegarcia/praat-1
static void Diagonalizer_and_CrossCorrelationTable_qdiag (Diagonalizer me, CrossCorrelationTables thee, double *cweights, long maxNumberOfIterations, double delta) {
	try {
		CrossCorrelationTable c0 = (CrossCorrelationTable) thy item[1];
		double **w = my data;
		long dimension = c0 -> numberOfColumns;

		autoEigen eigen = Thing_new (Eigen);
		autoCrossCorrelationTables ccts = Data_copy (thee);
		autoNUMmatrix<double> pinv (1, dimension, 1, dimension);
		autoNUMmatrix<double> d (1, dimension, 1, dimension);
		autoNUMmatrix<double> p (1, dimension, 1, dimension);
		autoNUMmatrix<double> m1 (1, dimension, 1, dimension);
		autoNUMmatrix<double> wc (1, dimension, 1, dimension);
		autoNUMvector<double> wvec (1, dimension);
		autoNUMvector<double> wnew (1, dimension);
		autoNUMvector<double> mvec (1, dimension);

		for (long i = 1; i <= dimension; i++) // Transpose W
			for (long j = 1; j <= dimension; j++) {
				wc[i][j] = w[j][i];
			}

		// d = diag(diag(W'*C0*W));
		// W = W*d^(-1/2);

		NUMdmatrix_normalizeColumnVectors (wc.peek(), dimension, dimension, c0 -> data);

		// scale eigenvectors for sphering
		// [vb,db] = eig(C0);
		// P = db^(-1/2)*vb';

		Eigen_initFromSymmetricMatrix (eigen.peek(), c0 -> data, dimension);
		for (long i = 1; i <= dimension; i++) {
			if (eigen -> eigenvalues[i] < 0) {
				Melder_throw (U"Covariance matrix not positive definite, eigenvalue[", i, U"] is negative.");
			}
			double scalef = 1 / sqrt (eigen -> eigenvalues[i]);
			for (long j = 1; j <= dimension; j++) {
				p[dimension - i + 1][j] = scalef * eigen -> eigenvectors[i][j];
			}
		}

		// P*C[i]*P'

		for (long ic = 1; ic <= thy size; ic++) {
			CrossCorrelationTable cov1 = (CrossCorrelationTable) thy item[ic];
			CrossCorrelationTable cov2 = (CrossCorrelationTable) ccts -> item[ic];
			NUMdmatrices_multiply_VCVp (cov2 -> data, p.peek(), dimension, dimension, cov1 -> data, 1);
		}

		// W = P'\W == inv(P') * W

		NUMpseudoInverse (p.peek(), dimension, dimension, pinv.peek(), 0);

		NUMdmatrices_multiply_VpC (w, pinv.peek(), dimension, dimension, wc.peek(), dimension);

		// initialisation for order KN^3

		for (long ic = 2; ic <= thy size; ic++) {
			CrossCorrelationTable cov = (CrossCorrelationTable) ccts -> item[ic];
			// C * W
			NUMdmatrices_multiply_VC (m1.peek(), cov -> data, dimension, dimension, w, dimension);
			// D += scalef * M1*M1'
			NUMdmatrices_multiplyScaleAdd (d.peek(), m1.peek(), dimension, dimension, 2 * cweights[ic]);
		}

		long iter = 0;
		double delta_w;

		autoMelderProgress progress (U"Simultaneous diagonalization of many CrossCorrelationTables...");
		try {
			do {
				// the standard diagonality measure is rather expensive to calculate so we compare the norms of
				// differences of eigenvectors.

				delta_w = 0;
				for (long kol = 1; kol <= dimension; kol++) {
					for (long i = 1; i <= dimension; i++) {
						wvec[i] = w[i][kol];
					}

					update_one_column (ccts.peek(), d.peek(), cweights, wvec.peek(), -1, mvec.peek());

					Eigen_initFromSymmetricMatrix (eigen.peek(), d.peek(), dimension);

					// Eigenvalues already sorted; get eigenvector of smallest !

					for (long i = 1; i <= dimension; i++) {
						wnew[i] = eigen -> eigenvectors[dimension][i];
					}

					update_one_column (ccts.peek(), d.peek(), cweights, wnew.peek(), 1, mvec.peek());
					for (long i = 1; i <= dimension; i++) {
						w[i][kol] = wnew[i];
					}

					// compare norms of eigenvectors. We have to compare ||wvec +/- w_new|| because eigenvectors
					//  may change sign.

					double normp = 0, normm = 0;
					for (long j = 1; j <= dimension; j++) {
						double dm = wvec[j] - wnew[j], dp = wvec[j] + wnew[j];
						normp += dm * dm; normm += dp * dp;
					}

					normp = normp < normm ? normp : normm;
					normp = sqrt (normp);
					delta_w = normp > delta_w ? normp : delta_w;
				}
				iter++;

				Melder_progress ((double) iter / (double) (maxNumberOfIterations + 1), U"Iteration: ", iter, U", norm: ", delta_w);
			} while (delta_w > delta && iter < maxNumberOfIterations);
		} catch (MelderError) {
			Melder_clearError ();
		}

		// Revert the sphering W = P'*W;
		// Take transpose to make W*C[i]W' diagonal instead of W'*C[i]*W => (P'*W)'=W'*P

		NUMmatrix_copyElements (w, wc.peek(), 1, dimension, 1, dimension);
		NUMdmatrices_multiply_VpC (w, wc.peek(), dimension, dimension, p.peek(), dimension); // W = W'*P: final result

		// Calculate the "real" diagonality measure
	//	double dm = CrossCorrelationTables_and_Diagonalizer_getDiagonalityMeasure (thee, me, cweights, 1, thy size);

	} catch (MelderError) {
		Melder_throw (me, U" & ", thee, U": no joint diagonalization (qdiag).");
	}
}
コード例 #26
0
ファイル: preproc.c プロジェクト: debasish83/ecos
/*
 * Sets up all data structures needed.
 * Replace by codegen
 */
pwork* ECOS_setup(idxint n, idxint m, idxint p, idxint l, idxint ncones, idxint* q,
                   pfloat* Gpr, idxint* Gjc, idxint* Gir,
                   pfloat* Apr, idxint* Ajc, idxint* Air,
                   pfloat* c, pfloat* h, pfloat* b)
{
    idxint i, j, k, cidx, conesize, lnz, amd_result, nK, *Ljc, *Lir, *P, *Pinv, *Sign;
    pwork* mywork;
	double Control [AMD_CONTROL], Info [AMD_INFO];		
	pfloat rx, ry, rz, *Lpr;
	spmat *At, *Gt, *KU;

#if PROFILING > 0
	timer tsetup;
#endif

#if PROFILING > 1
	timer tcreatekkt;
	timer tmattranspose;
	timer tordering;
#endif

#if PROFILING > 0
	tic(&tsetup);
#endif
   
#if PRINTLEVEL > 2
	PRINTTEXT("\n");		
	PRINTTEXT("  *******************************************************************************\n");
	PRINTTEXT("  * ECOS: Embedded Conic Solver - Sparse Interior Point method for SOCPs        *\n");
	PRINTTEXT("  *                                                                             *\n");
	PRINTTEXT("  * NOTE: The solver is based on L. Vandenberghe's 'The CVXOPT linear and quad- *\n");
	PRINTTEXT("  *       ratic cone program solvers', March 20, 2010. Available online:        *\n");
	PRINTTEXT("  *       [http://abel.ee.ucla.edu/cvxopt/documentation/coneprog.pdf]           *\n");
	PRINTTEXT("  *                                                                             *\n");
	PRINTTEXT("  *       This code uses T.A. Davis' sparse LDL package and AMD code.           *\n");
	PRINTTEXT("  *       [http://www.cise.ufl.edu/research/sparse]                             *\n");
	PRINTTEXT("  *                                                                             *\n");
	PRINTTEXT("  *       Written during a summer visit at Stanford University with S. Boyd.    *\n");
	PRINTTEXT("  *                                                                             *\n");
	PRINTTEXT("  * (C) Alexander Domahidi, Automatic Control Laboratory, ETH Zurich, 2012-13.  *\n");
	PRINTTEXT("  *                     Email: [email protected]                      *\n");
	PRINTTEXT("  *******************************************************************************\n");
	PRINTTEXT("\n\n");
    PRINTTEXT("PROBLEM SUMMARY:\n");
    PRINTTEXT("    Primal variables (n): %d\n", (int)n);
	PRINTTEXT("Equality constraints (p): %d\n", (int)p);
	PRINTTEXT("     Conic variables (m): %d\n", (int)m);
	PRINTTEXT("- - - - - - - - - - - - - - -\n");
    PRINTTEXT("         Size of LP cone: %d\n", (int)l);
    PRINTTEXT("          Number of SOCs: %d\n", (int)ncones);
    for( i=0; i<ncones; i++ ){
        PRINTTEXT("    Size of SOC #%02d: %d\n", (int)(i+1), (int)q[i]);
    }
#endif
	
	/* get work data structure */
    mywork = (pwork *)MALLOC(sizeof(pwork));
#if PRINTLEVEL > 2
    PRINTTEXT("Memory allocated for WORK struct\n");
#endif

	/* dimensions */
	mywork->n = n;
	mywork->m = m;
	mywork->p = p;
    mywork->D = l + ncones;
#if PRINTLEVEL > 2
    PRINTTEXT("Set dimensions\n");
#endif

	/* variables */
    mywork->x = (pfloat *)MALLOC(n*sizeof(pfloat));
    mywork->y = (pfloat *)MALLOC(p*sizeof(pfloat));
    mywork->z = (pfloat *)MALLOC(m*sizeof(pfloat));
    mywork->s = (pfloat *)MALLOC(m*sizeof(pfloat));
  	mywork->lambda = (pfloat *)MALLOC(m*sizeof(pfloat));
	mywork->dsaff_by_W = (pfloat *)MALLOC(m*sizeof(pfloat));
    mywork->dsaff = (pfloat *)MALLOC(m*sizeof(pfloat));
    mywork->dzaff = (pfloat *)MALLOC(m*sizeof(pfloat));
    mywork->saff = (pfloat *)MALLOC(m*sizeof(pfloat));
    mywork->zaff = (pfloat *)MALLOC(m*sizeof(pfloat));
	mywork->W_times_dzaff = (pfloat *)MALLOC(m*sizeof(pfloat));
#if PRINTLEVEL > 2
    PRINTTEXT("Memory allocated for variables\n");
#endif
    
    /* best iterates so far */
    mywork->best_x = (pfloat *)MALLOC(n*sizeof(pfloat));
    mywork->best_y = (pfloat *)MALLOC(p*sizeof(pfloat));
    mywork->best_z = (pfloat *)MALLOC(m*sizeof(pfloat));
    mywork->best_s = (pfloat *)MALLOC(m*sizeof(pfloat));
    mywork->best_info = (stats *)MALLOC(sizeof(stats));

	/* cones */
	mywork->C = (cone *)MALLOC(sizeof(cone));
#if PRINTLEVEL > 2
    PRINTTEXT("Memory allocated for cone struct\n");
#endif

	/* LP cone */
	mywork->C->lpc = (lpcone *)MALLOC(sizeof(lpcone));
	mywork->C->lpc->p = l;
	if( l > 0 ){
		mywork->C->lpc->w = (pfloat *)MALLOC(l*sizeof(pfloat));
		mywork->C->lpc->v = (pfloat *)MALLOC(l*sizeof(pfloat));
		mywork->C->lpc->kkt_idx = (idxint *)MALLOC(l*sizeof(idxint));
#if PRINTLEVEL > 2
        PRINTTEXT("Memory allocated for LP cone\n");
#endif
	} else {
		mywork->C->lpc->w = NULL;
		mywork->C->lpc->v = NULL;
		mywork->C->lpc->kkt_idx = NULL;
#if PRINTLEVEL > 2
        PRINTTEXT("No LP cone present, pointers filled with NULL\n");
#endif
	}


	/* Second-order cones */
	mywork->C->soc = (socone *)MALLOC(ncones*sizeof(socone));
	mywork->C->nsoc = ncones;
    cidx = 0;
    for( i=0; i<ncones; i++ ){
        conesize = (idxint)q[i];
        mywork->C->soc[i].p = conesize;
        mywork->C->soc[i].a = 0;
		mywork->C->soc[i].eta = 0;
        mywork->C->soc[i].q = (pfloat *)MALLOC((conesize-1)*sizeof(pfloat));
		mywork->C->soc[i].skbar = (pfloat *)MALLOC((conesize)*sizeof(pfloat));
		mywork->C->soc[i].zkbar = (pfloat *)MALLOC((conesize)*sizeof(pfloat));
#if CONEMODE == 0
        mywork->C->soc[i].Didx = (idxint *)MALLOC((conesize)*sizeof(idxint));
#endif 
#if CONEMODE > 0
        mywork->C->soc[i].colstart = (idxint *)MALLOC((conesize)*sizeof(idxint));
#endif
        cidx += conesize;
    }
#if PRINTLEVEL > 2
    PRINTTEXT("Memory allocated for second-order cones\n");
#endif

	/* info struct */
    mywork->info = (stats *)MALLOC(sizeof(stats));
#if PROFILING > 1
	mywork->info->tfactor = 0;
	mywork->info->tkktsolve = 0;
    mywork->info->tfactor_t1 = 0;
    mywork->info->tfactor_t2 = 0;
#endif
#if PRINTLEVEL > 2
    PRINTTEXT("Memory allocated for info struct\n");
#endif

    
#if defined EQUILIBRATE && EQUILIBRATE > 0
    /* equilibration vector */
    mywork->xequil = (pfloat *)MALLOC(n*sizeof(pfloat));
    mywork->Aequil = (pfloat *)MALLOC(p*sizeof(pfloat));
    mywork->Gequil = (pfloat *)MALLOC(m*sizeof(pfloat));
    
#if PRINTLEVEL > 2
    PRINTTEXT("Memory allocated for equilibration vectors\n");
#endif
#endif

	/* settings */
	mywork->stgs = (settings *)MALLOC(sizeof(settings));
	mywork->stgs->maxit = MAXIT;
	mywork->stgs->gamma = GAMMA;	
	mywork->stgs->delta = DELTA;
    mywork->stgs->eps = EPS;
	mywork->stgs->nitref = NITREF;
	mywork->stgs->abstol = ABSTOL;	
	mywork->stgs->feastol = FEASTOL;
	mywork->stgs->reltol = RELTOL;
    mywork->stgs->abstol_inacc = ATOL_INACC;
	mywork->stgs->feastol_inacc = FTOL_INACC;
	mywork->stgs->reltol_inacc = RTOL_INACC;
    mywork->stgs->verbose = VERBOSE;
#if PRINTLEVEL > 2
    PRINTTEXT("Written settings\n");
#endif

    mywork->c = c;
    mywork->h = h;
    mywork->b = b;
#if PRINTLEVEL > 2
    PRINTTEXT("Hung pointers for c, h and b into WORK struct\n");
#endif

    /* Store problem data */
  if(Apr && Ajc && Air) {
    mywork->A = createSparseMatrix(p, n, Ajc[n], Ajc, Air, Apr);
  } else {
    mywork->A = NULL;
  }
  if (Gpr && Gjc && Gir) {
	  mywork->G = createSparseMatrix(m, n, Gjc[n], Gjc, Gir, Gpr);
  } else {
    /* create an empty sparse matrix */
	mywork->G = createSparseMatrix(m, n, 0, Gjc, Gir, Gpr);
  }

#if defined EQUILIBRATE && EQUILIBRATE > 0
    set_equilibration(mywork);
    #if PRINTLEVEL > 2
        PRINTTEXT("Done equilibrating\n");
    #endif
#endif

#if PROFILING > 1
	mywork->info->ttranspose = 0;
	tic(&tmattranspose);
#endif
  if(mywork->A)
	  At = transposeSparseMatrix(mywork->A);
  else
    At = NULL;
#if PROFILING > 1	
	mywork->info->ttranspose += toc(&tmattranspose);
#endif
#if PRINTLEVEL > 2
    PRINTTEXT("Transposed A\n");
#endif
    
    
#if PROFILING > 1	
	tic(&tmattranspose);
#endif
	Gt = transposeSparseMatrix(mywork->G);    	
#if PROFILING > 1	
	mywork->info->ttranspose += toc(&tmattranspose);
#endif
#if PRINTLEVEL > 2
    PRINTTEXT("Transposed G\n");
#endif
    


     
  
    /* set up KKT system */
#if PROFILING > 1
	tic(&tcreatekkt);
#endif
	createKKT_U(Gt, At, mywork->C, &Sign, &KU);
#if PROFILING > 1
	mywork->info->tkktcreate = toc(&tcreatekkt);
#endif
#if PRINTLEVEL > 2
    PRINTTEXT("Created upper part of KKT matrix K\n");
#endif
    
    
	/* 
     * Set up KKT system related data
     * (L comes later after symbolic factorization) 
     */
    nK = KU->n;
    
#if DEBUG > 0
    dumpSparseMatrix(KU, "KU0.txt");
#endif
#if PRINTLEVEL > 2
    PRINTTEXT("Dimension of KKT matrix: %d\n", (int)nK);
    PRINTTEXT("Non-zeros in KKT matrix: %d\n", (int)KU->nnz);
#endif
    
    
    
    /* allocate memory in KKT system */
	mywork->KKT = (kkt *)MALLOC(sizeof(kkt));
	mywork->KKT->D = (pfloat *)MALLOC(nK*sizeof(pfloat));
	mywork->KKT->Parent = (idxint *)MALLOC(nK*sizeof(idxint));
	mywork->KKT->Pinv = (idxint *)MALLOC(nK*sizeof(idxint));
	mywork->KKT->work1 = (pfloat *)MALLOC(nK*sizeof(pfloat));
	mywork->KKT->work2 = (pfloat *)MALLOC(nK*sizeof(pfloat));
    mywork->KKT->work3 = (pfloat *)MALLOC(nK*sizeof(pfloat));
    mywork->KKT->work4 = (pfloat *)MALLOC(nK*sizeof(pfloat));
    mywork->KKT->work5 = (pfloat *)MALLOC(nK*sizeof(pfloat));
    mywork->KKT->work6 = (pfloat *)MALLOC(nK*sizeof(pfloat));
	mywork->KKT->Flag = (idxint *)MALLOC(nK*sizeof(idxint));	
	mywork->KKT->Pattern = (idxint *)MALLOC(nK*sizeof(idxint));
	mywork->KKT->Lnz = (idxint *)MALLOC(nK*sizeof(idxint));	
	mywork->KKT->RHS1 = (pfloat *)MALLOC(nK*sizeof(pfloat));
	mywork->KKT->RHS2 = (pfloat *)MALLOC(nK*sizeof(pfloat));
	mywork->KKT->dx1 = (pfloat *)MALLOC(mywork->n*sizeof(pfloat));
	mywork->KKT->dx2 = (pfloat *)MALLOC(mywork->n*sizeof(pfloat));
	mywork->KKT->dy1 = (pfloat *)MALLOC(mywork->p*sizeof(pfloat));
	mywork->KKT->dy2 = (pfloat *)MALLOC(mywork->p*sizeof(pfloat));
	mywork->KKT->dz1 = (pfloat *)MALLOC(mywork->m*sizeof(pfloat));
	mywork->KKT->dz2 = (pfloat *)MALLOC(mywork->m*sizeof(pfloat));
    mywork->KKT->Sign = (idxint *)MALLOC(nK*sizeof(idxint));
    mywork->KKT->PKPt = newSparseMatrix(nK, nK, KU->nnz);
	mywork->KKT->PK = (idxint *)MALLOC(KU->nnz*sizeof(idxint));

#if PRINTLEVEL > 2
    PRINTTEXT("Created memory for KKT-related data\n");    
#endif
    
    
    /* calculate ordering of KKT matrix using AMD */
	P = (idxint *)MALLOC(nK*sizeof(idxint));
#if PROFILING > 1
	tic(&tordering);
#endif
	AMD_defaults(Control);	
	amd_result = AMD_order(nK, KU->jc, KU->ir, P, Control, Info);	
#if PROFILING > 1	
	mywork->info->torder = toc(&tordering);
#endif

	if( amd_result == AMD_OK ){
#if PRINTLEVEL > 2
		PRINTTEXT("AMD ordering successfully computed.\n");
		AMD_info(Info);
#endif
	} else {
#if PRINTLEVEL > 2
		PRINTTEXT("Problem in AMD ordering, exiting.\n");
        AMD_info(Info);
#endif
        return NULL;
	}
	
	/* calculate inverse permutation and permutation mapping of KKT matrix */
	pinv(nK, P, mywork->KKT->Pinv);		
	Pinv = mywork->KKT->Pinv;
#if DEBUG > 0
    dumpDenseMatrix_i(P, nK, 1, "P.txt");
    dumpDenseMatrix_i(mywork->KKT->Pinv, nK, 1, "PINV.txt");
#endif
	permuteSparseSymmetricMatrix(KU, mywork->KKT->Pinv, mywork->KKT->PKPt, mywork->KKT->PK);

	/* permute sign vector */
    for( i=0; i<nK; i++ ){ mywork->KKT->Sign[Pinv[i]] = Sign[i]; }
#if PRINTLEVEL > 3
    PRINTTEXT("P = [");
    for( i=0; i<nK; i++ ){ PRINTTEXT("%d ", (int)P[i]); }
    PRINTTEXT("];\n");
    PRINTTEXT("Pinv = [");
    for( i=0; i<nK; i++ ){ PRINTTEXT("%d ", (int)Pinv[i]); }
    PRINTTEXT("];\n");
    PRINTTEXT("Sign = [");
    for( i=0; i<nK; i++ ){ PRINTTEXT("%+d ", (int)Sign[i]); }
    PRINTTEXT("];\n");
    PRINTTEXT("SignP = [");
    for( i=0; i<nK; i++ ){ PRINTTEXT("%+d ", (int)mywork->KKT->Sign[i]); }
    PRINTTEXT("];\n");
#endif
	
    
	
	/* symbolic factorization */	
	Ljc = (idxint *)MALLOC((nK+1)*sizeof(idxint));
#if PRINTLEVEL > 2
    PRINTTEXT("Allocated memory for cholesky factor L\n");
#endif    
	LDL_symbolic2(
		mywork->KKT->PKPt->n,    /* A and L are n-by-n, where n >= 0 */
		mywork->KKT->PKPt->jc,   /* input of size n+1, not modified */
		mywork->KKT->PKPt->ir,	 /* input of size nz=Ap[n], not modified */
		Ljc,					 /* output of size n+1, not defined on input */
		mywork->KKT->Parent,	 /* output of size n, not defined on input */
		mywork->KKT->Lnz,		 /* output of size n, not defined on input */
		mywork->KKT->Flag		 /* workspace of size n, not defn. on input or output */
	);
	

	/* assign memory for L */
	lnz = Ljc[nK];
#if PRINTLEVEL > 2
	PRINTTEXT("Nonzeros in L, excluding diagonal: %d\n", (int)lnz) ;
#endif
	Lir = (idxint *)MALLOC(lnz*sizeof(idxint));
	Lpr = (pfloat *)MALLOC(lnz*sizeof(pfloat));
	mywork->KKT->L = createSparseMatrix(nK, nK, lnz, Ljc, Lir, Lpr);
#if PRINTLEVEL > 2
	PRINTTEXT("Created Cholesky factor of K in KKT struct\n");
#endif
    

	/* permute KKT matrix - we work on this one from now on */
	permuteSparseSymmetricMatrix(KU, mywork->KKT->Pinv, mywork->KKT->PKPt, NULL);
#if DEBUG > 0
    dumpSparseMatrix(mywork->KKT->PKPt, "PKPt.txt");
#endif
    
#if CONEMODE > 0
    /* zero any off-diagonal elements in (permuted) scalings in KKT matrix */
    for (i=0; i<mywork->C->nsoc; i++) {
        for (j=1; j<mywork->C->soc[i].p; j++) {
            for (k=0; k<j; k++) {
                mywork->KKT->PKPt->pr[mywork->KKT->PK[mywork->C->soc[i].colstart[j]+k]] = 0;
            }
        }
    }
#endif
#if DEBUG > 0
     dumpSparseMatrix(mywork->KKT->PKPt, "PKPt0.txt");
#endif

	/* set up RHSp for initialization */
	k = 0; j = 0;
	for( i=0; i<n; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = 0; }
	for( i=0; i<p; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = b[i]; }
	for( i=0; i<l; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = h[i]; j++; }
	for( l=0; l<ncones; l++ ){ 
		for( i=0; i < mywork->C->soc[l].p; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = h[j++]; }
#if CONEMODE == 0
		mywork->KKT->RHS1[Pinv[k++]] = 0;
        mywork->KKT->RHS1[Pinv[k++]] = 0;
#endif
	}
#if PRINTLEVEL > 2
    PRINTTEXT("Written %d entries of RHS1\n", (int)k);
#endif
	
	/* set up RHSd for initialization */
	for( i=0; i<n; i++ ){ mywork->KKT->RHS2[Pinv[i]] = -c[i]; }
	for( i=n; i<nK; i++ ){ mywork->KKT->RHS2[Pinv[i]] = 0; }

	/* get scalings of problem data */
	rx = norm2(c, n); mywork->resx0 = MAX(1, rx);
	ry = norm2(b, p); mywork->resy0 = MAX(1, ry);
	rz = norm2(h, m); mywork->resz0 = MAX(1, rz);

	/* get memory for residuals */
	mywork->rx = (pfloat *)MALLOC(n*sizeof(pfloat));
	mywork->ry = (pfloat *)MALLOC(p*sizeof(pfloat));
	mywork->rz = (pfloat *)MALLOC(m*sizeof(pfloat));
	
    /* clean up */
    mywork->KKT->P = P;
	FREE(Sign);
  if(At) freeSparseMatrix(At);
	freeSparseMatrix(Gt);
	freeSparseMatrix(KU);
    
#if PROFILING > 0
	mywork->info->tsetup = toc(&tsetup);
#endif

    return mywork;
}
コード例 #27
0
ファイル: GMPReduced.c プロジェクト: bremond/siconos
/*
 * The equalities are eliminated.
 *
 *0=(Me_1 Me_2)(Re Ri)' + Qe
 *Vi=(Mi_1 Mi_2)(Re Ri)' + Qi
 *
 *Re=-Me_1^{-1}(Me_2Ri+Qe)
 *
 *Vi=(Mi_2-Mi_1 Me_1^{-1} Me_2)Ri+Qi-Mi1 Me_1^{-1} Qe
 *
 */
void GMPReducedSolve(GenericMechanicalProblem* pInProblem, double *reaction , double *velocity, int * info, SolverOptions* options, NumericsOptions* numerics_options)
{

  SparseBlockStructuredMatrix* m = pInProblem->M->matrix1;
  int nbRow = m->blocksize0[m->blocknumber0 - 1];
  int nbCol = m->blocksize1[m->blocknumber1 - 1];
  double *Me = (double *) malloc(nbRow * nbCol * sizeof(double));
  double *Qe = (double *) malloc(nbRow * sizeof(double));
  double *Mi = (double *) malloc(nbRow * nbCol * sizeof(double));
  double *Qi = (double *) malloc(nbRow * sizeof(double));
  int Me_size;
  int Mi_size;
  buildReducedGMP(pInProblem, Me, Mi, Qe, Qi, &Me_size, &Mi_size);

  if ((Me_size == 0 || Mi_size == 0))
  {
    genericMechanicalProblem_GS(pInProblem, reaction, velocity, info, options, numerics_options);
    free(Me);
    free(Qe);
    free(Mi);
    free(Qi);
    return;
  }

  double * pseduInvMe1 = (double *)malloc(Me_size * Me_size * sizeof(double));
  memcpy(pseduInvMe1, Me, Me_size * Me_size * sizeof(double));
  pinv(pseduInvMe1, Me_size, Me_size, 1e-16);
  double *Mi2 = Mi + Mi_size * Me_size;
  double *Mi1 = Mi;
  double *Me2 = Me + Me_size * Me_size;
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
  double *Me1 = Me;
  FILE * titi  = fopen("buildReducedGMP_output.txt", "w");
  printf("GMPReducedsolve\n");
  printDenseMatrice("Me1", titi, Me1, Me_size, Me_size);
  printDenseMatrice("Me2", titi, Me2, Me_size, Mi_size);
  printDenseMatrice("Mi1", titi, Mi1, Mi_size, Me_size);
  printDenseMatrice("Mi2", titi, Mi2, Mi_size, Mi_size);
  printDenseMatrice("Qe", titi, Qe, Me_size, 1);
  printDenseMatrice("Qi", titi, Qi, Mi_size, 1);
  printDenseMatrice("Me1inv", titi, pseduInvMe1, Me_size, Me_size);
#endif


  double * reducedProb = (double *)malloc(Mi_size * Mi_size * sizeof(double));
  memcpy(reducedProb, Mi2, Mi_size * Mi_size * sizeof(double));

  double * Mi1pseduInvMe1 = (double *)malloc(Mi_size * Me_size * sizeof(double));
  cblas_dgemm(CblasColMajor,CblasNoTrans, CblasNoTrans, Mi_size, Me_size, Me_size, -1.0, Mi1, Mi_size, pseduInvMe1, Me_size, 0.0, Mi1pseduInvMe1, Mi_size);
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
  printDenseMatrice("minusMi1pseduInvMe1", titi, Mi1pseduInvMe1, Mi_size, Me_size);
  fprintf(titi, "_minusMi1pseduInvMe1=-Mi1*Me1inv;\n");
#endif
  cblas_dgemv(CblasColMajor,CblasNoTrans, Mi_size, Me_size, 1.0, Mi1pseduInvMe1, Mi_size, Qe, 1, 1.0, Qi, 1);
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
  printDenseMatrice("newQi", titi, Qi, Mi_size, 1);
  fprintf(titi, "_newQi=Qi+_minusMi1pseduInvMe1*Qe;\n");
#endif
  cblas_dgemm(CblasColMajor,CblasNoTrans, CblasNoTrans, Mi_size, Mi_size, Me_size, 1.0, Mi1pseduInvMe1, Mi_size, Me2, Me_size, 1.0, reducedProb, Mi_size);
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
  printDenseMatrice("W", titi, reducedProb, Mi_size, Mi_size);
  fprintf(titi, "_W=Mi2+_minusMi1pseduInvMe1*Me2;\n");

#endif
  listNumericsProblem * curProblem = 0;
  GenericMechanicalProblem * _pnumerics_GMP = buildEmptyGenericMechanicalProblem();
  curProblem =  pInProblem->firstListElem;
  while (curProblem)
  {
    switch (curProblem->type)
    {
    case SICONOS_NUMERICS_PROBLEM_EQUALITY:
    {
      break;
    }
    case SICONOS_NUMERICS_PROBLEM_LCP:
    {
      addProblem(_pnumerics_GMP, curProblem->type, curProblem->size);
      break;
    }
    case SICONOS_NUMERICS_PROBLEM_FC3D:
    {
      FrictionContactProblem* pFC3D = (FrictionContactProblem*)addProblem(_pnumerics_GMP, curProblem->type, curProblem->size);
      *(pFC3D->mu) = *(((FrictionContactProblem*)curProblem->problem)->mu);
      break;
    }
    default:
      printf("GMPReduced  buildReducedGMP: problemType unknown: %d . \n", curProblem->type);
    }
    curProblem = curProblem->nextProblem;
  }
  NumericsMatrix numM;
  numM.storageType = 0;
  numM.matrix0 = reducedProb;
  numM.matrix1 = 0;
  numM.size0 = Mi_size;
  numM.size1 = Mi_size;
  _pnumerics_GMP->M = &numM;
  _pnumerics_GMP->q = Qi;
  double *Rreduced = (double *) malloc(Mi_size * sizeof(double));
  double *Vreduced = (double *) malloc(Mi_size * sizeof(double));
  genericMechanicalProblem_GS(_pnumerics_GMP, Rreduced, Vreduced, info, options, numerics_options);
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
  if (*info)
  {
    printf("\nGMPREduced failed!\n");
  }
  else
  {
    printf("\nGMPREduced succed!\n");
    printDenseMatrice("Ri", titi, Rreduced, Mi_size, 1);
    printDenseMatrice("Vi", titi, Vreduced, Mi_size, 1);
  }
#endif
  if (!*info)
  {
    /*Re computation*/
    double * Re = (double*)malloc(Me_size * sizeof(double));
    double * Rbuf = (double*)malloc(Me_size * sizeof(double));
    memcpy(Rbuf, Qe, Me_size * sizeof(double));
    cblas_dgemv(CblasColMajor,CblasNoTrans, Me_size, Mi_size, 1.0, Me2, Me_size, Rreduced, 1, 1.0, Rbuf, 1);
    cblas_dgemv(CblasColMajor,CblasNoTrans, Me_size, Me_size, -1.0, pseduInvMe1, Me_size, Rbuf, 1, 0.0, Re, 1);
  #ifdef GMP_DEBUG_GMPREDUCED_SOLVE
    fprintf(titi, "_Re=-Me1inv*(Me2*Ri+Qe);\n");
    printDenseMatrice("Re", titi, Re, Me_size, 1);
  #endif
    GMPReducedSolToSol(pInProblem, reaction, velocity, Re, Rreduced, Vreduced);
    double err;
    int tolViolate = GenericMechanical_compute_error(pInProblem, reaction, velocity, options->dparam[0], options, &err);
    if (tolViolate)
    {
      printf("GMPReduced, warnning, reduced problem solved, but error of intial probleme violated tol = %e, err= %e\n", options->dparam[0], err);
    }
    free(Re);
    free(Rbuf);
  }

#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
  fclose(titi);
#endif
  free(Rreduced);
  free(Vreduced);
  freeGenericMechanicalProblem(_pnumerics_GMP, NUMERICS_GMP_FREE_GMP);
  free(Me);
  free(Mi);
  free(Qe);
  free(Qi);
  free(pseduInvMe1);
  free(reducedProb);
  free(Mi1pseduInvMe1);
  //  GenericMechanicalProblem GMPOutProblem;
  //  SparseBlockStructuredMatrix mOut;

}
コード例 #28
0
ファイル: solver.cpp プロジェクト: AbuMussabRaja/icub-main
void EyePinvRefGen::run()
{
    if (genOn)
    {
        LockGuard guard(mutex);
        double timeStamp;

        // read encoders
        getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&timeStamp);
        updateTorsoBlockedJoints(chainNeck,fbTorso);
        updateTorsoBlockedJoints(chainEyeL,fbTorso);
        updateTorsoBlockedJoints(chainEyeR,fbTorso);

        // get current target
        Vector xd=commData->port_xd->get_xd();

        // update neck chain
        chainNeck->setAng(nJointsTorso+0,fbHead[0]);
        chainNeck->setAng(nJointsTorso+1,fbHead[1]);
        chainNeck->setAng(nJointsTorso+2,fbHead[2]);

        // ask for saccades (if possible)
        if (commData->saccadesOn && (saccadesRxTargets!=commData->port_xd->get_rx()) &&
            !commData->saccadeUnderway && (Time::now()-saccadesClock>commData->saccadesInhibitionPeriod))
        {
            Vector fph=xd; fph.push_back(1.0);
            fph=SE3inv(chainNeck->getH())*fph; fph[3]=0.0;
            double rot=CTRL_RAD2DEG*acos(fph[2]/norm(fph)); fph[3]=1.0;

            // estimate geometrically the target tilt and pan of the eyes
            Vector ang(3,0.0);
            ang[0]=-atan2(fph[1],fabs(fph[2]));
            ang[1]=atan2(fph[0],fph[2]);

            // enforce joints bounds
            ang[0]=sat(ang[0],lim(0,0),lim(0,1));
            ang[1]=sat(ang[1],lim(1,0),lim(1,1));

            // favor the smooth-pursuit in case saccades are small
            if (rot>commData->saccadesActivationAngle)
            {
                // init vergence
                ang[2]=fbHead[5];

                // get rid of eyes tilt
                Vector axis(4);
                axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; axis[3]=-ang[0];
                fph=axis2dcm(axis)*fph;

                // go on iff the point is in front of us
                if (fph[2]>0.0)
                {
                    double L,R;

                    // estimate geometrically the target vergence Vg=L-R                    
                    if (fph[0]>=eyesHalfBaseline)
                    {
                        L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
                        R=M_PI/2.0-atan2(fph[2],fph[0]-eyesHalfBaseline);
                    }
                    else if (fph[0]>-eyesHalfBaseline)
                    {
                        L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
                        R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
                    }
                    else
                    {
                        L=-(M_PI/2.0-atan2(fph[2],-fph[0]-eyesHalfBaseline));
                        R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
                    }

                    ang[2]=L-R;
                }

                // enforce joints bounds
                ang[2]=sat(ang[2],lim(2,0),lim(2,1));

                commData->set_qd(3,ang[0]);
                commData->set_qd(4,ang[1]);
                commData->set_qd(5,ang[2]);

                Vector vel(3,SACCADES_VEL);
                ctrl->doSaccade(ang,vel);
                saccadesClock=Time::now();
            }
        }

        // update eyes chains for convergence purpose
        updateNeckBlockedJoints(chainEyeL,fbHead);         updateNeckBlockedJoints(chainEyeR,fbHead);
        chainEyeL->setAng(nJointsTorso+3,qd[0]);           chainEyeR->setAng(nJointsTorso+3,qd[0]);
        chainEyeL->setAng(nJointsTorso+4,qd[1]+qd[2]/2.0); chainEyeR->setAng(nJointsTorso+4,qd[1]-qd[2]/2.0);

        // converge on target
        if (CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
        {
            Vector v=EYEPINVREFGEN_GAIN*(pinv(eyesJ)*(xd-fp));

            // update eyes chains in actual configuration for velocity compensation
            chainEyeL->setAng(nJointsTorso+3,fbHead[3]);               chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
            chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0); chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);

            // compensate neck rotation at eyes level
            if ((commData->eyesBoundVer>=0.0) || !CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
                commData->set_counterv(zeros(qd.length()));
            else
                commData->set_counterv(getEyesCounterVelocity(eyesJ,fp));
            
            // reset eyes controller and integral upon saccades transition on=>off
            if (saccadeUnderWayOld && !commData->saccadeUnderway)
            {
                ctrl->resetCtrlEyes();

                qd[0]=fbHead[3];
                qd[1]=fbHead[4];
                qd[2]=fbHead[5];
                I->reset(qd);
            }

            // update reference
            qd=I->integrate(v+commData->get_counterv());
        }
        else
            commData->set_counterv(zeros(qd.length()));

        // set a new target position
        commData->set_xd(xd);
        commData->set_x(fp,timeStamp);
        commData->set_fpFrame(chainNeck->getH());
        if (!commData->saccadeUnderway)
        {
            commData->set_qd(3,qd[0]);
            commData->set_qd(4,qd[1]);
            commData->set_qd(5,qd[2]);
        }

        // latch the saccades status
        saccadeUnderWayOld=commData->saccadeUnderway;
        saccadesRxTargets=commData->port_xd->get_rx();
    }
}
コード例 #29
0
ファイル: VCA.cpp プロジェクト: sedarsky/Vespucci
///
/// \brief Vespucci::Math::DimensionReduction::VCA
/// Vertex Component Analysis
/// \param R The dataset
/// \param endmembers Number of endmembers to compute
/// \param indices Row indices of pure components.
/// \param endmember_spectra Spectra of pure components (note that these are in
/// columns, not rows as in spectra_)
/// \param projected_data Projected data
/// \param fractional_abundances Purity of a given spectrum relative to endmember
/// \return Convergeance (no actual test implemented...)
///
bool Vespucci::Math::DimensionReduction::VCA(const arma::mat &R, arma::uword p,
         arma::uvec &indices, arma::mat &endmember_spectra,
         arma::mat &projected_data, arma::mat &fractional_abundances)
{
//Initializations
    arma::uword L = R.n_rows;
    arma::uword N = R.n_cols;
    if (L == 0 || N == 0){
        std::cerr << "No data!" << std::endl;
        return false;
    }

    if (p > L){
        std::cerr << "wrong number of endmembers (" << p << ")!"<< std::endl;
        std::cerr << "set to 5 or one less than number of spectra" << std::endl;
        p = (L < 5? 5: L-1);
    }
//mat of SNR
    arma::mat r_m = mean(R, 1);
    arma::mat R_m = arma::repmat(r_m, 1, N); //the mean of each spectral band
    arma::mat R_o = R - R_m; //mean-center the data
    arma::mat Ud;
    arma::vec Sd;
    arma::mat Vd;
    //arma::svds(Ud, Sd, Vd, arma::sp_mat(R_o * R_o.t()/N), p);
    Vespucci::Math::DimensionReduction::svds(R_o*R_o.t()/N, p, Ud, Sd, Vd);
    arma::mat x_p;
    try{
    x_p = Ud.t() * R_o;
    }catch(std::exception e){
        std::cout << "Ud.t() * R_o" << std::endl;
    }

    double SNR = Vespucci::Math::DimensionReduction::estimate_snr(R, r_m, x_p);
    double SNR_th = 15 + 10*log10(p);

//Choose projective projection or projection to p-1 subspace
    arma::mat y;
    if (SNR < SNR_th){
        arma::uword d = p - 1;
        Ud = Ud.cols(0, d-1);
        projected_data = Ud * x_p.rows(0, d-1) + R_m; //in dimension L
        arma::mat x = x_p.rows(0, d-1);//x_p = trans(Ud)*R_o, p-dimensional subspace
        //following three lines are one in arma::matlab...
        arma::mat sum_squares = sum(pow(x, 2));
        double c = sum_squares.max();
        c = std::sqrt(c);
        y = arma::join_vert(x, c*arma::ones(1, N));
      }
    else{
        arma::uword d = p;
        Vespucci::Math::DimensionReduction::svds(R*R.t()/N, p, Ud, Sd, Vd);
        arma::svds(Ud, Sd, Vd, arma::sp_mat(R*R.t()/N), d);//R_o is a mean centered version...
        x_p = Ud.t() * R;
        projected_data = Ud * x_p.rows(0, d-1);
        arma::mat x = Ud.t() * R;
        arma::mat u = arma::mean(x, 1);
        y = x / arma::repmat(sum(x % arma::repmat(u, 1, N)), d, 1);
    }


    // The VCA algorithm
    arma::vec w;
    w.set_size(p);
    arma::vec f;
    arma::rowvec v;
    indices.set_size(p);
    //there are no fill functions for arma::uvecs
    for (arma::uword i = 0; i < p; ++i)
        indices(i) = 0;

    arma::mat A = arma::zeros(p, p);
    double v_max;
    double sum_squares;
    arma::uvec q1;
    A(p-1, 0) = 1;
    for (arma::uword i = 0; i < p; ++i){
        w.randu();
        f = w - A*arma::pinv(A)*w;
        sum_squares = sqrt(sum(square(f)));
        f /= sum_squares;
        v = f.t() * y;
        v_max = arma::max(abs(v));
        q1 = arma::find(abs(v) == v_max, 1);
        indices(i) = q1(0);
        A.col(i) = y.col(indices(i)); //same as x.col(indices(i));
    }
    endmember_spectra = projected_data.cols(indices);
    fractional_abundances = arma::trans(pinv(endmember_spectra) * projected_data);
    return true;
}
コード例 #30
0
ファイル: unicycle_motions.cpp プロジェクト: aurone/mprims
std::vector<Pose2_cont>
generate_unicycle_motion(const Pose2_cont& start, const Pose2_cont& goal)
{
    DEBUG_PRINT("--------------------------------------------------------------------------------\n");
    DEBUG_PRINT("generating unicycle motion between %s and %s\n", to_string(start).c_str(), to_string(goal).c_str());
    DEBUG_PRINT("--------------------------------------------------------------------------------\n");

    auto almost_equals = [](double lhs, double rhs, double eps) { return fabs(lhs - rhs) < eps; };
    const double eps = 1e-6;

    // check for straight-line motion or turn-in-place
    double dx = goal.x - start.x;
    double dy = goal.y - start.y;
    double dtheta = shortest_angle_diff(goal.yaw, start.yaw);
    if ((dx == 0.0 && dy == 0.0) || (dtheta == 0.0)) {
        if (almost_equals(atan2(dy, dx), start.yaw, eps) && almost_equals(atan2(dy, dx), goal.yaw, eps)) {
            DEBUG_PRINT("Interpolated Motion\n");
            return create_interpolated_motion(start, goal);
        }
        else {
            DEBUG_PRINT("No unicycle motion for turns-in-place or skidding\n");
            return { };
        }
    }

    DEBUG_PRINT("Arc Motion\n");
    Eigen::Matrix2d R;
    R(0, 0) = cos(start.yaw);
    R(0, 1) = sin(goal.yaw) - sin(start.yaw);
    R(1, 0) = sin(start.yaw);
    R(1, 1) = -(cos(goal.yaw) - cos(start.yaw));

    Eigen::Matrix2d Rpinv;
    if (!pinv(R, Rpinv)) {
        DEBUG_PRINT("Failed to compute Moore-penrose pseudo-inverse");
        return { };
    }

    Eigen::Vector2d S = Rpinv * Eigen::Vector2d(goal.x - start.x, goal.y - start.y);

    double straight_length = S(0);
    double radius = S(1);

    if (fabs(radius) < 1e-6)  {
        DEBUG_PRINT("Unable to turn with radius %0.3f\n", radius);
        return { };
    }

    double w = shortest_angle_diff(goal.yaw, start.yaw) + (straight_length / radius);
    double v = radius * w;
    double tl = straight_length / v;

    if (straight_length < 0) {
        DEBUG_PRINT("Not allowed to go backwards (length = %0.3f)\n", straight_length);
        return { };
    }

    if (v < 0) {
        DEBUG_PRINT("Not allowed to go backwards (velocity = %0.3f)\n", v);
        return { };
    }

    if (tl < 0.0 || tl > 1.0) {
        DEBUG_PRINT("Another dimension! Another dimension! (tl = %0.3f)\n", tl);
        return { };
    }

    DEBUG_PRINT("R = [ %0.3f, %0.3f; %0.3f, %0.3f]\n", R(0, 0), R(0, 1), R(1, 0), R(1, 1));
    DEBUG_PRINT("S = [%0.3f, %0.3f]\n", S(0), S(1));
    DEBUG_PRINT("straight_length = %0.3f\n", straight_length);
    DEBUG_PRINT("radius = %0.3f\n", radius);
    DEBUG_PRINT("w = %0.3f\n", w);
    DEBUG_PRINT("v = %0.3f\n", v);
    DEBUG_PRINT("tl = %0.3f\n", tl);

    const double res = 0.1;
    double arc_length = w * (1.0 - tl);
    double total_length = fabs(straight_length) + fabs(arc_length);

    const int num_samples = (int)std::ceil(total_length / res);
    std::vector<Pose2_cont> interm_poses;
    interm_poses.resize(num_samples);

    for (int i = 0; i < num_samples; ++i) {
        double dt = (double)i / (double)(num_samples - 1);
        if (dt < tl) {
            double x = start.x + v * dt * cos(start.yaw);
            double y = start.y + v * dt * sin(start.yaw);
            double theta = start.yaw;
            interm_poses[i] = { x, y, theta };
        }
        else {
            double x = start.x + straight_length * cos(start.yaw) + radius * sin(w * (dt - tl) + start.yaw) - radius * sin(start.yaw);
            double y = start.y + straight_length * sin(start.yaw) - radius * cos(w * (dt - tl) + start.yaw) + radius * cos(start.yaw);
            double theta = start.yaw + w * (dt - tl);
            interm_poses[i] = { x, y, theta };
        }
    }

    return interm_poses;
}