コード例 #1
0
ファイル: Server.cpp プロジェクト: openhumanoids/exotica
 void Server::listParameters()
 {
   HIGHLIGHT("************* Parameters *************");
   for (auto & it : params_)
   {
     HIGHLIGHT("Parameter: "<<it.first);
   }
   HIGHLIGHT("**************************************");
 }
コード例 #2
0
ファイル: IMesh.cpp プロジェクト: openhumanoids/exotica
 void IMesh::initDebug(std::string ref)
 {
     imesh_mark_.scale.x = 0.005;
     imesh_mark_.color.a = imesh_mark_.color.r = 1;
     imesh_mark_.type = visualization_msgs::Marker::LINE_LIST;
     imesh_mark_.header.frame_id = ref;
     imesh_mark_.ns = getObjectName();
     imesh_mark_pub_ = server_->advertise<visualization_msgs::Marker>(ns_ +"/InteractionMesh", 1, true);
     HIGHLIGHT("InteractionMesh connectivity is published on ROS topic "<<imesh_mark_pub_.getTopic()<<", in reference frame "<<ref);
 }
コード例 #3
0
void *WorkScheduler::thread_execute_gpu(void *data)
{
	Device *device = (Device *)data;
	WorkPackage *work;
	
	while ((work = (WorkPackage *)BLI_thread_queue_pop(g_gpuqueue))) {
		HIGHLIGHT(work);
		device->execute(work);
		delete work;
	}
	
	return NULL;
}
コード例 #4
0
ファイル: OMPLsolver.cpp プロジェクト: openhumanoids/exotica
void OMPLsolver::Solve(Eigen::VectorXdRefConst q0,
                       Eigen::MatrixXd & solution)
{
    if (base_solver_->solve(q0, solution))
    {
        HIGHLIGHT(
            "OMPL solving succeeded, planning time "<<base_solver_->getPlanningTime()<<"sec");
    }
    else
    {
        throw_solve("OMPL solving failed");
    }
}
コード例 #5
0
ファイル: COM_WorkScheduler.cpp プロジェクト: diekev/blender
void *WorkScheduler::thread_execute_cpu(void *data)
{
	CPUDevice *device = (CPUDevice *)data;
	WorkPackage *work;
	BLI_thread_local_set(g_thread_device, device);
	while ((work = (WorkPackage *)BLI_thread_queue_pop(g_cpuqueue))) {
		HIGHLIGHT(work);
		device->execute(work);
		delete work;
	}
	
	return NULL;
}
コード例 #6
0
ファイル: Identity.cpp プロジェクト: openhumanoids/exotica
 void Identity::initialise(std::string& postureName,
     std::vector<std::string>& joints, bool skipUnknown)
 {
   if (!poses || !posesJointNames)
   {
     throw_named("Poses have not been set!");
   }
   std::map<std::string, Eigen::VectorXd>::const_iterator pose = poses->find(
       postureName);
   if (pose != poses->end())
   {
     useRef = true;
     jointMap.resize(0);
     jointRef.resize(0);
     for (int i = 0; i < joints.size(); i++)
     {
       int idext = getJointIDexternal(joints[i]);
       if (idext >= 0)
       {
         int id = getJointID(joints[i]);
         if (id >= 0)
         {
           jointMap.push_back(id);
           jointRef.conservativeResize(jointRef.rows() + 1);
           jointRef(jointRef.rows() - 1) = pose->second(idext);
           continue;
         }
       }
       else
       {
         if (!skipUnknown)
         {
           throw_named("Requesting unknown joint '"<<joints[i]<<"'");
         }
       }
     }
     if (jointMap.size() == 0)
     {
       HIGHLIGHT("No joints have been specified!");
     }
   }
   else
   {
     throw_named("Can't find pose '"<<postureName<<"'");
   }
 }
コード例 #7
0
ファイル: OMPLsolver.cpp プロジェクト: bmagyar/exotica
  EReturn OMPLsolver::getSimplifiedPath(ompl::geometric::PathGeometric &pg,
      Eigen::MatrixXd & traj, ob::PlannerTerminationCondition &ptc)
  {
    if (smooth_->data)
    {
      HIGHLIGHT("Simplifying solution");
      int original_cnt = pg.getStateCount();
      ros::Time start = ros::Time::now();

      //ompl_simple_setup_->simplifySolution(d);
      // Lets do our own simplifier ~:)
      if (original_cnt >= 3)
      {
        og::PathSimplifierPtr psf_ = ompl_simple_setup_->getPathSimplifier();
        const ob::SpaceInformationPtr &si =
            ompl_simple_setup_->getSpaceInformation();

        bool tryMore = false;
        if (ptc == false) tryMore = psf_->reduceVertices(pg);
        if (ptc == false) psf_->collapseCloseVertices(pg);
        int times = 0;
        while (tryMore && ptc == false)
        {
          tryMore = psf_->reduceVertices(pg);
          times++;
        }
        if (si->getStateSpace()->isMetricSpace())
        {
          if (ptc == false)
            tryMore = psf_->shortcutPath(pg);
          else
            tryMore = false;
          times = 0;
          while (tryMore && ptc == false)
          {
            tryMore = psf_->shortcutPath(pg);
            times++;
          }
        }

        std::vector<ob::State*> &states = pg.getStates();
        //	Calculate number of states required
        unsigned int length = 0;
        const int n1 = states.size() - 1;
        for (int i = 0; i < n1; ++i)
          length += si->getStateSpace()->validSegmentCount(states[i],
              states[i + 1]);
//				//	Forward reducing
//				HIGHLIGHT("States before forward reducing: "<<pg.getStateCount());
//				pg.interpolate(length);
//
//				bool need_backward = true;
//				for (int i = states.size() - 1; i > 0; i--)
//				{
//					if (si->checkMotion(states[0], states[i]))
//					{
//						ob::State *start = si->cloneState(states[0]);
//						pg.keepAfter(states[i]);
//						pg.prepend(start);
//						need_backward = (i == states.size() - 1) ? false : true;
//						break;
//					}
//				}
//
//				//	Backward reducing
//				ob::State *mid;
//				if (need_backward)
//				{
//					HIGHLIGHT("States before backward reducing: "<<pg.getStateCount());
//					pg.interpolate(length);
//					for (int i = 1; i < states.size(); i++)
//					{
//						if (si->checkMotion(states[states.size() - 1], states[i]))
//						{
//							ob::State *goal = si->cloneState(states[states.size() - 1]);
//							pg.keepBefore(states[i]);
//							pg.append(goal);
//							mid = si->cloneState(states[i]);
//							break;
//						}
//					}
//				}

        pg.interpolate(length);
      }

//			if (ompl_simple_setup_->haveSolutionPath())
//			{
//				pg.interpolate();
//			}
      HIGHLIGHT_NAMED("OMPLSolver",
          "Simplification took "<<ros::Duration(ros::Time::now()-start).toSec()<<"sec. States: "<<original_cnt<<"->"<<pg.getStateCount());
    }
    convertPath(pg, traj);

    return SUCCESS;
  }