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
00081
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
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
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
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
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
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
00209
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 }