bool Gaussian<T>::setCovariance(const MatrixT& sigma)
{
if(sigma.m != mu.n) {
cerr<<"Invalid dimensions on covariance matrix"<<endl;
return false;
}
CholeskyDecomposition<T> chol;
if(!chol.set(sigma)) {
cerr<<"Unable to set cholesky decomposition of covariance matrix"<<endl;
return false;
}
L = chol.L;
return true;
}
//B*ddq + C*dq = fext
void RobotDynamics3D::CalcAcceleration(Vector& ddq, const Vector& fext)
{
Matrix B;
GetKineticEnergyMatrix(B);
CholeskyDecomposition<Real> cholesky;
if(!cholesky.set(B)) {
cerr<<"Kinetic energy matrix is not positive definite!"<<endl;
cerr<<B<<endl;
Abort();
}
Vector Cdq;
GetCoriolisForces(Cdq);
Vector f_Cdq;
if(fext.n==0) f_Cdq.setNegative(Cdq);
else f_Cdq.sub(fext,Cdq);
cholesky.backSub(f_Cdq,ddq);
}
//B*ddq + C*dq = fext
void DynamicChain2D::GetAcceleration(Vector& ddq, const Vector& fext)
{
Matrix B;
GetKineticEnergyMatrix(B);
CholeskyDecomposition<Real> cholesky;
if(!cholesky.set(B)) {
LOG4CXX_ERROR(KrisLibrary::logger(),"Kinetic energy matrix is not positive definite!");
LOG4CXX_ERROR(KrisLibrary::logger(),B);
Abort();
}
Vector Cdq;
GetCoriolisForces(Cdq);
Vector f_Cdq;
if(fext.n==0) f_Cdq.setNegative(Cdq);
else f_Cdq.sub(fext,Cdq);
cholesky.backSub(f_Cdq,ddq);
}