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);
}
Exemplo n.º 4
0
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;
}