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

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

Revision: 19162
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19162&view=rev
Author:   isucan
Date:     2009-07-18 01:20:31 +0000 (Sat, 18 Jul 2009)

Log Message:
-----------
random and perturbed states for groups

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
    pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp

Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h	2009-07-18 01:09:23 UTC (rev 19161)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h	2009-07-18 01:20:31 UTC (rev 19162)
@@ -73,8 +73,20 @@
 	/** \brief Construct a random state */
 	void randomState(void);
 
+	/** \brief Construct a random state for a group */
+	void randomStateGroup(const std::string &group);
+
+	/** \brief Construct a random state for a group */
+	void randomStateGroup(int groupID);
+
 	/** \brief Perturb state. Each dimension is perturbed by a factor of its range */
 	void perturbState(double factor);
+
+	/** \brief Perturb state of a group. Each dimension is perturbed by a factor of its range */
+	void perturbStateGroup(double factor, const std::string &group);
+
+	/** \brief Perturb state of a group. Each dimension is perturbed by a factor of its range */
+	void perturbStateGroup(double factor, int groupID);
 	
 	/** \brief Update parameters so that they are within the specified bounds */
 	void enforceBounds(void);

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp	2009-07-18 01:09:23 UTC (rev 19161)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp	2009-07-18 01:20:31 UTC (rev 19162)
@@ -104,6 +104,23 @@
     }
 }
 
+void planning_models::StateParams::randomStateGroup(const std::string &group)
+{
+    randomStateGroup(m_owner->getGroupID(group));
+}
+
+
+void planning_models::StateParams::randomStateGroup(int groupID)
+{
+    assert(groupID >= 0 && groupID < (int)m_owner->getGroupCount());
+    for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
+    {
+	unsigned int j = m_mi.groupStateIndexList[groupID][i];
+	m_params[j] = (m_mi.stateBounds[2 * j + 1] - m_mi.stateBounds[2 * j]) * ((double)rand() / (RAND_MAX + 1.0)) +  m_mi.stateBounds[2 * j];
+	m_seen[j] = true;
+    }
+}
+    
 void planning_models::StateParams::randomState(void)
 {   
     for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
@@ -113,6 +130,23 @@
     }
 }
 
+void planning_models::StateParams::perturbStateGroup(double factor, const std::string &group)    
+{   
+    perturbStateGroup(factor, m_owner->getGroupID(group));
+}
+
+
+void planning_models::StateParams::perturbStateGroup(double factor, int groupID)    
+{   
+    assert(groupID >= 0 && groupID < (int)m_owner->getGroupCount());
+    for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
+    {
+	unsigned int j = m_mi.groupStateIndexList[groupID][i];
+	m_params[j] += factor * (m_mi.stateBounds[2 * j + 1] - m_mi.stateBounds[2 * j]) * (2.0 * ((double)rand() / (RAND_MAX + 1.0)) - 1.0);
+	m_seen[j] = true;
+    }
+}
+
 void planning_models::StateParams::perturbState(double factor)
 {   
     for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)


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