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)); } } }
static bool isClosed(RobotBase::ManipulatorPtr manip, float closedThreshold) { vector<int> gripperInds = manip->GetGripperIndices(); manip->GetRobot()->SetActiveDOFs(gripperInds); vector<double> dof_values; manip->GetRobot()->GetActiveDOFValues(dof_values); bool out =dof_values[0] < closedThreshold; cout << "isclosed: " << out << endl; return out; }
/** Get pointers to the robot and manipulator in the viewer_env, and * clone the environment. */ void JacoOpenraveThread::_post_init() { #ifdef HAVE_OPENRAVE while( !__robot ) { __robot = __viewer_env.robot->get_robot_ptr(); usleep(100); } while( !__manip ) { EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex()); __manip = __robot->SetActiveManipulator(__cfg_manipname); usleep(100); } // create cloned environment for planning logger->log_debug(name(), "Clone environment for planning"); openrave->clone(__planner_env.env, __planner_env.robot, __planner_env.manip); if( !__planner_env.env || !__planner_env.robot || !__planner_env.manip) { throw fawkes::Exception("Could not clone properly, received a NULL pointer"); } // set name of environment switch( __arm->config ) { case CONFIG_SINGLE: __planner_env.env->set_name("Planner"); break; case CONFIG_LEFT: __planner_env.env->set_name("Planner_Left"); break; case CONFIG_RIGHT: __planner_env.env->set_name("Planner_Right"); break; } // set active manipulator in planning environment { EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex()); RobotBase::ManipulatorPtr manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__cfg_manipname); __planner_env.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices()); } // Get chain of links from arm base to manipulator in viewer_env. Used for plotting joints __robot->GetChain( __manip->GetBase()->GetIndex(), __manip->GetEndEffector()->GetIndex(), __links); #endif //HAVE_OPENRAVE }
bool ProblemBuilder::ReadBasicInfo(const Value& v) { FAIL_IF_FALSE(v.isMember("n_steps")); FAIL_IF_FALSE(v.isMember("manip")); vector<RobotBasePtr> robots; m_env->GetRobots(robots); RobotBasePtr robot = robots[0]; IPI_LOG_WARNING("using first robot in environment"); RobotBase::ManipulatorPtr manip = GetManipulatorByName(*robot, v["manip"].asString()); // todo: support various other dof collections that aren't manipulators IntVec joint_inds = manip->GetArmIndices(); m_rad = RobotAndDOFPtr(new RobotAndDOF(robot, joint_inds)); int n_dof = joint_inds.size(); int n_steps = v["n_steps"].asInt(); DblVec lower, upper; m_rad->GetDOFLimits(lower, upper); vector<double> vlower, vupper; vector<string> names; for (int i=0; i < n_steps; ++i) { vlower.insert(vlower.end(), lower.data(), lower.data()+lower.size()); vupper.insert(vupper.end(), upper.data(), upper.data()+upper.size()); for (unsigned j=0; j < n_dof; ++j) { names.push_back( (boost::format("j_%i_%i")%i%j).str() ); } } m_prob->createVariables(names, vlower, vupper); m_prob->m_traj_vars = VarArray(n_steps, n_dof, m_prob->vars_.data()); DblVec cur_dofvals = m_rad->GetDOFValues(); // fix first timestep vars for (int j=0; j < n_dof; ++j) { m_prob->addLinearConstr(exprSub(AffExpr(m_prob->m_traj_vars(0,j)), cur_dofvals[j]), EQ); } return true; }
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; }
void JacoOpenraveThread::_plan_path(RefPtr<jaco_target_t> &from, RefPtr<jaco_target_t> &to) { #ifdef HAVE_OPENRAVE // update state of the trajectory __arm->target_mutex->lock(); to->trajec_state = TRAJEC_PLANNING; __arm->target_mutex->unlock(); // Update bodies in planner-environment // clone robot state, ignoring grabbed bodies { EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex()); EnvironmentMutex::scoped_lock plan_lock(__planner_env.env->get_env_ptr()->GetMutex()); __planner_env.robot->get_robot_ptr()->ReleaseAllGrabbed(); __planner_env.env->delete_all_objects(); /* // Old method. Somehow we encountered problems. OpenRAVE internal bug? RobotBase::RobotStateSaver saver(__viewer_env.robot->get_robot_ptr(), 0xffffffff&~KinBody::Save_GrabbedBodies&~KinBody::Save_ActiveManipulator&~KinBody::Save_ActiveDOF); saver.Restore( __planner_env.robot->get_robot_ptr() ); */ // New method. Simply set the DOF values as they are in __viewer_env vector<dReal> dofs; __viewer_env.robot->get_robot_ptr()->GetDOFValues(dofs); __planner_env.robot->get_robot_ptr()->SetDOFValues(dofs); } // then clone all objects __planner_env.env->clone_objects( __viewer_env.env ); // restore robot state { EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex()); // Set active manipulator and active DOFs (need for planner and IK solver!) RobotBase::ManipulatorPtr manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__cfg_manipname); __planner_env.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices()); // update robot state with attached objects { EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex()); /* // Old method. Somehow we encountered problems. OpenRAVE internal bug? RobotBase::RobotStateSaver saver(__viewer_env.robot->get_robot_ptr(), KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_GrabbedBodies); saver.Restore( __planner_env.robot->get_robot_ptr() ); */ // New method. Grab all bodies in __planner_env that are grabbed in __viewer_env by this manipulator vector<RobotBase::GrabbedInfoPtr> grabbed; __viewer_env.robot->get_robot_ptr()->GetGrabbedInfo(grabbed); for( vector<RobotBase::GrabbedInfoPtr>::iterator it=grabbed.begin(); it!=grabbed.end(); ++it ) { logger->log_debug(name(), "compare _robotlinkname '%s' with our manip link '%s'", (*it)->_robotlinkname.c_str(), manip->GetEndEffector()->GetName().c_str()); if( (*it)->_robotlinkname == manip->GetEndEffector()->GetName() ) { logger->log_debug(name(), "attach '%s'!", (*it)->_grabbedname.c_str()); __planner_env.robot->attach_object((*it)->_grabbedname.c_str(), __planner_env.env, __cfg_manipname.c_str()); } } } } // Set target point for planner. Check again for IK, avoiding collisions with the environment //logger->log_debug(name(), "setting target %f %f %f %f %f %f", // to->pos.at(0), to->pos.at(1), to->pos.at(2), to->pos.at(3), to->pos.at(4), to->pos.at(5)); __planner_env.robot->enable_ik_comparison(true); if( to->type == TARGET_CARTESIAN ) { if( !__planner_env.robot->set_target_euler(EULER_ZXZ, to->pos.at(0), to->pos.at(1), to->pos.at(2), to->pos.at(3), to->pos.at(4), to->pos.at(5)) ) { logger->log_warn(name(), "Planning failed, second IK check failed"); __arm->target_mutex->lock(); to->trajec_state = TRAJEC_PLANNING_ERROR; __arm->target_mutex->unlock(); return; } else { // set target angles. This changes the internal target type to ANGLES (see openrave/robot.*) // and will use BaseManipulation's MoveActiveJoints. Otherwise it will use MoveToHandPosition, // which does not have the filtering of IK solutions for the closest one as we have. vector<float> target; __planner_env.robot->get_target().manip->get_angles(target); __planner_env.robot->set_target_angles(target); } } else { vector<float> target; //TODO: need some kind cheking for env-collision, i.e. if the target is colllision-free. // For now expect the user to know what he does, when he sets joint angles directly __planner_env.robot->get_target().manip->set_angles_device(to->pos); __planner_env.robot->get_target().manip->get_angles(target); __planner_env.robot->set_target_angles(target); } // Set starting point for planner //logger->log_debug(name(), "setting start %f %f %f %f %f %f", // from->pos.at(0), from->pos.at(1), from->pos.at(2), from->pos.at(3), from->pos.at(4), from->pos.at(5)); __planner_env.manip->set_angles_device(from->pos); // Set planning parameters __planner_env.robot->set_target_plannerparams(__plannerparams); // Run planner try { __planner_env.env->run_planner(__planner_env.robot, __cfg_OR_sampling); } catch (fawkes::Exception &e) { logger->log_warn(name(), "Planning failed: %s", e.what_no_backtrace()); // TODO: better handling! // for now just skip planning, so the target_queue can be processed __arm->target_mutex->lock(); //to->type = TARGET_ANGULAR; to->trajec_state = TRAJEC_PLANNING_ERROR; __arm->target_mutex->unlock(); return; } // add trajectory to queue //logger->log_debug(name(), "plan successful, adding to queue"); __arm->trajec_mutex->lock(); // we can do the following becaouse get_trajectory_device() returns a new object, thus // can be safely deleted by RefPtr auto-deletion to->trajec = RefPtr<jaco_trajec_t>( __planner_env.robot->get_trajectory_device() ); __arm->trajec_mutex->unlock(); // update target. __arm->target_mutex->lock(); //change target type to ANGULAR and set target->pos accordingly. This makes final-checking // in goto_thread much easier to->type = TARGET_ANGULAR; to->pos = to->trajec->back(); // update trajectory state to->trajec_state = TRAJEC_READY; __arm->target_mutex->unlock(); #endif }