From 323b97d6a53aad2e55740e8ec5ba3ee6920d6e39 Mon Sep 17 00:00:00 2001
From: olivier-stasse <olivier.stasse@aist.go.jp>
Date: Fri, 11 Mar 2011 05:07:25 +0900
Subject: [PATCH] Revert "Add method for the construction of selection
matrices"
This reverts commit 8f792fefce2817754e6ab2d77a92ac56ddba48c3.
---
.../generator-vel-ref.cpp | 31 +++----------------
.../generator-vel-ref.hh | 9 +-----
2 files changed, 5 insertions(+), 35 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index f4e53a6a..50def749 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -73,9 +73,10 @@ GeneratorVelRef::preview(IntermedQPMat Matrices,
SupportFSM * FSM, std::deque<support_state_t> & deqSupportStates)
{
- //initialize the previewed support state before previewing
const support_state_t & CurrentSupport = deqSupportStates[0];
support_state_t PreviewedSupport;
+
+ //initialize the previewed support state before previewing
PreviewedSupport.Phase = CurrentSupport.Phase;
PreviewedSupport.Foot = CurrentSupport.Foot;
PreviewedSupport.StepsLeft = CurrentSupport.StepsLeft;
@@ -92,32 +93,7 @@ GeneratorVelRef::preview(IntermedQPMat Matrices,
}
-
-void
-GeneratorVelRef::generateSelectionMatrices(IntermedQPMat & Matrices,
- std::deque<support_state_t> & deqSupportStates)
-{
-
- IntermedQPMat::state_variant_t & State = Matrices.State();
- const int & NbPrwSteps = deqSupportStates.back().StepNumber;
- MAL_VECTOR_RESIZE(State.Vc,m_N);
- MAL_MATRIX_RESIZE(State.V,m_N,NbPrwSteps);
-
- std::deque<support_state_t>::iterator SS_it;
- SS_it = deqSupportStates.begin();
- SS_it++;
- for(int i=0;i<m_N;i++)
- {
- if(SS_it->StepNumber>0)
- State.V(i,SS_it->StepNumber-1) = State.VT(SS_it->StepNumber-1,i) = 1.0;
- else
- State.Vc(i) = 1.0;
- SS_it++;
- }
-
-}
-
-
+
void
GeneratorVelRef::computeGlobalReference(IntermedQPMat & Matrices, COMState TrunkStateT)
{
@@ -248,6 +224,7 @@ GeneratorVelRef::buildInvariantPart(QPProblem & Pb, IntermedQPMat & Matrices)
MAL_MATRIX(weightMTM,double);
const IntermedQPMat::objective_variant_t & Jerk = Matrices.Objective(IntermedQPMat::JERK);
+ // Final scaled products that are added to the QP
computeTerm(weightMTM, Jerk.weight, Jerk.UT, Jerk.U);
Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, 0, 0);
Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, m_N, m_N);
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index 5203fa39..682337c9 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -78,13 +78,6 @@ namespace PatternGeneratorJRL
void preview(IntermedQPMat Matrices,
SupportFSM * FSM, std::deque<support_state_t> & deqSupportStates);
- /// \brief Compute the selection matrices
- ///
- /// \param Matrices
- /// \param deqSupportStates
- void generateSelectionMatrices(IntermedQPMat & Matrices,
- std::deque<support_state_t> & deqSupportStates);
-
/// \brief Set the global reference from the local one and the orientation of the trunk frame
/// for the whole preview window
///
@@ -111,7 +104,7 @@ namespace PatternGeneratorJRL
/// \param DS
void addIneqConstraint(std::deque<LinearConstraintInequalityFreeFeet_t> ConstraintsDeque,
MAL_MATRIX (&DU, double), MAL_MATRIX (&DS, double));
-
+
/// \brief Generate a queue of inequality constraints on the ZMP for the whole preview window
///
/// \param CjrlFoot
--
GitLab