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

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

Revision: 17067
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17067&view=rev
Author:   isucan
Date:     2009-06-14 05:43:22 +0000 (Sun, 14 Jun 2009)

Log Message:
-----------
monitor position on path

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

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h	2009-06-14 01:14:02 UTC (rev 17066)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h	2009-06-14 05:43:22 UTC (rev 17067)
@@ -72,8 +72,11 @@
 	bool isStateValidAtGoal(const planning_models::KinematicModel::StateParams *state) const;
 	
 	/** Check if the path is valid */
-	bool isPathValid(const motion_planning_msgs::KinematicPath &path);
+	bool isPathValid(const motion_planning_msgs::KinematicPath &path) const;
 	
+	/** Return the index of the state on the path that is closest to the current state */
+	unsigned int positionOnPath(const motion_planning_msgs::KinematicPath &path) const;
+
 	/** Set the kinematic constraints the monitor should use when checking a path */
 	void setPathConstraints(const motion_planning_msgs::KinematicConstraints &kc);
 
@@ -81,18 +84,18 @@
 	void setGoalConstraints(const motion_planning_msgs::KinematicConstraints &kc);
 	
 	/** Transform the frames in which constraints are specified to the one requested */
-	void transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target);
+	void transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const;
 	
 	/** Transform the kinematic path to the frame requested */
-	void transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target);
+	void transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const;
 
     protected:
 	
 	/** Transform the joint parameters (if needed) to a target frame */
-	void transformJoint(const std::string &name, unsigned int index, std::vector<double> &params, roslib::Header &header, const std::string& target);
+	void transformJoint(const std::string &name, unsigned int index, std::vector<double> &params, roslib::Header &header, const std::string& target) const;
 	
 	/** Check the path assuming it is in the frame of the model */
-	bool isPathValidAux(const motion_planning_msgs::KinematicPath &path);
+	bool isPathValidAux(const motion_planning_msgs::KinematicPath &path) const;
 	
 	motion_planning_msgs::KinematicConstraints kcPath_;
 	motion_planning_msgs::KinematicConstraints kcGoal_;

Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-06-14 01:14:02 UTC (rev 17066)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-06-14 05:43:22 UTC (rev 17067)
@@ -49,7 +49,7 @@
     transformConstraintsToFrame(kcPath_, getFrameId());
 }
 
-void planning_environment::PlanningMonitor::transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target)
+void planning_environment::PlanningMonitor::transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const
 {
     for (unsigned int i = 0; i < kc.pose.size() ; ++i)
 	tf_->transformPose(target, kc.pose[i].pose, kc.pose[i].pose);
@@ -62,7 +62,7 @@
     }
 }
 
-void planning_environment::PlanningMonitor::transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target)
+void planning_environment::PlanningMonitor::transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const
 {    
     // if there are no planar of floating transforms, there is nothing to do
     if (getKinematicModel()->getModelInfo().planarJoints.empty() && getKinematicModel()->getModelInfo().floatingJoints.empty())
@@ -107,7 +107,7 @@
     kp.header = updatedHeader;
 }
 
-void planning_environment::PlanningMonitor::transformJoint(const std::string &name, unsigned int index, std::vector<double> &params, roslib::Header &header, const std::string& target)
+void planning_environment::PlanningMonitor::transformJoint(const std::string &name, unsigned int index, std::vector<double> &params, roslib::Header &header, const std::string& target) const
 {
     // planar joints and floating joints may need to be transformed 
     planning_models::KinematicModel::Joint *joint = getKinematicModel()->getJoint(name);
@@ -206,10 +206,54 @@
     return valid;    
 }
 
-bool planning_environment::PlanningMonitor::isPathValid(const motion_planning_msgs::KinematicPath &path)
+unsigned int planning_environment::PlanningMonitor::positionOnPath(const motion_planning_msgs::KinematicPath &path) const
 {
+    std::vector<double> current;
+    getRobotState()->copyParamsGroup(current, path.model_id);
+
+    // convert current state to path frame
     if (path.header.frame_id != getFrameId())
     {
+	roslib::Header header;
+	header.stamp = lastStateUpdate();
+	header.frame_id = getFrameId();
+	
+	std::vector<planning_models::KinematicModel::Joint*> joints;
+	joints.resize(path.names.size());
+	for (unsigned int j = 0 ; j < joints.size() ; ++j)
+	    joints[j] = getKinematicModel()->getJoint(path.names[j]);
+	
+	unsigned int u = 0;
+	for (unsigned int j = 0 ; j < joints.size() ; ++j)
+	{
+	    roslib::Header h = header;
+	    transformJoint(joints[j]->name, u, current, h, path.header.frame_id);
+	    u += joints[j]->usedParams;
+	}
+    }
+    
+    unsigned int p = 0;
+    double bestDif = -1.0;
+    
+    for (unsigned int i = 0 ; i < path.states.size() ; ++i)
+    {
+	double sum = 0.0;
+	for (unsigned int j = 0 ; j < current.size() ; ++j)
+	    sum += fabs(path.states[i].vals[j] - current[j]);
+	if (sum < bestDif || bestDif < 0.0)
+	{
+	    p = i;
+	    bestDif = sum;
+	}
+    }
+    
+    return p;
+}
+
+bool planning_environment::PlanningMonitor::isPathValid(const motion_planning_msgs::KinematicPath &path) const
+{
+    if (path.header.frame_id != getFrameId())
+    {
 	motion_planning_msgs::KinematicPath pathT = path;
 	transformPathToFrame(pathT, getFrameId());
 	return isPathValidAux(pathT);
@@ -218,7 +262,7 @@
 	return isPathValidAux(path);
 }
 
-bool planning_environment::PlanningMonitor::isPathValidAux(const motion_planning_msgs::KinematicPath &path)
+bool planning_environment::PlanningMonitor::isPathValidAux(const motion_planning_msgs::KinematicPath &path) const
 {    
     planning_models::KinematicModel::StateParams *sp = getKinematicModel()->newStateParams();
     sp->setParams(path.start_state.vals);


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