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-torque-control
Commits
2289c451
Commit
2289c451
authored
Feb 21, 2018
by
Olivier Stasse
Browse files
[python] Remove robot_data_test.py
parent
a7fea028
Changes
1
Hide whitespace changes
Inline
Side-by-side
python/dynamic_graph/sot/torque_control/tests/robot_data_test.py
deleted
100644 → 0
View file @
a7fea028
# TO DO: Checking that this data are making sense
# Right now this is put just to test the various functions
import
numpy
class
initRobotData
:
nbJoints
=
29
testRobotPath
=
"/opt/openrobots/share/simple_humanoid_description/urdf/simple_humanoid.urdf"
controlDT
=
0.005
maxCurrent
=
5
robotRef
=
"control-manager-robot"
urdftosot
=
(
12
,
13
,
14
,
15
,
23
,
24
,
25
,
26
,
27
,
28
,
16
,
17
,
18
,
19
,
20
,
21
,
22
,
6
,
7
,
8
,
9
,
10
,
11
,
0
,
1
,
2
,
3
,
4
,
5
)
ctrlManagerCurrentToControlGain
=
1.0
mapJointNameToID
=
{
'rhy'
:
0
,
'rhr'
:
1
,
'rhp'
:
2
,
'rk'
:
3
,
'rap'
:
4
,
'rar'
:
5
,
'lhy'
:
6
,
'lhr'
:
7
,
'lhp'
:
8
,
'lk'
:
9
,
'lap'
:
10
,
'lar'
:
11
,
'ty'
:
12
,
'tp'
:
13
,
'hp'
:
14
,
'rsp'
:
15
,
'rsr'
:
16
,
'rsy'
:
17
,
're'
:
18
,
'rwy'
:
19
,
'rwp'
:
20
,
'rh'
:
21
,
'lsp'
:
22
,
'lsr'
:
23
,
'lsy'
:
24
,
'le'
:
25
,
'lwy'
:
26
,
'lwp'
:
27
,
'lh'
:
28
}
mapJointLimits
=
{
0
:
[
-
0.785398
,
0.523599
],
1
:
[
-
0.610865
,
0.349066
],
2
:
[
-
2.18166
,
0.733038
],
3
:
[
-
0.0349066
,
2.61799
],
4
:
[
-
1.309
,
0.733038
],
5
:
[
-
0.349066
,
0.610865
],
6
:
[
-
0.523599
,
0.785398
],
7
:
[
-
0.349066
,
0.610865
],
8
:
[
-
2.18166
,
0.733038
],
9
:
[
-
0.0349066
,
2.61799
],
10
:
[
-
1.309
,
0.733038
],
11
:
[
-
0.610865
,
0.349066
],
12
:
[
-
0.785398
,
0.785398
],
13
:
[
-
0.0872665
,
1.0472
],
14
:
[
-
0.785398
,
0.78539
],
15
:
[
-
0.523599
,
0.785398
],
16
:
[
-
3.14159
,
1.0472
],
17
:
[
-
1.65806
,
0.174533
],
18
:
[
-
1.6057
,
1.6057
],
19
:
[
-
2.3911
,
0.0349066
],
20
:
[
-
1.6057
,
1.6057
],
21
:
[
-
1.6057
,
1.6057
],
22
:
[
-
1.0
,
1.0
],
23
:
[
-
3.14159
,
1.0472
],
24
:
[
-
0.174533
,
1.65806
],
25
:
[
-
1.6057
,
1.6057
],
26
:
[
-
2.3911
,
0.0349066
],
27
:
[
-
1.6057
,
1.6057
],
28
:
[
-
1.6057
,
1.6057
],
}
fMax
=
numpy
.
array
([
100.0
,
100.0
,
300.0
,
80.0
,
80.0
,
30.0
])
fMin
=-
fMax
mapForceIdToForceLimits
=
{
0
:
[
fMin
,
fMax
],
1
:
[
fMin
,
fMax
],
2
:
[
fMin
,
fMax
],
3
:
[
fMin
,
fMax
]
}
mapNameToForceId
=
{
"rf"
:
0
,
"lf"
:
1
,
"rh"
:
2
,
"lh"
:
3
}
indexOfForceSensors
=
()
FootFrameNames
=
{
"Right"
:
"RLEG_ANKLE_R"
,
"Left"
:
"LLEG_ANKLE_R"
}
RightFootSensorXYZ
=
(
0.0
,
0.0
,
-
0.085
)
def
init_and_set_controller_manager
(
self
,
cm
):
# Init should be called before addCtrlMode
# because the size of state vector must be known.
cm
.
init
(
self
.
controlDT
,
self
.
testRobotPath
,
self
.
robotRef
)
# Set the map from joint name to joint ID
for
key
in
self
.
mapJointNameToID
:
cm
.
setNameToId
(
key
,
self
.
mapJointNameToID
[
key
])
# Set the map joint limits for each id
for
key
in
self
.
mapJointLimits
:
cm
.
setJointLimitsFromId
(
key
,
self
.
mapJointLimits
[
key
][
0
],
\
self
.
mapJointLimits
[
key
][
1
])
# Set the force limits for each id
for
key
in
self
.
mapForceIdToForceLimits
:
cm
.
setForceLimitsFromId
(
key
,
tuple
(
self
.
mapForceIdToForceLimits
[
key
][
0
]),
\
tuple
(
self
.
mapForceIdToForceLimits
[
key
][
1
]))
# Set the force sensor id for each sensor name
for
key
in
self
.
mapNameToForceId
:
cm
.
setForceNameToForceId
(
key
,
self
.
mapNameToForceId
[
key
])
# Set the map from the urdf joint list to the sot joint list
cm
.
setJointsUrdfToSot
(
self
.
urdftosot
)
# Set the foot frame name
for
key
in
self
.
FootFrameNames
:
cm
.
setFootFrameName
(
key
,
self
.
FootFrameNames
[
key
])
cm
.
setRightFootSoleXYZ
(
self
.
RightFootSensorXYZ
)
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