[Personalrobots-commit] SF.net SVN: personalrobots:[17407] pkg/trunk

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

Revision: 17407
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17407&view=rev
Author:   isucan
Date:     2009-06-20 04:06:59 +0000 (Sat, 20 Jun 2009)

Log Message:
-----------
utility functions

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
    pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
    pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
    pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml

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-06-20 04:03:14 UTC (rev 17406)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h	2009-06-20 04:06:59 UTC (rev 17407)
@@ -63,6 +63,13 @@
 	
 	bool operator==(const StateParams &rhs) const;
 	
+	/** \brief Construct a default state: each value at 0.0, if
+	    within bounds. Otherwise, select middle point. */
+	void defaultState(void);
+	
+	/** \brief Construct a random state */
+	void randomState(void);
+
 	/** \brief Mark all values as unseen */
 	void reset(void);
 	

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-06-20 04:03:14 UTC (rev 17406)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-06-20 04:06:59 UTC (rev 17407)
@@ -64,15 +64,9 @@
 
 void planning_models::KinematicModel::defaultState(void)
 {
-    /* The default state of the robot. Place each value at 0.0, if
-       within bounds. Otherwise, select middle point. */
-    double params[m_mi.stateDimension];
-    for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
-	if (m_mi.stateBounds[2 * i] <= 0.0 && m_mi.stateBounds[2 * i + 1] >= 0.0)
-	    params[i] = 0.0;
-	else
-	    params[i] = (m_mi.stateBounds[2 * i] + m_mi.stateBounds[2 * i + 1]) / 2.0;
-    computeTransforms(params);
+    StateParams sp(this);
+    sp.defaultState();
+    computeTransforms(sp.getParams());
 }
 
 void planning_models::KinematicModel::computeTransforms(const double *params)

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp	2009-06-20 04:03:14 UTC (rev 17406)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp	2009-06-20 04:06:59 UTC (rev 17407)
@@ -39,6 +39,7 @@
 #include <algorithm>
 #include <sstream>
 #include <cmath>
+#include <cstdlib>
 
 planning_models::StateParams::StateParams(KinematicModel *model) : m_owner(model), m_mi(model->getModelInfo()), m_params(NULL)
 {
@@ -91,6 +92,27 @@
     m_seen = sp.m_seen;
 }
 
+void planning_models::StateParams::defaultState(void)
+{
+    for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+    {
+	if (m_mi.stateBounds[2 * i] <= 0.0 && m_mi.stateBounds[2 * i + 1] >= 0.0)
+	    m_params[i] = 0.0;
+	else
+	    m_params[i] = (m_mi.stateBounds[2 * i] + m_mi.stateBounds[2 * i + 1]) / 2.0;
+	m_seen[i] = true;
+    }
+}
+
+void planning_models::StateParams::randomState(void)
+{   
+    for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+    {
+	m_params[i] = (m_mi.stateBounds[2 * i + 1] - m_mi.stateBounds[2 * i]) * ((double)rand() / (RAND_MAX + 1.0)) +  m_mi.stateBounds[2 * i];
+	m_seen[i] = true;
+    }
+}
+
 void planning_models::StateParams::reset(void)
 {
     for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)

Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml	2009-06-20 04:03:14 UTC (rev 17406)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml	2009-06-20 04:06:59 UTC (rev 17407)
@@ -5,6 +5,7 @@
   base
   left_arm
   right_arm
+  arms
 
 ## the definition of each group
 groups:
@@ -43,6 +44,29 @@
     planner_configs:
       RRTConfig2 SBLConfig2 KPIECEConfig2r
       
+  arms:
+    links:
+      l_shoulder_pan_link
+      l_shoulder_lift_link
+      l_upper_arm_roll_link
+      l_upper_arm_link
+      l_elbow_flex_link
+      l_forearm_roll_link
+      l_forearm_link
+      l_wrist_flex_link
+      l_wrist_roll_link
+      r_shoulder_pan_link
+      r_shoulder_lift_link
+      r_upper_arm_roll_link
+      r_upper_arm_link
+      r_elbow_flex_link
+      r_forearm_roll_link
+      r_forearm_link
+      r_wrist_flex_link
+      r_wrist_roll_link
+    planner_configs:
+      RRTConfig2 SBLConfig2 
+
 ## the planner configurations; each config must have a type, which specifies
 ## the planner to be used; other parameters can be specified as well, depending 
 ## on the planner


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