Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
jrl-walkgen
Commits
aafa0766
Commit
aafa0766
authored
Apr 25, 2020
by
Olivier Stasse
Committed by
Guilhem Saurel
Apr 30, 2020
Browse files
Fix warnings
parent
407e6514
Changes
15
Hide whitespace changes
Inline
Side-by-side
include/jrl/walkgen/pinocchiorobot.hh
View file @
aafa0766
...
...
@@ -42,6 +42,14 @@ 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
;
...
...
src/Clock.cpp
View file @
aafa0766
...
...
@@ -46,7 +46,7 @@ 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 +58,23 @@ 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
;
}
...
...
src/Mathematics/PLDPSolver.cpp
View file @
aafa0766
...
...
@@ -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
();
auto
SizeOfL
=
m_ActivatedConstraints
.
size
();
if
(
SizeOfL
==
0
)
return
0
;
ODEBUG
(
"BackwardSubstitution "
<<
m_ItNb
);
for
(
int
i
=
SizeOfL
-
1
;
i
>=
0
;
i
--
)
{
for
(
auto
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
(
auto
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,7 @@ 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 +791,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
;
}
...
...
src/Mathematics/qld.cpp
View file @
aafa0766
...
...
@@ -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
;
...
...
src/MotionGeneration/GenerateMotionFromKineoWorks.cpp
View file @
aafa0766
...
...
@@ -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
];
...
...
@@ -300,12 +300,10 @@ 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
;
CountTarget
=
i
;
...
...
src/MotionGeneration/StepOverPlanner.cpp
View file @
aafa0766
...
...
@@ -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,11 @@ void StepOverPlanner::PolyPlannerFirstStep(
ZfootSpeedBound
(
0
)
=
0.0
;
ZfootSpeedBound
(
1
)
=
0.0
;
int
NumberIntermediate
=
0
,
NumberIntermediate2
=
0
,
Counter
=
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 +1099,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 +1116,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 +1683,6 @@ void StepOverPlanner::CreateBufferFirstPreview(
void
StepOverPlanner
::
m_SetObstacleParameters
(
istringstream
&
strm
)
{
bool
ReadObstacleParameters
=
false
;
ODEBUG
(
"I am reading the obstacle parameters"
<<
" "
);
...
...
@@ -1738,7 +1732,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 "
...
...
src/MotionGeneration/UpperBodyMotion.cpp
View file @
aafa0766
...
...
@@ -93,10 +93,8 @@ void UpperBodyMotion::ReadDataFile(string aFileName,
std
::
ifstream
aif
;
unsigned
int
NumberRows
,
NumberColumns
;
NumberRows
=
UpperBodyAngles
.
rows
();
NumberColumns
=
UpperBodyAngles
.
cols
();
auto
NumberRows
=
UpperBodyAngles
.
rows
();
auto
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
();
auto
NumberRows
=
UpperBodyAngles
.
rows
();
auto
NumberColumns
=
UpperBodyAngles
.
cols
();
aof
.
open
(
aFileName
.
c_str
(),
ofstream
::
out
);
if
(
aof
.
is_open
())
{
...
...
src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
View file @
aafa0766
...
...
@@ -277,7 +277,6 @@ COMState ZMPPreviewControlWithMultiBodyZMP::GetLastCOMFromFirstStage() {
int
ZMPPreviewControlWithMultiBodyZMP
::
SecondStageOfControl
(
COMState
&
finalCOMState
)
{
double
Deltazmpx2
,
Deltazmpy2
;
// Inverse Kinematics variables.
COMState
aCOMState
=
m_FIFOCOMStates
[
0
];
...
...
@@ -286,7 +285,8 @@ 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
))
{
...
...
@@ -309,7 +309,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 +451,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"
);
...
...
src/SimplePluginManager.cpp
View file @
aafa0766
...
...
@@ -114,7 +114,7 @@ bool SimplePluginManager::CallMethod(string &MethodName, istringstream &istrm) {
stringbuf
*
pbuf
;
pbuf
=
istrm
.
rdbuf
();
int
size
=
pbuf
->
in_avail
();
auto
size
=
pbuf
->
in_avail
();
char
aBuffer
[
65636
];
assert
(
size
<
65635
);
...
...
src/ZMPRefTrajectoryGeneration/ZMPConstrainedQPFastFormulation.cpp
View file @
aafa0766
...
...
@@ -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 : "
...
...
src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
View file @
aafa0766
...
...
@@ -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_
);
...
...
src/privatepgtypes.cpp
View file @
aafa0766
...
...
@@ -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
);
...
...
tests/DumpReferencesObjects.cpp
View file @
aafa0766
...
...
@@ -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
++
)
...
...
tests/TestKajita2003.cpp
View file @
aafa0766
...
...
@@ -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
);
...
...
tests/TestNaveau2015Online
s
imple64TestFGPI.datref.cmake
→
tests/TestNaveau2015Online
S
imple64TestFGPI.datref.cmake
View file @
aafa0766
File moved
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment