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
ef2180cc
Commit
ef2180cc
authored
Apr 14, 2016
by
Rohan Budhiraja
Browse files
change baseff controlValue rotatoin from matrix to quaternion
parent
b9cf2f3f
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/sot-hrp2-device.cpp
View file @
ef2180cc
...
...
@@ -55,7 +55,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
dg
::
Vector
data
(
3
);
data
.
setZero
();
accelerometerSOUT_
.
setConstant
(
data
);
gyrometerSOUT_
.
setConstant
(
data
);
baseff_
.
resize
(
12
);
baseff_
.
resize
(
7
);
using
namespace
dynamicgraph
::
command
;
std
::
string
docstring
;
...
...
@@ -244,11 +244,17 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
controlOut
[
"zmp"
].
setValues
(
ZMPRef
);
// Update position of freeflyer in global frame
for
(
int
i
=
0
;
i
<
3
;
++
i
)
baseff_
[
i
*
4
+
3
]
=
freeFlyerPose
()
(
i
,
3
);
for
(
unsigned
i
=
0
;
i
<
3
;
++
i
)
for
(
unsigned
j
=
0
;
j
<
3
;
++
j
)
baseff_
[
i
*
4
+
j
]
=
freeFlyerPose
()
(
i
,
j
);
Eigen
::
Vector3d
transq_
(
freeFlyerPose
().
translation
());
dg
::
sot
::
VectorQuaternion
qt_
(
freeFlyerPose
().
linear
());
//translation
for
(
int
i
=
0
;
i
<
3
;
i
++
)
baseff_
[
i
]
=
transq_
(
i
);
//rotation: quaternion
baseff_
[
3
]
=
qt_
.
w
();
baseff_
[
4
]
=
qt_
.
x
();
baseff_
[
5
]
=
qt_
.
y
();
baseff_
[
6
]
=
qt_
.
z
();
controlOut
[
"baseff"
].
setValues
(
baseff_
);
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