Browse files

Make sure that ROS trajectories without full upper posture can be loa…

…ded.
  • Loading branch information...
1 parent 5ce5cb8 commit c405381d7dbfad4906490b5e0ab1edee06608637 @thomas-moulard thomas-moulard committed Mar 16, 2012
Showing with 38 additions and 7 deletions.
  1. +38 −7 src/feet-follower-ros.cc
View
45 src/feet-follower-ros.cc
@@ -300,13 +300,44 @@ FeetFollowerRos::parseTrajectory (const std::string& rosParameter)
//FIXME: not generic enough.
waistYawElt (0) = reader.postureTrajectory ().data ()[i].position[0];
- for (unsigned i = 0; i < 6 + 12; ++i)
- postureElt (i) = 0.;
-
- for (unsigned j = 0; j < postureElt.size () - 6 - 12; ++j)
- postureElt (j + 6 + 12) =
- reader.postureTrajectory ().data ()[i].position[j + 1];
-
+ // Yaw only.
+ if (reader.postureTrajectory ().data ()[i].position.rows () == 1)
+ {
+ // Set to half-sitting.
+ static const double halfSitting[36] = {
+ // Free flyer
+ 0., 0., 0.648702, 0., 0. , 0.,
+
+ // Legs
+ 0., 0., -0.453786, 0.872665, -0.418879, 0.,
+ 0., 0., -0.453786, 0.872665, -0.418879, 0.,
+
+ // Chest and head
+ 0., 0., 0., 0.,
+
+ // Arms
+ 0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1,
+ 0.261799, 0.17453, 0., -0.523599, 0., 0., 0.1
+ };
+
+ for (unsigned i = 0; i < 6 + 30; ++i)
+ postureElt (i) = halfSitting[i];
+
+ // Override yaw using data.
+ postureElt (5) =
+ reader.postureTrajectory ().data ()[i].position[0];
+ }
+ // Full body.
+ else if
+ (reader.postureTrajectory ().data ()[i].position.rows () == 36)
+ {
+ for (unsigned i = 0; i < 6 + 12; ++i)
+ postureElt (i) = 0.;
+
+ for (unsigned j = 0; j < postureElt.size () - 6 - 12; ++j)
+ postureElt (j + 6 + 12) =
+ reader.postureTrajectory ().data ()[i].position[j + 1];
+ }
leftFootData.push_back (leftFootElt);
rightFootData.push_back (rightFootElt);

0 comments on commit c405381

Please sign in to comment.