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
sot-hrp2
Commits
357b1467
Commit
357b1467
authored
Dec 12, 2014
by
olivier stasse
Browse files
Merge pull request #10 from andreadelprete/master
Increment time of accelerometer, gyrometer and robotState output signals
parents
6ec2f2a1
078151f2
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/sot-hrp2-device.cpp
View file @
357b1467
...
...
@@ -80,68 +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.
;
updateRobotState
(
anglesIn
);
}
{
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_
);
}
{
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_
);
}
{
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_
);
}
{
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
);
}
...
...
@@ -151,7 +157,6 @@ void SoTHRP2Device::setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
setSensors
(
SensorsIn
);
}
void
SoTHRP2Device
::
nominalSetSensors
(
map
<
string
,
dgsot
::
SensorValues
>
&
SensorsIn
)
{
setSensors
(
SensorsIn
);
...
...
@@ -212,13 +217,3 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
controlOut
[
"baseff"
].
setValues
(
baseff_
);
sotDEBUGOUT
(
25
)
;
}
void
SoTHRP2Device
::
updateRobotState
(
const
vector
<
double
>
&
anglesIn
)
{
sotDEBUGIN
(
25
)
;
for
(
unsigned
i
=
0
;
i
<
anglesIn
.
size
();
++
i
)
mlRobotState
(
i
+
6
)
=
anglesIn
[
i
];
robotState_
.
setConstant
(
mlRobotState
);
sotDEBUGOUT
(
25
)
;
}
src/sot-hrp2-device.hh
View file @
357b1467
...
...
@@ -55,9 +55,6 @@ public:
void
getControl
(
std
::
map
<
std
::
string
,
dgsot
::
ControlValues
>
&
anglesOut
);
protected:
// Update output port with the control computed from the
// dynamic graph.
void
updateRobotState
(
const
std
::
vector
<
double
>
&
anglesIn
);
/// \brief Current integration step.
double
timestep_
;
...
...
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