Commit e0df8fb0 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Format]

parent 656554c7
Pipeline #9426 passed with stage
in 22 minutes and 4 seconds
......@@ -43,13 +43,9 @@ struct PinocchioRobotFoot_t {
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) {}
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 = (double)startingtime.tv_sec + 0.000001 * (double)startingtime.tv_usec;
m_StartingTime =
(double)startingtime.tv_sec + 0.000001 * (double)startingtime.tv_usec;
}
Clock::~Clock() {}
......@@ -58,23 +59,24 @@ void Clock::Reset() {
struct timeval startingtime;
gettimeofday(&startingtime, 0);
m_StartingTime = (double)startingtime.tv_sec + 0.000001 * (double)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 = (double)m_EndTimeStamp.tv_sec - (double)m_BeginTimeStamp.tv_sec +
0.000001 * (double)(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] =
(double)m_BeginTimeStamp.tv_sec +
0.000001 * (double)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());
......
......@@ -348,8 +348,7 @@ int PLDPSolver::BackwardSubstitution() {
for (std::vector<unsigned int>::size_type i = SizeOfL - 1;; i--) {
double tmp = 0.0;
m_v2[i] = m_y[i];
for (std::vector<unsigned int>::size_type k = i + 1;
k < SizeOfL; k++) {
for (std::vector<unsigned int>::size_type k = i + 1; k < SizeOfL; k++) {
if (k == SizeOfL - 1)
tmp = m_v2[i];
......@@ -361,7 +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;
if (i == 0)
break;
}
return 0;
}
......
......@@ -290,7 +290,7 @@ void GenerateMotionFromKineoWorks::ComputeUpperBodyPosition(
//! For each way-point of the path
for (unsigned int IdWayPoint = 1; IdWayPoint < m_Path.size(); IdWayPoint++) {
int CountTarget = -1;
double lX = 0.0, lY=0.0, lZ=0.0, dist = 1000000.0;
double lX = 0.0, lY = 0.0, lZ = 0.0, dist = 1000000.0;
//! The references are specific to the current hybrid model.
lX = m_Path[IdWayPoint].Joints[6];
......@@ -301,9 +301,9 @@ void GenerateMotionFromKineoWorks::ComputeUpperBodyPosition(
// 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]);
(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;
CountTarget = i;
......
......@@ -841,8 +841,7 @@ void StepOverPlanner::PolyPlannerFirstStep(
ZfootSpeedBound(0) = 0.0;
ZfootSpeedBound(1) = 0.0;
int NumberIntermediate = 0, Counter = 0,
CounterTemp = 0;
int NumberIntermediate = 0, Counter = 0, CounterTemp = 0;
double IntermediateTimeStep;
NumberIntermediate = 10;
......
......@@ -170,10 +170,10 @@ void ZMPPreviewControlWithMultiBodyZMP::CallToComAndFootRealization(
if (StageOfTheAlgorithm == 0) {
/* Update the current configuration vector */
m_PinocchioRobot->currentRPYConfiguration(CurrentConfiguration);
/* Update the current velocity vector */
m_PinocchioRobot->currentRPYVelocity(CurrentVelocity);
/* Update the current acceleration vector */
m_PinocchioRobot->currentRPYAcceleration(CurrentAcceleration);
}
......@@ -201,11 +201,10 @@ int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl(
<< aRightFAP.x << " " << aRightFAP.y << " " << aRightFAP.z,
"1ststage.dat");
int StageOfTheAlgorithm=0;
CallToComAndFootRealization(acompos, aLeftFAP, aRightFAP,
CurrentConfiguration, CurrentVelocity,
CurrentAcceleration, m_NumberOfIterations,
StageOfTheAlgorithm);
int StageOfTheAlgorithm = 0;
CallToComAndFootRealization(
acompos, aLeftFAP, aRightFAP, CurrentConfiguration, CurrentVelocity,
CurrentAcceleration, m_NumberOfIterations, StageOfTheAlgorithm);
if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY)
EvaluateMultiBodyZMP(-1);
......@@ -224,11 +223,11 @@ int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl(
"2ndStage.dat");
if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY) {
int StageOfTheAlgorithm=1;
int StageOfTheAlgorithm = 1;
CallToComAndFootRealization(
refandfinalCOMState, aLeftFAP, aRightFAP, CurrentConfiguration,
CurrentVelocity, CurrentAcceleration, m_NumberOfIterations - m_NL,
StageOfTheAlgorithm);
StageOfTheAlgorithm);
}
// Here it is assumed that the 4x4 CoM matrix
......@@ -290,17 +289,17 @@ int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
RightFootPosition = m_FIFORightFootPosition[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,
......@@ -561,11 +560,11 @@ int ZMPPreviewControlWithMultiBodyZMP::SetupIterativePhase(
<< aRightFAP.x << " " << aRightFAP.y << " " << aRightFAP.z,
"ZMPPCWMZOGSOC.dat");
int StageOfTheAlgorithm=0;
int StageOfTheAlgorithm = 0;
CallToComAndFootRealization(
m_FIFOCOMStates[localindex], m_FIFOLeftFootPosition[localindex],
m_FIFORightFootPosition[localindex], CurrentConfiguration, CurrentVelocity,
CurrentAcceleration, m_NumberOfIterations,
m_FIFORightFootPosition[localindex], CurrentConfiguration,
CurrentVelocity, CurrentAcceleration, m_NumberOfIterations,
StageOfTheAlgorithm);
EvaluateMultiBodyZMP(localindex);
......
......@@ -114,7 +114,7 @@ bool SimplePluginManager::CallMethod(string &MethodName, istringstream &istrm) {
stringbuf *pbuf;
pbuf = istrm.rdbuf();
std::streamsize size = pbuf->in_avail();
std::streamsize size = pbuf->in_avail();
char aBuffer[65636];
assert(size < 65635);
......
......@@ -221,7 +221,7 @@ void linear_inequality_t::clear() {
Dc_vec.setZero();
}
void linear_inequality_t::resize(int NbRows, int NbCols, bool ) {
void linear_inequality_t::resize(int NbRows, int NbCols, bool) {
D.X_mat.resize(NbRows, NbCols);
D.Y_mat.resize(NbRows, NbCols);
......
Markdown is supported
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