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

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

Revision: 3520
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3520&view=rev
Author:   isucan
Date:     2008-08-23 00:07:36 +0000 (Sat, 23 Aug 2008)

Log Message:
-----------
reorganized kinematic planning node. we now take into account requested planners and distance metrics

Modified Paths:
--------------
    pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
    pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
    pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
    pkg/trunk/robot_srvs/srv/KinematicPlanState.srv

Added Paths:
-----------
    pkg/trunk/motion_planning/kinematic_planning/include/KinematicPlanningXMLModel.h
    pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg

Removed Paths:
-------------
    pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h

Added: pkg/trunk/motion_planning/kinematic_planning/include/KinematicPlanningXMLModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/KinematicPlanningXMLModel.h	                        (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/KinematicPlanningXMLModel.h	2008-08-23 00:07:36 UTC (rev 3520)
@@ -0,0 +1,194 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \Author Ioan Sucan */
+
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include <collision_space/environment.h>
+#include <planning_models/kinematic.h>
+
+#include <vector>
+#include <string>
+#include <map>
+
+struct KinematicPlannerSetup
+{
+    ompl::Planner_t                                                         mp;
+    ompl::SpaceInformationKinematic_t                                       si;
+    ompl::SpaceInformation::StateValidityChecker_t                          svc;
+    std::map<std::string, ompl::SpaceInformation::StateDistanceEvaluator_t> sde;    
+    ompl::PathSmootherKinematic_t                                           smoother;
+};
+
+struct XMLModel
+{
+    XMLModel(void)
+    {
+	groupID          = -1;
+	collisionSpaceID = 0;
+	collisionSpace   = NULL;	    
+    }
+    
+    ~XMLModel(void)
+    {
+	for (std::map<std::string, KinematicPlannerSetup>::iterator i = planners.begin(); i != planners.end() ; ++i)
+	{
+	    delete i->second.mp;
+	    delete i->second.svc;
+	    for (std::map<std::string, ompl::SpaceInformation::StateDistanceEvaluator_t>::iterator j = i->second.sde.begin(); j != i->second.sde.end() ; ++j)
+		delete j->second;
+	    delete i->second.smoother;
+	    delete i->second.si;
+	}
+    }
+    
+    std::map<std::string, KinematicPlannerSetup>  planners;
+    collision_space::EnvironmentModel            *collisionSpace;
+    unsigned int                                  collisionSpaceID;
+    planning_models::KinematicModel              *kmodel;
+    std::string                                   groupName;
+    int                                           groupID;
+};
+
+
+/** This class configures an instance of SpaceInformationKinematic with data from a KinematicModel */
+class SpaceInformationXMLModel : public ompl::SpaceInformationKinematic
+{
+ public:
+    SpaceInformationXMLModel(planning_models::KinematicModel *kmodel, int groupID = -1, double divisions = 20.0) : SpaceInformationKinematic()
+    {	
+	m_kmodel = kmodel;
+	m_groupID = groupID;	    
+	m_divisions = divisions;
+	
+	/* compute the state space for this group */
+	m_stateDimension = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID].size() : m_kmodel->stateDimension;
+	m_stateComponent.resize(m_stateDimension);
+	
+	for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+	{	
+	    if (m_stateComponent[i].type == StateComponent::UNKNOWN)
+		m_stateComponent[i].type = StateComponent::NORMAL;
+	    int p = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID][i] * 2 : i * 2;
+	    m_stateComponent[i].minValue   = m_kmodel->stateBounds[p    ];
+	    m_stateComponent[i].maxValue   = m_kmodel->stateBounds[p + 1];
+	    m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
+	    
+	    for (unsigned int j = 0 ; j < m_kmodel->floatingJoints.size() ; ++j)
+		if (m_kmodel->floatingJoints[j] == p)
+		{
+		    m_floatingJoints.push_back(i);
+		    m_stateComponent[i + 3].type = StateComponent::QUATERNION;
+		    break;
+		}
+	    
+	    for (unsigned int j = 0 ; j < m_kmodel->planarJoints.size() ; ++j)
+		if (m_kmodel->planarJoints[j] == p)
+		{
+		    m_planarJoints.push_back(i);
+		    break;		    
+		}
+	}
+	updateResolution();
+    }
+        
+    virtual ~SpaceInformationXMLModel(void)
+    {
+    }
+    
+    /** For planar and floating joints, we have infinite
+       dimensions. The bounds for these dimensions are set by the
+       user. */
+    void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
+    {
+	for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
+	{
+	    int id = m_floatingJoints[i];		
+	    m_stateComponent[id    ].minValue = x0;
+	    m_stateComponent[id    ].maxValue = x1;
+	    m_stateComponent[id + 1].minValue = y0;
+	    m_stateComponent[id + 1].maxValue = y1;
+	    m_stateComponent[id + 2].minValue = z0;
+	    m_stateComponent[id + 2].maxValue = z1;
+	    for (int j = 0 ; j < 3 ; ++j)
+		m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+	}
+	updateResolution();
+    }
+    
+    void setPlanningArea(double x0, double y0, double x1, double y1)
+    {
+	for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
+	{
+	    int id = m_planarJoints[i];		
+	    m_stateComponent[id    ].minValue = x0;
+	    m_stateComponent[id    ].maxValue = x1;
+	    m_stateComponent[id + 1].minValue = y0;
+	    m_stateComponent[id + 1].maxValue = y1;
+	    for (int j = 0 ; j < 2 ; ++j)
+		m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+	}
+	updateResolution();
+    }
+    
+ protected:
+    
+    void updateResolution(void)
+    {
+	/* for movement in plane/space, we want to make sure the resolution is small enough */
+	for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
+	{
+	    if (m_stateComponent[m_planarJoints[i]].resolution > 0.1)
+		m_stateComponent[m_planarJoints[i]].resolution = 0.1;
+	    if (m_stateComponent[m_planarJoints[i] + 1].resolution > 0.1)
+		m_stateComponent[m_planarJoints[i] + 1].resolution = 0.1;
+	}
+	for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
+	{
+	    if (m_stateComponent[m_floatingJoints[i]].resolution > 0.1)
+		m_stateComponent[m_floatingJoints[i]].resolution = 0.1;
+	    if (m_stateComponent[m_floatingJoints[i] + 1].resolution > 0.1)
+		m_stateComponent[m_floatingJoints[i] + 1].resolution = 0.1;
+	    if (m_stateComponent[m_floatingJoints[i] + 2].resolution > 0.1)
+		m_stateComponent[m_floatingJoints[i] + 2].resolution = 0.1;
+	}
+    }
+    
+    double                           m_divisions;
+    planning_models::KinematicModel *m_kmodel;
+    int                              m_groupID;
+    std::vector<int>                 m_floatingJoints;
+    std::vector<int>                 m_planarJoints;
+    
+};    

Deleted: pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h	2008-08-22 23:54:37 UTC (rev 3519)
+++ pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h	2008-08-23 00:07:36 UTC (rev 3520)
@@ -1,146 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2008, Willow Garage, Inc.
-*  All rights reserved.
-* 
-*  Redistribution and use in source and binary forms, with or without
-*  modification, are permitted provided that the following conditions
-*  are met:
-* 
-*   * Redistributions of source code must retain the above copyright
-*     notice, this list of conditions and the following disclaimer.
-*   * Redistributions in binary form must reproduce the above
-*     copyright notice, this list of conditions and the following
-*     disclaimer in the documentation and/or other materials provided
-*     with the distribution.
-*   * Neither the name of the Willow Garage nor the names of its
-*     contributors may be used to endorse or promote products derived
-*     from this software without specific prior written permission.
-* 
-*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-*  POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-/** \Author Ioan Sucan */
-
-#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
-#include <planning_models/kinematic.h>
-
-/** This class configures an instance of SpaceInformationKinematic with data from a KinematicModel */
-class SpaceInformationXMLModel : public ompl::SpaceInformationKinematic
-{
- public:
-    SpaceInformationXMLModel(planning_models::KinematicModel *kmodel, int groupID = -1, double divisions = 20.0) : SpaceInformationKinematic()
-    {	
-	m_kmodel = kmodel;
-	m_groupID = groupID;	    
-	m_divisions = divisions;
-	
-	m_stateDimension = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID].size() : m_kmodel->stateDimension;
-	m_stateComponent.resize(m_stateDimension);
-	
-	for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
-	{	
-	    if (m_stateComponent[i].type == StateComponent::UNKNOWN)
-		m_stateComponent[i].type = StateComponent::NORMAL;
-	    int p = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID][i] * 2 : i * 2;
-	    m_stateComponent[i].minValue   = m_kmodel->stateBounds[p    ];
-	    m_stateComponent[i].maxValue   = m_kmodel->stateBounds[p + 1];
-	    m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
-	    
-	    for (unsigned int j = 0 ; j < m_kmodel->floatingJoints.size() ; ++j)
-		if (m_kmodel->floatingJoints[j] == p)
-		{
-		    m_floatingJoints.push_back(i);
-		    m_stateComponent[i + 3].type = StateComponent::QUATERNION;
-		    break;
-		}
-	    
-	    for (unsigned int j = 0 ; j < m_kmodel->planarJoints.size() ; ++j)
-		if (m_kmodel->planarJoints[j] == p)
-		{
-		    m_planarJoints.push_back(i);
-		    break;		    
-		}
-	}
-	updateResolution();
-    }
-        
-    virtual ~SpaceInformationXMLModel(void)
-    {
-    }
-    
-    
-    void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
-    {
-	for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
-	{
-	    int id = m_floatingJoints[i];		
-	    m_stateComponent[id    ].minValue = x0;
-	    m_stateComponent[id    ].maxValue = x1;
-	    m_stateComponent[id + 1].minValue = y0;
-	    m_stateComponent[id + 1].maxValue = y1;
-	    m_stateComponent[id + 2].minValue = z0;
-	    m_stateComponent[id + 2].maxValue = z1;
-	    for (int j = 0 ; j < 3 ; ++j)
-		m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
-	}
-	updateResolution();
-    }
-    
-    void setPlanningArea(double x0, double y0, double x1, double y1)
-    {
-	for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
-	{
-	    int id = m_planarJoints[i];		
-	    m_stateComponent[id    ].minValue = x0;
-	    m_stateComponent[id    ].maxValue = x1;
-	    m_stateComponent[id + 1].minValue = y0;
-	    m_stateComponent[id + 1].maxValue = y1;
-	    for (int j = 0 ; j < 2 ; ++j)
-		m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
-	}
-	updateResolution();
-    }
-    
- protected:
-    
-    void updateResolution(void)
-    {
-	/* for movement in plane/space, we want to make sure the resolution is small enough */
-	for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
-	{
-	    if (m_stateComponent[m_planarJoints[i]].resolution > 0.1)
-		m_stateComponent[m_planarJoints[i]].resolution = 0.1;
-	    if (m_stateComponent[m_planarJoints[i] + 1].resolution > 0.1)
-		m_stateComponent[m_planarJoints[i] + 1].resolution = 0.1;
-	}
-	for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
-	{
-	    if (m_stateComponent[m_floatingJoints[i]].resolution > 0.1)
-		m_stateComponent[m_floatingJoints[i]].resolution = 0.1;
-	    if (m_stateComponent[m_floatingJoints[i] + 1].resolution > 0.1)
-		m_stateComponent[m_floatingJoints[i] + 1].resolution = 0.1;
-	    if (m_stateComponent[m_floatingJoints[i] + 2].resolution > 0.1)
-		m_stateComponent[m_floatingJoints[i] + 2].resolution = 0.1;
-	}
-    }
-    
-    double                           m_divisions;
-    planning_models::KinematicModel *m_kmodel;
-    int                              m_groupID;
-    std::vector<int>                 m_floatingJoints;
-    std::vector<int>                 m_planarJoints;
-    
-};    

Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-08-22 23:54:37 UTC (rev 3519)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-08-23 00:07:36 UTC (rev 3520)
@@ -95,7 +95,7 @@
 #include <ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h>
 #include <ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h>
 
-#include "SpaceInformationXMLModel.h"
+#include "KinematicPlanningXMLModel.h"
 
 #include <profiling_utils/profiler.h>
 #include <string_utils/string_utils.h>
@@ -113,105 +113,151 @@
 
     KinematicPlanning(const std::string &robot_model) : ros::node("kinematic_planning"),
 							planning_node_util::NodeCollisionModel(dynamic_cast<ros::node*>(this),
-											       robot_model,
-											       new collision_space::EnvironmentModelODE())
+											       robot_model)
     {
-	advertise_service("plan_kinematic_path_state", &KinematicPlanning::plan);
+	advertise_service("plan_kinematic_path_state", &KinematicPlanning::planToState);
     }
     
-    ~KinematicPlanning(void)
+    virtual ~KinematicPlanning(void)
     {
-	for (std::map<std::string, Model*>::iterator i = m_models.begin() ; i != m_models.end() ; i++)
+	for (std::map<std::string, XMLModel*>::iterator i = m_models.begin() ; i != m_models.end() ; i++)
 	    delete i->second;
-    }    
+    }
     
-    bool plan(robot_srvs::KinematicPlanState::request &req, robot_srvs::KinematicPlanState::response &res)
+    bool areSpaceParamsValid(robot_msgs::KinematicSpaceParameters &params)
     {
-	if (m_models.find(req.model_id) == m_models.end())
+	if (m_models.find(params.model_id) == m_models.end())
 	{
-	    std::cerr << "Cannot plan for '" << req.model_id << "'. Model does not exist" << std::endl;
+	    std::cerr << "Cannot plan for '" << params.model_id << "'. Model does not exist" << std::endl;
 	    return false;	    
 	}
+
+	/* find the model */
+	XMLModel *m = m_models[params.model_id];
 	
-	Model   *m = m_models[req.model_id];
-	Planner &p = m->planners[0];
+	/* if the user did not specify a planner, use the first available one */
+	if (params.planner_id.empty())
+	{
+	    if (!m->planners.empty())
+		params.planner_id = m->planners.begin()->first;
+	}
 	
-	if (m->kmodel->stateDimension != req.start_state.get_vals_size())
+	/* check if desired planner exists */
+	std::map<std::string, KinematicPlannerSetup>::iterator plannerIt = m->planners.find(params.planner_id);
+	if (plannerIt == m->planners.end())
 	{
-	    std::cerr << "Dimension of start state expected to be " << m->kmodel->stateDimension << " but was received as " << req.start_state.get_vals_size() << std::endl;
-	    return false;	    
+	    std::cerr << "Motion planner not found: '" << params.planner_id << "'" << std::endl;
+	    return false;
 	}
 	
-	const int dim = req.goal_state.get_vals_size();
-	if ((int)p.si->getStateDimension() != dim)
+	KinematicPlannerSetup &psetup = plannerIt->second;
+
+	/* check if the desired distance metric is defined */
+	if (psetup.sde.find(params.distance_metric) == psetup.sde.end())
 	{
-	    std::cerr << "Dimension of start goal expected to be " << p.si->getStateDimension() << " but was received as " << dim << std::endl;
+	    std::cerr << "Distance evaluator not found: '" << params.distance_metric << "'" << std::endl;
 	    return false;
 	}
+	return true;
+    }
+    
+    bool isRequestValid(robot_srvs::KinematicPlanState::request &req)
+    {
+	if (!areSpaceParamsValid(req.params))
+	    return false;
+
+	XMLModel *m = m_models[req.params.model_id];
+	KinematicPlannerSetup &psetup = m->planners[req.params.planner_id];
 	
-	if (req.times <= 0)
+	if (m->kmodel->stateDimension != req.start_state.get_vals_size())
 	{
-	    std::cerr << "Request specifies motion plan should be computed " << req.times << " times" << std::endl;
+	    std::cerr << "Dimension of start state expected to be " << m->kmodel->stateDimension << " but was received as " << req.start_state.get_vals_size() << std::endl;
 	    return false;
 	}
 	
-	
+	if (psetup.si->getStateDimension() != req.goal_state.get_vals_size())
+	{
+	    std::cerr << "Dimension of start goal expected to be " << psetup.si->getStateDimension() << " but was received as " <<  req.goal_state.get_vals_size() << std::endl;
+	    return false;
+	}
+
+	return true;
+    }
+
+    void setupStateSpaceAndStartState(XMLModel *m, KinematicPlannerSetup &p,
+				      robot_msgs::KinematicSpaceParameters &params,
+				      robot_msgs::KinematicState &start_state)
+    {
 	/* set the workspace volume for planning */
 	// only area or volume should go through... not sure what happens on really complicated models where 
 	// we have both multiple planar and multiple floating joints...
-	static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningArea(req.volumeMin.x, req.volumeMin.y,
-								      req.volumeMax.x, req.volumeMax.y);
-        static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningVolume(req.volumeMin.x, req.volumeMin.y, req.volumeMin.z,
-									req.volumeMax.x, req.volumeMax.y, req.volumeMax.z);
-    
+	static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningArea(params.volumeMin.x, params.volumeMin.y,
+								      params.volumeMax.x, params.volumeMax.y);
+        static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningVolume(params.volumeMin.x, params.volumeMin.y, params.volumeMin.z,
+									params.volumeMax.x, params.volumeMax.y, params.volumeMax.z);
+
+	p.si->setStateDistanceEvaluator(p.sde[params.distance_metric]);
+	
 	/* set the starting state */
-        ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
-    
+	const unsigned int dim = p.si->getStateDimension();
+	ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
+	
 	if (m->groupID >= 0)
 	{
 	    /* set the pose of the whole robot */
-	    m->kmodel->computeTransforms(req.start_state.vals);
+	    m->kmodel->computeTransforms(start_state.vals);
 	    m->collisionSpace->updateRobotModel(m->collisionSpaceID);
 	    
 	    /* extract the components needed for the start state of the desired group */
-	    for (int i = 0 ; i < dim ; ++i)
-		start->values[i] = req.start_state.vals[m->kmodel->groupStateIndexList[m->groupID][i]];
+	    for (unsigned int i = 0 ; i < dim ; ++i)
+		start->values[i] = start_state.vals[m->kmodel->groupStateIndexList[m->groupID][i]];
 	}
 	else
 	{
 	    /* the start state is the complete state */
-	    for (int i = 0 ; i < dim ; ++i)
-		start->values[i] = req.start_state.vals[i];
+	    for (unsigned int i = 0 ; i < dim ; ++i)
+		start->values[i] = start_state.vals[i];
 	}
 	
 	p.si->addStartState(start);
-	
+    }
+    
+    void setupGoalState(KinematicPlannerSetup &p, robot_srvs::KinematicPlanState::request &req)
+    {
 	/* set the goal */
 	ompl::SpaceInformationKinematic::GoalStateKinematic_t goal = new ompl::SpaceInformationKinematic::GoalStateKinematic(p.si);
+	const unsigned int dim = p.si->getStateDimension();
 	goal->state = new ompl::SpaceInformationKinematic::StateKinematic(dim);
-	for (int i = 0 ; i < dim ; ++i)
+	for (unsigned int i = 0 ; i < dim ; ++i)
 	    goal->state->values[i] = req.goal_state.vals[i];
 	goal->threshold = req.threshold;
 	p.si->setGoal(goal);
+    }    
+    
+    void computePlan(KinematicPlannerSetup &p, int times, double allowed_time, bool interpolate,
+		     ompl::SpaceInformationKinematic::PathKinematic_t &bestPath, double &bestDifference)
+    {
+		
+	if (times <= 0)
+	{
+	    std::cerr << "Request specifies motion plan cannot be computed " << times << " times" << std::endl;
+	    return;
+	}
 
-	printf("=======================================\n");
-	p.si->printSettings();
-	printf("=======================================\n");	
+	/* do the planning */
+	bestPath = NULL;
+	bestDifference = 0.0;
+        double totalTime = 0.0;
+	ompl::SpaceInformation::Goal_t goal = p.si->getGoal();
 	
-	
-	/* do the planning */
-        ompl::SpaceInformationKinematic::PathKinematic_t bestPath = NULL;
-        double                                           bestDifference = 0.0;
-        double                                           totalTime = 0.0;
-    
 	m_collisionSpace->lock();
 	
 	profiling_utils::Profiler::Start();
 
-	for (int i = 0 ; i < req.times ; ++i)
+	for (int i = 0 ; i < times ; ++i)
 	{
 	    ros::Time startTime = ros::Time::now();
-	    bool ok = p.mp->solve(req.allowed_time); 
+	    bool ok = p.mp->solve(allowed_time); 
 	    double tsolve = (ros::Time::now() - startTime).to_double();	
 	    std::cout << (ok ? "[Success]" : "[Failure]") << " Motion planner spent " << tsolve << " seconds" << std::endl;
 	    totalTime += tsolve;
@@ -224,7 +270,7 @@
 		p.smoother->smoothVertices(path);
 		double tsmooth = (ros::Time::now() - startTime).to_double();
 		std::cout << "          Smoother spent " << tsmooth << " seconds (" << (tsmooth + tsolve) << " seconds in total)" << std::endl;
-		if (req.interpolate)
+		if (interpolate)
 		    p.si->interpolatePath(path);
 		if (bestPath == NULL || bestDifference > goal->getDifference() || 
 		    (bestPath && bestDifference == goal->getDifference() && bestPath->states.size() > path->states.size()))
@@ -241,34 +287,74 @@
 	}
 	
 	profiling_utils::Profiler::Stop();
+	m_collisionSpace->unlock();
 	
-	m_collisionSpace->unlock();
+        std::cout << std::endl << "Total planning time: " << totalTime << "; Average planning time: " << (totalTime / (double)times) << " (seconds)" << std::endl;
+    }
 
-        std::cout << std::endl << "Total planning time: " << totalTime << "; Average planning time: " << (totalTime / (double)req.times) << " (seconds)" << std::endl;
-
+    void fillSolution(KinematicPlannerSetup &p, ompl::SpaceInformationKinematic::PathKinematic_t bestPath, double bestDifference,
+		      robot_msgs::KinematicPath &path, double &distance)
+    {
+	const unsigned int dim = p.si->getStateDimension();
+	
 	/* copy the solution to the result */
 	if (bestPath)
 	{
-	    res.path.set_states_size(bestPath->states.size());
+	    path.set_states_size(bestPath->states.size());
 	    for (unsigned int i = 0 ; i < bestPath->states.size() ; ++i)
 	    {
-		res.path.states[i].set_vals_size(dim);
-		for (int j = 0 ; j < dim ; ++j)
-		    res.path.states[i].vals[j] = bestPath->states[i]->values[j];
+		path.states[i].set_vals_size(dim);
+		for (unsigned int j = 0 ; j < dim ; ++j)
+		    path.states[i].vals[j] = bestPath->states[i]->values[j];
 	    }
-	    res.distance = bestDifference;
+	    distance = bestDifference;
 	    delete bestPath;
 	}
 	else
 	{
-	    res.path.set_states_size(0);
-	    res.distance = -1.0;
+	    path.set_states_size(0);
+	    distance = -1.0;
 	}
-	
+    }
+    
+    void cleanupPlanningData(KinematicPlannerSetup &p)
+    {
 	/* cleanup */
 	p.si->clearGoal();
-	p.si->clearStartStates();
+	p.si->clearStartStates();	
+    }
+    
+    bool planToState(robot_srvs::KinematicPlanState::request &req, robot_srvs::KinematicPlanState::response &res)
+    {
+	if (!isRequestValid(req))
+	    return false;
 	
+	/* find the data we need */
+	XMLModel                   *m = m_models[req.params.model_id];
+	KinematicPlannerSetup &psetup = m->planners[req.params.planner_id];
+	
+	/* configure state space and starting state */
+	setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
+
+	/* add goal state */
+	setupGoalState(psetup, req);
+	
+	/* print some information */
+	printf("=======================================\n");
+	psetup.si->printSettings();
+	printf("=======================================\n");	
+	
+	/* compute actual motion plan */
+	ompl::SpaceInformationKinematic::PathKinematic_t bestPath       = NULL;
+	double                                           bestDifference = 0.0;	
+	computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
+	
+	/* fill in the results */
+	fillSolution(psetup, bestPath, bestDifference, res.path, res.distance);
+	
+	/* clear memory */
+	cleanupPlanningData(psetup);
+	
 	return true;
     }
 
@@ -278,7 +364,7 @@
 	defaultPosition();
 	
 	/* set the data for the model */
-	Model *model = new Model();
+	XMLModel *model = new XMLModel();
 	model->collisionSpaceID = 0;
 	model->collisionSpace = m_collisionSpace;
         model->kmodel = m_kmodel;
@@ -298,7 +384,7 @@
 
 	for (unsigned int i = 0 ; i < groups.size() ; ++i)
 	{
-	    Model *model = new Model();
+	    XMLModel *model = new XMLModel();
 	    model->collisionSpaceID = 0;
 	    model->collisionSpace = m_collisionSpace;
 	    model->kmodel = m_kmodel;
@@ -311,87 +397,58 @@
     
     void knownModels(std::vector<std::string> &model_ids)
     {
-	for (std::map<std::string, Model*>::const_iterator i = m_models.begin() ; i != m_models.end() ; ++i)
+	for (std::map<std::string, XMLModel*>::const_iterator i = m_models.begin() ; i != m_models.end() ; ++i)
 	    model_ids.push_back(i->first);
     }
     
 private:
     
-    class StateValidityPredicate;
-    
-    struct Planner
-    {
-	ompl::Planner_t                   mp;
-	ompl::SpaceInformationKinematic_t si;
-	StateValidityPredicate*           svc;
-	ompl::PathSmootherKinematic_t     smoother;
-    };
-    	
-    struct Model
-    {
-	Model(void)
-	{
-	    groupID          = -1;
-	    collisionSpaceID = 0;
-	    collisionSpace   = NULL;	    
-	}
-	
-	~Model(void)
-	{
-	    for (unsigned int i = 0 ; i < planners.size() ; ++i)
-	    {
-		delete planners[i].mp;
-		delete planners[i].svc;
-		delete planners[i].smoother;
-		delete planners[i].si;
-	    }
-	}
-	
-	std::vector<Planner>               planners;
-	collision_space::EnvironmentModel *collisionSpace;    	
-	unsigned int                       collisionSpaceID;
-	planning_models::KinematicModel   *kmodel;
-	std::string                        groupName;
-	int                                groupID;
-    };
-    
     class StateValidityPredicate : public ompl::SpaceInformation::StateValidityChecker
     {
     public:
-	StateValidityPredicate(Model *model) : ompl::SpaceInformation::StateValidityChecker()
+	StateValidityPredicate(XMLModel *model) : ompl::SpaceInformation::StateValidityChecker()
 	{
 	    m_model = model;
+	    m_kc = NULL;
 	}
 	
 	virtual bool operator()(const ompl::SpaceInformation::State_t state)
 	{
 	    m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
 	    m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
-
+	    
 	    bool collision = m_model->collisionSpace->isCollision(m_model->collisionSpaceID);
 	    return !collision;
 	}
 	
+	void setConstraints(robot_msgs::KinematicConstraint &kc)
+	{
+	    m_kc = &kc;
+	}
+	
     protected:
-	
-	Model *m_model;	
+	robot_msgs::KinematicConstraint *m_kc;
+	XMLModel                        *m_model;	
     };
-    
+
     robot_desc::URDF::Group* getURDFGroup(const std::string &gname)
     {
 	std::string urdfGroup = gname;
 	urdfGroup.erase(0, urdfGroup.find_last_of(":") + 1);
 	return m_urdf->getGroup(urdfGroup);
     }
-    
-    void createMotionPlanningInstances(Model* model)
+
+    /* instantiate the planners that can be used  */
+    void createMotionPlanningInstances(XMLModel* model)
     {
 	addRRTInstance(model);
+	addLazyRRTInstance(model);
     }
-    
-    void addRRTInstance(Model *model)
+
+    /* instantiate and configure RRT */
+    void addRRTInstance(XMLModel *model)
     {
-	Planner p;
+	KinematicPlannerSetup p;
 	std::cout << "Adding RRT instance for motion planning: " << model->groupName << std::endl;
 	
 	p.si       = new SpaceInformationXMLModel(model->kmodel, model->groupID);
@@ -425,13 +482,16 @@
 	    }
 	}	
 	
+	setupDistanceEvaluators(p);
 	p.si->setup();
-	model->planners.push_back(p);
+
+	model->planners["RRT"] = p;
     }
     
-    void addLazyRRTInstance(Model *model)
+    /* instantiate and configure LazyRRT */
+    void addLazyRRTInstance(XMLModel *model)
     {
-	Planner p;
+	KinematicPlannerSetup p;
 	std::cout << "Added LazyRRT instance for motion planning: " << model->groupName << std::endl;
 
 	p.si       = new SpaceInformationXMLModel(model->kmodel, model->groupID);
@@ -465,12 +525,19 @@
 	    }
 	}
 	
+	setupDistanceEvaluators(p);
 	p.si->setup();
-	model->planners.push_back(p);
+
+	model->planners["LazyRRT"] = p;
     }
-
-    std::map<std::string, Model*> m_models;
     
+    /* for each planner definition, define the set of distance metrics it can use */
+    void setupDistanceEvaluators(KinematicPlannerSetup &p)
+    {
+	p.sde["L2Square"] = new ompl::SpaceInformationKinematic::StateKinematicL2SquareDistanceEvaluator(p.si);
+    }
+    
+    std::map<std::string, XMLModel*> m_models;
 };
 
 void usage(const char *progname)

Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp	2008-08-22 23:54:37 UTC (rev 3519)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp	2008-08-23 00:07:36 UTC (rev 3520)
@@ -75,9 +75,9 @@
     {
 	robot_srvs::KinematicPlanState::request  req;
 	
-	req.model_id = "pr2::base";
+	req.params.model_id = "pr2::base";
+	req.params.distance_metric = "L2Square";
 	req.threshold = 0.01;
-	req.distance_metric = "L2Square";
 	req.interpolate = 1;
 	req.times = 10;
 	
@@ -90,8 +90,8 @@
 	
 	req.allowed_time = 30.0;
 	
-	req.volumeMin.x = -5.0 + m_basePos[0];	req.volumeMin.y = -5.0 + m_basePos[1];	req.volumeMin.z = 0.0;
-	req.volumeMax.x = 5.0 + m_basePos[0];	req.volumeMax.y = 5.0 + m_basePos[1];	req.volumeMax.z = 0.0;
+	req.params.volumeMin.x = -5.0 + m_basePos[0];	req.params.volumeMin.y = -5.0 + m_basePos[1];	req.params.volumeMin.z = 0.0;
+	req.params.volumeMax.x = 5.0 + m_basePos[0];	req.params.volumeMax.y = 5.0 + m_basePos[1];	req.params.volumeMax.z = 0.0;
 	
 	performCall(req);
     }
@@ -100,9 +100,9 @@
     {
 	robot_srvs::KinematicPlanState::request  req;
 	
-	req.model_id = "pr2::leftArm";
+	req.params.model_id = "pr2::leftArm";
+	req.params.distance_metric = "L2Square";
 	req.threshold = 0.01;
-	req.distance_metric = "L2Square";
 	req.interpolate = 1;
 	req.times = 1;
 
@@ -115,8 +115,8 @@
 
 	req.allowed_time = 3.0;
 	
-	req.volumeMin.x = -5.0 + m_basePos[0];	req.volumeMin.y = -5.0 + m_basePos[1];	req.volumeMin.z = 0.0;
-	req.volumeMax.x = 5.0 + m_basePos[0];	req.volumeMax.y = 5.0 + m_basePos[1];	req.volumeMax.z = 0.0;
+	req.params.volumeMin.x = -5.0 + m_basePos[0];	req.params.volumeMin.y = -5.0 + m_basePos[1];	req.params.volumeMin.z = 0.0;
+	req.params.volumeMax.x = 5.0 + m_basePos[0];	req.params.volumeMax.y = 5.0 + m_basePos[1];	req.params.volumeMax.z = 0.0;
 	
 	performCall(req);
     }
@@ -126,9 +126,9 @@
     {
 	robot_srvs::KinematicPlanState::request  req;
 	
-	req.model_id = "pr2::rightArm";
+	req.params.model_id = "pr2::rightArm";
+	req.params.distance_metric = "L2Square";
 	req.threshold = 0.01;
-	req.distance_metric = "L2Square";
 	req.interpolate = 1;
 	req.times = 1;
 
@@ -141,8 +141,8 @@
 
 	req.allowed_time = 30.0;
 	
-	req.volumeMin.x = -5.0 + m_basePos[0];	req.volumeMin.y = -5.0 + m_basePos[1];	req.volumeMin.z = 0.0;
-	req.volumeMax.x = 5.0 + m_basePos[0];	req.volumeMax.y = 5.0 + m_basePos[1];	req.volumeMax.z = 0.0;
+	req.params.volumeMin.x = -5.0 + m_basePos[0];	req.params.volumeMin.y = -5.0 + m_basePos[1];	req.params.volumeMin.z = 0.0;
+	req.params.volumeMax.x = 5.0 + m_basePos[0];	req.params.volumeMax.y = 5.0 + m_basePos[1];	req.params.volumeMax.z = 0.0;
 	
 	performCall(req);
     }
@@ -163,7 +163,7 @@
 		    printf("%f ", res.path.states[i].vals[j]);
 		printf("\n");
 	    }
-	    dpath.name = req.model_id;
+	    dpath.name = req.params.model_id;
 	    dpath.path = res.path;
 	    publish("display_kinematic_path", dpath);
 	}

Added: pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg	                        (rev 0)
+++ pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg	2008-08-23 00:07:36 UTC (rev 3520)
@@ -0,0 +1,24 @@
+# This message contains a set of parameters useful in setting up a
+# kinematic space for planning
+
+# The model (defined in pr2.xml as a group) for which the motion
+# planner should plan for
+string model_id
+
+
+# The name of the motion planner to use. If no name is specified,
+# a default motion planner will be used
+string planner_id
+
+
+# A string that identifies which distance metric the planner should use
+string distance_metric
+
+# Lower coordinate for a box defining the volume in the workspace the
+# motion planner is active in.  If planning in 2D, only first 2 values
+# (x, y) of the points are used.
+std_msgs/Point3DFloat64 volumeMin
+
+
+# Higher coordinate for the box described above
+std_msgs/Point3DFloat64 volumeMax

Modified: pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv	2008-08-22 23:54:37 UTC (rev 3519)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv	2008-08-23 00:07:36 UTC (rev 3520)
@@ -2,12 +2,10 @@
 # planner
 
 
+# Parameters for the state space 
+robot_msgs/KinematicSpaceParameters params
 
-# The model (defined in pr2.xml as a group) for which the motion
-# planner should plan for
-string model_id
 
-
 # The starting state for the robot. This is eth complete state of the
 # robot, all joint values, everything that needs to be specified to
 # completely define where each part of the robot is in space. The
@@ -37,20 +35,6 @@
 # The maximum amount of time the motion planner is allowed to plan for
 float64 allowed_time
 
-
-# A string that identifies which distance metric the planner should use
-string distance_metric
-
-
-# Lower coordinate for a box defining the volume in the workspace the
-# motion planner is active in.  If planning in 2D, only first 2 values
-# (x, y) of the points are used.
-std_msgs/Point3DFloat64 volumeMin
-
-
-# Higher coordinate for the box described above
-std_msgs/Point3DFloat64 volumeMax
-
 ---
 
 # The result of a motion plan: a kinematic path. If the motion plan is

Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv	2008-08-22 23:54:37 UTC (rev 3519)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv	2008-08-23 00:07:36 UTC (rev 3520)
@@ -1,13 +1,9 @@
 # This message contains the definition for a request to the motion
 # planner
 
+# Parameters for the state space 
+robot_msgs/KinematicSpaceParameters params
 
-
-# The model (defined in pr2.xml as a group) for which the motion
-# planner should plan for
-string model_id
-
-
 # The starting state for the robot. This is eth complete state of the
 # robot, all joint values, everything that needs to be specified to
 # completely define where each part of the robot is in space. The
@@ -39,24 +35,10 @@
 float64 allowed_time
 
 
-# A string that identifies which distance metric the planner should use
-string distance_metric
-
-
 # The threshold (distance) that is allowed between the returned
 # solution and the actual goal
 float64 threshold
 
-
-# Lower coordinate for a box defining the volume in the workspace the
-# motion planner is active in.  If planning in 2D, only first 2 values
-# (x, y) of the points are used.
-std_msgs/Point3DFloat64 volumeMin
-
-
-# Higher coordinate for the box described above
-std_msgs/Point3DFloat64 volumeMax
-
 ---
 
 # The result of a motion plan: a kinematic path. If the motion plan is


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