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; }
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)); }
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]; }