Esempio n. 1
0
    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));
            }
        }
    }
Esempio n. 2
0
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;
}
Esempio n. 3
0
/** 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
}
Esempio n. 4
0
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;

}
Esempio n. 5
0
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;
}
Esempio n. 6
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
}