boost::shared_ptr<GraspSpecification > GraspSpecification::readFromUrdf(const std::string &filename) {
TiXmlDocument doc( filename );
doc.LoadFile();
std::unique_ptr<GraspSpecification > u_gs(new GraspSpecification);
if (doc.Error())
{
std::cout << "ERROR: GraspSpecification::readFromXml: " << doc.ErrorDesc() << std::endl;
doc.ClearError();
return boost::shared_ptr<GraspSpecification >(NULL);
}
TiXmlElement *elementR = doc.FirstChildElement("robot");
if (!elementR)
{
std::cout << "ERROR: GraspSpecification::readFromXml: " << "Could not find the 'robot' element in the xml file" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
int grasp_specs_count = 0;
for (TiXmlElement* elementGS = elementR->FirstChildElement("grasp_specification"); elementGS; elementGS = elementGS->NextSiblingElement("grasp_specification")) {
grasp_specs_count++;
}
if (grasp_specs_count != 1) {
std::cout << "ERROR: GraspSpecification::readFromXml: wrong number of grasp_specification elements: " << grasp_specs_count << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
TiXmlElement* elementGS = elementR->FirstChildElement("grasp_specification");
for (TiXmlElement* elementI = elementGS->FirstChildElement("init_state"); elementI; elementI = elementI->NextSiblingElement("init_state")) {
const char *str = elementI->Attribute("joint");
if (!str) {
std::cout << "ERROR: GraspSpecification::readFromXml: init_state joint" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
std::string joint_name( str );
str = elementI->Attribute("position");
if (!str) {
std::cout << "ERROR: GraspSpecification::readFromXml: init_state position" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
u_gs->q_init_map_[joint_name] = string2double( str );
}
for (TiXmlElement* elementG = elementGS->FirstChildElement("goal_state"); elementG; elementG = elementG->NextSiblingElement("goal_state")) {
const char *str = elementG->Attribute("joint");
if (!str) {
std::cout << "ERROR: GraspSpecification::readFromXml: init_state joint" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
std::string joint_name( str );
str = elementG->Attribute("position");
if (!str) {
std::cout << "ERROR: GraspSpecification::readFromXml: init_state position" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
u_gs->q_goal_map_[joint_name] = string2double( str );
}
return boost::shared_ptr<GraspSpecification >(new GraspSpecification(*u_gs.get()));
}
int main(int argc, char **argv)
{
try {
const unsigned int d = 2;
const unsigned int p = 5;
// Create product basis
Teuchos::Array< Teuchos::RCP<const Stokhos::OneDOrthogPolyBasis<int,double> > > bases(d);
for (unsigned int i=0; i<d; i++)
bases[i] = Teuchos::rcp(new basis_type(p));
Teuchos::RCP<const Stokhos::CompletePolynomialBasis<int,double> > basis =
Teuchos::rcp(new Stokhos::CompletePolynomialBasis<int,double>(bases));
// Create approximation
Stokhos::OrthogPolyApprox<int,double> x(basis), u(basis), v(basis),
w(basis), w2(basis), w3(basis);
for (unsigned int i=0; i<d; i++) {
x.term(i, 1) = 1.0;
}
// Tensor product quadrature
Teuchos::RCP<const Stokhos::Quadrature<int,double> > quad =
Teuchos::rcp(new Stokhos::TensorProductQuadrature<int,double>(basis));
// Triple product tensor
Teuchos::RCP<Stokhos::Sparse3Tensor<int,double> > Cijk =
basis->computeTripleProductTensor();
// Quadrature expansion
Stokhos::QuadOrthogPolyExpansion<int,double> quad_exp(basis, Cijk, quad);
// Compute PCE via quadrature expansion
quad_exp.sin(u,x);
quad_exp.exp(v,x);
quad_exp.times(w,v,u);
// Compute tensor product Stieltjes basis for u and v
Teuchos::Array< Teuchos::RCP<const Stokhos::OneDOrthogPolyBasis<int,double> > > st_bases(2);
st_bases[0] =
Teuchos::rcp(new Stokhos::StieltjesPCEBasis<int,double>(
p, Teuchos::rcp(&u,false), quad, true));
st_bases[1] =
Teuchos::rcp(new Stokhos::StieltjesPCEBasis<int,double>(
p, Teuchos::rcp(&v,false), quad, true));
Teuchos::RCP<const Stokhos::CompletePolynomialBasis<int,double> > st_basis =
Teuchos::rcp(new Stokhos::CompletePolynomialBasis<int,double>(st_bases));
Stokhos::OrthogPolyApprox<int,double> u_st(st_basis), v_st(st_basis),
w_st(st_basis);
u_st.term(0, 0) = u.mean();
u_st.term(0, 1) = 1.0;
v_st.term(0, 0) = v.mean();
v_st.term(1, 1) = 1.0;
// Use Gram-Schmidt to orthogonalize Stieltjes basis of u and v
Teuchos::Array<double> st_points_0;
Teuchos::Array<double> st_weights_0;
Teuchos::Array< Teuchos::Array<double> > st_values_0;
st_bases[0]->getQuadPoints(p+1, st_points_0, st_weights_0, st_values_0);
Teuchos::Array<double> st_points_1;
Teuchos::Array<double> st_weights_1;
Teuchos::Array< Teuchos::Array<double> > st_values_1;
st_bases[1]->getQuadPoints(p+1, st_points_1, st_weights_1, st_values_1);
Teuchos::Array< Teuchos::Array<double> > st_points(st_points_0.size());
for (int i=0; i<st_points_0.size(); i++) {
st_points[i].resize(2);
st_points[i][0] = st_points_0[i];
st_points[i][1] = st_points_1[i];
}
Teuchos::Array<double> st_weights = st_weights_0;
Teuchos::RCP< Stokhos::GramSchmidtBasis<int,double> > gs_basis =
Teuchos::rcp(new Stokhos::GramSchmidtBasis<int,double>(st_basis,
st_points,
st_weights,
1e-15));
// Create quadrature for Gram-Schmidt basis using quad points and
// and weights from original basis mapped to Stieljtes basis
Teuchos::RCP< const Teuchos::Array< Teuchos::Array<double> > > points =
Teuchos::rcp(&st_points,false);
Teuchos::RCP< const Teuchos::Array<double> > weights =
Teuchos::rcp(&st_weights,false);
Teuchos::RCP<const Stokhos::Quadrature<int,double> > gs_quad =
Teuchos::rcp(new Stokhos::UserDefinedQuadrature<int,double>(gs_basis,
points,
weights));
// Triple product tensor
Teuchos::RCP<Stokhos::Sparse3Tensor<int,double> > gs_Cijk =
gs_basis->computeTripleProductTensor();
// Gram-Schmidt quadrature expansion
Stokhos::QuadOrthogPolyExpansion<int,double> gs_quad_exp(gs_basis,
gs_Cijk,
gs_quad);
Stokhos::OrthogPolyApprox<int,double> u_gs(gs_basis), v_gs(gs_basis),
w_gs(gs_basis);
// Map expansion in Stieltjes basis to Gram-Schmidt basis
gs_basis->transformCoeffs(u_st.coeff(), u_gs.coeff());
gs_basis->transformCoeffs(v_st.coeff(), v_gs.coeff());
// Compute w_gs = u_gs*v_gs in Gram-Schmidt basis
gs_quad_exp.times(w_gs, u_gs, v_gs);
// Project w_gs back to original basis
pce_quad_func gs_func(w_gs, *gs_basis);
quad_exp.binary_op(gs_func, w2, u, v);
// Triple product tensor
Teuchos::RCP<Stokhos::Sparse3Tensor<int,double> > st_Cijk =
st_basis->computeTripleProductTensor();
// Stieltjes quadrature expansion
Teuchos::RCP<const Stokhos::Quadrature<int,double> > st_quad =
Teuchos::rcp(new Stokhos::UserDefinedQuadrature<int,double>(st_basis,
points,
weights));
Stokhos::QuadOrthogPolyExpansion<int,double> st_quad_exp(st_basis,
st_Cijk,
st_quad);
// Compute w_st = u_st*v_st in Stieltjes basis
st_quad_exp.times(w_st, u_st, v_st);
// Project w_st back to original basis
pce_quad_func st_func(w_st, *st_basis);
quad_exp.binary_op(st_func, w3, u, v);
std::cout.precision(12);
std::cout << "w = " << std::endl << w;
std::cout << "w2 = " << std::endl << w2;
std::cout << "w3 = " << std::endl << w3;
std::cout << "w_gs = " << std::endl << w_gs;
std::cout << "w_st = " << std::endl << w_st;
std::cout.setf(std::ios::scientific);
std::cout << "w.mean() = " << w.mean() << std::endl
<< "w2.mean() = " << w2.mean() << std::endl
<< "w3.mean() = " << w3.mean() << std::endl
<< "w_gs.mean() = " << w_gs.mean() << std::endl
<< "w_st.mean() = " << w_st.mean() << std::endl
<< "w.std_dev() = " << w.standard_deviation() << std::endl
<< "w2.std_dev() = " << w2.standard_deviation() << std::endl
<< "w3.std_dev() = " << w3.standard_deviation() << std::endl
<< "w_gs.std_dev() = " << w_gs.standard_deviation() << std::endl
<< "w_st.std_dev() = " << w_st.standard_deviation() << std::endl;
}
catch (std::exception& e) {
std::cout << e.what() << std::endl;
}
}
