Ejemplo n.º 1
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
}
Ejemplo n.º 2
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;

}
Ejemplo n.º 3
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
}