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

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

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

Log Message:
-----------
clearing constraints

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-07-01 04:45:25 UTC (rev 18072)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h	2009-07-01 04:53:37 UTC (rev 18073)
@@ -77,7 +77,7 @@
 	/** \brief Check if the full state of the robot is valid */
 	bool isStateValidAtGoal(const planning_models::StateParams *state) const;
 	
-	/** \brief Check if the path is valid */
+	/** \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 Set the kinematic constraints the monitor should use when checking a path */
@@ -86,6 +86,9 @@
 	/** \brief Set the kinematic constraints the monitor should use when checking a path's last state (the goal) */
 	void setGoalConstraints(const motion_planning_msgs::KinematicConstraints &kc);
 	
+	/** \brief Clear previously set constraints */
+	void clearConstraints(void);
+	
 	/** \brief Transform the frames in which constraints are specified to the one requested */
 	bool transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const;
 	

Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-07-01 04:45:25 UTC (rev 18072)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp	2009-07-01 04:53:37 UTC (rev 18073)
@@ -60,6 +60,14 @@
     return true;
 }
 
+void planning_environment::PlanningMonitor::clearConstraints(void)
+{
+    kcPath_.joint_constraint.clear();
+    kcPath_.pose_constraint.clear();
+    kcGoal_.joint_constraint.clear();
+    kcGoal_.pose_constraint.clear();
+}
+
 void planning_environment::PlanningMonitor::setPathConstraints(const motion_planning_msgs::KinematicConstraints &kc)
 {
     kcPath_ = kc;
@@ -355,16 +363,6 @@
 	}
     }
 
-    // if we got to the last state, we also check the goal constraints
-    if (valid)
-    {
-	ks.add(getKinematicModel(), kcGoal_.joint_constraint);
-	ks.add(getKinematicModel(), kcGoal_.pose_constraint);
-	valid = ks.decide(sp->getParams());
-	if (verbose && !valid)
-	    ROS_INFO("isPathValid: Goal state does not satisfy constraints");
-    }
-    
     getEnvironmentModel()->setVerbose(vlevel);
     getKinematicModel()->unlock();
     getEnvironmentModel()->unlock();


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