Eigen::RowVectorXd Ur5MessageDecoder::getJointVelocitiesRT(QByteArray data)
{
Eigen::RowVectorXd jv(6);
for(int i=0;i<6;i++)
{
jv(i) = pickDouble(data,(37+i)*sizeof(double));
}
return jv;
}
/*
* Bessel function of noninteger order
*/
double yv(double v, double x)
{
double y, t;
int n;
y = floor(v);
if (y == v) {
n = int(v);
y = yn(n, x);
return(y);
}
t = PI * v;
y = (cos(t) * jv(v, x) - jv(-v, x)) / sin(t);
return(y);
}
bool CoMIK::computeSteps(float stepSize, float minumChange, int maxNStep)
{
std::vector<RobotNodePtr> rn = rns->getAllRobotNodes();
RobotPtr robot = rns->getRobot();
std::vector<float> jv(rns->getSize(), 0.0f);
int step = 0;
checkTolerances();
float lastDist = FLT_MAX;
while (step < maxNStep)
{
Eigen::VectorXf dTheta = this->computeStep(stepSize);
// Check for singularities
if (!isValid(dTheta))
{
VR_INFO << "Singular Jacobian" << endl;
return false;
}
for (unsigned int i = 0; i < rn.size(); i++)
{
jv[i] = (rn[i]->getJointValue() + dTheta[i]);
}
//rn[i]->setJointValue(rn[i]->getJointValue() + dTheta[i]);
robot->setJointValues(rns, jv);
// check tolerances
if (checkTolerances())
{
VR_INFO << "Tolerances ok, loop:" << step << endl;
return true;
}
float d = dTheta.norm();
if (dTheta.norm() < minumChange)
{
VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << endl;
return false;
}
if (checkImprovement && d > lastDist)
{
VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << ", last loop's norm:" << lastDist << "), loop:" << step << endl;
return false;
}
lastDist = d;
step++;
}
#ifndef NO_FAILURE_OUTPUT
//VR_INFO << "IK failed, loop:" << step << endl;
//VR_INFO << "error:" << (target - rns->getCoM().head(target.rows())).norm() << endl;
#endif
return false;
}
inline long double cyl_bessel_j(long double a, long double x)
{
#ifdef BOOST_MSVC
return jv((double)a, x);
#else
return jvl(a, x);
#endif
}
scalar linear_form_surf(int n, double *wt, Func<double> *v, Geom<double> *e, ExtData<scalar> *ext)
{
scalar result = 0;
for (int i = 0; i < n; i++)
{
double r = sqrt(e->x[i] * e->x[i] + e->y[i] * e->y[i]);
double theta = atan2(e->y[i], e->x[i]);
if (theta < 0) theta += 2.0*M_PI;
double j13 = jv(-1.0/3.0, r), j23 = jv(+2.0/3.0, r);
double cost = cos(theta), sint = sin(theta);
double cos23t = cos(2.0/3.0*theta), sin23t = sin(2.0/3.0*theta);
double Etau = e->tx[i] * (cos23t*sint*j13 - 2.0/(3.0*r)*j23*(cos23t*sint + sin23t*cost)) +
e->ty[i] * (-cos23t*cost*j13 + 2.0/(3.0*r)*j23*(cos23t*cost - sin23t*sint));
result += wt[i] * cplx(cos23t*j23, -Etau) * ((v->val0[i] * e->tx[i] + v->val1[i] * e->ty[i]));
}
return result;
}
virtual std::complex<double> value(int n, double *wt, Func<std::complex<double> > *u_ext[],
Func<double> *v, Geom<double> *e, ExtData<std::complex<double> > *ext) const
{
std::complex<double> result = 0;
for (int i = 0; i < n; i++) {
double r = std::sqrt(e->x[i] * e->x[i] + e->y[i] * e->y[i]);
double theta = std::atan2(e->y[i], e->x[i]);
if (theta < 0) theta += 2.0*M_PI;
double j13 = jv(-1.0/3.0, r), j23 = jv(+2.0/3.0, r);
double cost = std::cos(theta), sint = std::sin(theta);
double cos23t = std::cos(2.0/3.0*theta), sin23t = std::sin(2.0/3.0*theta);
double Etau = e->tx[i] * (cos23t*sint*j13 - 2.0/(3.0*r)*j23*(cos23t*sint + sin23t*cost)) +
e->ty[i] * (-cos23t*cost*j13 + 2.0/(3.0*r)*j23*(cos23t*cost - sin23t*sint));
result += wt[i] * std::complex<double>(cos23t*j23, -Etau) * ((v->val0[i] * e->tx[i] + v->val1[i] * e->ty[i]));
}
return -result;
}
static void exact_sol_val(double x, double y, std::complex<double>& e0, std::complex<double>& e1)
{
double t1 = x*x;
double t2 = y*y;
double t4 = std::sqrt(t1+t2);
double t5 = jv(-1.0/3.0,t4);
double t6 = 1/t4;
double t7 = jv(2.0/3.0,t4);
double t11 = (t5-2.0/3.0*t6*t7)*t6;
double t12 = std::atan2(y,x);
if (t12 < 0) t12 += 2.0*M_PI;
double t13 = 2.0/3.0*t12;
double t14 = std::cos(t13);
double t17 = std::sin(t13);
double t18 = t7*t17;
double t20 = 1/t1;
double t23 = 1/(1.0+t2*t20);
e0 = t11*y*t14-2.0/3.0*t18/x*t23;
e1 = -t11*x*t14-2.0/3.0*t18*y*t20*t23;
}
static void exact_sol(double x, double y, double z, scalar &e0, scalar &e1, scalar &e1dx, scalar &e0dy)
{
exact_sol_val(x, y, z, e0, e1);
double t1 = x*x;
double t2 = y*y;
double t3 = t1+t2;
double t4 = sqrt(t3);
double t5 = jv(2.0/3.0,t4);
double t6 = 1/t4;
double t7 = jv(-1.0/3.0,t4);
double t11 = (-t5-t6*t7/3.0)*t6;
double t14 = 1/t4/t3;
double t15 = t14*t5;
double t21 = t7-2.0/3.0*t6*t5;
double t22 = 1/t3*t21;
double t27 = atan2(y,x);
if (t27 < 0) t27 += 2.0*M_PI;
double t28 = 2.0/3.0*t27;
double t29 = cos(t28);
double t32 = t21*t14;
double t35 = t21*t6;
double t36 = t35*t29;
double t39 = sin(t28);
double t41 = 1/t1;
double t43 = 1.0+t2*t41;
double t44 = 1/t43;
double t47 = 4.0/3.0*t35/x*t39*y*t44;
double t48 = t5*t29;
double t49 = t1*t1;
double t52 = t43*t43;
double t53 = 1/t52;
double t57 = t5*t39;
double t59 = 1/t1/x;
e1dx =-(t11*x+2.0/3.0*t15*x-2.0/3.0*t22*x)
*t6*x*t29+t32*t1*t29-t36-t47+4.0/9.0*t48*t2/t49*t53+4.0/3.0*t57*y*t59*t44-4.0/3.0*t57*t2*y/t49/x*t53;
e0dy = (t11*y+2.0/3.0*t15*y-2.0/3.0*t22*y)*t6*y*t29-t32*t2*t29+t36-t47-4.0/9.0*t48*t41*t53+4.0/3.0*t57*t59*t53*y;
}
double struve(double v, double x)
{
double y, ya, f, g, h, t;
double onef2err, threef0err;
f = floor(v);
if ((v < 0) && (v - f == 0.5)) {
y = jv(-v, x);
f = 1.0 - f;
g = 2.0 * floor(f / 2.0);
if (g != f)
y = -y;
return(y);
}
t = 0.25 * x * x;
f = fabs(x);
g = 1.5 * fabs(v);
if ((f > 30.0) && (f > g)) {
onef2err = 1.0e38;
y = 0.0;
}
else {
y = onef2(1.0, 1.5, 1.5 + v, -t, &onef2err);
}
if ((f < 18.0) || (x < 0.0)) {
threef0err = 1.0e38;
ya = 0.0;
}
else {
ya = threef0(1.0, 0.5, 0.5 - v, -1.0 / t, &threef0err);
}
f = sqrt(PI);
h = pow(0.5 * x, v - 1.0);
if (onef2err <= threef0err) {
g = gam(v + 1.5);
y = y * h * t / (0.5 * f * g);
return(y);
}
else {
g = gam(v + 0.5);
ya = ya * h / (f * g);
ya = ya + yv(v, x);
return(ya);
}
}
bool HierarchicalIKSolver::computeSteps(float stepSize, float minChange, int maxSteps)
{
//std::vector<RobotNodePtr> rn = rns->getAllRobotNodes();
//RobotPtr robot = rns->getRobot();
//std::vector<float> jv(rns->getSize(),0.0f);
int step = 0;
checkTolerances();
//float lastDist = FLT_MAX;
while (step < maxSteps)
{
Eigen::VectorXf delta = computeStep(jacobies, stepSize);
if (!MathTools::isValid(delta))
{
#ifdef DEBUG
VR_INFO << "Singular Jacobian" << endl;
#endif
return false;
}
Eigen::VectorXf jv(delta.rows());
rns->getJointValues(jv);
jv += delta;
rns->setJointValues(jv);
if (checkTolerances())
{
#ifdef DEBUG
VR_INFO << "Tolerances ok, loop:" << step << endl;
#endif
return true;
}
if (delta.norm() < minChange)
{
#ifdef DEBUG
VR_INFO << "Could not improve result any more (dTheta.norm()=" << delta.norm() << "), loop:" << step << endl;
#endif
return false;
}
step++;
}
return false;
}
ZK gb(D d,H j,I t){H c=ct[t],g=c?c:-2,m=512;K x=ktn(c?KC:KG,m),y=ktn(xt,0);SQLULEN n=0;SQLRETURN r;while(1){r=SQLGetData(d,j,g,kG(x),xn=m,&n);if(SQL_SUCCEEDED(r))xn=n==SQL_NULL_DATA?0:n==SQL_NO_TOTAL||n>xn?xn:n,xn-=xn&&c&&!kG(x)[xn-1],jv(&y,x);else/*if(r==SQL_NO_DATA)*/break;}r0(x);R y;}
mat nnls_solver_with_missing(const mat & A, const mat & W, const mat & W1, const mat & H2, const umat & mask,
const double & eta, const double & beta, int max_iter, double rel_tol, int n_threads)
{
// A = [W, W1, W2] [H, H1, H2]^T.
// Where A may have missing values
// Note that here in the input W = [W, W2]
// compute x = [H, H1]^T given W, W2
// A0 = W2*H2 is empty when H2 is empty (no partial info in H)
// Return: x = [H, H1]
int n = A.n_rows, m = A.n_cols;
int k = W.n_cols - H2.n_cols;
int kW = W1.n_cols;
int nH = k+kW;
mat x(nH, m, fill::zeros);
if (n_threads < 0) n_threads = 0;
bool is_masked = !mask.empty();
#pragma omp parallel for num_threads(n_threads) schedule(dynamic)
for (int j = 0; j < m; j++)
{
// break if all entries of col_j are masked
if (is_masked && arma::all(mask.col(j)))
continue;
uvec non_missing = find_finite(A.col(j));
mat WtW(nH, nH); // WtW
update_WtW(WtW, W.rows(non_missing), W1.rows(non_missing), H2);
if (beta > 0) WtW += beta;
if (eta > 0) WtW.diag() += eta;
mat mu(nH, 1); // -WtA
uvec jv(1);
jv(0) = j;
//non_missing.t().print("non_missing = ");
//std::cout << "1.1" << std::endl;
if (H2.empty())
update_WtA(mu, W.rows(non_missing), W1.rows(non_missing), H2, A.submat(non_missing, jv));
else
update_WtA(mu, W.rows(non_missing), W1.rows(non_missing), H2.rows(j, j), A.submat(non_missing, jv));
//std::cout << "1.5" << std::endl;
vec x0(nH);
double tmp;
int i = 0;
double err1, err2 = 9999;
do {
x0 = x.col(j);
err1 = err2;
err2 = 0;
for (int l = 0; l < nH; l++)
{
if (is_masked && mask(l,j) > 0) continue;
tmp = x(l,j) - mu(l,0) / WtW(l,l);
if (tmp < 0) tmp = 0;
if (tmp != x(l,j))
{
mu.col(0) += (tmp - x(l,j)) * WtW.col(l);
}
x(l,j) = tmp;
tmp = std::abs(x(l,j) - x0(l));
if (tmp > err2) err2 = tmp;
}
} while(++i < max_iter && std::abs(err1 - err2) / (err1 + 1e-9) > rel_tol);
}
return x;
}
TEST(LoadingAndFK, SimpleRobot)
{
static const std::string MODEL1 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"myrobot\">"
"<link name=\"base_link\">"
" <inertial>"
" <mass value=\"2.81\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision name=\"my_collision\">"
" <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"</robot>";
static const std::string MODEL1_INFO =
"Model myrobot in frame odom_combined, of dimension 3\n"
"Joint values bounds:\n"
" base_joint/x [DBL_MIN, DBL_MAX]\n"
" base_joint/y [DBL_MIN, DBL_MAX]\n"
" base_joint/theta [-3.14159, 3.14159]\n"
"Available groups: \n"
" base (of dimension 3):\n"
" joints:\n"
" base_joint\n"
" links:\n"
" base_link\n"
" roots:\n"
" base_joint";
static const std::string SMODEL1 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"myrobot\">"
"<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
"<group name=\"base\">"
"<joint name=\"base_joint\"/>"
"</group>"
"</robot>";
boost::shared_ptr<urdf::ModelInterface> urdfModel = urdf::parseURDF(MODEL1);
boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model());
srdfModel->initString(*urdfModel, SMODEL1);
planning_models::KinematicModelPtr model(new planning_models::KinematicModel(urdfModel, srdfModel));
planning_models::KinematicState state(model);
EXPECT_EQ((unsigned int)3, state.getVariableCount());
state.setToDefaultValues();
const std::vector<planning_models::KinematicState::JointState*>& joint_states = state.getJointStateVector();
EXPECT_EQ((unsigned int)1, joint_states.size());
EXPECT_EQ((unsigned int)3, joint_states[0]->getVariableValues().size());
std::stringstream ssi;
model->printModelInfo(ssi);
EXPECT_TRUE(sameStringIgnoringWS(MODEL1_INFO, ssi.str())) << ssi.str();
std::map<std::string, double> joint_values;
joint_values["base_joint/x"] = 10.0;
joint_values["base_joint/y"] = 8.0;
//testing incomplete state
std::vector<std::string> missing_states;
state.setStateValues(joint_values, missing_states);
ASSERT_EQ(missing_states.size(), 1);
EXPECT_EQ(missing_states[0], std::string("base_joint/theta"));
joint_values["base_joint/theta"] = 0.0;
state.setStateValues(joint_values, missing_states);
ASSERT_EQ(missing_states.size(), 0);
EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5);
EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5);
EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5);
//making sure that values get copied
planning_models::KinematicState new_state(state);
EXPECT_NEAR(10.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5);
EXPECT_NEAR(8.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5);
EXPECT_NEAR(0.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5);
const std::map<std::string, unsigned int>& ind_map = model->getJointVariablesIndexMap();
std::vector<double> jv(state.getVariableCount(), 0.0);
jv[ind_map.at("base_joint/x")] = 10.0;
jv[ind_map.at("base_joint/y")] = 8.0;
jv[ind_map.at("base_joint/theta")] = 0.0;
state.setStateValues(jv);
EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5);
EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5);
EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "collision_proximity_test");
ros::NodeHandle nh;
std::string robot_description_name = nh.resolveName("robot_description", true);
srand ( time(NULL) ); // initialize random seed
ros::service::waitForService(ARM_QUERY_NAME);
ros::ServiceClient query_client = nh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(ARM_QUERY_NAME);
kinematics_msgs::GetKinematicSolverInfo::Request request;
kinematics_msgs::GetKinematicSolverInfo::Response response;
query_client.call(request, response);
int num_joints;
std::vector<double> min_limits, max_limits;
num_joints = (int) response.kinematic_solver_info.joint_names.size();
std::vector<std::string> joint_state_names = response.kinematic_solver_info.joint_names;
std::map<std::string, double> joint_state_map;
for(int i=0; i< num_joints; i++)
{
joint_state_map[joint_state_names[i]] = 0.0;
min_limits.push_back(response.kinematic_solver_info.limits[i].min_position);
max_limits.push_back(response.kinematic_solver_info.limits[i].max_position);
}
std::vector<std::vector<double> > valid_joint_states;
valid_joint_states.resize(TEST_NUM);
for(unsigned int i = 0; i < TEST_NUM; i++) {
std::vector<double> jv(7,0.0);
for(unsigned int j=0; j < min_limits.size(); j++)
{
jv[j] = gen_rand(std::max(min_limits[j],-M_PI),std::min(max_limits[j],M_PI));
}
valid_joint_states[i] = jv;
}
collision_proximity::CollisionProximitySpace* cps = new collision_proximity::CollisionProximitySpace(robot_description_name);
ros::ServiceClient planning_scene_client = nh.serviceClient<arm_navigation_msgs::GetPlanningScene>(planning_scene_name, true);
ros::service::waitForService(planning_scene_name);
arm_navigation_msgs::GetPlanningScene::Request ps_req;
arm_navigation_msgs::GetPlanningScene::Response ps_res;
arm_navigation_msgs::CollisionObject obj1;
obj1.header.stamp = ros::Time::now();
obj1.header.frame_id = "odom_combined";
obj1.id = "obj1";
obj1.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
obj1.shapes.resize(1);
obj1.shapes[0].type = arm_navigation_msgs::Shape::BOX;
obj1.shapes[0].dimensions.resize(3);
obj1.shapes[0].dimensions[0] = .1;
obj1.shapes[0].dimensions[1] = .1;
obj1.shapes[0].dimensions[2] = .75;
obj1.poses.resize(1);
obj1.poses[0].position.x = .6;
obj1.poses[0].position.y = -.6;
obj1.poses[0].position.z = .375;
obj1.poses[0].orientation.w = 1.0;
arm_navigation_msgs::AttachedCollisionObject att_obj;
att_obj.object = obj1;
att_obj.object.header.stamp = ros::Time::now();
att_obj.object.header.frame_id = "r_gripper_palm_link";
att_obj.link_name = "r_gripper_palm_link";
att_obj.touch_links.push_back("r_gripper_palm_link");
att_obj.touch_links.push_back("r_gripper_r_finger_link");
att_obj.touch_links.push_back("r_gripper_l_finger_link");
att_obj.touch_links.push_back("r_gripper_r_finger_tip_link");
att_obj.touch_links.push_back("r_gripper_l_finger_tip_link");
att_obj.touch_links.push_back("r_wrist_roll_link");
att_obj.touch_links.push_back("r_wrist_flex_link");
att_obj.touch_links.push_back("r_forearm_link");
att_obj.touch_links.push_back("r_gripper_motor_accelerometer_link");
att_obj.object.id = "obj2";
att_obj.object.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
att_obj.object.shapes[0].dimensions.resize(2);
att_obj.object.shapes[0].dimensions[0] = .025;
att_obj.object.shapes[0].dimensions[1] = .5;
att_obj.object.poses.resize(1);
att_obj.object.poses[0].position.x = .12;
att_obj.object.poses[0].position.y = 0.0;
att_obj.object.poses[0].position.z = 0.0;
att_obj.object.poses[0].orientation.w = 1.0;
ps_req.collision_object_diffs.push_back(obj1);
ps_req.attached_collision_object_diffs.push_back(att_obj);
arm_navigation_msgs::GetRobotState::Request rob_state_req;
arm_navigation_msgs::GetRobotState::Response rob_state_res;
ros::Rate slow_wait(1.0);
while(1) {
if(!nh.ok()) {
ros::shutdown();
exit(0);
}
planning_scene_client.call(ps_req,ps_res);
ROS_INFO_STREAM("Num coll " << ps_res.all_collision_objects.size() << " att " << ps_res.all_attached_collision_objects.size());
if(ps_res.all_collision_objects.size() > 0
&& ps_res.all_attached_collision_objects.size() > 0) break;
slow_wait.sleep();
}
ROS_INFO("After test");
ros::ServiceClient robot_state_service = nh.serviceClient<arm_navigation_msgs::GetRobotState>(robot_state_name, true);
ros::service::waitForService(robot_state_name);
planning_models::KinematicState* state = cps->setupForGroupQueries("right_arm_and_end_effector",
ps_res.complete_robot_state,
ps_res.allowed_collision_matrix,
ps_res.transformed_allowed_contacts,
ps_res.all_link_padding,
ps_res.all_collision_objects,
ps_res.all_attached_collision_objects,
ps_res.unmasked_collision_map);
ros::WallDuration tot_ode, tot_prox;
ros::WallDuration min_ode(1000.0,1000.0);
ros::WallDuration min_prox(1000.0, 1000.0);
ros::WallDuration max_ode;
ros::WallDuration max_prox;
std::vector<double> link_distances;
std::vector<std::vector<double> > distances;
std::vector<std::vector<tf::Vector3> > gradients;
std::vector<std::string> link_names;
std::vector<std::string> attached_body_names;
std::vector<collision_proximity::CollisionType> collisions;
unsigned int prox_num_in_collision = 0;
unsigned int ode_num_in_collision = 0;
ros::WallTime n1, n2;
for(unsigned int i = 0; i < TEST_NUM; i++) {
for(unsigned int j = 0; j < joint_state_names.size(); j++) {
joint_state_map[joint_state_names[j]] = valid_joint_states[i][j];
}
state->setKinematicState(joint_state_map);
n1 = ros::WallTime::now();
cps->setCurrentGroupState(*state);
bool in_prox_collision = cps->isStateInCollision();
n2 = ros::WallTime::now();
if(in_prox_collision) {
prox_num_in_collision++;
}
ros::WallDuration prox_dur(n2-n1);
if(prox_dur > max_prox) {
max_prox = prox_dur;
} else if (prox_dur < min_prox) {
min_prox = prox_dur;
}
tot_prox += prox_dur;
n1 = ros::WallTime::now();
bool ode_in_collision = cps->getCollisionModels()->isKinematicStateInCollision(*state);
n2 = ros::WallTime::now();
if(ode_in_collision) {
ode_num_in_collision++;
}
if(0){//in_prox_collision && !ode_in_collision) {
ros::Rate r(1.0);
while(nh.ok()) {
cps->getStateCollisions(link_names, attached_body_names, in_prox_collision, collisions);
ROS_INFO("Prox not ode");
cps->visualizeDistanceField();
cps->visualizeCollisions(link_names, attached_body_names, collisions);
cps->visualizeConvexMeshes(cps->getCollisionModels()->getGroupLinkUnion());
std::vector<std::string> objs = link_names;
objs.insert(objs.end(), attached_body_names.begin(), attached_body_names.end());
cps->visualizeObjectSpheres(objs);
//cps->visualizeVoxelizedLinks(collision_models->getGroupLinkUnion());
r.sleep();
}
exit(0);
}
if(0 && !in_prox_collision && ode_in_collision) {
ros::Rate r(1.0);
while(nh.ok()) {
ROS_INFO("Ode not prox");
cps->visualizeDistanceField();
cps->getStateCollisions(link_names, attached_body_names, in_prox_collision, collisions);
cps->visualizeCollisions(link_names, attached_body_names, collisions);
cps->visualizeConvexMeshes(cps->getCollisionModels()->getGroupLinkUnion());
std::vector<std::string> objs = link_names;
objs.insert(objs.end(), attached_body_names.begin(), attached_body_names.end());
cps->visualizeObjectSpheres(objs);
r.sleep();
}
//cps->visualizeVoxelizedLinks(collision_models->getGroupLinkUnion());
std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts;
std::vector<collision_space::EnvironmentModel::Contact> contact;
cps->getCollisionModels()->getCollisionSpace()->getCollisionContacts(allowed_contacts, contact, 10);
for(unsigned int i = 0; i < contact.size(); i++) {
std::string name1;
std::string name2;
if(contact[i].link1 != NULL) {
if(contact[i].link1_attached_body_index != 0) {
name1 = contact[i].link1->getAttachedBodyModels()[contact[i].link1_attached_body_index-1]->getName();
} else {
name1 = contact[i].link1->getName();
}
}
if(contact[i].link2 != NULL) {
if(contact[i].link2_attached_body_index != 0) {
name2 = contact[i].link2->getAttachedBodyModels()[contact[i].link2_attached_body_index-1]->getName();
} else {
name2 = contact[i].link2->getName();
}
} else if (!contact[i].object_name.empty()) {
name2 = contact[i].object_name;
}
ROS_INFO_STREAM("Contact " << i << " between " << name1 << " and " << name2);
}
exit(0);
if(0) {
std::vector<double> prox_link_distances;
std::vector<std::vector<double> > prox_distances;
std::vector<std::vector<tf::Vector3> > prox_gradients;
std::vector<std::string> prox_link_names;
std::vector<std::string> prox_attached_body_names;
cps->getStateGradients(prox_link_names, prox_attached_body_names, prox_link_distances, prox_distances, prox_gradients);
ROS_INFO_STREAM("Link size " << prox_link_names.size());
for(unsigned int i = 0; i < prox_link_names.size(); i++) {
ROS_INFO_STREAM("Link " << prox_link_names[i] << " closest distance " << prox_link_distances[i]);
}
for(unsigned int i = 0; i < prox_attached_body_names.size(); i++) {
ROS_INFO_STREAM("Attached body names " << prox_attached_body_names[i] << " closest distance " << prox_link_distances[i+prox_link_names.size()]);
}
exit(0);
}
}
ros::WallDuration ode_dur(n2-n1);
if(ode_dur > max_ode) {
max_ode = ode_dur;
} else if (ode_dur < min_ode) {
min_ode = ode_dur;
}
tot_ode += ode_dur;
}
//ROS_INFO_STREAM("Setup prox " << tot_prox_setup.toSec()/(TEST_NUM*1.0) << " ode " << tot_ode_setup.toSec()/(TEST_NUM*1.0));
ROS_INFO_STREAM("Av prox time " << (tot_prox.toSec()/(TEST_NUM*1.0)) << " min " << min_prox.toSec() << " max " << max_prox.toSec()
<< " percent in coll " << (prox_num_in_collision*1.0)/(TEST_NUM*1.0));
ROS_INFO_STREAM("Av ode time " << (tot_ode.toSec()/(TEST_NUM*1.0)) << " min " << min_ode.toSec() << " max " << max_ode.toSec()
<< " percent in coll " << (ode_num_in_collision*1.0)/(TEST_NUM*1.0));
ros::shutdown();
delete cps;
}
TEST(LoadingAndFK, SimpleRobot)
{
static const std::string MODEL1 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"myrobot\">"
"<link name=\"base_link\">"
" <inertial>"
" <mass value=\"2.81\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision name=\"my_collision\">"
" <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"</robot>";
static const std::string MODEL1_INFO =
"Complete model state dimension = 3\n"
"State bounds: [-3.14159, 3.14159] [-DBL_MAX, DBL_MAX] [-DBL_MAX, DBL_MAX]\n"
"Root joint : base_joint \n"
"Available groups: base \n"
"Group base has 1 roots: base_joint \n";
std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
planning_models::KinematicModel::MultiDofConfig config("base_joint");
config.type = "Planar";
config.parent_frame_id = "odom_combined";
config.child_frame_id = "base_link";
multi_dof_configs.push_back(config);
urdf::Model urdfModel;
urdfModel.initString(MODEL1);
std::vector<std::string> gc_joints;
gc_joints.push_back("base_joint");
std::vector<std::string> empty;
std::vector<planning_models::KinematicModel::GroupConfig> gcs;
gcs.push_back(planning_models::KinematicModel::GroupConfig("base", gc_joints, empty));
planning_models::KinematicModel* model(new planning_models::KinematicModel(urdfModel,gcs,multi_dof_configs));
//bracketing so the state gets destroyed before we bring down the model
{
planning_models::KinematicState state(model);
EXPECT_EQ((unsigned int)3, state.getDimension());
state.setKinematicStateToDefault();
const std::vector<planning_models::KinematicState::JointState*>& joint_states = state.getJointStateVector();
EXPECT_EQ((unsigned int)1, joint_states.size());
EXPECT_EQ((unsigned int)3, joint_states[0]->getJointStateValues().size());
std::stringstream ssi;
state.printStateInfo(ssi);
EXPECT_TRUE(sameStringIgnoringWS(MODEL1_INFO, ssi.str())) << ssi.str();
std::map<std::string, double> joint_values;
joint_values["planar_x"]=10.0;
joint_values["planar_y"]=8.0;
//testing incomplete state
std::vector<std::string> missing_states;
EXPECT_FALSE(state.setKinematicState(joint_values,
missing_states));
ASSERT_EQ(missing_states.size(),1);
EXPECT_EQ(missing_states[0],std::string("planar_th"));
joint_values["planar_th"]=0.0;
EXPECT_TRUE(state.setKinematicState(joint_values,
missing_states));
ASSERT_EQ(missing_states.size(),0);
EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
//making sure that values get copied
planning_models::KinematicState new_state(state);
EXPECT_NEAR(10.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
EXPECT_NEAR(8.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
EXPECT_NEAR(0.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
const std::map<std::string, unsigned int>& ind_map = state.getKinematicStateIndexMap();
std::vector<double> jv(state.getDimension(), 0.0);
jv[ind_map.at("planar_x")] = 10.0;
jv[ind_map.at("planar_y")] = 8.0;
jv[ind_map.at("planar_th")] = 0.0;
state.setKinematicState(jv);
EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
}
delete model;
}
int main(int argc, char *argv[]) {
Teuchos::GlobalMPISession mpiSession(&argc, &argv);
// This little trick lets us print to std::cout only if a (dummy) command-line argument is provided.
int iprint = argc - 1;
Teuchos::RCP<std::ostream> outStream;
Teuchos::oblackholestream bhs; // outputs nothing
if (iprint > 0)
outStream = Teuchos::rcp(&std::cout, false);
else
outStream = Teuchos::rcp(&bhs, false);
int errorFlag = 0;
// *** Example body.
try {
std::string filename = "input.xml";
Teuchos::RCP<Teuchos::ParameterList> parlist = Teuchos::rcp( new Teuchos::ParameterList() );
Teuchos::updateParametersFromXmlFile( filename, parlist.ptr() );
RealT V_th = parlist->get("Thermal Voltage", 0.02585);
RealT lo_Vsrc = parlist->get("Source Voltage Lower Bound", 0.0);
RealT up_Vsrc = parlist->get("Source Voltage Upper Bound", 1.0);
RealT step_Vsrc = parlist->get("Source Voltage Step", 1.e-2);
RealT true_Is = parlist->get("True Saturation Current", 1.e-12);
RealT true_Rs = parlist->get("True Saturation Resistance", 0.25);
RealT init_Is = parlist->get("Initial Saturation Current", 1.e-12);
RealT init_Rs = parlist->get("Initial Saturation Resistance", 0.25);
RealT lo_Is = parlist->get("Saturation Current Lower Bound", 1.e-16);
RealT up_Is = parlist->get("Saturation Current Upper Bound", 1.e-1);
RealT lo_Rs = parlist->get("Saturation Resistance Lower Bound", 1.e-2);
RealT up_Rs = parlist->get("Saturation Resistance Upper Bound", 30.0);
// bool use_lambertw = parlist->get("Use Analytical Solution",true);
bool use_scale = parlist->get("Use Scaling For Epsilon-Active Sets",true);
bool use_sqp = parlist->get("Use SQP", true);
// bool use_linesearch = parlist->get("Use Line Search", true);
// bool datatype = parlist->get("Get Data From Input File",false);
// bool use_adjoint = parlist->get("Use Adjoint Gradient Computation",false);
// int use_hessvec = parlist->get("Use Hessian-Vector Implementation",1); // 0 - FD, 1 - exact, 2 - GN
// bool plot = parlist->get("Generate Plot Data",false);
// RealT noise = parlist->get("Measurement Noise",0.0);
EqualityConstraint_DiodeCircuit<RealT> con(V_th,lo_Vsrc,up_Vsrc,step_Vsrc);
RealT alpha = 1.e-4; // regularization parameter (unused)
int ns = 101; // number of Vsrc components
int nz = 2; // number of optimization variables
Objective_DiodeCircuit<RealT> obj(alpha,ns,nz);
// Initialize iteration vectors.
Teuchos::RCP<std::vector<RealT> > z_rcp = Teuchos::rcp( new std::vector<RealT> (nz, 0.0) );
Teuchos::RCP<std::vector<RealT> > yz_rcp = Teuchos::rcp( new std::vector<RealT> (nz, 0.0) );
Teuchos::RCP<std::vector<RealT> > soln_rcp = Teuchos::rcp( new std::vector<RealT> (nz, 0.0) );
(*z_rcp)[0] = init_Is;
(*z_rcp)[1] = init_Rs;
(*yz_rcp)[0] = init_Is;
(*yz_rcp)[1] = init_Rs;
(*soln_rcp)[0] = true_Is;
(*soln_rcp)[1] = true_Rs;
ROL::StdVector<RealT> z(z_rcp);
ROL::StdVector<RealT> yz(yz_rcp);
ROL::StdVector<RealT> soln(soln_rcp);
Teuchos::RCP<ROL::Vector<RealT> > zp = Teuchos::rcp(&z,false);
Teuchos::RCP<ROL::Vector<RealT> > yzp = Teuchos::rcp(&yz,false);
Teuchos::RCP<std::vector<RealT> > u_rcp = Teuchos::rcp( new std::vector<RealT> (ns, 0.0) );
Teuchos::RCP<std::vector<RealT> > yu_rcp = Teuchos::rcp( new std::vector<RealT> (ns, 0.0) );
std::ifstream input_file("measurements.dat");
RealT temp, temp_scale;
for (int i=0; i<ns; i++) {
input_file >> temp;
input_file >> temp;
temp_scale = pow(10,int(log10(temp)));
(*u_rcp)[i] = temp_scale*(RealT)rand()/(RealT)RAND_MAX;
(*yu_rcp)[i] = temp_scale*(RealT)rand()/(RealT)RAND_MAX;
}
input_file.close();
ROL::StdVector<RealT> u(u_rcp);
ROL::StdVector<RealT> yu(yu_rcp);
Teuchos::RCP<ROL::Vector<RealT> > up = Teuchos::rcp(&u,false);
Teuchos::RCP<ROL::Vector<RealT> > yup = Teuchos::rcp(&yu,false);
Teuchos::RCP<std::vector<RealT> > jv_rcp = Teuchos::rcp( new std::vector<RealT> (ns, 1.0) );
ROL::StdVector<RealT> jv(jv_rcp);
Teuchos::RCP<ROL::Vector<RealT> > jvp = Teuchos::rcp(&jv,false);
ROL::Vector_SimOpt<RealT> x(up,zp);
ROL::Vector_SimOpt<RealT> y(yup,yzp);
// Check derivatives
obj.checkGradient(x,x,y,true,*outStream);
obj.checkHessVec(x,x,y,true,*outStream);
con.checkApplyJacobian(x,y,jv,true,*outStream);
con.checkApplyAdjointJacobian(x,yu,jv,x,true,*outStream);
con.checkApplyAdjointHessian(x,yu,y,x,true,*outStream);
// Check consistency of Jacobians and adjoint Jacobians.
con.checkAdjointConsistencyJacobian_1(jv,yu,u,z,true,*outStream);
con.checkAdjointConsistencyJacobian_2(jv,yz,u,z,true,*outStream);
// Check consistency of solves.
con.checkSolve(u,z,jv,true,*outStream);
con.checkInverseJacobian_1(jv,yu,u,z,true,*outStream);
con.checkInverseAdjointJacobian_1(yu,jv,u,z,true,*outStream);
// Initialize reduced objective function.
Teuchos::RCP<std::vector<RealT> > p_rcp = Teuchos::rcp( new std::vector<RealT> (ns, 0.0) );
ROL::StdVector<RealT> p(p_rcp);
Teuchos::RCP<ROL::Vector<RealT> > pp = Teuchos::rcp(&p,false);
Teuchos::RCP<ROL::Objective_SimOpt<RealT> > pobj = Teuchos::rcp(&obj,false);
Teuchos::RCP<ROL::EqualityConstraint_SimOpt<RealT> > pcon = Teuchos::rcp(&con,false);
ROL::Reduced_Objective_SimOpt<RealT> robj(pobj,pcon,up,pp);
// Check derivatives.
*outStream << "Derivatives of reduced objective" << std::endl;
robj.checkGradient(z,z,yz,true,*outStream);
robj.checkHessVec(z,z,yz,true,*outStream);
// Bound constraints
RealT tol = 1.e-12;
Teuchos::RCP<std::vector<RealT> > g0_rcp = Teuchos::rcp( new std::vector<RealT> (nz, 0.0) );;
ROL::StdVector<RealT> g0p(g0_rcp);
robj.gradient(g0p,z,tol);
*outStream << std::scientific << "Initial gradient = " << (*g0_rcp)[0] << " " << (*g0_rcp)[1] << "\n";
*outStream << std::scientific << "Norm of Gradient = " << g0p.norm() << "\n";
// Define scaling for epsilon-active sets (used in inequality constraints)
RealT scale;
if(use_scale){ scale = 1.0e-2/g0p.norm();}
else{ scale = 1.0;}
*outStream << std::scientific << "Scaling: " << scale << "\n";
/// Define constraints on Is and Rs
BoundConstraint_DiodeCircuit<RealT> bcon(scale,lo_Is,up_Is,lo_Rs,up_Rs);
//bcon.deactivate();
// Optimization
*outStream << "\n Initial guess " << (*z_rcp)[0] << " " << (*z_rcp)[1] << std::endl;
if (!use_sqp){
// Trust Region
ROL::Algorithm<RealT> algo_tr("Trust Region",*parlist);
std::clock_t timer_tr = std::clock();
algo_tr.run(z,robj,bcon,true,*outStream);
*outStream << "\n Solution " << (*z_rcp)[0] << " " << (*z_rcp)[1] << "\n" << std::endl;
*outStream << "Trust-Region required " << (std::clock()-timer_tr)/(RealT)CLOCKS_PER_SEC
<< " seconds.\n";
}
else{
// SQP.
//Teuchos::RCP<std::vector<RealT> > gz_rcp = Teuchos::rcp( new std::vector<RealT> (nz, 0.0) );
//ROL::StdVector<RealT> gz(gz_rcp);
//Teuchos::RCP<ROL::Vector<RealT> > gzp = Teuchos::rcp(&gz,false);
Teuchos::RCP<std::vector<RealT> > gu_rcp = Teuchos::rcp( new std::vector<RealT> (ns, 0.0) );
ROL::StdVector<RealT> gu(gu_rcp);
Teuchos::RCP<ROL::Vector<RealT> > gup = Teuchos::rcp(&gu,false);
//ROL::Vector_SimOpt<RealT> g(gup,gzp);
ROL::Vector_SimOpt<RealT> g(gup,zp);
Teuchos::RCP<std::vector<RealT> > c_rcp = Teuchos::rcp( new std::vector<RealT> (ns, 0.0) );
Teuchos::RCP<std::vector<RealT> > l_rcp = Teuchos::rcp( new std::vector<RealT> (ns, 0.0) );
ROL::StdVector<RealT> c(c_rcp);
ROL::StdVector<RealT> l(l_rcp);
ROL::Algorithm<RealT> algo_cs("Composite Step",*parlist);
//x.zero();
std::clock_t timer_cs = std::clock();
algo_cs.run(x,g,l,c,obj,con,true,*outStream);
*outStream << "\n Solution " << (*z_rcp)[0] << " " << (*z_rcp)[1] << "\n" << std::endl;
*outStream << "Composite Step required " << (std::clock()-timer_cs)/(RealT)CLOCKS_PER_SEC
<< " seconds.\n";
}
soln.axpy(-1.0, z);
*outStream << "Norm of error: " << soln.norm() << std::endl;
if (soln.norm() > 1e4*ROL::ROL_EPSILON) {
errorFlag = 1;
}
}
catch (std::logic_error err) {
*outStream << err.what() << "\n";
errorFlag = -1000;
}; // end try
if (errorFlag != 0)
std::cout << "End Result: TEST FAILED\n";
else
std::cout << "End Result: TEST PASSED\n";
return 0;
}
EvolDC1Buras::EvolDC1Buras(unsigned int dim_i, schemes scheme, orders order, const StandardModel& model)
: RGEvolutor(dim_i, scheme, order), model(model),
v(dim_i,0.), vi(dim_i,0.), js(dim_i,0.), h(dim_i,0.), gg(dim_i,0.), s_s(dim_i,0.),
jssv(dim_i,0.), jss(dim_i,0.), jv(dim_i,0.), vij(dim_i,0.), e(dim_i,0.), dim(dim_i)
{
/*magic numbers a & b */
for(int L=2; L>-1; L--){
/* L=2 --> u,d,s,c (nf=4) L=1 --> u,d,s,c,b (nf=5) L=0 --> u,d,s,c,b,t (nf=6)*/
nu = L; nd = L;
if(L == 1){nd = 3; nu = 2;}
if(L == 0){nd = 3; nu = 3;}
AnomalousDimension_DC1_Buras(LO,nu,nd).transpose().eigensystem(v,e);
vi = v.inverse();
for(unsigned int i = 0; i < dim; i++){
a[L][i] = e(i).real();
for (unsigned int j = 0; j < dim; j++) {
for (unsigned int k = 0; k < dim; k++) {
b[L][i][j][k] = v(i, k).real() * vi(k, j).real();
}
}
}
// LO evolutor in the standard basis
gg = vi * AnomalousDimension_DC1_Buras(NLO,nu,nd).transpose() * v;
double b0 = model.Beta0(6-L);
double b1 = model.Beta1(6-L);
for (unsigned int i = 0; i < dim; i++){
for (unsigned int j = 0; j < dim; j++){
s_s.assign( i, j, (b1 / b0) * (i==j) * e(i).real() - gg(i,j));
if(fabs(e(i).real() - e(j).real() + 2. * b0)>0.00000000001){
h.assign( i, j, s_s(i,j) / (2. * b0 + e(i) - e(j)));
}
}
}
js = v * h * vi;
jv = js * v;
vij = vi * js;
jss = v * s_s * vi;
jssv = jss * v;
for (unsigned int i = 0; i < dim; i++){
for (unsigned int j = 0; j < dim; j++){
if(fabs(e(i).real() - e(j).real() + 2. * b0) > 0.00000000001){
for(unsigned int k = 0; k < dim; k++){
c[L][i][j][k] = jv(i, k).real() * vi(k, j).real();
d[L][i][j][k] = -v(i, k).real() * vij(k, j).real();
}
}
else{
for(unsigned int k = 0; k < dim; k++){
c[L][i][j][k] = (1./(2. * b0)) * jssv(i, k).real() * vi(k, j).real();
d[L][i][j][k] = 0.;
}
}
}
}
}
}
void build_simple_matrix(
Epetra_Comm &comm, // Communicator to use
Epetra_CrsMatrix *&A, // OUTPUT: Matrix returned
itype nGlobalRows, // Number of global matrix rows and columns
bool testEpetra64, // if true, add 2*INT_MAX to each global ID
// to exercise Epetra64
bool verbose // if true, print out matrix information
)
{
Epetra_Map *rowMap = NULL; // Row map for the created matrix
Epetra_Map *colMap = NULL; // Col map for the created matrix
Epetra_Map *vectorMap = NULL; // Range/Domain map for the created matrix
long long offsetEpetra64;
build_maps(nGlobalRows, testEpetra64, comm,
&vectorMap, &rowMap, &colMap, offsetEpetra64, verbose);
// Create an integer vector nnzPerRow that is used to build the Epetra Matrix.
// nnzPerRow[i] is the number of entries for the ith global equation
int nMyRows = rowMap->NumMyElements();
std::vector<int> nnzPerRow(nMyRows+1, 0);
// Also create lists of the nonzeros to be assigned to processors.
// To save programming time and complexity, these vectors are allocated
// bigger than they may actually be needed.
std::vector<itype> iv(3*nMyRows+1);
std::vector<itype> jv(3*nMyRows+1);
std::vector<double> vv(3*nMyRows+1);
itype nMyNonzeros = 0;
for (itype i = 0, myrowcnt = 0; i < nGlobalRows; i++) {
if (rowMap->MyGID(i+offsetEpetra64)) {
// This processor owns part of this row; see whether it owns the nonzeros
if (i > 0 && (!colMap || colMap->MyGID(i-1+offsetEpetra64))) {
iv[nMyNonzeros] = i + offsetEpetra64;
jv[nMyNonzeros] = i-1 + offsetEpetra64;
vv[nMyNonzeros] = -1;
nMyNonzeros++;
nnzPerRow[myrowcnt]++;
}
if (!colMap || colMap->MyGID(i+offsetEpetra64)) {
iv[nMyNonzeros] = i + offsetEpetra64;
jv[nMyNonzeros] = i + offsetEpetra64;
vv[nMyNonzeros] = ((i == 0 || i == nGlobalRows-1) ? 1. : 2.);
nMyNonzeros++;
nnzPerRow[myrowcnt]++;
}
if (i < nGlobalRows - 1 && (!colMap || colMap->MyGID(i+1+offsetEpetra64))) {
iv[nMyNonzeros] = i + offsetEpetra64;
jv[nMyNonzeros] = i+1 + offsetEpetra64;
vv[nMyNonzeros] = -1;
nMyNonzeros++;
nnzPerRow[myrowcnt]++;
}
myrowcnt++;
}
}
// Create an Epetra_Matrix
A = new Epetra_CrsMatrix(Copy, *rowMap, &nnzPerRow[0], true);
int info;
for (int sum = 0, i=0; i < nMyRows; i++) {
if (nnzPerRow[i]) {
info = A->InsertGlobalValues(iv[sum],nnzPerRow[i],&vv[sum],&jv[sum]);
assert(info==0);
sum += nnzPerRow[i];
}
}
// Finish up
if (vectorMap)
info = A->FillComplete(*vectorMap, *vectorMap);
else
info = A->FillComplete();
assert(info==0);
}
inline double cyl_bessel_j(double a, double x)
{ return jv(a, x); }
