[Personalrobots-commit] SF.net SVN: personalrobots:[23832] pkg/trunk/motion_planning/ planning_models

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

Revision: 23832
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23832&view=rev
Author:   isucan
Date:     2009-09-04 16:16:14 +0000 (Fri, 04 Sep 2009)

Log Message:
-----------
keeping track of links to be updated

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_models/CMakeLists.txt
    pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
    pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp

Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt	2009-09-04 15:42:33 UTC (rev 23831)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt	2009-09-04 16:16:14 UTC (rev 23832)
@@ -3,7 +3,7 @@
 rospack(planning_models)
 rospack_add_boost_directories()
 
-set(ROS_BUILD_TYPE Debug)
+set(ROS_BUILD_TYPE Release)
 
 rospack_add_library(planning_models src/kinematic_model.cpp
                                      src/kinematic_state.cpp)

Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h	2009-09-04 15:42:33 UTC (rev 23831)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h	2009-09-04 16:16:14 UTC (rev 23832)
@@ -297,6 +297,9 @@
 	    /** \brief The list of joints that are roots in this group */
 	    std::vector<Joint*>                 jointRoots;
 
+	    /** \brief The list of links that are updated when computeTransforms() is called, in the order they are updated */
+	    std::vector<Link*>                  updatedLinks;
+	    
 	    /** \brief Perform forward kinematics starting at the roots
 		within a group. Links that are not in the group are also
 		updated, but transforms for joints that are not in the
@@ -434,6 +437,9 @@
 	/** \brief The index at which a joint starts reading values in the state vector */
 	std::vector<unsigned int>                         jointIndex_;
 	
+	/** \brief The list of links that are updated when computeTransforms() is called, in the order they are updated */
+	std::vector<Link*>                                updatedLinks_;	
+	
 	/** \brief The root joint */
 	Joint                                            *root_;
 	
@@ -453,7 +459,9 @@
 	btTransform                                       rootTransform_;
 	
 	boost::mutex                                      lock_;
-	
+
+
+	void buildConvenientDatastructures(void);	
 	void buildGroups(const std::map< std::string, std::vector<std::string> > &groups);
 	Joint* buildRecursive(Link *parent, const urdf::Link *link);
 	Joint* constructJoint(const urdf::Joint *urdfJoint, std::vector<double> &bounds);

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp	2009-09-04 15:42:33 UTC (rev 23831)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp	2009-09-04 16:16:14 UTC (rev 23832)
@@ -58,6 +58,7 @@
 	for (unsigned int i = 0 ; i < groups.size() ; ++i)
 	    groupContent[groups[i]->name] = groups[i]->jointNames;
 	buildGroups(groupContent);
+	buildConvenientDatastructures();
     }
     else
 	root_ = NULL;
@@ -95,6 +96,7 @@
 	{
 	    root_ = buildRecursive(NULL, root);
 	    buildGroups(groups);
+	    buildConvenientDatastructures();
 	}
     }
     else
@@ -173,6 +175,23 @@
     computeTransforms(params);
 }
 
+void planning_models::KinematicModel::buildConvenientDatastructures(void)
+{
+    if (root_)
+    {
+	std::queue<Link*> links;
+	links.push(root_->after);
+	while (!links.empty())
+	{
+	    Link *link = links.front();
+	    links.pop();
+	    updatedLinks_.push_back(link);
+	    for (unsigned int i = 0 ; i < link->after.size() ; ++i)
+		links.push(link->after[i]->after);
+	}
+    }
+}
+
 void planning_models::KinematicModel::buildGroups(const std::map< std::string, std::vector<std::string> > &groups)
 {
     for (std::map< std::string, std::vector<std::string> >::const_iterator it = groups.begin() ; it != groups.end() ; ++it)
@@ -387,23 +406,13 @@
 
 void planning_models::KinematicModel::computeTransforms(const double *params)
 {
-    for (unsigned int i = 0  ; i < jointList_.size() ; ++i)
+    const unsigned int js = jointList_.size();
+    for (unsigned int i = 0  ; i < js ; ++i)
 	jointList_[i]->updateVariableTransform(params + jointIndex_[i]);
     
-    if (root_)
-    {
-	std::queue<Link*> links;
-	links.push(root_->after);
-	while (!links.empty())
-	{
-	    Link *link = links.front();
-	    links.pop();
-	    
-	    link->computeTransform();
-	    for (unsigned int i = 0 ; i < link->after.size() ; ++i)
-		links.push(link->after[i]->after);
-	}
-    }
+    const unsigned int ls = updatedLinks_.size();
+    for (unsigned int i = 0 ; i < ls ; ++i)
+	updatedLinks_[i]->computeTransform();
 }
 
 const planning_models::KinematicModel::Joint* planning_models::KinematicModel::getRoot(void) const
@@ -862,6 +871,21 @@
 	if (!found)
 	    jointRoots.push_back(joints[i]);
     }
+
+    for (unsigned int i = 0 ; i < jointRoots.size() ; ++i)
+    {
+	std::queue<Link*> links;
+	links.push(jointRoots[i]->after);
+	
+	while (!links.empty())
+	{
+	    Link *link = links.front();
+	    links.pop();
+	    updatedLinks.push_back(link);
+	    for (unsigned int i = 0 ; i < link->after.size() ; ++i)
+		links.push(link->after[i]->after);
+	}
+    }
 }
 
 planning_models::KinematicModel::JointGroup::~JointGroup(void)
@@ -887,22 +911,11 @@
 
 void planning_models::KinematicModel::JointGroup::computeTransforms(const double *params)
 {
-    for (unsigned int i = 0  ; i < joints.size() ; ++i)
+    const unsigned int js = joints.size();
+    for (unsigned int i = 0  ; i < js ; ++i)
 	joints[i]->updateVariableTransform(params + jointIndex[i]);
 
-    for (unsigned int i = 0 ; i < jointRoots.size() ; ++i)
-    {
-	std::queue<Link*> links;
-	links.push(jointRoots[i]->after);
-	
-	while (!links.empty())
-	{
-	    Link *link = links.front();
-	    links.pop();
-	    
-	    link->computeTransform();
-	    for (unsigned int i = 0 ; i < link->after.size() ; ++i)
-		links.push(link->after[i]->after);
-	}
-    }
+    const unsigned int ls = updatedLinks.size();
+    for (unsigned int i = 0 ; i < ls ; ++i)
+	updatedLinks[i]->computeTransform();
 }


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