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));
}
}
}
InterfaceBasePtr RegisterSimulationFunction(int environmentid, boost::python::object simulationfn)
{
ModuleBasePtr module = RaveCreateModule(RaveGetEnvironment(environmentid), "PythonBinding");
if( !!module ) {
boost::shared_ptr<FunctionUserData> p = boost::dynamic_pointer_cast<FunctionUserData>(module->GetUserData());
p->simulationfn = simulationfn;
module->GetEnv()->AddModule(module,"");
}
return InterfaceBasePtr(module);
}
int main(int argc, char ** argv)
{
if( argc < 3 ) {
RAVELOG_INFO("ikloader robot iktype\n");
return 1;
}
string robotname = argv[1];
string iktype = argv[2];
RaveInitialize(true); // start openrave core
EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
{
// lock the environment to prevent changes
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
// load the scene
RobotBasePtr probot = penv->ReadRobotXMLFile(robotname);
if( !probot ) {
penv->Destroy();
return 2;
}
penv->Add(probot);
ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
ssin << "LoadIKFastSolver " << probot->GetName() << " " << iktype;
// if necessary, add free inc for degrees of freedom
//ssin << " " << 0.04f;
// get the active manipulator
RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
if( !pikfast->SendCommand(ssout,ssin) ) {
RAVELOG_ERROR("failed to load iksolver\n");
penv->Destroy();
return 1;
}
RAVELOG_INFO("testing random ik\n");
while(1) {
Transform trans;
trans.rot = quatFromAxisAngle(Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5));
trans.trans = Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)*2;
vector<dReal> vsolution;
if( pmanip->FindIKSolution(IkParameterization(trans),vsolution,IKFO_CheckEnvCollisions) ) {
stringstream ss; ss << "solution is: ";
for(size_t i = 0; i < vsolution.size(); ++i) {
ss << vsolution[i] << " ";
}
ss << endl;
RAVELOG_INFO(ss.str());
}
else {
// could fail due to collisions, etc
}
}
}
RaveDestroy(); // destroy
return 0;
}
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));
}
}
}
bool SimulationStep(dReal fElapsedTime) {
return _pmodule->SimulationStep(fElapsedTime);
}
