diff --git a/.gitignore b/.gitignore
index 0ae7b6f3ddadfd4415a055db0e5f5ef7de3be4ea..7b65363662207d46ffba35f4b83fbe31c13ff930 100644
--- a/.gitignore
+++ b/.gitignore
@@ -31,28 +31,40 @@ build/
*~
*.swp
_build-DEBUG-CB/
-_build-RELEASE-CB/
_build-RELEASE/
-cmake_metapod_configure_hrp2_14.cmake
-cmake_metapod_configure_simple_humanoid.cmake
-src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp_dirty
-_build-intit-herdt/
-tests/plot.gnu
-tests/plotTmp.gnu
-*.orig
-.gitignore.orig
-CMakeLists.txt.user
-_build-qmake/
-animate.gif
-buffer11_50ms.png
-buffer11_5ms.png
-convexHull.dat
-inc_dec.gnu
-loop.gnu
-plotBuffers.py
-plotBuffers.pyc
-plotBuffersDCOM.py
-plotCoM.gnu
-plot_tmp2.gnu
-_build-DEBUG/
-src/#StepStackHandler.cpp#
+src/Mathematics/PolynomeFoot_backup.cpp
+src/Mathematics/PolynomeFoot_backup.hh
+tests/CMakeCache.txt
+tests/CMakeFiles/
+tests/CPackConfig.cmake
+tests/CPackSourceConfig.cmake
+tests/CTestTestfile.cmake
+tests/DartConfiguration.tcl
+tests/Makefile
+tests/TestHerdt2010DynamicFilter.cpp
+tests/TestHerdt2010_backup.cpp
+tests/TestMorisawaStairs.cpp
+tests/cmake/
+tests/cmake_install.cmake
+tests/config.h
+tests/config.log
+tests/doc/
+tests/include/
+tests/jrl-walkgen.cbp
+tests/jrl-walkgen.pc
+tests/src/
+tests/tests/
+src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp.orig
+src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh.orig
+src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp.orig
+src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp.orig
+src/Mathematics/Bsplines.cpp.orig
+src/Mathematics/Bsplines.hh.orig
+src/PatternGeneratorInterfacePrivate.cpp.orig
+src/StepStackHandler.cpp.orig
+src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp.orig
+src/patterngeneratorinterfaceprivate.hh.orig
+tests/TestBsplines.cpp.orig
+tests/TestHerdt2010.cpp.orig
+
+
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ccf04afd5cc37ddfac3aec2fcb7837eae8866710..a7f815d07e0d9ff5f4af23297c7fb005174d2b7f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,11 +48,13 @@ ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.9.0")
ADD_REQUIRED_DEPENDENCY("abstract-robot-dynamics >= 1.15")
ADD_REQUIRED_DEPENDENCY("jrl-dynamics >= 1.19.1")
-# Optional ones.
+################################################################
ADD_OPTIONAL_DEPENDENCY("hrp2-14 >= 1.8-6")
ADD_OPTIONAL_DEPENDENCY("hrp2-dynamics >= 1.0.1")
ADD_OPTIONAL_DEPENDENCY("metapod >= 1.0.7")
MESSAGE(STATUS "hrp2-dynamics: " ${HRP2_DYNAMICS_FOUND})
+#################################################################
+
# Search for Boost.
# Boost.Test is used by the test suite.
diff --git a/ChangeLog b/ChangeLog
index 9a6d4e67511f680ac15f2f4f4621931fdeb293f9..97c67c77a04906e44bfd4fa1a0e8a71122a598dc 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -2,6 +2,206 @@ CHANGELOG
----------------------
[Current]
+ * Since hrp2 is only needed for the tests, the dependency is declared in the tests.
+ * Synchronize.
+ * Merge pull request #19 from gergondet/topic/fixPGInitialization
+ * synchronize cmake module
+ * Fix intialization to allow restart of Herdt's PG
+ * Fix PG initialization
+ * Add some ostream operators for walkgen structures
+ * Fix source naming inconsistency
+ * Merge pull request #17 from gergondet/topic/fixHerdtFinalPosAndRunningFlag
+ * Correctly set m_Running
+ * Set running value properly for Herdt's PG
+ * Put the CoM at the center of the feet when stopping
+
+[3.1.7]
+ * [cmake] Synchronize
+ * Update datref.cmake for Morisawa walking pattern generator.
+ * Merge branch 'topic/merge-rc-3.1.4'
+ * Synchronize
+ * Add a test for emergency stop.
+ * Remove debug output.
+ * Optimize code and remove initialisation problems (division by zero).
+ * Add test stress on Herdt WPG.
+ * Improve the test validation.
+ * Makes sure that ZMP, Left and Foot trajectory generation are synchronized.
+ * Makes jrl-walkgen less verbose.
+ * Cleaning. Remove unused code.
+ * Update ref values for unit test.
+ * First version with sensible output. The graphs have to be checked.
+ * First compiling version.
+ * Update the dependency in hrp2-14.
+ * Merge branch 'topic/rc-v3.1.4' into topic/merge-rc-3.1.4
+ * Simplify name handling for TestMorisawa2007.
+ * Add TestMorisaw2007ShortWalkTEstFGPI_64.datref.cmake
+ * [CMakeLists.txt] Add MACRO to generate Morisawa 2007 tests.
+ * Fix comparaison between x86_32 and x86_64.
+ * Fix problem related to the robot data path.
+ * [travis] Make cmake verbose
+ * Disable failing tests
+ * Synchronize
+ * Update README.md
+ * [travis] Add Travis support
+ * Fix directory to load default sample.wrl file for unitTesting.
+ * Update lib installation path (multiarch portability).
+ * Synchronize
+ * Revert "Interpolation of height with fifth order polynomials"
+ * Add vim swap files to .gitignore
+ * Synchronize.
+ * Make multi-body mode work
+ * Speed up computation
+ * Speed up add_term()
+ * Change the computation of selection matrices Vc..
+ * Merge branch 'topic/toes' of github.com:jrl-umi3218/jrl-walkgen into topic/toes
+ * Constraints on the com
+ * Merge branch 'topic/rc-v3.1.4 into topic/toes
+ * Enums and convex hull
+ * Remove PLDPHerdt ;(
+ * temporary
+ * Merge branch 'topic/toes' of github.com:jrl-umi3218/jrl-walkgen into toes
+ * change sign of foot constraint area in y-axis
+ * change sign for foot constraint in y-axis
+ * add access function of QP_N
+ * remove dependency of ROBOT
+ * Merge branch 'topic/rc-v3.1.4' into topic/toes
+ * Introduce pointer, rename
+ * Complete the list of maneuvers in TestHerdt2010
+ * Correct insertion of constraints
+ * CoM constraints, enums, clean up
+ * ...missing in the previous commit...
+ * Enums, rotations, clean up,
+ * Warning removal
+ * Improve, clarify and cohere compute_warm_start()
+ * Clean up
+ * Change enum type names and clean up
+ * Update indentation
+ * typo 2
+ * Typo
+ * UseWarmstart must be false if you dont use LSSOL
+ * Merge branch 'topic/rc-v3.1.4' of github.com:jrl-umi3218/jrl-walkgen into rc-v3.1.4
+ * Add the computation of initial solution wich respect all the constraints note : used in LSSOL (QLD don't support this)
+ * correct a segfault in LSSOL
+ * Add a comment
+ * compute the inverse of COP dynamic matrix U
+ * m_Degree must be initialize in the constructor
+ * COP double support constraint was wrong because security margins was counted two times
+ * Initialization + Attribute names
+ * Omit setting of velocity reference :HerdtOnLine
+ * Merge branch 'topic/rc-v3.1.4' of github.com:jrl-umi3218/jrl-walkgen into topic/rc-v3.1.4
+ * Include verification of sup. phase before interpretation
+ * Add a buffer to the modification of velocity reference It's usefull to avoid multi-thread problems
+ * Merge branch 'topic/rc-v3.1.4' of github.com:jrl-umi3218/jrl-walkgen into HEAD
+ * comment the part which concern the multi-body dynamics, because it's unused
+ * A minor bug with parenthesis (did not affect the program behavior)
+ * uninitialized data
+ * First part of warm-start with LSSOL
+ * Add interpolation of trunk orientations for N_ instants
+ * Correct a bug introduced in commit 5efc71.
+ * Add a patch in rigid-body-system to avoid wrong memory access.
+ * Correct test + warning removal.
+ * Move reset to the beginning of online routine
+ * Smaller refactoring
+ * I think there are five arguments...
+ * Add missing initialization in CallMethod
+ * Correct the type of an attribute (should be int rather than double).
+ * Add a patch preventing wrong memory access.
+ * Fix overflow problems inside for loops
+ * Align and complete previous commits
+ * Avoid the fall of the robot if it completed a pure rotation TODO to complete this : Center the CoM at the end of a movement
+ * Rename compute_term into multiply_terms
+ * Optimize and clarify the code
+ * Add a comment in OrientationsPreview class
+ * remove obsolete contents
+ * Correct a minor warning
+ * Documentation
+ * Remove object and warnings
+ * Correct the declaration of verify_angle_hip_joint. One of the parameters cannot be const.
+ * Remove debug traces et restore the use of QLD by default.
+ * Minor corrections in documentation
+ * Correct some rebase errors
+ * Add the second derivative for x,y,z,theta,omega and omega2.
+ * Use Polynomes of degree 5/6 instead of 3/4
+ * Add methods to initialize the Polynome of degree 5/6
+ * Simple rewriting of the code
+ * Add the computation of the second derivative to Polynome
+ * Correct typos
+ * Adding const operator wherever it's possible
+ * The link with lssol is now optional
+ * Add LSSOL solver to qp-problem and add some online tests for these Note : LSSOL sources are not included
+ * Disactivate equality constraints on feet positions
+ * remove cout
+ * Enhance jerk gain
+ * Minor changes
+ * Minor changes
+ * Method + minor bugfix
+ * Add methods for dynamics computation
+ * Add members and documentation
+ * Use of SupportStates_deq
+ * New method, some minor improvements
+ * :finish is deprecated for ZMPVelRef
+ * Minor changes
+ * Move solution interpretation and resize to OLFTG
+ * - Introduce enum type QPElement - Introduce enum type Solver - Remove predefined QP size - The first row of the constraints matrix is now zero!!! - m =
+ * Remove old CoP dynamics computation, doc., cleaning
+ * Change vertical interpolation to two order 5 polynomials
+ * Add computation of three-body cop dynamics
+ * Interpolation of height with fifth order polynomials
+ * Trajectory precomputation
+ * Add NbInstants to support_state_t
+ * see previous commit...
+ * Add precomputation of trajectory
+ * Remove obsolete todos
+ * Add member StepHeight_ and accessors
+ * Add computation of second derivative
+ * Move second derivative computation to parent class
+ * Include foot dynamics + Move declaration of types
+ * See previous commmit
+ * Switch to fifth order polynomials
+ * Move specification of dynamics to rigid-body...
+ * Minor bugfix
+ * Initialization
+ * Initialization
+ * New instance, documentation, pointer destruction
+ * Minor cleaning
+ * Add pointer member to simplified model
+ * Merge branch 'topic/new-classes' into topic/door
+ * Introduce classes for simulation of rigid bodies
+ * Complete unmerged details
+ * Merge topic/rc-v3.1.4
+ * Clean up and restructure slightly
+ * Bugfix
+ * Modify FSM to solve instability issues at the beginning
+ * The door is opening. The frontal constraints have to be verified
+ * Bugfixes, door fixed
+ * Minor changes
+ * PG cancellation and initialization
+ * Add call and move IQPMat
+ * Remove debug message
+ * Accessor, documentation
+ * Add external constraints on the CoM
+ * Merge with topic/rc-v3.1.4
+ * Minor temporary changes
+ * Add method
+ * Smaller refactoring
+ * Bugfix, Warnings, Refactoring
+ * Bugfix and clean up
+ * Bugfix and reformulation
+ * Add computation of external constraints and CoM position dynamics
+ * Add new object Door
+ * Add accessor to the constraints number and bugfix
+ * Add accessor to position matrices
+ * Add external object door
+ * Temporary change of the CoM initialization
+ * Documentation, clean up and bugfix
+ * Reformulated tests for Herdt2010 algorithm.
+ * Typo
+ * Add velocity output for the trunk direction, and bounds on hip joints angular velocity.
+ * Remove memory size display for qp-problem.
+
+[3.1.6]
+ * Remove debian .
+ * Update ChangeLog
* Update tests for Kajita 2003.
* Synchronize
* Fix ZMP computation for multibody correction.
diff --git a/include/jrl/walkgen/pgtypes.hh b/include/jrl/walkgen/pgtypes.hh
index 9624add3abe20a4d0ea7e5e016dc2e2ab75bea32..b54abd7eeac07cdccd8aee2b5b159c00514d7cb4 100644
--- a/include/jrl/walkgen/pgtypes.hh
+++ b/include/jrl/walkgen/pgtypes.hh
@@ -99,15 +99,22 @@ namespace PatternGeneratorJRL
a sequence of relative positions. */
struct RelativeFootPosition_s
{
- double sx,sy,theta;
+ double sx,sy,sz,theta;
double SStime;
double DStime;
int stepType; //1:normal walking 2:one step before obstacle
- //3:first leg over obstacle 4:second leg over obstacle 5:one step after obstacle
+ //3:first leg over obstacle 4:second leg over obstacle 5:one step after obstacle 6 :stepping stair
double DeviationHipHeight;
};
typedef struct RelativeFootPosition_s RelativeFootPosition;
+ inline std::ostream & operator<<(std::ostream & os, const RelativeFootPosition_s & rfp)
+ {
+ os << "sx " << rfp.sx << " sy " << rfp.sy << " theta " << rfp.theta << std::endl;
+ os << "SStime " << rfp.SStime << " DStime " << rfp.DStime << " stepType " << rfp.stepType << " DeviationHipHeight " << rfp.DeviationHipHeight;
+ return os;
+ }
+
/** Structure to store each of the ZMP value, with a given
direction at a certain time. */
struct ZMPPosition_s
@@ -122,6 +129,13 @@ namespace PatternGeneratorJRL
};
typedef struct ZMPPosition_s ZMPPosition;
+ inline std::ostream & operator<<(std::ostream & os, const ZMPPosition_s & zmp)
+ {
+ os << "px " << zmp.px << " py " << zmp.pz << " pz " << zmp.pz << " theta " << zmp.theta << std::endl;
+ os << "time " << zmp.time << " stepType " << zmp.stepType;
+ return os;
+ }
+
/// Structure to store the absolute foot position.
struct FootAbsolutePosition_t
{
@@ -143,6 +157,15 @@ namespace PatternGeneratorJRL
};
typedef struct FootAbsolutePosition_t FootAbsolutePosition;
+ inline std::ostream & operator<<(std::ostream & os, const FootAbsolutePosition & fap)
+ {
+ os << "x " << fap.x << " y " << fap.y << " z " << fap.z << " theta " << fap.theta << " omega " << fap.omega << " omega2 " << fap.omega2 << std::endl;
+ os << "dx " << fap.dx << " dy " << fap.dy << " dz " << fap.dz << " dtheta " << fap.dtheta << " domega " << fap.domega << " domega2 " << fap.domega2 << std::endl;
+ os << "ddx " << fap.ddx << " ddy " << fap.ddy << " ddz " << fap.ddz << " ddtheta " << fap.ddtheta << " ddomega " << fap.ddomega << " ddomega2 " << fap.ddomega2 << std::endl;
+ os << "time " << fap.time << " stepType " << fap.stepType;
+ return os;
+ }
+
// Linear constraint.
struct LinearConstraintInequality_s
{
@@ -174,6 +197,13 @@ namespace PatternGeneratorJRL
typedef struct SupportFeet_s
SupportFeet_t;
+ inline std::ostream & operator<<(std::ostream & os, const SupportFeet_s & sf)
+ {
+ os << "x " << sf.x << " y " << sf.y << " theta " << sf.theta << std::endl;
+ os << " StartTime " << sf.StartTime << " SupportFoot " << sf.SupportFoot;
+ return os;
+ }
+
/// Structure to store the absolute reference.
struct ReferenceAbsoluteVelocity_t
{
@@ -187,5 +217,11 @@ namespace PatternGeneratorJRL
};
typedef struct ReferenceAbsoluteVelocity_t ReferenceAbsoluteVelocity;
+ inline std::ostream & operator<<(std::ostream & os, const ReferenceAbsoluteVelocity_t & rav)
+ {
+ os << "x " << rav.x << " y " << rav.y << " z " << rav.z << " dYaw " << rav.dYaw;
+ return os;
+ }
+
}
#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e109f0d59afc26dfb87c37af64e6788dcbbe14f3..a3d286e841cf6138d749dd2b9700e30089fcea41 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -46,6 +46,7 @@ SET(SOURCES
# Mathematics/FootConstraintsAsLinearSystemForVelRef.cpp
Mathematics/FootHalfSize.cpp
Mathematics/OptCholesky.cpp
+ Mathematics/Bsplines.cpp
Mathematics/Polynome.cpp
Mathematics/PolynomeFoot.cpp
Mathematics/PLDPSolver.cpp
@@ -84,8 +85,8 @@ SET(SOURCES
PatternGeneratorInterfacePrivate.cpp
SimplePlugin.cpp
SimplePluginManager.cpp
+ pgtypes.cpp
Clock.cpp
- PGTypes.cpp
portability/gettimeofday.cc
privatepgtypes.cpp
)
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
index c7de348265db1fb43a3fa8f14a4b919584abcaf8..b4ab77e9486f585fc97a4e4e4a924534861209d5 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
@@ -34,16 +34,19 @@
using namespace PatternGeneratorJRL;
FootTrajectoryGenerationAbstract::FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM,
- CjrlFoot *aFoot)
+ CjrlFoot *aFoot)
: SimplePlugin(lSPM)
{
m_Omega = 0.0;
- m_Foot= aFoot;
+ m_Foot= aFoot;
m_SamplingPeriod = 0.005;
+ m_isStepStairOn = 1;
+ m_StepHeight = 0.07;
- std::string aMethodName[5] =
+
+ std::string aMethodName[5] =
{":omega",
- ":stepheight",
+ ":stepheight",
":singlesupporttime",
":doublesupporttime",
":samplingperiod"};
@@ -84,10 +87,10 @@ void FootTrajectoryGenerationAbstract::CallMethod(std::string &Method,
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & , //SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &, //NoneSupportFootAbsolutePositions,
- int , //CurrentAbsoluteIndex,
- int , //IndexInitial,
+ int , //CurrentAbsoluteIndex,
+ int , //IndexInitial,
double , //ModulatedSingleSupportTime,
- int , //StepType,
+ int , //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-1: To be implemented ");
@@ -95,12 +98,13 @@ void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolut
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & ,//SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> & ,//NoneSupportFootAbsolutePositions,
- int , // StartIndex,
+ int , // StartIndex,
int , //k,
double , //LocalInterpolationStartTime,
double , //ModulatedSingleSupportTime,
- int , //StepType,
+ int , //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-2: To be implemented ");
}
+
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
index ef4ae45c9f50e7054ab798d8a39543b1502faa7c..5089b10027f3ef5333c46db98ca2049eecccebc4 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
@@ -52,14 +52,14 @@ namespace PatternGeneratorJRL
/** @ingroup foottrajectorygeneration
This class defines the abstract interface to interact with foot generation object.
- Two parameters \f$ T_{DS} \f$ and \f$ T_{SS} \f$ defines respectively
+ Two parameters \f$ T_{DS} \f$ and \f$ T_{SS} \f$ defines respectively
the double support time and the single support time.
\f$ \omega \f$ defines the angle of the feet for landing and taking off.
- The foot rotates around the toe from \f$ 0 \f$ to \f$ \omega \f$
- for taking off. Whereas for the landing the foot rotates around the
+ The foot rotates around the toe from \f$ 0 \f$ to \f$ \omega \f$
+ for taking off. Whereas for the landing the foot rotates around the
heel from \f$ \omega \f$ to \f$ 0 \f$.
-
+
The sampling control time indicates the amount of time between two
iteration of the algorithm. This parameter is used in the method
UpdateFootPosition to compute the time from the iteration number
@@ -68,21 +68,21 @@ namespace PatternGeneratorJRL
An instance of a class derived from FootTrajectoryGenerationAbstract,
should call InitializeInternalDataStructures() once all the internal
parameters of the object are set.
-
+
The virtual function FreeInternalDataStructures() is used when changing some
parameters and by the destructor.
The most important function is UpdateFootPosition() which populates a
- queue of foot absolute positions data structure.
+ queue of foot absolute positions data structure.
+
+ See a derived class such as FootTrajectoryGenerationStandard
+ for more precise information on the usage and sample codes.
- See a derived class such as FootTrajectoryGenerationStandard
- for more precise information on the usage and sample codes.
-
*/
class FootTrajectoryGenerationAbstract :public SimplePlugin
{
public:
-
+
/*! Constructor: In order to compute some appropriate strategies,
this class needs to extract specific details from the humanoid model. */
FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM,
@@ -99,7 +99,7 @@ namespace PatternGeneratorJRL
This position is supposed to be constant.
@param NoneSupportFootAbsolutePositions: Queue of absolute position for the swinging
foot. This method will set the foot position at index NoneSupportFootAbsolutePositions
- of the queue.
+ of the queue.
@param CurrentAbsoluteIndex: Index in the queues of the foot position to be set.
@param IndexInitial: Index in the queues which correspond to the starting point
of the current single support phase.
@@ -109,8 +109,8 @@ namespace PatternGeneratorJRL
*/
virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
- int CurrentAbsoluteIndex,
- int IndexInitial,
+ int CurrentAbsoluteIndex,
+ int IndexInitial,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
@@ -123,56 +123,56 @@ namespace PatternGeneratorJRL
/*! Initialize internal data structures. */
virtual void InitializeInternalDataStructures()=0;
-
+
/*! Free internal data structures */
virtual void FreeInternalDataStructures()=0;
- /*! \name Setter and getter for parameters
+ /*! \name Setter and getter for parameters
@{
*/
-
- /*! \name Single Support Time
+
+ /*! \name Single Support Time
@{
*/
-
+
/*! \brief Set single support time */
void SetSingleSupportTime(double aTSingle)
{ m_TSingle =aTSingle;};
-
+
/*! \brief Get single support time */
double GetSingleSupportTime() const
{ return m_TSingle;};
/*! @} */
- /*! \name Double Support Time
+ /*! \name Double Support Time
@{
*/
/*! \brief Set double support time */
void SetDoubleSupportTime(double aTDouble)
{ m_TDouble =aTDouble;};
-
+
/*! \brief Get single support time */
double GetDoubleSupportTime() const
{ return m_TDouble;};
/*! @}*/
- /*! \name Sampling control Time
+ /*! \name Sampling control Time
@{
*/
-
+
/*! \brief Set single support time */
void SetSamplingPeriod(double aSamplingPeriod)
{ m_SamplingPeriod = aSamplingPeriod;};
-
+
/*! \brief Get single support time */
double GetSamplingPeriod() const
{ return m_SamplingPeriod;};
/*!@}*/
-
- /*! \name Omega.
+
+ /*! \name Omega.
@{*/
/*! Get Omega */
@@ -182,17 +182,24 @@ namespace PatternGeneratorJRL
/*! Set Omega */
void SetOmega(double anOmega)
{ m_Omega = anOmega; };
-
+
+ int GetSetStepStairOn() const
+ { return m_isStepStairOn; };
+
+ void SetStepStairOn(int aSSO)
+ { m_isStepStairOn= aSSO;};
+
/*! @} */
/*! @} */
- /*! \brief Reimplementation of the call method for the plugin manager.
+ /*! \brief Reimplementation of the call method for the plugin manager.
More explicitly this object will deal with the call which initialize
the feet behaviors (\f$omega\f$, \f$ stepheight \f$) .
*/
virtual void CallMethod(std::string &Method, std::istringstream &strm);
- protected:
+
+ protected:
/*! Sampling period of the control. */
double m_SamplingPeriod;
@@ -205,12 +212,17 @@ namespace PatternGeneratorJRL
/*! Store a pointer to Foot information. */
CjrlFoot * m_Foot;
-
+
/*! Omega the angle for taking off and landing. */
double m_Omega;
+
+ int m_isStepStairOn;
+
+ double m_StepHeight;
+
};
-
-
+
+
}
#endif /* _FOOT_TRAJECTORY_GENERATION_ABSTRACT_H_ */
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
index 51077a0113a3e4521c9b2108476f4b25c581dafe..758a39ca1fcea2ccd563f02d65b2d13629ec9b44 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Torea Foissotte
* Olivier Stasse
@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
@@ -52,21 +52,21 @@ void FootTrajectoryGenerationMultiple::SetNumberOfIntervals(int lNumberOfInterva
{
if (m_SetOfFootTrajectoryGenerationObjects.size()==(unsigned int)lNumberOfIntervals)
return;
-
+
for(unsigned int i=0;i<m_SetOfFootTrajectoryGenerationObjects.size();i++)
{
delete m_SetOfFootTrajectoryGenerationObjects[i];
}
-
+
m_SetOfFootTrajectoryGenerationObjects.resize(lNumberOfIntervals);
for(unsigned int i=0;i<m_SetOfFootTrajectoryGenerationObjects.size();i++)
{
m_SetOfFootTrajectoryGenerationObjects[i] = new FootTrajectoryGenerationStandard(getSimplePluginManager(),m_Foot);
m_SetOfFootTrajectoryGenerationObjects[i]->InitializeInternalDataStructures();
- }
+ }
m_NatureOfIntervals.resize(lNumberOfIntervals);
}
-
+
int FootTrajectoryGenerationMultiple::GetNumberOfIntervals() const
{
return m_SetOfFootTrajectoryGenerationObjects.size();
@@ -78,14 +78,14 @@ void FootTrajectoryGenerationMultiple::SetTimeIntervals(const vector<double> &lD
m_DeltaTj = lDeltaTj;
m_RefTime.resize(lDeltaTj.size());
double reftime=0.0;
-
+
for(unsigned int li=0;li<m_DeltaTj.size();li++)
{
m_RefTime[li] = reftime;
ODEBUG(" m_RefTime["<< li <<"]: " << setprecision(12) << m_RefTime[li] << " reftime: "<< setprecision(12) << reftime );
reftime+=m_DeltaTj[li];
}
-
+
}
void FootTrajectoryGenerationMultiple::GetTimeIntervals(vector<double> &lDeltaTj) const
@@ -100,27 +100,27 @@ bool FootTrajectoryGenerationMultiple::Compute(int axis, double t, double &resul
double reftime=0;
ODEBUG(" ====== CoM ====== ");
ODEBUG(" t: " << t << " reftime :" << reftime << " m_Sensitivity: "<< m_Sensitivity <<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
-
+
for(unsigned int j=0;j<m_DeltaTj.size();j++)
{
ODEBUG(" t: " << t << " reftime :" << reftime << " Tj["<<j << "]= "<< m_DeltaTj[j]);
-
+
if (((t+m_Sensitivity)>=reftime) && (t<=reftime+m_DeltaTj[j]+m_Sensitivity))
{
double deltaj=0.0;
deltaj = t-reftime;
-
+
if (m_SetOfFootTrajectoryGenerationObjects[j]!=0)
{
result = m_SetOfFootTrajectoryGenerationObjects[j]->Compute(axis,deltaj);
}
return true;
}
-
+
reftime+=m_DeltaTj[j];
}
ODEBUG(" reftime :" << reftime );
-
+
return false;
}
@@ -129,62 +129,73 @@ bool FootTrajectoryGenerationMultiple::Compute(double t, FootAbsolutePosition &
{
double deltaj = t - m_AbsoluteTimeReference - m_RefTime[IndexInterval];
ODEBUG("IndexInterval : " << IndexInterval );
+
m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->ComputeAll(aFootAbsolutePosition,deltaj);
aFootAbsolutePosition.stepType = m_NatureOfIntervals[IndexInterval];
+
+ /* ofstream aof;
+ string aFileName;
+
+ aFileName ="ex.dat";
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof << "deltaj " << deltaj << " t " << t << " m_Absoulute " << m_AbsoluteTimeReference << " Ref " << m_RefTime[IndexInterval] << " j " << IndexInterval << " foot "<< aFootAbsolutePosition.x << " "<< aFootAbsolutePosition.z << endl;
+ aof.close();*/
+
return true;
}
+
bool FootTrajectoryGenerationMultiple::Compute(double t, FootAbsolutePosition & aFootAbsolutePosition)
{
t -= m_AbsoluteTimeReference;
double reftime=0;
ODEBUG(" ====== Foot ====== " << m_DeltaTj.size());
- ODEBUG("t: " << setprecision(12) << t << " reftime :" << reftime <<
+ ODEBUG("t: " << setprecision(12) << t << " reftime :" << reftime <<
" m_Sensitivity: "<< m_Sensitivity <<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
-
+
for(unsigned int j=0;j<m_DeltaTj.size();j++)
{
ODEBUG("t: " << t << " reftime :" << setprecision(12) << reftime <<
- " Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j]
+ " Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j]
<<" max limit: " << setprecision(12) << (reftime+m_DeltaTj[j]+m_Sensitivity) );
ODEBUG(" Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j] );
if (((t+m_Sensitivity)>=reftime) && (t<=reftime+m_DeltaTj[j]+m_Sensitivity))
{
double deltaj=0.0;
deltaj = t-reftime;
-
+
if (m_SetOfFootTrajectoryGenerationObjects[j]!=0)
- {
+ {
m_SetOfFootTrajectoryGenerationObjects[j]->ComputeAll(aFootAbsolutePosition,deltaj);
aFootAbsolutePosition.stepType = m_NatureOfIntervals[j];
}
ODEBUG("t: " << t << " reftime :" << setprecision(12) << reftime <<
" AbsoluteTimeReference : " << m_AbsoluteTimeReference <<
- " Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j]
+ " Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j]
<<" max limit: " << setprecision(12) << (reftime+m_DeltaTj[j]+m_Sensitivity) );
- ODEBUG("X: " << aFootAbsolutePosition.x <<
- " Y: " << aFootAbsolutePosition.y <<
- " Z: " << aFootAbsolutePosition.z <<
+ ODEBUG("X: " << aFootAbsolutePosition.x <<
+ " Y: " << aFootAbsolutePosition.y <<
+ " Z: " << aFootAbsolutePosition.z <<
" Theta: " << aFootAbsolutePosition.theta <<
- " Omega: " << aFootAbsolutePosition.omega <<
- " stepType: " << aFootAbsolutePosition.stepType <<
+ " Omega: " << aFootAbsolutePosition.omega <<
+ " stepType: " << aFootAbsolutePosition.stepType <<
" NI: " << m_NatureOfIntervals[j] <<
" interval : " << j);
return true;
}
-
+
reftime+=m_DeltaTj[j];
}
ODEBUG(" reftime :" << reftime <<
" m_AbsoluteReferenceTime" << m_AbsoluteTimeReference);
- ODEBUG("t: " << setprecision(12) << t << " reftime :" << reftime <<
+ ODEBUG("t: " << setprecision(12) << t << " reftime :" << reftime <<
" m_Sensitivity: "<< m_Sensitivity <<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
return false;
}
-/*! This method specifies the nature of the interval.
+/*! This method specifies the nature of the interval.
*/
int FootTrajectoryGenerationMultiple::SetNatureInterval(unsigned int IntervalIndex,
int Nature)
@@ -193,24 +204,24 @@ int FootTrajectoryGenerationMultiple::SetNatureInterval(unsigned int IntervalInd
return -1;
m_NatureOfIntervals[IntervalIndex] = Nature;
return 0;
-
+
}
-/*! This method returns the nature of the interval.
+/*! This method returns the nature of the interval.
*/
int FootTrajectoryGenerationMultiple::GetNatureInterval(unsigned int IntervalIndex) const
{
if (IntervalIndex>=m_NatureOfIntervals.size())
return -100;
-
+
return m_NatureOfIntervals[IntervalIndex];
}
/*! This method specifies the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param PolynomeIndex: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param PolynomeIndex: Set to which axis the parameters will be applied.
@param TimeInterval: Set the time base of the polynome.
@param Position: Set the final position of the polynome at TimeInterval.
@param InitPosition: Initial position when computing the polynome at t=0.0.
@@ -226,7 +237,6 @@ int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeed(unsigned
if (IntervalIndex>=m_SetOfFootTrajectoryGenerationObjects.size())
return -1;
-
m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->SetParametersWithInitPosInitSpeed(AxisReference,
TimeInterval,
FinalPosition,
@@ -237,8 +247,8 @@ int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeed(unsigned
/*! This method specifies the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param PolynomeIndex: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param PolynomeIndex: Set to which axis the parameters will be applied.
@param TimeInterval: Set the time base of the polynome.
@param Position: Set the final position of the polynome at TimeInterval.
*/
@@ -291,8 +301,8 @@ int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeedInitAcc(u
/*! This method get the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param PolynomeIndex: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param PolynomeIndex: Set to which axis the parameters will be applied.
@param TimeInterval: Set the time base of the polynome.
@param Position: Set the final position of the polynome at TimeInterval.
@param InitPosition: Initial position when computing the polynome at t=0.0.
@@ -332,7 +342,7 @@ void FootTrajectoryGenerationMultiple::SetAbsoluteTimeReference(double lAbsolute
void FootTrajectoryGenerationMultiple::CallMethod(std::string &, //Method,
std::istringstream & ) //strm)
{
-
+
}
int FootTrajectoryGenerationMultiple::DisplayIntervals() const
{
@@ -343,7 +353,7 @@ int FootTrajectoryGenerationMultiple::DisplayIntervals() const
return 0;
}
-FootTrajectoryGenerationMultiple &
+FootTrajectoryGenerationMultiple &
FootTrajectoryGenerationMultiple::operator=
(const FootTrajectoryGenerationMultiple & aFTGM)
{
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh
index ba984cd92edbd03762d4be62afe6a3c8aa050b5b..45bc05f31b457fcf99c5080dea92b0401da01cdc 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Torea Foissotte
* Olivier Stasse
@@ -19,13 +19,13 @@
* You should have received a copy of the GNU Lesser General Public License
* along with jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file FootTrajectoryGenerationMultiple.h
\brief This object is in charge of maintaining the foot trajectory
- generation for several intervals.
+ generation for several intervals.
It relies on the FootTrajectoryGenerationStandard class.
@ingroup foottrajectorygeneration
@@ -41,7 +41,7 @@ namespace PatternGeneratorJRL
{
/*! @ingroup foottrajectorygeneration
-
+
This class generates a trajectory for a complete leg relying on a
set of description of the intervals.
More precisely this object handles a set of FootTrajectoryGenerationStandard
@@ -50,10 +50,10 @@ namespace PatternGeneratorJRL
of foot trajectory.
Each interval is described by a time duration \f[ \Delta T_j \f],
- its nature which can be double support and in single support
+ its nature which can be double support and in single support
two subcategories exist: support foot and flying.
-
-
+
+
*/
class FootTrajectoryGenerationMultiple : public SimplePlugin
{
@@ -81,14 +81,14 @@ namespace PatternGeneratorJRL
// Default destructor
~FootTrajectoryGenerationMultiple();
-
- /*! \brief Reimplementation of the call method for the plugin manager.
+
+ /*! \brief Reimplementation of the call method for the plugin manager.
More explicitly this object will deal with the call which initialize
the feet behaviors (\f$omega\f$, \f$ stepheight \f$) .
*/
virtual void CallMethod(std::string &Method, std::istringstream &strm);
-
+
/*! \name Methods related to the handling of the intervals.
@{
*/
@@ -100,7 +100,7 @@ namespace PatternGeneratorJRL
/*! \brief Set the time for each interval. */
void SetTimeIntervals(const std::vector<double> & lDeltaTj);
-
+
/*! \brief Get the time for each interval */
void GetTimeIntervals(std::vector<double> & lDeltaTj) const;
@@ -112,16 +112,16 @@ namespace PatternGeneratorJRL
/*! \brief Display intervals time. */
int DisplayIntervals() const;
-
+
/*! @} */
-
+
/*! \brief Compute the value asked for according to :
@param[in] axis: the axis along which the computation is done,
@param[in] t: the time,
@param[out] r: the result.
*/
bool Compute(int axis, double t, double & r);
-
+
/*! \brief Compute the value asked for according to :
@param[in] t: the time,
@param[out] aFootAbsolutePosition: a foot absolute position.
@@ -137,8 +137,8 @@ namespace PatternGeneratorJRL
/*! This method specifies the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param PolynomeIndex: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param PolynomeIndex: Set to which axis the parameters will be applied.
@param TimeInterval: Set the time base of the polynome.
@param Position: Set the final position of the polynome at TimeInterval.
*/
@@ -146,11 +146,11 @@ namespace PatternGeneratorJRL
int AxisReference,
double TimeInterval,
double FinalPosition);
-
+
/*! This method specifies the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param PolynomeIndex: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param PolynomeIndex: Set to which axis the parameters will be applied.
@param AxisReference: Index to the axis to be used.
@param TimeInterval: Set the time base of the polynome.
@param FinalPosition: Set the final position of the polynome at TimeInterval.
@@ -186,8 +186,8 @@ namespace PatternGeneratorJRL
/*! This method gets the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param PolynomeIndex: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param PolynomeIndex: Set to which axis the parameters will be applied.
@param AxisReference: Index to the axis to be used.
@param TimeInterval: Set the time base of the polynome.
@param FinalPosition: Set the final position of the polynome at TimeInterval.
@@ -201,8 +201,8 @@ namespace PatternGeneratorJRL
double &InitPosition,
double &InitSpeed);
- /*! \name Methods related to the Absolute Time Reference.
- This time specifies the beginning of the trajectory.
+ /*! \name Methods related to the Absolute Time Reference.
+ This time specifies the beginning of the trajectory.
@{ */
/*! Returns the time when the trajectory starts. */
@@ -210,9 +210,9 @@ namespace PatternGeneratorJRL
/*! Set the time when the trajectory starts. */
void SetAbsoluteTimeReference(double lAbsoluteTimeReference);
-
+
/*! @} */
-
+
FootTrajectoryGenerationMultiple & operator=(const FootTrajectoryGenerationMultiple & aFTGM);
protected:
@@ -220,7 +220,7 @@ namespace PatternGeneratorJRL
/*! \brief Handle a set of object allowing the generation of the foot trajectory.*/
std::vector<FootTrajectoryGenerationStandard *> m_SetOfFootTrajectoryGenerationObjects;
- /*! \brief Reference of humanoid specificities. */
+ /*! \brief Reference of humanoid specificities. */
CjrlFoot * m_Foot;
/*! \brief Set the absolute reference time for this set of intervals. */
@@ -234,7 +234,7 @@ namespace PatternGeneratorJRL
/*! \brief Reference time for the polynomials. */
std::vector<double> m_RefTime;
-
+
/*! \brief Sensitivity to numerical unstability when using time. */
double m_Sensitivity;
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 052e48dd2c0635007b9b45a3390902cf0e717713..5fba3aacbd6a215fc5db2b7c5817398d27e7678a 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
@@ -19,13 +19,13 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* This object generate all the values for the foot trajectories, */
#include <iostream>
#include <fstream>
-
+#include <math.h>
#include <Debug.hh>
#include "FootTrajectoryGenerationStandard.hh"
@@ -41,11 +41,12 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
m_PolynomeX = 0;
m_PolynomeY = 0;
m_PolynomeZ = 0;
+ m_BsplinesZ = 0;
m_PolynomeOmega = 0;
m_PolynomeOmega2 = 0;
m_PolynomeTheta = 0;
- /* Computes information on foot dimension
+ /* Computes information on foot dimension
from humanoid specific informations. */
double lWidth,lHeight,lDepth;
if (m_Foot!=0)
@@ -73,7 +74,7 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
m_AnklePositionRight[0] = -lDepth*0.5 + AnklePosition[0];
m_AnklePositionRight[1] = lWidth*0.5 - AnklePosition[1];
m_AnklePositionRight[2] = AnklePosition[2];
-
+
/* Compute Left foot coordinates */
if (m_Foot!=0)
{
@@ -96,13 +97,16 @@ FootTrajectoryGenerationStandard::~FootTrajectoryGenerationStandard()
{
if (m_PolynomeX!=0)
delete m_PolynomeX;
-
+
if (m_PolynomeY!=0)
delete m_PolynomeY;
if (m_PolynomeZ!=0)
delete m_PolynomeZ;
+ if (m_BsplinesZ!=0)
+ delete m_BsplinesZ;
+
if (m_PolynomeOmega!=0)
delete m_PolynomeOmega;
@@ -116,10 +120,11 @@ FootTrajectoryGenerationStandard::~FootTrajectoryGenerationStandard()
void FootTrajectoryGenerationStandard::InitializeInternalDataStructures()
{
FreeInternalDataStructures();
-
- m_PolynomeX = new Polynome7(0,0);
- m_PolynomeY = new Polynome7(0,0);
+
+ m_PolynomeX = new Polynome5(0,0);
+ m_PolynomeY = new Polynome5(0,0);
m_PolynomeZ = new Polynome4(0,0);
+ m_BsplinesZ = new ZBsplines(0,0,0,0);
m_PolynomeOmega = new Polynome3(0,0);
m_PolynomeOmega2 = new Polynome3(0,0);
m_PolynomeTheta = new Polynome5(0,0);
@@ -136,6 +141,9 @@ void FootTrajectoryGenerationStandard::FreeInternalDataStructures()
if (m_PolynomeZ!=0)
delete m_PolynomeZ;
+ if (m_BsplinesZ!=0)
+ delete m_BsplinesZ;
+
if (m_PolynomeOmega!=0)
delete m_PolynomeOmega;
@@ -153,17 +161,18 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex,
{
switch (PolynomeIndex)
{
-
+
case X_AXIS:
m_PolynomeX->SetParameters(TimeInterval,Position);
break;
-
+
case Y_AXIS:
m_PolynomeY->SetParameters(TimeInterval,Position);
break;
case Z_AXIS:
m_PolynomeZ->SetParameters(TimeInterval,Position);
+ m_BsplinesZ->SetParameters(TimeInterval,Position,TimeInterval/3.0,Position + m_StepHeight);
break;
case THETA_AXIS:
@@ -193,17 +202,31 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
{
switch (PolynomeIndex)
{
+
case X_AXIS:
ODEBUG2("Initspeed: " << InitSpeed << " ");
- m_PolynomeX->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeX->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
break;
-
+
case Y_AXIS:
- m_PolynomeY->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeY->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
break;
case Z_AXIS:
- m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+
+ m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+
+ //cout <<(FinalPosition - InitPosition) << " " << m_StepHeight << " "<<((FinalPosition - InitPosition) == m_StepHeight) << endl;
+
+ if ((FinalPosition - InitPosition) == m_StepHeight)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,InitPosition,0.5*TimeInterval,InitPosition+m_StepHeight);
+ else if (FinalPosition > InitPosition)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,1.5*TimeInterval/5.0,FinalPosition+m_StepHeight);//+abs(FinalPosition-InitPosition)*1.5);
+ else if (FinalPosition == InitPosition)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,0.5*TimeInterval,FinalPosition);//+abs(FinalPosition-InitPosition)*1.5);
+ else if (FinalPosition < InitPosition)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,6.0*TimeInterval/10.0,InitPosition+m_StepHeight/3.0);//abs(FinalPosition-InitPosition)*0.3);
+
break;
case THETA_AXIS:
@@ -240,8 +263,22 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
break;
case Z_AXIS:
+ // if (m_isStepStairOn == 0)
+ //{
m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- break;
+ //}
+ //else
+ //{
+ if ((FinalPosition - InitPosition) == m_StepHeight)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,InitPosition,0.5*TimeInterval,InitPosition+m_StepHeight);
+ else if (FinalPosition > InitPosition)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,1.5*TimeInterval/5.0,FinalPosition+m_StepHeight);//InitPosition+abs(FinalPosition-InitPosition)*1.5);
+ else if (FinalPosition == InitPosition)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,0.5*TimeInterval,FinalPosition);//InitPosition+abs(FinalPosition-InitPosition)*1.5);
+ else if (FinalPosition < InitPosition)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,6.0*TimeInterval/10.0,InitPosition+m_StepHeight/3.0);//+abs(FinalPosition-InitPosition)*0.3);
+
+ break;
case THETA_AXIS:
m_PolynomeTheta->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
@@ -270,19 +307,22 @@ int FootTrajectoryGenerationStandard::GetParametersWithInitPosInitSpeed(int Poly
{
switch (PolynomeIndex)
{
-
+
case X_AXIS:
ODEBUG2("Initspeed: " << InitSpeed << " ");
m_PolynomeX->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
-
+
case Y_AXIS:
m_PolynomeY->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
case Z_AXIS:
m_PolynomeZ->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- break;
+
+ m_BsplinesZ->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+
+ break;
case THETA_AXIS:
m_PolynomeTheta->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
@@ -303,21 +343,33 @@ int FootTrajectoryGenerationStandard::GetParametersWithInitPosInitSpeed(int Poly
return 0;
}
+
+
+
+
double FootTrajectoryGenerationStandard::ComputeAll(FootAbsolutePosition & aFootAbsolutePosition,
double Time)
{
aFootAbsolutePosition.x = m_PolynomeX->Compute(Time);
aFootAbsolutePosition.dx = m_PolynomeX->ComputeDerivative(Time);
aFootAbsolutePosition.ddx = m_PolynomeX->ComputeSecDerivative(Time);
- ODEBUG2("t: " << Time << " : " << aFootAbsolutePosition.x);
+ ODEBUG2("t: " << Time << " : " << aFootAbsolutePosition.x);
aFootAbsolutePosition.y = m_PolynomeY->Compute(Time);
aFootAbsolutePosition.dy = m_PolynomeY->ComputeDerivative(Time);
aFootAbsolutePosition.ddy = m_PolynomeY->ComputeSecDerivative(Time);
- aFootAbsolutePosition.z = m_PolynomeZ->Compute(Time);
- aFootAbsolutePosition.dz = m_PolynomeZ->ComputeDerivative(Time);
- aFootAbsolutePosition.ddz = m_PolynomeZ->ComputeSecDerivative(Time);
+ if (m_isStepStairOn == 0)
+ {
+ aFootAbsolutePosition.z = m_PolynomeZ->Compute(Time);
+ aFootAbsolutePosition.dz = m_PolynomeZ->ComputeDerivative(Time);
+ aFootAbsolutePosition.ddz = m_PolynomeZ->ComputeSecDerivative(Time);
+ }
+ else
+ {
+ aFootAbsolutePosition.z = m_BsplinesZ->ZComputePosition(Time);
+ aFootAbsolutePosition.dz = m_BsplinesZ->ZComputeVelocity(Time);
+ }
aFootAbsolutePosition.theta = m_PolynomeTheta->Compute(Time);
aFootAbsolutePosition.dtheta = m_PolynomeTheta->ComputeDerivative(Time);
@@ -338,17 +390,20 @@ double FootTrajectoryGenerationStandard::Compute(unsigned int PolynomeIndex, dou
switch (PolynomeIndex)
{
-
+
case X_AXIS:
r=m_PolynomeX->Compute(Time);
break;
-
+
case Y_AXIS:
r=m_PolynomeY->Compute(Time);
break;
case Z_AXIS:
+ if (m_isStepStairOn == 0)
r=m_PolynomeZ->Compute(Time);
+ else
+ r=m_BsplinesZ->ZComputePosition(Time);
break;
case THETA_AXIS:
@@ -386,7 +441,11 @@ double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int Polyn
break;
case Z_AXIS:
+ if (m_isStepStairOn == 0)
r=m_PolynomeZ->ComputeSecDerivative(Time);
+ else
+ r=m_BsplinesZ->ZComputeAcc(Time);
+
break;
case THETA_AXIS:
@@ -410,10 +469,10 @@ double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int Polyn
void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
- int CurrentAbsoluteIndex,
- int IndexInitial,
+ int CurrentAbsoluteIndex,
+ int IndexInitial,
double ModulatedSingleSupportTime,
- int StepType,
+ int StepType,
int /* LeftOrRight */)
{
unsigned int k = CurrentAbsoluteIndex - IndexInitial;
@@ -423,7 +482,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
double StartLanding = EndOfLiftOff + ModulatedSingleSupportTime;
// The foot support does not move.
- SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
+ SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
SupportFootAbsolutePositions[CurrentAbsoluteIndex-1];
SupportFootAbsolutePositions[CurrentAbsoluteIndex].stepType = (-1)*StepType;
@@ -432,7 +491,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
const FootAbsolutePosition & init_NSFAP = NoneSupportFootAbsolutePositions[IndexInitial];
curr_NSFAP.stepType = StepType;
-
+
if (LocalTime < EndOfLiftOff)
{
// Do not modify x, y and theta while liftoff.
@@ -447,7 +506,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
curr_NSFAP.y = init_NSFAP.y + m_PolynomeY->Compute(LocalTime - EndOfLiftOff);
curr_NSFAP.theta = init_NSFAP.theta + m_PolynomeTheta->Compute(LocalTime - EndOfLiftOff);
}
- else
+ else
{
// Do not modify x, y and theta while landing.
curr_NSFAP.x = init_NSFAP.x + m_PolynomeX->Compute(ModulatedSingleSupportTime);
@@ -455,11 +514,15 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
curr_NSFAP.theta = init_NSFAP.theta + m_PolynomeTheta->Compute(ModulatedSingleSupportTime);
}
+ if (m_isStepStairOn == 0)
curr_NSFAP.z = init_NSFAP.z + m_PolynomeZ->Compute(LocalTime);
+ else
+ curr_NSFAP.z = init_NSFAP.z + m_BsplinesZ->ZComputePosition(LocalTime);
+
ODEBUG2("x:" << curr_NSFAP.x << " LocalTime - EndOfLiftOff" << LocalTime - EndOfLiftOff
<< " " << m_PolynomeX->Compute(LocalTime - EndOfLiftOff));
// m_PolynomeX->print();
-
+
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
@@ -476,7 +539,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
m_Omega - m_PolynomeOmega2->Compute(LocalTime-EndOfLiftOff);
}
// Realize the landing.
- else
+ else
{
curr_NSFAP.omega =
m_PolynomeOmega->Compute(LocalTime - StartLanding) - m_Omega;
@@ -494,7 +557,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
// Make sure the foot is not going inside the floor.
double dX=0,Z1=0,Z2=0,X1=0,X2=0;
- double B=m_FootB,H=m_FootH,F=m_FootF;
+ double B=m_FootB,H=m_FootH,F=m_FootF;
if (lOmega<0)
{
@@ -530,7 +593,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
co = cos(lOmega);
so = sin(lOmega);
-
+
// COM Orientation
MAL_S3x3_MATRIX(Foot_R,double);
@@ -555,9 +618,9 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
curr_NSFAP.y += dFY ;
curr_NSFAP.z += dFZ ;
#endif
-
- ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
- << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
+
+ ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
+ << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
<< curr_NSFAP.x << " "
<< curr_NSFAP.y << " "
<< curr_NSFAP.z << " "
@@ -582,7 +645,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
double StartLanding = EndOfLiftOff + ModulatedSingleSupportTime;
// The foot support does not move.
- SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
+ SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
SupportFootAbsolutePositions[StartIndex-1];
SupportFootAbsolutePositions[CurrentAbsoluteIndex].stepType = (-1)*StepType;
@@ -593,13 +656,13 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
// Do not modify x, y and theta while liftoff.
// cout<<"no change"<<endl;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].x;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].y;
-
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].theta;
}
else if (LocalInterpolationStartTime < EndOfLiftOff && LocalInterpolationStartTime +InterpolationTime > EndOfLiftOff)
@@ -607,89 +670,109 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
// cout<<"rest changes"<<endl;
// DO MODIFY x, y and theta the remaining time.
// x, dx
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
m_PolynomeX->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
// NoneSupportFootAbsolutePositions[StartIndex-1].x;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
m_PolynomeX->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
// NoneSupportFootAbsolutePositions[StartIndex-1].dx;
//y, dy
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
- m_PolynomeY->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
+ m_PolynomeY->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
// NoneSupportFootAbsolutePositions[StartIndex-1].y;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
m_PolynomeY->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff); // +
// NoneSupportFootAbsolutePositions[StartIndex-1].dy;
//theta, dtheta
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
- m_PolynomeTheta->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
+ m_PolynomeTheta->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
//NoneSupportFootAbsolutePositions[StartIndex].theta;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
m_PolynomeTheta->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
// +NoneSupportFootAbsolutePositions[StartIndex].dtheta;
}
- else
+ else
{
// cout<<"all changes";
// DO MODIFY x, y and theta all the time.
// x, dx
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
m_PolynomeX->Compute(InterpolationTime);
//+NoneSupportFootAbsolutePositions[StartIndex-1].x;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
m_PolynomeX->ComputeDerivative(InterpolationTime);
//+NoneSupportFootAbsolutePositions[StartIndex-1].dx;
//y, dy
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
m_PolynomeY->Compute(InterpolationTime);
//+NoneSupportFootAbsolutePositions[StartIndex].y;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
m_PolynomeY->ComputeDerivative(InterpolationTime);
//+NoneSupportFootAbsolutePositions[StartIndex].dy;
//theta, dtheta
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
m_PolynomeTheta->Compute( InterpolationTime );
// +NoneSupportFootAbsolutePositions[StartIndex].theta;
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
m_PolynomeTheta->ComputeDerivative(InterpolationTime);
// + NoneSupportFootAbsolutePositions[StartIndex].dtheta;
}
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
- m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
- //m_AnklePositionRight[2];
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
- m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
- //m_AnklePositionRight[2];
-
+ if (m_isStepStairOn == 0)
+ {
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z = m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
+ //m_AnklePositionRight[2];
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz = m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
+
+ //m_AnklePositionRight[2];
+ }
+
+ else
+
+ {
+ if (m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime) == 0)
+ {
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z = NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].z;
+ }
+ else
+ {
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z = m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
+
+ }//m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
+ //m_AnklePositionRight[2];
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz = m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
+ //m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
+ //m_AnklePositionRight[2];
+ }
+
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
// First treat the lift-off.
if (LocalInterpolationStartTime+InterpolationTime<EndOfLiftOff)
{
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
- m_PolynomeOmega->Compute(InterpolationTime); // +
- // NoneSupportFootAbsolutePositions[StartIndex-1].omega;
-
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].domega =
- m_PolynomeOmega->Compute(InterpolationTime);// +
- // NoneSupportFootAbsolutePositions[StartIndex-1].domega;
-
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
+ m_PolynomeOmega->Compute(InterpolationTime); // +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].omega;
+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].domega =
+ m_PolynomeOmega->Compute(InterpolationTime);// +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].domega;
+
ProtectionNeeded=true;
}
// Prepare for the landing.
else if (LocalInterpolationStartTime+InterpolationTime<StartLanding)
{
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
m_Omega - m_PolynomeOmega2->Compute(LocalInterpolationStartTime+InterpolationTime-EndOfLiftOff)-
NoneSupportFootAbsolutePositions[StartIndex-1].omega2;
}
// Realize the landing.
- else
+ else
{
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
- m_PolynomeOmega->Compute(LocalInterpolationStartTime+InterpolationTime - StartLanding) +
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
+ m_PolynomeOmega->Compute(LocalInterpolationStartTime+InterpolationTime - StartLanding) +
NoneSupportFootAbsolutePositions[StartIndex-1].omega - m_Omega;
ProtectionNeeded=true;
}
@@ -705,7 +788,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
// Make sure the foot is not going inside the floor.
double dX=0,Z1=0,Z2=0,X1=0,X2=0;
- double B=m_FootB,H=m_FootH,F=m_FootF;
+ double B=m_FootB,H=m_FootH,F=m_FootF;
if (lOmega<0)
{
@@ -714,7 +797,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
Z1 = H*cos(-lOmega);
Z2 = B*sin(-lOmega);
dX = -(B - X1 + X2);
- dFZ = Z1 + Z2 - H;
+ dFZ = Z1 + Z2 - H;
}
else
{
@@ -723,7 +806,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
Z1 = H*cos(lOmega);
Z2 = F*sin(lOmega);
dX = (F - X1 + X2);
- dFZ = Z1 + Z2 - H;
+ dFZ = Z1 + Z2 - H;
}
dFX = c*dX;
dFY = s*dX;
@@ -741,7 +824,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
co = cos(lOmega);
so = sin(lOmega);
-
+
// COM Orientation
MAL_S3x3_MATRIX(Foot_R,double);
@@ -766,9 +849,9 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y += dFY ;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z += dFZ ;
#endif
-
- ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
- << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
+
+ ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
+ << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
<< NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x << " "
<< NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y << " "
<< NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z << " "
@@ -779,7 +862,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
void FootTrajectoryGenerationStandard::ComputingAbsFootPosFromQueueOfRelPos(deque<RelativeFootPosition> &RelativeFootPositions,
deque<FootAbsolutePosition> &AbsoluteFootPositions )
{
-
+
if (AbsoluteFootPositions.size()==0)
AbsoluteFootPositions.resize(RelativeFootPositions.size());
@@ -800,29 +883,29 @@ void FootTrajectoryGenerationStandard::ComputingAbsFootPosFromQueueOfRelPos(dequ
s = sin(RelativeFootPositions[i].theta*M_PI/180.0);
MM(0,0) = c; MM(0,1) = -s;
MM(1,0) = s; MM(1,1) = c;
-
+
/*! Update the orientation */
CurrentAbsTheta+= RelativeFootPositions[i].theta;
CurrentAbsTheta = fmod(CurrentAbsTheta,180.0);
-
+
/*! Extract the current absolute orientation matrix. */
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
Orientation(k,l) = CurrentSupportFootPosition(k,l);
-
+
/*! Put in a vector form the translation of the relative foot. */
v(0,0) = RelativeFootPositions[i].sx;
v(1,0) = RelativeFootPositions[i].sy;
-
+
/*! Compute the new orientation of the foot vector. */
Orientation = MAL_RET_A_by_B(MM , Orientation);
v2 = MAL_RET_A_by_B(Orientation, v);
-
+
/*! Update the world coordinates of the support foot. */
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
CurrentSupportFootPosition(k,l) = Orientation(k,l);
-
+
for(int k=0;k<2;k++)
CurrentSupportFootPosition(k,2) += v2(k,0);
@@ -846,6 +929,6 @@ void FootTrajectoryGenerationStandard::print()
m_PolynomeOmega2->print();
std::cout << "Polynome Yaw:" <<std::endl;
m_PolynomeTheta->print();
-
+
}
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
index a262cac99a9ffe0b82aba3e7aa11cc188fa41b6a..81fa2d01dce09d9295b542e1ef88c38b13c754ef 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
@@ -1,7 +1,7 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
+ *
*
- *
* Olivier Stasse
*
* JRL, CNRS/AIST
@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file FootTrajectoryGenerationStandard.h
@@ -34,13 +34,13 @@
#include <FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh>
#include <Mathematics/PolynomeFoot.hh>
-
+#include <Mathematics/Bsplines.hh>
namespace PatternGeneratorJRL
{
/** @ingroup foottrajectorygeneration
This class generates a trajectory for the swinging foot during single support phase.
- It uses a classical approach relying in polynome of 3rd orders for the position in the
+ It uses a classical approach relying in polynome of 3rd orders for the position in the
orthogonal plan as well as the direction.For the height modification a 4th order polynome
is used. Finally a landing and take off phase using an angular value (\f$\omega\f$).
*/
@@ -48,9 +48,9 @@ namespace PatternGeneratorJRL
{
public:
- /*!\name Constants related to the direction for the generation of the polynomes.
+ /*!\name Constants related to the direction for the generation of the polynomes.
@{ */
-
+
/*! \brief along the frontal direction */
const static unsigned int X_AXIS = 0;
/*! \brief along the left of the robot */
@@ -75,16 +75,16 @@ namespace PatternGeneratorJRL
/*! This method computes the position of the swinging foot during single support phase,
and maintain a constant position for the support foot.
- It uses polynomial of 3rd order for the X-axis, Y-axis,
+ It uses polynomial of 3rd order for the X-axis, Y-axis,
orientation in the X-Z axis, and orientation in the X-Y axis,
and finally it uses a 4th order polynome for the Z-axis.
-
+
@param SupportFootAbsolutePositions: Queue of absolute position for the support foot.
This method will set the foot position at index CurrentAbsoluteIndex of the queue.
This position is supposed to be constant.
@param NoneSupportFootAbsolutePositions: Queue of absolute position for the swinging
foot. This method will set the foot position at index NoneSupportFootAbsolutePositions
- of the queue.
+ of the queue.
@param CurrentAbsoluteIndex: Index in the queues of the foot position to be set.
@param IndexInitial: Index in the queues which correspond to the starting point
of the current single support phase.
@@ -94,11 +94,11 @@ namespace PatternGeneratorJRL
*/
virtual void UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
- int CurrentAbsoluteIndex,
- int IndexInitial,
+ int CurrentAbsoluteIndex,
+ int IndexInitial,
double ModulatedSingleSupportTime,
int StepType,int LeftOrRight);
-
+
virtual void UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int StartIndex, int k,
@@ -111,17 +111,17 @@ namespace PatternGeneratorJRL
In this specific case, it is in charge of creating the polynomial structures.
*/
virtual void InitializeInternalDataStructures();
-
+
/*! Free internal data structures.
In this specific case, it is in charge of freeing the polynomial data structures.
*/
virtual void FreeInternalDataStructures();
-
+
/*! This method specifies the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- It assumes an initial position and an initial speed set to zero.
- @param[in] AxisReference: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ It assumes an initial position and an initial speed set to zero.
+ @param[in] AxisReference: Set to which axis the parameters will be applied.
@param[in] TimeInterval: Set the time base of the polynome.
@param[in] Position: Set the final position of the polynome at TimeInterval.
*/
@@ -131,8 +131,8 @@ namespace PatternGeneratorJRL
/*! This method specifies the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param[in] AxisReference: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param[in] AxisReference: Set to which axis the parameters will be applied.
@param[in] TimeInterval: Set the time base of the polynome.
@param[in] FinalPosition: Set the final position of the polynome at TimeInterval.
@param[in] InitPosition: Initial position when computing the polynome at t=0.0.
@@ -146,8 +146,8 @@ namespace PatternGeneratorJRL
/*! This method get the parameters for each of the polynome used by this
object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
- @param[in] AxisReference: Set to which axis the parameters will be applied.
+ which those parameters are set is specified with PolynomeIndex.
+ @param[in] AxisReference: Set to which axis the parameters will be applied.
@param[in] TimeInterval: Set the time base of the polynome.
@param[in] FinalPosition: Set the final position of the polynome at TimeInterval.
@param[in] InitPosition: Initial position when computing the polynome at t=0.0.
@@ -179,26 +179,26 @@ namespace PatternGeneratorJRL
/*! Compute the value for a given polynome's second derivative. */
double ComputeSecDerivative(unsigned int PolynomeIndex, double Time);
- /*! Compute the absolute foot position from the queue of relative positions.
+ /*! Compute the absolute foot position from the queue of relative positions.
There is not direct dependency with time.
*/
void ComputingAbsFootPosFromQueueOfRelPos(deque<RelativeFootPosition> &RelativeFootPositions,
deque<FootAbsolutePosition> &AbsoluteFootPositions);
/*! Methods to compute a set of positions for the feet according to the discrete time given in parameters
- and the phase of walking.
+ and the phase of walking.
@{
*/
-
+
/*! @} */
-
+
void print();
protected:
-
+
/*! \brief Polynomes for X and Y axis positions*/
- Polynome7 *m_PolynomeX,*m_PolynomeY;
-
+ Polynome5 *m_PolynomeX,*m_PolynomeY;
+
/*! \brief Polynome for X-Y orientation */
Polynome5 *m_PolynomeTheta;
@@ -208,6 +208,11 @@ namespace PatternGeneratorJRL
/*! \brief Polynome for Z axis position. */
Polynome4 *m_PolynomeZ;
+ /*! \brief Bsplines for Z axis position. */
+ ZBsplines *m_BsplinesZ;
+
+ // int m_isStepStairOn;
+
/*! \brief Foot dimension. */
double m_FootB, m_FootH, m_FootF;
@@ -217,11 +222,9 @@ namespace PatternGeneratorJRL
/*! \brief Position of the ankle in the right foot. */
MAL_S3_VECTOR(m_AnklePositionRight,double);
-
-
};
-
-
+
+
}
#endif /* _FOOT_TRAJECTORY_GENERATION_ABSTRACT_H_ */
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
index 6074129d35751b221c40ce65992ba90d110de3b6..9c08b9f7bbe45f11adefa3c400917bf58449e43e 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Paul Evrard
* Francois Keith
@@ -20,7 +20,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
@@ -45,7 +45,7 @@ LeftAndRightFootTrajectoryGenerationMultiple(SimplePluginManager *lSPM,
m_LeftFootTrajectory = new FootTrajectoryGenerationMultiple(lSPM,m_Foot);
m_RightFootTrajectory = new FootTrajectoryGenerationMultiple(lSPM,m_Foot);
- string aMethodName[4] =
+ string aMethodName[4] =
{":omega",":stepheight", ":singlesupporttime",":doublesupporttime"};
for (int i=0;i<4;i++)
@@ -65,7 +65,7 @@ LeftAndRightFootTrajectoryGenerationMultiple(SimplePluginManager *lSPM,
LeftAndRightFootTrajectoryGenerationMultiple::
LeftAndRightFootTrajectoryGenerationMultiple(const LeftAndRightFootTrajectoryGenerationMultiple & aLRFTGM):
SimplePlugin(aLRFTGM.getSimplePluginManager())
-
+
{
LeftAndRightFootTrajectoryGenerationMultiple (aLRFTGM.getSimplePluginManager(),
aLRFTGM.getFoot());
@@ -116,57 +116,72 @@ void LeftAndRightFootTrajectoryGenerationMultiple::SetAnInterval(unsigned int In
<< FootFinalPosition.z << "," << FootInitialPosition.z << "," << FootInitialPosition.dz << ")");
aFTGM->SetNatureInterval(IntervalIndex,FootFinalPosition.stepType);
- // Init the first interval.
- // X axis.
- aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
- FootTrajectoryGenerationStandard::X_AXIS,
- m_DeltaTj[IntervalIndex],
- FootFinalPosition.x,
- FootInitialPosition.x,
- FootInitialPosition.dx);
+
// Y axis.
aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
FootTrajectoryGenerationStandard::Y_AXIS,
m_DeltaTj[IntervalIndex],
- FootFinalPosition.y,
- FootInitialPosition.y,
+ FootFinalPosition.y,
+ FootInitialPosition.y,
FootInitialPosition.dy);
- // Z axis.
aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
- FootTrajectoryGenerationStandard::Z_AXIS,
- m_DeltaTj[IntervalIndex],
- FootFinalPosition.z,
- FootInitialPosition.z,
- FootInitialPosition.dz);
+ FootTrajectoryGenerationStandard::Z_AXIS,
+ m_DeltaTj[IntervalIndex],
+ FootFinalPosition.z,
+ FootInitialPosition.z,
+ FootInitialPosition.dz);
// THETA
aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
FootTrajectoryGenerationStandard::THETA_AXIS,
m_DeltaTj[IntervalIndex],
- FootFinalPosition.theta,
- FootInitialPosition.theta,
+ FootFinalPosition.theta,
+ FootInitialPosition.theta,
FootInitialPosition.dtheta);
-
- // Omega
+
+ // Omega
aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
FootTrajectoryGenerationStandard::OMEGA_AXIS,
m_DeltaTj[IntervalIndex],
- FootFinalPosition.omega,
- FootInitialPosition.omega,
+ FootFinalPosition.omega,
+ FootInitialPosition.omega,
FootInitialPosition.domega);
-
+
// Omega 2
aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
FootTrajectoryGenerationStandard::OMEGA2_AXIS,
m_DeltaTj[IntervalIndex],
- FootFinalPosition.omega2,
- FootInitialPosition.omega2,
+ FootFinalPosition.omega2,
+ FootInitialPosition.omega2,
FootInitialPosition.domega2);
+
+ // X axis.
+ if (FootFinalPosition.z <= FootInitialPosition.z ) //down
+ aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
+ FootTrajectoryGenerationStandard::X_AXIS,
+ 0.8*m_DeltaTj[IntervalIndex],
+ FootFinalPosition.x,
+ FootInitialPosition.x,
+ FootInitialPosition.dx);
+ else if ((FootFinalPosition.z - FootInitialPosition.z ) == m_StepHeight) // normal walk
+ aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
+ FootTrajectoryGenerationStandard::X_AXIS,
+ 0.9*m_DeltaTj[IntervalIndex],
+ FootFinalPosition.x,
+ FootInitialPosition.x,
+ FootInitialPosition.dx);
+ else if (FootFinalPosition.z > FootInitialPosition.z ) // up
+ aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
+ FootTrajectoryGenerationStandard::X_AXIS,
+ 0.75*m_DeltaTj[IntervalIndex],
+ FootFinalPosition.x,
+ FootInitialPosition.x,
+ FootInitialPosition.dx);
}
void LeftAndRightFootTrajectoryGenerationMultiple::
-InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
+InitializeFromRelativeSteps_backup(deque<RelativeFootPosition> &RelativeFootPositions,
FootAbsolutePosition &LeftFootInitialPosition,
FootAbsolutePosition &RightFootInitialPosition,
deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
@@ -181,21 +196,21 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RelativeFootPositions.size())
SupportFootAbsoluteFootPositions.resize(RelativeFootPositions.size());
- unsigned int lNbOfIntervals = RelativeFootPositions.size();
+ unsigned int lNbOfIntervals = RelativeFootPositions.size();
/*! It is assumed that a set of relative positions for the support foot
are given as an input. */
deque<FootAbsolutePosition> AbsoluteFootPositions;
- /*! Those two variables are needed to compute intermediate
+ /*! Those two variables are needed to compute intermediate
initial positions for the feet. */
FootAbsolutePosition LeftFootTmpInitPos,RightFootTmpInitPos;
- /*! Those two variables are needed to compute intermediate
+ /*! Those two variables are needed to compute intermediate
final positions for the feet. */
FootAbsolutePosition LeftFootTmpFinalPos,RightFootTmpFinalPos;
-
+
AbsoluteFootPositions.resize(lNbOfIntervals);
lNbOfIntervals = 2*lNbOfIntervals+1;
-
+
/*! Resize the Left and Right foot trajectories. */
m_LeftFootTrajectory->SetNumberOfIntervals(lNbOfIntervals);
m_RightFootTrajectory->SetNumberOfIntervals(lNbOfIntervals);
@@ -210,14 +225,14 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
MAL_MATRIX_SET_IDENTITY(Orientation);
MAL_MATRIX_DIM(v,double,2,1);
MAL_MATRIX_DIM(v2,double,2,1);
-
+
if (m_DeltaTj.size()!=lNbOfIntervals)
m_DeltaTj.resize(lNbOfIntervals);
/*! Who is the first support foot. */
int SupportFoot=1; // Left
-
- if (
+
+ if (
// The flying foot is on the left, thus the support foot is on the right.
// and this is not the beginning of the stepping.
((RelativeFootPositions[0].sy>0) && (IgnoreFirst==false)) ||
@@ -226,7 +241,7 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
)
{
ODEBUG("Detect support foot on the right.");
- SupportFoot=-1;
+ SupportFoot=-1;
CurrentAbsTheta = RightFootInitialPosition.theta;
v2(0,0) = RightFootInitialPosition.x;
v2(1,0) = RightFootInitialPosition.y;
@@ -237,10 +252,10 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
CurrentAbsTheta = LeftFootInitialPosition.theta;
v2(0,0) = LeftFootInitialPosition.x;
v2(1,0) = LeftFootInitialPosition.y;
-
+
}
ODEBUG("Support Foot : " << v2(0,0) << " " << v2(1,0) << " " << CurrentAbsTheta);
-
+
// Initial Position of the current support foot.
c = cos(CurrentAbsTheta*M_PI/180.0);
s = sin(CurrentAbsTheta*M_PI/180.0);
@@ -250,31 +265,31 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
CurrentSupportFootPosition(k,l) = MM(k,l);
-
+
for(int k=0;k<2;k++)
CurrentSupportFootPosition(k,2) = v2(k,0);
-
-
+
+
/*! Initialize the temporary initial position. */
-
+
LeftFootTmpInitPos = LeftFootInitialPosition;
RightFootTmpInitPos = RightFootInitialPosition;
/* Keep track of the interval index once this is
for single support, once for double support */
int IntervalIndex=0;
- ODEBUG("LeftFootTmpInitPos.x " << LeftFootTmpInitPos.x << endl <<
- "LeftFootTmpInitPos.y " << LeftFootTmpInitPos.y << endl <<
- "LeftFootTmpInitPos.z " << LeftFootTmpInitPos.z << endl <<
- "LeftFootTmpInitPos.dx " << LeftFootTmpInitPos.dx << endl <<
- "LeftFootTmpInitPos.dy " << LeftFootTmpInitPos.dy << endl <<
+ ODEBUG("LeftFootTmpInitPos.x " << LeftFootTmpInitPos.x << endl <<
+ "LeftFootTmpInitPos.y " << LeftFootTmpInitPos.y << endl <<
+ "LeftFootTmpInitPos.z " << LeftFootTmpInitPos.z << endl <<
+ "LeftFootTmpInitPos.dx " << LeftFootTmpInitPos.dx << endl <<
+ "LeftFootTmpInitPos.dy " << LeftFootTmpInitPos.dy << endl <<
"LeftFootTmpInitPos.dz " << LeftFootTmpInitPos.dz << endl );
- ODEBUG("RightFootTmpInitPos.x " << RightFootTmpInitPos.x << endl <<
- "RightFootTmpInitPos.y " << RightFootTmpInitPos.y << endl <<
- "RightFootTmpInitPos.z " << RightFootTmpInitPos.z << endl <<
- "RightFootTmpInitPos.dx " << RightFootTmpInitPos.dx << endl <<
- "RightFootTmpInitPos.dy " << RightFootTmpInitPos.dy << endl <<
+ ODEBUG("RightFootTmpInitPos.x " << RightFootTmpInitPos.x << endl <<
+ "RightFootTmpInitPos.y " << RightFootTmpInitPos.y << endl <<
+ "RightFootTmpInitPos.z " << RightFootTmpInitPos.z << endl <<
+ "RightFootTmpInitPos.dx " << RightFootTmpInitPos.dx << endl <<
+ "RightFootTmpInitPos.dy " << RightFootTmpInitPos.dy << endl <<
"RightFootTmpInitPos.dz " << RightFootTmpInitPos.dz << endl );
bool FirstIntervalIsSingleSupport = true;
@@ -309,19 +324,19 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
IntervalIndex++;
-
+
ODEBUG("It: " << i << " Double support Phase :" << endl <<
- "\t Init Left: ( " <<
- LeftFootTmpInitPos.x << " , " <<
- LeftFootTmpInitPos.y << " ) " <<
- endl << "Right : ( " <<
- RightFootTmpInitPos.x << " , " <<
+ "\t Init Left: ( " <<
+ LeftFootTmpInitPos.x << " , " <<
+ LeftFootTmpInitPos.y << " ) " <<
+ endl << "Right : ( " <<
+ RightFootTmpInitPos.x << " , " <<
RightFootTmpInitPos.y << " ) " << endl <<
- "\t Final Left: ( " <<
- LeftFootTmpFinalPos.x << " , " <<
- LeftFootTmpFinalPos.y << " ) " <<
- endl << "Right : ( " <<
- RightFootTmpFinalPos.x << " , " <<
+ "\t Final Left: ( " <<
+ LeftFootTmpFinalPos.x << " , " <<
+ LeftFootTmpFinalPos.y << " ) " <<
+ endl << "Right : ( " <<
+ RightFootTmpFinalPos.x << " , " <<
RightFootTmpFinalPos.y << " ) " << endl <<
"\t RelativeFootPosition: ( " <<
RelativeFootPositions[i].sx << " , " <<
@@ -329,22 +344,22 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RelativeFootPositions[i].sx << " , " <<
RelativeFootPositions[i].theta << " )");
}
-
+
/*! Compute Orientation matrix related to the relative orientation
of the support foot */
c = cos(RelativeFootPositions[i].theta*M_PI/180.0);
s = sin(RelativeFootPositions[i].theta*M_PI/180.0);
MM(0,0) = c; MM(0,1) = -s;
MM(1,0) = s; MM(1,1) = c;
-
- /*! Update the orientation */
+
+ /*! Update the orientation */
CurrentAbsTheta+= RelativeFootPositions[i].theta;
-
+
/*! Extract the current absolute orientation matrix. */
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
Orientation(k,l) = CurrentSupportFootPosition(k,l);
-
+
/*! Put in a vector form the translation of the relative foot. */
v(0,0) = RelativeFootPositions[i].sx;
v(1,0) = RelativeFootPositions[i].sy;
@@ -352,14 +367,14 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
/*! Compute the new orientation of the foot vector. */
Orientation = MAL_RET_A_by_B(MM , Orientation);
v2 = MAL_RET_A_by_B(Orientation, v);
-
+
/*! Update the world coordinates of the support foot. */
if ((!IgnoreFirst) || (i>0))
{
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
CurrentSupportFootPosition(k,l) = Orientation(k,l);
-
+
for(int k=0;k<2;k++)
CurrentSupportFootPosition(k,2) += v2(k,0);
}
@@ -367,7 +382,7 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
AbsoluteFootPositions[i].x = CurrentSupportFootPosition(0,2);
AbsoluteFootPositions[i].y = CurrentSupportFootPosition(1,2);
AbsoluteFootPositions[i].theta = CurrentAbsTheta;
-
+
ODEBUG("CSFP:" << CurrentSupportFootPosition(0,2) << " " << CurrentSupportFootPosition(1,2));
/*! We deal with the single support phase,
@@ -391,7 +406,7 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RightFootTmpFinalPos.domega = 0.0;
RightFootTmpFinalPos.domega2 = 0.0;
RightFootTmpFinalPos.stepType = 1;
-
+
LeftFootTmpFinalPos = LeftFootTmpInitPos;
LeftFootTmpFinalPos.z = 0.0;
LeftFootTmpFinalPos.dz = 0.0;
@@ -418,7 +433,7 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RightFootTmpFinalPos.z = 0.0;
LeftFootTmpFinalPos.dz = 0.0;
RightFootTmpFinalPos.stepType = -1;
-
+
}
}
else
@@ -444,9 +459,9 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RightFootTmpFinalPos.domega = RightFootTmpInitPos.domega =0.0;
RightFootTmpFinalPos.domega2 = RightFootTmpInitPos.domega2 =0.0;
RightFootTmpFinalPos.stepType = 9;
-
+
}
-
+
if ((i!=0)|| (Continuity))
{
/* Initialize properly the interval in single support phase */
@@ -456,19 +471,19 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
SetAnInterval(IntervalIndex,m_LeftFootTrajectory,
LeftFootTmpInitPos,
LeftFootTmpFinalPos);
-
- ODEBUG("LeftInit: ( " << LeftFootTmpInitPos.x << " , "
- << LeftFootTmpInitPos.y << " , "
- << LeftFootTmpInitPos.z << " ) ( "
- << LeftFootTmpInitPos.dx << " , "
- << LeftFootTmpInitPos.dy << " , "
+
+ ODEBUG("LeftInit: ( " << LeftFootTmpInitPos.x << " , "
+ << LeftFootTmpInitPos.y << " , "
+ << LeftFootTmpInitPos.z << " ) ( "
+ << LeftFootTmpInitPos.dx << " , "
+ << LeftFootTmpInitPos.dy << " , "
<< LeftFootTmpInitPos.dz << " ) "
- << endl << "LeftFinal : ( "
- << LeftFootTmpFinalPos.x << " , "
- << LeftFootTmpFinalPos.y << " , "
- << LeftFootTmpFinalPos.z << " ) ( "
- << LeftFootTmpFinalPos.dx << " , "
- << LeftFootTmpFinalPos.dy << " , "
+ << endl << "LeftFinal : ( "
+ << LeftFootTmpFinalPos.x << " , "
+ << LeftFootTmpFinalPos.y << " , "
+ << LeftFootTmpFinalPos.z << " ) ( "
+ << LeftFootTmpFinalPos.dx << " , "
+ << LeftFootTmpFinalPos.dy << " , "
<< LeftFootTmpFinalPos.dz << " ) " );
ODEBUG("RightFootTmpInitPos.stepType="<<RightFootTmpInitPos.stepType);
@@ -478,20 +493,20 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RightFootTmpInitPos,
RightFootTmpFinalPos);
- ODEBUG("RightInit: ( " << RightFootTmpInitPos.x << " , "
- << RightFootTmpInitPos.y << " , "
- << RightFootTmpInitPos.z << " ) ( "
- << RightFootTmpInitPos.dx << " , "
- << RightFootTmpInitPos.dy << " , "
+ ODEBUG("RightInit: ( " << RightFootTmpInitPos.x << " , "
+ << RightFootTmpInitPos.y << " , "
+ << RightFootTmpInitPos.z << " ) ( "
+ << RightFootTmpInitPos.dx << " , "
+ << RightFootTmpInitPos.dy << " , "
<< RightFootTmpInitPos.dz << " ) "
- << endl << "RightFinal : ( "
- << RightFootTmpFinalPos.x << " , "
- << RightFootTmpFinalPos.y << " , "
- << RightFootTmpFinalPos.z << " ) ( "
- << RightFootTmpFinalPos.dx << " , "
- << RightFootTmpFinalPos.dy << " , "
+ << endl << "RightFinal : ( "
+ << RightFootTmpFinalPos.x << " , "
+ << RightFootTmpFinalPos.y << " , "
+ << RightFootTmpFinalPos.z << " ) ( "
+ << RightFootTmpFinalPos.dx << " , "
+ << RightFootTmpFinalPos.dy << " , "
<< RightFootTmpFinalPos.dz << " ) " );
-
+
// Switch from single support to double support.
IntervalIndex++;
}
@@ -527,8 +542,8 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
/* The final position become the new initial position */
LeftFootTmpInitPos = LeftFootTmpFinalPos;
RightFootTmpInitPos = RightFootTmpFinalPos;
-
-
+
+
/* Populate the set of support foot absolute positions */
SupportFootAbsoluteFootPositions[i].x = CurrentSupportFootPosition(0,2);
SupportFootAbsoluteFootPositions[i].y = CurrentSupportFootPosition(1,2);
@@ -542,10 +557,10 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
if the system is in real-time foot modification. In this case,
the representation of the intervals shift from:
ONE DOUBLE SUPPORT STARTING PHASE - 1st foot single support phase - double support phase - 2nd foot single support phase
- - double support phase - 3rd single support phase - ending double support phase
+ - double support phase - 3rd single support phase - ending double support phase
to
1st foot single support phase - double support phase - 2nd foot single support phase
- - double support phase - 3rd single support phase
+ - double support phase - 3rd single support phase
Two intervals are missing and should be set by default to the end position of the feet
if Continuity is set to true, and if the number of intervals so far is the number of
intervals minus 2.
@@ -566,9 +581,442 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RightFootTmpFinalPos);
IntervalIndex++;
}
-
+
}
-
+
+}
+
+
+void LeftAndRightFootTrajectoryGenerationMultiple::
+InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
+ FootAbsolutePosition &LeftFootInitialPosition,
+ FootAbsolutePosition &RightFootInitialPosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
+ bool IgnoreFirst, bool Continuity)
+{
+
+ ODEBUG("LeftFootInitialPosition.stepType: "<< LeftFootInitialPosition.stepType
+ << " RightFootInitialPosition.stepType: "<< RightFootInitialPosition.stepType);
+ /*! Makes sure the size of the SupportFootAbsolutePositions is the same than
+ the relative foot positions. */
+ if (SupportFootAbsoluteFootPositions.size()!=
+ RelativeFootPositions.size())
+ SupportFootAbsoluteFootPositions.resize(RelativeFootPositions.size());
+
+ unsigned int lNbOfIntervals = RelativeFootPositions.size();
+ /*! It is assumed that a set of relative positions for the support foot
+ are given as an input. */
+ deque<FootAbsolutePosition> AbsoluteFootPositions;
+
+ /*! Those two variables are needed to compute intermediate
+ initial positions for the feet. */
+ FootAbsolutePosition LeftFootTmpInitPos,RightFootTmpInitPos;
+ /*! Those two variables are needed to compute intermediate
+ final positions for the feet. */
+ FootAbsolutePosition LeftFootTmpFinalPos,RightFootTmpFinalPos;
+
+ AbsoluteFootPositions.resize(lNbOfIntervals);
+ lNbOfIntervals = 2*lNbOfIntervals+1;
+
+ /*! Resize the Left and Right foot trajectories. */
+ m_LeftFootTrajectory->SetNumberOfIntervals(lNbOfIntervals);
+ m_RightFootTrajectory->SetNumberOfIntervals(lNbOfIntervals);
+ ODEBUG("resize left and right foot trajectories: " << lNbOfIntervals);
+
+ /*! Compute the absolute coordinates of the steps. */
+ double CurrentAbsTheta=0.0,c=0.0,s=0.0;
+ MAL_MATRIX_DIM(MM,double,2,2);
+ MAL_MATRIX_DIM(CurrentSupportFootPosition,double,3,3);
+ MAL_MATRIX_SET_IDENTITY(CurrentSupportFootPosition);
+ MAL_MATRIX_DIM(Orientation,double,2,2);
+ MAL_MATRIX_SET_IDENTITY(Orientation);
+ MAL_MATRIX_DIM(v,double,2,1);
+ MAL_MATRIX_DIM(v2,double,2,1);
+
+ if (m_DeltaTj.size()!=lNbOfIntervals)
+ m_DeltaTj.resize(lNbOfIntervals);
+
+ /*! Who is the first support foot. */
+ int SupportFoot=1; // Left
+
+ if (
+ // The flying foot is on the left, thus the support foot is on the right.
+ // and this is not the beginning of the stepping.
+ ((RelativeFootPositions[0].sy>0) && (IgnoreFirst==false)) ||
+ // There is no flying foot because this is the beginning of the stepping.
+ ((RelativeFootPositions[0].sy<0) && (IgnoreFirst==true))
+ )
+ {
+ ODEBUG("Detect support foot on the right.");
+ SupportFoot=-1;
+ CurrentAbsTheta = RightFootInitialPosition.theta;
+ v2(0,0) = RightFootInitialPosition.x;
+ v2(1,0) = RightFootInitialPosition.y;
+ CurrentSupportFootPosition(2,2) = RightFootInitialPosition.z;
+ // v2(2,0) = RightFootInitialPosition.z;
+ }
+ else
+ {
+ ODEBUG("Detect support foot on the left.");
+ CurrentAbsTheta = LeftFootInitialPosition.theta;
+ v2(0,0) = LeftFootInitialPosition.x;
+ v2(1,0) = LeftFootInitialPosition.y;
+ CurrentSupportFootPosition(2,2) = LeftFootInitialPosition.z;
+ // v2(2,0) = LeftFootInitialPosition.z;
+ }
+ ODEBUG("Support Foot : " << v2(0,0) << " " << v2(1,0) << " " << CurrentAbsTheta);
+
+ // Initial Position of the current support foot.
+ c = cos(CurrentAbsTheta*M_PI/180.0);
+ s = sin(CurrentAbsTheta*M_PI/180.0);
+ MM(0,0) = Orientation(0,0) = c; MM(0,1) = Orientation(0,1) = -s;
+ MM(1,0) = Orientation(1,0) = s; MM(1,1) = Orientation(1,1) = c;
+ // MM(2,0) = Orientation(1,0) = 0; MM(2,1) = Orientation(1,1) = 0;
+ for(int k=0;k<2;k++)
+ for(int l=0;l<2;l++)
+ CurrentSupportFootPosition(k,l) = MM(k,l);
+
+ for(int k=0;k<2;k++)
+ CurrentSupportFootPosition(k,2) = v2(k,0);
+
+ /*! Initialize the temporary initial position. */
+
+ LeftFootTmpInitPos = LeftFootInitialPosition;
+ RightFootTmpInitPos = RightFootInitialPosition;
+
+ /* Keep track of the interval index once this is
+ for single support, once for double support */
+ int IntervalIndex=0;
+ ODEBUG("LeftFootTmpInitPos.x " << LeftFootTmpInitPos.x << endl <<
+ "LeftFootTmpInitPos.y " << LeftFootTmpInitPos.y << endl <<
+ "LeftFootTmpInitPos.z " << LeftFootTmpInitPos.z << endl <<
+ "LeftFootTmpInitPos.dx " << LeftFootTmpInitPos.dx << endl <<
+ "LeftFootTmpInitPos.dy " << LeftFootTmpInitPos.dy << endl <<
+ "LeftFootTmpInitPos.dz " << LeftFootTmpInitPos.dz << endl );
+
+
+ ODEBUG("RightFootTmpInitPos.x " << RightFootTmpInitPos.x << endl <<
+ "RightFootTmpInitPos.y " << RightFootTmpInitPos.y << endl <<
+ "RightFootTmpInitPos.z " << RightFootTmpInitPos.z << endl <<
+ "RightFootTmpInitPos.dx " << RightFootTmpInitPos.dx << endl <<
+ "RightFootTmpInitPos.dy " << RightFootTmpInitPos.dy << endl <<
+ "RightFootTmpInitPos.dz " << RightFootTmpInitPos.dz << endl );
+
+ bool FirstIntervalIsSingleSupport = true;
+ if (LeftFootInitialPosition.stepType>10)
+ FirstIntervalIsSingleSupport = false;
+
+ ODEBUG("CurrentSupportFootPosition: " << CurrentSupportFootPosition);
+ ODEBUG("RelativeFootPositions: " << RelativeFootPositions.size());
+ for(unsigned int i=0;i<RelativeFootPositions.size();i++)
+ {
+ if ((i!=0) || (FirstIntervalIsSingleSupport==false))
+ {
+ /*! At this stage the phase of double support is deal with */
+
+
+ ODEBUG("Double support phase");
+ //LeftFootTmpInitPos.z = CurrentSupportFootPosition(2,2);// 0.0;//LeftFootTmpInitPos.z + LeftFootTmpFinalPos.z/1.5;
+ LeftFootTmpInitPos.dz = 0;
+ LeftFootTmpInitPos.stepType=11;
+
+ SetAnInterval(IntervalIndex,m_LeftFootTrajectory,
+ LeftFootTmpInitPos,
+ LeftFootTmpInitPos);
+
+ /// RightFootTmpInitPos.z = CurrentSupportFootPosition(2,2);
+ RightFootTmpInitPos.dz = 0;
+ RightFootTmpInitPos.stepType=9;
+ SetAnInterval(IntervalIndex,m_RightFootTrajectory,
+ RightFootTmpInitPos,
+ RightFootTmpInitPos);
+ ODEBUG("LeftFootTmpInitPos.stepType="<<LeftFootTmpInitPos.stepType);
+ ODEBUG("RightFootTmpInitPos.stepType="<<RightFootTmpInitPos.stepType);
+ ODEBUG("End of Double support phase");
+
+ IntervalIndex++;
+
+
+ ODEBUG("It: " << i << " Double support Phase :" << endl <<
+ "\t Init Left: ( " <<
+ LeftFootTmpInitPos.x << " , " <<
+ LeftFootTmpInitPos.y << " , " <<
+ LeftFootTmpInitPos.z <<" ) " <<
+ endl << "Right : ( " <<
+ RightFootTmpInitPos.x << " , " <<
+ RightFootTmpInitPos.y << " , " <<
+ RightFootTmpInitPos.z <<") " << endl <<
+ "\t Final Left: ( " <<
+ LeftFootTmpFinalPos.x << " , " <<
+ LeftFootTmpFinalPos.y << " , " <<
+ RightFootTmpFinalPos.z << " ) " <<
+ endl << "Right : ( " <<
+ RightFootTmpFinalPos.x << " , " <<
+ RightFootTmpFinalPos.y << " , " <<
+ RightFootTmpFinalPos.z << " ) " << endl <<
+ "\t RelativeFootPosition: ( " <<
+ RelativeFootPositions[i].sx << " , " <<
+ RelativeFootPositions[i].sy << " , " <<
+ RelativeFootPositions[i].sz << " , " <<
+ RelativeFootPositions[i].theta << " )");
+ }
+
+ /*! Compute Orientation matrix related to the relative orientation
+ of the support foot */
+ c = cos(RelativeFootPositions[i].theta*M_PI/180.0);
+ s = sin(RelativeFootPositions[i].theta*M_PI/180.0);
+ MM(0,0) = c; MM(0,1) = -s;
+ MM(1,0) = s; MM(1,1) = c;
+ // MM(2,0) = 0; MM(2,1) = 0; MM(2,2) = 0;
+ /*! Update the orientation */
+ CurrentAbsTheta+= RelativeFootPositions[i].theta;
+
+ /*! Extract the current absolute orientation matrix. */
+ for(int k=0;k<2;k++)
+ for(int l=0;l<2;l++)
+ Orientation(k,l) = CurrentSupportFootPosition(k,l);
+
+ /*! Put in a vector form the translation of the relative foot. */
+ v(0,0) = RelativeFootPositions[i].sx;
+ v(1,0) = RelativeFootPositions[i].sy;
+ // v(2,0) = RelativeFootPositions[i].sz;
+ /*! Compute the new orientation of the foot vector. */
+ Orientation = MAL_RET_A_by_B(MM , Orientation);
+
+ v2 = MAL_RET_A_by_B(Orientation, v);
+
+ /*! Update the world coordinates of the support foot. */
+ if ((!IgnoreFirst) || (i>0))
+ {
+ for(int k=0;k<2;k++)
+ for(int l=0;l<2;l++)
+ CurrentSupportFootPosition(k,l) = Orientation(k,l);
+
+ for(int k=0;k<2;k++)
+ CurrentSupportFootPosition(k,2) += v2(k,0);
+
+ CurrentSupportFootPosition(2,2) += RelativeFootPositions[i].sz;
+ }
+
+ AbsoluteFootPositions[i].x = CurrentSupportFootPosition(0,2);
+ AbsoluteFootPositions[i].y = CurrentSupportFootPosition(1,2);
+ AbsoluteFootPositions[i].z = CurrentSupportFootPosition(2,2);
+ AbsoluteFootPositions[i].theta = CurrentAbsTheta;
+
+ ODEBUG("CSFP:" << CurrentSupportFootPosition(0,2) << " " << CurrentSupportFootPosition(1,2) <<" " << CurrentSupportFootPosition(2,2) << " "<< CurrentAbsTheta);
+
+ /*! We deal with the single support phase,
+ i.e. the target of the next single support phase
+ is the current target of the swinging foot. */
+ if ((!IgnoreFirst) || (i>0))
+ {
+ if (SupportFoot==1)
+ {
+ /*! The current support foot is the left one.*/
+ RightFootTmpFinalPos.x = CurrentSupportFootPosition(0,2);
+ RightFootTmpFinalPos.y = CurrentSupportFootPosition(1,2);
+ if (RightFootTmpFinalPos.z == CurrentSupportFootPosition(2,2))
+ RightFootTmpFinalPos.z = m_StepHeight + RightFootTmpFinalPos.z;
+ else
+ RightFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);//
+ RightFootTmpFinalPos.theta = CurrentAbsTheta;
+ RightFootTmpFinalPos.omega = m_Omega;
+ RightFootTmpFinalPos.omega2 = 0.0;
+ RightFootTmpFinalPos.dx = 0.0;
+ RightFootTmpFinalPos.dy = 0.0;
+ RightFootTmpFinalPos.dz = 0.0;
+ RightFootTmpFinalPos.dtheta = 0.0;
+ RightFootTmpFinalPos.domega = 0.0;
+ RightFootTmpFinalPos.domega2 = 0.0;
+ RightFootTmpFinalPos.stepType = 1;
+ LeftFootTmpFinalPos = LeftFootTmpInitPos;
+ LeftFootTmpFinalPos.dz = 0.0;
+ LeftFootTmpFinalPos.stepType = -1;
+ }
+ else
+ {
+ /*! The current support foot is the right one.*/
+ LeftFootTmpFinalPos.x = CurrentSupportFootPosition(0,2);
+ LeftFootTmpFinalPos.y = CurrentSupportFootPosition(1,2);
+
+ if (LeftFootTmpFinalPos.z == CurrentSupportFootPosition(2,2))
+ LeftFootTmpFinalPos.z = m_StepHeight + LeftFootTmpFinalPos.z;
+ else
+ LeftFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);//
+
+ LeftFootTmpFinalPos.theta = CurrentAbsTheta;
+ LeftFootTmpFinalPos.omega = m_Omega;
+ LeftFootTmpFinalPos.omega2 = 0.0;
+ LeftFootTmpFinalPos.dx = 0.0;
+ LeftFootTmpFinalPos.dy = 0.0;
+ LeftFootTmpFinalPos.dz = 0.0;
+ LeftFootTmpFinalPos.dtheta = 0.0;
+ LeftFootTmpFinalPos.domega = 0.0;
+ LeftFootTmpFinalPos.domega2 = 0.0;
+ LeftFootTmpFinalPos.stepType = 1;
+
+ RightFootTmpFinalPos = RightFootTmpInitPos;
+ // RightFootTmpFinalPos.z = 0.0;
+ RightFootTmpFinalPos.dz = 0.0;
+ RightFootTmpFinalPos.stepType = -1;
+ }
+ }
+ else
+ {
+ LeftFootTmpFinalPos = LeftFootTmpInitPos;
+
+ LeftFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
+ LeftFootTmpFinalPos.omega = 0.0;
+ LeftFootTmpFinalPos.omega2 = 0.0;
+ LeftFootTmpFinalPos.dx = LeftFootTmpInitPos.dx = 0.0;
+ LeftFootTmpFinalPos.dy = LeftFootTmpInitPos.dy =0.0;
+ LeftFootTmpFinalPos.dz = LeftFootTmpInitPos.dz =0.0;
+ LeftFootTmpFinalPos.domega = LeftFootTmpInitPos.domega =0.0;
+ LeftFootTmpFinalPos.domega2 = LeftFootTmpInitPos.domega2 =0.0;
+ LeftFootTmpFinalPos.stepType = 11;
+
+ RightFootTmpFinalPos = RightFootTmpInitPos;
+ RightFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
+ RightFootTmpFinalPos.omega = 0.0;
+ RightFootTmpFinalPos.omega2 = 0.0;
+ RightFootTmpFinalPos.dx = RightFootTmpInitPos.dx = 0.0;
+ RightFootTmpFinalPos.dy = RightFootTmpInitPos.dy =0.0;
+ RightFootTmpFinalPos.dz = RightFootTmpInitPos.dz =0.0;
+ RightFootTmpFinalPos.domega = RightFootTmpInitPos.domega =0.0;
+ RightFootTmpFinalPos.domega2 = RightFootTmpInitPos.domega2 =0.0;
+ RightFootTmpFinalPos.stepType = 9;
+
+ }
+
+ if ((i!=0)|| (Continuity))
+ {
+ /* Initialize properly the interval in single support phase */
+ ODEBUG("Single support phase");
+ ODEBUG("LeftFootTmpInitPos.stepType="<<LeftFootTmpInitPos.stepType);
+ ODEBUG("LeftFootTmpFinalPos.stepType="<<LeftFootTmpFinalPos.stepType);
+ SetAnInterval(IntervalIndex,m_LeftFootTrajectory,
+ LeftFootTmpInitPos,
+ LeftFootTmpFinalPos);
+
+
+ ODEBUG("LeftInit: ( " << LeftFootTmpInitPos.x << " , "
+ << LeftFootTmpInitPos.y << " , "
+ << LeftFootTmpInitPos.z << " ) ( "
+ << LeftFootTmpInitPos.dx << " , "
+ << LeftFootTmpInitPos.dy << " , "
+ << LeftFootTmpInitPos.dz << " ) "
+ << endl << "LeftFinal : ( "
+ << LeftFootTmpFinalPos.x << " , "
+ << LeftFootTmpFinalPos.y << " , "
+ << LeftFootTmpFinalPos.z << " ) ( "
+ << LeftFootTmpFinalPos.dx << " , "
+ << LeftFootTmpFinalPos.dy << " , "
+ << LeftFootTmpFinalPos.dz << " ) " );
+
+ ODEBUG("RightFootTmpInitPos.stepType="<<RightFootTmpInitPos.stepType);
+ ODEBUG("RightFootTmpFinalPos.stepType="<<RightFootTmpFinalPos.stepType);
+ ODEBUG("End of Single support phase");
+ SetAnInterval(IntervalIndex,m_RightFootTrajectory,
+ RightFootTmpInitPos,
+ RightFootTmpFinalPos);
+ // cout << RightFootTmpFinalPos.z << endl;
+ if (SupportFoot==1)
+ RightFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
+ else
+ LeftFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
+ ODEBUG("RightInit: ( " << RightFootTmpInitPos.x << " , "
+ << RightFootTmpInitPos.y << " , "
+ << RightFootTmpInitPos.z << " ) ( "
+ << RightFootTmpInitPos.dx << " , "
+ << RightFootTmpInitPos.dy << " , "
+ << RightFootTmpInitPos.dz << " ) "
+ << endl << "RightFinal : ( "
+ << RightFootTmpFinalPos.x << " , "
+ << RightFootTmpFinalPos.y << " , "
+ << RightFootTmpFinalPos.z << " ) ( "
+ << RightFootTmpFinalPos.dx << " , "
+ << RightFootTmpFinalPos.dy << " , "
+ << RightFootTmpFinalPos.dz << " ) " );
+ // Switch from single support to double support.
+ IntervalIndex++;
+ }
+
+ if ((!Continuity) && ((i==0) || (i==RelativeFootPositions.size()-1)))
+ {
+ /*! At this stage the phase of double support is deal with */
+ unsigned int limitk=1;
+
+ /*! If we are at the end a second double support phase has to be added. */
+ if (i==RelativeFootPositions.size()-1)
+ limitk=2;
+
+ for(unsigned int lk=0;lk<limitk;lk++)
+ {
+ LeftFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
+ LeftFootTmpFinalPos.dz = 0;
+ LeftFootTmpFinalPos.stepType = -1;
+
+ SetAnInterval(IntervalIndex,m_LeftFootTrajectory,
+ LeftFootTmpFinalPos,
+ LeftFootTmpFinalPos);
+ RightFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
+ RightFootTmpFinalPos.dz = 0;
+ RightFootTmpFinalPos.stepType = -1;
+ SetAnInterval(IntervalIndex,m_RightFootTrajectory,
+ RightFootTmpFinalPos,
+ RightFootTmpFinalPos);
+ IntervalIndex++;
+ }
+ }
+
+
+ /* The final position become the new initial position */
+ LeftFootTmpInitPos = LeftFootTmpFinalPos;
+ RightFootTmpInitPos = RightFootTmpFinalPos;
+
+
+ /* Populate the set of support foot absolute positions */
+ SupportFootAbsoluteFootPositions[i].x = CurrentSupportFootPosition(0,2);
+ SupportFootAbsoluteFootPositions[i].y = CurrentSupportFootPosition(1,2);
+ SupportFootAbsoluteFootPositions[i].z = CurrentSupportFootPosition(2,2);
+ SupportFootAbsoluteFootPositions[i].theta = CurrentAbsTheta;
+
+
+ if ((!IgnoreFirst) || (i>0))
+ SupportFoot=-SupportFoot;
+ }
+
+ /*! This part initializes correctly the last two intervals
+ if the system is in real-time foot modification. In this case,
+ the representation of the intervals shift from:
+ ONE DOUBLE SUPPORT STARTING PHASE - 1st foot single support phase - double support phase - 2nd foot single support phase
+ - double support phase - 3rd single support phase - ending double support phase
+ to
+ 1st foot single support phase - double support phase - 2nd foot single support phase
+ - double support phase - 3rd single support phase
+ Two intervals are missing and should be set by default to the end position of the feet
+ if Continuity is set to true, and if the number of intervals so far is the number of
+ intervals minus 2.
+ */
+ if ((Continuity) && (IntervalIndex==(int)(m_DeltaTj.size()-2)))
+ {
+ for(unsigned int lk=0;lk<2;lk++)
+ {
+ LeftFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
+ LeftFootTmpFinalPos.dz = 0;
+ SetAnInterval(IntervalIndex,m_LeftFootTrajectory,
+ LeftFootTmpFinalPos,
+ LeftFootTmpFinalPos);
+ RightFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
+ RightFootTmpFinalPos.dz = 0;
+ SetAnInterval(IntervalIndex,m_RightFootTrajectory,
+ RightFootTmpFinalPos,
+ RightFootTmpFinalPos);
+ IntervalIndex++;
+ }
+
+ }
+
}
void LeftAndRightFootTrajectoryGenerationMultiple::
@@ -579,7 +1027,7 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
{
FootAbsolutePosition aSupportFootAbsolutePosition;
- if (RelativeFootPositions[0].sy>0)
+ if (RelativeFootPositions[0].sy>0)
{
// The flying foot is on the left, thus the support foot is on the right.
ODEBUG("Detect support foot on the right.");
@@ -588,10 +1036,10 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
else
{
ODEBUG("Detect support foot on the left.");
- aSupportFootAbsolutePosition = LeftFootInitialPosition;
- }
+ aSupportFootAbsolutePosition = LeftFootInitialPosition;
+ }
aSupportFootAbsolutePosition.stepType=-1;
-
+
ComputeAbsoluteStepsFromRelativeSteps(RelativeFootPositions,
aSupportFootAbsolutePosition,
SupportFootAbsoluteFootPositions);
@@ -608,14 +1056,14 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
RelativeFootPositions.size())
SupportFootAbsoluteFootPositions.resize(RelativeFootPositions.size());
- unsigned int lNbOfIntervals = RelativeFootPositions.size();
+ unsigned int lNbOfIntervals = RelativeFootPositions.size();
/*! It is assumed that a set of relative positions for the support foot
are given as an input. */
deque<FootAbsolutePosition> AbsoluteFootPositions;
-
+
AbsoluteFootPositions.resize(lNbOfIntervals);
lNbOfIntervals = 2*lNbOfIntervals+1;
-
+
/*! Compute the absolute coordinates of the steps. */
double CurrentAbsTheta=0.0,c=0.0,s=0.0;
MAL_MATRIX_DIM(MM,double,2,2);
@@ -625,12 +1073,12 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
MAL_MATRIX_SET_IDENTITY(Orientation);
MAL_MATRIX_DIM(v,double,2,1);
MAL_MATRIX_DIM(v2,double,2,1);
-
+
ODEBUG("Detect support foot on the right.");
CurrentAbsTheta = SupportFootInitialAbsolutePosition.theta;
v2(0,0) = SupportFootInitialAbsolutePosition.x;
v2(1,0) = SupportFootInitialAbsolutePosition.y;
-
+
// Initial Position of the current support foot.
c = cos(CurrentAbsTheta*M_PI/180.0);
s = sin(CurrentAbsTheta*M_PI/180.0);
@@ -640,10 +1088,10 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
CurrentSupportFootPosition(k,l) = MM(k,l);
-
+
for(int k=0;k<2;k++)
CurrentSupportFootPosition(k,2) = v2(k,0);
-
+
/* Keep track of the interval index once this is
for single support, once for double support */
@@ -653,26 +1101,26 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
for(unsigned int i=0;i<RelativeFootPositions.size();i++)
{
- ODEBUG( i << " : " <<
- RelativeFootPositions[i].sx << " " <<
+ ODEBUG( i << " : " <<
+ RelativeFootPositions[i].sx << " " <<
RelativeFootPositions[i].sy);
-
+
/*! Compute Orientation matrix related to the relative orientation
of the support foot */
c = cos(RelativeFootPositions[i].theta*M_PI/180.0);
s = sin(RelativeFootPositions[i].theta*M_PI/180.0);
MM(0,0) = c; MM(0,1) = -s;
MM(1,0) = s; MM(1,1) = c;
-
+
/*! Update the orientation */
CurrentAbsTheta+= RelativeFootPositions[i].theta;
CurrentAbsTheta = fmod(CurrentAbsTheta,180.0);
-
+
/*! Extract the current absolute orientation matrix. */
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
Orientation(k,l) = CurrentSupportFootPosition(k,l);
-
+
/*! Put in a vector form the translation of the relative foot. */
v(0,0) = RelativeFootPositions[i].sx;
v(1,0) = RelativeFootPositions[i].sy;
@@ -680,21 +1128,21 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
/*! Compute the new orientation of the foot vector. */
Orientation = MAL_RET_A_by_B(MM , Orientation);
v2 = MAL_RET_A_by_B(Orientation, v);
-
+
/*! Update the world coordinates of the support foot. */
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
CurrentSupportFootPosition(k,l) = Orientation(k,l);
-
+
for(int k=0;k<2;k++)
CurrentSupportFootPosition(k,2) += v2(k,0);
AbsoluteFootPositions[i].x = CurrentSupportFootPosition(0,2);
AbsoluteFootPositions[i].y = CurrentSupportFootPosition(1,2);
AbsoluteFootPositions[i].theta = CurrentAbsTheta;
-
+
ODEBUG("CSFP:" << CurrentSupportFootPosition(0,2) << " " << CurrentSupportFootPosition(1,2));
-
+
/* Populate the set of support foot absolute positions */
SupportFootAbsoluteFootPositions[i].x = CurrentSupportFootPosition(0,2);
SupportFootAbsoluteFootPositions[i].y = CurrentSupportFootPosition(1,2);
@@ -713,7 +1161,7 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
if (ChangedInterval>=SupportFootAbsoluteFootPositions.size())
{
LTHROW("Pb: ChangedInterval is after the size of absolute foot stack.");
- return;
+ return;
}
MAL_S3x3_MATRIX(KM1,double);
@@ -722,7 +1170,7 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
MAL_S3x3_MATRIX_SET_IDENTITY(K);
MAL_S3x3_MATRIX(KP1,double);
MAL_S3x3_MATRIX_SET_IDENTITY(KP1);
-
+
double thetakm1,xkm1,ykm1,c,s;
// Change the previous relative position.
@@ -732,7 +1180,7 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
xkm1 = SupportFootInitialPosition.x;
ykm1 = SupportFootInitialPosition.y;
}
- else
+ else
{
thetakm1= SupportFootAbsoluteFootPositions[ChangedInterval-1].theta;
xkm1 = SupportFootAbsoluteFootPositions[ChangedInterval-1].x;
@@ -755,7 +1203,7 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
s = sin(thetak*M_PI/180.0);
K(0,0) = c; K(0,1) = -s; K(0,2) = xk;
K(1,0) = s; K(1,1) = c; K(1,2) = yk;
-
+
MAL_S3x3_MATRIX(iKM1,double);
MAL_S3x3_INVERSE(KM1,iKM1,double);
MAL_S3x3_MATRIX(relMotionM1,double);
@@ -774,22 +1222,22 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
thetakp1= SupportFootAbsoluteFootPositions[ChangedInterval+1].theta;
xkp1 = SupportFootAbsoluteFootPositions[ChangedInterval+1].x;
ykp1 = SupportFootAbsoluteFootPositions[ChangedInterval+1].y;
-
+
c = cos(thetakp1*M_PI/180.0);
s = sin(thetakp1*M_PI/180.0);
KP1(0,0) = c; KP1(0,1) = -s; KP1(0,2) = xkp1;
KP1(1,0) = s; KP1(1,1) = c; KP1(1,2) = ykp1;
-
+
MAL_S3x3_MATRIX(iK,double);
MAL_S3x3_INVERSE(K,iK,double);
MAL_S3x3_MATRIX(relMotionP1,double);
-
+
MAL_S3x3_C_eq_A_by_B(relMotionP1,iK,KP1);
-
+
RelativeFootPositions[ChangedInterval+1].sx = relMotionP1(0,2);
RelativeFootPositions[ChangedInterval+1].sy = relMotionP1(1,2);
RelativeFootPositions[ChangedInterval+1].theta = atan2(relMotionP1(1,0),relMotionP1(0,0));
-
+
}
ODEBUG("KP1 position: " << xkp1 << " " << ykp1 << " " << thetakp1 );
@@ -812,7 +1260,7 @@ bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition
}
return r;
}
- else
+ else
{
bool r = m_RightFootTrajectory->Compute(time,aFAP);
if (!r)
@@ -832,14 +1280,30 @@ bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition
{
ODEBUG(this << " " << m_LeftFootTrajectory << " " << m_RightFootTrajectory);
-
+
if (LeftOrRight==1)
return m_LeftFootTrajectory->Compute(time,aFAP,IndexInterval);
- else
+ else
return m_RightFootTrajectory->Compute(time,aFAP,IndexInterval);
-
+
return false;
}
+/*
+bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition(int LeftOrRight,
+ double time,
+ deque<FootAbsolutePosition> & adFAP,
+ unsigned int IndexInterval)
+{
+
+ ODEBUG(this << " " << m_LeftFootTrajectory << " " << m_RightFootTrajectory);
+
+ if (LeftOrRight==1)
+ return m_LeftFootTrajectory->Compute(time,adFAP,IndexInterval);
+ else
+ return m_RightFootTrajectory->Compute(time,adFAP,IndexInterval);
+
+ return false;
+}*/
void LeftAndRightFootTrajectoryGenerationMultiple::SetDeltaTj(vector<double> &aDeltaTj)
{
@@ -850,7 +1314,7 @@ void LeftAndRightFootTrajectoryGenerationMultiple::SetDeltaTj(vector<double> &aD
m_LeftFootTrajectory->SetTimeIntervals(m_DeltaTj);
if (m_RightFootTrajectory!=0)
m_RightFootTrajectory->SetTimeIntervals(m_DeltaTj);
-
+
}
void LeftAndRightFootTrajectoryGenerationMultiple::DisplayIntervals()
@@ -858,7 +1322,7 @@ void LeftAndRightFootTrajectoryGenerationMultiple::DisplayIntervals()
ODEBUG3("Left intervals");
m_LeftFootTrajectory->DisplayIntervals();
ODEBUG3("Right intervals");
- m_RightFootTrajectory->DisplayIntervals();
+ m_RightFootTrajectory->DisplayIntervals();
}
void LeftAndRightFootTrajectoryGenerationMultiple::GetDeltaTj(vector<double> &aDeltaTj) const
@@ -888,7 +1352,7 @@ double LeftAndRightFootTrajectoryGenerationMultiple::GetAbsoluteTimeReference()
res=-1;
else
res = LeftATR;
-
+
return res;
}
@@ -900,9 +1364,9 @@ void LeftAndRightFootTrajectoryGenerationMultiple::SetAbsoluteTimeReference(doub
m_RightFootTrajectory->SetAbsoluteTimeReference(anATR);
}
-LeftAndRightFootTrajectoryGenerationMultiple &
+LeftAndRightFootTrajectoryGenerationMultiple &
LeftAndRightFootTrajectoryGenerationMultiple::operator=
-(const LeftAndRightFootTrajectoryGenerationMultiple & aLRFTGM)
+(const LeftAndRightFootTrajectoryGenerationMultiple & aLRFTGM)
{
ODEBUG("Went through this.");
if (this == &aLRFTGM)
@@ -916,7 +1380,7 @@ LeftAndRightFootTrajectoryGenerationMultiple::operator=
*m_RightFootTrajectory = *(aLRFTGM.getRightFootTrajectory());
m_Foot = aLRFTGM.getFoot();
-
+
return *this;
}
@@ -930,3 +1394,5 @@ FootTrajectoryGenerationMultiple * LeftAndRightFootTrajectoryGenerationMultiple:
{
return m_RightFootTrajectory;
}
+
+
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
index 63152a9abfbfb789e39869769e52ceba54ad1322..b7a541e0ef5a4d74ec57457ebe4580a5cf83224e 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file LeftAndRightFootTrajectoryGenerationMultiple.h
@@ -45,9 +45,9 @@ namespace PatternGeneratorJRL
/*! @ingroup foottrajectorygeneration
The main goal of this class is to provide a simple interface to the
- objects derived from ZMPRefTrajectoryGeneration to generate the
+ objects derived from ZMPRefTrajectoryGeneration to generate the
foot trajectories.
-
+
It acts as a container for two FootTrajectoryGenerationMultiple objects
which handle several polynomials trajectory generation for the right and left
feet.
@@ -55,7 +55,7 @@ namespace PatternGeneratorJRL
It provides an initialization of the underlying objects assuming that
some basic informations have been provided: single support time, double support
time, omega, step height.
-
+
The information used follow the previously defined script like language,
but it could be extended for a clear separation between the two feet.
@@ -63,18 +63,18 @@ namespace PatternGeneratorJRL
class LeftAndRightFootTrajectoryGenerationMultiple : public SimplePlugin
{
- public:
+ public:
/*! \brief The constructor initialize the plugin part, and the data related to the humanoid. */
LeftAndRightFootTrajectoryGenerationMultiple(SimplePluginManager * lSPM,
CjrlFoot * inFoot);
/*! \brief Copy constructor. */
LeftAndRightFootTrajectoryGenerationMultiple(const LeftAndRightFootTrajectoryGenerationMultiple &);
-
+
/*! \brief Memory release. */
~LeftAndRightFootTrajectoryGenerationMultiple();
- /*! \brief Reimplementation of the call method for the plugin manager.
+ /*! \brief Reimplementation of the call method for the plugin manager.
More explicitly this object will deal with the call which initialize
the feet behaviors (\f$omega\f$, \f$ stepheight \f$) .
*/
@@ -87,17 +87,23 @@ namespace PatternGeneratorJRL
@param[in] LeftFootInitialPosition: The initial position of the left foot.
@param[in] RightFootInitialPosition: the initial position of the right foot.
@param[out] SupportFootAbsoluteFootPositions: The set of absolute foot positions
- corresponding to the set of relative foot positions (i.e given step by step
+ corresponding to the set of relative foot positions (i.e given step by step
and not every sampled control time).
@param[in] IgnoreFirst: Ignore the first double support phase, should be true at beginning of stepping.
@param[in] Continuity: Should be true if more steps should be added, false to stop stepping.
*/
+ void InitializeFromRelativeSteps_backup(deque<RelativeFootPosition> &RelativeFootPositions,
+ FootAbsolutePosition &LeftFootInitialPosition,
+ FootAbsolutePosition &RightFootInitialPosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
+ bool IgnoreFirst, bool Continuity);
+
void InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
FootAbsolutePosition &LeftFootInitialPosition,
FootAbsolutePosition &RightFootInitialPosition,
deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
bool IgnoreFirst, bool Continuity);
-
+
/*! \brief Method to compute the absolute position of the foot.
@param[in] LeftOrRight: -1 indicates the right foot, 1 indicates the left foot.
@param[in] time: The absolute time to be compared with the absolute reference defining the start
@@ -112,21 +118,26 @@ namespace PatternGeneratorJRL
@param[in] time: The absolute time to be compared with the absolute reference defining the start
of the trajectory.
@param[out] aFootAbsolutePosition: The data structure to be filled with the information
- \f$ (x,y,z,\omega, \omega_2, \theta) \f$.
+ \f$ (x,y,z,\omega, \omega_2, \theta) \f$.
@param[in] IndexInterval: On which interval to compute the foot position.
-
+
*/
bool ComputeAnAbsoluteFootPosition(int LeftOrRight, double time, FootAbsolutePosition & aFootAbsolutePosition,
unsigned int IndexInterval);
+ /*
+ bool ComputeAnAbsoluteFootPosition(int LeftOrRight,
+ double time,
+ std::deque<FootAbsolutePosition> & adFAP,
+ unsigned int IndexInterval);*/
- /*! \brief Method to compute absolute feet positions from a set of relative one.
+ /*! \brief Method to compute absolute feet positions from a set of relative one.
@param[in] RelativeFootPositions: The set of relative positions for the support foot.
@param[in] LeftFootInitialPosition: The initial position of the left foot.
@param[in] RightFootInitialPosition: the initial position of the right foot.
@param[out] SupportFootAbsoluteFootPositions: The set of absolute foot positions
- corresponding to the set of relative foot positions (i.e given step by step
- and not every sampled control time).
+ corresponding to the set of relative foot positions (i.e given step by step
+ and not every sampled control time)
*/
void ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
FootAbsolutePosition &LeftFootInitialPosition,
@@ -137,7 +148,7 @@ namespace PatternGeneratorJRL
@param[in] RelativeFootPositions: The set of relative positions for the support foot.
@param[in] SupportFootInitialPosition: The initial position of the support foot.
@param[out] SupportFootAbsoluteFootPositions: The set of absolute foot positions
- corresponding to the set of relative foot positions (i.e given step by step
+ corresponding to the set of relative foot positions (i.e given step by step
and not every sampled control time).
*/
void ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
@@ -150,14 +161,14 @@ namespace PatternGeneratorJRL
@param[in] ChangedInterval: The interval where the absolute foot position has been changed.
@param[in] SupportFootInitialPosition: The absolute foot position of the initial step in the stack of steps.
@param[out] SupportFootAbsoluteFootPositions: The set of absolute foot positions
- corresponding to the set of relative foot positions (i.e given step by step
+ corresponding to the set of relative foot positions (i.e given step by step
and not every sampled control time).
*/
void ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
FootAbsolutePosition &SupportFootInitialPosition,
deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
unsigned int ChangedInterval);
-
+
/*! Returns foot */
CjrlFoot * getFoot() const;
@@ -171,28 +182,28 @@ namespace PatternGeneratorJRL
/*! Left Foot Trajectory Generation object for several intervals. */
FootTrajectoryGenerationMultiple * m_LeftFootTrajectory;
-
+
/*! Right Foot Trajectory Generation object for several intervals. */
FootTrajectoryGenerationMultiple * m_RightFootTrajectory;
/*! Humanoid specificities object handler */
CjrlFoot * m_Foot;
-
+
/*! Set of time intervals */
std::vector<double> m_DeltaTj;
/*! Omega */
double m_Omega;
-
+
/*! Step height. */
double m_StepHeight;
/*! Single support time. */
double m_SingleSupportTime;
-
+
/*! Double support time. */
double m_DoubleSupportTime;
-
+
public:
/*! Set the intervals time */
void SetDeltaTj(std::vector<double> & aDeltaTj);
@@ -207,12 +218,12 @@ namespace PatternGeneratorJRL
void SetStepHeight(double aStepHeight);
/*! Get the step height. */
- double GetStepHeight() const;
+ double GetStepHeight() const;
/*! \name Methods related to the time reference.
@{ */
- /*! \brief This returns the absolute time reference.
+ /*! \brief This returns the absolute time reference.
As the object manipulates several trajectories generator,
the coherency of the returned informations has to be checked.
If this is not the case, i.e. the trajectory generators have different
@@ -230,7 +241,7 @@ namespace PatternGeneratorJRL
LeftAndRightFootTrajectoryGenerationMultiple & operator=
(const LeftAndRightFootTrajectoryGenerationMultiple & aLRFTGM);
-
+
};
}
#endif /* _LEFT_AND_RIGHT_FOOT_TRAJECTORY_GENERATION_MULTIPLE_H_ */
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index 8fc7948bcedf4170e8c58606572019ba223ec9ea..2c3def4cb2014817cb9efa82f306c0e4df0e6059 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -96,9 +96,6 @@ void
double EndOfLiftOff = (m_TSingle-UnlockedSwingPeriod)*0.5;
double StartLanding = EndOfLiftOff + UnlockedSwingPeriod;
- //cout << "EndOfLiftOff interpol = " << EndOfLiftOff << endl ;
- //cout << "UnlockedSwingPeriod interpol = " << UnlockedSwingPeriod << endl ;
-
FootAbsolutePosition & curr_NSFAP = NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex];
const FootAbsolutePosition & prev_NSFAP = NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1];
@@ -142,8 +139,33 @@ void
ComputeXYThetaFootPosition(InterpolationTime,curr_NSFAP);
}
- curr_NSFAP.z = m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
- curr_NSFAP.dz = m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
+ if (m_isStepStairOn == 0)
+ {
+ curr_NSFAP.z = m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
+ //m_AnklePositionRight[2];
+ curr_NSFAP.dz = m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
+
+ //m_AnklePositionRight[2];
+ }
+
+ else
+
+ {
+ if (m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime) == 0)
+ {
+ curr_NSFAP.z = prev_NSFAP.z;
+ }
+ else
+ {
+ curr_NSFAP.z = m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
+
+ }//m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
+ //m_AnklePositionRight[2];
+ curr_NSFAP.dz = m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
+ //m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
+ //m_AnklePositionRight[2];
+ }
+
bool ProtectionNeeded=false;
@@ -362,7 +384,16 @@ void
LastSFP->y, LastSFP->dy, LastSFP->ddy, LastSFP->dddy
);
if(CurrentSupport.StateChanged==true)
- SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
+ {
+
+
+ if (m_isStepStairOn == 0)
+ SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
+ else
+ SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::Z_AXIS,
+ m_TSingle,StepHeight_,
+ LastSFP->z, LastSFP->dz);
+ }
SetParameters(
FootTrajectoryGenerationStandard::THETA_AXIS,
@@ -423,18 +454,17 @@ void
}
}
else if (CurrentSupport.Phase == DS || Time+3.0/2.0*QP_T_ > CurrentSupport.TimeLimit)
- {
- cout << " double support phase || " << Time+3.0/2.0*QP_T_ << " > " << CurrentSupport.TimeLimit << endl ;
- for(int k = 0; k<=(int)(QP_T_/m_SamplingPeriod);k++)
{
- FinalRightFootTraj_deq[CurrentIndex+k]= FinalRightFootTraj_deq[CurrentIndex+k-1];
- FinalLeftFootTraj_deq[CurrentIndex+k]= FinalLeftFootTraj_deq[CurrentIndex+k-1];
- FinalLeftFootTraj_deq[CurrentIndex+k].time =
- FinalRightFootTraj_deq[CurrentIndex+k].time = Time+k*m_SamplingPeriod;
- FinalLeftFootTraj_deq[CurrentIndex+k].stepType =
- FinalRightFootTraj_deq[CurrentIndex+k].stepType = 10;
+ for(int k = 0; k<=(int)(QP_T_/m_SamplingPeriod);k++)
+ {
+ FinalRightFootTraj_deq[CurrentIndex+k]= FinalRightFootTraj_deq[CurrentIndex+k-1];
+ FinalLeftFootTraj_deq[CurrentIndex+k]= FinalLeftFootTraj_deq[CurrentIndex+k-1];
+ FinalLeftFootTraj_deq[CurrentIndex+k].time =
+ FinalRightFootTraj_deq[CurrentIndex+k].time = Time+k*m_SamplingPeriod;
+ FinalLeftFootTraj_deq[CurrentIndex+k].stepType =
+ FinalRightFootTraj_deq[CurrentIndex+k].stepType = 10;
+ }
}
- }
}
diff --git a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
index c64f995f1b32657b032585888f3695c9c0db2b95..1671e05f2b62b856b32684ed99b129b7722c8f93 100644
--- a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
+++ b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
@@ -32,7 +32,7 @@
#include <Debug.hh>
using namespace std;
-namespace PatternGeneratorJRL
+namespace PatternGeneratorJRL
{
AnalyticalZMPCOGTrajectory::AnalyticalZMPCOGTrajectory(int lNbOfIntervals)
@@ -71,7 +71,7 @@ namespace PatternGeneratorJRL
void AnalyticalZMPCOGTrajectory::SetNumberOfIntervals(unsigned int lNbOfIntervals)
{
m_NbOfIntervals = lNbOfIntervals;
-
+
m_V.resize(m_NbOfIntervals);
m_W.resize(m_NbOfIntervals);
m_DeltaTj.resize(m_NbOfIntervals);
@@ -101,7 +101,7 @@ namespace PatternGeneratorJRL
{
double deltaj=0.0;
deltaj = t-reftime;
-
+
r = cosh(m_omegaj[j]*deltaj) * m_V[j] +
sinh(m_omegaj[j]*deltaj) * m_W[j];
ODEBUG( " coefficients hyperbolique : " << r << \
@@ -116,11 +116,11 @@ namespace PatternGeneratorJRL
add2r = m_ListOfCOGPolynomials[j]->Compute(deltaj);
ODEBUG( " add2r: " << add2r <<" " \
<< "Polynomial ("<<j<<") with degree (supposed:"<< m_PolynomialDegree[j] << ")"<<endl \
- << "Coefficients:");
+ << "Coefficients:");
// m_ListOfCOGPolynomials[j]->print();
r+=add2r;
}
- else
+ else
return false;
return true;
}
@@ -130,7 +130,7 @@ namespace PatternGeneratorJRL
return false;
}
-
+
bool AnalyticalZMPCOGTrajectory::ComputeCOMSpeed(double t, double &r)
{
t -= m_AbsoluteTimeReference;
@@ -144,7 +144,7 @@ namespace PatternGeneratorJRL
{
double deltaj=0.0;
deltaj = t-reftime;
-
+
r = m_omegaj[j] * sinh(m_omegaj[j]*deltaj) * m_V[j] +
m_omegaj[j] * cosh(m_omegaj[j]*deltaj) * m_W[j];
ODEBUG( " coefficients hyperbolique : " << r << \
@@ -158,11 +158,11 @@ namespace PatternGeneratorJRL
add2r = m_ListOfCOGPolynomials[j]->ComputeDerivative(deltaj);
ODEBUG( " add2r: " << add2r <<" " \
<< "Polynomial ("<<j<<") with degree (supposed:"<< m_PolynomialDegree[j] << ")"<<endl \
- << "Coefficients:");
+ << "Coefficients:");
// m_ListOfCOGPolynomials[j]->print();
r+=add2r;
}
- else
+ else
return false;
return true;
}
@@ -176,7 +176,6 @@ namespace PatternGeneratorJRL
{
double deltaj=0.0;
deltaj = t- m_AbsoluteTimeReference - m_RefTime[j];
-
r = cosh(m_omegaj[j]*deltaj) * m_V[j] +
sinh(m_omegaj[j]*deltaj) * m_W[j];
r += m_ListOfCOGPolynomials[j]->Compute(deltaj);
@@ -187,7 +186,7 @@ namespace PatternGeneratorJRL
{
double deltaj=0.0;
deltaj = t- m_AbsoluteTimeReference - m_RefTime[j];
-
+
r = m_omegaj[j] * sinh(m_omegaj[j]*deltaj) * m_V[j] +
m_omegaj[j] * cosh(m_omegaj[j]*deltaj) * m_W[j];
r += m_ListOfCOGPolynomials[j]->ComputeDerivative(deltaj);
@@ -225,18 +224,18 @@ namespace PatternGeneratorJRL
" deltaj " << deltaj << \
" m_V[j]:" << m_V[j] << \
" m_W[j]:" << m_W[j] );
-
+
if (m_ListOfZMPPolynomials[j]!=0)
{
double add2r;
add2r = m_ListOfZMPPolynomials[j]->Compute(deltaj);
ODEBUG( " add2r: " << add2r << " " );
ODEBUG("Polynomial ("<<j<<") with degree (supposed:"<< m_PolynomialDegree[j] << ")");
- ODEBUG("Coefficients:");
+ ODEBUG("Coefficients:");
// m_ListOfZMPPolynomials[j]->print();
- r+=add2r;
+ r+=add2r;
}
- else
+ else
return false;
return true;
}
@@ -258,7 +257,7 @@ namespace PatternGeneratorJRL
{
double deltaj=0.0;
deltaj = t-reftime;
-
+
r = 0.0;
ODEBUG( " coefficients hyperbolique : " << r << \
" m_omegaj[j]=" << m_omegaj[j] << \
@@ -271,11 +270,11 @@ namespace PatternGeneratorJRL
add2r = m_ListOfCOGPolynomials[j]->ComputeDerivative(deltaj);
ODEBUG( " add2r: " << add2r <<" " \
<< "Polynomial ("<<j<<") with degree (supposed:"<< m_PolynomialDegree[j] << ")"<<endl \
- << "Coefficients:");
+ << "Coefficients:");
// m_ListOfCOGPolynomials[j]->print();
r+=add2r;
}
- else
+ else
return false;
return true;
}
@@ -316,11 +315,11 @@ namespace PatternGeneratorJRL
reftime += m_DeltaTj[j];
}
}
- else
+ else
cerr << "Pb while initializing the time intervals. " << lTj.size() << " " << m_NbOfIntervals << endl;
if ((int)lomegaj.size()==m_NbOfIntervals)
m_omegaj = lomegaj;
- else
+ else
cerr << "Pb while initializing the omega " <<endl;
}
@@ -328,7 +327,7 @@ namespace PatternGeneratorJRL
{
if ((int)lPolynomialDegree.size()==m_NbOfIntervals)
m_PolynomialDegree=lPolynomialDegree;
-
+
for(unsigned int i=0;i<m_PolynomialDegree.size();i++)
{
@@ -336,7 +335,7 @@ namespace PatternGeneratorJRL
if (m_ListOfCOGPolynomials[i]!=0)
delete m_ListOfCOGPolynomials[i];
m_ListOfCOGPolynomials[i] = new Polynome(m_PolynomialDegree[i]);
-
+
// m_ListOfCOGPolynomials[i]->print();
if (m_ListOfZMPPolynomials[i]!=0)
delete m_ListOfZMPPolynomials[i];
@@ -402,11 +401,11 @@ namespace PatternGeneratorJRL
vector<double> CoefsFromCOG;
vector<double> CoefsForZMP;
-
+
if ((m_ListOfCOGPolynomials[intervalindex]!=0) &&
(m_ListOfZMPPolynomials[intervalindex]!=0))
{
-
+
m_ListOfCOGPolynomials[intervalindex]->GetCoefficients(CoefsFromCOG);
// m_ListOfCOGPolynomials[j]->print();
CoefsForZMP.resize(CoefsFromCOG.size());
@@ -416,7 +415,7 @@ namespace PatternGeneratorJRL
for(unsigned int i=0;i<m_PolynomialDegree[intervalindex]-1;++i)
CoefsForZMP[i] = CoefsFromCOG[i] - ((double)(i+1.0)*(double)(i+2.0))*(CoefsFromCOG[i+2]/sTc);
m_ListOfZMPPolynomials[intervalindex]->SetCoefficients(CoefsForZMP);
-
+
}
}
@@ -454,7 +453,7 @@ namespace PatternGeneratorJRL
CoefsForZMP[3] = -2.0 * CoefsForZMP[2] /(3.0 * lTj);
m_ListOfZMPPolynomials[anIntervalj]->SetCoefficients(CoefsForZMP);
}
-
+
double Omegac = m_omegaj[anIntervalj];
if (m_ListOfCOGPolynomials[anIntervalj]!=0)
{
@@ -463,22 +462,22 @@ namespace PatternGeneratorJRL
CoefsForCOG[2] = CoefsForZMP[2];
CoefsForCOG[1] = CoefsForZMP[1] + CoefsForCOG[3] * 6/(Omegac *Omegac);
CoefsForCOG[0] = CoefsForZMP[0] + CoefsForCOG[2] * 2/(Omegac *Omegac);
- m_ListOfCOGPolynomials[anIntervalj]->SetCoefficients(CoefsForCOG);
- }
+ m_ListOfCOGPolynomials[anIntervalj]->SetCoefficients(CoefsForCOG);
+ }
}
double AnalyticalZMPCOGTrajectory::FluctuationMaximal()
{
-
+
double Tmax;
vector<double> CoefsForZMP;
m_ListOfZMPPolynomials[0]->GetCoefficients(CoefsForZMP);
Tmax = m_AbsoluteTimeReference - 2* CoefsForZMP[2] * m_DeltaTj[0]/
(2.0 * CoefsForZMP[2] + 3.0 * CoefsForZMP[3] * m_DeltaTj[0]);
-
+
return Tmax;
-
+
}
bool AnalyticalZMPCOGTrajectory::GetIntervalIndexFromTime(double t, unsigned int &j)
@@ -505,13 +504,13 @@ namespace PatternGeneratorJRL
}
return false;
}
-
+
ostream& operator <<(ostream &os,const AnalyticalZMPCOGTrajectory &obj)
{
vector<double> lV,lW;
vector<unsigned int> lPolynomialDegrees;
-
+
obj.GetPolynomialDegrees(lPolynomialDegrees);
unsigned int lNbOfIntervals;
@@ -528,7 +527,7 @@ namespace PatternGeneratorJRL
obj.GetFromListOfZMPPolynomials(i,aPolynome);
aPolynome->GetCoefficients(CoefsForZMP);
obj.GetFromListOfCOGPolynomials(i,aPolynome);
- aPolynome->GetCoefficients(CoefsForCOG);
+ aPolynome->GetCoefficients(CoefsForCOG);
os << " COG: " << endl;
for(unsigned int j=0;j<=lPolynomialDegrees[i];j++)
{
diff --git a/src/Mathematics/Bsplines.cpp b/src/Mathematics/Bsplines.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..931eed345645a627ad3a31556fd92d81f3bf5ff1
--- /dev/null
+++ b/src/Mathematics/Bsplines.cpp
@@ -0,0 +1,453 @@
+#include <iostream>
+#include <vector>
+
+#include <Debug.hh>
+#include <Mathematics/Bsplines.hh>
+
+
+using namespace::std;
+using namespace::PatternGeneratorJRL;
+
+
+Bsplines::Bsplines(int degree)
+{
+ m_degree = degree;
+ m_control_points.clear();
+ m_knot_vector.clear();
+}
+
+Bsplines::~Bsplines()
+{
+}
+
+void Bsplines::GenerateDegree()
+{
+ m_degree = (m_knot_vector.size()-1) - (m_control_points.size()-1) -1;
+ if (m_degree < 0)
+ {
+ cout << " Attention !! degree is smaller than 0 " << endl;
+ }
+}
+
+void Bsplines::GenerateKnotVector(std::string method)
+{
+ /*Calculer set of parameters*/
+ int i,j;
+ vector<double> set_of_pam;
+
+ if (method == "centripetal")
+ {
+ //cout << "centripetal" << endl;
+ set_of_pam.clear();
+ set_of_pam.reserve(m_control_points.size());
+ cout << m_control_points.size() << endl;
+ double L = 0.0;
+ double D = 0.0;
+ for (i=0;i<m_control_points.size()-1;i++)
+ {
+ L += sqrt ( sqrt(((m_control_points[i].x - m_control_points[i+1].x)*(m_control_points[i].x - m_control_points[i+1].x)) +
+ ((m_control_points[i].y - m_control_points[i+1].y)*(m_control_points[i].y - m_control_points[i+1].y)) ) );
+ }
+ for (i=0;i<m_control_points.size();i++)
+ {
+ if (i == 0)
+ {
+ set_of_pam.push_back(0.0);
+ }
+ else if (i == m_control_points.size()-1)
+ {
+ set_of_pam.push_back(1.0);
+ }
+ else
+ {
+ D += sqrt ( sqrt(((m_control_points[i].x - m_control_points[i+1].x)*(m_control_points[i].x - m_control_points[i+1].x)) +
+ ((m_control_points[i].y - m_control_points[i+1].y)*(m_control_points[i].y - m_control_points[i+1].y)) ) );
+ set_of_pam.push_back(D/L);
+ }
+ }
+
+ m_knot_vector.clear();
+ double U = 0.0;
+ for (i=0;i<=m_degree;i++)
+ {
+ m_knot_vector.push_back(0.0);
+ }
+
+ if (m_control_points.size()-1>=m_degree)
+ {
+ for (j=1;j<=m_control_points.size()-1-m_degree;j++)
+ {
+ i=j;
+ U=0.0;
+ while (i<=m_degree-1+j)
+ {
+ U +=set_of_pam[i];
+ i++;
+ }
+ m_knot_vector.push_back(U/double(m_degree));
+ }
+ }
+
+ for (i=0;i<=m_degree;i++)
+ {
+ m_knot_vector.push_back(1.0);
+ }
+ if (m_knot_vector.size() - 1 != m_control_points.size() + m_degree)
+ {
+ cout << "Knot vector cant be created. m_control_points.size()-1>=m_degree "<< endl;
+ m_knot_vector.clear();
+ }
+
+ }
+
+ else if (method =="universal")
+ {
+ m_knot_vector.clear();
+ //cout << "universal" << endl;
+ double U=0;
+ for (i=0;i<=m_degree;i++)
+ {
+ m_knot_vector.push_back(0.0);
+ }
+ for (i=1;i<=m_control_points.size()-1-m_degree;i++)
+ {
+ U = double(i)/(m_control_points.size()-1-m_degree+1);
+ m_knot_vector.push_back(U);
+ }
+ for (i=0;i<=m_degree;i++)
+ {
+ m_knot_vector.push_back(1.0);
+ }
+
+ }
+}
+
+double *Bsplines::ComputeBasisFunction(double t)
+{
+ double **m_basis_function;
+ m_basis_function = new double* [m_degree+1];
+ int i,j,n;
+
+ if (m_degree!= m_knot_vector.size() - m_control_points.size() -1 )
+ {
+ cout << "The parameters are not compatibles. Please recheck " << endl;
+ return NULL;
+ }
+ for(j=0;j <= m_degree;j++)
+ {
+ n = m_knot_vector.size() - 1 -j -1;
+ m_basis_function[j] = new double[n+1];
+ //cout << "order " ; cout << j << endl;
+ //cout << "n control point " ; cout << n << endl;
+
+ for(i=0;i <= n;i++)
+ {
+ if (j == 0 && m_knot_vector[i]<= t && t< m_knot_vector[i+1])
+ {
+ m_basis_function[j][i] = 1.0;
+ //cout << i << " "<< j << " "<<m_basis_function[j][i] << endl;
+ }
+
+ else if (j == 0)
+ {
+ m_basis_function[j][i] = 0.0;
+ //cout << i << " "<< j << " "<<m_basis_function[j][i] << endl;
+ }
+ else if (j != 0)
+ {
+ if ( m_knot_vector[i] != m_knot_vector[i+j] && m_knot_vector[i+j+1] != m_knot_vector[i+1] )
+ m_basis_function[j][i]= m_basis_function[j-1][i]*((t - m_knot_vector[i])/(m_knot_vector[i+j]-m_knot_vector[i]))
+ + m_basis_function[j-1][i+1]*((m_knot_vector[i+j+1] - t)/ (m_knot_vector[i+j+1]-m_knot_vector[i+1] ));
+
+ else if ( m_knot_vector[i] == m_knot_vector[i+j] && m_knot_vector[i+j+1] == m_knot_vector[i+1] )
+ m_basis_function[j][i] = 0.0;
+
+ else if (m_knot_vector[i] == m_knot_vector[i+j])
+ m_basis_function[j][i]= m_basis_function[j-1][i+1]*((m_knot_vector[i+j+1] - t)/ (m_knot_vector[i+j+1]-m_knot_vector[i+1] ));
+
+ else if (m_knot_vector[i+j+1] == m_knot_vector[i+1])
+ m_basis_function[j][i]= m_basis_function[j-1][i]*((t - m_knot_vector[i])/(m_knot_vector[i+j]-m_knot_vector[i]));
+
+ //cout << i << " "<< j << " "<<m_basis_function[j][i] << endl;
+ }
+ }
+ }
+ for(j=0;j < m_degree;j++)
+ {
+ delete[] m_basis_function[j];
+ }
+
+return m_basis_function[m_degree];
+}
+
+Point Bsplines::ComputeBsplines(double t)
+{
+ double *m_basis_function = ComputeBasisFunction(t);
+
+ Point C = {0.0,0.0};
+ if (m_degree!= m_knot_vector.size() - m_control_points.size() -1 )
+ {
+ cout << "The parameters are not compatibles. Please recheck " << endl;
+ return C;
+ }
+ if (m_degree == ((m_knot_vector.size()-1) - (m_control_points.size()-1)- 1) )
+ {
+ for (int i=0;i<m_control_points.size();i++)
+ {
+ C.x += m_basis_function[i] * m_control_points[i].x;
+ C.y += m_basis_function[i] * m_control_points[i].y;
+ }
+ }
+return C;
+}
+
+Bsplines Bsplines::DerivativeBsplines()
+{
+ std::vector<Point> Q;
+ Q.clear();
+ Q.reserve(m_control_points.size()-1);
+
+ Point T;
+ if (m_degree >=1)
+ {
+ for (int i=0;i<m_control_points.size()-1;i++)
+ {
+ T.x = ((m_control_points[i+1].x - m_control_points[i].x)*double(m_degree) )/ (m_knot_vector[i+m_degree+1] - m_knot_vector[i+1]);
+
+ T.y = ((m_control_points[i+1].y - m_control_points[i].y)*double(m_degree) )/ (m_knot_vector[i+m_degree+1] - m_knot_vector[i+1]);
+ Q.push_back(T);
+ }
+ Bsplines B(m_degree-1);
+ B.SetControlPoints(Q);
+ std::vector<double> new_knot_vector(m_knot_vector.begin()+1,m_knot_vector.end()-1);
+ B.SetKnotVector(new_knot_vector);
+ //cout <<"derivative function "<<endl;
+ //B.PrintKnotVector();
+ //B.PrintControlPoints();
+ //B.PrintDegree();
+ return B;
+ }
+ else
+ {
+ std::cout << "the function cannot be derivative " << std::endl;
+ return Bsplines(m_degree);
+ }
+}
+
+void Bsplines::SetDegree(int degree)
+{
+ m_degree = degree;
+}
+
+void Bsplines::SetControlPoints(std::vector<Point> &control_points)
+{
+ if (control_points.size()>=2)
+ {
+ m_control_points = control_points;
+ }
+ else
+ {
+ std::cout << "You must give at least 2 control points" << std::endl;
+ }
+}
+
+void Bsplines::SetKnotVector(std::vector<double> &knot_vector)
+{
+ m_knot_vector = knot_vector;
+}
+
+int Bsplines::GetDegree() const
+{
+ return m_degree;
+}
+
+std::vector<Point> Bsplines::GetControlPoints() const
+{
+ return m_control_points;
+}
+
+std::vector<double> Bsplines::GetKnotVector() const
+{
+ return m_knot_vector;
+}
+
+void Bsplines::PrintKnotVector() const
+{
+ std::cout << "Knot Vector: "<< std::endl;
+ for (int i = 0;i<m_knot_vector.size();i++)
+ {
+ std::cout << m_knot_vector[i] << " , ";
+ }
+ std::cout <<" " <<std::endl;
+}
+
+void Bsplines::PrintControlPoints() const
+{
+ std::cout << "Control Points : "<< std::endl;
+ for (int i = 0;i<m_control_points.size();i++)
+ {
+ std::cout << m_control_points[i].x << " , " << m_control_points[i].y << std::endl;
+ }
+}
+
+void Bsplines::PrintDegree() const
+{
+ std::cout << "Degree: " << m_degree << std::endl;
+}
+
+
+// Class ZBplines heritage of class Bsplines
+// create a foot trajectory of Z on function the time t
+
+
+ZBsplines::ZBsplines(double FT, double FP, double ToMP, double MP):Bsplines(4)
+{
+ SetParameters( FT, FP, ToMP, MP);
+}
+
+ZBsplines::~ZBsplines()
+{
+
+}
+
+double ZBsplines::ZComputePosition(double t)
+{
+ if (t<=m_FT)
+ return ComputeBsplines(t).y;
+ else
+ return m_FP;
+}
+
+double ZBsplines::ZComputeVelocity(double t)
+{
+ if (m_degree >=1){
+ if (t<m_FT)
+ return DerivativeBsplines().ComputeBsplines(t).y;
+ else
+ return DerivativeBsplines().ComputeBsplines(m_FT - t).y;
+ }
+ else
+ {
+ cout << "ERROR" << endl;
+ return -1;
+ }
+}
+
+double ZBsplines::ZComputeAcc(double t)
+{
+ if (m_degree >=2){
+ if (t<m_FT)
+ return DerivativeBsplines().DerivativeBsplines().ComputeBsplines(t).y;
+ else
+ return DerivativeBsplines().DerivativeBsplines().ComputeBsplines(m_FT - t).y;
+ }
+ else
+ {
+ cout << "ERROR" << endl;
+ return -1;
+ }
+}
+
+void ZBsplines::SetParameters(double FT, double FP, double ToMP, double MP)
+{
+ ZGenerateKnotVector(FT,ToMP);
+ ZGenerateControlPoints(0.0, FT, FP, ToMP, MP);
+}
+
+void ZBsplines::SetParametersWithInitPos(double IP, double FT, double FP, double ToMP, double MP)
+{
+ ZGenerateKnotVector(FT,ToMP);
+ ZGenerateControlPoints(IP, FT, FP, ToMP, MP);
+}
+
+void ZBsplines::GetParametersWithInitPosInitSpeed(double &FT,
+ double &FP,
+ double &InitPos,
+ double &InitSpeed)
+{
+ FT = m_FT;
+ FP = m_FP;
+ InitPos = ZComputePosition(0.0);
+ InitSpeed = ZComputeVelocity(0.0);
+}
+
+void ZBsplines::ZGenerateKnotVector(double FT, double ToMP)
+{
+ std::vector<double> knot;
+ knot.clear();
+ for (int i=0;i<=m_degree;i++)
+ {
+ knot.push_back(0.0);
+ }
+
+ knot.push_back(0.6*ToMP);
+ knot.push_back(ToMP);
+ knot.push_back(1.3*ToMP);
+
+ for (int i =0;i<=m_degree;i++)
+ {
+ knot.push_back(FT);
+ }
+
+ SetKnotVector(knot);
+}
+
+void ZBsplines::ZGenerateControlPoints(double IP, double FT, double FP, double ToMP, double MP)
+{
+ m_FT = FT;
+ m_FP = FP;
+ m_ToMP = ToMP;
+ m_MP = MP;
+ m_IP = IP;
+ std::vector<Point> control_points;
+ control_points.clear();
+ std::ofstream myfile1;
+ myfile1.open("control_point.txt");
+
+ Point A = {0.0,IP};
+ control_points.push_back(A);
+ myfile1 << A.x <<" "<< A.y<< endl;
+
+ A = {m_FT*0.05,IP};
+ control_points.push_back(A);
+ myfile1 << A.x <<" "<< A.y<< endl;
+
+ A = {m_FT*0.1,IP};
+ control_points.push_back(A);
+ myfile1 << A.x <<" "<< A.y<< endl;
+
+ A = {0.85*m_ToMP,m_MP};
+ control_points.push_back(A);
+ myfile1 << A.x <<" "<< A.y<< endl;
+
+ A = {1.15*m_ToMP,m_MP};
+ control_points.push_back(A);
+ myfile1 << A.x <<" "<< A.y<< endl;
+
+ A = {0.90*m_FT,m_FP};
+ control_points.push_back(A);
+ myfile1 << A.x <<" "<< A.y<< endl;
+
+ A = {0.95*m_FT,m_FP};
+ control_points.push_back(A);
+ myfile1 << A.x <<" "<< A.y<< endl;
+
+ A = {m_FT,m_FP};
+ control_points.push_back(A);
+ myfile1 << A.x <<" "<< A.y<< endl;
+
+ myfile1.close();
+
+ SetControlPoints(control_points);
+}
+
+double ZBsplines::GetMP()
+{
+ return m_MP;
+}
+
+
+double ZBsplines::GetFT()
+{
+ return m_FT;
+}
diff --git a/src/Mathematics/Bsplines.hh b/src/Mathematics/Bsplines.hh
new file mode 100644
index 0000000000000000000000000000000000000000..c3b6c5008111c6e13149594d993a1f41b9813631
--- /dev/null
+++ b/src/Mathematics/Bsplines.hh
@@ -0,0 +1,145 @@
+/** \file Bsplines.h
+ \brief Bsplines object for generating trajectoire from set of Points given. */
+
+
+#ifndef _BSPLINES_H_
+#define _BSPLINES_H_
+
+#include <vector>
+#include <iostream>
+#include <math.h>
+
+
+struct Point
+{
+ double x;
+ double y;
+};
+
+namespace PatternGeneratorJRL
+{
+
+/** Bspline class */
+ class Bsplines
+ {
+
+ public:
+ /*! Constructor */
+ Bsplines(int degree);
+
+ /*! Destructor */
+ ~Bsplines();
+
+ /*! Caculate Degree of Bsplines from m_control_points and m_knot_vector*/
+ void GenerateDegree();
+
+ /*! Create a Knot Vector from m_degree and m_control_points with an algo "method" */
+ void GenerateKnotVector(std::string method);
+
+ /*! Create a derivative Bsplines*/
+ Bsplines DerivativeBsplines();
+
+ /*!Compute Basic Function */
+ double *ComputeBasisFunction(double t);
+
+ /*!Compute Bsplines */
+ Point ComputeBsplines(double t);
+
+ /*! Set Degree */
+ void SetDegree(int degree);
+
+ /*! Set Control Points */
+ void SetControlPoints(std::vector<Point> &control_points) ;
+
+ /*! Set Knot Vector */
+ void SetKnotVector(std::vector<double> &knot_vector) ;
+
+ /*! Get Degree */
+ int GetDegree() const;
+
+ /*! Get Control Points */
+ std::vector<Point> GetControlPoints() const;
+
+ /*! Get Knot Vector*/
+ std::vector<double> GetKnotVector() const;
+
+ void PrintKnotVector() const;
+
+ void PrintControlPoints() const;
+
+ void PrintDegree() const;
+
+ protected:
+
+ int m_degree;
+
+ std::vector<Point> m_control_points;
+
+ std::vector<double> m_knot_vector;
+ };
+
+ /// Bsplines used for Z trajectory of stair steps
+ class ZBsplines : public Bsplines
+ {
+ public:
+ /** Constructor:
+ FT: Final time
+ FP: Final position
+ ToMP : Time of Max Position
+ MP : Max Position */
+ ZBsplines( double FT, double FP, double ToMP, double MP);
+
+ /*!Compute Position at time t */
+ double ZComputePosition(double t);
+
+ /*!Compute Velocity at time t */
+ double ZComputeVelocity(double t);
+
+ /*!Compute Acceleration at time t */
+ double ZComputeAcc(double t);
+
+ /** Detructor **/
+ ~ZBsplines();
+
+ /*! Set the parameters
+ This method assumes implicitly a initial position
+ initial speed and initial acceleration equal to zero.
+ The same for final speed and final acceleration.
+ Speed at Max Position is around zero.
+ */
+ void SetParameters(double FT, double FP, double ToMP, double MP);
+
+ void SetParametersWithInitPosInitSpeed(double FT, double FP, double ToMP, double MP,
+ double InitPos,
+ double InitSpeed);
+
+ void SetParametersWithInitPos(double IP, double FT, double FP, double ToMP, double MP);
+
+ void GetParametersWithInitPosInitSpeed(double &FT,
+ double &MP,
+ double &InitPos,
+ double &InitSpeed);
+
+ /*! Create a vector of Control Points with 8 Points :
+ {0.0,0.0},
+ {m_FT*0.05,0.0},
+ {m_FT*0.1,0.0}},
+ {0.85*m_ToMP,m_MP},
+ {1.15*m_ToMP,m_MP},
+ {0.85*m_FT,m_FP},
+ {0.9*m_FT,m_FP},
+ {m_FT,m_FP}*/
+ void ZGenerateControlPoints(double IP,double FT, double FP, double ToMP, double MP);
+
+ void ZGenerateKnotVector(double FT, double ToMP);
+
+ double GetMP();
+
+ double GetFT();
+
+ private:
+ double m_IP, m_FT, m_FP, m_ToMP, m_MP;
+ };
+
+}
+#endif /* _BSPLINES_H_*/
diff --git a/src/Mathematics/PolynomeFoot.cpp b/src/Mathematics/PolynomeFoot.cpp
index 244daedbebcba0a98be5c26bfe431767e97ca200..22280cd96d286a40f79379e1751e06f7cb49d9aa 100644
--- a/src/Mathematics/PolynomeFoot.cpp
+++ b/src/Mathematics/PolynomeFoot.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2006, 2007, 2008, 2009, 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* Polynomes object for generating foot trajectories. */
@@ -164,13 +164,13 @@ void Polynome4::GetParametersWithInitPosInitSpeed(double &FT,
Polynome4::~Polynome4()
{}
-Polynome5::Polynome5(double FT, double FP)
+Polynome5::Polynome5(double FT, double FP)
:PolynomeFoot(5,FT),
FP_(FP),
InitPos_(0.0),
InitSpeed_(0.0),
InitAcc_(0.0)
-
+
{
SetParameters(FT,FP);
}
diff --git a/src/Mathematics/PolynomeFoot.hh b/src/Mathematics/PolynomeFoot.hh
index d63149def89489de5cd530491e9d986b32da29b3..f53f8738b7fdf42a37a375c4c31a74b03d59a878 100644
--- a/src/Mathematics/PolynomeFoot.hh
+++ b/src/Mathematics/PolynomeFoot.hh
@@ -1,9 +1,9 @@
/*
- * Copyright 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2006, 2007, 2008, 2009, 2010,
*
* Andrei Herdt
* Florent Lamiraux
- * Mathieu Poirier
+ * Mathieu Poirier
* Olivier Stasse
*
* JRL, CNRS/AIST
@@ -21,7 +21,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/** \file PolynomeFoot.h
@@ -43,7 +43,6 @@
namespace PatternGeneratorJRL
{
-
class PolynomeFoot : public Polynome
{
protected :
@@ -78,7 +77,7 @@ namespace PatternGeneratorJRL
FP: Final position */
Polynome3(double FT, double FP);
- /*! Set the parameters
+ /*! Set the parameters
This method assumes implicitly a position
set to zero, and a speed equals to zero.
Final velocity is 0
@@ -101,7 +100,7 @@ namespace PatternGeneratorJRL
double &InitSpeed);
/// Destructor.
~Polynome3();
-
+
private:
/*! Store final time and final position. */
double FP_;
@@ -140,11 +139,11 @@ namespace PatternGeneratorJRL
/// Destructor.
~Polynome4();
-
+
private:
/*! Store final time and middle position. */
double MP_;
-
+
};
/// Polynome used for X,Y and Theta trajectories.
@@ -161,6 +160,8 @@ namespace PatternGeneratorJRL
/// Set the parameters
void SetParameters(double FT, double FP);
+
+ double Compute(double t);
/*! Set the parameters such that
the initial position, and initial speed
are different from zero.
@@ -181,6 +182,7 @@ namespace PatternGeneratorJRL
/// \brief Set parameters considering initial position, velocity, acceleration
void SetParameters(double FT, double FP,
double InitPos, double InitSpeed, double InitAcc, double InitJerk = 0.0);
+
/// Destructor.
~Polynome5();
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index 883276c81f4795b1296a99760c25a9563054bd5f..f35273ea53ef30eb1d3be1cfc55161c40feb5881 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -187,6 +187,7 @@ void ComAndFootRealizationByGeometry::
itJoint++;
}
InitializationMaps(FromRootToJoint2,ActuatedJoints,m_ChestIndexinConfiguration);
+ InitializationMaps(FromRootToJoint2,ActuatedJoints,m_ChestIndexinConfiguration);
}
void ComAndFootRealizationByGeometry::
@@ -266,6 +267,7 @@ void ComAndFootRealizationByGeometry::
FromRootToJoint =
getHumanoidDynamicRobot()->jointsBetween(*waist, *(LeftFoot->associatedAnkle()));
+
FromRootToJoint.erase(FromRootToJoint.begin());
InitializationMaps(FromRootToJoint,ActuatedJoints,
m_LeftLegIndexinConfiguration);
@@ -376,9 +378,9 @@ bool ComAndFootRealizationByGeometry::
lStartingWaistPose(0) = CurrentConfig(0); // 0.0
lStartingWaistPose(1) = CurrentConfig(1); // 0.0
lStartingWaistPose(2) = CurrentConfig(2);
- lStartingWaistPose(3) = 0.0;
- lStartingWaistPose(4) = 0.0;
- lStartingWaistPose(5) = 0.0;
+ lStartingWaistPose(3) = CurrentConfig(3); //0.0;
+ lStartingWaistPose(4) = CurrentConfig(4); //0.0;
+ lStartingWaistPose(5) = CurrentConfig(5); //0.0;
ODEBUG("Current Config: " << CurrentConfig);
return true;
@@ -434,6 +436,7 @@ bool ComAndFootRealizationByGeometry::
assert((MAL_S4x4_MATRIX_ACCESS_I_J(lFootPose, 2,1) == 0) &&
"Error in the walk pattern generator initialization:" &&
" Initial foot position is not flat");
+
InitFootPosition.omega =
atan2(MAL_S4x4_MATRIX_ACCESS_I_J(lFootPose, 2,0),
MAL_S4x4_MATRIX_ACCESS_I_J(lFootPose, 2,2))*180/M_PI;
@@ -463,9 +466,6 @@ bool ComAndFootRealizationByGeometry::
memset((char *)&InitLeftFootPosition,0,sizeof(FootAbsolutePosition));
memset((char *)&InitRightFootPosition,0,sizeof(FootAbsolutePosition));
- /* Initialize the waist pose.*/
- MAL_VECTOR_RESIZE(lStartingWaistPose,6);
-
// Compute the forward dynamics from the configuration vector provided by the user.
// Initialize waist pose.
InitializationHumanoid(BodyAnglesIni,lStartingWaistPose);
diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index c0e9932bfaa188312bdcd057ddace34726deba79..410ed84da87bce61ec1b18fb3b83059556471323 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -183,11 +183,12 @@ namespace PatternGeneratorJRL {
void PatternGeneratorInterfacePrivate::RegisterPluginMethods()
{
- std::string aMethodName[15] =
+ std::string aMethodName[16] =
{":LimitsFeasibility",
":ZMPShiftParameters",
":TimeDistributionParameters",
":stepseq",
+ ":stepstairseq",
":finish",
":StartOnLineStepSequencing",
":StopOnLineStepSequencing",
@@ -200,7 +201,7 @@ namespace PatternGeneratorJRL {
":setVelReference",
":setCoMPerturbationForce"};
- for(int i=0;i<15;i++)
+ for(int i=0;i<16;i++)
{
if (!SimplePlugin::RegisterMethod(aMethodName[i]))
{
@@ -463,13 +464,16 @@ namespace PatternGeneratorJRL {
void PatternGeneratorInterfacePrivate::ReadSequenceOfSteps(istringstream &strm)
{
// Read the data inside strm.
-
-
switch (m_StepStackHandler->GetWalkMode())
{
case 0:
case 4:
case 5:
+ case 6:
+ {
+ m_StepStackHandler->ReadStepSequenceAccordingToWalkMode(strm);
+ break;
+ }
case 3:
case 1:
{
@@ -543,7 +547,6 @@ namespace PatternGeneratorJRL {
deque<RelativeFootPosition> RelativeFootPositions;
m_ZMPVRQP->SetCurrentTime(m_InternalClock);
-
m_ZMPVRQP->InitOnLine(m_ZMPPositions,
m_COMBuffer,
m_LeftFootPositions,
@@ -573,6 +576,19 @@ namespace PatternGeneratorJRL {
ODEBUG("After finish and realize Step Sequence");
}
+ void PatternGeneratorInterfacePrivate::m_StepStairSequence(istringstream &strm)
+ {
+
+ ODEBUG("Step Sequence");
+ ofstream DebugFile;
+ m_StepStackHandler->ReadStepStairSequenceAccordingToWalkMode(strm);
+
+ ODEBUG("After reading Step Stair Sequence");
+ FinishAndRealizeStepSequence();
+ ODEBUG("After finish and realize Step Stair Sequence");
+ }
+
+
void PatternGeneratorInterfacePrivate::EvaluateStartingCOM(MAL_VECTOR( & Configuration,double),
MAL_S3_VECTOR( & lStartingCOMState,double))
{
@@ -720,6 +736,7 @@ namespace PatternGeneratorJRL {
{
ODEBUG(lRelativeFootPositions[i].sx << " " <<
lRelativeFootPositions[i].sy << " " <<
+ lRelativeFootPositions[i].sz << " " <<
lRelativeFootPositions[i].theta );
}
@@ -746,6 +763,7 @@ namespace PatternGeneratorJRL {
ODEBUG("Push a position in stack of steps:"<<
lRelativeFootPositions[0].sx << " " <<
lRelativeFootPositions[0].sy << " " <<
+ lRelativeFootPositions[0].sz << " " <<
lRelativeFootPositions[0].theta);
}
}
@@ -897,11 +915,11 @@ namespace PatternGeneratorJRL {
vector<double> lCurrentJointValues;
m_ZMPD->SetZMPShift(m_ZMPShift);
+
+
MAL_VECTOR_TYPE(double) lCurrentConfiguration;
lCurrentConfiguration = m_HumanoidDynamicRobot->currentConfiguration();
- ODEBUG("lCurrent Configuration :" << lCurrentConfiguration);
-
deque<RelativeFootPosition> lRelativeFootPositions;
CommonInitializationOfWalking(lStartingCOMState,
@@ -910,6 +928,8 @@ namespace PatternGeneratorJRL {
InitLeftFootAbsPos, InitRightFootAbsPos,
lRelativeFootPositions,lCurrentJointValues,true);
+
+
ODEBUG("lStartingCOMState: "
<< lStartingCOMState.x[0] << " "
<< lStartingCOMState.y[0] << " "
@@ -925,9 +945,12 @@ namespace PatternGeneratorJRL {
m_HumanoidDynamicRobot->currentConfiguration(lCurrentConfiguration);
ODEBUG("Size of lRelativeFootPositions :" << lRelativeFootPositions.size());
+ // cout <<"Size of lRelativeFootPositions :" << lRelativeFootPositions.size() << endl;
ODEBUG("ZMPInitialPoint" << lStartingZMPPosition(0) << " "
<< lStartingZMPPosition(1) << " " << lStartingZMPPosition(2) );
+
+
ODEBUG("COMBuffer: " << m_COMBuffer.size() );
// Create the ZMP reference.
@@ -937,6 +960,7 @@ namespace PatternGeneratorJRL {
InitLeftFootAbsPos,
InitRightFootAbsPos);
+
ODEBUG("First m_ZMPPositions" << m_ZMPPositions[0].px << " " << m_ZMPPositions[0].py);
deque<ZMPPosition> aZMPBuffer;
@@ -991,11 +1015,14 @@ namespace PatternGeneratorJRL {
gettimeofday(&time4,0);
ODEBUG4("FinishAndRealizeStepSequence() - 7 ","DebugGMFKW.dat");
// Read NL informations from ZMPRefPositions.
+
+
m_GlobalStrategyManager->Setup(m_ZMPPositions,
m_COMBuffer,
m_LeftFootPositions,
m_RightFootPositions);
+
gettimeofday(&time5,0);
m_count = 0;
@@ -1037,10 +1064,12 @@ namespace PatternGeneratorJRL {
strm >> aCmd;
ODEBUG("PARSECMD");
+
if (SimplePluginManager::CallMethod(aCmd,strm))
{
ODEBUG("Method " << aCmd << " found and handled.");
}
+
return 0;
}
void PatternGeneratorInterfacePrivate::ChangeOnLineStep(istringstream &strm,double &newtime)
@@ -1051,7 +1080,9 @@ namespace PatternGeneratorJRL {
double ltime = (double)m_ZMPM->GetTSingleSupport();
strm >> aFAP.x;
strm >> aFAP.y;
+ //strm >> aFAP.z;
strm >> aFAP.theta;
+
ChangeOnLineStep(ltime,aFAP,newtime);
}
}
@@ -1086,6 +1117,9 @@ namespace PatternGeneratorJRL {
else if (aCmd==":stepseq")
m_StepSequence(strm);
+ else if (aCmd==":stepstairseq")
+ m_StepStairSequence(strm);
+
else if (aCmd==":finish")
m_FinishAndRealizeStepSequence(strm);
diff --git a/src/PreviewControl/LinearizedInvertedPendulum2D.cpp b/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
index ba7fbc26c09b83023ccf733457c0f12de7a61d2e..90f6a58b0ccfa7056f1f021b70c419cedd46b980 100644
--- a/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
+++ b/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
@@ -199,38 +199,39 @@ int LinearizedInvertedPendulum2D::Interpolation(deque<COMState> &COMStates,
COMState & aCOMPos = COMStates[lCurrentPosition];
double lkSP;
lkSP = (lk+1) * m_SamplingPeriod;
- aCOMPos.x[0] =
- m_CoM.x[0] + // Position
- lkSP * m_CoM.x[1] + // Speed
- 0.5 * lkSP*lkSP * m_CoM.x[2] +// Acceleration
- lkSP * lkSP * lkSP * CX /6.0; // Jerk
-
- aCOMPos.x[1] =
- m_CoM.x[1] + // Speed
- lkSP * m_CoM.x[2] + // Acceleration
- 0.5 * lkSP * lkSP * CX; // Jerk
-
- aCOMPos.x[2] =
- m_CoM.x[2] + // Acceleration
- lkSP * CX; // Jerk
-
- aCOMPos.y[0] =
- m_CoM.y[0] + // Position
- lkSP * m_CoM.y[1] + // Speed
- 0.5 * lkSP*lkSP * m_CoM.y[2] + // Acceleration
- lkSP * lkSP * lkSP * CY /6.0; // Jerk
-
- aCOMPos.y[1] =
- m_CoM.y[1] + // Speed
- lkSP * m_CoM.y[2] + // Acceleration
- 0.5 * lkSP * lkSP * CY; // Jerk
-
- aCOMPos.y[2] =
- m_CoM.y[2] + // Acceleration
- lkSP * CY; // Jerk
-
- aCOMPos.yaw[0] = ZMPRefPositions[lCurrentPosition].theta;
-
+
+ aCOMPos.x[0] =
+ m_CoM.x[0] + // Position
+ lkSP * m_CoM.x[1] + // Speed
+ 0.5 * lkSP*lkSP * m_CoM.x[2] +// Acceleration
+ lkSP * lkSP * lkSP * CX /6.0; // Jerk
+
+ aCOMPos.x[1] =
+ m_CoM.x[1] + // Speed
+ lkSP * m_CoM.x[2] + // Acceleration
+ 0.5 * lkSP * lkSP * CX; // Jerk
+
+ aCOMPos.x[2] =
+ m_CoM.x[2] + // Acceleration
+ lkSP * CX; // Jerk
+
+ aCOMPos.y[0] =
+ m_CoM.y[0] + // Position
+ lkSP * m_CoM.y[1] + // Speed
+ 0.5 * lkSP*lkSP * m_CoM.y[2] + // Acceleration
+ lkSP * lkSP * lkSP * CY /6.0; // Jerk
+
+ aCOMPos.y[1] =
+ m_CoM.y[1] + // Speed
+ lkSP * m_CoM.y[2] + // Acceleration
+ 0.5 * lkSP * lkSP * CY; // Jerk
+
+ aCOMPos.y[2] =
+ m_CoM.y[2] + // Acceleration
+ lkSP * CY; // Jerk
+
+ aCOMPos.yaw[0] = ZMPRefPositions[lCurrentPosition].theta;
+
aCOMPos.z[0] = m_ComHeight;
aCOMPos.z[1] = 0;
aCOMPos.z[2] = 0;
@@ -279,10 +280,10 @@ com_t LinearizedInvertedPendulum2D::OneIteration(double ux, double uy)
ODEBUG4( m_xk[0] << " " << m_xk[1] << " " << m_xk[2] << " " <<
m_xk[3] << " " << m_xk[4] << " " << m_xk[5] << " " <<
- m_CoM.x << " " << m_CoM.y << " " <<
+ m_CoM.x << " " << m_CoM.y << " " <<
m_zk[0] << " " << m_zk[1] << " " <<
- Bux[0] << " " << Bux[1] << " " << Bux[2] << " " <<
- Buy[0] << " " << Buy[1] << " " << Buy[2] << " " <<
+ Bux[0] << " " << Bux[1] << " " << Bux[2] << " " <<
+ Buy[0] << " " << Buy[1] << " " << Buy[2] << " " <<
m_B(0,0) << " " << m_B(1,0) << " " << m_B(2,0) << " " <<
m_B(3,0) << " " << m_B(4,0) << " " << m_B(5,0) << " " ,
"Debug2DLIPM.dat");
diff --git a/src/PreviewControl/rigid-body-system.cpp b/src/PreviewControl/rigid-body-system.cpp
index e136b45e1592e8a7fd000812349135bdced7cef3..6748b2317fb2e4df9c1db51009c4f1766067c85f 100644
--- a/src/PreviewControl/rigid-body-system.cpp
+++ b/src/PreviewControl/rigid-body-system.cpp
@@ -57,7 +57,7 @@ RigidBodySystem::initialize( )
OFTG_->QPSamplingPeriod( T_ );
OFTG_->NbSamplingsPreviewed( N_ );
OFTG_->FeetDistance( 0.2 );
- OFTG_->StepHeight( 0.05 );
+ OFTG_->StepHeight( 0.03 );
// Initialize predetermined trajectories:
// --------------------------------------
diff --git a/src/StepStackHandler.cpp b/src/StepStackHandler.cpp
index a7d7ccb39d6259371e85f0211bfc4c6ef5bbb58a..cc1d8a6f78b1faaa83a8579b56fb0552902c2dae 100644
--- a/src/StepStackHandler.cpp
+++ b/src/StepStackHandler.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
@@ -19,11 +19,11 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! This object handle the step stack of the pattern generator.
- It allows also to create automatically stack of steps according to
+ It allows also to create automatically stack of steps according to
some high level functionnalities.
*/
#include <fstream>
@@ -50,7 +50,7 @@ StepStackHandler::StepStackHandler(SimplePluginManager *lSPM) : SimplePlugin(lSP
m_RelativeFootPositions.clear();
m_TransitionFinishOnLine=false;
- std::string aMethodName[8] =
+ std::string aMethodName[8] =
{":walkmode",
":singlesupporttime",
":doublesupporttime",
@@ -124,6 +124,53 @@ int StepStackHandler::GetWalkMode()
return m_WalkMode;
}
+void StepStackHandler::ReadStepStairSequenceAccordingToWalkMode(istringstream &strm)
+
+{
+ ODEBUG( "Standard Stepping on the Stairs Mode Selected" );
+
+ RelativeFootPosition aFootPosition;
+
+ while(!strm.eof())
+ {
+ if (!strm.eof())
+ strm >> aFootPosition.sx;
+ else break;
+ if (!strm.eof())
+ strm >> aFootPosition.sy;
+ else
+ break;
+ if (!strm.eof())
+ strm >> aFootPosition.sz;
+ else
+ break;
+ if (!strm.eof())
+ strm >> aFootPosition.theta;
+ else
+ break;
+
+ aFootPosition.DeviationHipHeight = 0;
+ aFootPosition.SStime=m_SingleSupportTime;
+ aFootPosition.DStime=m_DoubleSupportTime;
+ aFootPosition.stepType=1;
+ ODEBUG4(aFootPosition.sx << " " <<
+ aFootPosition.sy << " " <<
+ aFootPosition.sz << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
+ aFootPosition.DeviationHipHeight << " " ,
+ "DebugGMFKW.dat");
+
+ m_RelativeFootPositions.push_back(aFootPosition);
+ if (aFootPosition.sy>0)
+ m_KeepLastCorrectSupportFoot=-1;
+ else
+ m_KeepLastCorrectSupportFoot=1;
+
+ }
+ ODEBUG("m_RelativeFootPositions: " << m_RelativeFootPositions.size());
+}
void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
{
@@ -131,12 +178,13 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
switch (m_WalkMode)
{
case 0:
+
case 4:
- {
-
+ {
+
ODEBUG( "Standard Walk Mode Selected" );
RelativeFootPosition aFootPosition;
-
+
while(!strm.eof())
{
if (!strm.eof())
@@ -144,30 +192,31 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
else break;
if (!strm.eof())
strm >> aFootPosition.sy;
- else
+ else
break;
if (!strm.eof())
strm >> aFootPosition.theta;
- else
+ else
break;
-
+
aFootPosition.DeviationHipHeight = 0;
aFootPosition.SStime=m_SingleSupportTime;
aFootPosition.DStime=m_DoubleSupportTime;
aFootPosition.stepType=1;
ODEBUG4(aFootPosition.sx << " " <<
aFootPosition.sy << " " <<
- aFootPosition.theta << " " <<
- aFootPosition.SStime << " " <<
- aFootPosition.DStime << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
aFootPosition.DeviationHipHeight << " " ,
"DebugGMFKW.dat");
-
+
m_RelativeFootPositions.push_back(aFootPosition);
if (aFootPosition.sy>0)
m_KeepLastCorrectSupportFoot=-1;
else
m_KeepLastCorrectSupportFoot=1;
+
}
ODEBUG("m_RelativeFootPositions: " << m_RelativeFootPositions.size());
@@ -175,10 +224,12 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
}
case 3:
case 1:
- {
+ {
+
+
ODEBUG4( "Walk Mode with HipHeight Variation Selected","DebugGMFKW.dat" );
RelativeFootPosition aFootPosition;
-
+
ODEBUG4("Inside StepStack Handler","DebugGMFKW.dat");
while(!strm.eof())
{
@@ -199,39 +250,41 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
aFootPosition.stepType=1;
ODEBUG4(aFootPosition.sx << " " <<
aFootPosition.sy << " " <<
- aFootPosition.theta << " " <<
- aFootPosition.SStime << " " <<
- aFootPosition.DStime << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
aFootPosition.DeviationHipHeight << " " ,
"DebugFootPrint.dat");
+
+
m_RelativeFootPositions.push_back(aFootPosition);
if (aFootPosition.sy>0)
m_KeepLastCorrectSupportFoot=-1;
else
m_KeepLastCorrectSupportFoot=1;
-
+
}
ODEBUG4("Finito for the reading. StepStack Handler","DebugGMFKW.dat");
break;
-
+
}
case 2:
- {
+ {
ODEBUG( "Walk Mode with Obstacle StepOver Selected \
(obstacle parameters have to be set first, \
if not standard dimensions are used)" );
//cout << "I am calculating relative positions to negociate obstacle" << endl;
m_StOvPl->CalculateFootHolds(m_RelativeFootPositions);
-
+
break;
}
// With a varying double support time and a single support time.
case 5:
- {
-
+ {
+
ODEBUG3( "Standard Walk Mode Selected" );
RelativeFootPosition aFootPosition;
-
+
while(!strm.eof())
{
if (!strm.eof())
@@ -239,39 +292,39 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
else break;
if (!strm.eof())
strm >> aFootPosition.sy;
- else
+ else
break;
if (!strm.eof())
strm >> aFootPosition.theta;
- else
+ else
break;
if (!strm.eof())
strm >> aFootPosition.SStime;
- else
+ else
break;
if (!strm.eof())
strm >> aFootPosition.DStime;
- else
+ else
break;
-
+
aFootPosition.DeviationHipHeight = 0;
aFootPosition.stepType=1;
ODEBUG3("FootPositions:" << aFootPosition.sx << " " <<
aFootPosition.sy << " " <<
- aFootPosition.theta << " " <<
- aFootPosition.SStime << " " <<
- aFootPosition.DStime << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
aFootPosition.DeviationHipHeight << " " );
-
+
ODEBUG4(aFootPosition.sx << " " <<
aFootPosition.sy << " " <<
- aFootPosition.theta << " " <<
- aFootPosition.SStime << " " <<
- aFootPosition.DStime << " " <<
+ aFootPosition.theta << " " <<
+ aFootPosition.SStime << " " <<
+ aFootPosition.DStime << " " <<
aFootPosition.DeviationHipHeight << " " ,
"DebugGMFKW.dat");
-
+
m_RelativeFootPositions.push_back(aFootPosition);
if (aFootPosition.sy>0)
m_KeepLastCorrectSupportFoot=-1;
@@ -283,7 +336,7 @@ void StepStackHandler::ReadStepSequenceAccordingToWalkMode(istringstream &strm)
break;
}
- default:
+ default:
{
ODEBUG( "PLease select proper walk mode. \
(0 for normal walking ; \
@@ -319,7 +372,7 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
// Computes the last step value.
LastStep = OmegaTotal*R - NumberOfStep * StepMax;
-
+
// OmegaStep = arc_deg/(double)NumberOfStep;
OmegaStep = StepMax/R;
LastOmegaStep = OmegaTotal - OmegaStep * NumberOfStep;
@@ -343,7 +396,7 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
ODEBUG4(NumberOfStep << " "
<< LastStep<< " "
<< arc_deg, "DebugFootPrint.dat");
-
+
double Omegakp = 0.0,
Omegak=0.0;
@@ -360,7 +413,7 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
MAL_MATRIX_SET_IDENTITY(A);
MAL_MATRIX_DIM(Ap,double,2,2);
MAL_MATRIX_SET_IDENTITY(Ap);
-
+
Omegakp = Omegak;
Omegak = Omegak + OmegaStep;
ODEBUG("Omegak:" << Omegak );
@@ -368,7 +421,7 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
c = cos(Omegak*M_PI/180.0);
s = sin(Omegak*M_PI/180.0);
- // Transpose of the orientation matrix
+ // Transpose of the orientation matrix
// to get the inverse of the orientation matrix.
A(0,0) = c; A(0,1) = s;
A(1,0) = -s; A(1,1) = c;
@@ -376,23 +429,23 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
double cp,sp;
cp = cos(Omegakp*M_PI/180.0);
sp = sin(Omegakp*M_PI/180.0);
-
+
MAL_VECTOR_DIM(lv,double,2);
MAL_VECTOR_DIM(lv2,double,2);
lv(0) = (R+DirectionRay*SupportFoot*0.095)*s - (R-DirectionRay*SupportFoot*0.095)*sp;
lv(1) = -((R+DirectionRay*SupportFoot*0.095)*c - (R-DirectionRay*SupportFoot*0.095)*cp);
MAL_C_eq_A_by_B(lv2,A,lv);
- ODEBUG(" X: " << (R+DirectionRay*SupportFoot*0.095)*s << " " << (R-DirectionRay*SupportFoot*0.095)*sp
+ ODEBUG(" X: " << (R+DirectionRay*SupportFoot*0.095)*s << " " << (R-DirectionRay*SupportFoot*0.095)*sp
<< " " << StepMax << " " << lv(0) << " " << lv2(0) );
- ODEBUG(" Y: " << (R+DirectionRay*SupportFoot*0.095)*c << " " << (R-DirectionRay*SupportFoot*0.095)*cp
+ ODEBUG(" Y: " << (R+DirectionRay*SupportFoot*0.095)*c << " " << (R-DirectionRay*SupportFoot*0.095)*cp
<< " " << SupportFoot*0.19 << " " << lv(1) << " " << lv2(1));
-
+
aFootPosition.sx = lv2(0);
aFootPosition.sy = lv2(1);
-
+
}
-
+
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
m_RelativeFootPositions.push_back(aFootPosition);
@@ -412,18 +465,18 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
{
MAL_MATRIX_DIM(A,double,2,2);
MAL_MATRIX_SET_IDENTITY(A);
-
+
Omegakp = Omegak;
Omegak = Omegak + LastOmegaStep;
ODEBUG( "Omegak:" << Omegak );
double c,s;
c = cos(Omegak*M_PI/180.0);
s = sin(Omegak*M_PI/180.0);
-
+
double cp,sp;
cp = cos(Omegakp*M_PI/180.0);
sp = sin(Omegakp*M_PI/180.0);
-
+
A(0,0) = c; A(0,1) =s;
A(1,0) = -s; A(1,1) = c;
MAL_VECTOR_DIM(lv,double,2);
@@ -431,19 +484,19 @@ void StepStackHandler::CreateArcInStepStack( double x,double y, double R,
lv(0) = (R+DirectionRay*SupportFoot*0.095)*s - (R-DirectionRay*SupportFoot*0.095)*sp;
lv(1) = -((R+DirectionRay*SupportFoot*0.095)*c - (R-DirectionRay*SupportFoot*0.095)*cp);
MAL_C_eq_A_by_B(lv2,A,lv);
- ODEBUG(" X: " << (R+DirectionRay*SupportFoot*0.095)*s << " " << (R-DirectionRay*SupportFoot*0.095)*sp
+ ODEBUG(" X: " << (R+DirectionRay*SupportFoot*0.095)*s << " " << (R-DirectionRay*SupportFoot*0.095)*sp
<< " " << lv(0) << " " << lv2(0) );
- ODEBUG(" Y: " << (R+DirectionRay*SupportFoot*0.095)*c << " " << (R-DirectionRay*SupportFoot*0.095)*cp
+ ODEBUG(" Y: " << (R+DirectionRay*SupportFoot*0.095)*c << " " << (R-DirectionRay*SupportFoot*0.095)*cp
<< " " << lv(1) << " " << lv2(1) );
-
+
aFootPosition.sx = lv2(0);
aFootPosition.sy = lv2(1);
-
+
}
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
-
+
m_RelativeFootPositions.push_back(aFootPosition);
ODEBUG4(aFootPosition.sx<< " "
@@ -478,15 +531,15 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
// Computes the last step value.
LastStep = OmegaTotal*R - NumberOfStep * StepMax;
-
+
OmegaStep = StepMax/R;
LastOmegaStep = OmegaTotal - OmegaStep*NumberOfStep;
-
+
#if 0
ofstream DebugFile;
DebugFile.open("/tmp/output.txt",ofstream::out);
DebugFile << NumberOfStep << " "
- << OmegaStep << " "
+ << OmegaStep << " "
<< LastOmegaStep<< " "
<< arc_deg<< " "
<< endl;
@@ -495,8 +548,8 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
cosOmegaStep = cos(OmegaStep);
sinOmegaStep = sin(OmegaStep);
-
- // Make sure that Support Foot is the foot which
+
+ // Make sure that Support Foot is the foot which
// does not lead the motion.
if (SupportFoot*OmegaStep<0.0)
{
@@ -505,17 +558,17 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
aFootPosition.theta = 0;
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
-
+
m_RelativeFootPositions.push_back(aFootPosition);
#if 0
DebugFile.open("/tmp/output.txt",ofstream::app);
DebugFile << aFootPosition.sx<< " "
- << aFootPosition.sy<< " "
+ << aFootPosition.sy<< " "
<< aFootPosition.theta<< " "
<< endl;
DebugFile.close();
#endif
-
+
SupportFoot=-SupportFoot;
}
@@ -545,10 +598,10 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
for(int j=0;j<3;j++)
if (i==j)
{
- MFNSF(i,j) =
- MFSF(i,j) =
- Romega(i,j) =
- Mtmp(i,j) =
+ MFNSF(i,j) =
+ MFSF(i,j) =
+ Romega(i,j) =
+ Mtmp(i,j) =
iRomega(i,j)= 1.0;
}
else
@@ -560,17 +613,17 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
Mtmp2(i,j) =
iRomega(i,j) = 0.0;
}
-
+
MFSF(0,2)=-R;
MFSF(1,2)=-S;
MFNSF(0,2)=-R;
MFNSF(1,2)=S;
- MSupportFoot=MFSF;
+ MSupportFoot=MFSF;
Mtmp(1,2) = 0.19;
#if 0
DebugFile.open("/tmp/outputNL.txt",ofstream::app);
- DebugFile << MSupportFoot(0,2) << " "
+ DebugFile << MSupportFoot(0,2) << " "
<< MSupportFoot(1,2) << endl;
DebugFile.close();
#endif
@@ -582,22 +635,22 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
cosiOmegaStep = cos((i+1)*OmegaStep);
siniOmegaStep = sin((i+1)*OmegaStep);
-
+
Romega(0,0) = cosiOmegaStep;
Romega(0,1) = -siniOmegaStep;
Romega(1,0) = siniOmegaStep;
Romega(1,1) = cosiOmegaStep;
Romega(0,2) = 0;
Romega(1,2) = 0;
-
+
MAL_MATRIX(lTmp,double);
MAL_C_eq_A_by_B(lTmp,MSupportFoot,Romegastep);
MAL_INVERSE(lTmp, RiR,double);
-
+
ODEBUG(" Iteration " << i);
ODEBUG(" Romega " << Romega);
ODEBUG(" RiR " << RiR);
-
+
MAL_C_eq_A_by_B(FPos, Romega, MFNSF);
ODEBUG("FPos: " << FPos);
MAL_C_eq_A_by_B(FPos,RiR,FPos);
@@ -611,7 +664,7 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
m_RelativeFootPositions.push_back(aFootPosition);
MAL_C_eq_A_by_B(MSupportFoot, Romega,MFNSF);
-
+
#if 0
DebugFile.open("/tmp/outputL.txt",ofstream::app);
DebugFile << MSupportFoot(0,2) << " "
@@ -645,14 +698,14 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
for(int li=0;li<2;li++)
for(int lj=0;lj<2;lj++)
Mtmp2[li][lj]=MSupportFoot[li][lj];
-
+
Mtmp2 = Mtmp2*Mtmp;
*/
MSupportFoot = MAL_RET_A_by_B( MSupportFoot, Mtmp);
-
+
#if 0
DebugFile.open("/tmp/outputNL.txt",ofstream::app);
- DebugFile << MSupportFoot(0,2) << " "
+ DebugFile << MSupportFoot(0,2) << " "
<< MSupportFoot(1,2) << endl;
DebugFile.close();
#endif
@@ -662,7 +715,7 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
if (LastStep!=0.0)
{
double cosiOmegaStep,siniOmegaStep;
-
+
cosiOmegaStep = cos(LastOmegaStep+NumberOfStep*OmegaStep);
siniOmegaStep = sin(LastOmegaStep+NumberOfStep*OmegaStep);
for(int i=0;i<3;i++)
@@ -671,14 +724,14 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
Romega(i,j) = iRomega(i,j) = 1.0;
else
Romega(i,j) = iRomega(i,j) = 0.0;
-
+
Romega(0,0) = cosiOmegaStep;
Romega(0,1) = -siniOmegaStep;
Romega(1,0) = siniOmegaStep;
Romega(1,1) = cosiOmegaStep;
Romega(0,2) = 0;
Romega(1,2) = 0;
-
+
double coslOmegaStep,sinlOmegaStep;
coslOmegaStep = cos(LastOmegaStep);
sinlOmegaStep = sin(LastOmegaStep);
@@ -688,7 +741,7 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
iRomega(0,1) = -sinlOmegaStep;
iRomega(1,0) = sinlOmegaStep;
iRomega(1,1) = coslOmegaStep;
-
+
MAL_MATRIX(lTmp,double);
MAL_C_eq_A_by_B(lTmp,MSupportFoot,iRomega);
MAL_INVERSE(lTmp, RiR,double);
@@ -736,10 +789,10 @@ void StepStackHandler::CreateArcCenteredInStepStack( double R,
DebugFile.close();
#endif
MSupportFoot = MAL_RET_A_by_B( MSupportFoot , Mtmp);
-
+
#if 0
DebugFile.open("/tmp/outputNL.txt",ofstream::app);
- DebugFile << MSupportFoot(0,2) << " "
+ DebugFile << MSupportFoot(0,2) << " "
<< MSupportFoot(1,2) << endl;
DebugFile.close();
#endif
@@ -767,7 +820,7 @@ void StepStackHandler::StopOnLineStep()
// m_OnLineSteps = false;
m_TransitionFinishOnLine=true;
- // Correct the last support foot before cleaning up the
+ // Correct the last support foot before cleaning up the
// stack.
if (m_RelativeFootPositions.size()%2==0)
m_KeepLastCorrectSupportFoot = -m_KeepLastCorrectSupportFoot;
@@ -787,7 +840,7 @@ bool StepStackHandler::IsOnLineSteppingOn()
return m_OnLineSteps;
}
-void StepStackHandler::AddStandardOnLineStep(bool NewStep,
+void StepStackHandler::AddStandardOnLineStep(bool NewStep,
double NewStepX,
double NewStepY,
double NewTheta)
@@ -796,7 +849,7 @@ void StepStackHandler::AddStandardOnLineStep(bool NewStep,
ODEBUG("m_OnLineSteps: "<<m_OnLineSteps);
if (!m_OnLineSteps)
return;
-
+
ODEBUG("m_KeepLastCorrectSupportFoot" << m_KeepLastCorrectSupportFoot);
if (!NewStep)
{
@@ -806,7 +859,7 @@ void StepStackHandler::AddStandardOnLineStep(bool NewStep,
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
aFootPosition.stepType = 0;
-
+
}
else
{
@@ -817,17 +870,17 @@ void StepStackHandler::AddStandardOnLineStep(bool NewStep,
aFootPosition.SStime = m_SingleSupportTime;
aFootPosition.DStime = m_DoubleSupportTime;
aFootPosition.stepType = 0;
- cout << aFootPosition.sx << " "
- << aFootPosition.sy << " "
+ cout << aFootPosition.sx << " "
+ << aFootPosition.sy << " "
<< aFootPosition.theta << endl;
- }
+ }
ODEBUG("m_RelativeFootPositions:" << m_RelativeFootPositions.size());
m_RelativeFootPositions.push_back(aFootPosition);
ODEBUG("m_RelativeFootPositions:" << m_RelativeFootPositions.size());
-
+
m_KeepLastCorrectSupportFoot= - m_KeepLastCorrectSupportFoot;
-
+
}
@@ -856,7 +909,7 @@ void StepStackHandler::AddStepInTheStack(double sx, double sy,
aFootPosition.theta = theta;
aFootPosition.SStime = sstime;
aFootPosition.DStime = dstime;
- aFootPosition.stepType = 0;
+ aFootPosition.stepType = 0;
m_RelativeFootPositions.push_back(aFootPosition);
}
@@ -866,7 +919,7 @@ void StepStackHandler::PushFrontAStepInTheStack(RelativeFootPosition &aRFP)
m_RelativeFootPositions.push_front(aRFP);
}
-// Make sure that the previous motion will finish
+// Make sure that the previous motion will finish
// on the last specified correct support foot.
void StepStackHandler::FinishOnTheLastCorrectSupportFoot()
{
@@ -906,7 +959,7 @@ double StepStackHandler::GetDoubleTimeSupport()
void StepStackHandler::m_PartialStepSequence(istringstream &strm)
{
RelativeFootPosition aFootPosition;
-
+
while(!strm.eof())
{
@@ -915,11 +968,11 @@ void StepStackHandler::m_PartialStepSequence(istringstream &strm)
else break;
if (!strm.eof())
strm >> aFootPosition.sy;
- else
+ else
break;
if (!strm.eof())
strm >> aFootPosition.theta;
- else
+ else
break;
aFootPosition.DStime = m_DoubleSupportTime;
@@ -960,36 +1013,36 @@ void StepStackHandler::CallMethod(std::string &Method, std::istringstream &strm)
while(!strm.eof())
{
-
+
if (!strm.eof())
strm >> x;
else break;
-
+
if (!strm.eof())
strm >> y;
else break;
-
+
if (!strm.eof())
strm >> theta;
else break;
-
+
}
AddStandardOnLineStep(true,x,y,theta);
-
+
}
else if (Method==":arc")
{
double x,y,R=0.0,arc_deg;
int SupportFoot=-1;
-
-
+
+
while(!strm.eof())
{
-
+
if (!strm.eof())
strm >> x;
else break;
-
+
if (!strm.eof())
strm >> y;
else break;
@@ -997,14 +1050,14 @@ void StepStackHandler::CallMethod(std::string &Method, std::istringstream &strm)
if (!strm.eof())
strm >> arc_deg;
else break;
-
+
if (!strm.eof())
strm >> SupportFoot;
else break;
}
-
-
+
+
CreateArcInStepStack(x,y,R,arc_deg,SupportFoot);
}
else if (Method==":arccentered")
@@ -1012,10 +1065,10 @@ void StepStackHandler::CallMethod(std::string &Method, std::istringstream &strm)
double R,arc_deg;
int SupportFoot=-1;
ODEBUG4("m_CreateArcCenteredInStepStack 1", "DebugData.txt");
-
+
while(!strm.eof())
{
-
+
if (!strm.eof())
strm >> R;
else break;
@@ -1023,7 +1076,7 @@ void StepStackHandler::CallMethod(std::string &Method, std::istringstream &strm)
if (!strm.eof())
strm >> arc_deg;
else break;
-
+
if (!strm.eof())
strm >> SupportFoot;
else break;
diff --git a/src/StepStackHandler.hh b/src/StepStackHandler.hh
index e9b37bbb6ae31cc283c351455b6dcefac521a8e9..c7071f5097c35bb295b414c2af258de46d7af53f 100644
--- a/src/StepStackHandler.hh
+++ b/src/StepStackHandler.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
@@ -19,12 +19,12 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file StepStackHandler.h
\brief This object handle the step stack of the pattern generator.
- It allows also to create automatically stack of steps according to
+ It allows also to create automatically stack of steps according to
some high level functionnalities.
*/
@@ -46,10 +46,10 @@ namespace PatternGeneratorJRL
/*! @ingroup pgjrl
This class is in charge of handling the stack of footprints.
There is two modes currently:
- - An off-line mode, where the complete stack is send to
+ - An off-line mode, where the complete stack is send to
the ZMP reference trajectory generator object, and created off-line.
- - An on-line mode, where one step at a time is send to
- the ZMP reference trajectory generator object.
+ - An on-line mode, where one step at a time is send to
+ the ZMP reference trajectory generator object.
*/
class StepStackHandler : public SimplePlugin
{
@@ -77,29 +77,33 @@ namespace PatternGeneratorJRL
/*! \brief Set the link towards an instance of Step Over planner. */
void SetStepOverPlanner(StepOverPlanner *aSOP);
- /*! \brief Take a serie of string as an input and
+ /*! \brief Take a serie of string as an input and
read the steps according to the chosen walkmode. */
+
void ReadStepSequenceAccordingToWalkMode(std::istringstream &strm);
+ void ReadStepStairSequenceAccordingToWalkMode(std::istringstream &strm);
/*! \brief Real a partial sequence of steps
- without termination and immediate execution. */
+ without termination and immediate execution. */
void m_PartialStepSequence(std::istringstream &strm);
+ void m_PartialStepStairSequence(std::istringstream &strm);
+
/*! \brief Set the single time support. */
void SetSingleTimeSupport(double aSingleSupportTime);
/*! \brief Get the time for single support. */
double GetSingleTimeSupport();
-
+
/*! \brief Set the time for double support. */
void SetDoubleTimeSupport(double aDoubleSupportTime);
-
+
/*! \brief Get the time for double support. */
double GetDoubleTimeSupport();
/*! \brief Prepare the stack to start for a specific support foot. */
void PrepareForSupportFoot(int SupportFoot);
-
+
/*! \brief To force the last generated support foot. */
void FinishOnTheLastCorrectSupportFoot();
@@ -110,18 +114,18 @@ namespace PatternGeneratorJRL
void CopyRelativeFootPosition(std::deque<RelativeFootPosition> & lRelativeFootPositions,
bool PerformClean);
- /*! \name Method related to online stepping.
+ /*! \name Method related to online stepping.
@{
*/
-
+
/*! \brief Start On Line stepping. */
void StartOnLineStep();
-
+
/*! \brief Stop On Line stepping. */
void StopOnLineStep();
/*! \brief Add a standard step on the stack. */
- void AddStandardOnLineStep(bool NewStep,
+ void AddStandardOnLineStep(bool NewStep,
double NewStepX,
double NewStepY,
double Theta);
@@ -131,11 +135,11 @@ namespace PatternGeneratorJRL
bool IsOnLineSteppingOn();
/*! @} */
- /*! \brief Methods to handle the stack.
+ /*! \brief Methods to handle the stack.
@{
*/
- /*! \brief Remove the first step in the stack.
+ /*! \brief Remove the first step in the stack.
@return Returns true if this is the end of the sequence. */
bool RemoveFirstStepInTheStack();
@@ -144,6 +148,10 @@ namespace PatternGeneratorJRL
double theta, double sstime,
double dstime);
+ void AddStepStairInTheStack(double sx, double sy, double sz,
+ double theta, double sstime,
+ double dstime);
+
/*! \brief Push a step in front of the stack. */
void PushFrontAStepInTheStack(RelativeFootPosition &aRFP);
@@ -158,7 +166,7 @@ namespace PatternGeneratorJRL
/*! @} */
- /*! \name High level methods to create stack of steps for large motion.
+ /*! \name High level methods to create stack of steps for large motion.
@{
*/
/*! \brief Create a sequence of step to realize an arc of rayon R,
@@ -167,7 +175,7 @@ namespace PatternGeneratorJRL
the center of the arc.
*/
void CreateArcCenteredInStepStack( double R,
- double arc_deg,
+ double arc_deg,
int SupportFoot);
/*! \brief Create a sequence of steps to realize an arc of rayon R,
for arc_deg degrees, starting with the support foot defined
@@ -196,17 +204,17 @@ namespace PatternGeneratorJRL
int m_WalkMode;
/*! Link to the step over planner. */
- StepOverPlanner *m_StOvPl;
+ StepOverPlanner *m_StOvPl;
/*! Default value for Single time support and double time support. */
double m_SingleSupportTime, m_DoubleSupportTime;
/*! Variable for delta feasibility limit */
- double m_DeltaFeasibilityLimit;
-
+ double m_DeltaFeasibilityLimit;
+
/*! On line step stack handling. */
bool m_OnLineSteps;
-
+
/*! Transition for finishing on line stepping. */
bool m_TransitionFinishOnLine;
};
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp
index b60cd7bf4e09c431179d95d3920cfdc6748939b3..9c61029f0aca5bec98a781bbcdcb98abacb14ce1 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Olivier Stasse
*
@@ -18,15 +18,15 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* This object generate the reference value for the
ZMP based on a polynomail representation
- of the ZMP following
+ of the ZMP following
"Experimentation of Humanoid Walking Allowing Immediate
Modification of Foot Place Based on Analytical Solution"
- Morisawa, Harada, Kajita, Nakaoka, Fujiwara, Kanehiro, Hirukawa,
+ Morisawa, Harada, Kajita, Nakaoka, Fujiwara, Kanehiro, Hirukawa,
ICRA 2007, 3989--39994
*/
@@ -70,6 +70,7 @@ namespace PatternGeneratorJRL
m_DeltaTj.clear();
m_Omegaj.clear();
m_VerboseLevel=0;
+ m_isStepStairOn =1;
}
@@ -143,7 +144,7 @@ namespace PatternGeneratorJRL
for(unsigned int i=0;i<obj.m_DeltaTj.size();i++)
os << obj.m_DeltaTj[i] << " ";
os << endl;
-
+
os << "Step Types:" << endl;
for(unsigned int i=0;i<obj.m_StepTypes.size();i++)
os << obj.m_StepTypes[i] << " ";
@@ -155,7 +156,7 @@ namespace PatternGeneratorJRL
os << endl;
return os;
}
-
+
void AnalyticalMorisawaAbstract::displayDeltaTj(ostream &aos)
{
aos << "AnalyticalMorisawaCompact:";
@@ -169,5 +170,15 @@ namespace PatternGeneratorJRL
}
aos << endl;
}
-
+
+ void AnalyticalMorisawaAbstract::GetIsStepStairOn(int &isStepStairOn)
+ {
+ isStepStairOn = m_isStepStairOn;
+ }
+
+ void AnalyticalMorisawaAbstract::SetIsStepStairOn(int isStepStairOn)
+ {
+ m_isStepStairOn = isStepStairOn;
+ }
+
}
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.hh b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.hh
index 434adc3ab9424f26436633f9cfd707e9741d426b..a59468ab155b98c0f6808c90ac01591868352668 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.hh
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file AnalyticalMorisawaAbstract.h
@@ -26,8 +26,8 @@
ZMP based on a polynomial representation following the paper
"Experimentation of Humanoid Walking Allowing Immediate
Modification of Foot Place Based on Analytical Solution"
- Morisawa, Harada, Kajita, Nakaoka, Fujiwara, Kanehiro, Hirukawa,
- ICRA 2007, 3989--3994
+ Morisawa, Harada, Kajita, Nakaoka, Fujiwara, Kanehiro, Hirukawa,
+ ICRA 2007, 3989--3994
*/
#ifndef _ANALYTICAL_MORISAWA_ABSTRACT_H_
#define _ANALYTICAL_MORISAWA_ABSTRACT_H_
@@ -48,12 +48,12 @@ namespace PatternGeneratorJRL
@ingroup analyticalformulation
Using this method we assume again the linear inverted pendulum of Kajita,
- and therefore assume that
-
+ and therefore assume that
+
*/
class AnalyticalMorisawaAbstract: public ZMPRefTrajectoryGeneration
{
-
+
public:
const static unsigned int SINGLE_SUPPORT=0;
@@ -61,41 +61,41 @@ namespace PatternGeneratorJRL
/*! Constructor */
AnalyticalMorisawaAbstract(SimplePluginManager * lSPM);
-
+
/*! Destructor */
virtual ~AnalyticalMorisawaAbstract();
/*! \name Methods inherited from ZMPRefTrajectoryGeneration and reimplemented
@{ */
- /*! Returns the CoM and ZMP trajectory for some relative foot positions.
+ /*! Returns the CoM and ZMP trajectory for some relative foot positions.
Generate ZMP discreatization from a vector of foot position.
ASSUME A COMPLETE MOTION FROM END TO START, and GENERATE EVERY VALUE.
-
+
@param[out] ZMPPositions: Returns the ZMP reference values for the overall motion.
Those are absolute position in the world reference frame. The origin is the initial
position of the robot. The relative foot position specified are added.
-
+
@param[out] CoMStates: Returns the CoM reference values for the overall motion.
Those are absolute position in the world reference frame. The origin is the initial
position of the robot. The relative foot position specified are added.
- @param[in] RelativeFootPositions: The set of
+ @param[in] RelativeFootPositions: The set of
relative steps to be performed by the robot.
-
+
@param[out] LeftFootAbsolutePositions: Returns the absolute position of the left foot.
According to the macro FULL_POLYNOME the trajectory will follow a third order
- polynom or a fifth order. By experience it is wise to put a third order.
+ polynom or a fifth order. By experience it is wise to put a third order.
A null acceleration might cause problem for the compensation of the Z-axis momentum.
-
+
@param[out] RightFootAbsolutePositions: Returns the absolute position of the right foot.
-
+
@param[in] Xmax: The maximal distance of a hand along the X axis in the waist coordinates.
@param[in] lStartingCOMState: The starting position of the CoM.
@param[in] lStartingZMPPosition: The starting position of the ZMP.
-
+
@param[in] InitLeftFootAbsolutePosition: The absolute position of the left foot.
@param[in] InitRightFootAbsolutePosition: The absolute position of the right foot.
@@ -111,7 +111,7 @@ namespace PatternGeneratorJRL
MAL_S3_VECTOR_TYPE(double) &lStartingZMPPosition,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition) =0;
-
+
/*! Methods for on-line generation. (First version)
The queues will be updated as follows:
\li \c The first values necessary to start walking will be inserted.
@@ -119,7 +119,7 @@ namespace PatternGeneratorJRL
according to InitLeftFootAbsolutePosition and InitRightFootAbsolutePosition.
\li \c The RelativeFootPositions stack will be taken into account,
\li \c The starting COM Position.
- Returns the number of steps which has been completely put inside
+ Returns the number of steps which has been completely put inside
the queue of ZMP, and foot positions.
*/
virtual int InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
@@ -131,18 +131,18 @@ namespace PatternGeneratorJRL
deque<RelativeFootPosition> &RelativeFootPositions,
COMState & lStartingCOMState,
MAL_S3_VECTOR(&,double) aStartingZMPPosition) =0 ;
-
+
/* ! \brief Method to update the stacks on-line */
virtual void OnLine(double time,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMStates,
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & CoMStates,
deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions)=0;
-
+
/* ! Methods to update the stack on-line by inserting a new foot position. */
virtual void OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosition,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMStates,
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & CoMStates,
deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
bool EndSequence)=0;
@@ -152,7 +152,7 @@ namespace PatternGeneratorJRL
*/
virtual int OnLineFootChange(double time,
FootAbsolutePosition &aFootAbsolutePosition,
- deque<ZMPPosition> & FinalZMPPositions,
+ deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMStates,
deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
@@ -172,14 +172,14 @@ namespace PatternGeneratorJRL
/*! @} */
-
+
/*! \name Methods specifics to our current implementation.
@{ */
/*! Set the number of steps in advance
@return: true if everything went fine, false otherwise.
*/
bool SetNumberOfStepsInAdvance(int lNumberOfStepsInAdvance);
-
+
/*! Get the number of steps in advance.
@return: return the number of intervals.
*/
@@ -197,16 +197,21 @@ namespace PatternGeneratorJRL
/*! Set the \f[ \Delta T_j \f] */
int SetDeltaTj(std::vector<double> &GetDeltaTj);
- /*! \name Methods for the full linear system
+
+ void GetIsStepStairOn(int &isStepStairOn);
+
+ void SetIsStepStairOn(int isStepStairOn);
+
+ /*! \name Methods for the full linear system
@{ */
- /*! \brief Building the Z matrix to be inverted.
+ /*! \brief Building the Z matrix to be inverted.
@param lCoM: Profile of CoM for each interval.
@param lZMP: Profile of ZMP for each interval.
*/
virtual void BuildingTheZMatrix(std::vector<double> &lCoM, std::vector<double> &lZMP )=0;
- /*! \brief Building the w vector.
+ /*! \brief Building the w vector.
It is currently assume that all ZMP's speed will be
set to zero, as well as the final COM's speed.
The sequence of ZMPSequence is the final value of the
@@ -244,7 +249,7 @@ namespace PatternGeneratorJRL
/*! @} */
- /*! \brief Initialize automatically Polynomial degrees, and temporal intervals.
+ /*! \brief Initialize automatically Polynomial degrees, and temporal intervals.
@return True if succeedeed, false otherwise.
*/
virtual bool InitializeBasicVariables()=0;
@@ -254,12 +259,12 @@ namespace PatternGeneratorJRL
/*! \brief Compute the polynomial weights. */
virtual void ComputePolynomialWeights()=0;
-
+
/*! \brief Get the polynomial degrees for the trajectory designed with
this method. */
void GetPolynomialDegrees(std::vector<unsigned int> &lPolynomialDegrees);
-
+
friend std::ostream& operator <<(std::ostream &os,const AnalyticalMorisawaAbstract &obj);
/*! \brief Compute the preview control time window. */
@@ -273,8 +278,8 @@ namespace PatternGeneratorJRL
/*! @} */
protected:
- /*! \name Store the matrices used for compution.
- @{
+ /*! \name Store the matrices used for compution.
+ @{
*/
/*! One of the most important matrix which stores all the temporal
@@ -289,16 +294,16 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) m_y;
/*! @} */
-
- /*! \name Control variables.
+
+ /*! \name Control variables.
@{. */
-
+
/*! \brief Number of steps ($NbSteps$) on which the polynomials coefficients are computed.
*/
- int m_NumberOfStepsInAdvance;
-
+ int m_NumberOfStepsInAdvance;
+
/*! Set of number of intervals.
- The relationship with the number of steps in advance is :
+ The relationship with the number of steps in advance is :
2 * m_NumberOfStepsInAdvance */
int m_NumberOfIntervals;
@@ -311,51 +316,51 @@ namespace PatternGeneratorJRL
/*! Vector build upon the CoM and ZMP profile along the Z axis */
std::vector<double> m_Omegaj;
- /*! \brief Set of polynomial degrees.
+ /*! \brief Set of polynomial degrees.
The current strategy is to have the first and the last ones set to 4,
while the intermediate intervals uses a 3rd order polynomial.
*/
std::vector<unsigned int> m_PolynomialDegrees;
-
+
/*! \name Internal Methods to compute the full linear
- system.
+ system.
@{
*/
/*! \brief Building the Z1 Matrix */
virtual void ComputeZ1(unsigned int &lindex) =0 ;
-
- /*! \brief Building the Zj Matrix
- @param[in] intervalindex: Index of the interval,
+
+ /*! \brief Building the Zj Matrix
+ @param[in] intervalindex: Index of the interval,
@param[in] colindex: Index of the column inside the matrix,
@param[in] rowindex: Index of the row inside the matrix. */
- virtual void ComputeZj(unsigned int intervalindex,
- unsigned int &colindex,
+ virtual void ComputeZj(unsigned int intervalindex,
+ unsigned int &colindex,
unsigned int &rowindex) =0;
-
- /*! \brief Building the Zm Matrix
- @param[in] intervalindex: Index of the interval,
+
+ /*! \brief Building the Zm Matrix
+ @param[in] intervalindex: Index of the interval,
@param[in] colindex: Index of the column inside the matrix,
@param[in] rowindex: Index of the row inside the matrix.
*/
- virtual void ComputeZm(unsigned int intervalindex,
- unsigned int &colindex,
+ virtual void ComputeZm(unsigned int intervalindex,
+ unsigned int &colindex,
unsigned int &rowindex)=0;
/*! @} */
/*! \brief Absolute Reference time */
double m_AbsoluteTimeReference;
-
+
/*! @} */
/*! \brief Reference to the Humanoid Specificities. */
CjrlHumanoidDynamicRobot * m_HS;
-
-
- /*! \name Debugging fields
+
+
+ /*! \name Debugging fields
@{
*/
/*! \brief Verbose level for debugging purposes.*/
@@ -365,15 +370,17 @@ namespace PatternGeneratorJRL
void displayDeltaTj(ostream &aos);
/*! @} */
-
+
+ int m_isStepStairOn;
+
public:
- /*! \brief Get the absolute reference time of
+ /*! \brief Get the absolute reference time of
the system */
double GetAbsoluteTimeReference() const
{ return m_AbsoluteTimeReference; }
-
- /*! \brief Set the absolute reference time of
+
+ /*! \brief Set the absolute reference time of
the system */
void SetAbsoluteTimeReference(double anAbsoluteTimeReference)
{ m_AbsoluteTimeReference = anAbsoluteTimeReference; }
@@ -383,7 +390,7 @@ namespace PatternGeneratorJRL
CjrlHumanoidDynamicRobot * GetHumanoidSpecificites() const
{ return m_HS;};
- /*! \brief Set the reference of the object handling the
+ /*! \brief Set the reference of the object handling the
humanoid specificities */
void SetHumanoidSpecificities(CjrlHumanoidDynamicRobot *aHS)
{ m_HS = aHS; };
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index bb0993d735daa72f16bc78354cf656f357036417..9d20273ca884194102139ea7c84e41b552f50946 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -101,6 +101,7 @@ namespace PatternGeneratorJRL
/*! Dynamic allocation of the analytical trajectories for the ZMP and the COG */
m_AnalyticalZMPCoGTrajectoryX = new AnalyticalZMPCOGTrajectory(7);
m_AnalyticalZMPCoGTrajectoryY = new AnalyticalZMPCOGTrajectory(7);
+ // m_AnalyticalZMPCoGTrajectoryZ = new AnalyticalZMPCOGTrajectory(7);
/*! Dynamic allocation of the filters. */
m_FilterXaxisByPC = new FilteringAnalyticalTrajectoryByPreviewControl(lSPM,
@@ -2588,7 +2589,7 @@ new step has to be generate.
unsigned int lIndexInterval,lPrevIndexInterval;
m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(m_AbsoluteTimeReference,lIndexInterval);
lPrevIndexInterval = lIndexInterval;
-
+ double preCoMz = 0 ;
/*! Fill in the stacks: minimal strategy only 1 reference. */
for(double t=StartingTime; t<=EndTime; t+= samplingPeriod)
{
@@ -2640,14 +2641,102 @@ new step has to be generate.
FinalCoMPositions.push_back(aCOMPos);
+ ComputeCoMz(t,aCOMPos.z[0]);
+
+ aCOMPos.z[1] = (aCOMPos.z[0] - preCoMz) / m_SamplingPeriod;
+ preCoMz = aCOMPos.z[0];
+
+
+ FinalCoMPositions.push_back(aCOMPos);
ODEBUG4(aZMPPos.px << " " << aZMPPos.py << " " <<
- aCOMPos.x[0] << " " << aCOMPos.y[0] << " " <<
+ aCOMPos.x[0] << " " << aCOMPos.y[0] << " " << aCOMPos.z[0] <<" "<<
LeftFootAbsPos.x << " " << LeftFootAbsPos.y << " " << LeftFootAbsPos.z << " " <<
RightFootAbsPos.x << " " << RightFootAbsPos.y << " " << RightFootAbsPos.z << " " <<
samplingPeriod,"Test.dat");
}
}
+
+ void AnalyticalMorisawaCompact::ComputeCoMz(double t, double &CoMz)
+ {
+
+ int Index,Index2;
+ double moving_time = m_RelativeFootPositions[0].SStime + m_RelativeFootPositions[0].DStime;
+ double deltaZ;
+ // double static CoMzpre = CoMz;
+ double up=0.1,upRight = 0.9 ,upLeft = 0.0;
+ double upRight1 = 0.9 ,upLeft1 = 0.0;
+
+
+ double down = 0.1, downRight =0.9, downLeft = 0.0;
+
+ if (t >= moving_time){ // we start analyze since 2nd step
+
+ Index2 = int(t/moving_time);
+
+
+ if (Index2 < m_AbsoluteSupportFootPositions.size())
+ {
+ // climbing
+
+ // put first leg on the stairs with decrease of CoM //up// of stair height
+ // the CoM line will decrease between an //upLeft to upRight// interval of SStime.
+ // the CoM line will go up between an //upLeft1 to upRight1// interval of SStime while 2nd leg moving up on the stairs.
+ if (m_AbsoluteSupportFootPositions[Index2].z > m_AbsoluteSupportFootPositions[Index2-1].z) // first leg
+ {
+ deltaZ = (-m_AbsoluteSupportFootPositions[Index2].z + m_AbsoluteSupportFootPositions[Index2-1].z );
+ if (t <= Index2*moving_time + upRight*m_RelativeFootPositions[Index2].SStime && t >= Index2*moving_time + upLeft*m_RelativeFootPositions[Index2].SStime)
+ CoMz = (t-Index2*moving_time - upLeft*m_RelativeFootPositions[Index2].SStime)*up*deltaZ/((upRight-upLeft)*m_RelativeFootPositions[Index2].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z ;
+ else if (t < Index2*moving_time + upLeft*m_RelativeFootPositions[Index2].SStime)
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z ;
+ else
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z + up*deltaZ;
+ }
+ else if (m_AbsoluteSupportFootPositions[Index2].z == m_AbsoluteSupportFootPositions[Index2-1].z && m_RelativeFootPositions[Index2-1].sz > 0) // 2nd leg
+ {
+ deltaZ = (m_AbsoluteSupportFootPositions[Index2].z - m_AbsoluteSupportFootPositions[Index2-2].z );
+ if (t <= Index2*moving_time + upRight1*m_RelativeFootPositions[Index2].SStime && t >= Index2*moving_time + upLeft1*m_RelativeFootPositions[Index2].SStime)
+ CoMz = (t-Index2*moving_time - upLeft1*m_RelativeFootPositions[Index2].SStime)*(1+up)*deltaZ/((upRight1-upLeft1)*m_RelativeFootPositions[Index2].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-2].z - up*deltaZ ;
+ else if (t < Index2*moving_time + upLeft1*m_RelativeFootPositions[Index2].SStime)
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-2].z - up*deltaZ;
+ else
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z;
+ }
+
+ // going down
+ // the CoM line will decrease an //1+down// stair height between an //downLeft to downRight// interval of SStime while moving first leg down
+ // put the 2nd leg down while standing up the CoM.
+ else if (m_AbsoluteSupportFootPositions[Index2].z < m_AbsoluteSupportFootPositions[Index2-1].z && m_RelativeFootPositions[Index2].sz < 0) // first leg
+ {
+ deltaZ = (m_AbsoluteSupportFootPositions[Index2].z - m_AbsoluteSupportFootPositions[Index2-1].z );
+ if (t <= Index2*moving_time + downRight*m_RelativeFootPositions[Index2].SStime && t >= Index2*moving_time + downLeft*m_RelativeFootPositions[Index2].SStime)
+ CoMz = (t-Index2*moving_time- downLeft*m_RelativeFootPositions[Index2].SStime)*(1+down)*deltaZ/((downRight - downLeft)*m_RelativeFootPositions[Index2].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z ;
+ else if (t < Index2*moving_time + downLeft*m_RelativeFootPositions[Index2].SStime)
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z ;
+ else
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z + down*deltaZ;
+ }
+ else if (m_AbsoluteSupportFootPositions[Index2].z == m_AbsoluteSupportFootPositions[Index2-1].z && m_RelativeFootPositions[Index2-1].sz < 0) //second leg
+ {
+ deltaZ = (m_AbsoluteSupportFootPositions[Index2-2].z - m_AbsoluteSupportFootPositions[Index2].z );
+ if (t <= Index2*moving_time + m_RelativeFootPositions[Index2].SStime )
+ CoMz = (t-Index2*moving_time)*down*deltaZ/(m_RelativeFootPositions[Index2].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z - down*deltaZ ;
+ else
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z ;
+ }
+ else // normal walking
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z;
+ }
+
+ else //after final step
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions.back().z;
+ }
+ else //first step
+ CoMz = m_InitialPoseCoMHeight ;
+
+}
+
+
void AnalyticalMorisawaCompact::FillQueues(double StartingTime,
double EndTime,
deque<ZMPPosition> & FinalZMPPositions,
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
index 2f65a4b182d1b87c8f9cf73a33f61b862b9c7c9a..36f7db38c2885cb5c8f193a6620ba4635017a742 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2008, 2009, 2010,
+ * Copyright 2008, 2009, 2010,
*
* Alireza Nakhaei
* Olivier Stasse
@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file AnalyticalMorisawaCompact.h
@@ -27,10 +27,10 @@
This object generate the reference value for the
ZMP based on a polynomial representation
- of the ZMP following
+ of the ZMP following
"Experimentation of Humanoid Walking Allowing Immediate
Modification of Foot Place Based on Analytical Solution"
- Morisawa, Harada, Kajita, Nakaoka, Fujiwara, Kanehiro, Hirukawa,
+ Morisawa, Harada, Kajita, Nakaoka, Fujiwara, Kanehiro, Hirukawa,
ICRA 2007, 3989--39994
*/
#ifndef _ANALYTICAL_MORISAWA_FULL_H_
@@ -64,14 +64,14 @@ namespace PatternGeneratorJRL
double FinalCoMPos;
/*! ZMP profil for the chosen axis. */
- std::vector<double> ZMPProfil;
+ std::vector<double> ZMPProfil;
/*! Height ZMP profil.*/
std::vector<double> ZMPZ;
/*! Height COM profil. */
std::vector<double> CoMZ;
-
+
} CompactTrajectoryInstanceParameters;
/*! @ingroup analyticalformulation
@@ -90,10 +90,10 @@ namespace PatternGeneratorJRL
*/
class AnalyticalMorisawaCompact: public AnalyticalMorisawaAbstract
{
-
+
public:
- /*! \name Constants to handle errors
+ /*! \name Constants to handle errors
when changing foot steps.
@{ */
const static int ERROR_WRONG_FOOT_TYPE = -1;
@@ -107,44 +107,44 @@ namespace PatternGeneratorJRL
/*! @} */
/*! Constructor */
AnalyticalMorisawaCompact(SimplePluginManager * lSPM , CjrlHumanoidDynamicRobot *aHS);
-
+
/*! Destructor */
virtual ~AnalyticalMorisawaCompact();
/*! \name Methods inherited from ZMPRefTrajectoryGeneration and reimplemented
@{ */
- /*! Returns the CoM and ZMP trajectory for some relative foot positions.
+ /*! Returns the CoM and ZMP trajectory for some relative foot positions.
Generate ZMP discreatization from a vector of foot position.
ASSUME A COMPLETE MOTION FROM END TO START, and GENERATE EVERY VALUE.
-
+
@param[out] ZMPPositions: Returns the ZMP reference values for the overall motion.
Those are absolute position in the world reference frame. The origin is the initial
position of the robot. The relative foot position specified are added.
-
+
@param[out] CoMStates: Returns the CoM reference values for the overall motion.
Those are absolute position in the world reference frame. The origin is the initial
position of the robot. The relative foot position specified are added.
- @param[in] RelativeFootPositions: The set of
+ @param[in] RelativeFootPositions: The set of
relative steps to be performed by the robot.
-
+
@param[out] LeftFootAbsolutePositions: Returns the absolute position of the left foot.
According to the macro FULL_POLYNOME the trajectory will follow a third order
- polynom or a fifth order. By experience it is wise to put a third order.
+ polynom or a fifth order. By experience it is wise to put a third order.
A null acceleration might cause problem for the compensation of the Z-axis momentum.
-
+
@param[out] RightFootAbsolutePositions: Returns the absolute position of the right foot.
-
-
+
+
@param[in] Xmax: The maximal distance of a hand along the X axis in the waist coordinates.
-
+
@param[in] lStartingCOMState: The initial position of the CoM.
@param[in] lStartingZMPPosition: The initial position of the ZMP.
@param[in] InitLeftFootAbsolutePosition: The initial position of the left foot.
-
+
@param[in] InitRightFootAbsolutePosition: The initial position of the right foot.
*/
void GetZMPDiscretization(deque<ZMPPosition> & ZMPPositions,
@@ -157,8 +157,8 @@ namespace PatternGeneratorJRL
MAL_S3_VECTOR_TYPE(double) &lStartingZMPPosition,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition);
-
- /*! \brief Methods for on-line generation.
+
+ /*! \brief Methods for on-line generation.
The queues will be updated as follows:
- The first values necessary to start walking will be inserted.
- The initial positions of the feet will be taken into account
@@ -166,7 +166,7 @@ namespace PatternGeneratorJRL
- The RelativeFootPositions stack will be taken into account,
in this case only three steps will be removed from the stack,
- The starting COM Position.
- Returns the number of steps which has been completely put inside
+ Returns the number of steps which has been completely put inside
the queue of ZMP, and foot positions.
@param[out] FinalZMPPositions: The queue of ZMP reference positions.
@@ -189,11 +189,11 @@ namespace PatternGeneratorJRL
COMState &lStartingCOMState,
MAL_S3_VECTOR(&,double) lStartingZMPPosition
);
-
- /* ! \brief Methods to update the stack on-line by inserting a new foot position.
+
+ /* ! \brief Methods to update the stack on-line by inserting a new foot position.
The foot is put right at the end of the stack. This method is supposed to be called
when the first foot in the stack is finished.
-
+
@param[in] NewRelativeFootPosition: The new foot to put in the stack.
@param[out] FinalZMPPositions: The stack of final ZMP positions to be updated
with the new relative foot position.
@@ -205,16 +205,16 @@ namespace PatternGeneratorJRL
*/
void OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosition,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMStates,
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & CoMStates,
deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
bool EndSequence);
-
+
/* ! \brief Method to update the stacks on-line */
void OnLine(double time,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMStates,
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & CoMStates,
deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions);
@@ -222,8 +222,8 @@ namespace PatternGeneratorJRL
@return If the method failed it returns -1, 0 otherwise.
*/
int OnLineFootChange(double time,
- FootAbsolutePosition &aFootPosition,
- deque<ZMPPosition> & FinalZMPPositions,
+ FootAbsolutePosition &aFootPosition,
+ deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMStates,
deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
@@ -233,8 +233,8 @@ namespace PatternGeneratorJRL
@return If the method failed it returns -1, 0 otherwise.
*/
int OnLineFootChanges(double time,
- deque<FootAbsolutePosition> &FeetPosition,
- deque<ZMPPosition> & FinalZMPPositions,
+ deque<FootAbsolutePosition> &FeetPosition,
+ deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMStates,
deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
@@ -252,19 +252,19 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> &RightFootAbsolutePositions);
-
- /*! \brief Return the time at which it is optimal to regenerate a step in online mode.
+
+ /*! \brief Return the time at which it is optimal to regenerate a step in online mode.
*/
int ReturnOptimalTimeToRegenerateAStep();
-
+
/*! @} */
-
+
/*! \name Methods specifics to our current implementation.
@{ */
- /*! \name Methods for the full linear system
+ /*! \name Methods for the full linear system
@{ */
- /*! \brief Building the Z matrix to be inverted.
+ /*! \brief Building the Z matrix to be inverted.
@param[in] lCoMZ: Profile of the CoM's height trajectory for each interval.
@param[in] lZMPZ: Profile of the ZMP's height trajectory for each interval.
*/
@@ -273,7 +273,7 @@ namespace PatternGeneratorJRL
/*! \brief Building the Z matrix to be inverted. */
void BuildingTheZMatrix();
- /*! \brief Building the w vector.
+ /*! \brief Building the w vector.
It is currently assume that all ZMP's speed will be
set to zero, as well as the final COM's speed.
The sequence of ZMPSequence is the final value of the
@@ -311,7 +311,7 @@ namespace PatternGeneratorJRL
bool InitializeaAZCT);
/*! @} */
- /*! \brief Initialize automatically Polynomial degrees, and temporal intervals.
+ /*! \brief Initialize automatically Polynomial degrees, and temporal intervals.
@return True if succeedeed, false otherwise.
*/
bool InitializeBasicVariables();
@@ -332,13 +332,13 @@ namespace PatternGeneratorJRL
problem */
void ResetTheResolutionOfThePolynomial();
- /*! \brief For the current time t, we will change the foot position
- (NewPosX, NewPosY) during time interval IndexStep and IndexStep+1, using
- the AnalyticalZMPCOGTrajectory objects and their parameters.
+ /*! \brief For the current time t, we will change the foot position
+ (NewPosX, NewPosY) during time interval IndexStep and IndexStep+1, using
+ the AnalyticalZMPCOGTrajectory objects and their parameters.
IndexStep has to be a double support phase, because it determines
the landing position. It is also assumes that m_RelativeFootPositions
and m_AbsoluteSupportFootPositions are set for the new values.
-
+
@param[in] t : The current time.
@param[in] IndexStep: The index of the interval where the modification will start.
The modification of the foot position is done over 2 intervals.
@@ -371,11 +371,11 @@ namespace PatternGeneratorJRL
StepStackHandler *aStepStackHandler,
bool AddingAFootStep);
- /*! \brief For the current time t, we will change the foot position
+ /*! \brief For the current time t, we will change the foot position
(NewPosX, NewPosY) during time interval IndexStep and IndexStep+1.
IndexStep has to be a double support phase, because it determines
the landing position.
-
+
@param[in] t : The current time.
@param[in] IndexStep: The index of the interval where the modification will start.
The modification of the foot position is done over 2 intervals.
@@ -387,16 +387,16 @@ namespace PatternGeneratorJRL
int ChangeFootLandingPosition(double t,
vector<unsigned int> & IndexStep,
vector<FootAbsolutePosition> & NewFootAbsPos);
-
-
+
+
/*! @} */
-
+
/*! Put an error messages string in ErrorMessage,
according to ErrorIndex. */
void StringErrorMessage(int ErrorIndex, string & ErrorMessage);
- /*! \brief This method filter out the orthogonal trajectory to minimize the
- fluctuation involved by the time shift.
+ /*! \brief This method filter out the orthogonal trajectory to minimize the
+ fluctuation involved by the time shift.
*/
void FilterOutOrthogonalDirection(AnalyticalZMPCOGTrajectory & aAZCT,
CompactTrajectoryInstanceParameters &aCTIP,
@@ -406,21 +406,21 @@ namespace PatternGeneratorJRL
- /*! \name Feet Trajectory Generator methods
+ /*! \name Feet Trajectory Generator methods
@{ */
/*! Set the feet trajectory generator */
void SetFeetTrajectoryGenerator(LeftAndRightFootTrajectoryGenerationMultiple * aFeetTrajectoryGenerator);
-
+
/*! Get the feet trajectory generator */
LeftAndRightFootTrajectoryGenerationMultiple * GetFeetTrajectoryGenerator();
-
+
/*! Setter and getter for the ComAndZMPTrajectoryGeneration. */
inline ComAndFootRealization * getComAndFootRealization()
{ return m_kajitaDynamicFilter->getComAndFootRealization();};
/*! @} */
-
- /*! Simple plugin interfaces
+
+ /*! Simple plugin interfaces
@{
*/
/*! Register methods. */
@@ -428,30 +428,30 @@ namespace PatternGeneratorJRL
/*! Call methods according to the arguments. */
void CallMethod(std::string & Method, std::istringstream &strm);
-
+
/*! @} */
protected:
-
-
+
+
/*! \name Internal Methods to compute the full linear
- system.
+ system.
@{
*/
/*! \brief Building the Z1 Matrix */
void ComputeZ1(unsigned int &lindex);
- /*! \brief Building the Zj Matrix
- @param intervalindex: Index of the interval,
+ /*! \brief Building the Zj Matrix
+ @param intervalindex: Index of the interval,
@param colindex: Index of the column inside the matrix,
@param rowindex: Index of the row inside the matrix. */
- void ComputeZj(unsigned int intervalindex,
- unsigned int &colindex,
+ void ComputeZj(unsigned int intervalindex,
+ unsigned int &colindex,
unsigned int &rowindex);
-
+
/*! \brief Building the Zm Matrix */
- void ComputeZm(unsigned int intervalindex,
- unsigned int &colindex,
+ void ComputeZm(unsigned int intervalindex,
+ unsigned int &colindex,
unsigned int &rowindex);
/*! \brief Considering the current time given by LocalTime,
@@ -470,9 +470,9 @@ namespace PatternGeneratorJRL
and the index of the first interval. */
void NewTimeIntervals(unsigned IndexStartingInterval,
double NewTime);
-
+
/*! \brief Recompute the trajectories based on the current time (LocalTime),
- the new landing position and the time interval (IndexStep) when the
+ the new landing position and the time interval (IndexStep) when the
modification should take place.
*/
void ConstraintsChange(double LocalTime,
@@ -482,21 +482,21 @@ namespace PatternGeneratorJRL
CompactTrajectoryInstanceParameters &aCTIPY,
unsigned int IndexStartingInterval,
StepStackHandler *aStepStackHandler=0);
-
+
/*! \brief Compute the time to compensate for the ZMP fluctuation. */
double TimeCompensationForZMPFluctuation(FluctuationParameters &aFluctuationParameters,
double DeltaTInit);
/*! @} */
- /*! \name Internal Methods to generate steps and create the associated
- problem.
-
+ /*! \name Internal Methods to generate steps and create the associated
+ problem.
+
@{
- */
+ */
/*! \brief Build and solve the linear problem associated with a set of relative footholds.
- @param[in] lStartingCOMState: Specify the initial condition of the CoM \f$(x,y,z)\f$ for the
+ @param[in] lStartingCOMState: Specify the initial condition of the CoM \f$(x,y,z)\f$ for the
resolution of the linear problem. The matrix is defined as:
\f[
\left(
@@ -518,7 +518,7 @@ namespace PatternGeneratorJRL
@param[in] IgnoreFirstRelativeFoot: Boolean to ignore the first relative foot.
@param[in] DoNotPrepareLastFoot: Boolean to not perform for the ending sequence.
-
+
*/
int BuildAndSolveCOMZMPForASetOfSteps(MAL_S3x3_MATRIX(& ,double) lStartingCOMState,
FootAbsolutePosition &LeftFootInitialPosition,
@@ -548,7 +548,7 @@ namespace PatternGeneratorJRL
\param StartingTime: The starting time to fill in the queues.
\param EndTime: The ending time to fill in the queues.
\param FinalZMPPositions: The queue of ZMP positions. More specifically fill in \f$px\f$ and \f$py\f$.
- \param FinalCoMPositions: The queue of CoM positions. More specifically fill in \f$x,\dot{x},y,\dot{y}, z\f$,
+ \param FinalCoMPositions: The queue of CoM positions. More specifically fill in \f$x,\dot{x},y,\dot{y}, z\f$,
\param FinalLeftFootAbsolutePositions: The queue of Left Foot Absolute positions.
\param FinalRightFootAbsolutePositions: The queue of Right Foot Absolute positions.
*/
@@ -559,6 +559,8 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions);
+ void ComputeCoMz(double t, double &CoMz);
+
void FillQueues(double samplingPeriod,
double StartingTime,
double EndTime,
@@ -585,19 +587,22 @@ namespace PatternGeneratorJRL
precomputed Z matrix LU decomposition */
bool m_NeedToReset;
- /*! \brief Pointer to the preview control object used to
+ /*! \brief Pointer to the preview control object used to
filter out the orthogonal direction . */
PreviewControl * m_PreviewControl;
- /*! \name Object to handle trajectories.
+ /*! \name Object to handle trajectories.
@{
*/
/*! \brief Analytical sagital trajectories */
AnalyticalZMPCOGTrajectory *m_AnalyticalZMPCoGTrajectoryX;
-
+
/*! \brief Analytical sagital trajectories */
AnalyticalZMPCOGTrajectory *m_AnalyticalZMPCoGTrajectoryY;
+ /*! \brief Analytical sagital trajectories */
+ // AnalyticalZMPCOGTrajectory *m_AnalyticalZMPCoGTrajectoryZ;
+
/*! \brief Foot Trajectory Generator */
LeftAndRightFootTrajectoryGenerationMultiple * m_FeetTrajectoryGenerator;
LeftAndRightFootTrajectoryGenerationMultiple * m_BackUpm_FeetTrajectoryGenerator;
@@ -609,10 +614,10 @@ namespace PatternGeneratorJRL
/*! \brief Stores the relative foot positions currently in the buffer */
deque<RelativeFootPosition> m_RelativeFootPositions;
- /*! \brief Stores the absolute support foot positions currently in the buffer */
+ /*! \brief Stores the absolute support foot positions currently in the buffer */
deque<FootAbsolutePosition> m_AbsoluteSupportFootPositions;
- /*! \brief Store the currently realized support foot position.
+ /*! \brief Store the currently realized support foot position.
\warning This field makes sense only direct ON-LINE mode.
*/
FootAbsolutePosition m_AbsoluteCurrentSupportFootPosition;
@@ -640,7 +645,7 @@ namespace PatternGeneratorJRL
/*! \brief Height of the initial CoM position. */
double m_InitialPoseCoMHeight;
-
+
/*! \brief On-line change step mode */
unsigned int m_OnLineChangeStepMode;
@@ -654,6 +659,8 @@ namespace PatternGeneratorJRL
/*! \brief End phase */
bool m_EndPhase;
+
+
public:
/*! \name Methods related to the Preview Control object used
by this class. @{ */
@@ -663,7 +670,7 @@ namespace PatternGeneratorJRL
/*! Get the preview control object. */
PreviewControl * GetPreviewControl();
/*! @} */
-
+
/*! \brief Propagate Absolute Reference Time */
void PropagateAbsoluteReferenceTime(double x);
diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.h b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.h
deleted file mode 100644
index f5ac7f6f64c3e8a98c58c072177a1288b70212b8..0000000000000000000000000000000000000000
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.h
+++ /dev/null
@@ -1,225 +0,0 @@
-/*
- * Copyright 2010,
- *
- * Mehdi Benallegue
- * Andrei Herdt
- * Francois Keith
- * Olivier Stasse
- *
- * JRL, CNRS/AIST
- *
- * This file is part of walkGenJrl.
- * walkGenJrl is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * walkGenJrl is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Lesser Public License for more details.
- * You should have received a copy of the GNU Lesser General Public License
- * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
- *
- * Research carried out within the scope of the
- * Joint Japanese-French Robotics Laboratory (JRL)
- */
-/*
- * OrientationsPreview.h
- */
-
-#ifndef ORIENTATIONSPREVIEW_H_
-#define ORIENTATIONSPREVIEW_H_
-
-
-
-#include <deque>
-
-#include <privatepgtypes.h>
-#include <jrl/walkgen/pgtypes.hh>
-#include <abstract-robot-dynamics/joint.hh>
-
-namespace PatternGeneratorJRL
-{
- /// \brief The acceleration phase is fixed
- class OrientationsPreview {
-
- //
- // Public methods:
- //
- public:
-
- /// \name Accessors
- /// \{
- OrientationsPreview( CjrlJoint *aLeg );
- ~OrientationsPreview();
- /// \}
-
- /// \brief Preview feet and trunk orientations inside the preview window
- /// The orientations of the feet are adapted to the previewed orientation of the hip.
- /// The resulting velocities accelerations and orientations are verified against the limits.
- /// If the constraints can not be satisfied the rotational velocity of the trunk is reduced.
- /// The trunk is rotating with a constant speed after a constant acceleration phase of T_ length.
- /// During the initial double support phase the trunk is not rotating contrary to the following.
- ///
- /// \param[in] Time
- /// \param[in] Ref
- /// \param[in] StepDuration
- /// \param[in] LeftFootPositions_deq
- /// \param[in] RightFootPositions_deq
- /// \param[out] Solution Trunk and Foot orientations
- void preview_orientations(double Time,
- const reference_t & Ref,
- double StepDuration,
- const std::deque<FootAbsolutePosition> & LeftFootPositions_deq,
- const std::deque<FootAbsolutePosition> & RightFootPositions_deq,
- solution_t & Solution);
-
- /// \brief Interpolate previewed orientation of the trunk
- ///
- /// \param[in] Time
- /// \param[in] CurrentIndex
- /// \param[in] NewSamplingPeriod
- /// \param[in] PrwSupportStates_deq
- /// \param[out] FinalCOMTraj_deq
- void interpolate_trunk_orientation(double Time,
- int CurrentIndex,
- double NewSamplingPeriod,
- const std::deque<support_state_t> & PrwSupportStates_deq,
- std::deque<COMState> & FinalCOMTraj_deq);
-
- /// \name Accessors
- /// \{
- inline COMState const & CurrentTrunkState() const
- { return TrunkState_; };
- inline void CurrentTrunkState(const COMState & TrunkState)
- { TrunkState_ = TrunkState; };
- inline double SSLength() const
- { return SSPeriod_; };
- inline void SSLength( double SSPeriod)
- { SSPeriod_ = SSPeriod; };
- inline double SamplingPeriod() const
- { return T_; };
- inline void SamplingPeriod( double SamplingPeriod)
- { T_ = SamplingPeriod; };
- inline double NbSamplingsPreviewed() const
- { return N_; };
- inline void NbSamplingsPreviewed( double SamplingsPreviewed)
- { N_ = SamplingsPreviewed; };
- /// \}
-
- //
- // Private methods:
- //
- private:
-
- /// \brief Verify and eventually reduce the maximal acceleration of the hip joint necessary to attain the velocity reference in one sampling T_.
- /// The verification is based on the supposition that the final joint trajectory is composed by
- /// a fourth-order polynomial acceleration phase inside T_ and a constant velocity phase for the rest of the preview horizon.
- ///
- /// \param[in] Ref
- /// \param[in] CurrentSupport
- void verify_acceleration_hip_joint(const reference_t & Ref,
- const support_state_t & CurrentSupport);
-
- /// \brief Verify velocity of hip joint
- /// The velocity is verified only between previewed supports.
- /// The verification is based on the supposition that the final joint trajectory is a third-order polynomial.
- ///
- /// \param[in] Time
- /// \param[in] PreviewedSupportFoot
- /// \param[in] PreviewedSupportAngle
- /// \param[in] StepNumber
- /// \param[in] CurrentSupport
- /// \param[in] CurrentRightFootAngle
- /// \param[in] CurrentLeftFootAngle
- /// \param[in] CurrentLeftFootVelocity
- /// \param[in] CurrentRightFootVelocity
- void verify_velocity_hip_joint(double Time,
- double PreviewedSupportFoot,
- double PreviewedSupportAngle,
- unsigned StepNumber,
- const support_state_t & CurrentSupport,
- double CurrentRightFootAngle,
- double CurrentLeftFootAngle,
- double CurrentLeftFootVelocity,
- double CurrentRightFootVelocity);
-
- /// \brief Verify angle of hip joint
- /// Reduce final velocity of the trunk if necessary
- ///
- /// \param[in] CurrentSupport
- /// \param[in] PreviewedTrunkAngleEnd
- /// \param[in] TrunkState
- /// \param[in] TrunkStateT
- /// \param[in] CurrentSupportAngle
- /// \param[in] StepNumber
- ///
- /// \return AngleOK
- bool verify_angle_hip_joint(const support_state_t & CurrentSupport,
- double PreviewedTrunkAngleEnd,
- const COMState & TrunkState,
- COMState & TrunkStateT,
- double CurrentSupportFootAngle,
- unsigned StepNumber);
-
- /// \brief Fourth order polynomial trajectory
- /// \param[in] abcd Parameters
- /// \param[in] x
- ///
- /// \return Evaluation value
- double f(double a,double b,double c,double d,double x);
-
- /// \brief Fourth order polynomial trajectory derivative
- /// \param[in] abcd Parameters
- /// \param[in] x
- ///
- /// \return Evaluation value
- double df(double a,double b,double c,double d,double x);
-
-
- //
- // Private members:
- //
- private:
-
- /// \brief Angular limitations of the hip joints
- double lLimitLeftHipYaw_, uLimitLeftHipYaw_, lLimitRightHipYaw_, uLimitRightHipYaw_;
-
- /// \brief Maximal acceleration of a hip joint
- double uaLimitHipYaw_;
-
- /// \brief Upper crossing angle limit between the feet
- double uLimitFeet_;
-
- /// \brief Maximal velocity of a foot
- double uvLimitFoot_;
-
- /// \brief Single-support duration
- double SSPeriod_;
-
- /// \brief Number of sampling in a preview window
- double N_;
-
- /// \brief Time between two samplings
- double T_;
-
- /// \brief Rotation sense of the trunks angular velocity and acceleration
- double signRotVelTrunk_, signRotAccTrunk_;
-
- /// \brief
- double SupportTimePassed_;
-
- /// \brief Numerical precision
- const static double EPS_;
-
- /// \brief Current trunk state
- COMState TrunkState_;
- /// \brief State of the trunk at the first previewed sampling
- COMState TrunkStateT_;
-
-
-
- };
-}
-#endif /* ORIENTATIONSPREVIEW_H_ */
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp b/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp
index 5fc6198147a2aac6c4d9747d00e35656d3c244db..a104b0e2bb3b1529c406deb2d703b7ad6c460309 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Andrei Herdt
* Francois Keith
@@ -23,7 +23,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* This object generate all the values for the foot trajectories,
@@ -54,7 +54,7 @@ using namespace::PatternGeneratorJRL;
OnLineState::OnLineState()
-{
+{
m_CurrentState = IDLE_STATE;
}
@@ -76,7 +76,7 @@ OnLineState & OnLineState::operator=(unsigned int NewState)
ZMPDiscretization::ZMPDiscretization(SimplePluginManager *lSPM,
- string DataFile,
+ string DataFile,
CjrlHumanoidDynamicRobot *aHS)
: ZMPRefTrajectoryGeneration(lSPM)
{
@@ -103,7 +103,7 @@ ZMPDiscretization::ZMPDiscretization(SimplePluginManager *lSPM,
m_ZMPShift.resize(4);
for (unsigned int i=0;i<4;i++)
m_ZMPShift[i]=0.0;
-
+
m_ZMPNeutralPosition[0] = 0.0;
m_ZMPNeutralPosition[1] = 0.0;
@@ -116,7 +116,7 @@ ZMPDiscretization::ZMPDiscretization(SimplePluginManager *lSPM,
m_CurrentSupportFootPosition(i,j)= 1.0;
m_PrevCurrentSupportFootPosition = m_CurrentSupportFootPosition;
-
+
// Prepare size of the matrix used in on-line walking
MAL_MATRIX_RESIZE(m_vdiffsupppre,2,1);
for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(m_vdiffsupppre);i++)
@@ -134,7 +134,7 @@ ZMPDiscretization::~ZMPDiscretization()
{
if (m_FootTrajectoryGenerationStandard!=0)
delete m_FootTrajectoryGenerationStandard;
-
+
if (m_PolynomeZMPTheta!=0)
delete m_PolynomeZMPTheta;
}
@@ -151,7 +151,7 @@ void ZMPDiscretization::GetZMPDiscretization(deque<ZMPPosition> & FinalZMPPositi
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition)
{
-
+
InitOnLine(FinalZMPPositions,
FinalCOMStates,
LeftFootAbsolutePositions,
@@ -181,18 +181,18 @@ void ZMPDiscretization::DumpFootAbsolutePosition(string aFileName,
{
for(unsigned int i=0;i<aFootAbsolutePositions.size();i++)
{
- aof << aFootAbsolutePositions[i].time << " "
- << aFootAbsolutePositions[i].x << " "
- << aFootAbsolutePositions[i].y << " "
- << aFootAbsolutePositions[i].z << " "
- << aFootAbsolutePositions[i].omega << " "
- << aFootAbsolutePositions[i].theta << " "
- << aFootAbsolutePositions[i].stepType << " "
+ aof << aFootAbsolutePositions[i].time << " "
+ << aFootAbsolutePositions[i].x << " "
+ << aFootAbsolutePositions[i].y << " "
+ << aFootAbsolutePositions[i].z << " "
+ << aFootAbsolutePositions[i].omega << " "
+ << aFootAbsolutePositions[i].theta << " "
+ << aFootAbsolutePositions[i].stepType << " "
<< endl;
}
aof.close();
}
-
+
}
void ZMPDiscretization::ResetADataFile(string &DataFile)
@@ -203,10 +203,10 @@ void ZMPDiscretization::ResetADataFile(string &DataFile)
a_iof.open(DataFile.c_str(),std::ifstream::in);
if (a_iof.is_open())
{
-
+
a_iof.close();
}
-
+
}
}
void ZMPDiscretization::DumpDataFiles(string ZMPFileName, string FootFileName,
@@ -219,7 +219,7 @@ void ZMPDiscretization::DumpDataFiles(string ZMPFileName, string FootFileName,
{
for(unsigned int i=0;i<ZMPPositions.size();i++)
{
- aof << ZMPPositions[i].time << " " << ZMPPositions[i].px << " "
+ aof << ZMPPositions[i].time << " " << ZMPPositions[i].px << " "
<< ZMPPositions[i].py << " " << ZMPPositions[i].stepType << " 0.0" << endl;
}
aof.close();
@@ -230,7 +230,7 @@ void ZMPDiscretization::DumpDataFiles(string ZMPFileName, string FootFileName,
{
for(unsigned int i=0;i<SupportFootAbsolutePositions.size();i++)
{
- aof << SupportFootAbsolutePositions[i].x << " " << SupportFootAbsolutePositions[i].y << " "
+ aof << SupportFootAbsolutePositions[i].x << " " << SupportFootAbsolutePositions[i].y << " "
<< SupportFootAbsolutePositions[i].z << " " << SupportFootAbsolutePositions[i].stepType << " 0.0" << endl;
}
aof.close();
@@ -243,7 +243,7 @@ void ZMPDiscretization::InitializeFilter()
double T=0.05; // Arbritrary from Kajita's San Matlab files.
int n=0;
double sum=0,tmp=0;
-
+
assert(m_SamplingPeriod > 0);
n = (int)floor(T/m_SamplingPeriod);
m_ZMPFilterWindow.resize(n+1);
@@ -295,16 +295,16 @@ void ZMPDiscretization::FilterZMPRef(deque<ZMPPosition> &ZMPPositionsX,
ZMPPositionsY[i].theta = ZMPPositionsX[i].theta;
ZMPPositionsY[i].time = ZMPPositionsX[i].time;
ZMPPositionsY[i].stepType = ZMPPositionsX[i].stepType;
-
+
}
-
+
}
void ZMPDiscretization::SetZMPShift(vector<double> &ZMPShift)
{
-
+
for (unsigned int i=0;i<ZMPShift.size();i++)
{
m_ZMPShift[i] = ZMPShift[i];
@@ -316,7 +316,7 @@ void ZMPDiscretization::SetZMPShift(vector<double> &ZMPShift)
/* Start the online part of ZMP discretization. */
/* Initialiazation of the on-line stacks. */
-int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
+int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & FinalCoMStates,
deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
deque<FootAbsolutePosition> &RightFootAbsolutePositions,
@@ -337,24 +337,24 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
for(unsigned int i=0;i<3;i++)
for(unsigned int j=0;j<3;j++)
if (i==j)
- m_CurrentSupportFootPosition(i,j) = 1.0;
+ m_CurrentSupportFootPosition(i,j) = 1.0;
else
- m_CurrentSupportFootPosition(i,j) = 0.0;
+ m_CurrentSupportFootPosition(i,j) = 0.0;
m_PrevCurrentSupportFootPosition = m_CurrentSupportFootPosition;
ODEBUG4("ZMP::InitOnLine - Step 2 ","ZMDInitOnLine.txt");
- // Initialize position of the feet.
+ // Initialize position of the feet.
CurrentLeftFootAbsPos = InitLeftFootAbsolutePosition;
CurrentLeftFootAbsPos.z = 0.0;
CurrentLeftFootAbsPos.time = 0.0;
-
+
ODEBUG4("CurrentLeftFootAbsPos.y: " << CurrentLeftFootAbsPos.y, "ZMDInitOnLine.txt");
CurrentRightFootAbsPos = InitRightFootAbsolutePosition;
CurrentRightFootAbsPos.z = 0.0;
CurrentRightFootAbsPos.time = 0.0;
- // V pre is the difference between
+ // V pre is the difference between
// the current support position and the precedent.
ODEBUG4("ZMP::InitOnLine - Step 2.5 ","ZMDInitOnLine.txt");
@@ -362,8 +362,8 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
// between the direction of the left foot and the direction of the right foot.
double CurrentAbsZMPTheta=0;
CurrentAbsZMPTheta = (CurrentRightFootAbsPos.theta + CurrentLeftFootAbsPos.theta)/2.0;
- ODEBUG("CurrentZMPTheta at start: " << " "
- << CurrentRightFootAbsPos.theta << " "
+ ODEBUG("CurrentZMPTheta at start: " << " "
+ << CurrentRightFootAbsPos.theta << " "
<< CurrentLeftFootAbsPos.theta);
// Initialize who is support foot.
@@ -374,16 +374,16 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
m_AngleDiffToSupportFootTheta = CurrentRightFootAbsPos.theta - CurrentLeftFootAbsPos.theta;
m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentRightFootAbsPos.theta - CurrentAbsZMPTheta;
}
- else
+ else
{
m_vdiffsupppre(0,0) = -CurrentRightFootAbsPos.x + CurrentLeftFootAbsPos.x;
m_vdiffsupppre(1,0) = -CurrentRightFootAbsPos.y + CurrentLeftFootAbsPos.y;
m_AngleDiffToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentRightFootAbsPos.theta;
- m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentAbsZMPTheta;
+ m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentAbsZMPTheta;
}
ODEBUG4("ZMP::InitOnLine - Step 3 ","ZMDInitOnLine.txt");
- // Initialization of the ZMP position (stable values during the Preview control time window).
+ // Initialization of the ZMP position (stable values during the Preview control time window).
int AddArraySize;
{
assert(m_SamplingPeriod > 0);
@@ -406,26 +406,26 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
startingZMPREF[0] = 0.0;
startingZMPREF[1] = 0.0;
startingZMPREF[2] = lStartingZMPPosition(2);
-
+
// Make sure that the robot thinks it is at the position it thinks it is.
//double startingZMPREF[3] = { 0.00949035, 0.00142561, lStartingZMPPosition(2)};
double finalZMPREF[2] = {m_ZMPNeutralPosition[0],m_ZMPNeutralPosition[1]};
- ODEBUG("ZMPNeutralPosition: " << m_ZMPNeutralPosition[0] << " "
+ ODEBUG("ZMPNeutralPosition: " << m_ZMPNeutralPosition[0] << " "
<< m_ZMPNeutralPosition[1] << endl <<
"StartingZMPPosition(toto):" << lStartingZMPPosition(0) << " " << lStartingZMPPosition(1) << " " <<
lStartingZMPPosition(2) << endl
- << "lStartingCOMState: " << lStartingCOMState.x[0] << " "
+ << "lStartingCOMState: " << lStartingCOMState.x[0] << " "
<< lStartingCOMState.y[0] << " "
<< lStartingCOMState.z[0] << endl
<< "CurrentAbsTheta : " << CurrentAbsTheta << endl
<< "AddArraySize:"<< AddArraySize << " " << m_PreviewControlTime << " " <<m_SamplingPeriod << endl
- << "FinalZMPref :( " <<finalZMPREF[0]
+ << "FinalZMPref :( " <<finalZMPREF[0]
<< " , " <<finalZMPREF[1] << " ) " << ZMPPositions.size() <<endl
<< "InitRightFootAbsPos.z " << InitRightFootAbsolutePosition.z);
- ODEBUG( "lStartingCOMState: " << lStartingCOMState.x[0] << " "
+ ODEBUG( "lStartingCOMState: " << lStartingCOMState.x[0] << " "
<< lStartingCOMState.y[0] << " "
<< lStartingCOMState.z[0] );
-
+
ODEBUG4("ZMP::InitOnLine - Step 4 ","ZMDInitOnLine.txt");
for(unsigned int i=0;i<ZMPPositions.size();i++)
{
@@ -437,7 +437,7 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
// Smooth ramp
ZMPPositions[CurrentZMPindex].px = startingZMPREF[0] + (finalZMPREF[0] - startingZMPREF[0])*coef;
ZMPPositions[CurrentZMPindex].py = startingZMPREF[1] + (finalZMPREF[1] - startingZMPREF[1])*coef;
- ZMPPositions[CurrentZMPindex].pz = (-startingZMPREF[2] + InitRightFootAbsolutePosition.z) * icoef;
+ ZMPPositions[CurrentZMPindex].pz = (-startingZMPREF[2] + InitRightFootAbsolutePosition.z) * icoef;
ZMPPositions[CurrentZMPindex].theta = CurrentAbsTheta;
ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
ZMPPositions[CurrentZMPindex].stepType = 0;
@@ -447,38 +447,38 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
FinalCoMStates[CurrentZMPindex].z[1] = 0.0;
FinalCoMStates[CurrentZMPindex].z[2] = 0.0;
- FinalCoMStates[CurrentZMPindex].pitch[0] =
- FinalCoMStates[CurrentZMPindex].pitch[1] =
+ FinalCoMStates[CurrentZMPindex].pitch[0] =
+ FinalCoMStates[CurrentZMPindex].pitch[1] =
FinalCoMStates[CurrentZMPindex].pitch[2] = 0.0;
- FinalCoMStates[CurrentZMPindex].roll[0] =
- FinalCoMStates[CurrentZMPindex].roll[1] =
+ FinalCoMStates[CurrentZMPindex].roll[0] =
+ FinalCoMStates[CurrentZMPindex].roll[1] =
FinalCoMStates[CurrentZMPindex].roll[2] = 0.0;
- FinalCoMStates[CurrentZMPindex].yaw[0] =
+ FinalCoMStates[CurrentZMPindex].yaw[0] =
ZMPPositions[CurrentZMPindex].theta;
- FinalCoMStates[CurrentZMPindex].yaw[1] =
+ FinalCoMStates[CurrentZMPindex].yaw[1] =
FinalCoMStates[CurrentZMPindex].yaw[2] = 0.0;
-
+
// Set Left and Right Foot positions.
- LeftFootAbsolutePositions[CurrentZMPindex] =
+ LeftFootAbsolutePositions[CurrentZMPindex] =
CurrentLeftFootAbsPos;
- RightFootAbsolutePositions[CurrentZMPindex] =
+ RightFootAbsolutePositions[CurrentZMPindex] =
CurrentRightFootAbsPos;
- LeftFootAbsolutePositions[CurrentZMPindex].time =
- RightFootAbsolutePositions[CurrentZMPindex].time =
+ LeftFootAbsolutePositions[CurrentZMPindex].time =
+ RightFootAbsolutePositions[CurrentZMPindex].time =
m_CurrentTime;
- LeftFootAbsolutePositions[CurrentZMPindex].stepType =
+ LeftFootAbsolutePositions[CurrentZMPindex].stepType =
RightFootAbsolutePositions[CurrentZMPindex].stepType = 10;
-
+
m_CurrentTime += m_SamplingPeriod;
CurrentZMPindex++;
}
-
- // The first foot when walking dynamically
+
+ // The first foot when walking dynamically
// does not leave the soil, but needs to be treated for the first phase.
m_RelativeFootPositions.push_back(RelativeFootPositions[0]);
@@ -495,7 +495,7 @@ int ZMPDiscretization::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
}
FilterOutValues(ZMPPositions,
FinalZMPPositions,
- true);
+ true);
ODEBUG6("InitOnLine","DebugDataRFPos.txt" );
for(unsigned int i=1;i<RelativeFootPositions.size();i++)
@@ -527,26 +527,26 @@ void ZMPDiscretization::UpdateCurrentSupportFootPosition(RelativeFootPosition aR
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
Orientation(k,l) = m_CurrentSupportFootPosition(k,l);
-
+
// second position.
MAL_MATRIX_DIM(v,double,2,1);
MAL_MATRIX_DIM(v2,double,2,1);
v(0,0) = aRFP.sx;
v(1,0) = aRFP.sy;
-
+
Orientation = MAL_RET_A_by_B(MM , Orientation);
MAL_C_eq_A_by_B(v2,Orientation, v);
ODEBUG("v :" << v << " "
"v2 : " << v2 << " "
"Orientation : " << Orientation << " "
"CurrentSupportFootPosition: " << m_CurrentSupportFootPosition );
-
-
+
+
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
m_CurrentSupportFootPosition(k,l) = Orientation(k,l);
-
+
for(int k=0;k<2;k++)
m_CurrentSupportFootPosition(k,2) += v2(k,0);
@@ -559,7 +559,7 @@ void ZMPDiscretization::UpdateCurrentSupportFootPosition(RelativeFootPosition aR
void ZMPDiscretization::OnLine(double, // time,
- deque<ZMPPosition> & ,// FinalZMPPositions,
+ deque<ZMPPosition> & ,// FinalZMPPositions,
deque<COMState> & , //FinalCOMStates,
deque<FootAbsolutePosition> &,//FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &)//FinalRightFootAbsolutePositions)
@@ -568,10 +568,10 @@ void ZMPDiscretization::OnLine(double, // time,
}
/* The interface method which returns an appropriate update of the
- appropriate stacks (ZMPRef, FootPosition) depending on the
+ appropriate stacks (ZMPRef, FootPosition) depending on the
state of the relative steps stack. */
void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosition,
- deque<ZMPPosition> & FinalZMPPositions,
+ deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & FinalCOMStates,
deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
@@ -596,8 +596,8 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
double lTdble=m_Tdble, lTsingle=m_Tsingle;
- ODEBUG6(m_RelativeFootPositions[0].sx << " " <<
- m_RelativeFootPositions[0].sy << " " <<
+ ODEBUG6(m_RelativeFootPositions[0].sx << " " <<
+ m_RelativeFootPositions[0].sy << " " <<
m_RelativeFootPositions[0].theta,"DebugDataRFPos.txt" );
ODEBUG(" OnLineAddFoot: m_RelativeFootPositions.size: " << m_RelativeFootPositions.size());
ODEBUG(" OnLineAddFoot: "<< endl <<
@@ -606,12 +606,12 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
" NewRelativeFootPositions.theta: " << NewRelativeFootPosition.theta
);
if (m_RelativeFootPositions[1].DStime!=0.0)
- {
- lTdble =m_RelativeFootPositions[1].DStime;
- lTsingle =m_RelativeFootPositions[1].SStime;
+ {
+ lTdble =m_RelativeFootPositions[1].DStime;
+ lTsingle =m_RelativeFootPositions[1].SStime;
}
// Compute on the direction of the support foot.
- // double stheta = sin(m_RelativeFootPositions[1].theta*M_PI/180.0);
+ // double stheta = sin(m_RelativeFootPositions[1].theta*M_PI/180.0);
ODEBUG("Time of double support phase in OnLineFootAdd: "<< lTdble);
TimeFirstPhase = lTdble;
@@ -624,7 +624,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
m_AngleDiffToSupportFootTheta = CurrentRightFootAbsPos.theta - CurrentLeftFootAbsPos.theta;
m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentRightFootAbsPos.theta - CurrentAbsZMPTheta;
}
- else
+ else
{
WhoIsSupportFoot = 1;// Left
m_vdiffsupppre(0,0) = -CurrentRightFootAbsPos.x + CurrentLeftFootAbsPos.x;
@@ -632,31 +632,32 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
m_AngleDiffToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentRightFootAbsPos.theta;
m_AngleDiffFromZMPThetaToSupportFootTheta = CurrentLeftFootAbsPos.theta - CurrentAbsZMPTheta;
}
-
+
+
double TimeForThisFootPosition = TimeFirstPhase+ lTsingle;
ODEBUG4("TimeFirstPhase: " << TimeFirstPhase << " lTsingle: " << lTsingle,"DebugData.txt");
// Compute the size of cells to add inside the array.
assert(m_SamplingPeriod > 0);
double l2AddArraySize= TimeForThisFootPosition/m_SamplingPeriod;
int AddArraySize = (unsigned int)round(l2AddArraySize);
- ODEBUG("Added part: "<<AddArraySize << " " << l2AddArraySize <<
+ ODEBUG("Added part: "<<AddArraySize << " " << l2AddArraySize <<
" TimeForThisFootPosition " << TimeForThisFootPosition <<
" SamplingPeriod" << m_SamplingPeriod);
ZMPPositions.resize(AddArraySize);
LeftFootAbsolutePositions.resize(AddArraySize);
- RightFootAbsolutePositions.resize(AddArraySize);
-
+ RightFootAbsolutePositions.resize(AddArraySize);
+
m_CurrentAbsTheta+= m_RelativeFootPositions[0].theta;
// m_CurrentAbsTheta = fmod(m_CurrentAbsTheta,180);
// Computes the new ABSOLUTE position of the supporting foot .
UpdateCurrentSupportFootPosition(m_RelativeFootPositions[0]);
-
+
// First Phase of the step cycle.
assert(m_SamplingPeriod > 0);
double dSizeOf1stPhase =TimeFirstPhase/m_SamplingPeriod;
unsigned int SizeOf1stPhase = (unsigned int)round(dSizeOf1stPhase);
- ODEBUG("m_vdiffsupppre : " << m_vdiffsupppre);
+ ODEBUG("m_vdiffsupppre : " << m_vdiffsupppre);
double px0,py0,theta0, delta_x,delta_y;
ODEBUG("FinalZMPPositions.size() = " <<FinalZMPPositions.size());
@@ -664,30 +665,30 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
px0 = FinalZMPPositions.back().px;
py0 = FinalZMPPositions.back().py;
theta0 = FinalZMPPositions.back().theta;
-
+
MAL_VECTOR_DIM(ZMPInFootCoordinates,double,3);
-
+
ZMPInFootCoordinates[0] = m_ZMPNeutralPosition[0];
ZMPInFootCoordinates[1] = m_ZMPNeutralPosition[1];
ZMPInFootCoordinates[2] = 1.0;
-
+
MAL_VECTOR_DIM(ZMPInWorldCoordinates,double,3);
-
- MAL_C_eq_A_by_B(ZMPInWorldCoordinates,m_CurrentSupportFootPosition, ZMPInFootCoordinates);
-
+
+ MAL_C_eq_A_by_B(ZMPInWorldCoordinates,m_CurrentSupportFootPosition, ZMPInFootCoordinates);
+
delta_x = (ZMPInWorldCoordinates(0) - px0)/SizeOf1stPhase;
delta_y = (ZMPInWorldCoordinates(1) - py0)/SizeOf1stPhase;
-
+
ODEBUG("delta_x :"<< delta_x << " delta_y : " << delta_y << " m_CurrentSFP: " <<
- m_CurrentSupportFootPosition << " ZMPInFC : " <<
- ZMPInFootCoordinates << " ZMPinWC : " <<
- ZMPInWorldCoordinates << " px0: " << px0 << " py0:" << py0
+ m_CurrentSupportFootPosition << " ZMPInFC : " <<
+ ZMPInFootCoordinates << " ZMPinWC : " <<
+ ZMPInWorldCoordinates << " px0: " << px0 << " py0:" << py0
);
- ODEBUG4("Step 4 TimeForThisFootPosition " << TimeForThisFootPosition,"DebugData.txt");
-
- // ZMP profile is changed if the stepping over is on, and then
+ ODEBUG4("Step 4 TimeForThisFootPosition " << TimeForThisFootPosition,"DebugData.txt");
+
+ // ZMP profile is changed if the stepping over is on, and then
// depends on the phase during stepping over.
- bool DoIt = 1;
+ bool DoIt = 1;
if (DoIt)
{
if (m_RelativeFootPositions[1].stepType == 3)
@@ -696,7 +697,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
delta_x = (m_CurrentSupportFootPosition(0,2)+m_ZMPShift[0] - px0)/SizeOf1stPhase;
//delta_y = (m_CurrentSupportFootPosition(1,2)+(WhoIsSupportFoot)*m_ZMPShift3BeginY - py0)/SizeOf1stPhase;
delta_y = (m_CurrentSupportFootPosition(1,2) - py0)/SizeOf1stPhase;
-
+
}
if (m_RelativeFootPositions[1].stepType == 4)
{
@@ -705,53 +706,53 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
//delta_x = (CurrentSupportFootPosition(0,2)+m_ZMPShift4Begin - px0)/SizeOf1stPhase;
//delta_y = (CurrentSupportFootPosition(1,2)+(WhoIsSupportFoot)*m_ZMPShift3BeginY - py0)/SizeOf1stPhase;
}
-
+
if (m_RelativeFootPositions[1].stepType == 5)
{
delta_x = (m_CurrentSupportFootPosition(0,2)-(m_ZMPShift[0] +m_ZMPShift[2]+
m_ZMPShift[1] +m_ZMPShift[3]) - px0)/SizeOf1stPhase;
delta_y = (m_CurrentSupportFootPosition(1,2)- py0)/SizeOf1stPhase;
- //delta_x = (CurrentSupportFootPosition(0,2)-(m_ZMPShift3Begin +
+ //delta_x = (CurrentSupportFootPosition(0,2)-(m_ZMPShift3Begin +
// m_ZMPShift4Begin+m_ZMPShift3End + m_ZMPShift4End) - px0)/SizeOf1stPhase;
- //delta_y = (CurrentSupportFootPosition(1,2)-(WhoIsSupportFoot)*(m_ZMPShift3BeginY +
+ //delta_y = (CurrentSupportFootPosition(1,2)-(WhoIsSupportFoot)*(m_ZMPShift3BeginY +
// m_ZMPShift4BeginY+m_ZMPShift3EndY + m_ZMPShift4EndY) - py0)/SizeOf1stPhase;
}
-
- }
-
+
+ }
+
ODEBUG4(" GetZMPDiscretization: Step 5 " << AddArraySize << " " ,"DebugData.txt");
- ODEBUG("SizeOf1stPhase: " << SizeOf1stPhase << "dx: " << delta_x << " dy: " << delta_y);
+ ODEBUG("SizeOf1stPhase: " << SizeOf1stPhase << "dx: " << delta_x << " dy: " << delta_y);
- // First phase of the cycle aka Double support phase.
+ // First phase of the cycle aka Double support phase.
for(unsigned int k=0;k<SizeOf1stPhase;k++)
{
-
+
ZMPPositions[CurrentZMPindex].px = px0 + k*delta_x;
ZMPPositions[CurrentZMPindex].py = py0 + k*delta_y;
ZMPPositions[CurrentZMPindex].pz = 0;
ZMPPositions[CurrentZMPindex].theta =theta0;
-
-
+
+
ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
-
+
ZMPPositions[CurrentZMPindex].stepType = m_RelativeFootPositions[1].stepType+10;
-
+
// Right now the foot is not moving during the double support
// TO DO: whatever you need to do ramzi....
LeftFootAbsolutePositions[CurrentZMPindex] = FinalLeftFootAbsolutePositions.back();
-
+
// WARNING : This assume that you are walking on a plane.
LeftFootAbsolutePositions[CurrentZMPindex].z = 0.0;
-
+
RightFootAbsolutePositions[CurrentZMPindex] =FinalRightFootAbsolutePositions.back();
// WARNING : This assume that you are walking on a plane.
RightFootAbsolutePositions[CurrentZMPindex].z = 0.0;
-
- LeftFootAbsolutePositions[CurrentZMPindex].time =
+
+ LeftFootAbsolutePositions[CurrentZMPindex].time =
RightFootAbsolutePositions[CurrentZMPindex].time = m_CurrentTime;
- LeftFootAbsolutePositions[CurrentZMPindex].stepType =
+ LeftFootAbsolutePositions[CurrentZMPindex].stepType =
RightFootAbsolutePositions[CurrentZMPindex].stepType = m_RelativeFootPositions[1].stepType+10;
/*
ofstream aoflocal;
@@ -763,9 +764,9 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
CurrentZMPindex++;
}
//-- End Of First phase.
-
+
// Second Phase of the step cycle aka Single Support Phase.
-
+
// Compute relative feet position for the next step.
double lStepHeight=0;
@@ -794,15 +795,15 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
MAL_MATRIX_DIM(v,double,2,1);
Orientation(0,0) = c; Orientation(0,1) = -s;
Orientation(1,0) = s; Orientation(1,1) = c;
-
+
MAL_MATRIX_DIM(SubOrientation,double,2,2);
MAL_MATRIX_C_eq_EXTRACT_A(SubOrientation,m_CurrentSupportFootPosition,double,0,0,2,2);
- Orientation = MAL_RET_A_by_B(Orientation,SubOrientation) ;
-
+ Orientation = MAL_RET_A_by_B(Orientation,SubOrientation) ;
+
v(0,0) = m_RelativeFootPositions[1].sx;
v(1,0) = m_RelativeFootPositions[1].sy;
MAL_C_eq_A_by_B(vdiffsupp,Orientation,v);
-
+
vrel = vdiffsupp + m_vdiffsupppre;
// Compute relative feet orientation for the next step
@@ -813,7 +814,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
vrel(1,0)= 0.0;
RelTheta= 0.0;
NextTheta=0.0;
- lStepHeight = 0.0;
+ lStepHeight = 1.0;
}
ODEBUG(cout << "vrel: " << vrel(0,0) << " " << vrel(1,0));
@@ -824,10 +825,10 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
m_vdiffsupppre = vdiffsupp;
-
+
// m_AngleDiffToSupportFootTheta = NextTheta;
-
-
+
+
// Create the polynomes for the none-support foot.
// Change 08/12/2005: Speed up the modification of X and Y
// for vertical landing of the foot (Kajita-San's trick n 1)
@@ -835,13 +836,15 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
double ModulatedSingleSupportTime = lTsingle * m_ModulationSupportCoefficient;
double EndOfLiftOff = (lTsingle-ModulatedSingleSupportTime)*0.5;
- ODEBUG("ModulatedSingleSupportTime:" << ModulatedSingleSupportTime << " "
- << vrel(0,0) << " "
+ ODEBUG("ModulatedSingleSupportTime:" << ModulatedSingleSupportTime << " "
+ << vrel(0,0) << " "
<< vrel(1,0));
m_FootTrajectoryGenerationStandard->SetParameters(FootTrajectoryGenerationStandard::X_AXIS,
ModulatedSingleSupportTime,vrel(0,0));
m_FootTrajectoryGenerationStandard->SetParameters(FootTrajectoryGenerationStandard::Y_AXIS,
ModulatedSingleSupportTime,vrel(1,0));
+
+
m_FootTrajectoryGenerationStandard->SetParameters(FootTrajectoryGenerationStandard::Z_AXIS,
m_Tsingle,lStepHeight);
m_FootTrajectoryGenerationStandard->SetParameters(FootTrajectoryGenerationStandard::THETA_AXIS,
@@ -850,7 +853,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
EndOfLiftOff,m_Omega);
m_FootTrajectoryGenerationStandard->SetParameters(FootTrajectoryGenerationStandard::OMEGA2_AXIS,
ModulatedSingleSupportTime,2*m_Omega);
-
+
// m_FootTrajectoryGenerationStandard->print();
//m_PolynomeZMPTheta->SetParameters(lTsingle,NextZMPTheta);
@@ -861,13 +864,13 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
int indexinitial = CurrentZMPindex-1;
int SignRHAND=1, SignLHAND=1;
- /*//polynomial planning for the stepover
-
+ /*//polynomial planning for the stepover
+
if (m_RelativeFootPositions[0].stepType==3)
{
StepOverPolyPlanner(m_RelativeFootPositions[0].stepType);
};
- */
+ */
double px02,py02;
px02 = ZMPPositions[CurrentZMPindex-1].px;
py02 = ZMPPositions[CurrentZMPindex-1].py;
@@ -881,28 +884,28 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
ZMPInFootCoordinates[0] = m_ZMPNeutralPosition[0];
ZMPInFootCoordinates[1] = m_ZMPNeutralPosition[1];
ZMPInFootCoordinates[2] = 1.0;
-
+
MAL_VECTOR_DIM(ZMPInWorldCoordinates,double,3);
- MAL_C_eq_A_by_B(ZMPInWorldCoordinates,m_CurrentSupportFootPosition,
- ZMPInFootCoordinates);
+ MAL_C_eq_A_by_B(ZMPInWorldCoordinates,m_CurrentSupportFootPosition,
+ ZMPInFootCoordinates);
ODEBUG4("CSFP: " << m_CurrentSupportFootPosition << endl <<
- "ZMPiWC" << ZMPInWorldCoordinates << endl, "DebugData.txt");
-
+ "ZMPiWC" << ZMPInWorldCoordinates << endl, "DebugData.txt");
+
ZMPPositions[CurrentZMPindex].px = ZMPInWorldCoordinates(0);
ZMPPositions[CurrentZMPindex].py = ZMPInWorldCoordinates(1);
ZMPPositions[CurrentZMPindex].pz = 0;
ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
-
+
if (DoIt)
{
if ((m_RelativeFootPositions[1].stepType == 3)||(m_RelativeFootPositions[1].stepType == 4))
{
-
-
+
+
if (m_RelativeFootPositions[1].stepType == 3)
- {
+ {
delta_x = (m_CurrentSupportFootPosition(0,2)+m_ZMPShift[1] - px02)/SizeOfSndPhase;
delta_y = (m_CurrentSupportFootPosition(1,2) - py02)/SizeOfSndPhase;
//delta_x = (CurrentSupportFootPosition(0,2)+
@@ -919,7 +922,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
//delta_y = (CurrentSupportFootPosition(1,2)+
// (WhoIsSupportFoot)*m_ZMPShift4EndY - py02)/SizeOfSndPhase;
}
-
+
ZMPPositions[CurrentZMPindex].px = ZMPPositions[CurrentZMPindex-1].px + delta_x;
ZMPPositions[CurrentZMPindex].py = ZMPPositions[CurrentZMPindex-1].py + delta_y;
ZMPPositions[CurrentZMPindex].pz = 0;
@@ -927,7 +930,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
}
- ZMPPositions[CurrentZMPindex].theta = m_PolynomeZMPTheta->Compute(k*m_SamplingPeriod) +
+ ZMPPositions[CurrentZMPindex].theta = m_PolynomeZMPTheta->Compute(k*m_SamplingPeriod) +
ZMPPositions[indexinitial].theta;
ZMPPositions[CurrentZMPindex].stepType = WhoIsSupportFoot*m_RelativeFootPositions[0].stepType;
@@ -952,21 +955,21 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
1);
SignLHAND=-1;
SignRHAND=1;
-
+
}
-
- LeftFootAbsolutePositions[CurrentZMPindex].time =
+
+ LeftFootAbsolutePositions[CurrentZMPindex].time =
RightFootAbsolutePositions[CurrentZMPindex].time = m_CurrentTime;
-
+
m_CurrentTime += m_SamplingPeriod;
CurrentZMPindex++;
}
if (WhoIsSupportFoot==1)
WhoIsSupportFoot = -1;//Right
- else
+ else
WhoIsSupportFoot = 1;// Left
m_RelativeFootPositions.pop_front();
@@ -974,42 +977,42 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
for(unsigned int i=0;i<ZMPPositions.size();i++)
{
COMState aCOMState;
- aCOMState.x[0] =
- aCOMState.x[1] =
+ aCOMState.x[0] =
+ aCOMState.x[1] =
aCOMState.x[2] = 0.0;
- aCOMState.y[0] =
- aCOMState.y[1] =
+ aCOMState.y[0] =
+ aCOMState.y[1] =
aCOMState.y[2] = 0.0;
aCOMState.z[0] = m_ComHeight;
-
- aCOMState.pitch[0] =
+
+ aCOMState.pitch[0] =
aCOMState.pitch[1] =
aCOMState.pitch[2] =
aCOMState.roll[0] =
- aCOMState.roll[1] =
- aCOMState.roll[2] =
+ aCOMState.roll[1] =
+ aCOMState.roll[2] =
aCOMState.yaw[1] =
aCOMState.yaw[2] = 0.0;
-
+
aCOMState.z[1] = aCOMState.z[2] = 0.0;
aCOMState.yaw[0] = ZMPPositions[i].theta;
-
+
FinalCOMStates.push_back(aCOMState);
FinalLeftFootAbsolutePositions.push_back(LeftFootAbsolutePositions[i]);
FinalRightFootAbsolutePositions.push_back(RightFootAbsolutePositions[i]);
-
+
}
FilterOutValues(ZMPPositions,
- FinalZMPPositions,false);
+ FinalZMPPositions,false);
ODEBUG_CODE(DumpReferences(FinalZMPPositions,ZMPPositions));
-
+
if (EndSequence)
{
-
+
// End Phase of the walking includes the filtering.
EndPhaseOfTheWalking(FinalZMPPositions,
FinalCOMStates,
@@ -1022,7 +1025,7 @@ void ZMPDiscretization::OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosi
void ZMPDiscretization::DumpReferences(deque<ZMPPosition> & FinalZMPPositions,
deque<ZMPPosition> & ZMPPositions)
{
-
+
ofstream dbg_aof("DebugZMPRefPos.dat",ofstream::app);
for(unsigned int i=0;i<ZMPPositions.size();i++)
@@ -1031,7 +1034,7 @@ void ZMPDiscretization::DumpReferences(deque<ZMPPosition> & FinalZMPPositions,
<< ZMPPositions[i].py << endl;
}
dbg_aof.close();
-
+
dbg_aof.open("DebugFinalZMPRefPos.dat",ofstream::app);
for(unsigned int i=0;i<FinalZMPPositions.size();i++)
{
@@ -1039,7 +1042,7 @@ void ZMPDiscretization::DumpReferences(deque<ZMPPosition> & FinalZMPPositions,
<< FinalZMPPositions[i].py << endl;
}
dbg_aof.close();
-
+
}
void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
@@ -1059,7 +1062,7 @@ void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
r=i-j+lshift;
if (r<0)
{
-
+
if (InitStep)
{
ltmp[0] += m_ZMPFilterWindow[j]*ZMPPositions[lshift].px;
@@ -1092,7 +1095,7 @@ void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
ltmp[2] += m_ZMPFilterWindow[j]*ZMPPositions[r].pz;
}
}
-
+
ZMPPosition aZMPPos;
aZMPPos.px = ltmp[0];
aZMPPos.py = ltmp[1];
@@ -1100,7 +1103,7 @@ void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
aZMPPos.theta = ZMPPositions[i].theta;
aZMPPos.time = ZMPPositions[i].time;
aZMPPos.stepType = ZMPPositions[i].stepType;
-
+
FinalZMPPositions.push_back(aZMPPos);
}
ODEBUG("ZMPPosition.back=( " <<ZMPPositions.back().px << " , " << ZMPPositions.back().py << " )");
@@ -1110,7 +1113,7 @@ void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
int ZMPDiscretization::OnLineFootChange(double ,//time,
FootAbsolutePosition & ,//aFootAbsolutePosition,
- deque<ZMPPosition> & ,//FinalZMPPositions,
+ deque<ZMPPosition> & ,//FinalZMPPositions,
deque<COMState> & ,//CoMStates,
deque<FootAbsolutePosition> & ,//FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> & ,//FinalRightFootAbsolutePositions,
@@ -1146,7 +1149,7 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
assert(m_SamplingPeriod > 0);
double dlAddArraySize = m_Tdble/(2*m_SamplingPeriod);
unsigned int AddArraySize = (unsigned int)round(dlAddArraySize);
-
+
unsigned int currentsize = 0;
unsigned int CurrentZMPindex = 0;
ZMPPositions.resize(currentsize+AddArraySize);
@@ -1163,36 +1166,36 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
// We assume that the last positon of the ZMP
// will the middle of the two last position
// of the support foot.
- ODEBUG("Cur/Prev SuppFootPos (X) : " <<m_CurrentSupportFootPosition(0,2)
+ ODEBUG("Cur/Prev SuppFootPos (X) : " <<m_CurrentSupportFootPosition(0,2)
<< " " << m_PrevCurrentSupportFootPosition(0,2));
- ODEBUG("Cur/Prev SuppFootPos (Y) : " <<m_CurrentSupportFootPosition(1,2)
+ ODEBUG("Cur/Prev SuppFootPos (Y) : " <<m_CurrentSupportFootPosition(1,2)
<< " " << m_PrevCurrentSupportFootPosition(1,2));
pxf = 0.5*(m_CurrentSupportFootPosition(0,2) + m_PrevCurrentSupportFootPosition(0,2));
pyf = 0.5*(m_CurrentSupportFootPosition(1,2) + m_PrevCurrentSupportFootPosition(1,2));
-
+
delta_x = (pxf - px0)/(double)SizeOfEndPhase;
delta_y = (pyf - py0)/(double)SizeOfEndPhase;
-
+
ZMPPositions[0].px = px0 + delta_x;
ZMPPositions[0].py = py0 + delta_y;
ZMPPositions[0].time = m_CurrentTime;
-
+
ZMPPositions[0].theta = FinalZMPPositions.back().theta;
ZMPPositions[0].stepType=0;
CurrentZMPindex++;
- LeftFootAbsolutePosition =
+ LeftFootAbsolutePosition =
FinalLeftFootAbsolutePositions.back();
- RightFootAbsolutePosition =
+ RightFootAbsolutePosition =
FinalRightFootAbsolutePositions.back();
-
- LeftFootAbsolutePosition.time =
+
+ LeftFootAbsolutePosition.time =
RightFootAbsolutePosition.time = m_CurrentTime;
-
- LeftFootAbsolutePosition.stepType =
+
+ LeftFootAbsolutePosition.stepType =
RightFootAbsolutePosition.stepType = 0;
-
+
FinalLeftFootAbsolutePositions.push_back(LeftFootAbsolutePosition);
FinalRightFootAbsolutePositions.push_back(RightFootAbsolutePosition);
@@ -1202,15 +1205,15 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
{
// Set ZMP positions.
- ZMPPositions[CurrentZMPindex].px =
+ ZMPPositions[CurrentZMPindex].px =
ZMPPositions[CurrentZMPindex-1].px + delta_x;
- ZMPPositions[CurrentZMPindex].py =
+ ZMPPositions[CurrentZMPindex].py =
ZMPPositions[CurrentZMPindex-1].py + delta_y;
ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
- ZMPPositions[CurrentZMPindex].theta =
+ ZMPPositions[CurrentZMPindex].theta =
ZMPPositions[CurrentZMPindex-1].theta;
- ZMPPositions[CurrentZMPindex].stepType =0;
+ ZMPPositions[CurrentZMPindex].stepType =0;
// Set CoM Positions.
COMState aCOMState;
@@ -1222,15 +1225,15 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
FinalCOMStates.push_back(aCOMState);
// Set Feet positions.
- LeftFootAbsolutePosition =
+ LeftFootAbsolutePosition =
FinalLeftFootAbsolutePositions.back();
- RightFootAbsolutePosition =
+ RightFootAbsolutePosition =
FinalRightFootAbsolutePositions.back();
-
- LeftFootAbsolutePosition.time =
+
+ LeftFootAbsolutePosition.time =
RightFootAbsolutePosition.time = m_CurrentTime;
- LeftFootAbsolutePosition.stepType =
+ LeftFootAbsolutePosition.stepType =
RightFootAbsolutePosition.stepType = 0;
FinalLeftFootAbsolutePositions.push_back(LeftFootAbsolutePosition);
@@ -1248,14 +1251,14 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
double ldAddArraySize = 3.0*m_PreviewControlTime/m_SamplingPeriod;
AddArraySize = (int)ldAddArraySize;
}
-
+
currentsize = ZMPPositions.size();
ZMPPositions.resize(currentsize+AddArraySize);
ODEBUG4(" GetZMPDiscretization: Step 8 ","DebugData.txt");
for(unsigned int i=0;i<AddArraySize;i++)
- {
+ {
// Set ZMP Positions
ZMPPositions[CurrentZMPindex].px = ZMPPositions[CurrentZMPindex-1].px;
@@ -1275,18 +1278,18 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
FinalCOMStates.push_back(aCOMState);
// Set Feet Positions
- LeftFootAbsolutePosition=
+ LeftFootAbsolutePosition=
FinalLeftFootAbsolutePositions.back();
- RightFootAbsolutePosition =
+ RightFootAbsolutePosition =
FinalRightFootAbsolutePositions.back();
-
- LeftFootAbsolutePosition.time =
+
+ LeftFootAbsolutePosition.time =
RightFootAbsolutePosition.time = m_CurrentTime;
-
- LeftFootAbsolutePosition.stepType =
+
+ LeftFootAbsolutePosition.stepType =
RightFootAbsolutePosition.stepType = 0;
-
+
FinalLeftFootAbsolutePositions.push_back(LeftFootAbsolutePosition);
FinalRightFootAbsolutePositions.push_back(RightFootAbsolutePosition);
@@ -1301,11 +1304,11 @@ void ZMPDiscretization::EndPhaseOfTheWalking( deque<ZMPPosition> &FinalZMPPosit
void ZMPDiscretization::RegisterMethodsForScripting()
{
- std::string aMethodName[3] =
+ std::string aMethodName[3] =
{":prevzmpinitprofil",
":zeroinitprofil",
":previewcontroltime"};
-
+
for(int i=0;i<3;i++)
{
if (!RegisterMethod(aMethodName[i]))
@@ -1341,5 +1344,5 @@ void ZMPDiscretization::CallMethod(std::string &Method, std::istringstream &strm
}
ZMPRefTrajectoryGeneration::CallMethod(Method,strm);
-
+
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
deleted file mode 100644
index 7e08b57e1e59aa9072252fc55163561a521a9a04..0000000000000000000000000000000000000000
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ /dev/null
@@ -1,390 +0,0 @@
-/*
- * Copyright 2010,
- *
- * Medhi Benallegue
- * Andrei Herdt
- * Francois Keith
- * Olivier Stasse
- *
- * JRL, CNRS/AIST
- *
- * This file is part of walkGenJrl.
- * walkGenJrl is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * walkGenJrl is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Lesser Public License for more details.
- * You should have received a copy of the GNU Lesser General Public License
- * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
- *
- * Research carried out within the scope of the
- * Joint Japanese-French Robotics Laboratory (JRL)
- */
-/*! Generate ZMP and CoM trajectories using Herdt2010IROS
- */
-
-#ifndef _ZMPVELOCITYREFERENCEDQP_WITH_CONSTRAINT_H_
-#define _ZMPVELOCITYREFERENCEDQP_WITH_CONSTRAINT_H_
-
-#include <PreviewControl/PreviewControl.hh>
-#include <PreviewControl/LinearizedInvertedPendulum2D.hh>
-#include <PreviewControl/rigid-body-system.hh>
-#include <Mathematics/relative-feet-inequalities.hh>
-#include <ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.hh>
-#include <PreviewControl/SupportFSM.hh>
-#include <ZMPRefTrajectoryGeneration/OrientationsPreview.hh>
-#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
-#include <privatepgtypes.hh>
-#include <ZMPRefTrajectoryGeneration/generator-vel-ref.hh>
-#include <Mathematics/intermediate-qp-matrices.hh>
-#include <jrl/walkgen/pgtypes.hh>
-#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
-
-// metapod includes
-#include <metapod/models/hrp2_14/hrp2_14.hh>
-#ifndef METAPOD_TYPEDEF
-#define METAPOD_TYPEDEF
- typedef double LocalFloatType;
- typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
- typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
- typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type Node;
-#endif
-
-namespace PatternGeneratorJRL
-{
-
-
- class ZMPDiscretization;
- class ZMPVelocityReferencedQP : public ZMPRefTrajectoryGeneration
- {
-
- //
- // Public methods:
- //
- public:
-
- ZMPVelocityReferencedQP(SimplePluginManager *SPM, string DataFile,
- CjrlHumanoidDynamicRobot *aHS=0 );
-
- ~ZMPVelocityReferencedQP();
-
-
- /// \brief Handle plugins (SimplePlugin interface)
- void CallMethod(std::string &Method, std::istringstream &strm);
-
- /*! \name Call method to handle on-line generation of ZMP reference trajectory.
- @{*/
-
- /*! Methods for on-line generation. (First version!)
- The queues will be updated as follows:
- - The first values necessary to start walking will be inserted.
- - The initial positions of the feet will be taken into account
- according to InitLeftFootAbsolutePosition and InitRightFootAbsolutePosition.
- - The RelativeFootPositions stack will NOT be taken into account,
- - The starting COM Position will NOT be taken into account.
- Returns the number of steps which has been completely put inside
- the queue of ZMP, and foot positions.
- */
- int InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & FinalCoMPositions_deq,
- deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
- FootAbsolutePosition & InitLeftFootAbsolutePosition,
- FootAbsolutePosition & InitRightFootAbsolutePosition,
- deque<RelativeFootPosition> &RelativeFootPositions,
- COMState & lStartingCOMState,
- MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition);
-
-
- /// \brief Update the stacks on-line
- void OnLine(double time,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & FinalCOMTraj_deq,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
-
-
- /// \name Accessors and mutators
- /// \{
- /// \brief Set the reference (velocity only as for now) through the Interface (slow)
- void Reference(istringstream &strm)
- {
- strm >> NewVelRef_.Local.X;
- strm >> NewVelRef_.Local.Y;
- strm >> NewVelRef_.Local.Yaw;
- }
- inline void Reference(double dx, double dy, double dyaw)
- {
- NewVelRef_.Local.X = dx;
- NewVelRef_.Local.Y = dy;
- NewVelRef_.Local.Yaw = dyaw;
- }
-
- inline bool Running()
- { return Running_; }
-
- /// \brief Set the final-stage trigger
- inline void EndingPhase(bool EndingPhase)
- { EndingPhase_ = EndingPhase;}
-
- void setCoMPerturbationForce(double x,double y);
- void setCoMPerturbationForce(istringstream &strm);
-
- solution_t & Solution()
- { return Solution_; }
-
- inline const int & QP_N(void) const
- { return QP_N_; }
-
- /// \brief Setter and getter for the ComAndZMPTrajectoryGeneration.
- inline bool setComAndFootRealization(ComAndFootRealization * aCFR)
- { ComAndFootRealization_ = aCFR; return true;};
- inline ComAndFootRealization * getComAndFootRealization()
- { return ComAndFootRealization_;};
- /// \}
-
- inline double InterpolationPeriod()
- { return InterpolationPeriod_ ; }
- inline void InterpolationPeriod( double T )
- { InterpolationPeriod_ = T ; }
-
-
- //
- // Private members:
- //
- private:
-
- /// \brief (Updated) Reference
- reference_t VelRef_;
- /// \brief Temporary (updating) reference
- reference_t NewVelRef_;
-
- /// \brief Total mass of the robot
- double RobotMass_;
-
- /// \brief Perturbation trigger
- bool PerturbationOccured_;
-
- /// \brief Final stage trigger
- bool EndingPhase_;
-
- /// \brief PG running
- bool Running_;
-
- /// \brief Time at which the online mode will stop
- double TimeToStopOnLineMode_;
-
- /// \brief Time at which the problem should be updated
- double UpperTimeLimitToUpdate_;
-
- /// \brief Security margin for trajectory queues
- double TimeBuffer_;
-
- /// \brief Additional term on the acceleration of the CoM
- MAL_VECTOR(PerturbationAcceleration_,double);
-
- /// \brief Sampling period considered in the QP
- double QP_T_;
-
- /// \brief Nb. samplings inside preview window
- int QP_N_;
-
- /// \brief 2D LIPM to simulate the evolution of the robot's CoM.
- LinearizedInvertedPendulum2D LIPM_ ;
- LinearizedInvertedPendulum2D LIPM_subsampled_ ;
-
- /// \brief Simplified robot model
- RigidBodySystem * Robot_ ;
-
- /// \brief Finite State Machine to simulate the evolution of the support states.
- SupportFSM * SupportFSM_;
-
- /// \brief Decoupled optimization problem to compute the evolution of feet angles.
- OrientationsPreview * OrientPrw_;
- OrientationsPreview * OrientPrw_DF_;
-
- /// \brief Generator of QP problem
- GeneratorVelRef * VRQPGenerator_;
-
- /// \brief Intermediate QP data
- IntermedQPMat * IntermedData_;
-
- /// \brief Object creating linear inequalities relative to feet centers.
- RelativeFeetInequalities * RFI_;
-
- /// \brief Final optimization problem
- QPProblem Problem_;
-
- /// \brief Previewed Solution
- solution_t Solution_;
-
- /// \brief Copy of the QP_ solution
- solution_t solution_ ;
-
- /// \brief Store a reference to the object to solve posture resolution.
- ComAndFootRealization * ComAndFootRealization_;
-
- /// \brief HDR allow the computation of the dynamic filter
- CjrlHumanoidDynamicRobot * HDR_ ;
-
- /// \brief Pointer to the Preview Control object.
- PreviewControl *PC_;
-
- /// \brief State of the Preview control.
- MAL_MATRIX( m_deltax,double);
- MAL_MATRIX( m_deltay,double);
-
- /// \brief Buffers for the Kajita's dynamic filter
- deque<ZMPPosition> ZMPTraj_deq_ ;
- deque<COMState> COMTraj_deq_ ;
- deque<FootAbsolutePosition> LeftFootTraj_deq_ ;
- deque<FootAbsolutePosition> RightFootTraj_deq_ ;
-
- deque<COMState> tmpCoM_ ;
- deque<ZMPPosition> tmpZMP_ ;
- deque<FootAbsolutePosition> tmpRF_ ;
- deque<FootAbsolutePosition> tmpLF_ ;
-
- /// \brief Index where to begin the interpolation
- unsigned CurrentIndex_ ;
-
- /// \brief Interpolation Period for the dynamic filter
- double InterpolationPeriod_ ;
-
- /// \brief Step Period of the robot
- double StepPeriod_ ;
-
- /// \brief Period where the robot is on ONE feet
- double SSPeriod ;
-
- /// \brief Period where the robot is on TWO feet
- double DSPeriod ;
-
- /// \brief Maximum distance between the feet
- double FeetDistance ;
-
- /// \brief Maximum height of the feet
- double StepHeight ;
-
- /// \brief Height of the CoM
- double CoMHeight_ ;
-
- /// \brief Number of interpolated point needed for control computed during QP_T_
- unsigned NbSampleControl_ ;
-
- /// \brief Number of interpolated point needed for the dynamic filter computed during QP_T_
- unsigned NbSampleInterpolation_ ;
-
- /// \brief Buffers for the CoM an Feet computation, i.e. the simplify inverse kinematics.
- vector <MAL_VECTOR_TYPE(double)> ConfigurationTraj_ ;
- vector <MAL_VECTOR_TYPE(double)> VelocityTraj_ ;
- vector <MAL_VECTOR_TYPE(double)> AccelerationTraj_ ;
- MAL_VECTOR_TYPE(double) PreviousConfiguration_ ;
- MAL_VECTOR_TYPE(double) PreviousVelocity_ ;
- MAL_VECTOR_TYPE(double) PreviousAcceleration_ ;
-
- /// \brief Buffers for the uotput of the Kajita preview control algorithm.
- std::deque<COMState> ComStateBuffer_ ;
-
- /// \brief force acting the CoM of the robot expressed in the Euclidean Frame
- Force_HRP2_14 m_force ;
-
- /// \brief Used to compute once, the initial difference between the ZMP and the ZMPMB
- bool Once_ ;
- double DInitX_, DInitY_ ;
- COMState InitStateLIPM_ ;
- COMState InitStateOrientPrw_ ;
- COMState FinalCurrentStateOrientPrw_ ;
- COMState FinalPreviewStateOrientPrw_ ;
-
- /// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler
- ///and the ZMP Multibody computed from the articular trajectory
- std::deque<ZMPPosition> DeltaZMPMBPositions_ ;
-
- /// \brief Set configuration vectors (q, dq, ddq, torques) to reference values.
- Robot_Model::confVector m_torques, m_q, m_dq, m_ddq;
-
- /// \brief Initialize the robot with the autogenerated files by MetapodFromUrdf
- Robot_Model m_robot;
-
- /// \brief Standard polynomial trajectories for the feet.
- OnLineFootTrajectoryGeneration * OFTG_DF_ ;
- OnLineFootTrajectoryGeneration * OFTG_control_ ;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW // to use the vector of eigen used by metapod
-
- void GetZMPDiscretization(std::deque<ZMPPosition> & ZMPPositions,
- std::deque<COMState> & COMStates,
- std::deque<RelativeFootPosition> &RelativeFootPositions,
- std::deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
- std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
- double Xmax,
- COMState & lStartingCOMState,
- MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
- FootAbsolutePosition & InitLeftFootAbsolutePosition,
- FootAbsolutePosition & InitRightFootAbsolutePosition);
-
- void OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosition,
- std::deque<ZMPPosition> & FinalZMPPositions,
- std::deque<COMState> & COMStates,
- std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
- bool EndSequence);
-
- int OnLineFootChange(double time,
- FootAbsolutePosition & aFootAbsolutePosition,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMPositions,
- deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
- StepStackHandler * aStepStackHandler);
-
- void EndPhaseOfTheWalking(deque<ZMPPosition> & ZMPPositions,
- deque<COMState> & FinalCOMTraj_deq,
- deque<FootAbsolutePosition> & LeftFootAbsolutePositions,
- deque<FootAbsolutePosition> & RightFootAbsolutePositions);
-
- int ReturnOptimalTimeToRegenerateAStep();
-
- int DynamicFilter(double time,
- std::deque<COMState> & FinalCOMTraj_deq
- );
-
- void CallToComAndFootRealization(
- const COMState & acomp,
- const FootAbsolutePosition & aLeftFAP,
- const FootAbsolutePosition & aRightFAP,
- MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- const unsigned IterationNumber
- );
-
- void CoMZMPInterpolation(
- std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
- std::deque<COMState> & COMTraj_deq , // OUTPUT
- const std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
- const std::deque<FootAbsolutePosition> & RightFootTraj_deq,// INPUT
- const solution_t * Solution, // INPUT
- LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
- const unsigned numberOfSample, // INPUT
- const int IterationNumber); // INPUT
-
- void ControlInterpolation(
- std::deque<COMState> & FinalCOMTraj_deq, // OUTPUT
- std::deque<ZMPPosition> & FinalZMPTraj_deq, // OUTPUT
- std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq, // OUTPUT
- std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq, // OUTPUT
- double time); // INPUT
-
- void DynamicFilterInterpolation(double time);
- };
-}
-
-#include <ZMPRefTrajectoryGeneration/ZMPDiscretization.hh>
-#endif /* _ZMPQP_WITH_CONSTRAINT_H_ */
diff --git a/src/patterngeneratorinterfaceprivate.hh b/src/patterngeneratorinterfaceprivate.hh
index c64a7d20e2be55f4820cc62335776bf266ddf6bd..72594a4cfaa9869eae3901ac36ed42cc7adeda20 100644
--- a/src/patterngeneratorinterfaceprivate.hh
+++ b/src/patterngeneratorinterfaceprivate.hh
@@ -426,6 +426,8 @@ namespace PatternGeneratorJRL
/*! \brief Realize a sequence of steps. */
virtual void m_StepSequence(std::istringstream &strm);
+ virtual void m_StepStairSequence(std::istringstream &strm);
+
private:
@@ -603,7 +605,7 @@ namespace PatternGeneratorJRL
bool m_NewStep;
/*! \brief X, Y, Theta coordinates of the new step.*/
- double m_NewStepX, m_NewStepY, m_NewTheta;
+ double m_NewStepX, m_NewStepY, m_NewStepZ, m_NewTheta;
/*! @} */
/*! \brief Add automatically the first new step */
diff --git a/src/PGTypes.cpp b/src/pgtypes.cpp
similarity index 90%
rename from src/PGTypes.cpp
rename to src/pgtypes.cpp
index 2c8b89b3a305a2d3bbc36fd1c583a86b41a787d9..8620e9c962e0b63b84cc3ad715fa951da234fd17 100644
--- a/src/PGTypes.cpp
+++ b/src/pgtypes.cpp
@@ -29,9 +29,9 @@ namespace PatternGeneratorJRL
{
for(unsigned int i=0;i<3;i++)
{
- x[i] = aCS.x[i];
- y[i] = aCS.y[i];
- z[i] = aCS.z[i];
+ x[i] = aCS.x[i];
+ y[i] = aCS.y[i];
+ z[i] = aCS.z[i];
};
yaw = aCS.yaw[0];
pitch = aCS.pitch[0];
@@ -43,9 +43,9 @@ namespace PatternGeneratorJRL
{
for(unsigned int i=0;i<3;i++)
{
- x[i] = aCS.x[i];
- y[i] = aCS.y[i];
- z[i] = aCS.z[i];
+ x[i] = aCS.x[i];
+ y[i] = aCS.y[i];
+ z[i] = aCS.z[i];
};
yaw[0] = aCS.yaw; yaw[1] = yaw[2] = 0.0;
pitch[0] = aCS.pitch; pitch[1] = pitch[2] = 0.0;
@@ -57,8 +57,8 @@ namespace PatternGeneratorJRL
{
for(unsigned int i=0;i<3;i++)
{
- x[i] = 0.0; y[i] = 0.0;
- yaw[i] = 0.0; pitch[i] = 0.0; roll[i] = 0.0;
+ x[i] = 0.0; y[i] = 0.0;
+ yaw[i] = 0.0; pitch[i] = 0.0; roll[i] = 0.0;
}
}
diff --git a/src/privatepgtypes.cpp b/src/privatepgtypes.cpp
index 70767fc054d5e748c9e29b9d74b8ee85e4d2a9b8..1fab455cbe591b89e6e62915da1e722b636343ad 100644
--- a/src/privatepgtypes.cpp
+++ b/src/privatepgtypes.cpp
@@ -377,4 +377,5 @@ namespace PatternGeneratorJRL
reference_t::frame_t::frame_t(const frame_t & F):
X(F.X), Y(F.X), Yaw(F.Yaw), X_vec(F.X_vec), Y_vec(F.Y_vec), Yaw_vec(F.Yaw_vec)
{}
+
}
diff --git a/src/privatepgtypes.hh b/src/privatepgtypes.hh
index db2290b0bb582d593a3c3bc61866ed0928a8ad83..34606a5249cec3d106d6b1fa213aaeffec482f86 100644
--- a/src/privatepgtypes.hh
+++ b/src/privatepgtypes.hh
@@ -49,11 +49,37 @@ namespace PatternGeneratorJRL
LEFT, RIGHT
};
+ inline std::ostream & operator<<(std::ostream & out, const foot_type_e & ft)
+ {
+ switch(ft)
+ {
+ case LEFT:
+ out << "LEFT";
+ break;
+ default:
+ out << "RIGHT";
+ }
+ return out;
+ }
+
enum PhaseType
{
SS, DS
};
+ inline std::ostream & operator<<(std::ostream & out, const PhaseType & pt)
+ {
+ switch(pt)
+ {
+ case SS:
+ out << "SingleSupport";
+ break;
+ default:
+ out << "DoubleSupport";
+ }
+ return out;
+ }
+
enum ineq_e
{
INEQ_COP, INEQ_COM, INEQ_FEET
@@ -169,6 +195,13 @@ namespace PatternGeneratorJRL
reference_t(const reference_t &);
};
+ inline std::ostream & operator<<(std::ostream & out, const reference_t & Ref)
+ {
+ out << "Global: (" << Ref.Global.X << ", " << Ref.Global.Y << ", " << Ref.Global.Yaw << ")" << std::endl;
+ out << "Local: (" << Ref.Local.X << ", " << Ref.Local.Y << ", " << Ref.Local.Yaw << ")";
+ return out;
+ }
+
/// \brief Convex hull
struct convex_hull_t
{
@@ -286,6 +319,23 @@ namespace PatternGeneratorJRL
support_state_t();
};
+ inline std::ostream & operator<<(std::ostream & out, const support_state_t & st)
+ {
+ out << "SupportState" << std::endl;
+ out << "PhaseType " << st.Phase << std::endl;
+ out << "Foot " << st.Foot << std::endl;
+ out << "NbStepsLeft " << st.NbStepsLeft << std::endl;
+ out << "StepNumber " << st.StepNumber << std::endl;
+ out << "NbInstants " << st.NbInstants << std::endl;
+ out << "TimeLimit " << st.TimeLimit << std::endl;
+ out << "StartTime " << st.StartTime << std::endl;
+ out << "X " << st.X << std::endl;
+ out << "Y " << st.Y << std::endl;
+ out << "Yaw " << st.Yaw << std::endl;
+ out << "StateChanged " << st.StateChanged;
+ return out;
+ }
+
/// \brief Solution
struct solution_t
{
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index dc704fc051cce70dcbcffe260af39638ca0fdedf..4f52bdf0ab98b33f530ea561ffa4c98670e0dbe2 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -15,6 +15,13 @@
INCLUDE(CTest)
+# Optional test dependencies.
+# We put them here to prevent them to be listed in the .pc file.
+ADD_OPTIONAL_DEPENDENCY("hrp2-14 >= 1.8-6")
+ADD_OPTIONAL_DEPENDENCY("hrp2-dynamics >= 1.0.1")
+
+MESSAGE(STATUS "hrp2-dynamics: " ${HRP2_DYNAMICS_FOUND})
+
# Import jrl-mal flags.
ADD_DEFINITIONS(${JRL_MAL_CFLAGS})
LINK_DIRECTORIES(${JRL_DYNAMICS_LIBRARY_DIRS})
@@ -44,6 +51,15 @@ ADD_EXECUTABLE(TestOptCholesky
../src/Mathematics/OptCholesky.cpp
)
+#########################
+# Test Bspline #
+#########################
+MESSAGE(STATUS "Bspline taken into account")
+ADD_EXECUTABLE(TestBsplines
+ TestBsplines.cpp
+ ../src/Mathematics/Bsplines.cpp
+ )
+
#########################
# Test Ricatti Equation #
#########################
@@ -54,50 +70,62 @@ ADD_EXECUTABLE(TestRiccatiEquation
PKG_CONFIG_USE_DEPENDENCY(TestRiccatiEquation jrl-dynamics)
+
######################
# Test Morisawa 2007 #
######################
-math(EXPR BITS "8*${CMAKE_SIZEOF_VOID_P}")
-CONFIG_FILES_CMAKE(TestMorisawa2007OnLine${BITS}TestFGPI.datref)
-CONFIG_FILES_CMAKE(TestMorisawa2007ShortWalk${BITS}TestFGPI.datref)
-
-MACRO(ADD_MORISAWA_2007 test_morisawa_arg)
- SET(testmorisawa2007 "${test_morisawa_arg}${BITS}")
-
- ADD_EXECUTABLE(${testmorisawa2007}
- ../src/portability/gettimeofday.cc
- TestMorisawa2007.cpp
- CommonTools.cpp
- TestObject.cpp
- ClockCPUTime.cpp
- )
- TARGET_LINK_LIBRARIES(${testmorisawa2007} ${PROJECT_NAME})
- PKG_CONFIG_USE_DEPENDENCY(${testmorisawa2007} jrl-dynamics)
- PKG_CONFIG_USE_DEPENDENCY(${testmorisawa2007} hrp2-dynamics)
- ADD_DEPENDENCIES(${testmorisawa2007} ${PROJECT_NAME})
+IF(HRP2_DYNAMICS_FOUND)
+ IF(HRP2_14_FOUND)
- MESSAGE(STATUS "jrl data dir: " ${JRL_DYNAMICS_PKGDATAROOTDIR})
- SET(samplemodelpath ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/)
- SET(samplespec
- ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleSpecificities.xml
- )
- SET(sampleljr
- ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleLinkJointRank.xml
- )
- SET(sampleinitconfig
- ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleInitConfig.dat)
+ math(EXPR BITS "8*${CMAKE_SIZEOF_VOID_P}")
+ CONFIG_FILES_CMAKE(TestMorisawa2007OnLine${BITS}TestFGPI.datref)
+ CONFIG_FILES_CMAKE(TestMorisawa2007ShortWalk${BITS}TestFGPI.datref)
+ # CONFIG_FILES_CMAKE(TestMorisawa2007Climbing${BITS}TestFGPI.datref)
+
+ MACRO(ADD_MORISAWA_2007 test_morisawa_arg)
+ SET(testmorisawa2007 "${test_morisawa_arg}${BITS}")
+
+ ADD_EXECUTABLE(${testmorisawa2007}
+ ../src/portability/gettimeofday.cc
+ TestMorisawa2007.cpp
+ CommonTools.cpp
+ TestObject.cpp
+ ClockCPUTime.cpp
+ )
+ TARGET_LINK_LIBRARIES(${testmorisawa2007} ${PROJECT_NAME})
+ PKG_CONFIG_USE_DEPENDENCY(${testmorisawa2007} jrl-dynamics)
+ PKG_CONFIG_USE_DEPENDENCY(${testmorisawa2007} hrp2-dynamics)
+ ADD_DEPENDENCIES(${testmorisawa2007} ${PROJECT_NAME})
+
+ MESSAGE(STATUS "jrl data dir: " ${JRL_DYNAMICS_PKGDATAROOTDIR})
+ SET(samplemodelpath ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/)
+ SET(samplespec
+ ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleSpecificities.xml
+ )
+ SET(sampleljr
+ ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleLinkJointRank.xml
+ )
+ SET(sampleinitconfig
+ ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleInitConfig.dat)
+
+ LIST(APPEND LOGGING_WATCHED_VARIABLES samplespec sampleljr)
+
+ # This test is disabled for now as it fails.
+ # FIXME: fix the test and/or the implementation
+ ADD_TEST(${testmorisawa2007}
+ ${testmorisawa2007}
+ ${samplemodelpath} sample.wrl ${samplespec} ${sampleljr} ${sampleinitconfig})
+ ENDMACRO(ADD_MORISAWA_2007)
+
+ ADD_MORISAWA_2007(TestMorisawa2007OnLine)
+ ADD_MORISAWA_2007(TestMorisawa2007ShortWalk)
+ ADD_MORISAWA_2007(TestMorisawa2007Climbing)
+ ADD_MORISAWA_2007(TestMorisawa2007GoingDown)
- LIST(APPEND LOGGING_WATCHED_VARIABLES samplespec sampleljr)
+ ENDIF(HRP2_14_FOUND)
+ENDIF(HRP2_DYNAMICS_FOUND)
- # This test is disabled for now as it fails.
- # FIXME: fix the test and/or the implementation
- ADD_TEST(${testmorisawa2007}
- ${testmorisawa2007}
- ${samplemodelpath} sample.wrl ${samplespec} ${sampleljr} ${sampleinitconfig})
-ENDMACRO(ADD_MORISAWA_2007)
-ADD_MORISAWA_2007(TestMorisawa2007OnLine)
-ADD_MORISAWA_2007(TestMorisawa2007ShortWalk)
###################
# Test Herdt 2010 #
diff --git a/tests/CommonTools.cpp b/tests/CommonTools.cpp
index 95fbfd74400c76353fd194b7351e225732109462..51f0dd63c1a560b478aa9d6abaa653b4977b36f3 100644
--- a/tests/CommonTools.cpp
+++ b/tests/CommonTools.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Andrei Herdt
* Olivier Stasse
@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
@@ -59,15 +59,15 @@ namespace PatternGeneratorJRL {
":previewcontroltime 1.6",
":omega 0.0",
":stepheight 0.07",
- ":singlesupporttime 0.78",
- ":doublesupporttime 0.02",
+ ":singlesupporttime 1.56",
+ ":doublesupporttime 0.04",
":armparameters 0.5",
":LimitsFeasibility 0.0",
":ZMPShiftParameters 0.015 0.015 0.015 0.015",
":TimeDistributionParameters 2.0 3.7 1.7 3.0",
":UpperBodyMotionParameters -0.1 -1.0 0.0"
};
-
+
for(int i=0;i<9;i++)
{
std::istringstream strm(lBuffer[i]);
@@ -85,28 +85,28 @@ namespace PatternGeneratorJRL {
lStartingWaistPose,
InitLeftFootAbsPos,
InitRightFootAbsPos);
-
- cout << "Starting COM Position: "
+
+ cout << "Starting COM Position: "
<< lStartingCOMPosition.x[0] << " "
<< lStartingCOMPosition.y[0] << " "
<< lStartingCOMPosition.z[0] << endl;
- cout << "Starting Left Foot Pos: "
+ cout << "Starting Left Foot Pos: "
<< InitLeftFootAbsPos.x << " "
<< InitLeftFootAbsPos.y << " "
- << InitLeftFootAbsPos.z << " "
+ << InitLeftFootAbsPos.z << " "
<< InitLeftFootAbsPos.theta<< " "
<< InitLeftFootAbsPos.omega << " "
- << InitLeftFootAbsPos.omega2 << " "
+ << InitLeftFootAbsPos.omega2 << " "
<< endl;
- cout << "Starting Right Foot Pos: "
+ cout << "Starting Right Foot Pos: "
<< InitRightFootAbsPos.x << " "
<< InitRightFootAbsPos.y << " "
- << InitRightFootAbsPos.z << " "
+ << InitRightFootAbsPos.z << " "
<< InitRightFootAbsPos.theta<< " "
<< InitRightFootAbsPos.omega << " "
- << InitRightFootAbsPos.omega2 << " "
+ << InitRightFootAbsPos.omega2 << " "
<< endl;
}
@@ -132,8 +132,8 @@ namespace PatternGeneratorJRL {
LINK_JOINT_RANK \
INITIAL_CONFIGURATION" << endl;
exit(-1);
- }
- else
+ }
+ else
{
VRMLPath=argv[1];
VRMLFileName=argv[2];
diff --git a/tests/TestBsplines.cpp b/tests/TestBsplines.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..55851f8b6f25a3beca999ccda253ab7793d1781f
--- /dev/null
+++ b/tests/TestBsplines.cpp
@@ -0,0 +1,79 @@
+/*
+ * Copyright 2009, 2010, 2014
+ *
+ *
+ * Olivier Stasse, Huynh Ngoc Duc
+ *
+ * JRL, CNRS/AIST
+ *
+ * This file is part of walkGenJrl.
+ * walkGenJrl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * walkGenJrl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Lesser Public License for more details.
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Research carried out within the scope of the
+ * Joint Japanese-French Robotics Laboratory (JRL)
+ */
+/*! \file TestBsplines.cpp
+ \brief This Example shows you how to use Bsplines to create a foot trajectory on Z . */
+#include <stdlib.h>
+#include <iostream>
+#include <fstream>
+#include "Mathematics/Bsplines.hh"
+
+using namespace std;
+
+int main()
+{
+ PatternGeneratorJRL::ZBsplines *Z;
+ double t=0.0;
+ int m_degree;
+ int i , j;
+ ofstream myfile;
+ myfile.open("TestBsplines.txt");
+
+ //Create the parameters of foot trajectory on Z
+ double m_FT = 0.7;
+ double m_FP = 0.2;
+ double m_MP = 0.3;
+ double m_ToMP = m_FT/3.0;
+
+ //Create an object for test
+ Z = new PatternGeneratorJRL::ZBsplines(m_FT, m_FP, m_ToMP, m_MP);
+
+ Z->PrintDegree();
+ Z->PrintKnotVector();
+ Z->PrintControlPoints();
+
+ for (int k=1; k<1000;k++)
+ {
+
+ t=double(k)*Z->GetKnotVector().back()/1000.0;
+ //cout << k << endl;
+ myfile << t << " " << Z->ZComputePosition(t) << " " << Z->ZComputeVelocity(t)<< " " << Z->ZComputeAcc(t)<< endl;
+ // time - Position - Velocity - Acceleration
+ //cout << t << " " << Z->ZComputePosition(t)<<" "<< Z->ZComputeVelocity(t)<< " "<< Z->ZComputeAcc(t)<< endl;
+ }
+ myfile.close();
+ delete Z;
+
+ //draw a foot trajectory with the data given from bsplines
+ myfile.open("DrawTestBsplines.gnu");
+ myfile << "set term wxt 0" << endl;
+ myfile << "plot 'control_point.txt' with points, 'TestBsplines.txt' using 1:2 with lines title 'Pos'"<< endl;
+ myfile << "set term wxt 1" << endl;
+ myfile << "plot 'TestBsplines.txt' using 1:2 with lines title 'Pos', 'TestBsplines.txt' using 1:3 with lines title 'Speed','TestBsplines.txt' using 1:4 with lines title 'Acc'"<< endl;
+ myfile.close();
+
+ return 0;
+}
+
+
diff --git a/tests/TestHerdt2010.cpp b/tests/TestHerdt2010.cpp
index 73445d53f275fd7fa6670fe446df0fd6b92f9371..3ab2a33181f9c1a9e1963e11a4d2075859824486 100644
--- a/tests/TestHerdt2010.cpp
+++ b/tests/TestHerdt2010.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Andrei Herdt
* Olivier Stasse
@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file This file tests A. Herdt's walking algorithm for
@@ -48,28 +48,28 @@ public:
{
m_TestProfile = TestProfile;
};
-
+
typedef void (TestHerdt2010::* localeventHandler_t)(PatternGeneratorInterface &);
-
- struct localEvent
+
+ struct localEvent
{
unsigned time;
localeventHandler_t Handler ;
};
-
+
protected:
-
-
+
+
void startOnLineWalking(PatternGeneratorInterface &aPGI)
{
CommonInitialization(aPGI);
-
+
{
istringstream strm2(":SetAlgoForZmpTrajectory Herdt");
aPGI.ParseCmd(strm2);
-
+
}
{
istringstream strm2(":singlesupporttime 0.7");
@@ -85,7 +85,8 @@ protected:
aPGI.ParseCmd(strm2);
}
{
- istringstream strm2(":numberstepsbeforestop 2");
+ // m_TestProfile.m_isStepStairOn = 0;
+ istringstream strm2(":numberstepsbeforestop 1");
aPGI.ParseCmd(strm2);
}
}
@@ -93,11 +94,11 @@ protected:
void startEmergencyStop(PatternGeneratorInterface &aPGI)
{
CommonInitialization(aPGI);
-
+
{
istringstream strm2(":SetAlgoForZmpTrajectory Herdt");
aPGI.ParseCmd(strm2);
-
+
}
{
istringstream strm2(":singlesupporttime 0.7");
@@ -178,7 +179,7 @@ protected:
void walkForward(PatternGeneratorInterface &aPGI)
{
{
- istringstream strm2(":setVelReference 0.2 0.0 0.0");
+ istringstream strm2(":setVelReference 0.02 0.0 0.0");
aPGI.ParseCmd(strm2);
}
}
@@ -202,7 +203,7 @@ protected:
void chooseTestProfile()
{
-
+
switch(m_TestProfile)
{
@@ -222,15 +223,16 @@ protected:
void generateEventOnLineWalking()
{
- struct localEvent
+ struct localEvent
{
unsigned time;
localeventHandler_t Handler ;
};
- #define localNbOfEvents 12
+ #define localNbOfEvents 2
struct localEvent events [localNbOfEvents] =
- { { 5*200,&TestHerdt2010::walkForward},
+ {
+ /* { 5*200,&TestHerdt2010::walkForward},
{10*200,&TestHerdt2010::walkSidewards},
{25*200,&TestHerdt2010::startTurningRightOnSpot},
{35*200,&TestHerdt2010::walkForward},
@@ -241,11 +243,24 @@ protected:
{85*200,&TestHerdt2010::startTurningLeft},
{95*200,&TestHerdt2010::startTurningRight},
{105*200,&TestHerdt2010::stop},
- {110*200,&TestHerdt2010::stopOnLineWalking}};
-
+ {110*200,&TestHerdt2010::stopOnLineWalking}*/
+
+ { 5*200,&TestHerdt2010::walkForward},
+ // {10*200,&TestHerdt2010::walkForward},
+ // {25*200,&TestHerdt2010::walkForward},
+ // {35*200,&TestHerdt2010::walkForward},
+ {45*200,&TestHerdt2010::stop}
+ /* {55*200,&TestHerdt2010::walkForward},
+ {65*200,&TestHerdt2010::walkForward},
+ {75*200,&TestHerdt2010::walkForward},
+ {85*200,&TestHerdt2010::walkForward},
+ {95*200,&TestHerdt2010::walkForward},
+ {105*200,&TestHerdt2010::stop},
+ {110*200,&TestHerdt2010::stopOnLineWalking}*/};
+
// Test when triggering event.
for(unsigned int i=0;i<localNbOfEvents;i++)
- {
+ {
if ( m_OneStep.NbOfIt==events[i].time)
{
ODEBUG3("********* GENERATE EVENT OLW ***********");
@@ -263,10 +278,10 @@ protected:
{10*200,&TestHerdt2010::startTurningRight2},
{15.2*200,&TestHerdt2010::stop},
{20.8*200,&TestHerdt2010::stopOnLineWalking}};
-
+
// Test when triggering event.
for(unsigned int i=0;i<localNbOfEventsEMS;i++)
- {
+ {
if ( m_OneStep.NbOfIt==events[i].time)
{
ODEBUG3("********* GENERATE EVENT EMS ***********");
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index f6e9e46f738d7c1ec1ea586faa8c5b9e7debe321..70b784c7d3f3731b9ab5b1318e3ae4e41943cb9b 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -35,8 +35,10 @@ using namespace::PatternGeneratorJRL::TestSuite;
using namespace std;
enum Profiles_t {
- PROFIL_ANALYTICAL_ONLINE_WALKING, // 1
- PROFIL_ANALYTICAL_SHORT_STRAIGHT_WALKING // 2
+ PROFIL_ANALYTICAL_ONLINE_WALKING, // 1
+ PROFIL_ANALYTICAL_SHORT_STRAIGHT_WALKING, // 2
+ PROFIL_ANALYTICAL_CLIMBING_STAIRS, // 3
+ PROFIL_ANALYTICAL_GOING_DOWN_STAIRS, // 4
};
#define NBOFPREDEFONLINEFOOTSTEPS 11
@@ -617,6 +619,87 @@ protected:
0.2 -0.19 0.0\
0.2 0.19 0.0\
0.0 -0.19 0.0");
+// istringstream strm2(":stepstairseq 0.0 -0.105 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.2 -0.19 0.0 0.0\
+// 0.0 0.19 0.0 0.0\
+// ");
+// /*0.3 0.19 0.15 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.2 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.3 0.19 0.15 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.2 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.3 0.19 0.15 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.2 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.3 0.19 -0.15 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.2 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.3 0.19 -0.15 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.3 0.19 -0.15 0.0\
+// 0.0 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.2 -0.19 0.0 0.0\
+// 0.2 0.19 0.0 0.0\
+// 0.0 -0.19 0.0 0.0\*/
+ aPGI.ParseCmd(strm2);
+ }
+
+ }
+
+ void AnalyticalClimbingStairs(PatternGeneratorInterface &aPGI)
+ {
+ CommonInitialization(aPGI);
+ {
+ istringstream strm2(":SetAlgoForZmpTrajectory Morisawa");
+ aPGI.ParseCmd(strm2);
+ }
+
+ {
+ istringstream strm2(":stepstairseq 0.0 -0.105 0.0 0.0\
+ 0.3 0.19 0.15 0.0\
+ 0.0 -0.19 0.0 0.0\
+ 0.3 0.19 0.15 0.0\
+ 0.0 -0.19 0.0 0.0\
+ 0.3 0.19 0.15 0.0\
+ 0.0 -0.19 0.0 0.0\
+ ");
+ aPGI.ParseCmd(strm2);
+ }
+
+ }
+
+ void AnalyticalGoingDownStairs(PatternGeneratorInterface &aPGI)
+ {
+ CommonInitialization(aPGI);
+ {
+ istringstream strm2(":SetAlgoForZmpTrajectory Morisawa");
+ aPGI.ParseCmd(strm2);
+ }
+
+ {
+ istringstream strm2(":stepstairseq 0.0 -0.105 0.0 0.0\
+ 0.32 0.19 -0.15 0.0\
+ 0.0 -0.19 0.0 0.0\
+ 0.32 0.19 -0.15 0.0\
+ 0.0 -0.19 0.0 0.0\
+ 0.32 0.19 -0.15 0.0\
+ 0.0 -0.19 0.0 0.0\
+ ");
aPGI.ParseCmd(strm2);
}
@@ -631,6 +714,15 @@ protected:
AnalyticalShortStraightWalking(*m_PGI);
break;
+ case PROFIL_ANALYTICAL_CLIMBING_STAIRS:
+ AnalyticalClimbingStairs(*m_PGI);
+ break;
+
+ case PROFIL_ANALYTICAL_GOING_DOWN_STAIRS:
+ AnalyticalGoingDownStairs(*m_PGI);
+ break;
+
+
case PROFIL_ANALYTICAL_ONLINE_WALKING:
StartAnalyticalOnLineWalking(*m_PGI);
break;
@@ -644,6 +736,11 @@ protected:
{
if (m_TestProfile==PROFIL_ANALYTICAL_SHORT_STRAIGHT_WALKING)
return;
+ if (m_TestProfile==PROFIL_ANALYTICAL_CLIMBING_STAIRS)
+ return;
+ if (m_TestProfile==PROFIL_ANALYTICAL_GOING_DOWN_STAIRS)
+ return;
+
unsigned int StoppingTime = 70*200;
@@ -719,14 +816,21 @@ int PerformTests(int argc, char *argv[])
std::string CompleteName = string(argv[0]);
unsigned found = CompleteName.find_last_of("/\\");
std::string TestName = CompleteName.substr(found+1);
- int TestProfiles[2] = { PROFIL_ANALYTICAL_ONLINE_WALKING,
- PROFIL_ANALYTICAL_SHORT_STRAIGHT_WALKING};
+ int TestProfiles[4] = { PROFIL_ANALYTICAL_ONLINE_WALKING,
+ PROFIL_ANALYTICAL_SHORT_STRAIGHT_WALKING,
+ PROFIL_ANALYTICAL_CLIMBING_STAIRS,
+ PROFIL_ANALYTICAL_GOING_DOWN_STAIRS};
int indexProfile=-1;
if (TestName.compare(16,6,"OnLine")==0)
indexProfile=0;
if (TestName.compare(16,9,"ShortWalk")==0)
indexProfile=1;
+ if (TestName.compare(16,8,"Climbing")==0)
+ indexProfile=2;
+ if (TestName.compare(16,9,"GoingDown")==0)
+ indexProfile=3;
+
if (indexProfile==-1)
{
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index c0f544be77b67df368fb771a0914aa08871969c7..4a7f97bbc6984dde5d30d50da83742d395895245 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -44,7 +44,6 @@ namespace PatternGeneratorJRL
{
namespace TestSuite
{
-
double filterprecision(double adb)
{
if (fabs(adb)<1e-7)
@@ -81,8 +80,6 @@ namespace PatternGeneratorJRL
void TestObject::init()
{
-
-
// Instanciate and initialize.
string RobotFileName = m_VRMLPath + m_VRMLFileName;
@@ -125,7 +122,6 @@ namespace PatternGeneratorJRL
if (m_PGI!=0)
delete m_PGI;
-
}
void TestObject::SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR,
@@ -135,6 +131,7 @@ namespace PatternGeneratorJRL
aDebugHDR = 0;
}
+
void TestObject::CreateAndInitializeHumanoidRobot(string &RobotFileName,
string &SpecificitiesFileName,
string &LinkJointRank,
@@ -330,6 +327,7 @@ namespace PatternGeneratorJRL
}
}
+
void TestObject::fillInDebugFiles( )
{
if (m_DebugFGPI)
@@ -590,6 +588,7 @@ namespace PatternGeneratorJRL
m_clock.fillInStatistics();
+
/*! Fill the debug files with appropriate information. */
fillInDebugFiles();
}
diff --git a/tests/TestObject.hh b/tests/TestObject.hh
index a0c55240cf4a3ec74d62c8a2960d81cc0e4d8782..4c71b975d798e422d5dcd8e36c14790d387fe35e 100644
--- a/tests/TestObject.hh
+++ b/tests/TestObject.hh
@@ -44,6 +44,8 @@
#include <jrl/mal/matrixabstractlayer.hh>
#include <jrl/dynamics/dynamicsfactory.hh>
#include <jrl/walkgen/patterngeneratorinterface.hh>
+#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
+#include <hrp2-dynamics/hrp2OptHumanoidDynamicRobot.h>
#include "CommonTools.hh"
#include "ClockCPUTime.hh"
@@ -69,6 +71,7 @@ namespace PatternGeneratorJRL
/*! \brief Initialize the test object. */
void init();
+ // void initIK();
/*! \brief Perform test. */
bool doTest(std::ostream &os);
@@ -126,6 +129,7 @@ namespace PatternGeneratorJRL
/*! \brief Initial Configuration */
MAL_VECTOR(InitialPosition,double);
+
/*! @} */
/* !\name Handle on the Humanoids models