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* ORCModuleCreate(void* env, const char* modulename)
{
ModuleBasePtr module = RaveCreateModule(GetEnvironment(env), modulename);
if( !module ) {
return NULL;
}
return new InterfaceBasePtr(module);
}
bool teo::TeoSimRateThread::threadInit() {
printf("[TeoSimRateThread] begin: threadInit()\n");
jmcMs = this->getRate();
yarp::os::ConstString externObj = DEFAULT_EXTERN_OBJ;
if(externObj=="redCan") {
OpenRAVE::RaveLoadPlugin("ExternObj");
OpenRAVE::ModuleBasePtr pExternObj = RaveCreateModule(environmentPtr,"ExternObj"); // create the module
environmentPtr->Add(pExternObj,true); // load the module, calls main and also enables good destroy.
std::stringstream cmdin,cmdout;
cmdin << "Open"; // default maxiter:4000
RAVELOG_INFO("%s\n",cmdin.str().c_str());
if( !pExternObj->SendCommand(cmdout,cmdin) ) {
fprintf(stderr,"Bad send Open command.\n");
}
printf("Sent Open command.\n");
}
lastTime = yarp::os::Time::now();
printf("[TeoSimRateThread] end: threadInit()\n");
return true;
}
virtual void demothread(int argc, char ** argv) {
string scenefilename = "/home/yo/asibot/main/app/ravebot/models/asibot_kitchen_wheelchair.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
KinBodyPtr objPtr = penv->GetKinBody("asibot");
if(objPtr) {
if(vrobots.at(1)->Grab(objPtr)) printf("[success] object asibot exists and grabbed.\n");
else printf("[warning] object asibot exists but not grabbed.\n");
} else printf("[fail] object asibot does not exist.\n");
printf("begin wait\n");
sleep(5);
printf("end wait\n");
RobotBasePtr probot = vrobots.at(1);
vector<dReal> v(2);
/* vector<int> vindices(probot->GetDOF());
for(size_t i = 0; i < vindices.size(); ++i) {
vindices[i] = i;
}
probot->SetActiveDOFs(vindices);*/
vector<int> vindices; // send it empty instead
//probot->SetActiveDOFs(vindices,DOF_X|DOF_Y|DOF_RotationAxis,Vector(0,0,1));
probot->SetActiveDOFs(vindices,DOF_X|DOF_Y,Vector(0,0,1));
ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
penv->Add(pbasemanip,true,probot->GetName()); // load the module
int iteracion = 0;
while(IsOk()) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
// find a set of free joint values for the robot
{
RobotBase::RobotStateSaver saver(probot); // save the state
while(1) {
Transform T = probot->GetTransform();
if(iteracion%2) {
v[0] = T.trans.x;
v[1] = T.trans.y+1;
//v[2] = T.trans.z;
iteracion++;
} else {
v[0] = T.trans.x;
v[1] = T.trans.y-0.5;
//v[2] = T.trans.z;
iteracion++;
}
if(iteracion==5) vrobots.at(1)->Release(objPtr);
probot->SetActiveDOFValues(v);
if( !penv->CheckCollision(probot) && !probot->CheckSelfCollision() ) {
break;
}
}
// robot state is restored
}
stringstream cmdin,cmdout;
cmdin << "MoveActiveJoints maxiter 2000 maxtries 1 goal ";
for(size_t i = 0; i < v.size(); ++i) {
cmdin << v[i] << " ";
}
// start the planner and run the robot
RAVELOG_INFO("%s\n",cmdin.str().c_str());
if( !pbasemanip->SendCommand(cmdout,cmdin) ) {
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));
}
}
}
