Commit 9650b279 authored by mnaveau's avatar mnaveau
Browse files

merging some feature from the master branch

parent 69ee0f8e
......@@ -61,6 +61,16 @@ namespace PatternGeneratorJRL
};
inline std::ostream & operator<<(std::ostream & os, const COMPosition_s & aCp)
{
for(size_t i = 0; i < 3; ++i)
{
os << "x[" << i << "] " << aCp.x[i] << " y[" << i << "] " << aCp.y[i] << " z[" << i << "] " << aCp.z[i] << std::endl;
}
os << "yaw " << aCp.yaw << " pitch " << aCp.pitch << " roll " << aCp.roll;
return os;
}
typedef struct COMPosition_s COMPosition;
typedef struct COMPosition_s WaistState;
......@@ -80,8 +90,8 @@ namespace PatternGeneratorJRL
friend std::ostream & operator<<(std::ostream &os, const struct COMState_s & acs);
};
typedef struct COMState_s COMState;
......
......@@ -144,11 +144,13 @@ int CoMAndFootOnlyStrategy::EvaluateStartingState(MAL_VECTOR(&,double) BodyAngle
lStartingWaistPose,
InitLeftFootPosition, InitRightFootPosition);
ODEBUG("EvaluateStartingCOM: m_StartingCOMState: " << lStartingCOMState);
aStartingCOMState.x[0] = lStartingCOMState(0);
aStartingCOMState.y[0] = lStartingCOMState(1);
aStartingCOMState.z[0] = lStartingCOMState(2);
aStartingCOMState.yaw[0] = lStartingWaistPose(5);
aStartingCOMState.pitch[0] = lStartingWaistPose(4);
aStartingCOMState.roll[0] = lStartingWaistPose(3);
aStartingZMPPosition= (*itCFR)->GetCOGInitialAnkles();
}
......
......@@ -125,6 +125,11 @@ namespace PatternGeneratorJRL
inline void CoM( const com_t & CoM )
{ StateMatrices_.CoM = CoM; };
inline trunk_t const & Trunk() const
{ return StateMatrices_.Trunk; }
inline void Trunk( const trunk_t & Trunk )
{ StateMatrices_.Trunk = Trunk; }
inline reference_t const & Reference() const
{ return StateMatrices_.Ref; };
inline reference_t & Reference()
......
......@@ -366,13 +366,19 @@ InitializationHumanoid(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
getHumanoidDynamicRobot()->currentConfiguration();
// Set to zero the free floating root.
CurrentConfig[0] = 0.0;
CurrentConfig[1] = 0.0;
CurrentConfig[2] = 0.0;
CurrentConfig[3] = 0.0;
CurrentConfig[4] = 0.0;
CurrentConfig[5] = 0.0;
if(lStartingWaistPose.size())
{
CurrentConfig[0] = lStartingWaistPose(0);
CurrentConfig[1] = lStartingWaistPose(1);
CurrentConfig[2] = lStartingWaistPose(2);
CurrentConfig[3] = lStartingWaistPose(3);
CurrentConfig[4] = lStartingWaistPose(4);
CurrentConfig[5] = lStartingWaistPose(5);
}
else
{
lStartingWaistPose.resize(6,0);
}
// Initialize the configuration vector.
for(unsigned int i=0;i<m_GlobalVRMLIDtoConfiguration.size();i++)
......
......@@ -397,8 +397,8 @@ namespace PatternGeneratorJRL {
delete m_ZMPM;
ODEBUG4("Destructor: did m_ZMPM","DebugPGI.txt");
if ((*m_ComAndFootRealization.begin())!=0)
delete (*m_ComAndFootRealization.begin());
if (m_ComAndFootRealization[0]!=0)
delete (m_ComAndFootRealization[0]);
if (m_FeetTrajectoryGenerator!=0)
delete m_FeetTrajectoryGenerator;
......@@ -1512,10 +1512,6 @@ namespace PatternGeneratorJRL {
// to be done only when the robot has finish a motion.
UpdateAbsolutePosition(UpdateAbsMotionOrNot);
ODEBUG("Return true");
//cout << "CoM 0 :\n" << *m_ComAndFootRealization[0] << endl ;
//cout << "CoM 1 :\n" << *m_ComAndFootRealization[1] << endl ;
return m_Running;
}
......
/*
* Copyright 2010,
* Copyright 2010,
*
* Andrei Herdt
* 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)
*/
/*
......@@ -92,7 +92,7 @@ OrientationsPreview::preview_orientations(double Time,
bool TrunkVelOK = false;
bool TrunkAngleOK = false;
// In case of double support the next support angle is fixed
// ds -> FirstFootPreviewed == 0
// ss -> FirstFootPreviewed == 1
......@@ -291,7 +291,9 @@ OrientationsPreview::verify_angle_hip_joint(const support_state_t & CurrentSuppo
return false;
}
else
{
return true;
}
}
......@@ -351,7 +353,7 @@ OrientationsPreview::verify_velocity_hip_joint(double Time,
d = (-2.0*c-b/T)/(3.0*T);
PreviewedSupportAngle = f(a,b,c,d,T);
}
}
}
......
......@@ -332,6 +332,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
COMState & lStartingCOMState,
MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition)
{
UpperTimeLimitToUpdate_ = 0.0;
FootAbsolutePosition CurrentLeftFootAbsPos, CurrentRightFootAbsPos;
......@@ -343,16 +344,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
// INITIALIZE FEET POSITIONS:
// --------------------------
CurrentLeftFootAbsPos = InitLeftFootAbsolutePosition;
CurrentLeftFootAbsPos.z = 0.0;
CurrentLeftFootAbsPos.dz = CurrentLeftFootAbsPos.ddz = 0.0;
CurrentLeftFootAbsPos.time = 0.0;
CurrentLeftFootAbsPos.theta = 0.0;
CurrentRightFootAbsPos = InitRightFootAbsolutePosition;
CurrentRightFootAbsPos.z = 0.0;
CurrentRightFootAbsPos.dz = CurrentRightFootAbsPos.ddz = 0.0;
CurrentRightFootAbsPos.time = 0.0;
CurrentRightFootAbsPos.theta = 0.0;
// FILL THE QUEUES:
// ----------------
......@@ -368,6 +360,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
FinalLeftFootTraj_deq.resize(AddArraySize);
FinalRightFootTraj_deq.resize(AddArraySize);
int CurrentZMPindex=0;
m_CurrentTime = 0;
for( unsigned int i=0;i<FinalZMPTraj_deq.size();i++ )
{
// Smooth ramp
......@@ -400,9 +393,9 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
CurrentSupport.TimeLimit = 1000000000;
CurrentSupport.NbStepsLeft = 1;
CurrentSupport.StateChanged = false;
CurrentSupport.X = 0.0;
CurrentSupport.Y = 0.1;
CurrentSupport.Yaw = 0.0;
CurrentSupport.X = 0.0 ; //CurrentLeftFootAbsPos.x;
CurrentSupport.Y = 0.1 ; //CurrentLeftFootAbsPos.y;
CurrentSupport.Yaw = 0.0 ; //CurrentLeftFootAbsPos.theta*M_PI/180;
CurrentSupport.StartTime = 0.0;
IntermedData_->SupportState(CurrentSupport);
......@@ -423,6 +416,9 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
CoM_(CoM);
IntermedData_->CoM(CoM_());
// Initialize preview of orientations
OrientPrw_->CurrentTrunkState( lStartingCOMState );
// initialisation of a second object that allow the interpolation along 1.6s
CoM2_.SetComHeight(lStartingCOMState.z[0]);
CoM2_.InitializeSystem();
......@@ -451,7 +447,9 @@ ZMPVelocityReferencedQP::OnLine(double time,
// If on-line mode not activated we go out.
if (!m_OnLineMode)
{
return;
}
// Test if the end of the online mode has been reached.
if ((EndingPhase_) &&
......@@ -512,16 +510,20 @@ ZMPVelocityReferencedQP::OnLine(double time,
// SOLVE PROBLEM:
// --------------
if (Solution_.useWarmStart)
{
VRQPGenerator_->compute_warm_start( Solution_ );//TODO: Move to update_problem or build_constraints?
}
Problem_.solve( QLD, Solution_, NONE );
if(Solution_.Fail>0)
Problem_.dump( time );
{
Problem_.dump( time );
}
static int iteration = 0 ;
if (iteration == 269)
{
Solution_.print(cout);
}
static int iteration = 0 ;
if (iteration == 269)
{
Solution_.print(cout);
}
// INTERPOLATE THE NEXT COMPUTED COM STATE:
......
......@@ -684,10 +684,6 @@ namespace PatternGeneratorJRL
/*! Buffer of ZMP positions */
deque<ZMPPosition> m_ZMPPositions;
/*! Buffer of reference ZMP positions */
deque<ZMPPosition> m_ZMPReference;
/*! Buffer of Absolute foot position (World frame) */
deque<FootAbsolutePosition> m_FootAbsolutePositions;
......
Supports Markdown
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