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-talos
Commits
43892dd4
Commit
43892dd4
authored
Nov 05, 2019
by
Guilhem Saurel
Browse files
[Python 3] Format
parent
5821a3ae
Changes
15
Hide whitespace changes
Inline
Side-by-side
src/dynamic_graph/sot/pyrene/prologue.py
View file @
43892dd4
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
print
(
"Prologue Pyrene TALOS Robot"
)
from
dynamic_graph.entity
import
PyEntityFactoryClass
from
dynamic_graph.sot.pyrene.robot
import
Robot
# Create the device.
# This entity behaves exactly like robotsimu except:
# 1. it does not provide the increment command
# 2. it forwards the robot control to the sot-abstract
# controller.
def
makeRobot
():
print
(
"Prologue Pyrene TALOS Robot"
)
def
makeRobot
():
"""
Create the device.
This entity behaves exactly like robotsimu except:
1. it does not provide the increment command
2. it forwards the robot control to the sot-abstract
controller.
"""
DeviceTalos
=
PyEntityFactoryClass
(
'DeviceTalos'
)
# Create the robot using the device.
robot
=
Robot
(
name
=
'robot'
,
device
=
DeviceTalos
(
'PYRENE'
))
robot
=
Robot
(
name
=
'robot'
,
device
=
DeviceTalos
(
'PYRENE'
))
robot
.
dynamic
.
com
.
recompute
(
0
)
robot
.
dynamic
.
com
.
recompute
(
0
)
_com
=
robot
.
dynamic
.
com
.
value
robot
.
device
.
zmp
.
value
=
(
_com
[
0
],
_com
[
1
],
0.
)
return
robot
####################################
# --- IMPORTANT --- #
# #
...
...
src/dynamic_graph/sot/pyrene/robot.py
View file @
43892dd4
# -*- coding: utf-8 -*-
# Copyright 2016, Olivier Stasse, CNRS
from
dynamic_graph.sot.talos
import
Talos
import
numpy
as
np
class
Robot
(
Talos
):
class
Robot
(
Talos
):
"""
This class instantiates LAAS TALOS Robot
"""
halfSitting
=
(
0.0
,
0.0
,
1.018213
,
0.00
,
0.0
,
0.0
,
# Free flyer
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
# Left Leg
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
# Right Leg
0.0
,
0.006761
,
# Chest
0.25847
,
0.173046
,
-
0.0002
,
-
0.525366
,
0.0
,
-
0.0
,
0.1
,
-
0.005
,
# Left Arm
-
0.25847
,
-
0.173046
,
0.0002
,
-
0.525366
,
0.0
,
0.0
,
0.1
,
-
0.005
,
# Right Arm
0.
,
0.
# Head
)
halfSitting
=
(
0.0
,
0.0
,
1.018213
,
0.00
,
0.0
,
0.0
,
#Free flyer
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Left Leg
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Right Leg
0.0
,
0.006761
,
#Chest
0.25847
,
0.173046
,
-
0.0002
,
-
0.525366
,
0.0
,
-
0.0
,
0.1
,
-
0.005
,
#Left Arm
-
0.25847
,
-
0.173046
,
0.0002
,
-
0.525366
,
0.0
,
0.0
,
0.1
,
-
0.005
,
#Right Arm
0.
,
0.
#Head
)
def
__init__
(
self
,
name
,
device
=
None
,
tracer
=
None
):
Talos
.
__init__
(
self
,
name
,
self
.
halfSitting
,
device
,
tracer
)
def
__init__
(
self
,
name
,
device
=
None
,
tracer
=
None
):
Talos
.
__init__
(
self
,
name
,
self
.
halfSitting
,
device
,
tracer
)
"""
TODO:Confirm these values
# Define camera positions w.r.t gaze.
...
...
@@ -80,4 +108,5 @@ class Robot (Talos):
matrixToTuple(cameraTopRightPosition), "gaze"))
"""
__all__
=
[
"Robot"
]
src/dynamic_graph/sot/pyrene/seqplay.py
View file @
43892dd4
...
...
@@ -2,41 +2,45 @@
# Copyright 2012, CNRS-LAAS, Florent Lamiraux
import
numpy
as
np
from
math
import
sqrt
from
dynamic_graph.sot.pyrene.robot
import
Robot
from
dynamic_graph.sot.core
import
RPYToMatrix
from
dynamic_graph.sot.tools.se3
import
SE3
,
SO3
,
R3
from
dynamic_graph.sot.pyrene.robot
import
Robot
from
dynamic_graph.sot.tools.se3
import
R3
,
SE3
robot
=
Robot
(
'seqplay'
)
rpy2matrix
=
RPYToMatrix
(
'rpy2matrix'
)
robot
=
Robot
(
'seqplay'
)
rpy2matrix
=
RPYToMatrix
(
'rpy2matrix'
)
m
=
56.868
g
=
9.81
pos
=
None
zmp
=
None
hip
=
None
def
convert
(
filename
)
:
def
convert
(
filename
):
"""
Convert a seqplay file in OpenHRP format to sot-tool format
"""
global
pos
,
zmp
,
hip
openhrpPos
=
np
.
genfromtxt
(
filename
+
'.pos'
)
openhrpZmp
=
np
.
genfromtxt
(
filename
+
'.zmp'
)
nbConfig
=
len
(
openhrpPos
)
if
len
(
openhrpZmp
)
!=
nbConfig
:
raise
RuntimeError
(
filename
+
".pos and "
+
filename
+
".zmp have different lengths."
)
openhrpPos
=
np
.
genfromtxt
(
filename
+
'.pos'
)
openhrpZmp
=
np
.
genfromtxt
(
filename
+
'.zmp'
)
nbConfig
=
len
(
openhrpPos
)
if
len
(
openhrpZmp
)
!=
nbConfig
:
raise
RuntimeError
(
filename
+
".pos and "
+
filename
+
".zmp have different lengths."
)
try
:
openhrpHip
=
np
.
genfromtxt
(
filename
+
'.hip'
)
openhrpHip
=
np
.
genfromtxt
(
filename
+
'.hip'
)
except
IOError
:
hip
=
[]
for
i
in
range
(
len
(
openhrpPos
)):
hip
.
append
((
openhrpPos
[
i
][
0
],
0
,
0
,
0
,))
openhrpHip
=
np
.
array
(
hip
)
if
len
(
openhrpHip
)
!=
nbConfig
:
raise
RuntimeError
(
filename
+
".pos and "
+
filename
+
".hip have different lengths."
)
for
i
in
range
(
len
(
openhrpPos
)):
hip
.
append
((
openhrpPos
[
i
][
0
],
0
,
0
,
0
,
))
openhrpHip
=
np
.
array
(
hip
)
if
len
(
openhrpHip
)
!=
nbConfig
:
raise
RuntimeError
(
filename
+
".pos and "
+
filename
+
".hip have different lengths."
)
t
=
1
featurePos
=
[]
...
...
@@ -49,110 +53,122 @@ def convert (filename) :
fixedFoot
=
None
fixedLeftFoot
=
None
fixedRightFoot
=
None
for
(
pos
,
zmp
,
hip
)
in
zip
(
openhrpPos
,
openhrpZmp
,
openhrpHip
)
:
translation
=
3
*
(
0.
,)
config
=
list
(
translation
+
tuple
(
hip
[
1
:])
+
tuple
(
pos
[
1
:
31
]))
robot
.
dynamic
.
position
.
value
=
tuple
(
config
)
for
(
pos
,
zmp
,
hip
)
in
zip
(
openhrpPos
,
openhrpZmp
,
openhrpHip
):
translation
=
3
*
(
0.
,
)
config
=
list
(
translation
+
tuple
(
hip
[
1
:])
+
tuple
(
pos
[
1
:
31
]))
robot
.
dynamic
.
position
.
value
=
tuple
(
config
)
robot
.
dynamic
.
position
.
time
=
t
robot
.
com
.
recompute
(
t
)
robot
.
leftAnkle
.
position
.
recompute
(
t
)
robot
.
rightAnkle
.
position
.
recompute
(
t
)
lf
=
SE3
(
robot
.
leftAnkle
.
position
.
value
)
*
R3
(
0.
,
0.
,
-
0.107
)
rf
=
SE3
(
robot
.
rightAnkle
.
position
.
value
)
*
R3
(
0.
,
0.
,
-
0.107
)
robot
.
com
.
recompute
(
t
)
robot
.
leftAnkle
.
position
.
recompute
(
t
)
robot
.
rightAnkle
.
position
.
recompute
(
t
)
lf
=
SE3
(
robot
.
leftAnkle
.
position
.
value
)
*
R3
(
0.
,
0.
,
-
0.107
)
rf
=
SE3
(
robot
.
rightAnkle
.
position
.
value
)
*
R3
(
0.
,
0.
,
-
0.107
)
# find support foot
rpy2matrix
.
sin
.
value
=
tuple
(
hip
[
1
:])
rpy2matrix
.
sout
.
recompute
(
t
)
waist
=
SE3
(
rpy2matrix
.
sout
.
value
,
translation
)
zmpGlob
=
waist
*
R3
(
tuple
(
zmp
[
1
:]))
rpy2matrix
.
sin
.
value
=
tuple
(
hip
[
1
:])
rpy2matrix
.
sout
.
recompute
(
t
)
waist
=
SE3
(
rpy2matrix
.
sout
.
value
,
translation
)
zmpGlob
=
waist
*
R3
(
tuple
(
zmp
[
1
:]))
# fr = m g * (zmpGlob - lf | rf - lf)/||rf - lf||^2
# fl = (m g - fr)
fr
=
m
*
g
*
((
zmpGlob
-
lf
)
*
(
rf
-
lf
)
/
((
rf
-
lf
)
*
(
rf
-
lf
)))
fr
=
m
*
g
*
((
zmpGlob
-
lf
)
*
(
rf
-
lf
)
/
((
rf
-
lf
)
*
(
rf
-
lf
)))
fl
=
m
*
g
-
fr
if
(
lf
-
zmpGlob
)
*
(
lf
-
zmpGlob
)
<
(
rf
-
zmpGlob
)
*
(
rf
-
zmpGlob
)
:
if
(
lf
-
zmpGlob
)
*
(
lf
-
zmpGlob
)
<
(
rf
-
zmpGlob
)
*
(
rf
-
zmpGlob
):
supportFoot
=
lf
fixedFoot
=
fixedLeftFoot
else
:
else
:
supportFoot
=
rf
fixedFoot
=
fixedRightFoot
t
+=
1
t
+=
1
# move support foot to previous value
if
fixedFoot
is
None
:
config
[
2
]
-=
supportFoot
[
2
]
config
[
2
]
-=
supportFoot
[
2
]
else
:
config
[
0
]
+=
fixedFoot
[
0
]
-
supportFoot
[
0
]
config
[
1
]
+=
fixedFoot
[
1
]
-
supportFoot
[
1
]
config
[
2
]
+=
fixedFoot
[
2
]
-
supportFoot
[
2
]
config
[
0
]
+=
fixedFoot
[
0
]
-
supportFoot
[
0
]
config
[
1
]
+=
fixedFoot
[
1
]
-
supportFoot
[
1
]
config
[
2
]
+=
fixedFoot
[
2
]
-
supportFoot
[
2
]
robot
.
dynamic
.
position
.
value
=
tuple
(
config
)
robot
.
dynamic
.
position
.
value
=
tuple
(
config
)
robot
.
dynamic
.
position
.
time
=
t
robot
.
com
.
recompute
(
t
)
robot
.
leftAnkle
.
position
.
recompute
(
t
)
robot
.
rightAnkle
.
position
.
recompute
(
t
)
featurePos
.
append
(
config
)
featureCom
.
append
(
robot
.
com
.
value
)
featureLa
.
append
(
robot
.
leftAnkle
.
position
.
value
)
featureRa
.
append
(
robot
.
rightAnkle
.
position
.
value
)
forceLeftFoot
.
append
((
0.
,
0.
,
fl
,
0.
,
0.
,
0.
,))
forceRightFoot
.
append
((
0.
,
0.
,
fr
,
0.
,
0.
,
0.
,))
robot
.
com
.
recompute
(
t
)
robot
.
leftAnkle
.
position
.
recompute
(
t
)
robot
.
rightAnkle
.
position
.
recompute
(
t
)
featurePos
.
append
(
config
)
featureCom
.
append
(
robot
.
com
.
value
)
featureLa
.
append
(
robot
.
leftAnkle
.
position
.
value
)
featureRa
.
append
(
robot
.
rightAnkle
.
position
.
value
)
forceLeftFoot
.
append
((
0.
,
0.
,
fl
,
0.
,
0.
,
0.
,
))
forceRightFoot
.
append
((
0.
,
0.
,
fr
,
0.
,
0.
,
0.
,
))
t
+=
1
fixedLeftFoot
=
\
SE3
(
robot
.
leftAnkle
.
position
.
value
)
*
R3
(
0.
,
0.
,
-
0.107
)
fixedRightFoot
=
\
SE3
(
robot
.
rightAnkle
.
position
.
value
)
*
R3
(
0.
,
0.
,
-
0.107
)
filePos
=
open
(
filename
+
'.posture'
,
'w'
)
fileLa
=
open
(
filename
+
'.la'
,
'w'
)
fileRa
=
open
(
filename
+
'.ra'
,
'w'
)
fileCom
=
open
(
filename
+
'.com'
,
'w'
)
fileFl
=
open
(
filename
+
'.fl'
,
'w'
)
fileFr
=
open
(
filename
+
'.fr'
,
'w'
)
fixedLeftFoot
=
SE3
(
robot
.
leftAnkle
.
position
.
value
)
*
R3
(
0.
,
0.
,
-
0.107
)
fixedRightFoot
=
SE3
(
robot
.
rightAnkle
.
position
.
value
)
*
R3
(
0.
,
0.
,
-
0.107
)
filePos
=
open
(
filename
+
'.posture'
,
'w'
)
fileLa
=
open
(
filename
+
'.la'
,
'w'
)
fileRa
=
open
(
filename
+
'.ra'
,
'w'
)
fileCom
=
open
(
filename
+
'.com'
,
'w'
)
fileFl
=
open
(
filename
+
'.fl'
,
'w'
)
fileFr
=
open
(
filename
+
'.fr'
,
'w'
)
dt
=
.
005
for
(
pos
,
la
,
ra
,
com
,
force_lf
,
force_rf
,
i
)
in
zip
(
featurePos
,
featureLa
,
featureRa
,
featureCom
,
forceLeftFoot
,
forceRightFoot
,
xrange
(
10000000
))
:
t
=
i
*
dt
filePos
.
write
(
"{0}"
.
format
(
t
))
fileLa
.
write
(
"{0}"
.
format
(
t
))
fileRa
.
write
(
"{0}"
.
format
(
t
))
fileCom
.
write
(
"{0}"
.
format
(
t
))
fileFl
.
write
(
"{0}"
.
format
(
t
))
fileFr
.
write
(
"{0}"
.
format
(
t
))
for
(
pos
,
la
,
ra
,
com
,
force_lf
,
force_rf
,
i
)
in
zip
(
featurePos
,
featureLa
,
featureRa
,
featureCom
,
forceLeftFoot
,
forceRightFoot
,
range
(
10000000
)):
t
=
i
*
dt
filePos
.
write
(
"{0}"
.
format
(
t
))
fileLa
.
write
(
"{0}"
.
format
(
t
))
fileRa
.
write
(
"{0}"
.
format
(
t
))
fileCom
.
write
(
"{0}"
.
format
(
t
))
fileFl
.
write
(
"{0}"
.
format
(
t
))
fileFr
.
write
(
"{0}"
.
format
(
t
))
for
x
in
pos
:
filePos
.
write
(
"
\t
{0}"
.
format
(
x
))
filePos
.
write
(
"
\t
{0}"
.
format
(
x
))
for
row
in
la
:
for
x
in
row
:
fileLa
.
write
(
"
\t
{0}"
.
format
(
x
))
fileLa
.
write
(
"
\t
{0}"
.
format
(
x
))
for
row
in
ra
:
for
x
in
row
:
fileRa
.
write
(
"
\t
{0}"
.
format
(
x
))
fileRa
.
write
(
"
\t
{0}"
.
format
(
x
))
for
x
in
com
:
fileCom
.
write
(
"
\t
{0}"
.
format
(
x
))
fileCom
.
write
(
"
\t
{0}"
.
format
(
x
))
for
x
in
force_lf
:
fileFl
.
write
(
"
\t
{0}"
.
format
(
x
))
fileFl
.
write
(
"
\t
{0}"
.
format
(
x
))
for
x
in
force_rf
:
fileFr
.
write
(
"
\t
{0}"
.
format
(
x
))
filePos
.
write
(
"
\n
"
)
fileLa
.
write
(
"
\n
"
)
fileRa
.
write
(
"
\n
"
)
fileCom
.
write
(
"
\n
"
)
fileFl
.
write
(
"
\n
"
)
fileFr
.
write
(
"
\n
"
)
filePos
.
close
()
fileLa
.
close
()
fileRa
.
close
()
fileCom
.
close
()
fileFl
.
close
()
fileFr
.
close
()
fileFr
.
write
(
"
\t
{0}"
.
format
(
x
))
filePos
.
write
(
"
\n
"
)
fileLa
.
write
(
"
\n
"
)
fileRa
.
write
(
"
\n
"
)
fileCom
.
write
(
"
\n
"
)
fileFl
.
write
(
"
\n
"
)
fileFr
.
write
(
"
\n
"
)
filePos
.
close
()
fileLa
.
close
()
fileRa
.
close
()
fileCom
.
close
()
fileFl
.
close
()
fileFr
.
close
()
if
__name__
==
'__main__'
:
#convert ('/opt/grx3.0/HRP2LAAS/etc/walkfwd')
pass
# convert('/opt/grx3.0/HRP2LAAS/etc/walkfwd')
src/dynamic_graph/sot/talos/__init__.py
View file @
43892dd4
# -*- coding: utf-8 -*-
# Copyright 2016, Olivier Stasse CNRS
# flake8: noqa
from
__future__
import
print_function
from
dynamic_graph.sot.talos.talos
import
Talos
src/dynamic_graph/sot/talos/talos.py
View file @
43892dd4
...
...
@@ -3,14 +3,13 @@
from
__future__
import
print_function
import
numpy
as
np
from
dynamic_graph.sot.dynamics_pinocchio.humanoid_robot
import
AbstractHumanoidRobot
from
dynamic_graph.sot.dynamics_pinocchio
import
DynamicPinocchio
import
pinocchio
as
se3
from
dynamic_graph
import
plug
from
dynamic_graph.sot.core.math_small_entities
import
Derivator_of_Vector
from
dynamic_graph.sot.dynamics_pinocchio
import
DynamicPinocchio
from
dynamic_graph.sot.dynamics_pinocchio.humanoid_robot
import
AbstractHumanoidRobot
from
pinocchio.robot_wrapper
import
RobotWrapper
import
pinocchio
as
se3
from
rospkg
import
RosPack
# Internal helper tool.
def
matrixToTuple
(
M
):
...
...
@@ -20,19 +19,14 @@ def matrixToTuple(M):
res
.
append
(
tuple
(
i
))
return
tuple
(
res
)
class
Talos
(
AbstractHumanoidRobot
):
"""
This class defines a Talos robot
"""
forceSensorInLeftAnkle
=
((
1.
,
0.
,
0.
,
0.
),
(
0.
,
1.
,
0.
,
0.
),
(
0.
,
0.
,
1.
,
-
0.107
),
(
0.
,
0.
,
0.
,
1.
))
forceSensorInRightAnkle
=
((
1.
,
0.
,
0.
,
0.
),
(
0.
,
1.
,
0.
,
0.
),
(
0.
,
0.
,
1.
,
-
0.107
),
(
0.
,
0.
,
0.
,
1.
))
forceSensorInLeftAnkle
=
((
1.
,
0.
,
0.
,
0.
),
(
0.
,
1.
,
0.
,
0.
),
(
0.
,
0.
,
1.
,
-
0.107
),
(
0.
,
0.
,
0.
,
1.
))
forceSensorInRightAnkle
=
((
1.
,
0.
,
0.
,
0.
),
(
0.
,
1.
,
0.
,
0.
),
(
0.
,
0.
,
1.
,
-
0.107
),
(
0.
,
0.
,
0.
,
1.
))
"""
TODO: Confirm the position and existence of these sensors
accelerometerPosition = np.matrix ((
...
...
@@ -50,36 +44,35 @@ class Talos(AbstractHumanoidRobot):
))
"""
def
smallToFull
(
self
,
config
):
#Gripper position in full configuration: 27:34, and 41:48
#Small configuration: 36 DOF
#Full configuration: 50 DOF
res
=
config
[
0
:
27
]
+
7
*
(
0.
,)
+
config
[
27
:
34
]
+
7
*
(
0.
,
)
+
config
[
34
:]
#
Gripper position in full configuration: 27:34, and 41:48
#
Small configuration: 36 DOF
#
Full configuration: 50 DOF
res
=
config
[
0
:
27
]
+
7
*
(
0.
,
)
+
config
[
27
:
34
]
+
7
*
(
0.
,
)
+
config
[
34
:]
return
res
def
__init__
(
self
,
name
,
initialConfig
,
device
=
None
,
tracer
=
None
):
self
.
OperationalPointsMap
=
{
'left-wrist'
:
'arm_left_7_joint'
,
'right-wrist'
:
'arm_right_7_joint'
,
'left-ankle'
:
'leg_left_6_joint'
,
'right-ankle'
:
'leg_right_6_joint'
,
'gaze'
:
'head_2_joint'
,
'waist'
:
'root_joint'
,
'chest'
:
'torso_2_joint'
}
def
__init__
(
self
,
name
,
initialConfig
,
device
=
None
,
tracer
=
None
):
self
.
OperationalPointsMap
=
{
'left-wrist'
:
'arm_left_7_joint'
,
'right-wrist'
:
'arm_right_7_joint'
,
'left-ankle'
:
'leg_left_6_joint'
,
'right-ankle'
:
'leg_right_6_joint'
,
'gaze'
:
'head_2_joint'
,
'waist'
:
'root_joint'
,
'chest'
:
'torso_2_joint'
}
from
rospkg
import
RosPack
rospack
=
RosPack
()
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
# Create a wrapper to access the dynamic model provided through an urdf file.
from
pinocchio.robot_wrapper
import
RobotWrapper
import
pinocchio
as
se3
from
dynamic_graph.sot.dynamics_pinocchio
import
fromSotToPinocchio
pinocchioRobot
=
RobotWrapper
()
pinocchioRobot
.
initFromURDF
(
urdfPath
,
urdfDir
,
se3
.
JointModelFreeFlyer
())
self
.
pinocchioModel
=
pinocchioRobot
.
model
self
.
pinocchioData
=
pinocchioRobot
.
data
AbstractHumanoidRobot
.
__init__
(
self
,
name
,
tracer
)
AbstractHumanoidRobot
.
__init__
(
self
,
name
,
tracer
)
self
.
OperationalPoints
.
append
(
'waist'
)
self
.
OperationalPoints
.
append
(
'chest'
)
...
...
@@ -97,25 +90,20 @@ class Talos(AbstractHumanoidRobot):
# TODO For position limit, we remove the first value to get
# a vector of the good size because SoT use euler angles and not
# quaternions...
self
.
device
.
setPositionBounds
(
self
.
pinocchioModel
.
lowerPositionLimit
.
T
.
tolist
()[
0
][
1
:],
self
.
pinocchioModel
.
upperPositionLimit
.
T
.
tolist
()[
0
][
1
:])
self
.
device
.
setVelocityBounds
(
(
-
self
.
pinocchioModel
.
velocityLimit
).
T
.
tolist
()[
0
],
self
.
pinocchioModel
.
velocityLimit
.
T
.
tolist
()[
0
])
self
.
device
.
setTorqueBounds
(
(
-
self
.
pinocchioModel
.
effortLimit
).
T
.
tolist
()[
0
],
self
.
pinocchioModel
.
effortLimit
.
T
.
tolist
()[
0
])
self
.
device
.
setPositionBounds
(
self
.
pinocchioModel
.
lowerPositionLimit
.
T
.
tolist
()[
0
][
1
:],
self
.
pinocchioModel
.
upperPositionLimit
.
T
.
tolist
()[
0
][
1
:])
self
.
device
.
setVelocityBounds
((
-
self
.
pinocchioModel
.
velocityLimit
).
T
.
tolist
()[
0
],
self
.
pinocchioModel
.
velocityLimit
.
T
.
tolist
()[
0
])
self
.
device
.
setTorqueBounds
((
-
self
.
pinocchioModel
.
effortLimit
).
T
.
tolist
()[
0
],
self
.
pinocchioModel
.
effortLimit
.
T
.
tolist
()[
0
])
self
.
halfSitting
=
initialConfig
self
.
device
.
set
(
self
.
halfSitting
)
plug
(
self
.
device
.
state
,
self
.
dynamic
.
position
)
self
.
AdditionalFrames
.
append
(
(
"leftFootForceSensor"
,
self
.
forceSensorInLeftAnkle
,
self
.
OperationalPointsMap
[
"left-ankle"
]))
(
"leftFootForceSensor"
,
self
.
forceSensorInLeftAnkle
,
self
.
OperationalPointsMap
[
"left-ankle"
]))
self
.
AdditionalFrames
.
append
(
(
"rightFootForceSensor"
,
self
.
forceSensorInRightAnkle
,
self
.
OperationalPointsMap
[
"right-ankle"
]))
(
"rightFootForceSensor"
,
self
.
forceSensorInRightAnkle
,
self
.
OperationalPointsMap
[
"right-ankle"
]))
self
.
dimension
=
self
.
dynamic
.
getDimension
()
self
.
plugVelocityFromDevice
=
True
...
...
@@ -128,21 +116,21 @@ class Talos(AbstractHumanoidRobot):
plug
(
self
.
device
.
state
,
self
.
velocityDerivator
.
sin
)
plug
(
self
.
velocityDerivator
.
sout
,
self
.
dynamic
.
velocity
)
else
:
self
.
dynamic
.
velocity
.
value
=
self
.
dimension
*
(
0.
,)
self
.
dynamic
.
velocity
.
value
=
self
.
dimension
*
(
0.
,
)
# Initialize acceleration derivator if chosen
if
self
.
enableAccelerationDerivator
:
self
.
accelerationDerivator
=
\
Derivator_of_Vector
(
'accelerationDerivator'
)
self
.
accelerationDerivator
.
dt
.
value
=
self
.
timeStep
plug
(
self
.
velocityDerivator
.
sout
,
self
.
accelerationDerivator
.
sin
)
plug
(
self
.
velocityDerivator
.
sout
,
self
.
accelerationDerivator
.
sin
)
plug
(
self
.
accelerationDerivator
.
sout
,
self
.
dynamic
.
acceleration
)
else
:
self
.
dynamic
.
acceleration
.
value
=
self
.
dimension
*
(
0.
,)
self
.
dynamic
.
acceleration
.
value
=
self
.
dimension
*
(
0.
,
)
# Create operational points based on operational points map (if provided)
if
self
.
OperationalPointsMap
is
not
None
:
self
.
initializeOpPoints
()
__all__
=
[
Talos
]
tests/appli-test-simple-seq-play.py
View file @
43892dd4
#from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
# flake8: noqa
# from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from
dynamic_graph
import
plug
# Create the solver
# Connects the sequence player to the posture task
from
dynamic_graph.sot.core
import
SOT
,
FeatureGeneric
,
GainAdaptive
,
Selec_of_vector
,
Task
from
dynamic_graph.sot.core.matrix_util
import
matrixToTuple
from
dynamic_graph.sot.tools
import
SimpleSeqPlay
from
numpy
import
eye
from
dynamic_graph.sot.core
import
Task
,
FeatureGeneric
,
GainAdaptive
from
dynamic_graph.sot.core.meta_tasks
import
setGain
from
dynamic_graph.sot.
core.matrix_util
import
matrixToTuple
from
numpy
import
identity
,
hstack
,
zeros
from
dynamic_graph.sot.
tools
import
SimpleSeqPlay
from
numpy
import
eye
,
hstack
,
identity
,
zeros
# Create the posture task
task_name
=
"posture_task"
taskPosture
=
Task
(
task_name
)
taskPosture
.
dyn
=
robot
.
dynamic
taskPosture
.
feature
=
FeatureGeneric
(
'feature_'
+
task_name
)
taskPosture
.
featureDes
=
FeatureGeneric
(
'feature_des_'
+
task_name
)
taskPosture
.
gain
=
GainAdaptive
(
"gain_"
+
task_name
)
taskPosture
.
feature
=
FeatureGeneric
(
'feature_'
+
task_name
)
taskPosture
.
featureDes
=
FeatureGeneric
(
'feature_des_'
+
task_name
)
taskPosture
.
gain
=
GainAdaptive
(
"gain_"
+
task_name
)
robotDim
=
robot
.
dynamic
.
getDimension
()
first_6
=
zeros
((
32
,
6
))
other_dof
=
identity
(
robotDim
-
6
)
first_6
=
zeros
((
32
,
6
))
other_dof