[Personalrobots-commit] SF.net SVN: personalrobots:[19303] pkg/trunk/motion_planning/ planning_environment

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

Revision: 19303
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19303&view=rev
Author:   isucan
Date:     2009-07-21 17:21:18 +0000 (Tue, 21 Jul 2009)

Log Message:
-----------
finding closest states on path

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
    pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h	2009-07-21 15:07:08 UTC (rev 19302)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h	2009-07-21 17:21:18 UTC (rev 19303)
@@ -83,7 +83,16 @@
 	
 	/** \brief Check if the path is valid. Path constraints are considered, but goal constraints are not  */
 	bool isPathValid(const motion_planning_msgs::KinematicPath &path, bool verbose) const;
+
+	/** \brief Check if a segment of the path is valid. Path constraints are considered, but goal constraints are not  */
+	bool isPathValid(const motion_planning_msgs::KinematicPath &path, unsigned int start, unsigned int end, bool verbose) const;
 	
+	/** \brief Find the index of the state on the path that is closest to a given state */
+	int  closestStateOnPath(const motion_planning_msgs::KinematicPath &path, const planning_models::StateParams *state) const;
+
+	/** \brief Find the index of the state on the path segment that is closest to a given state */
+	int  closestStateOnPath(const motion_planning_msgs::KinematicPath &path, unsigned int start, unsigned int end, const planning_models::StateParams *state) const;
+	
 	/** \brief Set the kinematic constraints the monitor should use when checking a path */
 	void setPathConstraints(const motion_planning_msgs::KinematicConstraints &kc);
 
@@ -117,8 +126,11 @@
 	bool transformJoint(const std::string &name, unsigned int index, std::vector<double> &params, roslib::Header &header, const std::string& target) const;
 	
 	/** \brief Check the path assuming it is in the frame of the model */
-	bool isPathValidAux(const motion_planning_msgs::KinematicPath &path, bool verbose) const;
+	bool isPathValidAux(const motion_planning_msgs::KinematicPath &path, unsigned int start, unsigned int end, bool verbose) const;
 
+	/** \brief Find the index of the state on the path that is closest to a given state assuming the path is in the frame of the model */
+	int closestStateOnPathAux(const motion_planning_msgs::KinematicPath &path, unsigned int start, unsigned int end, const planning_models::StateParams *state) const;
+	
 	/** \brief User callback when a collision is found */
 	boost::function<void(collision_space::EnvironmentModel::Contact&)> onCollisionContact_;
 	

Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-07-21 15:07:08 UTC (rev 19302)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-07-21 17:21:18 UTC (rev 19303)
@@ -357,21 +357,102 @@
     return valid;    
 }
 
+int planning_environment::PlanningMonitor::closestStateOnPath(const motion_planning_msgs::KinematicPath &path, const planning_models::StateParams *state) const
+{
+    return closestStateOnPath(path, 0, path.states.size() - 1, state);
+}
+
+int planning_environment::PlanningMonitor::closestStateOnPath(const motion_planning_msgs::KinematicPath &path, unsigned int start, unsigned int end, const planning_models::StateParams *state) const
+{
+    if (end >= path.states.size())
+	end = path.states.size() - 1;
+    if (start > end)
+	return -1;
+    
+    if (path.header.frame_id != getFrameId())
+    {
+	motion_planning_msgs::KinematicPath pathT = path;
+	if (transformPathToFrame(pathT, getFrameId()))
+	    return closestStateOnPathAux(pathT, start, end, state);
+	else
+	    return -1;
+    }
+    else
+	return closestStateOnPathAux(path, start, end, state);  
+}
+
+int planning_environment::PlanningMonitor::closestStateOnPathAux(const motion_planning_msgs::KinematicPath &path, unsigned int start, unsigned int end, const planning_models::StateParams *state) const
+{
+    double dist = 0.0;
+    int    pos  = -1;
+
+    // get the joints this path is for
+    std::vector<planning_models::KinematicModel::Joint*> joints(path.names.size());
+    for (unsigned int j = 0 ; j < joints.size() ; ++j)
+    {
+	joints[j] = getKinematicModel()->getJoint(path.names[j]);
+	if (joints[j] == NULL)
+	{
+	    ROS_ERROR("Unknown joint '%s' found on path", path.names[j].c_str());
+	    return -1;
+	}
+    }
+    
+    for (unsigned int i = start ; i <= end ; ++i)
+    {
+	unsigned int u = 0;
+	double       d = 0.0;
+	for (unsigned int j = 0 ; j < joints.size() ; ++j)
+	{
+	    if (path.states[i].vals.size() < u + joints[j]->usedParams)
+	    {
+		ROS_ERROR("Incorrect state specification on path");
+		return -1;
+	    }
+	    
+	    const double *jparams = state->getParamsJoint(joints[j]->name);	    
+	    for (unsigned int k = 0 ; k < joints[j]->usedParams ; ++k)
+	    {
+		double diff = fabs(path.states[i].vals[u + k] - jparams[k]);
+		d += diff * diff;
+	    }
+	    u += joints[j]->usedParams;
+	}
+	
+	if (pos < 0 || d < dist)
+	{
+	    pos = i;
+	    dist = d;
+	}
+    }
+    
+    return pos;
+}
+
 bool planning_environment::PlanningMonitor::isPathValid(const motion_planning_msgs::KinematicPath &path, bool verbose) const
 {
+    return isPathValid(path, 0, path.states.size() - 1, verbose);
+}
+
+bool planning_environment::PlanningMonitor::isPathValid(const motion_planning_msgs::KinematicPath &path, unsigned int start, unsigned int end, bool verbose) const
+{
+    if (end >= path.states.size())
+	end = path.states.size() - 1;
+    if (start > end)
+	return true;
     if (path.header.frame_id != getFrameId())
     {
 	motion_planning_msgs::KinematicPath pathT = path;
 	if (transformPathToFrame(pathT, getFrameId()))
-	  return isPathValidAux(pathT, verbose);
+	    return isPathValidAux(pathT, start, end, verbose);
 	else
 	    return false;
     }
     else
-      return isPathValidAux(path, verbose);
+	return isPathValidAux(path, start, end, verbose); 
 }
 
-bool planning_environment::PlanningMonitor::isPathValidAux(const motion_planning_msgs::KinematicPath &path, bool verbose) const
+bool planning_environment::PlanningMonitor::isPathValidAux(const motion_planning_msgs::KinematicPath &path, unsigned int start, unsigned int end, bool verbose) const
 {    
     boost::scoped_ptr<planning_models::StateParams> sp(getKinematicModel()->newStateParams());
     
@@ -415,7 +496,7 @@
     }
     
     // check every state
-    for (unsigned int i = 0 ; valid && i < path.states.size() ; ++i)
+    for (unsigned int i = start ; valid && i <= end ; ++i)
     {
 	unsigned int u = 0;
 	for (unsigned int j = 0 ; j < joints.size() ; ++j)


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