Skip to content
Snippets Groups Projects
Commit ce742802 authored by Francois Keith's avatar Francois Keith
Browse files

Remove useless files (only the .hh were used).

parent 98939c2e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/ZMPRefTrajectoryGeneration/OrientationsPreview.h deleted 100644 → 0
+ 0
− 225
View file @ 98939c2e
/*
* 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_ */
src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h deleted 100644 → 0
+ 0
− 242
View file @ 98939c2e
/*
* 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/LinearizedInvertedPendulum2D.h>
#include <PreviewControl/rigid-body-system.hh>
#include <Mathematics/relative-feet-inequalities.hh>
#include <ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.h>
#include <PreviewControl/SupportFSM.h>
#include <ZMPRefTrajectoryGeneration/OrientationsPreview.h>
#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
#include <privatepgtypes.h>
#include <ZMPRefTrajectoryGeneration/generator-vel-ref.hh>
#include <Mathematics/intermediate-qp-matrices.hh>
#include <jrl/walkgen/pgtypes.hh>
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> & CoMStates,
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> & CoMStates,
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_; }
/// \}
//
// 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 CoM_;
/// \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_;
/// \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_;
public:
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();
};
}
#include <ZMPRefTrajectoryGeneration/ZMPDiscretization.h>
#endif /* _ZMPQP_WITH_CONSTRAINT_H_ */
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment