// 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;
}
BHMotion() : mtsTaskPeriodic( "BHMotion", 0.001, true ){
q.SetSize(4);
q.SetAll(0.0);
mtsInterfaceRequired* input = AddInterfaceRequired( "Input" );
mtsInterfaceRequired* output = AddInterfaceRequired( "Output" );
input->AddFunction( "GetPositionJoint", GetPositions );
output->AddFunction( "SetPositionJoint", SetPositions );
}
WAMMotion( double period ) :
mtsTaskPeriodic( "WAMMotion", period, true ){
q.SetSize(7);
q.SetAll(0.0);
mtsInterfaceRequired* input = AddInterfaceRequired( "Input" );
mtsInterfaceRequired* output = AddInterfaceRequired( "Output" );
input->AddFunction( "GetPositionJoint", GetPositions );
output->AddFunction( "SetPositionJoint", SetPositions );
}
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];
}
}
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;
}
vctDynamicVector<double>
robManipulator::InverseDynamics( const vctDynamicVector<double>& q,
const vctDynamicVector<double>& qd,
const vctDynamicVector<double>& qdd ) const {
vctDynamicVector<double> ccg = CCG(q, qd);
doublereal* Inertterm = new doublereal[ links.size() ];
for( size_t i=0; i<links.size(); i++ )
Inertterm [i] = 0;
// Ensure there's an acceleration. Orthewise only consider ccg
if( 0.0 < qdd.Norm() ){
char LOW = 'L';
integer N = links.size();
integer LDA = links.size();
integer INC = 1;
doublereal ALPHA = 1.0;
doublereal BETA = 0.0;
doublereal** A = rmatrix(0, links.size()-1, 0, links.size()-1);
JSinertia(A,q);
// must copy to an array for symv
doublereal* qddv = new doublereal[ links.size() ];
for( size_t i=0; i<links.size(); i++ ) qddv[i] = qdd[i];
symv(&LOW, &N,
&ALPHA, &A[0][0], &LDA,
qddv, &INC,
&BETA, Inertterm, &INC);
free_rmatrix(A, 0, 0);
delete[] qddv;
}
// add inertia+coriolis+grav
vctDynamicVector<double> trq( links.size(), 0.0 );
for( size_t i=0; i<links.size(); i++ )
trq[i] = Inertterm[i] + ccg[i];
delete[] Inertterm;
return trq;
}
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;
}
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 );
}
}
}
}
