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

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

Revision: 3971
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3971&view=rev
Author:   isucan
Date:     2008-09-05 01:40:07 +0000 (Fri, 05 Sep 2008)

Log Message:
-----------
motion planning test for base + right arm

Modified Paths:
--------------
    pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml

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-09-05 01:11:50 UTC (rev 3970)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp	2008-09-05 01:40:07 UTC (rev 3971)
@@ -156,6 +156,39 @@
 	performCall(req);
     }
     
+    
+    void runTestBaseRightArm(void)
+    {
+	robot_srvs::KinematicPlanState::request  req;
+	
+	req.params.model_id = "pr2::base_right_arm";
+	req.params.distance_metric = "L2Square";
+	req.params.planner_id = "SBL";
+	req.threshold = 0.01;
+	req.interpolate = 1;
+	req.times = 1;
+
+	initialState(req.start_state);
+	req.start_state.vals[0] -= 1.0;
+
+	req.goal_state.set_vals_size(11);
+
+	for (unsigned int i = 0 ; i < 3 ; ++i)
+	    req.goal_state.vals[i] = m_basePos[i];
+	req.goal_state.vals[0] += 2.0;
+	req.goal_state.vals[2] = M_PI + M_PI/4.0;
+
+	for (unsigned int i = 3 ; i < req.goal_state.vals_size ; ++i)
+	    req.goal_state.vals[i] = 0.0;
+	
+	req.allowed_time = 30.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);
+    }
+    
     void performCall(robot_srvs::KinematicPlanState::request &req)
     {	
 	robot_srvs::KinematicPlanState::response res;
@@ -189,13 +222,12 @@
 	req.params.distance_metric = "L2Square";
 	req.params.planner_id = "RRT";
 	req.interpolate = 1;
-	req.times = 2;
+	req.times = 4;
 
 	initialState(req.start_state);
 
-	req.start_state.vals[0] += 0.5;
-	req.start_state.vals[1] += 1.5;
-	req.start_state.vals[2] = -M_PI/2.0;
+	req.start_state.vals[0] += 2.0;
+	req.start_state.vals[2] = M_PI + M_PI/4.0;
 
 
 	// the goal region is basically the position of a set of bodies
@@ -217,7 +249,7 @@
 	req.constraints.pose[0].pose.position.z = 0.74;
 	req.constraints.pose[0].position_distance = 0.01;
 	*/
-	req.allowed_time = 3.0;
+	req.allowed_time = 0.1;
 	
 	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;
@@ -296,6 +328,9 @@
     case 'e':
 	plan.runTestLeftEEf();    
 	break;
+    case 'x':
+	plan.runTestBaseRightArm();
+	break;
     default:
 	printf("Unknown test\n");
 	break;

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml	2008-09-05 01:11:50 UTC (rev 3970)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml	2008-09-05 01:40:07 UTC (rev 3971)
@@ -95,6 +95,20 @@
     forearm_roll_right
     wrist_flex_right
     gripper_roll_right
+
+    <map name="RRT" flag="planning">
+      <elem key="range" value="0.75" />
+    </map>
+    
+    <map name="LazyRRT" flag="planning">
+      <elem key="range" value="0.75" />
+    </map>
+
+    <map name="SBL" flag="planning">
+      <elem key="projection" value="5 6"/>
+      <elem key="celldim" value="0.1 0.1"/>
+    </map>
+
   </group>
   
   <group name="arms" flags="planning">


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