Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
//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);
}
Exemplo n.º 3
0
//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);
}