Esempio n. 1
0
void SimKinect::initializeOpenRave(const std::string& filename)
{
	RaveInitialize(true, OpenRAVE::Level_Info);
	OpenRAVE::EnvironmentBasePtr env = OpenRAVE::RaveCreateEnvironment();
	bool success = env->Load(filename);
	ALWAYS_ASSERT(success);

	std::vector<OpenRAVE::RobotBasePtr> robots;
	env->GetRobots(robots);
	OpenRAVE::RobotBasePtr robot = robots[0];

	std::vector<OpenRAVE::KinBodyPtr> bodies;
	env->GetBodies(bodies);
	for (int i=0; i < bodies.size(); ++i) {
		OpenRAVE::KinBody& body = *bodies[i];
		LOG_INFO("Parsing kinbody %s", body.GetName().c_str());

		// TODO: Generalize to all objects in the scene except for the robot (PR2)
		if (body.GetName().compare("mug")==0)
		{
			const std::vector<OpenRAVE::KinBody::LinkPtr>& links = body.GetLinks();
			for (int i=0; i < links.size(); ++i) {
				LOG_INFO("\t\tParsing kinbody link %s", links[i]->GetName().c_str());
				if (links[i]->GetGeometries().size() > 0) {
					extractGeomFromLink(*links[i]);
				}
			}
		}
	}
}
OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type,
    std::string const &interface_name, std::istream &sinput, OpenRAVE::EnvironmentBasePtr env)
{

    if (type == OpenRAVE::PT_Sensor && interface_name == "bhtactilesensor") {
        std::string node_name, owd_namespace, robot_name, link_prefix;
        sinput >> node_name >> owd_namespace >> robot_name >> link_prefix;

        // Initialize the ROS node.
        RAVELOG_DEBUG("name = %s  namespace = %s\n", node_name.c_str(), owd_namespace.c_str());
        if (sinput.fail()) {
            RAVELOG_ERROR("BHTactileSensor is missing the node_name, owd_namespace, robot_name, or link_prefix parameter(s).\n");
            return OpenRAVE::InterfaceBasePtr();
        }

        if (!ros::isInitialized()) {
            int argc = 0;
            ros::init(argc, NULL, node_name, ros::init_options::AnonymousName);
            RAVELOG_DEBUG("Starting ROS node '%s'.\n", node_name.c_str());
        } else {
            RAVELOG_DEBUG("Using existing ROS node '%s'\n", ros::this_node::getName().c_str());
        }

        OpenRAVE::RobotBasePtr robot = env->GetRobot(robot_name);
        if (!robot) {
            throw OPENRAVE_EXCEPTION_FORMAT("There is no robot named '%s'.",
                                            robot_name.c_str(), OpenRAVE::ORE_InvalidArguments);
        }
        return boost::make_shared<BHTactileSensor>(env, robot, owd_namespace, link_prefix);
    } else {
Esempio n. 3
0
void ViewerTest::SetViewer(OpenRAVE::EnvironmentBasePtr &penv, const std::string &viewername)
{
    OpenRAVE::ViewerBasePtr viewer = OpenRAVE::RaveCreateViewer(penv, viewername);
    BOOST_ASSERT(!!viewer);
    // attach it to the environment:
    penv->Add(viewer);
    // finally call the viewer's infinite loop (this is why a separate thread is needed)
    bool showgui = true;
    viewer->main(showgui);
}
OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type, const std::string& interfacename, std::istream& sinput, OpenRAVE::EnvironmentBasePtr penv)
{
    if (type == OpenRAVE::PT_CollisionChecker && interfacename == "or_velma_q5q6_checker")
    {
        VelmaQ5Q6CollisionChecker *checker = new VelmaQ5Q6CollisionChecker(penv, penv->GetCollisionChecker());
        return OpenRAVE::InterfaceBasePtr(checker);
    }

    return InterfaceBasePtr();
}
Esempio n. 5
0
	bool checkifCollision(const std::vector<float> &sampleconfig,OpenRAVE::EnvironmentBasePtr env, OpenRAVE::RobotBasePtr robot)
	{
		std::vector<OpenRAVE::dReal> config(sampleconfig.begin(),sampleconfig.end());
		robot->SetActiveDOFValues(config);
		bool collision = env->CheckCollision(robot);
		bool selfcollision = robot->CheckSelfCollision();
		
		// float lolimit={-0.564602,-0.3536,-2.12131,-0.650001,-10000,-2.00001,-10000};
		// float uplimit ={2.13539,1.2963,-0.15,3.75,10000,-0.1,1000};

		if(collision||selfcollision)
			std::cout<<"Collision!"<<std::endl;
		return (collision||selfcollision);
	}
Esempio n. 6
0
MatrixP setup_particles(rave::EnvironmentBasePtr env) {
	rave::KinBodyPtr table = env->GetKinBody("table");
	rave::KinBody::LinkPtr base = table->GetLink("base");
	rave::Vector extents = base->GetGeometry(0)->GetBoxExtents();

	rave::Vector table_pos = table->GetTransform().trans;
	double x_min, x_max, y_min, y_max, z_min, z_max;
	x_min = table_pos.x - extents.x;
	x_max = table_pos.x + extents.x;
	y_min = table_pos.y - extents.y;
	y_max = table_pos.y + extents.y;
	z_min = table_pos.z + extents.z;
	z_max = table_pos.z + extents.z + .2;

	MatrixP P;

	// uniform
	for(int m=0; m < M_DIM; ++m) {
		P(0,m) = mm_utils::uniform(x_min, x_max);
		P(1,m) = mm_utils::uniform(y_min, y_max);
		P(2,m) = mm_utils::uniform(z_min, z_max);
	}

	// two clumps
//	for(int m=0; m < M_DIM; ++m) {
//		if (m < M_DIM/2) {
//			P(0,m) = mm_utils::uniform(x_min, .7*x_min + .3*x_max);
//			P(1,m) = mm_utils::uniform(y_min, y_max);
//			P(2,m) = mm_utils::uniform(z_min, z_max);
//		} else {
//			P(0,m) = mm_utils::uniform(.3*x_min + .7*x_max, x_max);
//			P(1,m) = mm_utils::uniform(y_min, y_max);
//			P(2,m) = mm_utils::uniform(z_min, z_max);
//		}
//	}

	return P;
}
Esempio n. 7
0
    // this method is copied from OctomapServer.cpp and modified
    void OctomapInterface::insertScan(const tf::Point& sensorOriginTf, const PCLPointCloud& ground, const PCLPointCloud& nonground){
        octomap::point3d sensorOrigin = octomap::pointTfToOctomap(sensorOriginTf);

        if (!m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMin)
            || !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax))
        {
            ROS_ERROR_STREAM("Could not generate Key for origin "<<sensorOrigin);
        }

        //
        // add mask information to the point cloud
        //
        OpenRAVE::EnvironmentBasePtr  penv = GetEnv();

        // create a vector of masked bodies
        std::vector<OpenRAVE::KinBodyPtr > maskedBodies;
        {
            boost::mutex::scoped_lock(m_maskedObjectsMutex);
            for (std::vector<std::string >::const_iterator o_it = m_maskedObjects.begin(); o_it != m_maskedObjects.end(); o_it++)
            {
                KinBodyPtr pbody = penv->GetKinBody(*o_it);
                if (pbody.get() != NULL)
                {
                    maskedBodies.push_back( pbody );
                }
            }
        }

        int numBodies = maskedBodies.size();

        // instead of direct scan insertion, compute update to filter ground:
        octomap::KeySet free_cells, occupied_cells;
        // insert ground points only as free:
        for (PCLPointCloud::const_iterator it = ground.begin(); it != ground.end(); ++it){
            octomap::point3d point(it->x, it->y, it->z);
            // maxrange check
            if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange) ) {
                point = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange;
            }

            // only clear space (ground points)
            if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
                free_cells.insert(m_keyRay.begin(), m_keyRay.end());
            }

            octomap::OcTreeKey endKey;
            if (m_octree->coordToKeyChecked(point, endKey)){
                updateMinKey(endKey, m_updateBBXMin);
                updateMaxKey(endKey, m_updateBBXMax);
            } else{
                ROS_ERROR_STREAM("Could not generate Key for endpoint "<<point);
            }
        }

        penv->Add(m_sphere, true);
        OpenRAVE::Transform tr;

        // all other points: free on ray, occupied on endpoint:
        for (PCLPointCloud::const_iterator it = nonground.begin(); it != nonground.end(); ++it){
            octomap::point3d point(it->x, it->y, it->z);
            tr.trans.x = it->x;
            tr.trans.y = it->y;
            tr.trans.z = it->z;
            m_sphere->SetTransform(tr);
            bool maskedHit = false;
            for (int b_idx = 0; b_idx < numBodies; b_idx++)
            {
                if (penv->CheckCollision(maskedBodies[b_idx], m_sphere))
                {
                    maskedHit = true;
                    break;
                }
            }

            if (maskedHit)
            {
                // free cells
                if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
                    free_cells.insert(m_keyRay.begin(), m_keyRay.end());
                }
            }
            else if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange) ) { // maxrange check

                // free cells
                if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
                    free_cells.insert(m_keyRay.begin(), m_keyRay.end());
                }
                // occupied endpoint
                octomap::OcTreeKey key;
                if (m_octree->coordToKeyChecked(point, key)){
                    occupied_cells.insert(key);

                    updateMinKey(key, m_updateBBXMin);
                    updateMaxKey(key, m_updateBBXMax);
                }
            } else {// ray longer than maxrange:;
                octomap::point3d new_end = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange;
                if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)){
                    free_cells.insert(m_keyRay.begin(), m_keyRay.end());

                    octomap::OcTreeKey endKey;
                    if (m_octree->coordToKeyChecked(new_end, endKey)){
                        updateMinKey(endKey, m_updateBBXMin);
                        updateMaxKey(endKey, m_updateBBXMax);
                    } else{
                        ROS_ERROR_STREAM("Could not generate Key for endpoint "<<new_end);
                    }
                }
            }
        }
        penv->Remove(m_sphere);

        // mark free cells only if not seen occupied in this cloud
        for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it){
            if (occupied_cells.find(*it) == occupied_cells.end()){
                m_octree->updateNode(*it, false);
            }
        }

        // now mark all occupied cells:
        for (octomap::KeySet::iterator it = occupied_cells.begin(), end=free_cells.end(); it!= end; it++) {
            m_octree->updateNode(*it, true);
        }

        // TODO: eval lazy+updateInner vs. proper insertion
        // non-lazy by default (updateInnerOccupancy() too slow for large maps)
        //m_octree->updateInnerOccupancy();
        octomap::point3d minPt, maxPt;
        ROS_DEBUG_STREAM("Bounding box keys (before): " << m_updateBBXMin[0] << " " <<m_updateBBXMin[1] << " " << m_updateBBXMin[2] << " / " <<m_updateBBXMax[0] << " "<<m_updateBBXMax[1] << " "<< m_updateBBXMax[2]);

        // TODO: snap max / min keys to larger voxels by m_maxTreeDepth
      //   if (m_maxTreeDepth < 16)
      //   {
      //      OcTreeKey tmpMin = getIndexKey(m_updateBBXMin, m_maxTreeDepth); // this should give us the first key at depth m_maxTreeDepth that is smaller or equal to m_updateBBXMin (i.e. lower left in 2D grid coordinates)
      //      OcTreeKey tmpMax = getIndexKey(m_updateBBXMax, m_maxTreeDepth); // see above, now add something to find upper right
      //      tmpMax[0]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
      //      tmpMax[1]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
      //      tmpMax[2]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
      //      m_updateBBXMin = tmpMin;
      //      m_updateBBXMax = tmpMax;
      //   }

        // TODO: we could also limit the bbx to be within the map bounds here (see publishing check)
        minPt = m_octree->keyToCoord(m_updateBBXMin);
        maxPt = m_octree->keyToCoord(m_updateBBXMax);
        ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<<maxPt);
        ROS_DEBUG_STREAM("Bounding box keys (after): " << m_updateBBXMin[0] << " " <<m_updateBBXMin[1] << " " << m_updateBBXMin[2] << " / " <<m_updateBBXMax[0] << " "<<m_updateBBXMax[1] << " "<< m_updateBBXMax[2]);

        if (m_compressMap)
            m_octree->prune();
    }
TEST_F(FeatureConstraintsTest, constrainConfiguration)
{
  // setting up environment and robot
  OpenRAVE::RaveInitialize();
  OpenRAVE::EnvironmentBasePtr env = OpenRAVE::RaveCreateEnvironment();
  OpenRAVE::RobotBasePtr herb = env->ReadRobotXMLFile("robots/herb2_padded_nosensors.robot.xml");
  env->Add(herb);
 
  OpenRAVE::RobotBase::ManipulatorPtr right_arm;
  std::vector<OpenRAVE::RobotBase::ManipulatorPtr> herb_manipulators = herb->GetManipulators();
  for (unsigned int i=0; i<herb_manipulators.size(); i++)
  {
    if(herb_manipulators[i] && (herb_manipulators[i]->GetName().compare("right_wam") == 0))
    {
      right_arm = herb_manipulators[i];
    }
  }
 
  herb->SetActiveManipulator(right_arm);
  herb->SetActiveDOFs(right_arm->GetArmIndices());

  // putting the bottle in the robot's right hand
  OpenRAVE::KinBodyPtr bottle_body = OpenRAVE::RaveCreateKinBody(env, "");
  ASSERT_TRUE(env->ReadKinBodyURI(bottle_body, "objects/household/fuze_bottle.kinbody.xml"));
  bottle_body->SetName("bottle");
  env->Add(bottle_body);

  OpenRAVE::geometry::RaveTransformMatrix<OpenRAVE::dReal> m;
  m.rotfrommat(-0.86354026,  0.50427591,  0.00200643, 
               -0.25814712, -0.44547139,  0.85727201,
                0.43319543,  0.73977094,  0.51485985);
  m.trans.x = 1.10322189;
  m.trans.y = -0.24693695;
  m.trans.z = 0.87205762;
  bottle_body->SetTransform(OpenRAVE::Transform(m));

  OpenRAVE::Transform bottle_transform1 = bottle_body->GetTransform();

  ASSERT_TRUE(herb->Grab(bottle_body));

  // adding a flying pan
  OpenRAVE::KinBodyPtr pan_body =  OpenRAVE::RaveCreateKinBody(env, "");
  ASSERT_TRUE(env->ReadKinBodyURI(pan_body, "objects/household/pan.kinbody.xml"));
  pan_body->SetName("pan");
  env->Add(pan_body);

  m.rotfrommat(2.89632833e-03,   9.99995806e-01,   1.19208782e-07,
               -9.99995806e-01,   2.89632833e-03,  -1.18864027e-07,
               -1.19208797e-07,  -1.18864013e-07,   1.00000000e+00);
  m.trans.x = 0.58;
  m.trans.y = -0.02;
  m.trans.z = 0.33;
  pan_body->SetTransform(OpenRAVE::Transform(m));

  // giving constraints access to environment and robot
  RobotPtr robot = RobotPtr(new Robot(herb));
  EnvironmentPtr environment = EnvironmentPtr(new Environment(env));
  constraints.setRobot(robot);
  constraints.setEnvironment(environment);

  // first test
  constraints.calculateConstraintValues();
  EXPECT_FALSE(constraints.areConstraintsFulfilled()); 

  // moving robot and bottle to new start configuration
  std::vector<double> new_config;
  new_config.push_back(2.0);
  new_config.push_back(1.0);
  new_config.push_back(-0.8);
  new_config.push_back(1.0);
  new_config.push_back(-1.0);
  new_config.push_back(0.0);
  new_config.push_back(0.0);

  robot->setJointValues(new_config);
  constraints.setJointValues(robot->getJointValues());
  constraints.calculateConstraintValues();

  // test we've really moved
  OpenRAVE::Transform bottle_transform2 = bottle_body->GetTransform();
  ASSERT_GT((bottle_transform1.trans - bottle_transform2.trans).lengthsqr3(), 0.1);
  ASSERT_GT((bottle_transform1.rot - bottle_transform2.rot).lengthsqr4(), 0.1);

  // actual projection test
  ASSERT_EQ(robot->getDOF(), constraints.getJointValues().size());

  std::cout << "\nprior to constraining:\n";
  std::vector<double> tmp = constraints.getTaskValues();
  for(unsigned int i=0; i<tmp.size(); i++)
  {
    std::cout << tmp[i] << " ";
  } 

  constraints.constrainCurrentConfiguration();
  std::cout << "\npost to constraining:\n";
  tmp = constraints.getTaskValues();
  for(unsigned int i=0; i<tmp.size(); i++)
  {
    std::cout << tmp[i] << " ";
  } 
  std::cout << "\n";

  EXPECT_TRUE(constraints.areConstraintsFulfilled());

  OpenRAVE::RaveDestroy();
}