bool TaskSpaceRegion::Initialize(EnvironmentBasePtr penv_in)
{
_volume = 0;
_sumbounds = 0;
_dimensionality = 0;
for(int i = 0; i < 6; i++)
{
//compute volume in whatever dimensionality this defines
//when Bw values are backwards, it signifies an axis flip (not an error)
if(Bw[i][1] != Bw[i][0])
{
_volume = _volume * fabs(Bw[i][1] - Bw[i][0]);
_sumbounds = _sumbounds + fabs(Bw[i][1] - Bw[i][0]);
_dimensionality++;
}
}
if( stricmp(relativebodyname.c_str(), "NULL") == 0 )
{
prelativetolink.reset();
}
else
{
KinBodyPtr pobject;
pobject = penv_in->GetKinBody(relativebodyname.c_str());
if(pobject.get() == NULL)
{
RAVELOG_INFO("Error: could not find the specified object to attach frame\n");
return false;
}
//find the link
vector<KinBody::LinkPtr> vlinks = pobject->GetLinks();
bool bGotLink = false;
for(int j =0; j < vlinks.size(); j++)
{
if(strcmp(relativelinkname.c_str(), vlinks[j]->GetName().c_str()) == 0 )
{
RAVELOG_INFO("frame link: %s:%s\n",vlinks[j]->GetParent()->GetName().c_str(),vlinks[j]->GetName().c_str() );
prelativetolink = vlinks[j];
bGotLink = true;
break;
}
}
if(!bGotLink)
{
RAVELOG_INFO("Error: could not find the specified link of the object to attach frame\n");
return false;
}
}
//Print();
return true;
}
KinBodyItem::KinBodyItem(QtCoinViewerPtr viewer, KinBodyPtr pchain, ViewGeometry viewmode) : Item(viewer), _viewmode(viewmode)
{
_pchain = pchain;
bGrabbed = false;
_userdata = 0;
_bReload = false;
_bDrawStateChanged = false;
networkid = pchain->GetEnvironmentId();
_geometrycallback = pchain->RegisterChangeCallback(KinBody::Prop_LinkGeometry, boost::bind(&KinBodyItem::GeometryChangedCallback,this));
_drawcallback = pchain->RegisterChangeCallback(KinBody::Prop_LinkDraw, boost::bind(&KinBodyItem::DrawChangedCallback,this));
}
void Read(KinBodyPtr& pbody, const std::vector<char>& data,const AttributesList& atts)
{
_ProcessAtts(atts);
if( !pbody ) {
pbody = RaveCreateKinBody(_penv,_bodytype);
}
pbody->SetName(_bodyname);
Assimp::XFileParserOpenRAVE parser(data);
_Read(pbody,parser.GetImportedData());
if( pbody->GetName().size() == 0 ) {
pbody->SetName("body");
}
}
void ReadFile(KinBodyPtr& pbody, const std::string& filename, const AttributesList& atts)
{
std::ifstream f(filename.c_str());
if( !f ) {
throw OPENRAVE_EXCEPTION_FORMAT("failed to read %s filename",filename,ORE_InvalidArguments);
}
f.seekg(0,ios::end);
std::vector<char> filedata(static_cast<size_t>(f.tellg())+1, 0); // need a null-terminator
f.seekg(0,ios::beg);
f.read(&filedata[0], filedata.size());
Read(pbody,filedata,atts);
pbody->__struri = filename;
#if defined(HAVE_BOOST_FILESYSTEM) && BOOST_VERSION >= 103600 // stem() was introduced in 1.36
boost::filesystem::path bfpath(filename);
#if defined(BOOST_FILESYSTEM_VERSION) && BOOST_FILESYSTEM_VERSION >= 3
pbody->SetName(bfpath.stem().string());
#else
pbody->SetName(bfpath.stem());
#endif
#endif
}
object GetLinkVelocities(PyKinBodyPtr pykinbody)
{
CHECK_POINTER(pykinbody);
KinBodyPtr pbody = openravepy::GetKinBody(pykinbody);
if( pbody->GetLinks().size() == 0 ) {
return numeric::array(boost::python::list());
}
std::vector<std::pair<Vector,Vector> > velocities;
if( !_pPhysicsEngine->GetLinkVelocities(pbody,velocities) ) {
return object();
}
npy_intp dims[] = { velocities.size(),6};
PyObject *pyvel = PyArray_SimpleNew(2,dims, sizeof(dReal)==8 ? PyArray_DOUBLE : PyArray_FLOAT);
dReal* pfvel = (dReal*)PyArray_DATA(pyvel);
for(size_t i = 0; i < velocities.size(); ++i) {
pfvel[6*i+0] = velocities[i].first.x;
pfvel[6*i+1] = velocities[i].first.y;
pfvel[6*i+2] = velocities[i].first.z;
pfvel[6*i+3] = velocities[i].second.x;
pfvel[6*i+4] = velocities[i].second.y;
pfvel[6*i+5] = velocities[i].second.z;
}
return static_cast<numeric::array>(handle<>(pyvel));
}
int main(int argc, char **argv)
{
std::string environment_uri;
std::string robot_name;
std::string plugin_name;
size_t num_trials;
bool self = false;
bool profile = false;
// Parse arguments.
po::options_description desc("Profile OpenRAVE's memory usage.");
desc.add_options()
("num-samples", po::value<size_t>(&num_trials)->default_value(10000),
"number of samples to run")
("self", po::value<bool>(&self)->zero_tokens(),
"run self-collision checks")
("profile", po::value<bool>(&profile)->zero_tokens(),
"remove objects from environment")
("environment_uri",
po::value<std::string>(&environment_uri)->required(),
"number of samples to run")
("robot", po::value<std::string>(&robot_name)->required(),
"robot_name")
("collision_checker",
po::value<std::string>(&plugin_name)->required(),
"collision checker name")
("help",
"print usage information")
;
po::positional_options_description pd;
pd.add("environment_uri", 1);
pd.add("robot", 1);
pd.add("collision_checker", 1);
po::variables_map vm;
po::store(
po::command_line_parser(argc, argv)
.options(desc)
.positional(pd).run(),
vm
);
po::notify(vm);
if (vm.count("help")) {
std::cout << desc << std::endl;
return 1;
}
// Create the OpenRAVE environment.
RaveInitialize(true);
EnvironmentBasePtr const env = RaveCreateEnvironment();
CollisionCheckerBasePtr const collision_checker
= RaveCreateCollisionChecker(env, plugin_name);
env->SetCollisionChecker(collision_checker);
env->StopSimulation();
// "/usr/share/openrave-0.9/data/wamtest1.env.xml"
env->Load(environment_uri);
KinBodyPtr const body = env->GetKinBody(robot_name);
// Generate random configuations.
std::vector<OpenRAVE::dReal> lower;
std::vector<OpenRAVE::dReal> upper;
body->GetDOFLimits(lower, upper);
std::vector<std::vector<double> > data;
data = benchmarks::DataUtils::GenerateRandomConfigurations(
num_trials, lower, upper);
//
RAVELOG_INFO("Running %d collision checks.\n", num_trials);
boost::timer const timer;
if (profile) {
std::string const prof_name = str(
format("CheckCollision.%s.prof") % plugin_name);
RAVELOG_INFO("Writing gperftools information to '%s'.\n",
prof_name.c_str()
);
ProfilerStart(prof_name.c_str());
}
size_t num_collision = 0;
for (size_t i = 0; i < num_trials; ++i) {
body->SetDOFValues(data[i]);
bool is_collision;
if (self) {
is_collision = body->CheckSelfCollision();
} else {
is_collision = env->CheckCollision(body);
}
num_collision += !!is_collision;
}
if (profile) {
ProfilerStop();
}
double const duration = timer.elapsed();
RAVELOG_INFO(
"Ran %d collision checks (%d in collision) in %f seconds (%f checks per second).\n",
num_trials, num_collision, duration, num_trials / duration
);
return 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();
}
KinBody::LinkPtr GetCppLink(py::object py_link, EnvironmentBasePtr env) {
KinBodyPtr parent = GetCppKinBody(py_link.attr("GetParent")(), env);
int idx = py::extract<int>(py_link.attr("GetIndex")());
return parent->GetLinks()[idx];
}
