std::string JavaScript::run(std::string code)
{
v8::Context::Scope context_scope(context);
v8::HandleScope handle_scope;
v8::Local<v8::String> source = v8::String::New(code.c_str());
v8::Local<v8::Script> script = v8::Script::Compile(source);
v8::Local<v8::Value> result = script->Run();
v8::String::Utf8Value ucs(result);
return std::string (*ucs);
}
void
run_test(Cost_Map &map, Point &start, Point &goal, vector<int> ×) {
struct timeval pre;
struct timeval post;
/// printf("UCS\n");
UCS ucs(&map, start, goal);
gettimeofday(&pre, NULL);
Path path = ucs.search();
gettimeofday(&post, NULL);
double reference_cost = path.length;
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
/// printf("A*\n");
Manhattan Manhattan(&map, start, goal);
gettimeofday(&pre, NULL);
path = Manhattan.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
/// printf("A*\n");
Euclidean euclidean(&map, start, goal);
gettimeofday(&pre, NULL);
path = euclidean.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
/// printf("A*\n");
Octile octile(&map, start, goal);
gettimeofday(&pre, NULL);
path = octile.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
/// printf("Coarse single\n");
CUCS_Heuristic cucs(&map, start, goal);
gettimeofday(&pre, NULL);
path = cucs.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
/// printf("BB\n");
Boundaries_Blocking bb2(&map, start, goal, 2, xscale, yscale);
gettimeofday(&pre, NULL);
path = bb2.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
Boundaries_Blocking bb(&map, start, goal, levels, xscale, yscale);
gettimeofday(&pre, NULL);
path = bb.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
/// printf("BN\n");
Boundaries_NonBlocking bnb2(&map, start, goal, 2, xscale, yscale);
gettimeofday(&pre, NULL);
path = bnb2.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
Boundaries_NonBlocking bnb(&map, start, goal, levels, xscale, yscale);
gettimeofday(&pre, NULL);
path = bnb.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
/// printf("CB\n");
Corners_Blocking cb2(&map, start, goal, 2, xscale, yscale);
gettimeofday(&pre, NULL);
path = cb2.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
Corners_Blocking cb(&map, start, goal, levels, xscale, yscale);
gettimeofday(&pre, NULL);
path = cb.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
/// printf("CN\n");
Corners_NonBlocking cnb2(&map, start, goal, 2, xscale, yscale);
gettimeofday(&pre, NULL);
path = cnb2.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
Corners_NonBlocking cnb(&map, start, goal, levels, xscale, yscale);
gettimeofday(&pre, NULL);
path = cnb.search();
gettimeofday(&post, NULL);
check_and_update(start, goal, pre, post, reference_cost, path.length, times);
}
int
main(int argc, char** argv) {
if (argc < 9) {
printf("Usage:\n%s <map_file> <width> <height> <x map scale> <y map scale> <levels> <xscale> <yscale> [<start x> <start y> <goal x> <goal y>]\n", argv[0]);
return 0;
}
unsigned short w;
unsigned short h;
unsigned short sx;
unsigned short sy;
sscanf(argv[2], "%hu", &w);
sscanf(argv[3], "%hu", &h);
sscanf(argv[4], "%hu", &sx);
sscanf(argv[5], "%hu", &sy);
sscanf(argv[6], "%d", &levels);
sscanf(argv[7], "%d", &xscale);
sscanf(argv[8], "%d", &yscale);
unsigned short start_x = 0;
unsigned short start_y = 0;
unsigned short goal_x = 0;
unsigned short goal_y = 0;
if (argc == 13) {
sscanf(argv[9], "%hu", &start_x);
sscanf(argv[10], "%hu", &start_y);
sscanf(argv[11], "%hu", &goal_x);
sscanf(argv[12], "%hu", &goal_y);
}
char* map_filename = argv[1];
Cost_Map map(map_filename, w, h, sx, sy);
printf("Read map\n");
unsigned short max_width = map.width;
unsigned short max_height = map.height;
// Add random threat
/// for (int i = 0; i < 10000; i++) {
/// unsigned short x = rand() % max_width;
/// unsigned short y = rand() % max_height;
/// unsigned short r = rand() % 50;
/// double value = rand() % 100;
/// map.add_circular_threat(x, y, r, value);
/// }
/// fprintf(stderr, "Added threat\n");
map.print_small(stdout);
fflush(stdout);
vector<double> averages;
Point start(0, 0, 0);
Point goal(0, 0, 0);
int iters = 5;
if (argc == 9)
iters = 100;
for (int n = 0; n < iters; n++) {
if (argc == 9) {
// Get random locations
while (true) {
start.x = rand() % max_width;
start.y = rand() % max_height;
goal.x = rand() % max_width;
goal.y = rand() % max_height;
if ((map.get_map_val(start.x, start.y) != HUGE_VAL) &&
(map.get_map_val(goal.x, goal.y) != HUGE_VAL) &&
(abs(start.x - goal.x) + abs(start.y - goal.y) > 100)) {
printf("UCS test\n");
UCS ucs(&map, start, goal);
Path path = ucs.search();
if (path.length > 0)
break;
}
}
} else {
start.x = start_x;
start.y = start_y;
goal.x = goal_x;
goal.y = goal_y;
}
printf("(%d, %d) to (%d, %d) ", start.x, start.y, goal.x, goal.y);
vector<int> times;
run_test(map, start, goal, times);
for (unsigned int i = 0; i < times.size(); i++)
printf("%d ", times[i]);
if (averages.size() == 0) {
for (unsigned int i = 0; i < times.size(); i++)
averages.push_back(times[i]);
} else {
for (unsigned int i = 0; i < times.size(); i++)
averages[i] += (times[i] - averages[i]) / n;
}
printf("\n");
}
for (unsigned int i = 0; i < averages.size(); i++)
printf("%.2lf\n", averages[i] / 1000);
return 0;
}
void graph::ucsu(const std::string& v1, const std::string& v2, std::ofstream& fp, std::ofstream& fl) const
{
ucs(v1, v2, fp, fl, &graph::unreliable_cost);
}
TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
{
robot_state::RobotState ks(kmodel);
ks.setToDefaultValues();
ks.update();
robot_state::RobotState ks_const(kmodel);
ks_const.setToDefaultValues();
ks_const.update();
robot_state::Transforms &tf = ps->getTransformsNonConst();
kinematic_constraints::JointConstraint jc1(kmodel);
std::map<std::string, double> state_values;
moveit_msgs::JointConstraint torso_constraint;
torso_constraint.joint_name = "torso_lift_joint";
torso_constraint.position = ks.getVariablePosition("torso_lift_joint");
torso_constraint.tolerance_above = 0.01;
torso_constraint.tolerance_below = 0.01;
torso_constraint.weight = 1.0;
EXPECT_TRUE(jc1.configure(torso_constraint));
kinematic_constraints::JointConstraint jc2(kmodel);
moveit_msgs::JointConstraint jcm2;
jcm2.joint_name = "r_elbow_flex_joint";
jcm2.position = ks.getVariablePosition("r_elbow_flex_joint");
jcm2.tolerance_above = 0.01;
jcm2.tolerance_below = 0.01;
jcm2.weight = 1.0;
EXPECT_TRUE(jc2.configure(jcm2));
moveit_msgs::PositionConstraint pcm;
pcm.link_name = "l_wrist_roll_link";
pcm.target_point_offset.x = 0;
pcm.target_point_offset.y = 0;
pcm.target_point_offset.z = 0;
pcm.constraint_region.primitives.resize(1);
pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
pcm.constraint_region.primitives[0].dimensions.resize(1);
pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
pcm.constraint_region.primitive_poses.resize(1);
pcm.constraint_region.primitive_poses[0].position.x = 0.55;
pcm.constraint_region.primitive_poses[0].position.y = 0.2;
pcm.constraint_region.primitive_poses[0].position.z = 1.25;
pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
pcm.weight = 1.0;
pcm.header.frame_id = kmodel->getModelFrame();
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "l_wrist_roll_link";
ocm.header.frame_id = kmodel->getModelFrame();
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.4;
ocm.weight = 1.0;
std::vector<kinematic_constraints::JointConstraint> js;
js.push_back(jc1);
boost::shared_ptr<constraint_samplers::JointConstraintSampler> jcsp(new constraint_samplers::JointConstraintSampler(ps, "arms_and_torso"));
EXPECT_TRUE(jcsp->configure(js));
std::vector<kinematic_constraints::JointConstraint> js2;
js2.push_back(jc2);
boost::shared_ptr<constraint_samplers::JointConstraintSampler> jcsp2(new constraint_samplers::JointConstraintSampler(ps, "arms"));
EXPECT_TRUE(jcsp2->configure(js2));
kinematic_constraints::PositionConstraint pc(kmodel);
EXPECT_TRUE(pc.configure(pcm, tf));
kinematic_constraints::OrientationConstraint oc(kmodel);
EXPECT_TRUE(oc.configure(ocm, tf));
boost::shared_ptr<constraint_samplers::IKConstraintSampler> iksp(new constraint_samplers::IKConstraintSampler(ps, "left_arm"));
EXPECT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
EXPECT_TRUE(iksp->isValid());
std::vector<constraint_samplers::ConstraintSamplerPtr> cspv;
cspv.push_back(jcsp2);
cspv.push_back(iksp);
cspv.push_back(jcsp);
constraint_samplers::UnionConstraintSampler ucs(ps, "arms_and_torso", cspv);
//should have reordered to place whole body first
constraint_samplers::JointConstraintSampler* jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[0].get());
EXPECT_TRUE(jcs);
EXPECT_EQ(jcs->getJointModelGroup()->getName(), "arms_and_torso");
constraint_samplers::JointConstraintSampler* jcs2 = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[1].get());
EXPECT_TRUE(jcs2);
EXPECT_EQ(jcs2->getJointModelGroup()->getName(), "arms");
for (int t = 0 ; t < 100; ++t)
{
EXPECT_TRUE(ucs.sample(ks, ks_const, 100));
ks.update();
ks_const.update();
EXPECT_TRUE(jc1.decide(ks).satisfied);
EXPECT_TRUE(jc2.decide(ks).satisfied);
EXPECT_TRUE(pc.decide(ks).satisfied);
}
//now we add a position constraint on right arm
pcm.link_name = "r_wrist_roll_link";
ocm.link_name = "r_wrist_roll_link";
cspv.clear();
kinematic_constraints::PositionConstraint pc2(kmodel);
EXPECT_TRUE(pc2.configure(pcm, tf));
kinematic_constraints::OrientationConstraint oc2(kmodel);
EXPECT_TRUE(oc2.configure(ocm, tf));
boost::shared_ptr<constraint_samplers::IKConstraintSampler> iksp2(new constraint_samplers::IKConstraintSampler(ps, "right_arm"));
EXPECT_TRUE(iksp2->configure(constraint_samplers::IKSamplingPose(pc2, oc2)));
EXPECT_TRUE(iksp2->isValid());
//totally disjoint, so should break ties based on alphabetical order
cspv.clear();
cspv.push_back(iksp2);
cspv.push_back(iksp);
constraint_samplers::UnionConstraintSampler ucs2(ps, "arms_and_torso", cspv);
constraint_samplers::IKConstraintSampler* ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs2.getSamplers()[0].get());
ASSERT_TRUE(ikcs_test);
EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "left_arm");
//now we make left depends on right, right should stay first
pcm.link_name = "l_wrist_roll_link";
ocm.link_name = "l_wrist_roll_link";
pcm.header.frame_id = "r_wrist_roll_link";
ocm.header.frame_id = "r_wrist_roll_link";
EXPECT_TRUE(pc.configure(pcm, tf));
EXPECT_TRUE(oc.configure(ocm, tf));
ASSERT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
cspv.clear();
cspv.push_back(iksp2);
cspv.push_back(iksp);
constraint_samplers::UnionConstraintSampler ucs3(ps, "arms_and_torso", cspv);
ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs3.getSamplers()[0].get());
EXPECT_TRUE(ikcs_test);
EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "right_arm");
}
