Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[3220] pkg/trunk (Page 6)

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 3220
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3220&view=rev
Author:   isucan
Date:     2008-08-18 21:48:42 +0000 (Mon, 18 Aug 2008)

Log Message:
-----------
added path interpolation option for motion plans + fixed call parameters

Modified Paths:
--------------
    pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
    pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
    pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
    pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
    pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
    pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
    pkg/trunk/robot_srvs/srv/KinematicPlanState.srv

Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================

--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-08-18 21:48:42 UTC (rev 3220)
@@ -197,6 +197,8 @@
 	    p.si->smoother->smoothVertices(path);
 	    double tsmooth = (ros::Time::now() - startTime).to_double();	
 	    printf("Smoother spent %f seconds (%f seconds in total)\n", tsmooth, tsmooth + tsolve);
+	    if (req.interpolate)
+		p.si->interpolatePath(path);
 	}	
 	
 	m_collisionSpace->unlock();

Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h	2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h	2008-08-18 21:48:42 UTC (rev 3220)
@@ -85,11 +85,11 @@
 	class StateValidityChecker
 	{
 	public:
-	  /** Destructor */
-	  virtual ~StateValidityChecker(void){
-
-	  }
-
+	    /** Destructor */
+	    virtual ~StateValidityChecker(void)
+	    {
+	    }
+	    
 	    /** Return true if the state is valid */
 	    virtual bool operator()(const State_t state) = 0;
 	};
@@ -101,10 +101,10 @@
 	class StateDistanceEvaluator
 	{
 	public:
-	  /** Destructor */
-	  virtual ~StateDistanceEvaluator(void){
-
-	  }
+	    /** Destructor */
+	    virtual ~StateDistanceEvaluator(void)
+	    {
+	    }
 	    /** Return true if the state is valid */
 	    virtual double operator()(const State_t state1, const State_t state2) = 0;
 	};
@@ -302,9 +302,9 @@
 	}
 	
 	/** Clear the goal. Memory is freed. */
-	void clearGoal(bool free = true)
+	void clearGoal(void)
 	{
-	    if (free && m_goal)
+	    if (m_goal)
 		delete m_goal;
 	    m_goal = NULL;
 	}

Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp	2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp	2008-08-18 21:48:42 UTC (rev 3220)
@@ -189,11 +189,10 @@
 		state->values[k] = s1->values[k] + (double)j * step[k];
 	    newStates.push_back(state);
 	}
-	
-	newStates.push_back(s2);
     }
+    newStates.push_back(path->states[n1]);
     
-    path->states.swap(newStates);    
+    path->states.swap(newStates);
 }
 
 void ompl::SpaceInformationKinematic::printSettings(FILE *out) const

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-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp	2008-08-18 21:48:42 UTC (rev 3220)
@@ -56,6 +56,16 @@
 	return m_haveBasePos;
     }
     
+    void initialState(robot_msgs::KinematicState &state)
+    {
+	state.set_vals_size(45);
+	for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
+	    state.vals[i] = 0.0;
+	for (unsigned int i = 0 ; i < 3 ; ++i)
+	    state.vals[i] = m_basePos[i];
+    }
+    
+    
     void runTestBase(void)
     {
 	robot_srvs::KinematicPlanState::request  req;
@@ -63,20 +73,15 @@
 	req.model_id = "pr2::base";
 	req.threshold = 0.01;
 	req.distance_metric = "L2Square";
+	req.interpolate = 1;
 	
-	req.start_state.set_vals_size(45);
-	for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
-	    req.start_state.vals[i] = 0.0;
+	initialState(req.start_state);
 	
 	req.goal_state.set_vals_size(3);
 	for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
-	{
-	    req.goal_state.vals[i] = m_basePos[i];
 	    req.start_state.vals[i] = m_basePos[i];
-	}
 	req.goal_state.vals[0] += 3.5;
 	
-
 	req.allowed_time = 5.0;
 	
 	req.volumeMin.x = -5.0 + m_basePos[0];	req.volumeMin.y = -5.0 + m_basePos[1];	req.volumeMin.z = 0.0;
@@ -92,11 +97,10 @@
 	req.model_id = "pr2::leftArm";
 	req.threshold = 0.01;
 	req.distance_metric = "L2Square";
+	req.interpolate = 1;
+
+	initialState(req.start_state);
 	
-	req.start_state.set_vals_size(45);
-	for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
-	    req.start_state.vals[i] = 0.0;
-	
 	req.goal_state.set_vals_size(7);
 	for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
 	    req.goal_state.vals[i] = 0.0;
@@ -118,11 +122,10 @@
 	req.model_id = "pr2::rightArm";
 	req.threshold = 0.01;
 	req.distance_metric = "L2Square";
+	req.interpolate = 1;
+
+	initialState(req.start_state);
 	
-	req.start_state.set_vals_size(45);
-	for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
-	    req.start_state.vals[i] = 0.0;
-	
 	req.goal_state.set_vals_size(7);
 	for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
 	    req.goal_state.vals[i] = 0.0;
@@ -187,9 +190,23 @@
     while (!plan.haveBasePos())
 	dur.sleep();
 
-    //    plan.runTestLeftArm();    
-    //    plan.runTestBase();
-    plan.runTestRightArm();    
+    char test = (argc < 2) ? 'b' : argv[1][0];
+    
+    switch (test)
+    {
+    case 'l':
+	plan.runTestLeftArm();    
+	break;
+    case 'b':    
+        plan.runTestBase();
+	break;
+    case 'r':
+	plan.runTestRightArm();    
+	break;
+    default:
+	printf("Unknown test\n");
+	break;
+    } 
 
     plan.shutdown();
     

Modified: pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp	2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp	2008-08-18 21:48:42 UTC (rev 3220)
@@ -103,7 +103,7 @@
     PlanningWorldViewer(const std::string &robot_model) : ros::node("planning_world_viewer"),
 							  planning_node_util::NodeODECollisionModel(dynamic_cast<ros::node*>(this), robot_model)
     {
-	subscribe("display_kinematic_path", m_displayPath, &PlanningWorldViewer::displayPathCallback);
+	subscribe("display_kinematic_path", m_displayPath, &PlanningWorldViewer::displayPathCallback, 1);
 	
 	m_follow = true;
 	m_displayRobot = true;
@@ -130,6 +130,20 @@
 	m_collisionSpace->unlock();
     }
     
+    bool checkCollision(void)
+    {
+	bool result = false;
+	if (m_checkCollision && m_collisionSpace && m_collisionSpace->getModelCount() == 1)
+	{
+	    m_collisionSpace->lock();
+	    ros::Time startTime = ros::Time::now();
+	    result = m_collisionSpace->isCollision(0);
+	    printf("Collision: %d     [%f s]\n", result, (ros::Time::now() - startTime).to_double());
+	    m_collisionSpace->unlock();
+	}
+	return result;
+    }
+    
     void baseUpdate(void)
     {
 	planning_node_util::NodeODECollisionModel::baseUpdate();
@@ -140,12 +154,8 @@
 	    m_collisionSpace->lock();
 	    m_collisionSpace->getModel(0)->computeTransforms(m_basePos, group);
 	    m_collisionSpace->updateRobotModel(0);
-	    if (m_checkCollision)
-	    {
-		ros::Time startTime = ros::Time::now();
-		printf("Collision: %d     [%f s]\n", m_collisionSpace->isCollision(0), (ros::Time::now() - startTime).to_double());
-	    }
 	    m_collisionSpace->unlock();
+	    checkCollision();
 	}
     }
     
@@ -167,13 +177,14 @@
     {
 	bool follow = m_follow;
 	m_follow = false;
-	ros::Duration sleepTime(0.2);
+	ros::Duration sleepTime(0.4);
 	int groupID = m_kmodel->getGroupID(m_displayPath.name);
 	
 	for (unsigned int i = 0 ; i < m_displayPath.path.get_states_size() ; ++i)
 	{
 	    m_kmodel->computeTransforms(m_displayPath.path.states[i].vals, groupID);
 	    m_collisionSpace->updateRobotModel(0);
+	    checkCollision();
 	    sleepTime.sleep();
 	}
 	m_follow = follow;
@@ -240,8 +251,6 @@
     
     void display(void)
     {
-	//	printf("disp\n");
-	
 	m_displayLock.lock();
 	m_spaces.displaySpaces();
 	m_displayLock.unlock();

Modified: pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv	2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv	2008-08-18 21:48:42 UTC (rev 3220)
@@ -25,7 +25,11 @@
 # No state in the produced motion plan will violate these constraints
 robot_msgs/KinematicConstraint[] constraints
 
+# Boolean flag: if true, the returned path will contain interpolated
+# states as well
 
+byte interpolate
+
 # The maximum amount of time the motion planner is allowed to plan for
 float64 allowed_time
 

Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv	2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv	2008-08-18 21:48:42 UTC (rev 3220)
@@ -28,6 +28,11 @@
 # No state in the produced motion plan will violate these constraints
 robot_msgs/KinematicConstraint[] constraints
 
+# Boolean flag: if true, the returned path will contain interpolated
+# states as well
+
+byte interpolate
+
 # The maximum amount of time the motion planner is allowed to plan for
 float64 allowed_time
 


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




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[3220] pkg/trunk (Page 6)

personalrobots-commit