TEST_F(LoadPlanningModelsPr2, StateSpaceCopy)
{
ompl_interface::ModelBasedStateSpaceSpecification spec(kmodel_, "right_arm");
ompl_interface::JointModelStateSpace ss(spec);
ss.setPlanningVolume(-1, 1, -1, 1, -1, 1);
ss.setup();
std::ofstream fout("ompl_interface_test_state_space_diagram1.dot");
ss.diagram(fout);
bool passed = false;
try
{
ss.sanityChecks();
passed = true;
}
catch(ompl::Exception &ex)
{
logError("Sanity checks did not pass: %s", ex.what());
}
EXPECT_TRUE(passed);
robot_state::RobotState kstate(kmodel_);
kstate.setToRandomValues();
EXPECT_TRUE(kstate.distance(kstate) < 1e-12);
ompl::base::State *state = ss.allocState();
for (int i = 0 ; i < 10 ; ++i)
{
robot_state::RobotState kstate2(kstate);
EXPECT_TRUE(kstate.distance(kstate2) < 1e-12);
ss.copyToOMPLState(state, kstate);
kstate.getJointStateGroup(ss.getJointModelGroupName())->setToRandomValues();
std::cout << (kstate.getLinkState("r_wrist_roll_link")->getGlobalLinkTransform().translation() -
kstate2.getLinkState("r_wrist_roll_link")->getGlobalLinkTransform().translation()) << std::endl;
EXPECT_TRUE(kstate.distance(kstate2) > 1e-12);
ss.copyToRobotState(kstate, state);
std::cout << (kstate.getLinkState("r_wrist_roll_link")->getGlobalLinkTransform().translation() -
kstate2.getLinkState("r_wrist_roll_link")->getGlobalLinkTransform().translation()) << std::endl;
EXPECT_TRUE(kstate.distance(kstate2) < 1e-12);
}
ss.freeState(state);
}
TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached)
{
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
shapes::ShapeConstPtr shape(shapes::createMeshFromResource(kinect_dae_resource_));
Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
pos2.translation().x() = 10.0;
cworld_->getWorld()->addToObject("kinect", shape, pos1);
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
kstate.update();
ros::WallTime before = ros::WallTime::now();
cworld_->checkRobotCollision(req, res, *crobot_, kstate);
double first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
cworld_->checkRobotCollision(req, res, *crobot_, kstate);
double second_check = (ros::WallTime::now()-before).toSec();
EXPECT_LT(second_check, .05);
collision_detection::CollisionWorld::ObjectPtr object = cworld_->getWorld()->getObject("kinect");
cworld_->getWorld()->removeObject("kinect");
robot_state::RobotState kstate1(kmodel_);
robot_state::RobotState kstate2(kmodel_);
kstate1.setToDefaultValues();
kstate2.setToDefaultValues();
kstate1.update();
kstate2.update();
std::vector<std::string> touch_links;
kstate1.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
EigenSTL::vector_Affine3d other_poses;
other_poses.push_back(pos2);
// This creates a new set of constant properties for the attached body, which happens to be the same as the one above;
kstate2.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
//going to take a while, but that's fine
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req,res,kstate1);
EXPECT_TRUE(res.collision);
before = ros::WallTime::now();
crobot_->checkSelfCollision(req,res,kstate1, *acm_);
first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
req.verbose = true;
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req,res,kstate2, *acm_);
second_check = (ros::WallTime::now()-before).toSec();
EXPECT_LT(first_check, .05);
EXPECT_LT(fabs(first_check-second_check), .1);
}
