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
5277d15d
Commit
5277d15d
authored
Jun 15, 2015
by
olivier stasse
Browse files
Merge pull request #12 from andreadelprete/master
Add output signals to device for motor currents and PD gains
parents
6f3830c5
125c59f0
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/sot-hrp2-device.cpp
View file @
5277d15d
...
...
@@ -38,6 +38,9 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
accelerometerSOUT_
(
"StackOfTasks("
+
RobotName
+
")::output(vector)::accelerometer"
),
gyrometerSOUT_
(
"StackOfTasks("
+
RobotName
+
")::output(vector)::gyrometer"
),
currentSOUT_
(
"StackOfTasks("
+
RobotName
+
")::output(vector)::currents"
),
p_gainsSOUT_
(
"StackOfTasks("
+
RobotName
+
")::output(vector)::p_gains"
),
d_gainsSOUT_
(
"StackOfTasks("
+
RobotName
+
")::output(vector)::d_gains"
),
mlforces
(
6
),
pose
(),
accelerometer_
(
3
),
...
...
@@ -47,7 +50,8 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
{
sotDEBUGIN
(
25
)
;
for
(
int
i
=
0
;
i
<
4
;
++
i
)
{
withForceSignals
[
i
]
=
true
;
}
signalRegistration
(
robotState_
<<
accelerometerSOUT_
<<
gyrometerSOUT_
);
signalRegistration
(
robotState_
<<
accelerometerSOUT_
<<
gyrometerSOUT_
<<
currentSOUT_
<<
p_gainsSOUT_
<<
d_gainsSOUT_
);
ml
::
Vector
data
(
3
);
data
.
setZero
();
accelerometerSOUT_
.
setConstant
(
data
);
gyrometerSOUT_
.
setConstant
(
data
);
...
...
@@ -149,6 +153,39 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
pseudoTorqueSOUT
.
setTime
(
t
);
}
it
=
SensorsIn
.
find
(
"currents"
);
if
(
it
!=
SensorsIn
.
end
())
{
const
std
::
vector
<
double
>&
currents
=
SensorsIn
[
"currents"
].
getValues
();
currents_
.
resize
(
currents
.
size
());
for
(
std
::
size_t
i
=
0
;
i
<
currents
.
size
();
++
i
)
currents_
(
i
)
=
currents
[
i
];
currentSOUT_
.
setConstant
(
currents_
);
currentSOUT_
.
setTime
(
t
);
}
it
=
SensorsIn
.
find
(
"p_gains"
);
if
(
it
!=
SensorsIn
.
end
())
{
const
std
::
vector
<
double
>&
p_gains
=
SensorsIn
[
"p_gains"
].
getValues
();
p_gains_
.
resize
(
p_gains
.
size
());
for
(
std
::
size_t
i
=
0
;
i
<
p_gains
.
size
();
++
i
)
p_gains_
(
i
)
=
p_gains
[
i
];
p_gainsSOUT_
.
setConstant
(
p_gains_
);
p_gainsSOUT_
.
setTime
(
t
);
}
it
=
SensorsIn
.
find
(
"d_gains"
);
if
(
it
!=
SensorsIn
.
end
())
{
const
std
::
vector
<
double
>&
d_gains
=
SensorsIn
[
"d_gains"
].
getValues
();
d_gains_
.
resize
(
d_gains
.
size
());
for
(
std
::
size_t
i
=
0
;
i
<
d_gains
.
size
();
++
i
)
d_gains_
(
i
)
=
d_gains
[
i
];
d_gainsSOUT_
.
setConstant
(
d_gains_
);
d_gainsSOUT_
.
setTime
(
t
);
}
sotDEBUGOUT
(
25
);
}
...
...
src/sot-hrp2-device.hh
View file @
5277d15d
...
...
@@ -74,6 +74,12 @@ protected:
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
accelerometerSOUT_
;
/// Rotation velocity measured by gyrometers
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
gyrometerSOUT_
;
/// motor currents
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
currentSOUT_
;
/// proportional and derivative position-control gains
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
p_gainsSOUT_
;
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
d_gainsSOUT_
;
/// Intermediate variables to avoid allocation during control
ml
::
Vector
mlforces
;
...
...
@@ -83,5 +89,8 @@ protected:
ml
::
Vector
gyrometer_
;
std
::
vector
<
double
>
baseff_
;
ml
::
Vector
torques_
;
ml
::
Vector
currents_
;
ml
::
Vector
p_gains_
;
ml
::
Vector
d_gains_
;
};
#endif
/* _SOT_HRP2Device_H_*/
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