C++ vctDynamicVector类代码示例

// A is column major!
vctDynamicMatrix<double>
robManipulator::JSinertia( const vctDynamicVector<double>& q ) const {

  if( q.size() != links.size() ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": Expected " << links.size() << " values. "
                      << "Got " << q.size()
                      << std::endl;
    return vctDynamicMatrix<double>();
  }

  vctDynamicMatrix<double> A( links.size(), links.size(), 0.0 );

  for(size_t c=0; c<q.size(); c++){
    vctDynamicVector<double> qd( q.size(), 0.0 );  // velocities to zero
    vctDynamicVector<double> qdd( q.size(), 0.0 ); // accelerations to zero
    vctFixedSizeVector<double,6> fext(0.0);
    qdd[c] = 1.0;                                  // ith acceleration to 1

    vctDynamicVector<double> h = RNE( q, qd, qdd, fext, 0.0  );
    for( size_t r=0; r<links.size(); r++ )
      { A[c][r] = h[r]; }
  }

  return A;

}

osaCANBusFrame::osaCANBusFrame( osaCANBusFrame::ID id, 
			      const vctDynamicVector<osaCANBusFrame::Data>& data){

  // Clear up everything before starting
  this->id = 0;                              // default ID 
  this->nbytes = 0;                          // no data
  for(osaCANBusFrame::DataLength i=0; i<8; i++) // clear the data
    { this->data[i] = 0x00; }

  // A can ID has 11 bits. Ensure that only 11 bits are used
  if( (~0x07FF) & id ){
    CMN_LOG_RUN_WARNING << CMN_LOG_DETAILS
			<< ": Illegal CAN id: " << id 
			<< std::endl;
  }

  else{
    // Now check that no more than 8 bytes are given
    if( 8 < data.size() ){
      CMN_LOG_RUN_WARNING << CMN_LOG_DETAILS
			  << ": Illegal message length: " << data.size()
			  << std::endl;
    }

    else{
      this->id = (0x07FF & id);                         // Copy the CAN ID
      this->nbytes = data.size();                       // Copy the data length
      for(osaCANBusFrame::DataLength i=0; i<nbytes; i++) // Copy the data
	{ this->data[i] = data[i]; }
    }
  }

}

double cisstAlgorithmICP_RobustICP::ComputeEpsilon(vctDynamicVector<double> &sampleDist)
{
  unsigned int numSamps = sampleDist.size();

  double minDist = sampleDist.MinElement();
  double maxDist = sampleDist.MaxElement();

  unsigned int numBins = 16;
  double binWidth = (maxDist - minDist) / (double)numBins;

  // build histogram of match distances
  vctDynamicVector<unsigned int> bins(numBins, (unsigned int)0);
  unsigned int sampleBin;
  for (unsigned int i = 0; i < numSamps; i++)
  {
    if (sampleDist[i] == maxDist)
    { // handle max case
      sampleBin = numBins - 1;
    }
    else
    {
      sampleBin = (unsigned int)floor((sampleDist[i] - minDist) / binWidth);
    }

    bins(sampleBin)++;
  }

  // find histogram peak
  unsigned int peakBin = numBins;  // initialize to invalid bin
  unsigned int peakBinSize = 0;
  for (unsigned int i = 0; i < numBins; i++)
  {
    if (bins(i) >= peakBinSize)
    {
      peakBin = i;
      peakBinSize = bins(i);
    }
  }
  // find valley following peak
  //  (valley bin must be <= 60% of peak bin size)  
  double valleyThresh = 0.6 * (double)peakBinSize;
  unsigned int valleyBin = peakBin + 1;
  for (unsigned int i = peakBin + 1; i < numBins; i++)
  {
    if ((double)bins(i) <= valleyThresh)
    {
      break;
    }
    valleyBin = i + 1;
  }

  // set epsilon to the smallest distance in the valley bin
  double epsilon = minDist + valleyBin * binWidth;

  //printHistogram(bins, peakBin, valleyBin, minDist, maxDist, binWidth);

  return epsilon;
}

osaPIDAntiWindup::Errno
osaPIDAntiWindup::Evaluate
( const vctDynamicVector<double>& qs,
  const vctDynamicVector<double>& q,
  vctDynamicVector<double>& tau,
  double dt ){

    if( dt <= 0 ){
        std::cerr << "Invalid time increment: " << dt << std::endl;
        return osaPIDAntiWindup::EFAILURE;
    }
    
    if( qs.size() != Kp.size() ){
        std::cerr << "size(qs) = " << qs.size() << " "
                          << "N = "        << Kp.size() << std::endl;
        return osaPIDAntiWindup::EFAILURE;
    }
    
    if( q.size() != Kp.size() ){
        std::cerr << "size(q) = " << q.size() << " "
                          << "N = "       << Kp.size() << std::endl;
        return osaPIDAntiWindup::EFAILURE;
    }

    // error = desired - current
    vctDynamicVector<double> e = qs - q;

    // evaluate the velocity
    vctDynamicVector<double> qd = (q - qold)/dt;
    
    // error time derivative
    vctDynamicVector<double> ed = (e - eold)/dt;      
    
    // command with anti windup
    for( size_t i=0; i<commands.size(); i++ ){
        I[i] += ( Ki[i]*e[i] + Kt[i]*(outputs[i]-commands[i]) ) * dt;
        commands[i] = Kp[i]*e[i] + Kd[i]*ed[i] + I[i];
    }

    // set the output to the command
    outputs = commands;

    // saturate the output
    for( size_t i=0; i<outputs.size(); i++ ){
        if( outputs[i] < -limits[i] ) { outputs[i] = -limits[i]; }
        if( limits[i]  < outputs[i] ) { outputs[i] =  limits[i]; }
    }
    
    tau = outputs;

    eold = e;
    qold = q;
    
    return osaPIDAntiWindup::ESUCCESS;
    
}

void Camera::getFrame(vctDynamicVector<vct3> & points) {

	sm = PXCSenseManager::CreateInstance();
	PXCCaptureManager *cm = sm->QueryCaptureManager();

	sm->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, 640, 480, 30);

	pxcStatus sts = sm->Init();
	if (sts < PXC_STATUS_NO_ERROR) {
		std::cout << "DIDN\'T WORK" << std::endl;
	}

	PXCCapture::Device *device = sm->QueryCaptureManager()->QueryDevice();
	device->ResetProperties(PXCCapture::STREAM_TYPE_ANY);

	std::cout << device->SetDepthUnit(1000) << std::endl;

	PXCProjection *projection = device->CreateProjection();


	pxcStatus sts2 = sm->AcquireFrame(false);
	if (sts2 >= PXC_STATUS_NO_ERROR) {
		PXCCapture::Sample *sample = sm->QuerySample();
		PXCImage* image = (*sample)[streamType];
		PXCImage::ImageInfo info = {};
		PXCImage::ImageData data;
		image->AcquireAccess(PXCImage::ACCESS_READ, &data);
		PXCImage::ImageInfo dinfo = sample->depth->QueryInfo();
		int dpitch = data.pitches[0] / sizeof(short);
		short *dpixels = (short*)data.planes[0];
		int i = 0;
		points.SetSize(dinfo.width * dinfo.height);

		PXCPoint3DF32 * vertices = new PXCPoint3DF32[dinfo.width * dinfo.height];
		projection->QueryVertices(image, vertices);

		int c = 0;
		for (int i = 0; i < points.size(); i++) {
			PXCPoint3DF32 point = vertices[i];
			if (point.z != 0) {
				vct3 newPoint(point.x, point.y, point.z);
				points[c++] = newPoint;
			}
		}
		points.resize(c);
		image->ReleaseAccess(&data);
	}
	projection->Release();
	std::cout << sts2 << std::endl;
}

osaPDGC::osaPDGC( const std::string& robfile,
                  const vctFrame4x4<double>& Rtw0,
                  const vctDynamicMatrix<double>& Kp,
                  const vctDynamicMatrix<double>& Kd,
                  const vctDynamicVector<double>& qinit ):
    robManipulator( robfile, Rtw0 ),
    Kp( Kp ),
    Kd( Kd ),
    qold( qinit ),
    eold( qinit.size(), 0.0 ) {

    if( Kp.rows() != links.size() || Kp.cols() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(Kp) = [" << Kp.rows()
                          << ", "           << Kp.cols() << " ] "
                          << "NxN = ["      << links.size()
                          << ", "           << links.size() << " ]"
                          << std::endl;
    }

    if( Kd.rows() != links.size() || Kd.cols() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(Kd) = [" << Kd.rows()
                          << ", "           << Kd.cols() << " ] "
                          << "NxN = ["      << links.size()
                          << ", "           << links.size() << " ]"
                          << std::endl;
    }

    if( qold.size() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(qold) = " << qold.size() << " "
                          << "N = "          << links.size() << std::endl;
    }

}

osaPIDAntiWindup::osaPIDAntiWindup( const vctDynamicVector<double>& Kp,
                                    const vctDynamicVector<double>& Ki,
                                    const vctDynamicVector<double>& Kd,
                                    const vctDynamicVector<double>& Kt,
                                    const vctDynamicVector<double>& limits,
                                    const vctDynamicVector<double>& qinit ) :
    Kp( Kp ),
    Ki( Ki ),
    Kd( Kd ),
    Kt( Kt ),
    I( Ki.size(), 0.0 ),
    commands( limits.size(), 0.0 ),
    outputs( limits.size(), 0.0 ),
    limits( limits.size(), 0.0 ),
    qold( qinit ),
    eold( qinit.size(), 0.0 ){

    if( this->Kp.size() != this->Kd.size() || 
        this->Kp.size() != this->Ki.size() || 
        this->Kp.size() != this->Kt.size() ){
        std::cerr << "Size mismatch:" 
                          << " size(Kp) = " << this->Kp.size()
                          << " size(Ki) = " << this->Ki.size()
                          << " size(Kd) = " << this->Kd.size()
                          << " size(Kt) = " << this->Kt.size()
                          << std::endl;
    }

    if( this->Kp.size() != this->limits.size() ){
        std::cerr << "Size mismatch: initializing " << this->Kp.size() 
                          << " controllers with " << this->limits.size() 
                          << " limit values."
                          << std::endl;
    }

    if( this->Kp.size() != this->qold.size() ){
        std::cerr << "Size mismatch: initializing " << this->Kp.size() 
                          << " controllers with " << this->qold.size() 
                          << " values."
                          << std::endl;
    }

    for( size_t i=0; i<this->limits.size(); i++ )
        { this->limits[i] = fabs( limits[i] ); }

}

vctDynamicVector<double>
robManipulator::CCG( const vctDynamicVector<double>& q,
                     const vctDynamicVector<double>& qd ) const {

  if( q.size() != qd.size() ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": Size of q and qd do not match."
                      << std::endl;
    return vctDynamicVector<double>();
  }



  return RNE( q,           // call Newton-Euler with only the joints positions
          qd,          // and the joints velocities
              vctDynamicVector<double>( q.size(), 0.0 ),
              vctFixedSizeVector<double,6>(0.0) );
}

void cisstAlgorithmICP_IMLP::SetSampleCovariances(
  vctDynamicVector<vct3x3> &Mi,
  vctDynamicVector<vct3x3> &MsmtMi)
{
  if (Mi.size() != nSamples || MsmtMi.size() != nSamples)
  {
    std::cout << "ERROR: number of covariances matrices does not match number of samples" << std::endl;
  }

  Mxi.SetSize(nSamples);
  eigMxi.SetSize(nSamples);
  for (unsigned int s = 0; s<nSamples; s++)
  {
    Mxi[s] = Mi[s];
    ComputeCovEigenValues_SVD(Mi[s], eigMxi[s]);
  }

  this->MsmtMxi = MsmtMi;
}

osaPDGC::Errno
osaPDGC::Evaluate
( const vctDynamicVector<double>& qs,
  const vctDynamicVector<double>& q,
  vctDynamicVector<double>& tau,
  double dt ) {

    if( qs.size() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(qs) = " << qs.size() << " "
                          << "N = "        << links.size() << std::endl;
        return osaPDGC::EFAILURE;
    }

    if( q.size() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(q) = " << q.size() << " "
                          << "N = "       << links.size() << std::endl;
        return osaPDGC::EFAILURE;
    }

    // evaluate the velocity
    vctDynamicVector<double> qd( links.size(), 0.0 );
    if( 0 < dt ) qd = (q - qold)/dt;

    // error = current - desired
    vctDynamicVector<double> e = q - qs;

    // error time derivative
    vctDynamicVector<double> ed( links.size(), 0.0 );
    if( 0 < dt ) ed = (e - eold)/dt;

    // Compute the coriolis+gravity load
    vctDynamicVector<double> ccg =
        CCG( q, vctDynamicVector<double>( links.size(), 0.0 ) );

    tau = ccg - Kp*e - Kd*ed;

    eold = e;
    qold = q;

    return osaPDGC::ESUCCESS;

}

void prmJointTypeToFactor(const vctDynamicVector<prmJointType> & types,
                          const double prismaticFactor,
                          const double revoluteFactor,
                          vctDynamicVector<double> & factors)
{
    // set unitFactor;
    for (size_t i = 0; i < types.size(); i++) {
        switch (types.at(i)) {
        case PRM_JOINT_PRISMATIC:
            factors.at(i) = prismaticFactor;
            break;
        case PRM_JOINT_REVOLUTE:
            factors.at(i) = revoluteFactor;
            break;
        default:
            factors.at(i) = 1.0;
            break;
        }
    }
}

vctFrame4x4<double>
robManipulator::ForwardKinematics( const vctDynamicVector<double>& q,
                                   int N ) const {

  if( N == 0 ) return Rtw0;

  // if N < 0 then we want the end-effector
  if( N < 0 ) N = links.size();

  if( ((int)q.size()) < N ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": Expected " << N << " joint positions but "
                      << "size(q) = " << q.size() << "."
                      << std::endl;
    return Rtw0;
  }

  // no link? then return the transformation of the base
  if( links.empty() ){
    //CMN_LOG_RUN_WARNING << CMN_LOG_DETAILS
    //                << ": Manipulator has no link."
    //                        << std::endl;
    return Rtw0;
  }

  // set the position/orientation of link 0 to the base * its tranformation
  // setting the link's transformation is necessary in order to render the link
  // in opengl
  vctFrame4x4<double> Rtwi = Rtw0*links[0].ForwardKinematics( q[0] );

  // for link 1 to N
  for(int i=1; i<N; i++)
    Rtwi = Rtwi * links[i].ForwardKinematics( q[i] );

  if( tools.size() == 1 ){
    if( tools[0] != NULL )
      { return Rtwi * tools[0]->ForwardKinematics( q, 0 ); }
  }

  return Rtwi;
}

nmrSymmetricEigenProblem::Data::Data( vctDynamicMatrix<double>& A,
				      vctDynamicVector<double>& D,
				      vctDynamicMatrix<double>& V ) :
  JOBZ( 'V' ),
  RANGE( 'A' ),
  UPLO( 'U' ),

  N( A.rows() ),
  A( A.Pointer() ),
  LDA( A.cols() ),

  VL( 0 ),
  VU( 0 ),
  IL( 0 ),
  IU( 0 ),

  DLAMCH( 'S' ),
  ABSTOL( dlamch_( &DLAMCH ) ),
  
  W( D.Pointer() ),
  Z( V.Pointer() ),
  LDZ( V.cols() ),
  
  ISUPPZ( new CISSTNETLIB_INTEGER[ 2*N ] ),
  WORK( NULL ),
  LWORK( -1 ),

  IWORK( NULL ),
  LIWORK( -1 ){

  CheckSystem( A, D, V );

  CISSTNETLIB_DOUBLE work;
  CISSTNETLIB_INTEGER iwork;

  dsyevr_( &JOBZ, &RANGE, &UPLO,
	   &N, this->A, &LDA, 
	   &VL, &VU, 
	   &IL, &IU, 
	   &ABSTOL, 
	   &M, W, 
	   Z, &LDZ, ISUPPZ,
	   &work, &LWORK,
	   &iwork, &LIWORK, 
	   &INFO );

  LWORK = work;
  WORK = new CISSTNETLIB_DOUBLE[LWORK];

  LIWORK = iwork;
  IWORK = new CISSTNETLIB_INTEGER[LIWORK];
  
}

void 
nmrSymmetricEigenProblem::Data::CheckSystem
( const vctDynamicMatrix<double>& A,
  const vctDynamicVector<double>& D,
  const vctDynamicMatrix<double>& V ){

  // test that number of rows match
  if( A.rows() != A.cols() ){
    std::ostringstream message;
    message << "nmrSymmetricEigenProblem: Matrix A has " << A.rows() << " rows "
	    << " and "  << A.cols() << " columns.";
    cmnThrow( std::runtime_error( message.str() ) );
  }

  // check for fortran format
  if( !A.IsFortran() ){
    std::string message( "nmrSymmetricEigenProblem: Invalid matrix A format." );
    cmnThrow( std::runtime_error( message ) );
  }

  // test that number of rows match
  if( V.rows() != A.rows() || V.cols() != A.cols() ){
    std::ostringstream message;
    message << "nmrSymmetricEigenProblem: Matrix V has " << V.rows() << " rows "
	    << " and "  << V.cols() << " columns.";
    cmnThrow( std::runtime_error( message.str() ) );
  }

  if( !V.IsFortran() ){
    std::string message( "nmrSymmetricEigenProblem: Invalid matrix V format." );
    cmnThrow( std::runtime_error( message ) );
  }

  if( D.size() != A.rows() ){
    std::ostringstream message;
    message << "nmrSymmetricEigenProblem: Vector D has " << D.size() << " rows";
    cmnThrow( std::runtime_error( message.str() ) );
  }

}

void robManipulator::JSinertia( double **A,
                                const vctDynamicVector<double>& q ) const {

  if( q.size() != links.size() ){
    std::cerr << "robManipulator::JSinertia: Expected " << links.size()
              << " values. Got " << q.size()
              << std::endl;
    return;
  }

  for(size_t c=0; c<links.size(); c++){
    vctDynamicVector<double> qd( q.size(), 0.0 ); // velocities to zero
    vctDynamicVector<double> qdd(q.size(), 0.0 ); // accelerations to zero
    vctFixedSizeVector<double,6> fext(0.0);
    qdd[c] = 1.0;                                 // ith acceleration to 1

    vctDynamicVector<double> h = RNE( q, qd, qdd, fext, 0  );
    for( size_t r=0; r<links.size(); r++ )
      A[c][r] = h[r];
  }

}