[Personalrobots-commit] SF.net SVN: personalrobots:[18531] pkg/trunk/motion_planning/ planning_environment

[<is...@us...>]

Revision: 18531
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18531&view=rev
Author:   isucan
Date:     2009-07-09 01:26:32 +0000 (Thu, 09 Jul 2009)

Log Message:
-----------
instantiating a Bullet collision model as well

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
    pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h	2009-07-09 01:25:49 UTC (rev 18530)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h	2009-07-09 01:26:32 UTC (rev 18531)
@@ -38,7 +38,7 @@
 #define PLANNING_ENVIRONMENT_COLLISION_MODELS_
 
 #include "planning_environment/robot_models.h"
-#include <collision_space/environmentODE.h>
+#include <collision_space/environment.h>
 
 namespace planning_environment
 {
@@ -68,6 +68,7 @@
 	{
 	    RobotModels::reload();
 	    ode_collision_model_.reset();
+	    bullet_collision_model_.reset();
 	    loadCollision(collision_check_links_);
 	}
 	
@@ -77,11 +78,19 @@
 	    return ode_collision_model_;
 	}
 
+	/** \brief Return the instance of the constructed Bullet collision model */
+	const boost::shared_ptr<collision_space::EnvironmentModel> &getBulletCollisionModel(void) const
+	{
+	    return bullet_collision_model_;
+	}
+
     protected:
 	
 	void loadCollision(const std::vector<std::string> &links);
+	void setupModel(boost::shared_ptr<collision_space::EnvironmentModel> &model, const std::vector<std::string> &links);
 	
 	boost::shared_ptr<collision_space::EnvironmentModel> ode_collision_model_;
+	boost::shared_ptr<collision_space::EnvironmentModel> bullet_collision_model_;
 
 	double                                               scale_;
 	double                                               padd_;

Modified: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp	2009-07-09 01:25:49 UTC (rev 18530)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp	2009-07-09 01:26:32 UTC (rev 18531)
@@ -35,26 +35,36 @@
 /** \author Ioan Sucan */
 
 #include "planning_environment/collision_models.h"
+#include <collision_space/environmentODE.h>
+#include <collision_space/environmentBullet.h>
 
+void planning_environment::CollisionModels::setupModel(boost::shared_ptr<collision_space::EnvironmentModel> &model, const std::vector<std::string> &links)
+{
+    model->lock();
+    model->addRobotModel(kmodel_, links, scale_, padd_);
+    
+    // form all pairs of links that can collide and add them as self-collision groups
+    for (unsigned int i = 0 ; i < self_collision_check_groups_.size() ; ++i)
+	for (unsigned int g1 = 0 ; g1 < self_collision_check_groups_[i].first.size() ; ++g1)
+	    for (unsigned int g2 = 0 ; g2 < self_collision_check_groups_[i].second.size() ; ++g2)
+	    {
+		std::vector<std::string> scg;
+		scg.push_back(self_collision_check_groups_[i].first[g1]);
+		scg.push_back(self_collision_check_groups_[i].second[g2]);
+		model->addSelfCollisionGroup(scg);
+	    }
+    model->updateRobotModel();
+    model->unlock();    
+}
+
 void planning_environment::CollisionModels::loadCollision(const std::vector<std::string> &links)
 {
     if (loadedModels())
     {
 	ode_collision_model_ = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelODE());
-	ode_collision_model_->lock();
-	ode_collision_model_->addRobotModel(kmodel_, links, scale_, padd_);
-
-	// form all pairs of links that can collide and add them as self-collision groups
-	for (unsigned int i = 0 ; i < self_collision_check_groups_.size() ; ++i)
-	    for (unsigned int g1 = 0 ; g1 < self_collision_check_groups_[i].first.size() ; ++g1)
-		for (unsigned int g2 = 0 ; g2 < self_collision_check_groups_[i].second.size() ; ++g2)
-		{
-		    std::vector<std::string> scg;
-		    scg.push_back(self_collision_check_groups_[i].first[g1]);
-		    scg.push_back(self_collision_check_groups_[i].second[g2]);
-		    ode_collision_model_->addSelfCollisionGroup(scg);
-		}
-	ode_collision_model_->updateRobotModel();
-	ode_collision_model_->unlock();
+	setupModel(ode_collision_model_, links);
+	
+	bullet_collision_model_ = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelBullet());
+	setupModel(bullet_collision_model_, links);
     }
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.