Skip to content
GitLab
Menu
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-talos
Commits
5f21c47f
Commit
5f21c47f
authored
Jan 15, 2018
by
Olivier Stasse
Browse files
Set default values for the force sensors.
parent
b3e13023
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/sot-talos-device.cpp
View file @
5f21c47f
...
...
@@ -85,6 +85,10 @@ SoTTalosDevice::SoTTalosDevice(std::string RobotName):
accelerometerSOUT_
.
setConstant
(
data
);
gyrometerSOUT_
.
setConstant
(
data
);
baseff_
.
resize
(
7
);
dg
::
Vector
dataForces
(
6
);
dataForces
.
setZero
();
for
(
int
i
=
0
;
i
<
4
;
i
++
)
forcesSOUT
[
i
]
->
setConstant
(
dataForces
);
using
namespace
dynamicgraph
::
command
;
std
::
string
docstring
;
/* Command increment. */
...
...
@@ -105,6 +109,7 @@ SoTTalosDevice::~SoTTalosDevice()
void
SoTTalosDevice
::
setSensorsForce
(
map
<
string
,
dgsot
::
SensorValues
>
&
SensorsIn
,
int
t
)
{
sotDEBUGIN
(
15
);
map
<
string
,
dgsot
::
SensorValues
>::
iterator
it
;
it
=
SensorsIn
.
find
(
"forces"
);
if
(
it
!=
SensorsIn
.
end
())
...
...
@@ -114,17 +119,17 @@ void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn,
const
vector
<
double
>&
forcesIn
=
it
->
second
.
getValues
();
for
(
int
i
=
0
;
i
<
4
;
++
i
)
{
s
td
::
cout
<<
"Force sensor "
<<
i
<<
std
::
endl
;
s
otDEBUG
(
15
)
<<
"Force sensor "
<<
i
<<
std
::
endl
;
for
(
int
j
=
0
;
j
<
6
;
++
j
)
{
dgforces_
(
j
)
=
forcesIn
[
i
*
6
+
j
];
s
td
::
cout
<<
"Force value "
<<
j
<<
":"
<<
dgforces_
(
j
)
<<
std
::
endl
;
s
otDEBUG
(
15
)
<<
"Force value "
<<
j
<<
":"
<<
dgforces_
(
j
)
<<
std
::
endl
;
}
forcesSOUT
[
i
]
->
setConstant
(
dgforces_
);
forcesSOUT
[
i
]
->
setTime
(
t
);
}
}
else
std
::
cout
<<
"No forces in the map"
<<
std
::
endl
;
sotDEBUGIN
(
15
)
;
}
void
SoTTalosDevice
::
setSensorsIMU
(
map
<
string
,
dgsot
::
SensorValues
>
&
SensorsIn
,
int
t
)
...
...
@@ -368,3 +373,30 @@ void SoTTalosDevice::getControl(map<string,dgsot::ControlValues> &controlOut)
ODEBUG5FULL
(
"end"
);
sotDEBUGOUT
(
25
)
;
}
using
namespace
dynamicgraph
::
sot
;
namespace
dynamicgraph
{
namespace
sot
{
#ifdef WIN32
const
char
*
DebugTrace
::
DEBUG_FILENAME_DEFAULT
=
"c:/tmp/sot-core-traces.txt"
;
#else // WIN32
const
char
*
DebugTrace
::
DEBUG_FILENAME_DEFAULT
=
"/tmp/sot-core-traces.txt"
;
#endif // WIN32
#ifdef VP_DEBUG
# ifdef WIN32
std
::
ofstream
debugfile
(
"C:/tmp/sot-core-traces.txt"
,
std
::
ios
::
trunc
&
std
::
ios
::
out
);
# else // WIN32
std
::
ofstream
debugfile
(
"/tmp/sot-core-traces.txt"
,
std
::
ios
::
trunc
&
std
::
ios
::
out
);
# endif // WIN32
#else // VP_DEBUG
std
::
ofstream
debugfile
;
#endif // VP_DEBUG
}
/* namespace sot */
}
/* namespace dynamicgraph */
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a 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