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
Guilhem Saurel
sot-hrp2
Commits
078151f2
Commit
078151f2
authored
Dec 12, 2014
by
andreadelprete
Browse files
Fix indentation
parent
f050ffa9
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/sot-hrp2-device.cpp
View file @
078151f2
...
...
@@ -80,74 +80,74 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
it
=
SensorsIn
.
find
(
"forces"
);
if
(
it
!=
SensorsIn
.
end
())
{
{
// Implements force recollection.
const
vector
<
double
>&
forcesIn
=
it
->
second
.
getValues
();
for
(
int
i
=
0
;
i
<
4
;
++
i
)
{
for
(
int
j
=
0
;
j
<
6
;
++
j
)
mlforces
(
j
)
=
forcesIn
[
i
*
6
+
j
];
forcesSOUT
[
i
]
->
setConstant
(
mlforces
);
forcesSOUT
[
i
]
->
setTime
(
t
);
}
// Implements force recollection.
const
vector
<
double
>&
forcesIn
=
it
->
second
.
getValues
();
for
(
int
i
=
0
;
i
<
4
;
++
i
)
{
for
(
int
j
=
0
;
j
<
6
;
++
j
)
mlforces
(
j
)
=
forcesIn
[
i
*
6
+
j
];
forcesSOUT
[
i
]
->
setConstant
(
mlforces
);
forcesSOUT
[
i
]
->
setTime
(
t
);
}
}
it
=
SensorsIn
.
find
(
"attitude"
);
if
(
it
!=
SensorsIn
.
end
())
{
const
vector
<
double
>&
attitude
=
it
->
second
.
getValues
();
for
(
unsigned
int
i
=
0
;
i
<
3
;
++
i
)
for
(
unsigned
int
j
=
0
;
j
<
3
;
++
j
)
pose
(
i
,
j
)
=
attitude
[
i
*
3
+
j
];
attitudeSOUT
.
setConstant
(
pose
);
attitudeSOUT
.
setTime
(
t
);
}
{
const
vector
<
double
>&
attitude
=
it
->
second
.
getValues
();
for
(
unsigned
int
i
=
0
;
i
<
3
;
++
i
)
for
(
unsigned
int
j
=
0
;
j
<
3
;
++
j
)
pose
(
i
,
j
)
=
attitude
[
i
*
3
+
j
];
attitudeSOUT
.
setConstant
(
pose
);
attitudeSOUT
.
setTime
(
t
);
}
it
=
SensorsIn
.
find
(
"joints"
);
if
(
it
!=
SensorsIn
.
end
())
{
const
vector
<
double
>&
anglesIn
=
it
->
second
.
getValues
();
mlRobotState
.
resize
(
anglesIn
.
size
()
+
6
);
for
(
unsigned
i
=
0
;
i
<
6
;
++
i
)
mlRobotState
(
i
)
=
0.
;
for
(
unsigned
i
=
0
;
i
<
anglesIn
.
size
();
++
i
)
mlRobotState
(
i
+
6
)
=
anglesIn
[
i
];
robotState_
.
setConstant
(
mlRobotState
);
robotState_
.
setTime
(
t
);
}
{
const
vector
<
double
>&
anglesIn
=
it
->
second
.
getValues
();
mlRobotState
.
resize
(
anglesIn
.
size
()
+
6
);
for
(
unsigned
i
=
0
;
i
<
6
;
++
i
)
mlRobotState
(
i
)
=
0.
;
for
(
unsigned
i
=
0
;
i
<
anglesIn
.
size
();
++
i
)
mlRobotState
(
i
+
6
)
=
anglesIn
[
i
];
robotState_
.
setConstant
(
mlRobotState
);
robotState_
.
setTime
(
t
);
}
it
=
SensorsIn
.
find
(
"accelerometer_0"
);
if
(
it
!=
SensorsIn
.
end
())
{
const
vector
<
double
>&
accelerometer
=
SensorsIn
[
"accelerometer_0"
].
getValues
();
for
(
std
::
size_t
i
=
0
;
i
<
3
;
++
i
)
accelerometer_
(
i
)
=
accelerometer
[
i
];
accelerometerSOUT_
.
setConstant
(
accelerometer_
);
accelerometerSOUT_
.
setTime
(
t
);
}
{
const
vector
<
double
>&
accelerometer
=
SensorsIn
[
"accelerometer_0"
].
getValues
();
for
(
std
::
size_t
i
=
0
;
i
<
3
;
++
i
)
accelerometer_
(
i
)
=
accelerometer
[
i
];
accelerometerSOUT_
.
setConstant
(
accelerometer_
);
accelerometerSOUT_
.
setTime
(
t
);
}
it
=
SensorsIn
.
find
(
"gyrometer_0"
);
if
(
it
!=
SensorsIn
.
end
())
{
const
vector
<
double
>&
gyrometer
=
SensorsIn
[
"gyrometer_0"
].
getValues
();
for
(
std
::
size_t
i
=
0
;
i
<
3
;
++
i
)
gyrometer_
(
i
)
=
gyrometer
[
i
];
gyrometerSOUT_
.
setConstant
(
gyrometer_
);
gyrometerSOUT_
.
setTime
(
t
);
}
{
const
vector
<
double
>&
gyrometer
=
SensorsIn
[
"gyrometer_0"
].
getValues
();
for
(
std
::
size_t
i
=
0
;
i
<
3
;
++
i
)
gyrometer_
(
i
)
=
gyrometer
[
i
];
gyrometerSOUT_
.
setConstant
(
gyrometer_
);
gyrometerSOUT_
.
setTime
(
t
);
}
it
=
SensorsIn
.
find
(
"torques"
);
if
(
it
!=
SensorsIn
.
end
())
{
const
std
::
vector
<
double
>&
torques
=
SensorsIn
[
"torques"
].
getValues
();
torques_
.
resize
(
torques
.
size
());
for
(
std
::
size_t
i
=
0
;
i
<
torques
.
size
();
++
i
)
torques_
(
i
)
=
torques
[
i
];
pseudoTorqueSOUT
.
setConstant
(
torques_
);
pseudoTorqueSOUT
.
setTime
(
t
);
}
{
const
std
::
vector
<
double
>&
torques
=
SensorsIn
[
"torques"
].
getValues
();
torques_
.
resize
(
torques
.
size
());
for
(
std
::
size_t
i
=
0
;
i
<
torques
.
size
();
++
i
)
torques_
(
i
)
=
torques
[
i
];
pseudoTorqueSOUT
.
setConstant
(
torques_
);
pseudoTorqueSOUT
.
setTime
(
t
);
}
sotDEBUGOUT
(
25
);
}
...
...
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