void ORCEnvironmentUnock(void* env)
{
#if BOOST_VERSION < 103500
throw OPENRAVE_EXCEPTION_FORMAT0("unlocking with boost version < 1.35 is not supported", ORE_Failed);
#else
GetEnvironment(env)->GetMutex().unlock();
#endif
}
virtual void demothread(int argc, char ** argv) {
string scenefilename = "data/pa10grasp2.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
RobotBasePtr probot = vrobots.at(0);
// find a manipulator chain to move
for(size_t i = 0; i < probot->GetManipulators().size(); ++i) {
if( probot->GetManipulators()[i]->GetName().find("arm") != string::npos ) {
probot->SetActiveManipulator(probot->GetManipulators()[i]);
break;
}
}
RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
// load inverse kinematics using ikfast
ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
vector<dReal> vsolution;
ssin << "LoadIKFastSolver " << probot->GetName() << " " << (int)IKP_Transform6D;
if( !pikfast->SendCommand(ssout,ssin) ) {
RAVELOG_ERROR("failed to load iksolver\n");
}
if( !pmanip->GetIkSolver()) {
throw OPENRAVE_EXCEPTION_FORMAT0("need ik solver",ORE_Assert);
}
ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
penv->Add(pbasemanip,true,probot->GetName()); // load the module
while(IsOk()) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
// find a new manipulator position and feed that into the planner. If valid, robot will move to it safely.
Transform t = pmanip->GetEndEffectorTransform();
t.trans += Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f);
t.rot = quatMultiply(t.rot,quatFromAxisAngle(Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f)*0.2f));
ssin.str("");
ssin.clear();
ssin << "MoveToHandPosition pose " << t;
// start the planner and run the robot
RAVELOG_INFO("%s\n",ssin.str().c_str());
if( !pbasemanip->SendCommand(ssout,ssin) ) {
continue;
}
}
// unlock the environment and wait for the robot to finish
while(!probot->GetController()->IsDone() && IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
}
void SetManipulatorBias(RobotBase::ManipulatorConstPtr pmanip, const Vector& vbiasdirection, dReal nullsampleprob, dReal nullbiassampleprob, dReal deltasampleprob)
{
#ifdef OPENRAVE_HAS_LAPACK
_pmanip = pmanip;
_vbiasdirection = vbiasdirection;
_vbiasdofdirection.resize(0);
_vbiasnullspace.resize(0);
_nullsampleprob = nullsampleprob;
_nullbiassampleprob = nullbiassampleprob;
_deltasampleprob = deltasampleprob;
_busebiasing = true;
RAVELOG_VERBOSE_FORMAT("set bias nullsampleprob %f nullbiassampleprob %f deltasampleprob %f", _nullsampleprob%_nullbiassampleprob%_deltasampleprob);
#else
throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot set manipulator bias since lapack is not supported"), ORE_CommandNotSupported);
#endif
}
