halfsteps_pattern_generator.cpp
Go to the documentation of this file.
00001 #include <stdexcept>
00002 #include <boost/date_time.hpp>
00003 #include <boost/math/constants/constants.hpp>
00004 
00005 #include <Eigen/LU>
00006 
00007 #include "halfsteps_pattern_generator.hh"
00008 #include "newPGstepStudy.h"
00009 
00010 #include <angles/angles.h>
00011 #include <walk_interfaces/util.hh>
00012 #include <walk_msgs/conversion.hh>
00013 #include <walk_msgs/Footprint2d.h>
00014 
00015 static const double g = 9.81;
00016 
00017 HalfStepsPatternGenerator::HalfStepsPatternGenerator
00018 (const double& timeBeforeZmpShift,
00019  const double& timeAfterZmpShift,
00020  const double& step)
00021   : walk::PatternGenerator<HalfStepsPatternGenerator>(),
00022     timeBeforeZmpShift_(timeBeforeZmpShift),
00023     timeAfterZmpShift_(timeAfterZmpShift),
00024     step_(step)
00025 {}
00026 
00027 HalfStepsPatternGenerator::HalfStepsPatternGenerator
00028 (const HalfStepsPatternGenerator& pg)
00029   : walk::PatternGenerator<HalfStepsPatternGenerator>(),
00030     timeBeforeZmpShift_(pg.timeBeforeZmpShift_),
00031     timeAfterZmpShift_(pg.timeAfterZmpShift_),
00032     step_(pg.step_)
00033 {}
00034 
00035 HalfStepsPatternGenerator::~HalfStepsPatternGenerator()
00036 {}
00037 
00038 HalfStepsPatternGenerator&
00039 HalfStepsPatternGenerator::operator= (const HalfStepsPatternGenerator& pg)
00040 {
00041   if (&pg == this)
00042     return *this;
00043   return *this;
00044 }
00045 
00046 void
00047 HalfStepsPatternGenerator::computeTrajectories()
00048 {
00049   CnewPGstepStudy pg;
00050   StepFeatures stepFeatures;
00051 
00052   double comZ = initialCenterOfMassPosition()[2];
00053 
00054   Eigen::Matrix<double, 6, 1> initialStep;
00055 
00056   double sign = (startWithLeftFoot() ? -1. : 1.);
00057 
00058   initialStep(0) =
00059     (std::fabs(initialLeftFootPosition() (0, 3)
00060                - initialRightFootPosition() (0, 3)) / 2.);
00061   initialStep(1) =
00062     sign *
00063     std::fabs(initialLeftFootPosition() (1, 3)
00064               - initialRightFootPosition() (1, 3)) / 2.;
00065   initialStep(2) = 0.;
00066 
00067   initialStep(3) = -initialStep (0);
00068   initialStep(4) = -initialStep (1);
00069   initialStep(5) = std::atan2(initialRightFootPosition() (1,0),
00070                               initialRightFootPosition() (0,0));
00071 
00072   std::vector<double> stepData;
00073   for (unsigned i = 0; i < 6; ++i)
00074     stepData.push_back(initialStep (i));
00075 
00076   for (unsigned i = 0; i < 6; ++i)
00077     ROS_DEBUG_STREAM
00078       ("initial parameter " << i << ": " << initialStep (i));
00079 
00080   // Footprints
00081   // Convert footprints into relative positions.
00082   walk::HomogeneousMatrix3d world_M_footprint;
00083   if (startWithLeftFoot ())
00084     world_M_footprint = initialRightFootPosition ();
00085   else
00086     world_M_footprint = initialLeftFootPosition ();
00087 
00088   walk::HomogeneousMatrix3d world_M_newfootprint;
00089   walk::HomogeneousMatrix3d footprint_M_newfootprint;
00090 
00091   for (unsigned i = 0; i < this->footprints().size (); ++i)
00092     {
00093       walk::trans2dToTrans3d
00094         (world_M_newfootprint,
00095          this->footprints()[i].position[0],
00096          this->footprints()[i].position[1],
00097          this->footprints()[i].position[2]);
00098 
00099       footprint_M_newfootprint =
00100         world_M_footprint.inverse () * world_M_newfootprint;
00101 
00102       ROS_DEBUG_STREAM
00103         ("add relative footstep:\n" << footprint_M_newfootprint);
00104 
00105       walk_msgs::Footprint2d footprint;
00106       walk_msgs::convertHomogeneousMatrix3dToFootprint2d
00107         (footprint, footprint_M_newfootprint);
00108 
00109       if (footprint.x != footprint.x
00110           || footprint.y != footprint.y
00111           || footprint.theta != footprint.theta)
00112         throw std::runtime_error("Nan detected in footprints");
00113 
00114       stepData.push_back(this->footprints()[i].slideUp);
00115       stepData.push_back(this->footprints()[i].horizontalDistance);
00116       stepData.push_back(this->footprints()[i].stepHeight);
00117       stepData.push_back(this->footprints()[i].slideDown);
00118       stepData.push_back(footprint.x);
00119       stepData.push_back(footprint.y);
00120       stepData.push_back(angles::to_degrees (footprint.theta));
00121 
00122       ROS_DEBUG_STREAM
00123         ("footstep " << i << ": slide up: " << this->footprints()[i].slideUp);
00124       ROS_DEBUG_STREAM
00125         ("footstep " << i
00126          << ": horizontal distance: "
00127          << this->footprints()[i].horizontalDistance);
00128       ROS_DEBUG_STREAM
00129         ("footstep " << i
00130          << ": step height: " << this->footprints()[i].stepHeight);
00131       ROS_DEBUG_STREAM
00132         ("footstep " << i
00133          << ": slide down: " << this->footprints()[i].slideDown);
00134       ROS_DEBUG_STREAM
00135         ("footstep " << i << ": X: " << footprint.x);
00136       ROS_DEBUG_STREAM
00137         ("footstep " << i << ": Y: " << footprint.y);
00138       ROS_DEBUG_STREAM
00139         ("footstep " << i << ": Theta: " << footprint.theta);
00140 
00141       world_M_footprint = world_M_newfootprint;
00142     }
00143 
00144   pg.produceSeqSlidedHalfStepFeatures
00145     (stepFeatures, step_, comZ, g,
00146      timeBeforeZmpShift_, timeAfterZmpShift_,
00147      timeBeforeZmpShift_ + timeAfterZmpShift_, stepData,
00148      // first support foot
00149      startWithLeftFoot() ? 'R' : 'L');
00150 
00151   getLeftFootTrajectory().data().resize(stepFeatures.size);
00152   getRightFootTrajectory().data().resize(stepFeatures.size);
00153   getCenterOfMassTrajectory().data().resize(stepFeatures.size);
00154   getZmpTrajectory().data().resize(stepFeatures.size);
00155   getPostureTrajectory().data().resize(stepFeatures.size);
00156 
00157   using boost::posix_time::milliseconds;
00158 
00159   for (unsigned i = 0; i < stepFeatures.size; ++i)
00160     {
00161       // Left foot.
00162       getLeftFootTrajectory().data()[i].duration = milliseconds(step_ * 1e3);
00163       getLeftFootTrajectory().data()[i].position.setIdentity();
00164       getLeftFootTrajectory().data()[i].position (0,3) =
00165         stepFeatures.leftfootXtraj[i];
00166       getLeftFootTrajectory().data()[i].position (1,3) =
00167         stepFeatures.leftfootYtraj[i];
00168       getLeftFootTrajectory().data()[i].position (2,3) =
00169         stepFeatures.leftfootHeight[i];
00170 
00171       double theta = angles::from_degrees (stepFeatures.leftfootOrient[i]);
00172       getLeftFootTrajectory().data()[i].position (0,0) = std::cos(theta);
00173       getLeftFootTrajectory().data()[i].position (0,1) = -std::sin(theta);
00174       getLeftFootTrajectory().data()[i].position (1,0) = std::sin(theta);
00175       getLeftFootTrajectory().data()[i].position (1,1) = std::cos(theta);
00176 
00177       // Right foot.
00178       getRightFootTrajectory().data()[i].duration = milliseconds(step_ * 1e3);
00179       getRightFootTrajectory().data()[i].position.setIdentity();
00180       getRightFootTrajectory().data()[i].position (0,3) =
00181         stepFeatures.rightfootXtraj[i];
00182       getRightFootTrajectory().data()[i].position (1,3) =
00183         stepFeatures.rightfootYtraj[i];
00184       getRightFootTrajectory().data()[i].position (2,3) =
00185         stepFeatures.rightfootHeight[i];
00186 
00187       theta = angles::from_degrees (stepFeatures.rightfootOrient[i]);
00188       getRightFootTrajectory().data()[i].position (0,0) = std::cos(theta);
00189       getRightFootTrajectory().data()[i].position (0,1) = -std::sin(theta);
00190       getRightFootTrajectory().data()[i].position (1,0) = std::sin(theta);
00191       getRightFootTrajectory().data()[i].position (1,1) = std::cos(theta);
00192 
00193       // Center of mass
00194       getCenterOfMassTrajectory().data()[i].duration =
00195         milliseconds(step_ * 1e3);
00196       getCenterOfMassTrajectory().data()[i].position[0] =
00197         stepFeatures.comTrajX[i];
00198       getCenterOfMassTrajectory().data()[i].position[1] =
00199         stepFeatures.comTrajY[i];
00200       getCenterOfMassTrajectory().data()[i].position[2] = comZ;
00201 
00202       // ZMP
00203       getZmpTrajectory().data()[i].duration = milliseconds(step_ * 1e3);
00204       getZmpTrajectory().data()[i].position.setIdentity();
00205       getZmpTrajectory().data()[i].position[0] = stepFeatures.zmpTrajX[i];
00206       getZmpTrajectory().data()[i].position[1] = stepFeatures.zmpTrajY[i];
00207 
00208       //Posture
00209       //FIXME:
00210       getPostureTrajectory().data()[i].duration = milliseconds(step_ * 1e3);
00211       getPostureTrajectory().data()[i].position.resize(1, 36);
00212       getPostureTrajectory().data()[i].position[0] =
00213         angles::from_degrees (stepFeatures.waistOrient[i]);
00214     }
00215 }


halfsteps_pattern_generator
Author(s): Nicolas Perrin, Thomas Moulard/thomas.moulard@gmail.com
autogenerated on Sat Dec 28 2013 17:05:31