Fix foot signal problem.

laas · Mar 7, 2012 · 1131940 · 1131940
1 parent de90a20
commit 1131940
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 19 deletions.
diff --git a/halfsteps_pattern_generator/samples/tutorial_getting_started.py b/halfsteps_pattern_generator/samples/tutorial_getting_started.py
@@ -9,11 +9,11 @@
     print "Calling getPath service..."
     try:
         client = HalfStepPatternGeneratorClient()
-        client.initial_left_foot_position.position.y = -0.19
-        client.initial_right_foot_position.position.y = +0.19
+        client.initial_left_foot_position.position.y = +0.19
+        client.initial_right_foot_position.position.y = -0.19
         client.initial_center_of_mass_position.z = 0.8
-        client.final_left_foot_position.position.y = -0.19
-        client.final_right_foot_position.position.y = +0.19
+        client.final_left_foot_position.position.y = +0.19
+        client.final_right_foot_position.position.y = -0.19
         client.final_center_of_mass_position.z = 0.8
         client.start_with_left_foot = True
         client.footprints = []
@@ -40,14 +40,14 @@ def makeFootprint(x, y):
             return footprint
 
 
-        client.footprints.append(makeFootprint(1 * 0.25, -0.19))
         client.footprints.append(makeFootprint(1 * 0.25, +0.19))
+        client.footprints.append(makeFootprint(1 * 0.25, -0.19))
 
-        client.footprints.append(makeFootprint(2 * 0.25, -0.19))
         client.footprints.append(makeFootprint(2 * 0.25, +0.19))
+        client.footprints.append(makeFootprint(2 * 0.25, -0.19))
 
-        client.footprints.append(makeFootprint(3 * 0.25, -0.19))
         client.footprints.append(makeFootprint(3 * 0.25, +0.19))
+        client.footprints.append(makeFootprint(3 * 0.25, -0.19))
 
         if not not client():
             print("Service call succeed")

diff --git a/...teps_pattern_generator/src/libhalfsteps_pattern_generator/halfsteps_pattern_generator.cpp b/...teps_pattern_generator/src/libhalfsteps_pattern_generator/halfsteps_pattern_generator.cpp
@@ -52,10 +52,14 @@ HalfStepsPatternGenerator::computeTrajectories()
   double comZ = initialCenterOfMassPosition()[2];
 
   Eigen::Matrix<double, 6, 1> initialStep;
+
+  double sign = (startWithLeftFoot() ? -1. : 1.);
+
   initialStep(0) =
-    std::fabs(initialLeftFootPosition() (0, 3)
-	      - initialRightFootPosition() (0, 3)) / 2.;
+    (std::fabs(initialLeftFootPosition() (0, 3)
+	       - initialRightFootPosition() (0, 3)) / 2.);
   initialStep(1) =
+    sign *
     std::fabs(initialLeftFootPosition() (1, 3)
 	      - initialRightFootPosition() (1, 3)) / 2.;
   initialStep(2) = 0.;
@@ -71,37 +75,41 @@ HalfStepsPatternGenerator::computeTrajectories()
 
   // Footprints
   // Convert footprints into relative positions.
-  walk::HomogeneousMatrix3d previousPosition;
+  walk::HomogeneousMatrix3d world_M_footprint;
   if (startWithLeftFoot ())
-    previousPosition = initialRightFootPosition ();
+    world_M_footprint = initialRightFootPosition ();
   else
-    previousPosition = initialLeftFootPosition ();
+    world_M_footprint = initialLeftFootPosition ();
 
-  walk::HomogeneousMatrix3d newPosition;
-  walk::HomogeneousMatrix3d relativePosition;
+  walk::HomogeneousMatrix3d world_M_newfootprint;
+  walk::HomogeneousMatrix3d footprint_M_newfootprint;
 
   for (unsigned i = 0; i < this->footprints().size (); ++i)
     {
       walk::trans2dToTrans3d
-	(newPosition,
+	(world_M_newfootprint,
 	 this->footprints()[i].position[0],
 	 this->footprints()[i].position[1],
 	 this->footprints()[i].position[2]);
 
-      relativePosition = newPosition * previousPosition.inverse ();
+      footprint_M_newfootprint =
+	world_M_footprint.inverse () * world_M_newfootprint;
+
+      ROS_DEBUG_STREAM
+	("add relative footstep:\n" << footprint_M_newfootprint);
 
       walk_msgs::Footprint2d footprint;
       walk_msgs::convertHomogeneousMatrix3dToFootprint2d
-	(footprint, relativePosition);
+	(footprint, footprint_M_newfootprint);
       stepData.push_back(this->footprints()[i].slideUp);
-      stepData.push_back(this->footprints()[i].horizontalDistance);
+      stepData.push_back(-this->footprints()[i].horizontalDistance);
       stepData.push_back(this->footprints()[i].stepHeight);
       stepData.push_back(this->footprints()[i].slideDown);
       stepData.push_back(footprint.x);
       stepData.push_back(footprint.y);
       stepData.push_back(angles::to_degrees (footprint.theta));
 
-      previousPosition = newPosition;
+      world_M_footprint = world_M_newfootprint;
     }
 
   pg.produceSeqSlidedHalfStepFeatures