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

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

Revision: 2442
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2442&view=rev
Author:   isucan
Date:     2008-08-01 16:42:47 +0000 (Fri, 01 Aug 2008)

Log Message:
-----------
removed anchor definition

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

Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-08-01 16:37:32 UTC (rev 2441)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-08-01 16:42:47 UTC (rev 2442)
@@ -285,6 +285,15 @@
 	/* copy the solution to the result */
 	if (ok)
 	{
+
+#ifdef DISPLAY_ODE_SPACES
+	if (m->groupID >= 0)
+	{
+	    /* set the pose of the whole robot */
+	    m->kmodel->computeTransforms(req.start_state.vals);
+	    m->collisionSpace->updateRobotModel(m->collisionSpaceID);
+	}
+#endif
 	    ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
 	    res.path.set_states_size(path->states.size());
 	    for (unsigned int i = 0 ; i < path->states.size() ; ++i)

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-01 16:37:32 UTC (rev 2441)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp	2008-08-01 16:42:47 UTC (rev 2442)
@@ -48,18 +48,17 @@
 	robot_srvs::KinematicMotionPlan::request  req;
 	robot_srvs::KinematicMotionPlan::response res;
 	
-	req.model_id = "pr2::rightArm";
+	req.model_id = "pr2::base";
 	
 	req.start_state.set_vals_size(32);
 	for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
 	    req.start_state.vals[i] = 0.0;
 
-	for (unsigned int i = 18 ; i < 25 ; ++i)
-	    req.start_state.vals[i] = 0.1;
-	
-	req.goal_state.set_vals_size(7);
+	req.goal_state.set_vals_size(3);
 	for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
-	    req.goal_state.vals[i] = -0.4;
+	    req.goal_state.vals[i] = 0.0;
+	req.goal_state.vals[0] = 1.0;
+	req.goal_state.vals[2] = M_PI/2;
 	
 	req.allowed_time = 15.0;
 	
@@ -70,6 +69,12 @@
 	{
 	    unsigned int nstates = res.path.get_states_size();
 	    printf("Obtained solution path with %u states\n", nstates);
+	    for (unsigned int i = 0 ; i < nstates ; ++i)
+	    {
+		for (unsigned int j = 0 ; j < res.path.states[i].get_vals_size() ; ++j)
+		    printf("%f ", res.path.states[i].vals[j]);
+		printf("\n");
+	    }	    
 	}
 	else
 	    fprintf(stderr, "Service 'plan_kinematic_path' failed\n");	  

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml	2008-08-01 16:37:32 UTC (rev 2441)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml	2008-08-01 16:42:47 UTC (rev 2442)
@@ -428,7 +428,6 @@
 
     <rpy> 0 0 0</rpy> <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
     <xyz> 0.25 0.1 0.3+wheel_clearance_offset</xyz> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
-    <xyz> 0 0 0</xyz> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
 
     <joint type="planar"> 
     </joint>
@@ -997,6 +996,10 @@
   
   <!-- Define groups of links; a link may be part of multiple groups -->
 
+  <group name="base" flags="plan">
+    base
+  </group>
+  
   <group name="leftArm" flags="plan">
     shoulder_pan_left
     shoulder_pitch_left


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