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