TEST_F(FclCollisionDetectionTester, DiffSceneTester)
{
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
kstate.update();
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
collision_detection::CollisionRobotFCL new_crobot(*(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
ros::WallTime before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
double first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
double second_check = (ros::WallTime::now()-before).toSec();
EXPECT_LT(fabs(first_check-second_check), .05);
std::vector<shapes::ShapeConstPtr> shapes;
shapes.resize(1);
shapes[0].reset(shapes::createMeshFromResource(kinect_dae_resource_));
EigenSTL::vector_Affine3d poses;
poses.push_back(Eigen::Affine3d::Identity());
std::vector<std::string> touch_links;
kstate.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link");
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
second_check = (ros::WallTime::now()-before).toSec();
//the first check is going to take a while, as data must be constructed
EXPECT_LT(second_check, .1);
collision_detection::CollisionRobotFCL other_new_crobot(*(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
second_check = (ros::WallTime::now()-before).toSec();
EXPECT_LT(fabs(first_check-second_check), .05);
}
TEST_F(FclCollisionDetectionTester, TestChangingShapeSize)
{
robot_state::RobotState kstate1(kmodel_);
kstate1.setToDefaultValues();
kstate1.update();
collision_detection::CollisionRequest req1;
collision_detection::CollisionResult res1;
ASSERT_FALSE(res1.collision);
EigenSTL::vector_Affine3d poses;
std::vector<shapes::ShapeConstPtr> shapes;
for(unsigned int i = 0; i < 5; i++)
{
cworld_->getWorld()->removeObject("shape");
shapes.clear();
poses.clear();
shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1+i*.0001, 1+i*.0001, 1+i*.0001)));
poses.push_back(Eigen::Affine3d::Identity());
cworld_->getWorld()->addToObject("shape", shapes, poses);
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
ASSERT_TRUE(res.collision);
}
Eigen::Affine3d kinect_pose = Eigen::Affine3d::Identity();
shapes::ShapePtr kinect_shape;
kinect_shape.reset(shapes::createMeshFromResource(kinect_dae_resource_));
cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
collision_detection::CollisionRequest req2;
collision_detection::CollisionResult res2;
cworld_->checkCollision(req2, res2, *crobot_, kstate1, *acm_);
ASSERT_TRUE(res2.collision);
for(unsigned int i = 0; i < 5; i++) {
cworld_->getWorld()->removeObject("shape");
shapes.clear();
poses.clear();
shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1+i*.0001, 1+i*.0001, 1+i*.0001)));
poses.push_back(Eigen::Affine3d::Identity());
cworld_->getWorld()->addToObject("shape", shapes, poses);
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
ASSERT_TRUE(res.collision);
}
}
TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed)
{
EigenSTL::vector_Affine3d poses;
std::vector<shapes::ShapeConstPtr> shapes;
for(unsigned int i = 0; i < 10000; i++) {
poses.push_back(Eigen::Affine3d::Identity());
shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01)));
}
ros::WallTime start = ros::WallTime::now();
cworld_->getWorld()->addToObject("map", shapes, poses);
double t = (ros::WallTime::now()-start).toSec();
EXPECT_GE(1.0, t);
// this is not really a failure; it is just that slow;
// looking into doing collision checking with a voxel grid.
logInform("Adding boxes took %g", t);
}
TEST_F(LoadPlanningModelsPr2, FullTest)
{
moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model, srdf_model));
moveit::core::RobotState ks(robot_model);
ks.setToDefaultValues();
moveit::core::RobotState ks2(robot_model);
ks2.setToDefaultValues();
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
shapes::Shape* shape = new shapes::Box(.1,.1,.1);
shapes.push_back(shapes::ShapeConstPtr(shape));
poses.push_back(Eigen::Affine3d::Identity());
std::set<std::string> touch_links;
trajectory_msgs::JointTrajectory empty_state;
moveit::core::AttachedBody *attached_body =
new moveit::core::AttachedBody(robot_model->getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state);
ks.attachBody(attached_body);
std::vector<const moveit::core::AttachedBody*> attached_bodies_1;
ks.getAttachedBodies(attached_bodies_1);
ASSERT_EQ(attached_bodies_1.size(), 1);
std::vector<const moveit::core::AttachedBody*> attached_bodies_2;
ks2 = ks;
ks2.getAttachedBodies(attached_bodies_2);
ASSERT_EQ(attached_bodies_2.size(), 1);
ks.clearAttachedBody("box");
attached_bodies_1.clear();
ks.getAttachedBodies(attached_bodies_1);
ASSERT_EQ(attached_bodies_1.size(), 0);
ks2 = ks;
attached_bodies_2.clear();
ks2.getAttachedBodies(attached_bodies_2);
ASSERT_EQ(attached_bodies_2.size(), 0);
}
void MotionPlanningFrame::attachObjectToState(robot_state::RobotState& state,
const std::string& object_id,
const std::string& link_id,
const Eigen::Affine3d& T_object_link)
{
state.clearAttachedBodies();
if (object_id.empty())
return;
// Compute the object key.
KeyPoseMap world_state = computeWorldKeyPoseMap();
std::string object_key = computeNearestFrameKey(object_id, link_id, state, world_state);
// Extract the object from the world.
collision_detection::CollisionWorld::ObjectConstPtr object;
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
{
planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
object = ps->getWorld()->getObject(object_key);
}
if (!object)
{
if (!object_id.empty())
ROS_WARN("Failed to attach %s to %s. Object does not exist.",
object_id.c_str(), link_id.c_str());
return;
}
// Compute poses relative to link.
shapes = object->shapes_;
poses = object->shape_poses_;
for (int i = 0; i < poses.size(); i++)
poses[i] = T_object_link * poses[0].inverse() * poses[i];
// Attach collision object to robot.
moveit_msgs::AttachedCollisionObject aco; // For dummy arguments.
state.attachBody(object_id, shapes, poses, aco.touch_links, link_id, aco.detach_posture);
// Update state.
state.update();
}
//TEST_F(LoadPlanningModelsPr2, robot_state::RobotState *Copy)
TEST_F(LoadPlanningModelsPr2, FullTest)
{
robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
robot_state::RobotState ks(kmodel);
ks.setToDefaultValues();
robot_state::RobotState ks2(kmodel);
ks2.setToDefaultValues();
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
shapes::Shape* shape = new shapes::Box(.1,.1,.1);
shapes.push_back(shapes::ShapeConstPtr(shape));
poses.push_back(Eigen::Affine3d::Identity());
std::set<std::string> touch_links;
robot_state::AttachedBody attached_body(ks.getLinkState("r_gripper_palm_link")->getLinkModel(), "box", shapes, poses, touch_links);
ks.attachBody(&attached_body);
std::vector<const robot_state::AttachedBody*> attached_bodies_1;
ks.getAttachedBodies(attached_bodies_1);
ASSERT_EQ(attached_bodies_1.size(),1);
std::vector<const robot_state::AttachedBody*> attached_bodies_2;
ks2 = ks;
ks2.getAttachedBodies(attached_bodies_2);
ASSERT_EQ(attached_bodies_2.size(),1);
ks.clearAttachedBody("box");
attached_bodies_1.clear();
ks.getAttachedBodies(attached_bodies_1);
ASSERT_EQ(attached_bodies_1.size(),0);
ks2 = ks;
attached_bodies_2.clear();
ks2.getAttachedBodies(attached_bodies_2);
ASSERT_EQ(attached_bodies_2.size(),0);
}
TEST_F(FclCollisionDetectionTester, AttachedBodyTester) {
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
kstate.update();
Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
pos1.translation().x() = 5.0;
// kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
kstate.update();
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_FALSE(res.collision);
shapes::Shape* shape = new shapes::Box(.1,.1,.1);
cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
res = collision_detection::CollisionResult();
cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
ASSERT_TRUE(res.collision);
//deletes shape
cworld_->getWorld()->removeObject("box");
shape = new shapes::Box(.1,.1,.1);
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
shapes.push_back(shapes::ShapeConstPtr(shape));
poses.push_back(Eigen::Affine3d::Identity());
std::vector<std::string> touch_links;
kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_TRUE(res.collision);
//deletes shape
kstate.clearAttachedBody("box");
touch_links.push_back("r_gripper_palm_link");
touch_links.push_back("r_gripper_motor_accelerometer_link");
shapes[0].reset(new shapes::Box(.1,.1,.1));
kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
kstate.update();
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_FALSE(res.collision);
pos1.translation().x() = 5.01;
shapes::Shape* coll = new shapes::Box(.1, .1, .1);
cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
res = collision_detection::CollisionResult();
cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
ASSERT_TRUE(res.collision);
acm_->setEntry("coll", "r_gripper_palm_link", true);
res = collision_detection::CollisionResult();
cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
ASSERT_TRUE(res.collision);
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "acorn_play");
ros::NodeHandle nh;
InteractiveRobot robot;
// create a PlanningScene
g_planning_scene = new planning_scene::PlanningScene(robot.robotModel());
// Add the world geometry to the PlanningScene's collision detection world
Eigen::Affine3d world_cube_pose;
double world_cube_size;
robot.getWorldGeometry(world_cube_pose, world_cube_size);
g_world_cube_shape.reset(new shapes::Box(world_cube_size, world_cube_size, world_cube_size));
g_planning_scene->getWorldNonConst()->addToObject("world_cube", g_world_cube_shape, world_cube_pose);
// Create a marker array publisher for publishing contact points
g_marker_array_publisher = new ros::Publisher(nh.advertise<visualization_msgs::MarkerArray>("interactive_robot_marray",100));
// Attach a bar object to the right gripper
robot_state::LinkState *link = robot.robotState()->getLinkState("r_gripper_palm_link");
//robot_state::LinkState *link = robot.robotState()->getLinkState("r_gripper_r_finger_link");
//robot_state::LinkState *link = robot.robotState()->getLinkState("r_wrist_roll_link");
//
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
shapes::ShapePtr bar_shape;
bar_shape.reset(new shapes::Cylinder(0.02,1.0));
//bar_shape.reset(new shapes::Box(0.02,.02,1));
shapes.push_back(bar_shape);
poses.push_back(Eigen::Affine3d(Eigen::Translation3d(0.12,0,0)));
const robot_model::JointModelGroup* r_gripper_group = robot.robotModel()->getJointModelGroup("right_gripper");
const std::vector<std::string>& touch_links = r_gripper_group->getLinkModelNames();
robot.robotState()->attachBody("bar",
shapes,
poses,
touch_links,
"r_gripper_palm_link");
robot.setUserCallback(userCallback);
help();
ros::spin();
delete g_planning_scene;
delete g_marker_array_publisher;;
ros::shutdown();
return 0;
}
