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;
}
}
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];
}
vctDynamicMatrix<double> nmrLSMinNorm( vctDynamicMatrix<double>& vctA,
vctDynamicMatrix<double>& vctb,
nmrLSMinNorm::Data& data,
CISSTNETLIB_DOUBLE rcond ){
// data pointers
CISSTNETLIB_DOUBLE* A = vctA.Pointer();
CISSTNETLIB_DOUBLE* B = vctb.Pointer();
// check if we need to reallocate data
if( data.M != int(vctA.rows()) ||
data.N != int(vctA.cols()) ||
data.NRHS != int(vctb.cols()) ){
if( data.S != NULL ) { delete[] data.S; }
if( data.WORK != NULL ) { delete[] data.WORK; }
data = nmrLSMinNorm::Data( vctA, vctb, rcond );
}
// copy the data for underdetermined systems
if( data.underdetermined ){ B = data.B.Pointer(); }
// solve the LS with minimum norm
dgelss_( &data.M, &data.N, &data.NRHS,
&A[0], &data.LDA,
&B[0], &data.LDB,
&data.S[0], &data.RCOND, &data.RANK,
&data.WORK[0], &data.LWORK, &data.INFO );
data.CheckInfo();
// Assign???
vctDynamicMatrix<double> vctx( data.N, data.NRHS, VCT_COL_MAJOR );
if( data.underdetermined ){
for( int r=0; r<data.N; r++ ){
for( int c=0; c<data.NRHS; c++ ){
vctx[r][c] = data.B[r][c];
}
}
}
else{
for( int r=0; r<data.N; r++ ){
for( int c=0; c<data.NRHS; c++ ){
vctx[r][c] = vctb[r][c];
}
}
}
return vctx;
}
bool robManipulator::JacobianSpatial(const vctDynamicVector<double>& q,
vctDynamicMatrix<double>& J) const
{
JacobianSpatial(q);
if ((J.rows() != 6) && (J.cols() != links.size()))
return false;
for (size_t r = 0; r < 6; r++)
for (size_t c = 0; c < links.size(); c++)
J.Element(r,c) = Js[c][r];
return true;
}
nmrLSMinNorm::Data::Data( const vctDynamicMatrix<double>& A,
const vctDynamicMatrix<double>& b,
double rcond ) :
M( A.rows() ),
N( A.cols() ),
NRHS( b.cols() ),
LDA( M ),
LDB( (M<N) ? N : M ),
S( new CISSTNETLIB_DOUBLE[ (M<N) ? M : N ] ),
RCOND( rcond ),
WORK( NULL ),
LWORK( -1 ), // -1 to determined the optimal work space size
INFO( 0 ),
underdetermined( (M<N) ? true : false ){
CheckSystem( A, b );
// this call determines the optimal work space size
CISSTNETLIB_DOUBLE work[1]; // size will be here
dgelss_( &M, &N, &NRHS,
NULL, &LDA,
NULL, &LDB,
&S[0], &RCOND, &RANK,
&work[0], &LWORK, &INFO );
LWORK = work[0]; // copy the work space size
WORK = new CISSTNETLIB_DOUBLE[LWORK]; // allocate the work space
// if system is underdetermined (M<N) we need a larger b matrix
if( underdetermined ){
B.SetSize( LDB, NRHS, VCT_COL_MAJOR );
for( int r=0; r<M; r++ ){
for( int c=0; c<NRHS; c++ ){
B[r][c] = b[r][c];
}
}
}
}
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::AddIdentificationColumn
( vctDynamicMatrix<double>& J,
vctFixedSizeMatrix<double,4,4>& delRt ) const{
size_t c = J.cols();
J.resize( 6, c+1 );
J[0][c] = delRt[0][3]; // dx
J[1][c] = delRt[1][3]; // dy
J[2][c] = delRt[2][3]; // dz
J[3][c] = (delRt[2][1] - delRt[1][2])/2.0; // average wx
J[4][c] = (delRt[0][2] - delRt[2][0])/2.0; // average wy
J[5][c] = (delRt[1][0] - delRt[0][1])/2.0; // average wz
}
osaOpenNI::Errno
osaOpenNI::Convert3DToProjectiveMask(const vctDynamicMatrix<double>& rangedata,
vctDynamicMatrix<bool>& mask) {
// allocate the arrays
XnUInt32 cnt = rangedata.cols();
XnPoint3D* wrld = new XnPoint3D[ cnt ];
XnPoint3D* proj = new XnPoint3D[ cnt ];
// copy the 3d points to the 3d array
for (XnUInt32 i=0; i<cnt; i++) {
wrld[i].X = -rangedata[0][i]*1000.0;
wrld[i].Y = rangedata[1][i]*1000.0;
wrld[i].Z = rangedata[2][i]*1000.0;
}
// convert to projective
XnStatus status;
status = Data->depthgenerator.ConvertRealWorldToProjective(cnt, wrld, proj);
if (status != XN_STATUS_OK) {
CMN_LOG_RUN_ERROR << "Failed to convert world to projective: "
<< xnGetStatusString(status) << std::endl;
return osaOpenNI::EFAILURE;
}
// use this to find the size of the mask
xn::DepthMetaData depthMD;
Data->depthgenerator.GetMetaData(depthMD);
mask.SetSize(depthMD.YRes(), depthMD.XRes());
mask.SetAll(false);
for (XnUInt32 i=0; i<cnt; i++) {
mask[ proj[i].Y ][ proj[i].X ] = true;
}
delete[] wrld;
delete[] proj;
return osaOpenNI::ESUCCESS;
}