void write_transformed_weak_form(EquationArray const & weak_form,
latex_logger<InterfaceType> & log)
{
// Restriction to the discrete space
log << "\\section{Discrete Approximation by Finite Elements}\n";
log << "The discrete approximation to the continuous problem is obtained by a Galerkin approach:\n";
log << "\\begin{align}\n";
log << " u_h = \\sum_j \\alpha_j \\varphi_j \n";
log << "\\end{align}\n";
log << "with trial functions $\\varphi_j$.\n";
//log << "Thus, instead of solving for the continuous function $u$, only the coefficients $\\alpha_i$ need to be computed.\n";
log << "In a Galerkin approach, test functions $v$ are also chosen from a finite-dimensional space:\n";
log << "\\begin{align}\n";
log << " v_h = \\sum_i \\beta_i \\psi_i \n";
log << "\\end{align}\n";
log << "Due to integral transformations, it is sufficient to define the trial and test functions on the reference cell.\n";
log << "After transformation to the reference cell, the weak form on a cell reads\n";
// give new name to local variables:
viennamath::variable xi(0);
viennamath::variable eta(1);
viennamath::variable nu(2);
log.translator().customize(xi, "\\xi");
log.translator().customize(eta, "\\eta");
log.translator().customize(nu, "\\nu");
viennamath::function_symbol u0(0, viennamath::unknown_tag<>());
viennamath::function_symbol u1(1, viennamath::unknown_tag<>());
viennamath::function_symbol u2(2, viennamath::unknown_tag<>());
viennamath::function_symbol v0(0, viennamath::test_tag<>());
viennamath::function_symbol v1(1, viennamath::test_tag<>());
viennamath::function_symbol v2(2, viennamath::test_tag<>());
log.translator().customize(u0, "\\tilde{u}_0");
log.translator().customize(u1, "\\tilde{u}_1");
log.translator().customize(u2, "\\tilde{u}_2");
log.translator().customize(v0, "\\tilde{v}_0");
log.translator().customize(v1, "\\tilde{v}_1");
log.translator().customize(v2, "\\tilde{v}_2");
// [JW] switched to $ math expressions, otherwise the equation is cut
// off at the right page border ...
//
//log << "\\begin{align}\n";
log << "\\newline\\newline$\n";
for (typename EquationArray::const_iterator it = weak_form.begin();
it != weak_form.end();
++it)
log << log.translator()(*it) << " \\ . \n";
//log << "\\end{align}\n";
log << "$\\newline\\newline\n";
//
//log << "write transformed weak form\n";
}
R TypeOfFE_P0Lagrange1d::operator()(const FElement & K,const R1 & PHat,const KN_<R> & u,int componante,int op) const
{
R u0(u(K(0)));
R r=0;
if (op==0)
r = u0;
else r=0;
return r;
}
latex_logger(std::string const & filename) : stream_(filename.c_str()), filename_(filename), something_written(false)
{
viennamath::function_symbol u0(0, viennamath::unknown_tag<>());
viennamath::function_symbol u1(1, viennamath::unknown_tag<>());
viennamath::function_symbol u2(2, viennamath::unknown_tag<>());
viennamath::function_symbol v0(0, viennamath::test_tag<>());
viennamath::function_symbol v1(1, viennamath::test_tag<>());
viennamath::function_symbol v2(2, viennamath::test_tag<>());
translator_.customize(u0, "u_0");
translator_.customize(u1, "u_1");
translator_.customize(u2, "u_2");
translator_.customize(v0, "v_0");
translator_.customize(v1, "v_1");
translator_.customize(v2, "v_2");
}
//
// Compute m_aff[0] and m_aff[1] according to the current curve
//
void ShiftTraceTool::updateCurveAffs() {
if (m_curveStatus != ThreePointsCurve) {
m_aff[0] = m_aff[1] = TAffine();
} else {
double phi0 = 0, phi1 = 0;
TPointD center;
if (circumCenter(center, m_p0, m_p1, m_p2)) {
TPointD v0 = normalize(m_p0 - center);
TPointD v1 = normalize(m_p1 - center);
TPointD v2 = normalize(m_p2 - center);
TPointD u0(-v0.y, v0.x);
TPointD u1(-v1.y, v1.x);
phi0 = atan2((v2 * u0), (v2 * v0)) * 180.0 / 3.1415;
phi1 = atan2((v2 * u1), (v2 * v1)) * 180.0 / 3.1415;
}
m_aff[0] = TTranslation(m_p2 - m_p0) * TRotation(m_p0, phi0);
m_aff[1] = TTranslation(m_p2 - m_p1) * TRotation(m_p1, phi1);
}
}
R TypeOfFE_P1Lagrange1d::operator()(const FElement & K,const R1 & PHat,const KN_<R> & u,int componante,int op) const
{
R u0(u(K(0))), u1(u(K(1)));
R r=0;
if (op==0)
{
R l0=1-PHat.x,l1=PHat.x;
r = u0*l0+u1*l1;
}
else if(op==op_dx )
{
const Element & T=K.T;
R1 D[2];
T.Gradlambda(D);
r = D[0].x*u0 + D[1].x*u1 ;
}
// cout << r << "\t";
return r;
}
int main (int argc, char** argv)
{
int N;
double T;
parseArguments(argc, argv, N, T);
double h = (b - a) / N;
double tau = k * h * h;
int iterations = (int)std::floor(T / tau);
std::vector<double> u0(N, 0), u1(N, 0);
u0[0] = A;
u0[N - 1] = B;
u1[0] = A;
u1[N - 1] = B;
#pragma omp parallel
{
for (int iteration = 0; iteration < iterations; ++iteration)
{
#pragma omp for schedule(static)
for (int index = 1; index < N - 1; ++index)
{
//std::cerr << "thread is " << omp_get_thread_num() << std::endl;
u1[index] = u0[index] + k * (u0[index - 1] - 2 * u0[index] + u0[index + 1]);
}
#pragma omp barrier
#pragma omp single
{
u0.swap(u1);
}
}
}
if (N < 10)
{
for (int index = 0; index < N; ++index)
{
std::cerr << std::setprecision(8) << std::fixed << a + h * index << " " << u0[index] << std::endl;
}
}
return 0;
}
R TypeOfFE_P1ttdc::operator()(const FElement & K,const R2 & P1Hat,const KN_<R> & u,int componante,int op) const
{
R2 PHat=Shrink1(P1Hat);
R u0(u(K(0))), u1(u(K(1))), u2(u(K(2)));
R r=0;
if (op==0)
{
R l0=1-PHat.x-PHat.y,l1=PHat.x,l2=PHat.y;
r = u0*l0+u1*l1+l2*u2;
}
else
{
const Triangle & T=K.T;
R2 D0 = T.H(0)*cshrink1 , D1 = T.H(1)*cshrink1 , D2 = T.H(2)*cshrink1 ;
if (op==1)
r = D0.x*u0 + D1.x*u1 + D2.x*u2 ;
else
r = D0.y*u0 + D1.y*u1 + D2.y*u2 ;
}
// cout << r << "\t";
return r;
}
void FemPoroelasticResidualLocalAssembler::assembleComponents
( const NumLib::TimeStep ×tep,
const MeshLib::IElement &e,
const std::vector<size_t> &vec_order,
const std::vector<LocalVectorType> &vec_x0,
const std::vector<LocalVectorType> &vec_x1,
std::vector<LocalVectorType> &vec_r
)
{
assert(vec_order.size()==3);
//TODO how do you know 1st is ux and 3rd is p? who decides it?
const size_t id_ux = 0;
const size_t id_uy = 1;
const size_t id_p = 2;
const size_t u_order = vec_order[id_ux];
assert(u_order==vec_order[id_uy]);
const size_t p_order = vec_order[id_p];
const LocalVectorType &ux0 = vec_x0[id_ux];
const LocalVectorType &uy0 = vec_x0[id_uy];
const LocalVectorType &ux1 = vec_x1[id_ux];
const LocalVectorType &uy1 = vec_x1[id_uy];
// combine ux and uy
LocalVectorType u0(ux0.rows()*2);
LocalVectorType u1(ux0.rows()*2);
for (int i=0; i<ux0.rows(); i++) {
u0(i) = ux0(i);
u0(i+ux0.rows()) = uy0(i);
u1(i) = ux1(i);
u1(i+ux0.rows()) = uy1(i);
}
const LocalVectorType &p0 = vec_x0[id_p];
const LocalVectorType &p1 = vec_x1[id_p];
// ------------------------------------------------------------------------
// Element
// ------------------------------------------------------------------------
const size_t dim = e.getDimension();
const size_t n_strain_components = getNumberOfStrainComponents(dim);
const size_t nnodes_u = e.getNumberOfNodes(u_order);
const size_t nnodes_p = e.getNumberOfNodes(p_order);
const NumLib::TXPosition e_pos(NumLib::TXPosition::Element, e.getID());
// ------------------------------------------------------------------------
// Transient
// ------------------------------------------------------------------------
const double dt = timestep.getTimeStepSize();
const double theta = 1.0;
// ------------------------------------------------------------------------
// Material (assuming element constant)
// ------------------------------------------------------------------------
size_t mat_id = e.getGroupID();
size_t fluid_id = 0; //TODO
Ogs6FemData* femData = Ogs6FemData::getInstance();
MaterialLib::PorousMedia* pm = femData->list_pm[mat_id];
MaterialLib::Solid *solidphase = femData->list_solid[mat_id];
MaterialLib::Fluid *fluidphase = femData->list_fluid[fluid_id];
// solid
double rho_s = .0;
if (solidphase->density!=NULL)
solidphase->density->eval(e_pos, rho_s);
LocalMatrixType De = LocalMatrixType::Zero(n_strain_components, n_strain_components);
MathLib::LocalMatrix nv(1,1);
MathLib::LocalMatrix E(1,1);
solidphase->poisson_ratio->eval(e_pos, nv);
solidphase->Youngs_modulus->eval(e_pos, E);
double Lambda, G, K;
MaterialLib::calculateLameConstant(nv(0,0), E(0,0), Lambda, G, K);
MaterialLib::setElasticConsitutiveTensor(dim, Lambda, G, De);
// fluid
double mu = .0;
fluidphase->dynamic_viscosity->eval(e_pos, mu);
double rho_f = .0;
fluidphase->density->eval(e_pos, rho_f);
// media
double k;
pm->permeability->eval(e_pos, k);
double n = .0;
pm->porosity->eval(e_pos, n);
double s = .0;
pm->storage->eval(e_pos, s);
double k_mu;
k_mu = k / mu;
// ------------------------------------------------------------------------
// Body force
// ------------------------------------------------------------------------
LocalVectorType body_force = LocalVectorType::Zero(dim);
bool hasGravity = false;
if (hasGravity) {
body_force[dim-1] = rho_s * 9.81;
}
// ------------------------------------------------------------------------
// Local component assembly
// ------------------------------------------------------------------------
LocalMatrixType Kuu = LocalMatrixType::Zero(nnodes_u*dim, nnodes_u*dim);
LocalMatrixType Cup = LocalMatrixType::Zero(nnodes_u*dim, nnodes_p);
LocalMatrixType Kpp = LocalMatrixType::Zero(nnodes_p, nnodes_p);
LocalMatrixType Mpp = LocalMatrixType::Zero(nnodes_p, nnodes_p);
LocalMatrixType Cpu = LocalMatrixType::Zero(nnodes_p, nnodes_u*dim);
LocalVectorType Fu = LocalVectorType::Zero(nnodes_u*dim);
LocalVectorType Fp = LocalVectorType::Zero(nnodes_p);
// temp matrix
LocalMatrixType B = LocalMatrixType::Zero(n_strain_components, nnodes_u*dim);
LocalMatrixType Nuvw = LocalMatrixType::Zero(dim, nnodes_u*dim);
const LocalMatrixType m = get_m(dim);
//
FemLib::IFiniteElement* fe_u = _feObjects.getFeObject(e, u_order);
FemLib::IFiniteElement* fe_p = _feObjects.getFeObject(e, p_order);
FemLib::IFemNumericalIntegration *q_u = fe_u->getIntegrationMethod();
double gp_x[3], real_x[3];
for (size_t j=0; j<q_u->getNumberOfSamplingPoints(); j++) {
q_u->getSamplingPoint(j, gp_x);
fe_u->computeBasisFunctions(gp_x);
fe_p->computeBasisFunctions(gp_x);
fe_u->getRealCoordinates(real_x);
double fac_u = fe_u->getDetJ() * q_u->getWeight(j);
//--- local component ----
// set N,B
LocalMatrixType &Nu = *fe_u->getBasisFunction();
LocalMatrixType &dNu = *fe_u->getGradBasisFunction();
setNu_Matrix_byComponent(dim, nnodes_u, Nu, Nuvw);
setB_Matrix_byComponent(dim, nnodes_u, dNu, B);
LocalMatrixType &Np = *fe_p->getBasisFunction();
// K_uu += B^T * D * B
Kuu.noalias() += fac_u * B.transpose() * De * B;
// C_up += B^T * m * Np
Cup.noalias() += fac_u * B.transpose() * m * Np;
// Fu += N^T * b
if (hasGravity) {
Fu.noalias() += fac_u * Nuvw.transpose() * body_force;
}
}
Fu.noalias() += (theta - 1) * Kuu * u0 + (1-theta)* Cup * p0;
FemLib::IFemNumericalIntegration *q_p = fe_p->getIntegrationMethod();
for (size_t j=0; j<q_p->getNumberOfSamplingPoints(); j++) {
q_p->getSamplingPoint(j, gp_x);
fe_u->computeBasisFunctions(gp_x);
fe_p->computeBasisFunctions(gp_x);
fe_p->getRealCoordinates(real_x);
double fac = fe_p->getDetJ() * q_p->getWeight(j);
//--- local component ----
// set N,B
LocalMatrixType &dNu = *fe_u->getGradBasisFunction();
setB_Matrix_byComponent(dim, nnodes_u, dNu, B);
LocalMatrixType &Np = *fe_p->getBasisFunction();
LocalMatrixType &dNp = *fe_p->getGradBasisFunction();
// M_pp += Np^T * S * Np
Mpp.noalias() += fac * Np.transpose() * s * Np;
// K_pp += dNp^T * K * dNp
Kpp.noalias() += fac * dNp.transpose() * k_mu * dNp;
// C_pu += Np^T * m^T * B
Cpu.noalias() += fac * Np.transpose() * m.transpose() * B;
}
// Backward euler
Fp = (1.0/dt * Mpp - (1-theta)*Kpp)* p0 + 1.0/dt * Cpu * u0;
// r = K*u - RHS
LocalVectorType r_u = Kuu * u1 - Cup * p1 - Fu;
LocalVectorType r_p = 1.0/dt * Cpu * u1 + (1.0/dt * Mpp + theta * Kpp) * p1 - Fp;
// if (e.getID()==0) {
// std::cout << "u1=" << std::endl << u1 << std::endl;
// std::cout << "p1=" << std::endl << p1 << std::endl;
// std::cout << "Fp=" << std::endl << Fp << std::endl;
// std::cout << "r_p=" << std::endl << r_p << std::endl;
// }
//
for (size_t i=0; i<dim; i++) {
vec_r[i] = r_u.segment(i*nnodes_u, nnodes_u);
}
vec_r[id_p] = r_p;
}
int main ( void )
/******************************************************************************/
/*
Purpose:
MAIN is the main program for FD1D_HEAT_IMPLICIT.
Discussion:
FD1D_HEAT_IMPLICIT solves the 1D heat equation with an implicit method.
This program solves
dUdT - k * d2UdX2 = F(X,T)
over the interval [A,B] with boundary conditions
U(A,T) = UA(T),
U(B,T) = UB(T),
over the time interval [T0,T1] with initial conditions
U(X,T0) = U0(X)
The code uses the finite difference method to approximate the
second derivative in space, and an implicit backward Euler approximation
to the first derivative in time.
The finite difference form can be written as
U(X,T+dt) - U(X,T) ( U(X-dx,T) - 2 U(X,T) + U(X+dx,T) )
------------------ = F(X,T) + k * ------------------------------------
dt dx * dx
so that we have the following linear system for the values of U at time T+dt:
- k * dt / dx / dx * U(X-dt,T+dt)
+ ( 1 + 2 * k * dt / dx / dx ) * U(X, T+dt)
- k * dt / dx / dx * U(X+dt,T+dt)
= dt * F(X, T+dt)
+ U(X, T)
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
01 June 2009
Author:
John Burkardt
*/
{
double *a;
double *b;
double *fvec;
int i;
int info;
int j;
int job;
double k;
double *t;
double t_delt;
char *t_file = "t.txt";
double t_max;
double t_min;
int t_num;
double *u;
char *u_file = "u.txt";
double w;
double *x;
double x_delt;
char *x_file = "x.txt";
double x_max;
double x_min;
int x_num;
timestamp ( );
printf ( "\n" );
printf ( "FD1D_HEAT_IMPLICIT\n" );
printf ( " C version\n" );
printf ( "\n" );
printf ( " Finite difference solution of\n" );
printf ( " the time dependent 1D heat equation\n" );
printf ( "\n" );
printf ( " Ut - k * Uxx = F(x,t)\n" );
printf ( "\n" );
printf ( " for space interval A <= X <= B with boundary conditions\n" );
printf ( "\n" );
printf ( " U(A,t) = UA(t)\n" );
printf ( " U(B,t) = UB(t)\n" );
printf ( "\n" );
printf ( " and time interval T0 <= T <= T1 with initial condition\n" );
printf ( "\n" );
printf ( " U(X,T0) = U0(X).\n" );
printf ( "\n" );
printf ( " A second order difference approximation is used for Uxx.\n" );
printf ( "\n" );
printf ( " A first order backward Euler difference approximation\n" );
printf ( " is used for Ut.\n" );
k = 5.0E-07;
/*
Set X values.
*/
x_min = 0.0;
x_max = 0.3;
x_num = 11;
x_delt = ( x_max - x_min ) / ( double ) ( x_num - 1 );
x = ( double * ) malloc ( x_num * sizeof ( double ) );
for ( i = 0; i < x_num; i++ )
{
x[i] = ( ( double ) ( x_num - i - 1 ) * x_min
+ ( double ) ( i ) * x_max )
/ ( double ) ( x_num - 1 );
}
/*
Set T values.
*/
t_min = 0.0;
t_max = 22000.0;
t_num = 51;
t_delt = ( t_max - t_min ) / ( double ) ( t_num - 1 );
t = ( double * ) malloc ( t_num * sizeof ( double ) );
for ( j = 0; j < t_num; j++ )
{
t[j] = ( ( double ) ( t_num - j - 1 ) * t_min
+ ( double ) ( j ) * t_max )
/ ( double ) ( t_num - 1 );
}
/*
Set the initial data, for time T_MIN.
*/
u = ( double * ) malloc ( x_num * t_num * sizeof ( double ) );
u0 ( x_min, x_max, t_min, x_num, x, u );
/*
The matrix A does not change with time. We can set it once,
factor it once, and solve repeatedly.
*/
w = k * t_delt / x_delt / x_delt;
a = ( double * ) malloc ( 3 * x_num * sizeof ( double ) );
a[0+0*3] = 0.0;
a[1+0*3] = 1.0;
a[0+1*3] = 0.0;
for ( i = 1; i < x_num - 1; i++ )
{
a[2+(i-1)*3] = - w;
a[1+ i *3] = 1.0 + 2.0 * w;
a[0+(i+1)*3] = - w;
}
a[2+(x_num-2)*3] = 0.0;
a[1+(x_num-1)*3] = 1.0;
a[2+(x_num-1)*3] = 0.0;
/*
Factor the matrix.
*/
info = r83_np_fa ( x_num, a );
b = ( double * ) malloc ( x_num * sizeof ( double ) );
fvec = ( double * ) malloc ( x_num * sizeof ( double ) );
for ( j = 1; j < t_num; j++ )
{
/*
Set the right hand side B.
*/
b[0] = ua ( x_min, x_max, t_min, t[j] );
f ( x_min, x_max, t_min, t[j-1], x_num, x, fvec );
for ( i = 1; i < x_num - 1; i++ )
{
b[i] = u[i+(j-1)*x_num] + t_delt * fvec[i];
}
b[x_num-1] = ub ( x_min, x_max, t_min, t[j] );
free ( fvec );
job = 0;
fvec = r83_np_sl ( x_num, a, b, job );
for ( i = 0; i < x_num; i++ )
{
u[i+j*x_num] = fvec[i];
}
}
r8mat_write ( x_file, 1, x_num, x );
r8mat_write ( t_file, 1, t_num, t );
r8mat_write ( u_file, x_num, t_num, u );
printf ( "\n" );
printf ( " X data written to \"%s\".\n", x_file );
printf ( " T data written to \"%s\".\n", t_file );
printf ( " U data written to \"%s\".\n", u_file );
free ( a );
free ( b );
free ( fvec );
free ( t );
free ( u );
free ( x );
/*
Terminate.
*/
printf ( "\n" );
printf ( "FD1D_HEAT_IMPLICIT\n" );
printf ( " Normal end of execution.\n" );
printf ( "\n" );
timestamp ( );
return 0;
}
Stokhos::MonoProjPCEBasis<ordinal_type, value_type>::
MonoProjPCEBasis(
ordinal_type p,
const Stokhos::OrthogPolyApprox<ordinal_type, value_type>& pce,
const Stokhos::Quadrature<ordinal_type, value_type>& quad,
const Stokhos::Sparse3Tensor<ordinal_type, value_type>& Cijk,
bool limit_integration_order_) :
RecurrenceBasis<ordinal_type, value_type>("Monomial Projection", p, true),
limit_integration_order(limit_integration_order_),
pce_sz(pce.basis()->size()),
pce_norms(pce.basis()->norm_squared()),
a(pce_sz),
b(pce_sz),
basis_vecs(pce_sz, p+1),
new_pce(p+1)
{
// If the original basis is normalized, we can use the standard QR
// factorization. For simplicity, we renormalize the PCE coefficients
// for a normalized basis
Stokhos::OrthogPolyApprox<ordinal_type, value_type> normalized_pce(pce);
for (ordinal_type i=0; i<pce_sz; i++) {
pce_norms[i] = std::sqrt(pce_norms[i]);
normalized_pce[i] *= pce_norms[i];
}
// Evaluate PCE at quad points
ordinal_type nqp = quad.size();
Teuchos::Array<value_type> pce_vals(nqp);
const Teuchos::Array<value_type>& weights = quad.getQuadWeights();
const Teuchos::Array< Teuchos::Array<value_type> >& quad_points =
quad.getQuadPoints();
const Teuchos::Array< Teuchos::Array<value_type> >& basis_values =
quad.getBasisAtQuadPoints();
for (ordinal_type i=0; i<nqp; i++) {
pce_vals[i] = normalized_pce.evaluate(quad_points[i], basis_values[i]);
}
// Form Kylov matrix up to order pce_sz
matrix_type K(pce_sz, pce_sz);
// Compute matrix
matrix_type A(pce_sz, pce_sz);
typedef Stokhos::Sparse3Tensor<ordinal_type, value_type> Cijk_type;
for (typename Cijk_type::k_iterator k_it = Cijk.k_begin();
k_it != Cijk.k_end(); ++k_it) {
ordinal_type k = index(k_it);
for (typename Cijk_type::kj_iterator j_it = Cijk.j_begin(k_it);
j_it != Cijk.j_end(k_it); ++j_it) {
ordinal_type j = index(j_it);
value_type val = 0;
for (typename Cijk_type::kji_iterator i_it = Cijk.i_begin(j_it);
i_it != Cijk.i_end(j_it); ++i_it) {
ordinal_type i = index(i_it);
value_type c = value(i_it) / (pce_norms[j]*pce_norms[k]);
val += pce[i]*c;
}
A(k,j) = val;
}
}
// Each column i is given by projection of the i-th order monomial
// onto original basis
vector_type u0 = Teuchos::getCol(Teuchos::View, K, 0);
u0(0) = 1.0;
for (ordinal_type i=1; i<pce_sz; i++)
u0(i) = 0.0;
for (ordinal_type k=1; k<pce_sz; k++) {
vector_type u = Teuchos::getCol(Teuchos::View, K, k);
vector_type up = Teuchos::getCol(Teuchos::View, K, k-1);
u.multiply(Teuchos::NO_TRANS, Teuchos::NO_TRANS, 1.0, A, up, 0.0);
}
/*
for (ordinal_type j=0; j<pce_sz; j++) {
for (ordinal_type i=0; i<pce_sz; i++) {
value_type val = 0.0;
for (ordinal_type k=0; k<nqp; k++)
val += weights[k]*std::pow(pce_vals[k],j)*basis_values[k][i];
K(i,j) = val;
}
}
*/
std::cout << K << std::endl << std::endl;
// Compute QR factorization of K
ordinal_type ws_size, info;
value_type ws_size_query;
Teuchos::Array<value_type> tau(pce_sz);
Teuchos::LAPACK<ordinal_type,value_type> lapack;
lapack.GEQRF(pce_sz, pce_sz, K.values(), K.stride(), &tau[0],
&ws_size_query, -1, &info);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"GEQRF returned value " << info);
ws_size = static_cast<ordinal_type>(ws_size_query);
Teuchos::Array<value_type> work(ws_size);
lapack.GEQRF(pce_sz, pce_sz, K.values(), K.stride(), &tau[0],
&work[0], ws_size, &info);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"GEQRF returned value " << info);
// Get Q
lapack.ORGQR(pce_sz, pce_sz, pce_sz, K.values(), K.stride(), &tau[0],
&ws_size_query, -1, &info);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"ORGQR returned value " << info);
ws_size = static_cast<ordinal_type>(ws_size_query);
work.resize(ws_size);
lapack.ORGQR(pce_sz, pce_sz, pce_sz, K.values(), K.stride(), &tau[0],
&work[0], ws_size, &info);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"ORGQR returned value " << info);
// Get basis vectors
for (ordinal_type j=0; j<p+1; j++)
for (ordinal_type i=0; i<pce_sz; i++)
basis_vecs(i,j) = K(i,j);
// Compute T = Q'*A*Q
matrix_type prod(pce_sz,pce_sz);
prod.multiply(Teuchos::TRANS, Teuchos::NO_TRANS, 1.0, K, A, 0.0);
matrix_type T(pce_sz,pce_sz);
T.multiply(Teuchos::NO_TRANS, Teuchos::NO_TRANS, 1.0, prod, K, 0.0);
//std::cout << T << std::endl;
// Recursion coefficients are diagonal and super diagonal
b[0] = 1.0;
for (ordinal_type i=0; i<pce_sz-1; i++) {
a[i] = T(i,i);
b[i+1] = T(i,i+1);
}
a[pce_sz-1] = T(pce_sz-1,pce_sz-1);
// Setup rest of basis
this->setup();
// Project original PCE into the new basis
vector_type u(pce_sz);
for (ordinal_type i=0; i<pce_sz; i++)
u[i] = normalized_pce[i];
new_pce.multiply(Teuchos::TRANS, Teuchos::NO_TRANS, 1.0, basis_vecs, u,
0.0);
for (ordinal_type i=0; i<=p; i++)
new_pce[i] /= this->norms[i];
}
//Draw this plane using OpenGL.
void MGConstructionPlane::draw()const{
if(disabled()||!valid())
return;
glPushAttrib(GL_LINE_BIT);//=========GL_ALL_ATTRIB_BITS
glDisable(GL_LIGHTING);
glDisable(GL_LINE_STIPPLE);
const MGPosition& origin=m_plane.root_point();
const double* origind=origin.data();
MGVector udire=m_plane.u_deriv()*m_uspan;
MGVector vdire=m_plane.v_deriv()*m_vspan;
MGPosition u0(origin-udire*m_unum), u1(origin+udire*m_unum);
MGPosition v0(origin-vdire*m_vnum), v1(origin+vdire*m_vnum);
int i,j;
MGPosition vS1(v0), vE1(v1), vS2(v0), vE2(v1);
MGPosition uS1(u0), uE1(u1), uS2(u0), uE2(u1);
glLineWidth(1.f);
m_lineColor.exec();
glBegin(GL_LINES);
for(i=10; i<=m_unum; i+=10){
for(int i1=1; i1<=9; i1++){
vS1+=udire; vE1+=udire;
glVertex3dv(vS1.data());glVertex3dv(vE1.data());
vS2-=udire; vE2-=udire;
glVertex3dv(vS2.data());glVertex3dv(vE2.data());
}
vS1+=udire; vE1+=udire;
vS2-=udire; vE2-=udire;
}
for(j=10; j<=m_vnum; j+=10){
for(int j1=1; j1<=9; j1++){
uS1+=vdire; uE1+=vdire;
glVertex3dv(uS1.data());glVertex3dv(uE1.data());
uS2-=vdire; uE2-=vdire;
glVertex3dv(uS2.data());glVertex3dv(uE2.data());
}
uS1+=vdire; uE1+=vdire;
uS2-=vdire; uE2-=vdire;
}
glEnd();
glLineWidth(2.f);
////u minus axis and v minus axis.
glBegin(GL_LINE_STRIP);
glVertex3dv(u0.data());glVertex3dv(origind);glVertex3dv(v0.data());
glEnd();
vS1=vS2=v0; vE1=vE2=v1;
uS1=uS2=u0; uE1=uE2=u1;
glBegin(GL_LINES);
MGVector udire10=udire*10.;
for(i=10; i<=m_unum; i+=10){
vS1+=udire10; vE1+=udire10;
glVertex3dv(vS1.data());glVertex3dv(vE1.data());
vS2-=udire10; vE2-=udire10;
glVertex3dv(vS2.data());glVertex3dv(vE2.data());
}
MGVector vdire10=vdire*10.;
for(j=10; j<=m_vnum; j+=10){
uS1+=vdire10; uE1+=vdire10;
glVertex3dv(uS1.data());glVertex3dv(uE1.data());
uS2-=vdire10; uE2-=vdire10;
glVertex3dv(uS2.data());glVertex3dv(uE2.data());
}
glEnd();
glLineWidth(2.f);
m_uaxisColor.exec();////u plus axis.
glBegin(GL_LINES);
glVertex3dv(origind);glVertex3dv(u1.data());
glEnd();
m_vaxisColor.exec();////v plus axis.
glBegin(GL_LINES);
glVertex3dv(origind);glVertex3dv(v1.data());
glEnd();
glPopAttrib();
}
int setupAndSolveQP(NewQPControllerData *pdata, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input, double t, Map<VectorXd> &q, Map<VectorXd> &qd, const Ref<Matrix<bool, Dynamic, 1>> &b_contact_force, QPControllerOutput *qp_output, std::shared_ptr<QPControllerDebugData> debug) {
// The primary solve loop for our controller. This constructs and solves a Quadratic Program and produces the instantaneous desired torques, along with reference positions, velocities, and accelerations. It mirrors the Matlab implementation in atlasControllers.InstantaneousQPController.setupAndSolveQP(), and more documentation can be found there.
// Note: argument `debug` MAY be set to NULL, which signals that no debug information is requested.
// look up the param set by name
AtlasParams *params;
std::map<string,AtlasParams>::iterator it;
it = pdata->param_sets.find(qp_input->param_set_name);
if (it == pdata->param_sets.end()) {
mexWarnMsgTxt("Got a param set I don't recognize! Using standing params instead");
it = pdata->param_sets.find("standing");
if (it == pdata->param_sets.end()) {
mexErrMsgTxt("Could not fall back to standing parameters either. I have to give up here.");
}
}
// cout << "using params set: " + it->first + ", ";
params = &(it->second);
// mexPrintf("Kp_accel: %f, ", params->Kp_accel);
int nu = pdata->B.cols();
int nq = pdata->r->num_positions;
// zmp_data
Map<Matrix<double, 4, 4, RowMajor>> A_ls(&qp_input->zmp_data.A[0][0]);
Map<Matrix<double, 4, 2, RowMajor>> B_ls(&qp_input->zmp_data.B[0][0]);
Map<Matrix<double, 2, 4, RowMajor>> C_ls(&qp_input->zmp_data.C[0][0]);
Map<Matrix<double, 2, 2, RowMajor>> D_ls(&qp_input->zmp_data.D[0][0]);
Map<Matrix<double, 4, 1>> x0(&qp_input->zmp_data.x0[0][0]);
Map<Matrix<double, 2, 1>> y0(&qp_input->zmp_data.y0[0][0]);
Map<Matrix<double, 2, 1>> u0(&qp_input->zmp_data.u0[0][0]);
Map<Matrix<double, 2, 2, RowMajor>> R_ls(&qp_input->zmp_data.R[0][0]);
Map<Matrix<double, 2, 2, RowMajor>> Qy(&qp_input->zmp_data.Qy[0][0]);
Map<Matrix<double, 4, 4, RowMajor>> S(&qp_input->zmp_data.S[0][0]);
Map<Matrix<double, 4, 1>> s1(&qp_input->zmp_data.s1[0][0]);
Map<Matrix<double, 4, 1>> s1dot(&qp_input->zmp_data.s1dot[0][0]);
// // whole_body_data
if (qp_input->whole_body_data.num_positions != nq) mexErrMsgTxt("number of positions doesn't match num_dof for this robot");
Map<VectorXd> q_des(qp_input->whole_body_data.q_des.data(), nq);
Map<VectorXd> condof(qp_input->whole_body_data.constrained_dofs.data(), qp_input->whole_body_data.num_constrained_dofs);
PIDOutput pid_out = wholeBodyPID(pdata, t, q, qd, q_des, ¶ms->whole_body);
qp_output->q_ref = pid_out.q_ref;
// mu
// NOTE: we're using the same mu for all supports
double mu;
if (qp_input->num_support_data == 0) {
mu = 1.0;
} else {
mu = qp_input->support_data[0].mu;
for (int i=1; i < qp_input->num_support_data; i++) {
if (qp_input->support_data[i].mu != mu) {
mexWarnMsgTxt("Currently, we assume that all supports have the same value of mu");
}
}
}
const int dim = 3, // 3D
nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now
assert(nu+6 == nq);
vector<DesiredBodyAcceleration> desired_body_accelerations;
desired_body_accelerations.resize(qp_input->num_tracked_bodies);
Vector6d body_pose_des, body_v_des, body_vdot_des;
Vector6d body_vdot;
for (int i=0; i < qp_input->num_tracked_bodies; i++) {
int body_id0 = qp_input->body_motion_data[i].body_id - 1;
double weight = params->body_motion[body_id0].weight;
desired_body_accelerations[i].body_id0 = body_id0;
Map<Matrix<double, 6, 4,RowMajor>>coefs_rowmaj(&qp_input->body_motion_data[i].coefs[0][0]);
Matrix<double, 6, 4> coefs = coefs_rowmaj;
evaluateCubicSplineSegment(t - qp_input->body_motion_data[i].ts[0], coefs, body_pose_des, body_v_des, body_vdot_des);
desired_body_accelerations[i].body_vdot = bodyMotionPD(pdata->r, q, qd, body_id0, body_pose_des, body_v_des, body_vdot_des, params->body_motion[body_id0].Kp, params->body_motion[body_id0].Kd);
desired_body_accelerations[i].weight = weight;
desired_body_accelerations[i].accel_bounds = params->body_motion[body_id0].accel_bounds;
// mexPrintf("body: %d, vdot: %f %f %f %f %f %f weight: %f\n", body_id0,
// desired_body_accelerations[i].body_vdot(0),
// desired_body_accelerations[i].body_vdot(1),
// desired_body_accelerations[i].body_vdot(2),
// desired_body_accelerations[i].body_vdot(3),
// desired_body_accelerations[i].body_vdot(4),
// desired_body_accelerations[i].body_vdot(5),
// weight);
// mexPrintf("tracking body: %d, coefs[:,0]: %f %f %f %f %f %f coefs(", body_id0,
}
int n_body_accel_eq_constraints = 0;
for (int i=0; i < desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight < 0)
n_body_accel_eq_constraints++;
}
MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;
pdata->r->doKinematics(q,false,qd);
//---------------------------------------------------------------------
vector<SupportStateElement> available_supports = loadAvailableSupports(qp_input);
vector<SupportStateElement> active_supports = getActiveSupports(pdata->r, pdata->map_ptr, q, qd, available_supports, b_contact_force, params->contact_threshold, pdata->default_terrain_height);
int num_active_contact_pts=0;
for (vector<SupportStateElement>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
num_active_contact_pts += iter->contact_pts.size();
}
pdata->r->HandC(q,qd,(MatrixXd*)nullptr,pdata->H,pdata->C,(MatrixXd*)nullptr,(MatrixXd*)nullptr,(MatrixXd*)nullptr);
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head(6);
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (params->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
pdata->Ak = pdata->Ag.topRows(3);
pdata->Akdot = pdata->Agdot.topRows(3);
}
Vector3d xcom;
// consider making all J's into row-major
pdata->r->getCOM(xcom);
pdata->r->getCOMJac(pdata->J);
pdata->r->getCOMJacDot(pdata->Jdot);
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdot_xy = pdata->Jdot.topRows(2);
MatrixXd Jcom,Jcomdot;
if (x0.size()==6) {
Jcom = pdata->J;
Jcomdot = pdata->Jdot;
}
else {
Jcom = pdata->J_xy;
Jcomdot = pdata->Jdot_xy;
}
MatrixXd B,JB,Jp,Jpdot,normals;
int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,B,JB,Jp,Jpdot,normals,pdata->default_terrain_height);
int neps = nc*dim;
VectorXd x_bar,xlimp;
MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
if (nc>0) {
if (x0.size()==6) {
// x,y,z com
xlimp.resize(6);
xlimp.topRows(3) = xcom;
xlimp.bottomRows(3) = Jcom*qd;
}
else {
xlimp.resize(4);
xlimp.topRows(2) = xcom.topRows(2);
xlimp.bottomRows(2) = Jcom*qd;
}
x_bar = xlimp-x0;
D_float = JB.topRows(6);
D_act = JB.bottomRows(nu);
}
int nf = nc*nd; // number of contact force variables
int nparams = nq+nf+neps;
Vector3d kdot_des;
if (include_angular_momentum) {
VectorXd k = pdata->Ak*qd;
kdot_des = -params->Kp_ang*k; // TODO: parameterize
}
//----------------------------------------------------------------------
// QP cost function ----------------------------------------------------
//
// min: ybar*Qy*ybar + ubar*R*ubar + (2*S*xbar + s1)*(A*x + B*u) +
// w_qdd*quad(qddot_ref - qdd) + w_eps*quad(epsilon) +
// w_grf*quad(beta) + quad(kdot_des - (A*qdd + Adot*qd))
VectorXd f(nparams);
{
if (nc > 0) {
// NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi)
VectorXd tmp = C_ls*xlimp;
VectorXd tmp1 = Jcomdot*qd;
MatrixXd tmp2 = R_DQyD_ls*Jcom;
pdata->fqp = tmp.transpose()*Qy*D_ls*Jcom;
// mexPrintf("fqp head: %f %f %f\n", pdata->fqp(0), pdata->fqp(1), pdata->fqp(2));
pdata->fqp += tmp1.transpose()*tmp2;
pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*Jcom;
pdata->fqp -= u0.transpose()*tmp2;
pdata->fqp -= y0.transpose()*Qy*D_ls*Jcom;
pdata->fqp -= (params->whole_body.w_qdd.array()*pid_out.qddot_des.array()).matrix().transpose();
if (include_angular_momentum) {
pdata->fqp += qd.transpose()*pdata->Akdot.transpose()*params->W_kdot*pdata->Ak;
pdata->fqp -= kdot_des.transpose()*params->W_kdot*pdata->Ak;
}
f.head(nq) = pdata->fqp.transpose();
} else {
f.head(nq) = -pid_out.qddot_des;
}
}
f.tail(nf+neps) = VectorXd::Zero(nf+neps);
int neq = 6+neps+6*n_body_accel_eq_constraints+qp_input->whole_body_data.num_constrained_dofs;
MatrixXd Aeq = MatrixXd::Zero(neq,nparams);
VectorXd beq = VectorXd::Zero(neq);
// constrained floating base dynamics
// H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float
Aeq.topLeftCorner(6,nq) = pdata->H_float;
beq.topRows(6) = -pdata->C_float;
if (nc>0) {
Aeq.block(0,nq,6,nc*nd) = -D_float;
}
if (nc > 0) {
// relative acceleration constraint
Aeq.block(6,0,neps,nq) = Jp;
Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf); // note: obvious sparsity here
Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps); // note: obvious sparsity here
beq.segment(6,neps) = (-Jpdot -params->Kp_accel*Jp)*qd;
}
// add in body spatial equality constraints
// VectorXd body_vdot;
MatrixXd orig = MatrixXd::Zero(4,1);
orig(3,0) = 1;
int equality_ind = 6+neps;
MatrixXd Jb(6,nq);
MatrixXd Jbdot(6,nq);
for (int i=0; i<desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight < 0) { // negative implies constraint
if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) {
pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(desired_body_accelerations[i].body_vdot(j))) {
Aeq.block(equality_ind,0,1,nq) = Jb.row(j);
beq[equality_ind++] = -Jbdot.row(j)*qd + desired_body_accelerations[i].body_vdot(j);
}
}
}
}
}
if (qp_input->whole_body_data.num_constrained_dofs>0) {
// add joint acceleration constraints
for (int i=0; i<qp_input->whole_body_data.num_constrained_dofs; i++) {
Aeq(equality_ind,(int)condof[i]-1) = 1;
beq[equality_ind++] = pid_out.qddot_des[(int)condof[i]-1];
}
}
int n_ineq = 2*nu+2*6*desired_body_accelerations.size();
MatrixXd Ain = MatrixXd::Zero(n_ineq,nparams); // note: obvious sparsity here
VectorXd bin = VectorXd::Zero(n_ineq);
// linear input saturation constraints
// u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta)
// using transpose instead of inverse because B is orthogonal
Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act;
Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act;
bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax;
Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams);
bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin;
int constraint_start_index = 2*nu;
for (int i=0; i<desired_body_accelerations.size(); i++) {
pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = Jb;
bin.segment(constraint_start_index,6) = -Jbdot*qd + desired_body_accelerations[i].accel_bounds.max;
constraint_start_index += 6;
Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = -Jb;
bin.segment(constraint_start_index,6) = Jbdot*qd - desired_body_accelerations[i].accel_bounds.min;
constraint_start_index += 6;
}
for (int i=0; i<n_ineq; i++) {
// remove inf constraints---needed by gurobi
if (std::isinf(double(bin(i)))) {
Ain.row(i) = 0*Ain.row(i);
bin(i)=0;
}
}
GRBmodel * model = nullptr;
int info=-1;
// set obj,lb,up
VectorXd lb(nparams), ub(nparams);
lb.head(nq) = pdata->qdd_lb;
ub.head(nq) = pdata->qdd_ub;
lb.segment(nq,nf) = VectorXd::Zero(nf);
ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf);
lb.tail(neps) = -params->slack_limit*VectorXd::Ones(neps);
ub.tail(neps) = params->slack_limit*VectorXd::Ones(neps);
VectorXd alpha(nparams);
MatrixXd Qnfdiag(nf,1), Qneps(neps,1);
vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 ); // nq, nf, neps // this one is for gurobi
VectorXd w = (params->whole_body.w_qdd.array() + REG).matrix();
#ifdef USE_MATRIX_INVERSION_LEMMA
double max_body_accel_weight = -numeric_limits<double>::infinity();
for (int i=0; i < desired_body_accelerations.size(); i++) {
max_body_accel_weight = max(max_body_accel_weight, desired_body_accelerations[i].weight);
}
bool include_body_accel_cost_terms = desired_body_accelerations.size() > 0 && max_body_accel_weight > 1e-10;
if (pdata->use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms)
{
// TODO: update to include angular momentum, body accel objectives.
// We want Hqp inverse, which I can compute efficiently using the
// matrix inversion lemma (see wikipedia):
// inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A)
if (nc>0) {
MatrixXd Wi = ((1/(params->whole_body.w_qdd.array() + REG)).matrix()).asDiagonal();
if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero
pdata->Hqp = Wi - Wi*Jcom.transpose()*(R_DQyD_ls.inverse() + Jcom*Wi*Jcom.transpose()).inverse()*Jcom*Wi;
}
}
else {
pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG));
}
#ifdef TEST_FAST_QP
if (nc>0) {
MatrixXd Hqp_test(nq,nq);
MatrixXd W = w.asDiagonal();
Hqp_test = (Jcom.transpose()*R_DQyD_ls*Jcom + W).inverse();
if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) {
mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse.");
}
}
#endif
Qnfdiag = MatrixXd::Constant(nf,1,1/REG);
Qneps = MatrixXd::Constant(neps,1,1/(.001+REG));
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha);
//if (info<0) mexPrintf("fastQP info = %d. Calling gurobi.\n", info);
}
else {
#endif
if (nc>0) {
pdata->Hqp = Jcom.transpose()*R_DQyD_ls*Jcom;
if (include_angular_momentum) {
pdata->Hqp += pdata->Ak.transpose()*params->W_kdot*pdata->Ak;
}
pdata->Hqp += params->whole_body.w_qdd.asDiagonal();
pdata->Hqp += REG*MatrixXd::Identity(nq,nq);
} else {
pdata->Hqp = (1+REG)*MatrixXd::Identity(nq,nq);
}
// add in body spatial acceleration cost terms
for (int i=0; i<desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight > 0) {
if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) {
pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(desired_body_accelerations[i].body_vdot[j])) {
pdata->Hqp += desired_body_accelerations[i].weight*(Jb.row(j)).transpose()*Jb.row(j);
f.head(nq) += desired_body_accelerations[i].weight*(qd.transpose()*Jbdot.row(j).transpose() - desired_body_accelerations[i].body_vdot[j])*Jb.row(j).transpose();
}
}
}
}
}
Qnfdiag = MatrixXd::Constant(nf,1,params->w_grf+REG);
Qneps = MatrixXd::Constant(neps,1,params->w_slack+REG);
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
if (pdata->use_fast_qp > 0)
{ // set up and call fastqp
info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha);
//if (info<0) mexPrintf("fastQP info=%d... calling Gurobi.\n", info);
}
else {
// use gurobi active set
model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.vbasis,pdata->state.vbasis_len,pdata->state.cbasis,pdata->state.cbasis_len,alpha);
CGE(GRBgetintattr(model,"NumVars",&(pdata->state.vbasis_len)), pdata->env);
CGE(GRBgetintattr(model,"NumConstrs",&(pdata->state.cbasis_len)), pdata->env);
info=66;
//info = -1;
}
if (info<0) {
model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.active,alpha);
int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env);
//if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status);
}
#ifdef USE_MATRIX_INVERSION_LEMMA
}
#endif
//----------------------------------------------------------------------
// Solve for inputs ----------------------------------------------------
qp_output->qdd = alpha.head(nq);
VectorXd beta = alpha.segment(nq,nc*nd);
// use transpose because B_act is orthogonal
qp_output->u = pdata->B_act.transpose()*(pdata->H_act*qp_output->qdd + pdata->C_act - D_act*beta);
//y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta);
bool foot_contact[2];
foot_contact[0] = b_contact_force(pdata->rpc.body_ids.r_foot) == 1;
foot_contact[1] = b_contact_force(pdata->rpc.body_ids.l_foot) == 1;
qp_output->qd_ref = velocityReference(pdata, t, q, qd, qp_output->qdd, foot_contact, &(params->vref_integrator), &(pdata->rpc));
// Remember t for next time around
pdata->state.t_prev = t;
// If a debug pointer was passed in, fill it with useful data
if (debug) {
debug->active_supports.resize(active_supports.size());
for (int i=0; i < active_supports.size(); i++) {
debug->active_supports[i] = active_supports[i];
}
debug->nc = nc;
debug->normals = normals;
debug->B = B;
debug->alpha = alpha;
debug->f = f;
debug->Aeq = Aeq;
debug->beq = beq;
debug->Ain_lb_ub = Ain_lb_ub;
debug->bin_lb_ub = bin_lb_ub;
debug->Qnfdiag = Qnfdiag;
debug->Qneps = Qneps;
debug->x_bar = x_bar;
debug->S = S;
debug->s1 = s1;
debug->s1dot = s1dot;
debug->s2dot = qp_input->zmp_data.s2dot;
debug->A_ls = A_ls;
debug->B_ls = B_ls;
debug->Jcom = Jcom;
debug->Jcomdot = Jcomdot;
debug->beta = beta;
}
// if we used gurobi, clean up
if (model) {
GRBfreemodel(model);
}
// GRBfreeenv(env);
return info;
}
int main(int argc,char *argv[]) {
int_8 N, l;
float cfl = 0.5;
clock_t t;
if (argc == 1) { N = 20; l = 0;}
else if (argc == 2) {N = atoi(argv[1]); l = 0;}
else if (argc == 3) {N = atoi(argv[1]); l = atoi(argv[2]);}
else {N = atoi(argv[1]); l = atoi(argv[2]); cfl = atof(argv[3]); }
double L = 20;
Block2* grid = new Block2({0, 0, 0}, {L, 0.1*L, 0}, N, 1);
grid->cfl = cfl;
grid->levelHighBound[0] = l;
grid->levelHighBound[1] = 0;
grid->levelHighBound[2] = 0;
grid->addVar({"phi", "phi2", "ref"});
auto u = grid->getVar("u");
auto v = grid->getVar("v");
auto phi = grid->getVar("phi");
auto phi2 = grid->getVar("phi2");
auto ref = grid->getVar("ref");
phi->set(0);
u->set(0.1);
v->set(0.0);
VecX<double> u0(grid->listCell.size(), 0.0);
auto pi= 4*atan(1.0);
for (auto i = 0; i < grid->listCell.size(); ++i) {
auto x = grid->listCell[i]->getCoord();
u0[i] = 1 + sin(pi*x[0]/L);
}
u->set(u0);
phi->itmax = 10;
phi->tol = 1e-8;
phi->solver = "Gauss";
double xp = 1.8;
double xm = 6.0;
double e = 1.0/N/pow(2,grid->levelMax[0])*(2);
for (auto c:grid->listCell) {
auto h= heavy(c->getCoord().x()-xp, e)-heavy(c->getCoord().x()-xm, e);
phi->set(c->id, h);
phi2->set(c->id, h);
ref->set(c->id, h);
}
for (auto j = 0; j < l; ++j) {
grid->solBasedAdapt2(grid->getError(ref), 1e-5, 1e-3);
//grid->valAdapt(phi, 0.1, 0.9);
grid->adapt();
double e = 1.0/N/pow(2,grid->levelMax[0])*(2);
for (auto c:grid->listCell) {
auto h= heavy(c->getCoord().x()-xp, e)-heavy(c->getCoord().x()-xm, e);
phi->set(c->id, h);
phi2->set(c->id, h);
ref->set(c->id, h);
}
}
grid->writeVTK("H");
double endTime = L; //0.8/u->data.max();
double tend = endTime;
// endTime = 1;
auto vel = grid->getVel();
double time = 0;
auto writeTime = 0.1;
auto writeCnt = writeTime;
while (time <= tend) {
for (auto i = 0; i < grid->listCell.size(); ++i) {
auto x = grid->listCell[i]->getCoord();
u->set(i, (1.0 + sin(pi*x.x()/L))*cos(2*pi*time/tend));
}
cout << u->data.max() << " " << u->data.min() <<endl;
grid->setDt(2.0);
time += grid->dt;
auto up = grid->interp(u, xp);
auto um = grid->interp(u, xm);
xp += up*grid->dt;
xm += um*grid->dt;
for (auto c:grid->listCell) {
auto h= heavy(c->getCoord().x()-xp, e)-heavy(c->getCoord().x()-xm, e);
ref->set(c->id, h);
}
for (auto i = 0; i < grid->listCell.size(); ++i) {
auto x = grid->listCell[i]->getCoord();
phi2->set(i, grid->interp(phi2, x - Vec3(u->get(i), v->get(i), 0)*grid->dt));
}
writeCnt -= grid->dt;
auto vel = grid->getVel();
cout << "Time: " << time << " " << grid->dt << " --> " << "xp: " << xp << "xm: "<< xm << endl;
grid->advanceDiv(phi, vel); //, true);
// FOU
// grid->lockBC(phi);
// phi->solve(grid->ddt(1.0)+grid->divRK2E(vel,1.0));
// grid->unlockBC();
// for (auto f:grid->listFace) {
// int n = f->next;
// int p = f->prev;
// Vec3 norm = f->vol();
// auto xf = f->getCoord();
// double phif;
// auto nid = grid->searchVertexbyCoords(xf, f->node[0]);
// auto uf1 = Vec3(grid->listVertex[nid]->evalPhi(u, &xf),0,0);
// // Vec3 x1 = xf - 0.125*grid->dt*uf1;
// // nid = grid->searchVertexbyCoords(x1, nid);
// // auto uf2 = Vec3(grid->listVertex[nid]->evalPhi(u, &x1),0,0);
// // Vec3 x2 = xf - 0.250*grid->dt*uf2;
// // nid = grid->searchVertexbyCoords(x2, nid);
// // auto uf3 = Vec3(grid->listVertex[nid]->evalPhi(u, &x2),0,0);
// // Vec3 x3 = xf - 0.5*grid->dt*(uf1 - 2*uf2 + 2*uf3);
// // nid = grid->searchVertexbyCoords(x3, nid);
// // auto uf4 = Vec3(grid->listVertex[nid]->evalPhi(u, &x3),0,0);
// // Vec3 x0 = xf - grid->dt/12*(uf1 + 4*uf3 - uf4);
// Vec3 x0 = xf - grid->dt*0.5*uf1;
// auto uf = uf1;
// double flux = uf*norm;
// // if (p>=0 && n>=0) {
// // FOU
// // if (flux > 0)
// // flux *= phi->get(p);
// // else
// // flux *= phi->get(n);
// // nid = grid->searchVertexbyCoords(x0, nid);
// phif = max(0.0, min(1.0, grid->interp(phi, x0, nid))); //listVertex[nid]->evalPhi(phi, &x0)));
// auto invvoln = (n < 0) ? 1.0 : 1.0/grid->listCell[n]->vol().abs();
// auto invvolp = (p < 0) ? 1.0 : 1.0/grid->listCell[p]->vol().abs();
// if (n >= 0) phi->data[n] += flux*phif*grid->dt*invvoln;
// if (p >= 0) phi->data[p] -= flux*phif*grid->dt*invvolp;
// // }
// }
grid->solBasedAdapt2(grid->getError(phi)); //, 1e-5, 1e-3);// grid->valGrad(phi)); //
//grid->valAdapt(phi, 0.1, 0.9);
grid->adapt();
if (writeCnt <= 0 || time >= endTime) {
grid->writeVTK("H");
writeCnt = writeTime;
}
}
// NEW
// for (auto f:grid->listFace) {
// int n = f->next;
// int p = f->prev;
// auto xf = f->getCoord();
// double uf, phif;
// auto nid = grid->searchVertexbyCoords(xf, f->node[0]);
// uf = grid->listVertex[nid]->evalPhi(u, &xf);
// Vec3 x0 = xf - Vec3(0.5*grid->dt*uf,0,0);
// nid = grid->searchVertexbyCoords(x0, nid);
// phif = grid->listVertex[nid]->evalPhi(phi, &x0);
// cout << phif << " ";
// }
// // Speed test: Matrix construction and solution old and new techniques
// LinSys old(N); VecX<double> x0(N); VecX<double> err0(N);
// old.x = &x0;
// old.error = &err0;
// t = clock();
// for (auto i =0; i < N; ++i) {
// old.A[i][i] = 4;
// if (i > 0) old.A[i][i-1] = -1;
// if (i < N-1) old.A[i][i+1] = -1;
// if (i-N >= 0) old.A[i][i-N] = -1;
// if (i+N < N) old.A[i][i+N] = -1;
// old.b[i] = 3*i+1;
// }
// old.setVECX(old.A*old.b);
// t = clock() - t;
// float setup0 = float(t)/CLOCKS_PER_SEC;
// t = clock();
// old.BiCGSTAB();
// t = clock() - t;
// float solve0 = float(t)/CLOCKS_PER_SEC;
// //cout << *old.x << endl;
// cout << old.error->abs() << endl;
// triLinSys sp(N); VecX<double> x1(N); VecX<double> err1(N);
// sp.x = &x1;
// sp.error = &err1;
// t = clock();
// for (auto i =0; i < N; ++i) {
// sp.A += {(double)i, (double)i, 4}; //old.A[i][i] = 2;
// if (i > 0) sp.A += {(double)i, (double)i-1, -1}; //old.A[i][i-1] = -1;
// if (i < N-1) sp.A += {(double)i, (double)i+1, -1}; //old.A[i][i+1] = -1;
// if (i-N >= 0) sp.A += {(double)i, (double)i-N, -1}; //old.A[i][i-1] = -1;
// if (i+N < N) sp.A += {(double)i, (double)i+N, -1}; //old.A[i][i+1] = -1;
// sp.b[i] = 3*i+1;
// }
// sp.setMat(sp.A);
// sp.b = sp.S*sp.b;
// t = clock()-t;
// float setup1 = float(t)/CLOCKS_PER_SEC;
// t = clock();
// sp.BiCGSTAB();
// t = clock()-t;
// float solve1 = float(t)/CLOCKS_PER_SEC;
// cout << " ----------- " << endl ;
// //cout << *sp.x << endl;
// cout << sp.error->abs() << endl;
// cout << "OLD: setup-> " << setup0 << " sec, solve-> " << solve0 << "sec."<<endl;
// cout << "NEW: setup-> " << setup1 << " sec, solve-> " << solve1 << "sec."<<endl;
// // cout << "Abs" << endl;
// Triplets vt, bt;
// triLinSys Ab;
// vt += {{0,0,4}, {0,1,-1}, {1,2,-1}, {2,1,-1}};
// vt += {{2,2,4}, {3,2,-1}, {3,3,4}, {3,4,-1}};
// bt += {{4,4,4}, {4,3,-1}, {1,1,4}, {2,3,-1}};
// bt += {{1,0,-1}, {2,1,-2}, {1,4,-6}, {2,2,1}};
// bt(2,2) = 7;
// Ab.setMat(-(2*vt-bt));
// for (auto i = 0; i < Ab.S.aij.size(); ++i) {
// cout << i << " " << Ab.S.aij[i] << " " << Ab.S.val[i] << endl;
// }
// // VecX<double> b(5);
// // cout << "----"<< endl<< Ab.A.rank << " " << b.size() << endl;
// // for (auto i = 0; i < Ab.A.rank; ++i) b[i] = i+1;
// // Ab.b = Ab.A*b;
// // old.setVECX(Ab.b);
// // VecX<double> x(b.size());
// // Ab.error = &b;
// // Ab.x = &x;
// // Ab.BiCGSTAB();
// // cout << *Ab.x << endl;
// // cout << Ab.error->abs() << endl;
// // cout << old.A<<endl;
// // cout << old.b<<endl;
// // old.error = &b;
// // old.x = &x;
// // old.BiCGSTAB();
// // cout << *old.x << endl;
// // cout << old.error->abs() << endl;
return 0;
};
ASMmxBase::VolumeVec ASMmxBase::establishBases(Go::SplineVolume* svol,
MixedType type)
{
VolumeVec result(2);
// With mixed methods we need two separate spline spaces
if (type == FULL_CONT_RAISE_BASIS1 || type == FULL_CONT_RAISE_BASIS2)
{
// basis1 should be one degree higher than basis2 and C^p-1 continuous
int ndim = svol->dimension();
Go::BsplineBasis b1 = svol->basis(0).extendedBasis(svol->order(0)+1);
Go::BsplineBasis b2 = svol->basis(1).extendedBasis(svol->order(1)+1);
Go::BsplineBasis b3 = svol->basis(2).extendedBasis(svol->order(2)+1);
/* To lower order and regularity this can be used instead
std::vector<double>::const_iterator first = ++surf->basis(0).begin();
std::vector<double>::const_iterator last = --surf->basis(0).end();
Go::BsplineBasis b1 = Go::BsplineBasis(surf->order_u()-1,first,last);
first = ++surf->basis(1).begin();
last = --surf->basis(1).end();
Go::BsplineBasis b2 = Go::BsplineBasis(surf->order_v()-1,first,last);
*/
// Compute parameter values of the Greville points
size_t i;
RealArray ug(b1.numCoefs()), vg(b2.numCoefs()), wg(b3.numCoefs());
for (i = 0; i < ug.size(); i++)
ug[i] = b1.grevilleParameter(i);
for (i = 0; i < vg.size(); i++)
vg[i] = b2.grevilleParameter(i);
for (i = 0; i < wg.size(); i++)
wg[i] = b3.grevilleParameter(i);
if (svol->rational()) {
std::vector<double> rCoefs(svol->rcoefs_begin(), svol->rcoefs_end());
// we normally would set coefs as (x*w, y*w, w)
// however, gotools use this representation internally already.
// instance a Bspline surface in ndim+1
Go::SplineVolume vol2(svol->basis(0), svol->basis(1), svol->basis(2), rCoefs.begin(), ndim+1, false);
// interpolate the Bspline surface onto new basis
RealArray XYZ((ndim+1)*ug.size()*vg.size()*wg.size());
vol2.gridEvaluator(XYZ,ug,vg,wg);
std::unique_ptr<Go::SplineVolume> svol3(Go::VolumeInterpolator::regularInterpolation(b1,b2,b3,ug,vg,wg,XYZ,ndim+1,false,XYZ));
// new rational coefs are (x/w', y/w', w')
// apparently gotools will rescale coeffs on surface creation.
result[0].reset(new Go::SplineVolume(svol3->basis(0), svol3->basis(1), svol3->basis(2), svol3->coefs_begin(), ndim, true));
} else {
RealArray XYZ(ndim*ug.size()*vg.size()*wg.size());
// Evaluate the spline surface at all points
svol->gridEvaluator(ug,vg,wg,XYZ);
// Project the coordinates onto the new basis (the 2nd XYZ is dummy here)
result[0].reset(Go::VolumeInterpolator::regularInterpolation(b1,b2,b3,
ug,vg,wg,XYZ,ndim,
false,XYZ));
}
result[1].reset(new Go::SplineVolume(*svol));
}
else if (type == REDUCED_CONT_RAISE_BASIS1 || type == REDUCED_CONT_RAISE_BASIS2)
{
// Order-elevate basis1 such that it is of one degree higher than basis2
// but only C^p-2 continuous
result[0].reset(new Go::SplineVolume(*svol));
result[0]->raiseOrder(1,1,1);
result[1].reset(new Go::SplineVolume(*svol));
}
else if (ASMmxBase::Type == ASMmxBase::DIV_COMPATIBLE)
{
result.resize(4);
// basis1 should be one degree higher than basis2 and C^p-1 continuous
int ndim = svol->dimension();
Go::BsplineBasis a1 = svol->basis(0);
Go::BsplineBasis a2 = svol->basis(1);
Go::BsplineBasis a3 = svol->basis(2);
Go::BsplineBasis b1 = svol->basis(0).extendedBasis(svol->order(0)+1);
Go::BsplineBasis b2 = svol->basis(1).extendedBasis(svol->order(1)+1);
Go::BsplineBasis b3 = svol->basis(2).extendedBasis(svol->order(2)+1);
// Compute parameter values of the Greville points
size_t i;
RealArray u0(a1.numCoefs()), v0(a2.numCoefs()), w0(a3.numCoefs());
for (i = 0; i < u0.size(); i++)
u0[i] = a1.grevilleParameter(i);
for (i = 0; i < v0.size(); i++)
v0[i] = a2.grevilleParameter(i);
for (i = 0; i < w0.size(); i++)
w0[i] = a3.grevilleParameter(i);
RealArray ug(b1.numCoefs()), vg(b2.numCoefs()), wg(b3.numCoefs());
for (i = 0; i < ug.size(); i++)
ug[i] = b1.grevilleParameter(i);
for (i = 0; i < vg.size(); i++)
vg[i] = b2.grevilleParameter(i);
for (i = 0; i < wg.size(); i++)
wg[i] = b3.grevilleParameter(i);
// Evaluate the spline surface at all points
// Project the coordinates onto the new basis (the 2nd XYZ is dummy here)
RealArray XYZ0(ndim*ug.size()*v0.size()*w0.size()), XYZ1(ndim*u0.size()*vg.size()*w0.size()), XYZ2(ndim*u0.size()*v0.size()*wg.size());
svol->gridEvaluator(ug,v0,w0,XYZ0);
svol->gridEvaluator(u0,vg,w0,XYZ1);
svol->gridEvaluator(u0,v0,wg,XYZ2);
result[0].reset(Go::VolumeInterpolator::regularInterpolation(b1,a2,a3,
ug,v0,w0,XYZ0,ndim,
false,XYZ0));
result[1].reset(Go::VolumeInterpolator::regularInterpolation(a1,b2,a3,
u0,vg,w0,XYZ1,ndim,
false,XYZ1));
result[2].reset(Go::VolumeInterpolator::regularInterpolation(a1,a2,b3,
u0,v0,wg,XYZ2,ndim,
false,XYZ2));
result[3].reset(new Go::SplineVolume(*svol));
geoBasis = 4;
}
if (type == FULL_CONT_RAISE_BASIS2 || type == REDUCED_CONT_RAISE_BASIS2)
std::swap(result[0], result[1]);
return result;
}
