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

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

Revision: 18070
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18070&view=rev
Author:   isucan
Date:     2009-07-01 04:08:49 +0000 (Wed, 01 Jul 2009)

Log Message:
-----------
functions to allow state perturbation

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-01 03:26:49 UTC (rev 18069)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h	2009-07-01 04:08:49 UTC (rev 18070)
@@ -69,6 +69,12 @@
 	/** \brief Construct a random state */
 	void randomState(void);
 
+	/** \brief Perturb state. Each dimension is perturbed by a factor of its range */
+	void perturbState(double factor);
+	
+	/** \brief Update parameters so that they are within the specified bounds */
+	void enforceBounds(void);
+	
 	/** \brief Mark all values as unseen */
 	void reset(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-01 03:26:49 UTC (rev 18069)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp	2009-07-01 04:08:49 UTC (rev 18070)
@@ -113,6 +113,25 @@
     }
 }
 
+void planning_models::StateParams::perturbState(double factor)
+{   
+    for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+	m_params[i] += factor * (m_mi.stateBounds[2 * i + 1] - m_mi.stateBounds[2 * i]) * (2.0 * ((double)rand() / (RAND_MAX + 1.0)) - 1.0);
+    enforceBounds();
+}
+
+void planning_models::StateParams::enforceBounds(void)
+{  
+    for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+    {
+	if (m_params[i] > m_mi.stateBounds[2 * i + 1])
+	    m_params[i] = m_mi.stateBounds[2 * i + 1];
+	else
+	    if (m_params[i] < m_mi.stateBounds[2 * i])
+		m_params[i] = m_mi.stateBounds[2 * i];
+    }
+}
+
 void planning_models::StateParams::reset(void)
 {
     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.