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::Errno
nmrSymmetricEigenProblem
( vctDynamicMatrix<double>& A,
vctDynamicVector<double>& D,
vctDynamicMatrix<double>& V,
nmrSymmetricEigenProblem::Data& data ){
// check if we need to reallocate data
if( ( data.JOBZ == 'N' && data.RANGE == 'U' && data.UPLO == 'L' ) ||
data.N != int(A.rows()) ||
data.ISUPPZ == NULL ||
data.WORK == NULL ||
data.IWORK == NULL ){
data.Free();
data = nmrSymmetricEigenProblem::Data( A, D, V );
}
dsyevr_( &data.JOBZ, &data.RANGE, &data.UPLO,
&data.N, data.A, &data.LDA,
&data.VL, &data.VU,
&data.IL, &data.IU,
&data.ABSTOL,
&data.M, data.W,
data.Z, &data.LDZ, data.ISUPPZ,
data.WORK, &data.LWORK,
data.IWORK, &data.LIWORK,
&data.INFO );
if( data.INFO == 0 ) { return nmrSymmetricEigenProblem::ESUCCESS; }
return nmrSymmetricEigenProblem::EFAILURE;
}
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;
}
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;
}
void nmrLSMinNorm::Data::CheckSystem( const vctDynamicMatrix<double>& A,
const vctDynamicMatrix<double>& b ) const{
// test that number of rows match
if( A.rows() != b.rows() ){
std::ostringstream message;
message << "nmrLSMinNorm: Matrix A has " << A.rows() << " rows. "
<< "Matrix B has " << b.rows() << " rows.";
cmnThrow( std::runtime_error( message.str() ) );
}
// check for fortran format
if( !( A.IsFortran() ) ){
std::string message( "nmrLSMinNorm: Invalid matrix A format." );
cmnThrow( std::runtime_error( message ) );
}
if( !( b.IsFortran() ) ){
std::string message( "nmrLSMinNorm: Invalid matrix b format." );
cmnThrow( std::runtime_error( message ) );
}
}
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() ) );
}
}