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
19496ce2
Commit
19496ce2
authored
Aug 21, 2017
by
Rohan Budhiraja
Committed by
GitHub
Aug 21, 2017
Browse files
Merge pull request #15 from proyan/master
[sot-hrp2-device]
parents
f0c2869c
099af79b
Changes
4
Hide whitespace changes
Inline
Side-by-side
src/sot-hrp2-controller.cpp
View file @
19496ce2
...
...
@@ -76,6 +76,12 @@ setSecondOrderIntegration(void)
device_
.
setSecondOrderIntegration
();
}
void
SoTHRP2Controller
::
setNoIntegration
(
void
)
{
device_
.
setNoIntegration
();
}
void
SoTHRP2Controller
::
nominalSetSensors
(
map
<
string
,
dgsot
::
SensorValues
>
&
SensorsIn
)
{
...
...
src/sot-hrp2-controller.hh
View file @
19496ce2
...
...
@@ -49,6 +49,7 @@ class SoTHRP2Controller: public
void
getControl
(
std
::
map
<
std
::
string
,
dgsot
::
ControlValues
>
&
anglesOut
);
void
setSecondOrderIntegration
(
void
);
void
setNoIntegration
(
void
);
/// Embedded python interpreter accessible via Corba
boost
::
shared_ptr
<
dynamicgraph
::
Interpreter
>
interpreter_
;
...
...
src/sot-hrp2-device.cpp
View file @
19496ce2
...
...
@@ -34,7 +34,6 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
dgsot
::
Device
(
RobotName
),
timestep_
(
TIMESTEP_DEFAULT
),
previousState_
(),
robotState_
(
"StackOfTasks("
+
RobotName
+
")::output(vector)::robotState"
),
accelerometerSOUT_
(
"StackOfTasks("
+
RobotName
+
")::output(vector)::accelerometer"
),
gyrometerSOUT_
(
"StackOfTasks("
+
RobotName
+
")::output(vector)::gyrometer"
),
...
...
@@ -50,7 +49,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
{
sotDEBUGIN
(
25
)
;
for
(
int
i
=
0
;
i
<
4
;
++
i
)
{
withForceSignals
[
i
]
=
true
;
}
signalRegistration
(
robotState_
<<
accelerometerSOUT_
<<
gyrometerSOUT_
signalRegistration
(
accelerometerSOUT_
<<
gyrometerSOUT_
<<
currentSOUT_
<<
p_gainsSOUT_
<<
d_gainsSOUT_
);
dg
::
Vector
data
(
3
);
data
.
setZero
();
accelerometerSOUT_
.
setConstant
(
data
);
...
...
@@ -138,6 +137,36 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
robotState_
.
setTime
(
t
);
}
it
=
SensorsIn
.
find
(
"base_velocity"
);
bool
ffVelSensorFound_
=
false
;
Eigen
::
VectorXd
base_v
(
6
);
if
(
it
!=
SensorsIn
.
end
())
{
ffVelSensorFound_
=
true
;
const
vector
<
double
>&
basevel
=
it
->
second
.
getValues
();
base_v
<<
basevel
[
0
],
basevel
[
1
],
basevel
[
2
],
basevel
[
3
],
basevel
[
4
],
basevel
[
5
];
}
it
=
SensorsIn
.
find
(
"joint_velocities"
);
if
(
it
!=
SensorsIn
.
end
())
{
const
vector
<
double
>&
velIn
=
it
->
second
.
getValues
();
robotVelocity
.
resize
(
velIn
.
size
()
+
6
);
if
(
ffVelSensorFound_
==
true
)
{
robotVelocity
.
head
<
6
>
()
=
base_v
;
}
else
robotVelocity
.
head
<
6
>
().
setZero
();
for
(
unsigned
i
=
0
;
i
<
velIn
.
size
();
++
i
)
robotVelocity
(
i
+
6
)
=
velIn
[
i
];
robotVelocity_
.
setConstant
(
robotVelocity
);
robotVelocity_
.
setTime
(
t
);
}
it
=
SensorsIn
.
find
(
"accelerometer_0"
);
if
(
it
!=
SensorsIn
.
end
())
{
...
...
@@ -224,8 +253,7 @@ void SoTHRP2Device::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsI
void
SoTHRP2Device
::
getControl
(
map
<
string
,
dgsot
::
ControlValues
>
&
controlOut
)
{
sotDEBUGIN
(
25
)
;
vector
<
double
>
anglesOut
;
anglesOut
.
resize
(
state_
.
size
());
vector
<
double
>
controlOut_
;
// Integrate control
increment
(
timestep_
);
...
...
@@ -233,14 +261,11 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
sotDEBUG
(
25
)
<<
"diff = "
<<
((
previousState_
.
size
()
==
state_
.
size
())
?
(
state_
-
previousState_
)
:
state_
)
<<
std
::
endl
;
previousState_
=
state_
;
// Specify the joint values for the controller.
if
(
anglesOut
.
size
()
!=
state_
.
size
()
-
6
)
anglesOut
.
resize
(
state_
.
size
()
-
6
);
controlOut_
.
resize
(
state_
.
size
()
-
6
);
for
(
unsigned
int
i
=
6
;
i
<
state_
.
size
();
++
i
)
angles
Out
[
i
-
6
]
=
state_
(
i
);
controlOut
[
"
joints
"
].
setValues
(
angles
Out
);
control
Out
_
[
i
-
6
]
=
state_
(
i
);
controlOut
[
"
control
"
].
setValues
(
control
Out
_
);
// Read zmp reference from input signal if plugged
int
time
=
controlSIN
.
getTime
();
...
...
src/sot-hrp2-device.hh
View file @
19496ce2
...
...
@@ -63,14 +63,6 @@ protected:
/// \brief Previous robot configuration.
dg
::
Vector
previousState_
;
/// \brief Robot state provided by OpenHRP.
///
/// This corresponds to the real encoders values and take into
/// account the stabilization step. Therefore, this usually
/// does *not* match the state control input signal.
///
dynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
robotState_
;
/// Accelerations measured by accelerometers
dynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
accelerometerSOUT_
;
/// Rotation velocity measured by gyrometers
...
...
@@ -85,6 +77,7 @@ protected:
/// Intermediate variables to avoid allocation during control
dg
::
Vector
mlforces
;
dg
::
Vector
mlRobotState
;
dg
::
Vector
robotVelocity
;
dgsot
::
MatrixRotation
pose
;
dg
::
Vector
accelerometer_
;
dg
::
Vector
gyrometer_
;
...
...
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