inline void BasisSet::pointD(BasisSet *set, const Eigen::Vector3d &delta,
const double &dr2, int basis,
Eigen::MatrixXd &out)
{
// D type orbitals have six components and each component has a different
// independent MO weighting. Many things can be cached to save time though
double xx = 0.0, yy = 0.0, zz = 0.0, xy = 0.0, xz = 0.0, yz = 0.0;
// Now iterate through the D type GTOs and sum their contributions
unsigned int cIndex = set->m_cIndices[basis];
for (unsigned int i = set->m_gtoIndices[basis];
i < set->m_gtoIndices[basis+1]; ++i) {
// Calculate the common factor
double tmpGTO = exp(-set->m_gtoA[i] * dr2);
xx += set->m_gtoCN[cIndex++] * tmpGTO; // Dxx
yy += set->m_gtoCN[cIndex++] * tmpGTO; // Dyy
zz += set->m_gtoCN[cIndex++] * tmpGTO; // Dzz
xy += set->m_gtoCN[cIndex++] * tmpGTO; // Dxy
xz += set->m_gtoCN[cIndex++] * tmpGTO; // Dxz
yz += set->m_gtoCN[cIndex++] * tmpGTO; // Dyz
}
// Save values to the matrix
int baseIndex = set->m_moIndices[basis];
out.coeffRef(baseIndex , 0) = delta.x() * delta.x() * xx;
out.coeffRef(baseIndex+1, 0) = delta.y() * delta.y() * yy;
out.coeffRef(baseIndex+2, 0) = delta.z() * delta.z() * zz;
out.coeffRef(baseIndex+3, 0) = delta.x() * delta.y() * xy;
out.coeffRef(baseIndex+4, 0) = delta.x() * delta.z() * xz;
out.coeffRef(baseIndex+5, 0) = delta.y() * delta.z() * yz;
}
void Scaler::calcMaxMin(const std::vector<Eigen::MatrixXd>& src, Eigen::MatrixXd& max, Eigen::MatrixXd& min)
{
if(src.size() == 0)
{
throw nn::Exception("Scaler::calcMaxMin", "Vector size of src is zero.");
}
if(src[0].rows() == 0)
{
throw nn::Exception("Scaler::calcMaxMin", "src[0].rows() is zero.");
}
max = src[0];
min = src[0];
for(uint32_t i = 0; i < src.size(); ++i)
{
for(uint32_t j = 0; j < src[i].rows(); ++j)
{
if(max.coeff(j) < src[i].coeff(j))
max.coeffRef(j) = src[i].coeff(j);
if(min.coeff(j) > src[i].coeff(j))
min.coeffRef(j) = src[i].coeff(j);
}
}
}
Eigen::MatrixXd Tra_via0( double x0 , double v0 , double a0,
double xf , double vf , double af,
double smp , double tf )
/*
simple minimum jerk trajectory
x0 : position at initial state
v0 : velocity at initial state
a0 : acceleration at initial state
xf : position at final state
vf : velocity at final state
af : acceleration at final state
smp : sampling time
tf : movement time
*/
{
Eigen::MatrixXd A( 3 , 3 );
Eigen::MatrixXd B( 3 , 1 );
A << pow( tf , 3 ) , pow( tf , 4 ) , pow( tf , 5 ),
3 * pow( tf , 2 ) , 4 * pow( tf , 3 ) , 5 * pow( tf , 4 ),
6 * tf , 12 * pow( tf , 2 ) , 20 * pow( tf , 3 );
B << xf - x0 - v0 * tf - a0 * pow( tf , 2 ) / 2,
vf - v0 - a0 * tf,
af - a0 ;
Eigen::Matrix<double,3,1> C = A.inverse() * B;
double N;
N = tf / smp;
int NN = round( N + 1 );
Eigen::MatrixXd Time = Eigen::MatrixXd::Zero( NN , 1 );
Eigen::MatrixXd Tra = Eigen::MatrixXd::Zero( NN , 1 );
int i;
for ( i = 1; i <= NN; i++ )
Time.coeffRef( i - 1 , 0 ) = ( i - 1 ) * smp;
for ( i = 1; i <= NN; i++ )
{
Tra.coeffRef(i-1,0) =
x0 +
v0 * Time.coeff( i - 1 ) +
0.5 * a0 * pow( Time.coeff( i - 1 ) , 2 ) +
C.coeff( 0 , 0 ) * pow( Time.coeff( i - 1 ) , 3 ) +
C.coeff( 1 , 0 ) * pow( Time.coeff( i - 1 ) , 4 ) +
C.coeff( 2 , 0 ) * pow( Time.coeff( i - 1 ) , 5 );
}
return Tra;
}
void computeC_symmetric_full( Eigen::MatrixXd &W, Eigen::MatrixXd &C, const Eigen::MatrixXd &D, int n)
{
#pragma omp parallel for schedule(dynamic)
for (int i=0; i<n; ++i)
{
for (int j=0; j<n; ++j)
{
double s=0.0;
s = 0.5*(W.col(j)+W.col(i)).sum(); // if the matrix is dense this option is incredibly FAST!
C.coeffRef(i,j) += 2.0*(W.coeffRef(i,j)+s)/(D.coeffRef(i)+D.coeffRef(j));
}
}
}
void compute_C_asymmetric_full(Eigen::MatrixXd &W, Eigen::MatrixXd &C, const Eigen::MatrixXd &D, int n)
{
#pragma omp parallel for schedule(dynamic)
for (int i=0; i<n; ++i)
{
for (int j=0; j<n; ++j)
{
double s=0.0;
s = 0.5*(W.row(j)+W.row(i)).sum();// row access is slower than columns access
C.coeffRef(i,j) += 2.0*(W.coeffRef(i,j)+s)/(D.coeffRef(i)+D.coeffRef(j));
}
}
}
void JointControl::computeGeneralizedForce(Eigen::VectorXd& tau)
{
tau = Eigen::VectorXd::Zero(mnp_->getDOF());
Eigen::VectorXd error = qd_ - mnp_->q();
Eigen::MatrixXd Kpv = param_->getKpJoint().block(0, 0, mnp_->getDOF(), mnp_->getDOF());
for(uint32_t i = 0; i < Kpv.rows(); ++i)
{
Kpv.coeffRef(i, i) /= param_->getKvJoint().block(0, 0, mnp_->getDOF(), mnp_->getDOF()).coeff(i, i);
}
Eigen::VectorXd dqd = Kpv * error;
//const double dq_max = 25.0;
for(uint32_t i = 0; i < dqd.rows(); ++i)
{
if(dqd[i] < -param_->getDqMax())
{
dqd[i] = -param_->getDqMax();
}
else if(dqd[i] > param_->getDqMax())
{
dqd[i] = param_->getDqMax();
}
}
Eigen::VectorXd tau_unit = -param_->getKvJoint().block(0, 0, mnp_->getDOF(), mnp_->getDOF()) * (mnp_->dq() - dqd);
tau = tau_ = mnp_->getMassMatrix() * tau_unit;
}
void TrainingData::setOutput(uint32_t idx, std::vector<double>& output)
{
if(idx >= output_.size())
{
std::stringstream msg;
msg << "Failed to set training data. idx should be smaller than " << output_.size() << "." << std::endl
<< " idx : " << idx;
throw nn::Exception("TrainingData::setOutput", msg.str());
}
if(output.size() != output_[0].rows())
{
std::stringstream msg;
msg << "Specified data's row number is different from pre existed data's row number." << std::endl
<< " data row num : " << output.size() << std::endl
<< " existed data row num : " << output_[0].rows();
throw nn::Exception("TrainingData::setOutput", msg.str());
}
Eigen::MatrixXd tmp = Eigen::MatrixXd::Zero(output.size(), 1);
for(uint32_t i = 0; i < output.size(); ++i)
{
tmp.coeffRef(i) = output[i];
}
output_[idx] = tmp;
}
void Scaler::denormalize(Eigen::MatrixXd& src, const Eigen::MatrixXd& min, const Eigen::MatrixXd& range)
{
src = (src - Eigen::MatrixXd::Constant(src.rows(), src.cols(), min_)) / range_;
for(uint32_t i = 0; i < src.rows(); ++i)
{
src.coeffRef(i) *= range.coeff(i);
}
src = src + min;
}
void Scaler::normalize(Eigen::MatrixXd& src, const Eigen::MatrixXd& min, const Eigen::MatrixXd& range)
{
src = src - min;
for(unsigned int i = 0; i < src.rows(); ++i)
{
src.coeffRef(i) /= range.coeff(i);
}
src = src * range_ + Eigen::MatrixXd::Constant(src.rows(), src.cols(), min_);
}
inline void BasisSet::pointS(BasisSet *set, const double &dr2, int basis,
Eigen::MatrixXd &out)
{
// S type orbitals - the simplest of the calculations with one component
double tmp = 0.0;
unsigned int cIndex = set->m_cIndices[basis];
for (unsigned int i = set->m_gtoIndices[basis];
i < set->m_gtoIndices[basis+1]; ++i) {
tmp += set->m_gtoCN[cIndex++] * exp(-set->m_gtoA[i] * dr2);
}
out.coeffRef(set->m_moIndices[basis], 0) = tmp;
}
void compute_C_asymmetric_sparse(Eigen::MatrixXd &W, Eigen::MatrixXd &C, const Eigen::MatrixXd &D, int n)
{
#pragma omp parallel for schedule(dynamic)
for (int i=0; i<n; ++i)
{
for (int j=0; j<n; ++j)
{
double s=0.0;
for (int k=0; k<n; ++k)
{
// if the matrix is symmetric this option is much faster, access k
// as rows instead as columns
double Wjk = W.coeffRef(k,j);
int delta_jk = (Wjk!=0);
if (delta_jk==0)
continue;
double Wik = W.coeffRef(k,i);
int delta_ik = (Wik!=0);
if (delta_ik==0)
continue;
s+= (delta_ik*delta_jk*(Wik+Wjk)*0.5);
}
C.coeffRef(i,j) += 2.0*(W.coeffRef(i,j)+s)/(D.coeffRef(i)+D.coeffRef(j));
}
}
}
inline void BasisSet::pointP(BasisSet *set, const Vector3d &delta,
const double &dr2, int basis,
Eigen::MatrixXd &out)
{
double x = 0.0, y = 0.0, z = 0.0;
// Now iterate through the P type GTOs and sum their contributions
unsigned int cIndex = set->m_cIndices[basis];
for (unsigned int i = set->m_gtoIndices[basis];
i < set->m_gtoIndices[basis+1]; ++i) {
double tmpGTO = exp(-set->m_gtoA[i] * dr2);
x += set->m_gtoCN[cIndex++] * tmpGTO;
y += set->m_gtoCN[cIndex++] * tmpGTO;
z += set->m_gtoCN[cIndex++] * tmpGTO;
}
// Save values to the matrix
int baseIndex = set->m_moIndices[basis];
out.coeffRef(baseIndex , 0) = x * delta.x();
out.coeffRef(baseIndex+1, 0) = y * delta.y();
out.coeffRef(baseIndex+2, 0) = z * delta.z();
}
inline void BasisSet::pointD5(BasisSet *set, const Eigen::Vector3d &delta,
const double &dr2, int basis,
Eigen::MatrixXd &out)
{
// D type orbitals have six components and each component has a different
// independent MO weighting. Many things can be cached to save time though
double d0 = 0.0, d1p = 0.0, d1n = 0.0, d2p = 0.0, d2n = 0.0;
// Now iterate through the D type GTOs and sum their contributions
unsigned int cIndex = set->m_cIndices[basis];
for (unsigned int i = set->m_gtoIndices[basis];
i < set->m_gtoIndices[basis+1]; ++i) {
// Calculate the common factor
double tmpGTO = exp(-set->m_gtoA[i] * dr2);
d0 += set->m_gtoCN[cIndex++] * tmpGTO;
d1p += set->m_gtoCN[cIndex++] * tmpGTO;
d1n += set->m_gtoCN[cIndex++] * tmpGTO;
d2p += set->m_gtoCN[cIndex++] * tmpGTO;
d2n += set->m_gtoCN[cIndex++] * tmpGTO;
}
// Calculate the prefactors
double xx = delta.x() * delta.x();
double yy = delta.y() * delta.y();
double zz = delta.z() * delta.z();
double xy = delta.x() * delta.y();
double xz = delta.x() * delta.z();
double yz = delta.y() * delta.z();
// Save values to the matrix
int baseIndex = set->m_moIndices[basis];
out.coeffRef(baseIndex , 0) = (zz - dr2) * d0;
out.coeffRef(baseIndex+1, 0) = xz * d1p;
out.coeffRef(baseIndex+2, 0) = yz * d1n;
out.coeffRef(baseIndex+3, 0) = (xx - yy) * d2p;
out.coeffRef(baseIndex+4, 0) = xy * d2n;
}
void Scaler::normalize(Eigen::MatrixXd& src, const Eigen::MatrixXd& min, const Eigen::MatrixXd& range)
{
src = src - min;
for(uint32_t i = 0; i < src.rows(); ++i)
{
if(range.coeff(i) == 0.0)
{
std::stringstream msg;
msg << "range.coeff(" << i << ") is zero." << std::endl
<< "This means " << i << "-th data is always constant and shouldn't be included as a training data." << std::endl
<< "If you'd like to include the data somehow, it is recommended that you use neural network without using scaler.";
throw nn::Exception("Scaler::normalize", msg.str());
}
src.coeffRef(i) /= range.coeff(i);
}
src = src * range_ + Eigen::MatrixXd::Constant(src.rows(), src.cols(), min_);
}
bool IOUtils::getValues(std::ifstream& ifs, Eigen::MatrixXd& dst)
{
if(!ifs)
return false;
std::string str;
if(!std::getline(ifs, str))
return false;
std::vector<std::string> words;
StrUtils::separate(str, words, ",;: \t");
dst = Eigen::MatrixXd::Zero(words.size(), 1);
for(unsigned int j = 0; j < words.size(); ++j)
{
StrUtils::convertToNum(words[j], dst.coeffRef(j, 0));
}
return true;
}
Eigen::MatrixXd Tra_vian_q( int n ,
double x0 , double v0 , double a0 ,
Eigen::MatrixXd x ,
double xf , double vf , double af ,
double smp , Eigen::MatrixXd t , double tf)
/*
minimum jerk trajectory with via-points
(via-point constraints: position at each point)
n : the number of via-points
x0 : position at initial state
v0 : velocity at initial state
a0 : acceleration at initial state
x : position matrix at via-points state ( size : n x 1 )
xf : position at final state
vf : velocity at final state
af : acceleration at final state
smp : sampling time
t : time matrix passing through via-points state ( size : n x 1 )
tf : movement time
*/
{
int i , j , k ;
/* Calculation Matrix B */
Eigen::MatrixXd B = Eigen::MatrixXd::Zero( n + 3 , 1 );
for ( i = 1; i <= n; i++ )
{
B.coeffRef( i - 1 , 0 ) =
x.coeff( i - 1 , 0 ) -
x0 -
v0 * t.coeff( i - 1 , 0 ) -
( a0 / 2 ) * pow( t.coeff( i - 1 , 0 ) , 2 ) ;
}
B.coeffRef( n , 0 ) =
xf -
x0 -
v0 * tf -
( a0 / 2 ) * pow( tf , 2 ) ;
B.coeffRef( n + 1 , 0 ) =
vf -
v0 -
a0 * tf ;
B.coeffRef( n + 2 , 0 ) =
af -
a0 ;
/* Calculation Matrix A */
Eigen::MatrixXd A1 = Eigen::MatrixXd::Zero( n , 3 );
for ( i = 1; i <= n; i++ )
{
A1.coeffRef( i - 1 , 0 ) = pow( t.coeff( i - 1 , 0 ) , 3 );
A1.coeffRef( i - 1 , 1 ) = pow( t.coeff( i - 1 , 0 ) , 4 );
A1.coeffRef( i - 1 , 2 ) = pow( t.coeff( i - 1 , 0 ) , 5 );
}
Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero( n , n );
for ( i = 1; i <= n; i++ )
{
for ( j = 1; j <= n; j++ )
{
if ( i > j )
k = i;
else
k = j;
A2.coeffRef( j - 1 , i - 1 ) =
pow( ( t.coeff( k - 1 , 0 ) - t.coeff( i - 1 , 0 ) ) , 5 ) / 120;
}
}
Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero( 3 , n + 3 );
A3.coeffRef( 0 , 0 ) = pow( tf , 3 );
A3.coeffRef( 0 , 1 ) = pow( tf , 4 );
A3.coeffRef( 0 , 2 ) = pow( tf , 5 );
A3.coeffRef( 1 , 0 ) = 3 * pow( tf , 2 );
A3.coeffRef( 1 , 1 ) = 4 * pow( tf , 3 );
A3.coeffRef( 1 , 2 ) = 5 * pow( tf , 4 );
A3.coeffRef( 2 , 0 ) = 6 * tf;
A3.coeffRef( 2 , 1 ) = 12 * pow( tf , 2 );
A3.coeffRef( 2 , 2 ) = 20 * pow( tf , 3 );
for ( i = 1; i <= n; i++ )
{
A3.coeffRef( 0 , i + 2 ) = pow( tf - t.coeff( i - 1 , 0 ) , 5 ) / 120;
A3.coeffRef( 1 , i + 2 ) = pow( tf - t.coeff( i - 1 , 0 ) , 4 ) / 24;
A3.coeffRef( 2 , i + 2 ) = pow( tf - t.coeff( i - 1 , 0 ) , 3 ) / 6;
}
Eigen::MatrixXd A = Eigen::MatrixXd::Zero( n + 3 , n + 3 );
A.block( 0 , 0 , n , 3 ) = A1;
A.block( 0 , 3 , n , n ) = A2;
A.block( n , 0 , 3 , n + 3 ) = A3;
/* Calculation Matrix C (coefficient of polynomial function) */
Eigen::MatrixXd C( 2 * n + 3 , 1 );
//C = A.inverse() * B;
C = A.colPivHouseholderQr().solve(B);
/* Time */
int NN;
double N;
N = tf / smp ;
NN = round( N ) ;
Eigen::MatrixXd Time = Eigen::MatrixXd::Zero( NN + 1 , 1 );
for ( i = 1; i <= NN + 1; i++ )
Time.coeffRef( i - 1 , 0 ) = ( i - 1 ) * smp;
/* Time_via */
Eigen::MatrixXd Time_via = Eigen::MatrixXd::Zero( n , 1 );
for ( i = 1; i <= n; i++ )
Time_via.coeffRef( i - 1 , 0 ) = round( t.coeff( i - 1 , 0 ) / smp ) + 2;
/* Minimum Jerk Trajectory with Via-points */
Eigen::MatrixXd Tra_jerk_via = Eigen::MatrixXd::Zero( NN + 1 , 1 );
for ( i = 1; i <= NN + 1; i++ )
{
Tra_jerk_via.coeffRef( i - 1 , 0 ) =
x0 +
v0 * Time.coeff( i - 1 , 0 ) +
0.5 * a0 * pow( Time.coeff( i - 1 , 0 ) , 2 ) +
C.coeff( 0 , 0 ) * pow( Time.coeff( i - 1 , 0 ) , 3 ) +
C.coeff( 1 , 0 ) * pow( Time.coeff( i - 1 , 0 ) , 4 ) +
C.coeff( 2 , 0 ) * pow( Time.coeff( i - 1 , 0 ) , 5 ) ;
}
for ( i = 1 ; i <= n; i++ )
{
for ( j = Time_via.coeff( i - 1 , 0 ); j <= NN + 1; j++ )
{
Tra_jerk_via.coeffRef( j - 1 , 0 ) =
Tra_jerk_via.coeff( j - 1 , 0 ) +
C.coeff( i + 2 , 0 ) * pow( ( Time.coeff( j - 1 , 0 ) - t.coeff( i - 1 , 0 ) ) , 5 ) / 120 ;
}
}
return Tra_jerk_via;
}
