Unverified Commit 5e99cb86 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #15 from nim65s/devel

Fix orientation pb + Dynamical filter initialization
parents fdd2a0f5 e0df8fb0
......@@ -13,6 +13,7 @@ SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
OPTION(USE_LSSOL "Do you want to use the solver lssol?" OFF)
OPTION(USE_QUADPROG "Do you want to use the solver eigen-quadprog?" ON)
OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
OPTION(FULL_BUILD_TESTING "Complete and long testing" OFF)
# Project configuration
SET(PROJECT_USE_CMAKE_EXPORT TRUE)
......
......@@ -42,6 +42,10 @@ struct PinocchioRobotFoot_t {
double soleHeight; // x axis
Eigen::Vector3d anklePosition;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
inline PinocchioRobotFoot_t()
: associatedAnkle(0), soleDepth(0.0), soleWidth(0.0), soleHeight(0.0),
anklePosition(0.0, 0.0, 0.0) {}
};
typedef PinocchioRobotFoot_t PRFoot;
......
......@@ -46,7 +46,8 @@ Clock::Clock() {
struct timeval startingtime;
gettimeofday(&startingtime, 0);
m_StartingTime = startingtime.tv_sec + 0.000001 * startingtime.tv_usec;
m_StartingTime =
(double)startingtime.tv_sec + 0.000001 * (double)startingtime.tv_usec;
}
Clock::~Clock() {}
......@@ -58,22 +59,24 @@ void Clock::Reset() {
struct timeval startingtime;
gettimeofday(&startingtime, 0);
m_StartingTime = startingtime.tv_sec + 0.000001 * startingtime.tv_usec;
m_StartingTime =
(double)startingtime.tv_sec + 0.000001 * (double)startingtime.tv_usec;
}
void Clock::StartTiming() { gettimeofday(&m_BeginTimeStamp, 0); }
void Clock::StopTiming() {
gettimeofday(&m_EndTimeStamp, 0);
double ltime = m_EndTimeStamp.tv_sec - m_BeginTimeStamp.tv_sec +
0.000001 * (m_EndTimeStamp.tv_usec - m_BeginTimeStamp.tv_usec);
double ltime =
(double)m_EndTimeStamp.tv_sec - (double)m_BeginTimeStamp.tv_sec +
0.000001 * (double)(m_EndTimeStamp.tv_usec - m_BeginTimeStamp.tv_usec);
m_MaximumTime = m_MaximumTime < ltime ? ltime : m_MaximumTime;
m_TotalTime += ltime;
m_DataBuffer[(m_NbOfIterations * 2) % 3000000] =
m_BeginTimeStamp.tv_sec + 0.000001 * m_BeginTimeStamp.tv_usec -
m_StartingTime;
(double)m_BeginTimeStamp.tv_sec +
0.000001 * (double)m_BeginTimeStamp.tv_usec - m_StartingTime;
m_DataBuffer[(m_NbOfIterations * 2 + 1) % 3000000] = ltime;
}
......
......@@ -52,11 +52,8 @@ int CoMAndFootOnlyStrategy::InitInterObjects(
int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition, Eigen::VectorXd &ZMPRefPos,
COMState &finalCOMPosition,
Eigen::VectorXd &, // CurrentConfiguration,
Eigen::VectorXd &, // CurrentVelocity,
Eigen::VectorXd &) // CurrentAcceleration)
{
COMState &finalCOMPosition, Eigen::VectorXd &CurrentConfiguration,
Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration) {
ODEBUG("Begin OneGlobalStepOfControl "
<< m_LeftFootPositions->size() << " " << m_RightFootPositions->size()
<< " " << m_COMBuffer->size() << " " << m_ZMPPositions->size());
......@@ -87,6 +84,18 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(
return -4;
}
CurrentConfiguration[3] = finalCOMPosition.roll[0];
CurrentConfiguration[4] = finalCOMPosition.pitch[0];
CurrentConfiguration[5] = finalCOMPosition.yaw[0];
CurrentVelocity[3] = finalCOMPosition.roll[1];
CurrentVelocity[4] = finalCOMPosition.pitch[1];
CurrentVelocity[5] = finalCOMPosition.yaw[1];
CurrentAcceleration[3] = finalCOMPosition.roll[2];
CurrentAcceleration[4] = finalCOMPosition.pitch[2];
CurrentAcceleration[5] = finalCOMPosition.yaw[2];
if (m_ZMPPositions->size() > 0) {
ZMPPosition aZMPPosition = (*m_ZMPPositions)[0];
ZMPRefPos(0) = aZMPPosition.px;
......
......@@ -339,16 +339,17 @@ int PLDPSolver::BackwardSubstitution() {
// LL^t v2 = v1 <-> L y = v1 with L^t v2 = y
// y solved with first phase.
// So now we are looking for v2.
unsigned int SizeOfL = m_ActivatedConstraints.size();
std::vector<unsigned int>::size_type SizeOfL = m_ActivatedConstraints.size();
if (SizeOfL == 0)
return 0;
ODEBUG("BackwardSubstitution " << m_ItNb);
for (int i = SizeOfL - 1; i >= 0; i--) {
for (std::vector<unsigned int>::size_type i = SizeOfL - 1;; i--) {
double tmp = 0.0;
m_v2[i] = m_y[i];
for (int k = i + 1; k < (int)SizeOfL; k++) {
if (k == (int)SizeOfL - 1)
for (std::vector<unsigned int>::size_type k = i + 1; k < SizeOfL; k++) {
if (k == SizeOfL - 1)
tmp = m_v2[i];
m_v2[i] -= m_L[k * m_NbMaxOfConstraints + i] * m_v2[k];
......@@ -359,6 +360,8 @@ int PLDPSolver::BackwardSubstitution() {
ODEBUG("BS: m_L[i*m_NbMaxOfConstraints+i]:"
<< m_L[i * m_NbMaxOfConstraints + i] << " " << m_y[i]);
ODEBUG("m_v2[" << i << " ] = " << m_v2[i] << " " << tmp);
if (i == 0)
break;
}
return 0;
}
......@@ -789,7 +792,7 @@ int PLDPSolver::SolveProblem(
struct timeval current;
gettimeofday(&current, 0);
double r = (double)(current.tv_sec - begin.tv_sec) +
0.000001 * (current.tv_usec - begin.tv_usec);
0.000001 * (double)(current.tv_usec - begin.tv_usec);
if (r > m_AmountOfLimitedComputationTime) {
ContinueAlgo = false;
}
......
......@@ -394,18 +394,14 @@ doublereal *eps1;
/* System generated locals */
integer c_dim1, c_offset, a_dim1, a_offset, i__1;
/* Builtin functions */
/* integer s_wsfe(), do_fio(), e_wsfe(); */
/* Local variables */
static doublereal diag;
/* extern int ql0002_(); */
static integer nact, info;
static doublereal zero;
static integer i, j, idiag, maxit;
static integer i, j, maxit;
static doublereal qpeps;
static integer in, mn, lw;
static doublereal ten;
static logical lql;
static integer inw1, inw2;
......@@ -450,7 +446,6 @@ doublereal *eps1;
lql = TRUE_;
}
zero = 0.;
ten = 10.;
maxit = (*m + *n) * 40;
qpeps = cmache_1.eps;
inw1 = 1;
......@@ -496,18 +491,6 @@ L20:
if (info == 2) {
goto L90;
}
idiag = 0;
if (diag > zero && diag < 1e3) {
idiag = (integer)diag;
}
/*
if (*iprint > 0 && idiag > 0) {
io___16.ciunit = *iout;
s_wsfe(&io___16);
do_fio(&c__1, (char *)&idiag, (ftnlen)sizeof(integer));
e_wsfe();
}
*/
if (info < 0) {
goto L70;
}
......@@ -658,14 +641,14 @@ integer *lw;
static doublereal ga, gb;
static integer ia, id;
static doublereal fdiffa;
static integer ii, il, kk, jl, ip, ir, nm, is, iu, iw, ju, ix, iz, nu, iy;
static integer ii, il, kk, jl, ir, nm, is, iu, iw, ju, ix, iz, nu, iy;
static doublereal parinc, parnew;
static integer ira, irb, iwa;
static doublereal one;
static integer iwd, iza;
static doublereal res;
static integer ipp, iwr, iws;
static integer iwr, iws;
static doublereal sum;
static integer iww, iwx, iwy;
static doublereal two;
......@@ -1082,8 +1065,6 @@ L250:
L280:
ir = iwr;
ip = iww + 1;
ipp = iww + *n;
il = iws + 1;
iu = iws + *nact;
i__2 = iu;
......@@ -1829,8 +1810,6 @@ L770:
/* CALCULATE THE NEXT CONSTRAINT TO DROP. */
L775:
ip = iww + 1;
ipp = iww + *nact;
kdrop = 0;
if (*nact == 0) {
goto L791;
......
......@@ -300,11 +300,9 @@ void GenerateMotionFromKineoWorks::ComputeUpperBodyPosition(
//! Find the closest (X,Y,Z) position in the remaining
// part of the CoM buffer.
for (unsigned int i = count; i < m_COMBuffer.size(); i++) {
double ldist = (lX - m_COMBuffer[i].x[0]) * (lX - m_COMBuffer[i].x[0])
/*+
(lY - m_COMBuffer[i].y[0])*(lY-m_COMBuffer[i].y[0]) +
(lZ - m_COMBuffer[i].z[0])*(lZ-m_COMBuffer[i].z[0]) */
;
double ldist = (lX - m_COMBuffer[i].x[0]) * (lX - m_COMBuffer[i].x[0]) +
(lY - m_COMBuffer[i].y[0]) * (lY - m_COMBuffer[i].y[0]) +
(lZ - m_COMBuffer[i].z[0]) * (lZ - m_COMBuffer[i].z[0]);
if (ldist < dist) {
dist = ldist;
......
......@@ -362,7 +362,8 @@ void StepOverPlanner::DoubleSupportFeasibility() {
Eigen::Vector3d ToTheHip;
Eigen::Matrix<double, 6, 1> LeftLegAngles;
Eigen::Matrix<double, 6, 1> RightLegAngles;
LeftLegAngles.Zero();
RightLegAngles.Zero();
Eigen::Vector3d AnkleBeforeObst;
Eigen::Vector3d AnkleAfterObst;
Eigen::Vector3d TempCOMState;
......@@ -778,8 +779,8 @@ void StepOverPlanner::PolyPlannerFirstStep(
double StepLenght;
double Omega1, Omega2, OmegaImpact;
double xOffset;
double Point1X, Point1Y = 0.0, Point1Z;
double Point2X, Point2Y = 0.0, Point2Z;
double Point1X, Point1Z;
double Point2X, Point2Z;
double Point3Z;
StepTime = aStepOverFootBuffer[m_StartDoubleSupp].time -
......@@ -801,12 +802,10 @@ void StepOverPlanner::PolyPlannerFirstStep(
Point1X = StepLenght - m_heelToAnkle - m_ObstacleParameters.d - xOffset -
m_tipToAnkle * cos(Omega1 * M_PI / 180.0);
Point1Y = 0.00;
Point1Z = m_ObstacleParameters.h - m_tipToAnkle * sin(Omega1 * M_PI / 180.0);
Point2X = StepLenght - m_heelToAnkle + xOffset +
m_heelToAnkle * cos(Omega2 * M_PI / 180.0);
Point2Y = 0.00;
Point2Z = m_ObstacleParameters.h - m_tipToAnkle * sin(Omega2 * M_PI / 180.0);
Point3Z = Point1Z + 0.04;
......@@ -842,12 +841,10 @@ void StepOverPlanner::PolyPlannerFirstStep(
ZfootSpeedBound(0) = 0.0;
ZfootSpeedBound(1) = 0.0;
int NumberIntermediate = 0, NumberIntermediate2 = 0, Counter = 0,
CounterTemp = 0;
int NumberIntermediate = 0, Counter = 0, CounterTemp = 0;
double IntermediateTimeStep;
NumberIntermediate = 10;
NumberIntermediate2 = 20;
ZfootPos.resize(2 + 3 * NumberIntermediate);
TimeIntervalsZ.resize(2 + 3 * NumberIntermediate);
......@@ -1101,8 +1098,8 @@ void StepOverPlanner::PolyPlannerSecondStep(
double StepLenght;
double Omega1, Omega2, OmegaImpact;
double xOffset;
double Point1X, Point1Y, Point1Z;
double Point2X, Point2Y, Point2Z;
double Point1X, Point1Z;
double Point2X, Point2Z;
double Point3Z;
StepTime = aStepOverFootBuffer[m_EndStepOver].time -
......@@ -1118,12 +1115,10 @@ void StepOverPlanner::PolyPlannerSecondStep(
Point1X = m_StepOverStepLenght - m_heelToAnkle - m_ObstacleParameters.d -
xOffset - m_tipToAnkle * cos(Omega1 * M_PI / 180.0);
Point1Y = 0.0;
Point1Z = m_ObstacleParameters.h + m_tipToAnkle * sin(Omega1 * M_PI / 180.0);
Point2X = m_StepOverStepLenght - m_heelToAnkle + xOffset +
m_heelToAnkle * cos(Omega2 * M_PI / 180.0);
Point2Y = 0.0;
Point2Z = Point1Z;
// m_ObstacleParameters.h+0.04;//-m_tipToAnkle*sin(Omega2*M_PI/180.0);
......@@ -1687,8 +1682,6 @@ void StepOverPlanner::CreateBufferFirstPreview(
void StepOverPlanner::m_SetObstacleParameters(istringstream &strm) {
bool ReadObstacleParameters = false;
ODEBUG("I am reading the obstacle parameters"
<< " ");
......@@ -1738,7 +1731,6 @@ void StepOverPlanner::m_SetObstacleParameters(istringstream &strm) {
if (!strm.eof()) {
bool lObstacleDetected;
strm >> lObstacleDetected;
ReadObstacleParameters = true;
break;
} else {
cout << "Not enough inputs for completion of "
......
......@@ -93,10 +93,8 @@ void UpperBodyMotion::ReadDataFile(string aFileName,
std::ifstream aif;
unsigned int NumberRows, NumberColumns;
NumberRows = UpperBodyAngles.rows();
NumberColumns = UpperBodyAngles.cols();
Eigen::MatrixXd::Index NumberRows = UpperBodyAngles.rows();
Eigen::MatrixXd::Index NumberColumns = UpperBodyAngles.cols();
double r;
......@@ -116,9 +114,8 @@ void UpperBodyMotion::ReadDataFile(string aFileName,
void UpperBodyMotion::WriteDataFile(string aFileName,
Eigen::MatrixXd &UpperBodyAngles) {
ofstream aof;
unsigned int NumberRows, NumberColumns;
NumberRows = UpperBodyAngles.rows();
NumberColumns = UpperBodyAngles.cols();
Eigen::MatrixXd::Index NumberRows = UpperBodyAngles.rows();
Eigen::MatrixXd::Index NumberColumns = UpperBodyAngles.cols();
aof.open(aFileName.c_str(), ofstream::out);
if (aof.is_open()) {
......
......@@ -201,9 +201,10 @@ int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl(
<< aRightFAP.x << " " << aRightFAP.y << " " << aRightFAP.z,
"1ststage.dat");
CallToComAndFootRealization(acompos, aLeftFAP, aRightFAP,
CurrentConfiguration, CurrentVelocity,
CurrentAcceleration, m_NumberOfIterations, 0);
int StageOfTheAlgorithm = 0;
CallToComAndFootRealization(
acompos, aLeftFAP, aRightFAP, CurrentConfiguration, CurrentVelocity,
CurrentAcceleration, m_NumberOfIterations, StageOfTheAlgorithm);
if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY)
EvaluateMultiBodyZMP(-1);
......@@ -222,9 +223,11 @@ int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl(
"2ndStage.dat");
if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY) {
int StageOfTheAlgorithm = 1;
CallToComAndFootRealization(
refandfinalCOMState, aLeftFAP, aRightFAP, CurrentConfiguration,
CurrentVelocity, CurrentAcceleration, m_NumberOfIterations - m_NL, 1);
CurrentVelocity, CurrentAcceleration, m_NumberOfIterations - m_NL,
StageOfTheAlgorithm);
}
// Here it is assumed that the 4x4 CoM matrix
......@@ -277,7 +280,6 @@ COMState ZMPPreviewControlWithMultiBodyZMP::GetLastCOMFromFirstStage() {
int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
COMState &finalCOMState) {
double Deltazmpx2, Deltazmpy2;
// Inverse Kinematics variables.
COMState aCOMState = m_FIFOCOMStates[0];
......@@ -286,17 +288,18 @@ int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
LeftFootPosition = m_FIFOLeftFootPosition[0];
RightFootPosition = m_FIFORightFootPosition[0];
#if 0
double Deltazmpx2, Deltazmpy2;
// Preview control on delta ZMP.
if ((m_StageStrategy == ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY) ||
(m_StageStrategy == ZMPCOM_TRAJECTORY_FULL)) {
ODEBUG2(m_FIFODeltaZMPPositions[0].px << " "
<< m_FIFODeltaZMPPositions[0].py);
<< m_FIFODeltaZMPPositions[0].py);
ODEBUG("Second Stage Size of FIFODeltaZMPPositions: "
<< m_FIFODeltaZMPPositions.size() << " " << m_Deltax << " "
<< m_Deltay << " " << m_sxDeltazmp << " " << m_syDeltazmp << " "
<< Deltazmpx2 << " " << Deltazmpy2);
<< m_FIFODeltaZMPPositions.size() << " " << m_Deltax << " "
<< m_Deltay << " " << m_sxDeltazmp << " " << m_syDeltazmp << " "
<< Deltazmpx2 << " " << Deltazmpy2);
m_PC->OneIterationOfPreview(m_Deltax, m_Deltay, m_sxDeltazmp, m_syDeltazmp,
m_FIFODeltaZMPPositions, 0, Deltazmpx2,
......@@ -309,7 +312,6 @@ int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
aCOMState.y[i] += m_Deltay(i, 0);
}
}
#endif
ODEBUG2("Delta :" << m_Deltax(0, 0) << " " << m_Deltay(0, 0) << " "
<< aCOMState.x[0] << " " << aCOMState.y[0]);
......@@ -452,7 +454,7 @@ int ZMPPreviewControlWithMultiBodyZMP::Setup(
}
int ZMPPreviewControlWithMultiBodyZMP::SetupFirstPhase(
deque<ZMPPosition> &ZMPRefPositions, deque<COMState> &COMStates,
deque<ZMPPosition> &ZMPRefPositions, deque<COMState> &,
deque<FootAbsolutePosition> &LeftFootPositions,
deque<FootAbsolutePosition> &RightFootPositions) {
ODEBUG6("Beginning of Setup 0 ", "DebugData.txt");
......@@ -558,10 +560,12 @@ int ZMPPreviewControlWithMultiBodyZMP::SetupIterativePhase(
<< aRightFAP.x << " " << aRightFAP.y << " " << aRightFAP.z,
"ZMPPCWMZOGSOC.dat");
int StageOfTheAlgorithm = 0;
CallToComAndFootRealization(
m_FIFOCOMStates[localindex], m_FIFORightFootPosition[localindex],
m_FIFOLeftFootPosition[localindex], CurrentConfiguration, CurrentVelocity,
CurrentAcceleration, m_NumberOfIterations, 0);
m_FIFOCOMStates[localindex], m_FIFOLeftFootPosition[localindex],
m_FIFORightFootPosition[localindex], CurrentConfiguration,
CurrentVelocity, CurrentAcceleration, m_NumberOfIterations,
StageOfTheAlgorithm);
EvaluateMultiBodyZMP(localindex);
......
......@@ -114,12 +114,12 @@ bool SimplePluginManager::CallMethod(string &MethodName, istringstream &istrm) {
stringbuf *pbuf;
pbuf = istrm.rdbuf();
int size = pbuf->in_avail();
std::streamsize size = pbuf->in_avail();
char aBuffer[65636];
assert(size < 65635);
memset(aBuffer, 0, size + 1);
for (int i = 0; i < size; i++)
for (std::streamsize i = 0; i < size; i++)
aBuffer[i] = (char)pbuf->sbumpc();
ODEBUG5(aBuffer, "PgDebug.txt");
......
......@@ -1237,7 +1237,7 @@ int ZMPConstrainedQPFastFormulation::BuildZMPTrajectoryFromFootTrajectory(
// Compute CPU consumption time.
gettimeofday(&end, 0);
CurrentCPUTime = end.tv_sec - start.tv_sec +
CurrentCPUTime = (double)(end.tv_sec - start.tv_sec) +
0.000001 * (double)(end.tv_usec - start.tv_usec);
TotalAmountOfCPUTime += CurrentCPUTime;
ODEBUG("Current Time : "
......
......@@ -198,7 +198,6 @@ void GeneratorVelRef::initialize_matrices() {
initialize_matrices(IneqCoM);
IntermedQPMat::state_variant_t &State = IntermedData_->State();
bool Preserve = true;
State.VcshiftX.resize(N_);
State.VcshiftX.setZero();
State.VcshiftY.resize(N_);
......
......@@ -221,7 +221,7 @@ void linear_inequality_t::clear() {
Dc_vec.setZero();
}
void linear_inequality_t::resize(int NbRows, int NbCols, bool Preserve) {
void linear_inequality_t::resize(int NbRows, int NbCols, bool) {
D.X_mat.resize(NbRows, NbCols);
D.Y_mat.resize(NbRows, NbCols);
......
......@@ -167,8 +167,11 @@ IF(BUILD_TESTING)
ADD_JRL_WALKGEN_TEST(TestKajita2003StraightWalking TestKajita2003.cpp)
ADD_JRL_WALKGEN_TEST(TestKajita2003Circle TestKajita2003.cpp)
ADD_JRL_WALKGEN_TEST(TestKajita2003PbFlorentSeq1 TestKajita2003.cpp)
ADD_JRL_WALKGEN_TEST(TestKajita2003PbFlorentSeq2 TestKajita2003.cpp)
ADD_JRL_WALKGEN_TEST(TestKajita2003WalkingOnSpot TestKajita2003.cpp)
IF(FULL_BUILD_TESTING)
ADD_JRL_WALKGEN_TEST(TestKajita2003PbFlorentSeq2 TestKajita2003.cpp)
ENDIF(FULL_BUILD_TESTING)
ENDIF(BUILD_TESTING)
##########################
......@@ -184,11 +187,13 @@ ENDIF(BUILD_TESTING)
# Disabled as the test fail : random results oscillating around mean behaviour
IF(BUILD_TESTING)
ADD_JRL_WALKGEN_TEST(TestNaveau2015Online TestNaveau2015.cpp)
SET_TESTS_PROPERTIES("TestNaveau2015Online${BITS}" PROPERTIES TIMEOUT 7200)
ADD_JRL_WALKGEN_TEST(TestNaveau2015OnlineSimple TestNaveau2015.cpp)
IF (FULL_BUILD_TESTING)
ADD_JRL_WALKGEN_TEST(TestNaveau2015Online TestNaveau2015.cpp)
SET_TESTS_PROPERTIES("TestNaveau2015Online${BITS}" PROPERTIES TIMEOUT 7200)
ENDIF(FULL_BUILD_TESTING)
ENDIF(BUILD_TESTING)
#ADD_JRL_WALKGEN_EXE(TestNaveau2015OnlineSimple TestNaveau2015.cpp)
#####################
# Add user examples #
......
......@@ -396,7 +396,7 @@ void DumpReferencesObjects::fillInTestsFormat2(
prepareFile(aof, prefix, anOneStep);
Eigen::VectorXd &aTauVec = anOneStep.m_DebugPR->currentTau();
vec_db.resize(aTauVec.size());
if (m_prevTorquesF2.size() != aTauVec.size())
if (m_prevTorquesF2.size() != (std::vector<double>::size_type)aTauVec.size())
m_prevTorquesF2.resize(aTauVec.size());
for (Eigen::VectorXd::Index i = 0; i < aTauVec.size(); i++)
......
......@@ -145,25 +145,6 @@ protected:
aPGI.ParseCmd(strm2);
}
// {
// istringstream strm2(":stepseq 0.0 -0.09 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.0 0.18 0.0 0.0");
// aPGI.ParseCmd(strm2);
// }
{
istringstream strm2(":singlesupporttime 0.9");
aPGI.ParseCmd(strm2);
......
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