// 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]; } } } }
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; }
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] ); } }
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; }
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; }
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; }
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]; } }
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; }
void Run(){ ProcessQueuedCommands(); prmPositionJointGet qin; GetPositions( qin ); prmPositionJointSet qout; qout.SetSize( 7 ); qout.Goal() = q; SetPositions( qout ); for( size_t i=0; i<q.size(); i++ ) q[i] += 0.001; }
void cisstAlgorithmICP_RobustICP::printHistogram( vctDynamicVector<unsigned int> bins, unsigned int peakBin, unsigned int valleyBin, double minDist, double maxDist, double binWidth) { std::stringstream ss; unsigned int numBins = bins.size(); ss << std::endl << "Match Distance Histogram:" << std::endl; ss << "minDist: " << minDist << std::endl; ss << "maxDist: " << maxDist << std::endl; ss << std::endl; // bin header ss << " bins | size | dist " << std::endl; for (unsigned int i = 0; i < numBins; i++) { ss << " "; ss.width(4); ss << i; ss << " | "; ss.width(4); ss << bins(i); ss << " | " << minDist + i*binWidth << " "; if (i == peakBin) { ss << " <--- peak bin"; } if (i == valleyBin) { ss << " <--- valley bin"; } ss << std::endl; } ss << std::endl; //// bin contents //ss << " "; //for (unsigned int i = 0; i < numBins; i++) //{ // ss << "| "; ss.width(4); ss << bins(i) << " "; //} //ss << std::endl; std::cout << ss.str() << std::endl; }
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; } } }
void Run(){ ProcessQueuedCommands(); if( IsEnabled() ){ if( !online ){ online = true; for( size_t i=0; i<qready.size(); i++ ){ if( 0.015 < fabs( q[i] - qready[i] ) ){ online = false; if( q[i] < qready[i] ) { q[i] += 0.0005; } else if( qready[i] < q[i] ) { q[i] -= 0.0005; } } } std::cout << q << std::endl; // this goes to the ikin mtsFrm4x4 mtsRt( manipulator->ForwardKinematics( q ) ); SetPositionCartesian( mtsRt ); // this goes to the teleop prmTelemetry.Position().FromRaw( mtsRt ); prmTelemetry.SetValid( true ); if( online ) { std::cout << "System is online. 'q' to quit" << std::endl; } } else{ bool valid = false; prmCommand.GetValid( valid ); if( valid ){ vctFrm3 frm3Rt; prmCommand.GetPosition( frm3Rt ); // this goes to the ikin vctQuaternionRotation3<double> R( frm3Rt.Rotation(), VCT_NORMALIZE ); mtsFrm4x4 mtsRt( vctFrame4x4<double>( R, frm3Rt.Translation() ) ); SetPositionCartesian( mtsRt ); // return the same value to the telop prmTelemetry.Position().FromRaw( mtsRt ); prmTelemetry.SetValid( true ); } } } }
void GenerateSamplePointSet_PointCloud_SurfaceNoise( cisstMesh &mesh, TestParameters ¶ms, unsigned int randSeed, unsigned int &randSeqPos, std::ifstream &randnStream, vctDynamicVector<vct3> &samples, vctDynamicVector<vct3> &sampleNorms, vctDynamicVector<vct3> &noisySamples, unsigned int trialNum) { // get custom params CustomTestParams_PointCloud_SurfaceNoise *pCustomParams = dynamic_cast<CustomTestParams_PointCloud_SurfaceNoise*>(params.pCustomTestParams); std::stringstream saveSamplesPath; std::stringstream saveNoisySamplesPath; std::stringstream saveNoiseCovPath; saveSamplesPath << params.outputCommonDir << "/SaveSamples_" << trialNum << ".pts"; saveNoisySamplesPath << params.outputCommonDir << "/SaveNoisySamples_" << trialNum << ".pts"; saveNoiseCovPath << params.outputCommonDir << "/SaveNoiseCov_" << trialNum << ".txt"; std::string strSaveSamplesPath = saveSamplesPath.str(); std::string strSaveNoisySamplesPath = saveNoisySamplesPath.str(); std::string strSaveNoiseCovPath = saveNoiseCovPath.str(); std::string *pSaveSamplesPath = &strSaveSamplesPath; std::string *pSaveNoisySamplesPath = &strSaveNoisySamplesPath; std::string *pSaveNoiseCovPath = &strSaveNoiseCovPath; #ifdef DISABLE_OUTPUT_FILES pSaveSamplesPath = NULL; pSaveNoisySamplesPath = NULL; pSaveNoiseCovPath = NULL; #endif vctDynamicVector<unsigned int> sampleDatums(params.nSamples); vctDynamicVector<vct3x3> sampleCov(params.nSamples, vct3x3(0.0)); vctDynamicVector<vct3x3> noiseCov(params.nSamples, vct3x3(0.0)); vctDynamicVector<vct3x3> surfaceModelCov(params.nSamples, vct3x3(0.0)); //vctDynamicVector<vct3x3> noiseInvCov(params.nSamples, vct3x3(0.0)); // Generate Samples GenerateSamples( mesh, randSeed, randSeqPos, params.nSamples, samples, sampleNorms, sampleDatums, pSaveSamplesPath); // Generate Sample Noise GenerateSampleErrors_SurfaceNoise( randSeed, randSeqPos, randnStream, pCustomParams->sampleNoise_InPlaneSD, pCustomParams->sampleNoise_PerpPlaneSD, samples, sampleNorms, noisySamples, noiseCov, //noiseInvCov, pCustomParams->percentOutliers, pCustomParams->minPosOffsetOutlier, pCustomParams->maxPosOffsetOutlier, pSaveNoisySamplesPath, pSaveNoiseCovPath); // Set Samples in Algorithm switch (params.algType) { case TestParameters::StdICP: { // configure algorithm samples cisstAlgorithmICP_StdICP *pAlg = dynamic_cast<cisstAlgorithmICP_StdICP*>(params.pAlg); pAlg->SetSamples(noisySamples); break; } case TestParameters::IMLP: { std::stringstream saveSampleCovPath; std::stringstream saveSurfaceModelCovPath; saveSampleCovPath << params.outputDataDir << "/SaveSampleCov_" << trialNum << ".txt"; saveSurfaceModelCovPath << params.outputDataDir << "/SaveSurfaceModelCov_" << trialNum << ".txt"; std::string strSaveSampleCovPath = saveSampleCovPath.str(); std::string strSaveSurfaceModelCovPath = saveSurfaceModelCovPath.str(); std::string *pSaveSampleCovPath = &strSaveSampleCovPath; std::string *pSaveSurfaceModelCovPath = &strSaveSurfaceModelCovPath; #ifdef MINIMIZE_OUTPUT_FILES pSaveSampleCovPath = NULL; pSaveSurfaceModelCovPath = NULL; #endif #ifdef DISABLE_OUTPUT_FILES pSaveSampleCovPath = NULL; pSaveSurfaceModelCovPath = NULL; #endif unsigned int nSamps = samples.size(); // Set Sample Covariances // noise covariance if (pCustomParams->bSampleCov_ApplyNoiseModel) { for (unsigned int i = 0; i < nSamps; i++) { sampleCov(i) += noiseCov(i); } } // surface model covariance if (pCustomParams->bSampleCov_ApplySurfaceModel) { // compute surface model ComputeCovariances_SurfaceModel( pCustomParams->surfaceModel_InPlaneSD, pCustomParams->surfaceModel_PerpPlaneSD, samples, sampleNorms, surfaceModelCov, pSaveSurfaceModelCovPath); // add suface model to sample covariances for (unsigned int i = 0; i < nSamps; i++) { sampleCov(i) += surfaceModelCov(i); } } // save sample covariances if (pSaveSampleCovPath) { WriteToFile_Cov(sampleCov, *pSaveSampleCovPath); } // configure algorithm samples cisstAlgorithmICP_IMLP *pAlg = dynamic_cast<cisstAlgorithmICP_IMLP*>(params.pAlg); //cisstAlgorithmICP_IMLP_PointCloud *pAlg = dynamic_cast<cisstAlgorithmICP_IMLP_PointCloud*>(params.pAlg); if (!pAlg) { std::cerr << "ERROR: algorithm class unrecognized class" << std::endl; } //pAlg->SetSamples(noisySamples); //pAlg->SetSampleCovariances(sampleCov); pAlg->SetSamples(noisySamples); pAlg->SetSampleCovariances(sampleCov, noiseCov); break; } case TestParameters::RobustICP: { // configure algorithm samples cisstAlgorithmICP_RobustICP *pAlg = dynamic_cast<cisstAlgorithmICP_RobustICP*>(params.pAlg); pAlg->SetSamples(noisySamples); break; } //case TestParameters::CPD: //{ // break; //} default: std::cout << std::endl << "=====> ERROR: Algorithm Type not Recognized by Sample Generator" << std::endl << std::endl; assert(0); break; } // switch AlgType }
robManipulator::Errno robManipulator::InverseKinematics( vctDynamicVector<double>& q, const vctFrame4x4<double>& Rts, double tolerance, size_t Niterations, double LAMBDA ){ if( q.size() != links.size() ){ CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS << ": Expected " << links.size() << " joints values. " << " Got " << q.size() << std::endl; return robManipulator::EFAILURE; } if( links.size() == 0 ){ CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS << ": The manipulator has no links." << std::endl; return robManipulator::EFAILURE; } // A is a pointer to the 6xN spatial Jacobian integer M = 6; // The number or rows of matrix A integer N = links.size(); // The number of columns of matrix A integer NRHS = 1; // The number of right hand side vectors integer LDA = M; // The leading dimension of the array A. // B is a pointer the the N vector containing the solution doublereal* B; if( N < 6 ) { B = new doublereal[6]; } // The N-by-NRHS matrix of else { B = new doublereal[N]; } integer LDB = N; // The leading dimension of the array B. // These values are used for the SVD computation integer INFO; // The info code integer INC = 1; // The index increment doublereal ndq=1; // norm of the iteration error size_t i; // the iteration counter char TRANSN = 'N'; // "N"ormal char TRANST = 'T'; // "T"transpose doublereal ALPHA = 1.0; doublereal* dq = new doublereal[N]; // loop until Niter are executed or the error is bellow the tolerance for( i=0; i<Niterations && tolerance<ndq; i++ ){ // Evaluate the forward kinematics vctFrame4x4<double,VCT_ROW_MAJOR> Rt = ForwardKinematics( q ); // Evaluate the spatial Jacobian (also evaluate the forward kin) JacobianSpatial( q ); // compute the translation error vctFixedSizeVector<double,3> dt( Rts[0][3]-Rt[0][3], Rts[1][3]-Rt[1][3], Rts[2][3]-Rt[2][3] ); // compute the orientation error // first build the [ n o a ] vectors vctFixedSizeVector<double,3> n1( Rt[0][0], Rt[1][0], Rt[2][0] ); vctFixedSizeVector<double,3> o1( Rt[0][1], Rt[1][1], Rt[2][1] ); vctFixedSizeVector<double,3> a1( Rt[0][2], Rt[1][2], Rt[2][2] ); vctFixedSizeVector<double,3> n2( Rts[0][0], Rts[1][0], Rts[2][0] ); vctFixedSizeVector<double,3> o2( Rts[0][1], Rts[1][1], Rts[2][1] ); vctFixedSizeVector<double,3> a2( Rts[0][2], Rts[1][2], Rts[2][2] ); // This is the orientation error vctFixedSizeVector<double,3> dr = 0.5*( (n1%n2) + (o1%o2) + (a1%a2) ); // combine both errors in one R^6 vector doublereal e[6] = { dt[0], dt[1], dt[2], dr[0], dr[1], dr[2] }; // for( integer j=0; j<N; j++ ) { B[j] = 0.0; } for( integer j=0; j<6; j++ ) { B[j] = e[j]; } // weights doublereal I[6][6] = { { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0 }, { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 }, { 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 } }; // We need to solve dq = J' ( JJ' + lambda I )^-1 e // I = JJ' + lambda I gemm( &TRANSN, &TRANST, &M, &M, &N, &ALPHA, &Js[0][0], &LDA, &Js[0][0], &LDA, &LAMBDA, &(I[0][0]), &M ); // solve B = I\B integer IPIV[6]; LDB=6; gesv( &M, &NRHS, &(I[0][0]), &LDA, &(IPIV[0]), &(B[0]), &LDB, &INFO ); // should check the pivots // dq = J'B doublereal GAMMA = 0.0; gemv( &TRANST, &M, &N, &ALPHA, &(Js[0][0]), &LDA, &(B[0]), &INC, &GAMMA, dq, &INC ); // compute the L2 norm of the error ndq = nrm2(&N, dq, &INC); // update the solution for(size_t j=0; j<links.size(); j++) q[j] += dq[j]; } NormalizeAngles(q); delete[] B; delete[] dq; if( i==Niterations ) return robManipulator::EFAILURE; else return robManipulator::ESUCCESS; }
robManipulator::Errno robManipulator::InverseKinematics( vctDynamicVector<double>& q, const vctFrame4x4<double>& Rts, double tolerance, size_t Niterations ){ if( q.size() != links.size() ){ CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS << ": Expected " << links.size() << " joints values. " << " Got " << q.size() << std::endl; return robManipulator::EFAILURE; } if( links.size() == 0 ){ CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS << ": The manipulator has no links." << std::endl; return robManipulator::EFAILURE; } // A is a pointer to the 6xN spatial Jacobian int M = 6; // The number of rows of matrix A int N = links.size(); // The number of columns of matrix A int NHRS = 1; // The number of right hand side vectors double* A = &(Js[0][0]); // Pointer to the spatial Jacobian int LDA = M; // The leading dimension of the array A. // B is a pointer the the N vector containing the solution double* B; // The N-by-NRHS matrix of right hand side matrix int LDB; // The leading dimension of the array B. if( N < 6 ) { B = new double[6]; LDB = 6; } else { B = new double[N]; LDB = N; } // These values are used for the SVD computation double* S = new double[M]; // The singular values of A in decreasing order double RCOND = -1; // Use machine precision to determine rank(A) int RANK; // The effective rank of A int LWORK = 128; // The (safe) size of the workspace double WORK[128]; // The workspace int INFO; // The info code int INC = 1; // The index increment double ndq=1; // norm of the iteration error size_t i = 0; // the iteration counter // loop until Niter are executed or the error is bellow the tolerance for( i=0; i<Niterations && tolerance<ndq; i++ ){ // Evaluate the forward kinematics vctFrame4x4<double,VCT_ROW_MAJOR> Rt( ForwardKinematics( q ) ); // Evaluate the spatial Jacobian (also evaluate the forward kin) JacobianSpatial( q ); // compute the translation error vctFixedSizeVector<double,3> dt( Rts[0][3]-Rt[0][3], Rts[1][3]-Rt[1][3], Rts[2][3]-Rt[2][3] ); // compute the orientation error // first build the [ n o a ] vectors vctFixedSizeVector<double,3> n1( Rt[0][0], Rt[1][0], Rt[2][0] ); vctFixedSizeVector<double,3> o1( Rt[0][1], Rt[1][1], Rt[2][1] ); vctFixedSizeVector<double,3> a1( Rt[0][2], Rt[1][2], Rt[2][2] ); vctFixedSizeVector<double,3> n2( Rts[0][0], Rts[1][0], Rts[2][0] ); vctFixedSizeVector<double,3> o2( Rts[0][1], Rts[1][1], Rts[2][1] ); vctFixedSizeVector<double,3> a2( Rts[0][2], Rts[1][2], Rts[2][2] ); // This is the orientation error vctFixedSizeVector<double,3> dr = 0.5*( (n1%n2) + (o1%o2) + (a1%a2) ); // combine both errors in one R^6 vector double e[6] = { dt[0], dt[1], dt[2], dr[0], dr[1], dr[2] }; // get a pointer for( int j=0; j<N; j++ ) { B[j] = 0.0; } for( int j=0; j<6; j++ ) { B[j] = e[j]; } // compute the minimum norm solution gelss( &M, &N, &NHRS, // 6xN matrix A, &LDA, // Jacobian matrix B, &LDB, // error vector S, &RCOND, &RANK, // SVD parameters WORK, &LWORK, &INFO ); // process the errors (if any) if(INFO < 0) CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS << ": the i-th argument of gelss is illegal." << std::endl; if(0 < INFO) CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS << ": gelss failed to converge." << std::endl; // compute the L2 norm of the error ndq = nrm2(&N, B, &INC); // update the solution for(size_t j=0; j<links.size(); j++) q[j] += B[j]; } NormalizeAngles(q); delete[] S; delete[] B; std::cerr << "Nb iter " << i << "/" << Niterations << std::endl; if( i==(Niterations-1) ) return robManipulator::EFAILURE; else return robManipulator::ESUCCESS; }