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
talos_integration_tests
Commits
8499c36f
Commit
8499c36f
authored
May 19, 2020
by
Olivier Stasse
Committed by
olivier stasse
May 25, 2020
Browse files
Fixing
#6
and trying to fix
#7
parent
651da839
Changes
16
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
8499c36f
...
...
@@ -56,9 +56,6 @@ add_project_dependency(dynamic_graph_bridge REQUIRED)
# Installing python based scripts
catkin_install_python
(
PROGRAMS
scripts/appli.py
scripts/appli_dcmZmpControl_file.py
scripts/appli_online_walking.py
scripts/start_talos_gazebo_kine.py
scripts/start_sot_talos_balance.py
scripts/start_sot_online_walking.py
...
...
@@ -73,6 +70,8 @@ foreach(dir launch tests)
)
endforeach
()
ADD_SUBDIRECTORY
(
src
)
# Testing part
if
(
CATKIN_ENABLE_TESTING
)
find_package
(
rostest REQUIRED
)
...
...
scripts/appli.py
deleted
100644 → 0
View file @
651da839
from
dynamic_graph.sot.core.meta_tasks_kine
import
MetaTaskKine6d
,
MetaTaskKineCom
,
gotoNd
from
dynamic_graph.sot.core.matrix_util
import
matrixToTuple
from
numpy
import
eye
taskRH
=
MetaTaskKine6d
(
'rh'
,
robot
.
dynamic
,
'rh'
,
robot
.
OperationalPointsMap
[
'right-wrist'
])
handMgrip
=
eye
(
4
);
handMgrip
[
0
:
3
,
3
]
=
(
0.1
,
0
,
0
)
taskRH
.
opmodif
=
matrixToTuple
(
handMgrip
)
taskRH
.
feature
.
frame
(
'desired'
)
# --- STATIC COM (if not walking)
taskCom
=
MetaTaskKineCom
(
robot
.
dynamic
)
robot
.
dynamic
.
com
.
recompute
(
0
)
taskCom
.
featureDes
.
errorIN
.
value
=
robot
.
dynamic
.
com
.
value
taskCom
.
task
.
controlGain
.
value
=
10
# --- CONTACTS
#define contactLF and contactRF
contactLF
=
MetaTaskKine6d
(
'contactLF'
,
robot
.
dynamic
,
'LF'
,
robot
.
OperationalPointsMap
[
'left-ankle'
])
contactLF
.
feature
.
frame
(
'desired'
)
contactLF
.
gain
.
setConstant
(
10
)
contactLF
.
keep
()
locals
()[
'contactLF'
]
=
contactLF
contactRF
=
MetaTaskKine6d
(
'contactRF'
,
robot
.
dynamic
,
'RF'
,
robot
.
OperationalPointsMap
[
'right-ankle'
])
contactRF
.
feature
.
frame
(
'desired'
)
contactRF
.
gain
.
setConstant
(
10
)
contactRF
.
keep
()
locals
()[
'contactRF'
]
=
contactRF
from
dynamic_graph
import
plug
from
dynamic_graph.sot.core.sot
import
SOT
sot
=
SOT
(
'sot'
)
sot
.
setSize
(
robot
.
dynamic
.
getDimension
())
plug
(
sot
.
control
,
robot
.
device
.
control
)
from
dynamic_graph.ros
import
RosPublish
ros_publish_state
=
RosPublish
(
"ros_publish_state"
)
ros_publish_state
.
add
(
"vector"
,
"state"
,
"/sot_control/state"
)
from
dynamic_graph
import
plug
plug
(
robot
.
device
.
state
,
ros_publish_state
.
state
)
robot
.
device
.
after
.
addDownsampledSignal
(
"ros_publish_state.trigger"
,
100
)
#target = (0.5,-0.2,1.0)
#addRobotViewer(robot, small=False)
#robot.viewer.updateElementConfig('zmp',target+(0,0,0))
#gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
sot
.
push
(
contactRF
.
task
.
name
)
sot
.
push
(
contactLF
.
task
.
name
)
sot
.
push
(
taskCom
.
task
.
name
)
robot
.
device
.
control
.
recompute
(
0
)
scripts/appli_dcmZmpControl_file.py
deleted
100644 → 0
View file @
651da839
# flake8: noqa
from
math
import
sqrt
import
numpy
as
np
from
dynamic_graph
import
plug
from
dynamic_graph.sot.core
import
SOT
,
Derivator_of_Vector
,
FeaturePosture
,
MatrixHomoToPoseQuaternion
,
Task
from
dynamic_graph.sot.core.matrix_util
import
matrixToTuple
from
dynamic_graph.sot.core.meta_tasks_kine
import
MetaTaskKine6d
,
MetaTaskKineCom
,
gotoNd
from
dynamic_graph.sot.dynamic_pinocchio
import
DynamicPinocchio
from
dynamic_graph.tracer_real_time
import
TracerRealTime
from
rospkg
import
RosPack
import
sot_talos_balance.talos.base_estimator_conf
as
base_estimator_conf
import
sot_talos_balance.talos.control_manager_conf
as
cm_conf
import
sot_talos_balance.talos.ft_calibration_conf
as
ft_conf
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
*
cm_conf
.
CTRL_MAX
=
100.0
# temporary hack
robot
.
timeStep
=
robot
.
device
.
getTimeStep
()
dt
=
robot
.
timeStep
# --- Pendulum parameters
robot_name
=
'robot'
robot
.
dynamic
.
com
.
recompute
(
0
)
robotDim
=
robot
.
dynamic
.
getDimension
()
mass
=
robot
.
dynamic
.
data
.
mass
[
0
]
h
=
robot
.
dynamic
.
com
.
value
[
2
]
g
=
9.81
omega
=
sqrt
(
g
/
h
)
# --- Parameter server
robot
.
param_server
=
create_parameter_server
(
param_server_conf
,
dt
)
# --- Initial feet and waist
robot
.
dynamic
.
createOpPoint
(
'LF'
,
robot
.
OperationalPointsMap
[
'left-ankle'
])
robot
.
dynamic
.
createOpPoint
(
'RF'
,
robot
.
OperationalPointsMap
[
'right-ankle'
])
robot
.
dynamic
.
createOpPoint
(
'WT'
,
robot
.
OperationalPointsMap
[
'waist'
])
robot
.
dynamic
.
LF
.
recompute
(
0
)
robot
.
dynamic
.
RF
.
recompute
(
0
)
robot
.
dynamic
.
WT
.
recompute
(
0
)
# -------------------------- DESIRED TRAJECTORY --------------------------
rospack
=
RosPack
()
data_folder
=
rospack
.
get_path
(
'sot-talos-balance'
)
+
"/data/"
folder
=
data_folder
+
test_folder
+
'/'
# --- Trajectory generators
# --- General trigger
robot
.
triggerTrajGen
=
BooleanIdentity
(
'triggerTrajGen'
)
robot
.
triggerTrajGen
.
sin
.
value
=
0
# --- CoM
robot
.
comTrajGen
=
create_com_trajectory_generator
(
dt
,
robot
)
robot
.
comTrajGen
.
x
.
recompute
(
0
)
# trigger computation of initial value
robot
.
comTrajGen
.
playTrajectoryFile
(
folder
+
'CoM.dat'
)
plug
(
robot
.
triggerTrajGen
.
sout
,
robot
.
comTrajGen
.
trigger
)
# --- Left foot
robot
.
lfTrajGen
=
create_pose_rpy_trajectory_generator
(
dt
,
robot
,
'LF'
)
robot
.
lfTrajGen
.
x
.
recompute
(
0
)
# trigger computation of initial value
robot
.
lfToMatrix
=
PoseRollPitchYawToMatrixHomo
(
'lf2m'
)
plug
(
robot
.
lfTrajGen
.
x
,
robot
.
lfToMatrix
.
sin
)
robot
.
lfTrajGen
.
playTrajectoryFile
(
folder
+
'LeftFoot.dat'
)
plug
(
robot
.
triggerTrajGen
.
sout
,
robot
.
lfTrajGen
.
trigger
)
# --- Right foot
robot
.
rfTrajGen
=
create_pose_rpy_trajectory_generator
(
dt
,
robot
,
'RF'
)
robot
.
rfTrajGen
.
x
.
recompute
(
0
)
# trigger computation of initial value
robot
.
rfToMatrix
=
PoseRollPitchYawToMatrixHomo
(
'rf2m'
)
plug
(
robot
.
rfTrajGen
.
x
,
robot
.
rfToMatrix
.
sin
)
robot
.
rfTrajGen
.
playTrajectoryFile
(
folder
+
'RightFoot.dat'
)
plug
(
robot
.
triggerTrajGen
.
sout
,
robot
.
rfTrajGen
.
trigger
)
# --- ZMP
robot
.
zmpTrajGen
=
create_zmp_trajectory_generator
(
dt
,
robot
)
robot
.
zmpTrajGen
.
x
.
recompute
(
0
)
# trigger computation of initial value
# robot.zmpTrajGen.playTrajectoryFile(folder + 'ZMP.dat')
plug
(
robot
.
triggerTrajGen
.
sout
,
robot
.
zmpTrajGen
.
trigger
)
# --- Waist
robot
.
waistTrajGen
=
create_orientation_rpy_trajectory_generator
(
dt
,
robot
,
'WT'
)
robot
.
waistTrajGen
.
x
.
recompute
(
0
)
# trigger computation of initial value
robot
.
waistMix
=
Mix_of_vector
(
"waistMix"
)
robot
.
waistMix
.
setSignalNumber
(
3
)
robot
.
waistMix
.
addSelec
(
1
,
0
,
3
)
robot
.
waistMix
.
addSelec
(
2
,
3
,
3
)
robot
.
waistMix
.
default
.
value
=
[
0.0
]
*
6
robot
.
waistMix
.
signal
(
"sin1"
).
value
=
[
0.0
]
*
3
plug
(
robot
.
waistTrajGen
.
x
,
robot
.
waistMix
.
signal
(
"sin2"
))
robot
.
waistToMatrix
=
PoseRollPitchYawToMatrixHomo
(
'w2m'
)
plug
(
robot
.
waistMix
.
sout
,
robot
.
waistToMatrix
.
sin
)
robot
.
waistTrajGen
.
playTrajectoryFile
(
folder
+
'WaistOrientation.dat'
)
plug
(
robot
.
triggerTrajGen
.
sout
,
robot
.
waistTrajGen
.
trigger
)
# --- Interface with controller entities
wp
=
DummyWalkingPatternGenerator
(
'dummy_wp'
)
wp
.
init
()
wp
.
omega
.
value
=
omega
#wp.waist.value = robot.dynamic.WT.value # wait receives a full homogeneous matrix, but only the rotational part is controlled
#wp.footLeft.value = robot.dynamic.LF.value
#wp.footRight.value = robot.dynamic.RF.value
#wp.com.value = robot.dynamic.com.value
#wp.vcom.value = [0.]*3
#wp.acom.value = [0.]*3
plug
(
robot
.
waistToMatrix
.
sout
,
wp
.
waist
)
plug
(
robot
.
lfToMatrix
.
sout
,
wp
.
footLeft
)
plug
(
robot
.
rfToMatrix
.
sout
,
wp
.
footRight
)
plug
(
robot
.
comTrajGen
.
x
,
wp
.
com
)
plug
(
robot
.
comTrajGen
.
dx
,
wp
.
vcom
)
plug
(
robot
.
comTrajGen
.
ddx
,
wp
.
acom
)
#plug(robot.zmpTrajGen.x, wp.zmp)
robot
.
wp
=
wp
# --- Compute the values to use them in initialization
robot
.
wp
.
comDes
.
recompute
(
0
)
robot
.
wp
.
dcmDes
.
recompute
(
0
)
robot
.
wp
.
zmpDes
.
recompute
(
0
)
# -------------------------- ESTIMATION --------------------------
# --- Base Estimation
robot
.
device_filters
=
create_device_filters
(
robot
,
dt
)
robot
.
imu_filters
=
create_imu_filters
(
robot
,
dt
)
robot
.
base_estimator
=
create_base_estimator
(
robot
,
dt
,
base_estimator_conf
)
robot
.
m2qLF
=
MatrixHomoToPoseQuaternion
(
'm2qLF'
)
plug
(
robot
.
dynamic
.
LF
,
robot
.
m2qLF
.
sin
)
plug
(
robot
.
m2qLF
.
sout
,
robot
.
base_estimator
.
lf_ref_xyzquat
)
robot
.
m2qRF
=
MatrixHomoToPoseQuaternion
(
'm2qRF'
)
plug
(
robot
.
dynamic
.
RF
,
robot
.
m2qRF
.
sin
)
plug
(
robot
.
m2qRF
.
sout
,
robot
.
base_estimator
.
rf_ref_xyzquat
)
# robot.be_filters = create_be_filters(robot, dt)
## --- Reference frame
#rf = SimpleReferenceFrame('rf')
#rf.init(robot_name)
#plug(robot.dynamic.LF, rf.footLeft)
#plug(robot.dynamic.RF, rf.footRight)
#rf.reset.value = 1
#robot.rf = rf
## --- State transformation
#stf = StateTransformation("stf")
#stf.init()
#plug(robot.rf.referenceFrame,stf.referenceFrame)
#plug(robot.base_estimator.q,stf.q_in)
#plug(robot.base_estimator.v,stf.v_in)
#robot.stf = stf
# --- Conversion
e2q
=
EulerToQuat
(
'e2q'
)
plug
(
robot
.
base_estimator
.
q
,
e2q
.
euler
)
robot
.
e2q
=
e2q
# --- Kinematic computations
robot
.
rdynamic
=
DynamicPinocchio
(
"real_dynamics"
)
robot
.
rdynamic
.
setModel
(
robot
.
dynamic
.
model
)
robot
.
rdynamic
.
setData
(
robot
.
rdynamic
.
model
.
createData
())
plug
(
robot
.
base_estimator
.
q
,
robot
.
rdynamic
.
position
)
robot
.
rdynamic
.
velocity
.
value
=
[
0.0
]
*
robotDim
robot
.
rdynamic
.
acceleration
.
value
=
[
0.0
]
*
robotDim
# --- CoM Estimation
cdc_estimator
=
DcmEstimator
(
'cdc_estimator'
)
cdc_estimator
.
init
(
dt
,
robot_name
)
plug
(
robot
.
e2q
.
quaternion
,
cdc_estimator
.
q
)
plug
(
robot
.
base_estimator
.
v
,
cdc_estimator
.
v
)
robot
.
cdc_estimator
=
cdc_estimator
# --- DCM Estimation
estimator
=
DummyDcmEstimator
(
"dummy"
)
estimator
.
omega
.
value
=
omega
estimator
.
mass
.
value
=
1.0
plug
(
robot
.
cdc_estimator
.
c
,
estimator
.
com
)
plug
(
robot
.
cdc_estimator
.
dc
,
estimator
.
momenta
)
estimator
.
init
()
robot
.
estimator
=
estimator
# --- Force calibration
robot
.
ftc
=
create_ft_calibrator
(
robot
,
ft_conf
)
# --- ZMP estimation
zmp_estimator
=
SimpleZmpEstimator
(
"zmpEst"
)
robot
.
rdynamic
.
createOpPoint
(
'sole_LF'
,
'left_sole_link'
)
robot
.
rdynamic
.
createOpPoint
(
'sole_RF'
,
'right_sole_link'
)
plug
(
robot
.
rdynamic
.
sole_LF
,
zmp_estimator
.
poseLeft
)
plug
(
robot
.
rdynamic
.
sole_RF
,
zmp_estimator
.
poseRight
)
plug
(
robot
.
ftc
.
left_foot_force_out
,
zmp_estimator
.
wrenchLeft
)
plug
(
robot
.
ftc
.
right_foot_force_out
,
zmp_estimator
.
wrenchRight
)
zmp_estimator
.
init
()
robot
.
zmp_estimator
=
zmp_estimator
# -------------------------- ADMITTANCE CONTROL --------------------------
# --- DCM controller
Kp_dcm
=
[
8.0
]
*
3
Ki_dcm
=
[
0.0
,
0.0
,
0.0
]
# zero (to be set later)
gamma_dcm
=
0.2
dcm_controller
=
DcmController
(
"dcmCtrl"
)
dcm_controller
.
Kp
.
value
=
Kp_dcm
dcm_controller
.
Ki
.
value
=
Ki_dcm
dcm_controller
.
decayFactor
.
value
=
gamma_dcm
dcm_controller
.
mass
.
value
=
mass
dcm_controller
.
omega
.
value
=
omega
plug
(
robot
.
cdc_estimator
.
c
,
dcm_controller
.
com
)
plug
(
robot
.
estimator
.
dcm
,
dcm_controller
.
dcm
)
plug
(
robot
.
wp
.
zmpDes
,
dcm_controller
.
zmpDes
)
plug
(
robot
.
wp
.
dcmDes
,
dcm_controller
.
dcmDes
)
dcm_controller
.
init
(
dt
)
robot
.
dcm_control
=
dcm_controller
Ki_dcm
=
[
1.0
,
1.0
,
1.0
]
# this value is employed later
# --- CoM admittance controller
Kp_adm
=
[
0.0
,
0.0
,
0.0
]
# zero (to be set later)
com_admittance_control
=
ComAdmittanceController
(
"comAdmCtrl"
)
com_admittance_control
.
Kp
.
value
=
Kp_adm
plug
(
robot
.
zmp_estimator
.
zmp
,
com_admittance_control
.
zmp
)
com_admittance_control
.
zmpDes
.
value
=
robot
.
wp
.
zmpDes
.
value
# should be plugged to robot.dcm_control.zmpRef
plug
(
robot
.
wp
.
acomDes
,
com_admittance_control
.
ddcomDes
)
com_admittance_control
.
init
(
dt
)
com_admittance_control
.
setState
(
robot
.
wp
.
comDes
.
value
,
[
0.0
,
0.0
,
0.0
])
robot
.
com_admittance_control
=
com_admittance_control
Kp_adm
=
[
15.0
,
15.0
,
0.0
]
# this value is employed later
# --- Control Manager
robot
.
cm
=
create_ctrl_manager
(
cm_conf
,
dt
,
robot_name
=
'robot'
)
robot
.
cm
.
addCtrlMode
(
'sot_input'
)
robot
.
cm
.
setCtrlMode
(
'all'
,
'sot_input'
)
robot
.
cm
.
addEmergencyStopSIN
(
'zmp'
)
# -------------------------- SOT CONTROL --------------------------
# --- Upper body
robot
.
taskUpperBody
=
Task
(
'task_upper_body'
)
robot
.
taskUpperBody
.
feature
=
FeaturePosture
(
'feature_upper_body'
)
q
=
list
(
robot
.
dynamic
.
position
.
value
)
robot
.
taskUpperBody
.
feature
.
state
.
value
=
q
robot
.
taskUpperBody
.
feature
.
posture
.
value
=
q
# robotDim = robot.dynamic.getDimension() # 38
robot
.
taskUpperBody
.
feature
.
selectDof
(
18
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
19
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
20
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
21
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
22
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
23
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
24
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
25
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
26
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
27
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
28
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
29
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
30
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
31
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
32
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
33
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
34
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
35
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
36
,
True
)
robot
.
taskUpperBody
.
feature
.
selectDof
(
37
,
True
)
robot
.
taskUpperBody
.
controlGain
.
value
=
100.0
robot
.
taskUpperBody
.
add
(
robot
.
taskUpperBody
.
feature
.
name
)
plug
(
robot
.
dynamic
.
position
,
robot
.
taskUpperBody
.
feature
.
state
)
# --- CONTACTS
#define contactLF and contactRF
robot
.
contactLF
=
MetaTaskKine6d
(
'contactLF'
,
robot
.
dynamic
,
'LF'
,
robot
.
OperationalPointsMap
[
'left-ankle'
])
robot
.
contactLF
.
feature
.
frame
(
'desired'
)
robot
.
contactLF
.
gain
.
setConstant
(
300
)
plug
(
robot
.
wp
.
footLeftDes
,
robot
.
contactLF
.
featureDes
.
position
)
#.errorIN?
locals
()[
'contactLF'
]
=
robot
.
contactLF
robot
.
contactRF
=
MetaTaskKine6d
(
'contactRF'
,
robot
.
dynamic
,
'RF'
,
robot
.
OperationalPointsMap
[
'right-ankle'
])
robot
.
contactRF
.
feature
.
frame
(
'desired'
)
robot
.
contactRF
.
gain
.
setConstant
(
300
)
plug
(
robot
.
wp
.
footRightDes
,
robot
.
contactRF
.
featureDes
.
position
)
#.errorIN?
locals
()[
'contactRF'
]
=
robot
.
contactRF
# --- COM height
robot
.
taskComH
=
MetaTaskKineCom
(
robot
.
dynamic
,
name
=
'comH'
)
plug
(
robot
.
wp
.
comDes
,
robot
.
taskComH
.
featureDes
.
errorIN
)
robot
.
taskComH
.
task
.
controlGain
.
value
=
100.
robot
.
taskComH
.
feature
.
selec
.
value
=
'100'
# --- COM
robot
.
taskCom
=
MetaTaskKineCom
(
robot
.
dynamic
)
plug
(
robot
.
com_admittance_control
.
comRef
,
robot
.
taskCom
.
featureDes
.
errorIN
)
plug
(
robot
.
com_admittance_control
.
dcomRef
,
robot
.
taskCom
.
featureDes
.
errordotIN
)
robot
.
taskCom
.
task
.
controlGain
.
value
=
0
robot
.
taskCom
.
task
.
setWithDerivative
(
True
)
robot
.
taskCom
.
feature
.
selec
.
value
=
'011'
# --- Waist
robot
.
keepWaist
=
MetaTaskKine6d
(
'keepWaist'
,
robot
.
dynamic
,
'WT'
,
robot
.
OperationalPointsMap
[
'waist'
])
robot
.
keepWaist
.
feature
.
frame
(
'desired'
)
robot
.
keepWaist
.
gain
.
setConstant
(
300
)
plug
(
robot
.
wp
.
waistDes
,
robot
.
keepWaist
.
featureDes
.
position
)
robot
.
keepWaist
.
feature
.
selec
.
value
=
'111000'
locals
()[
'keepWaist'
]
=
robot
.
keepWaist
# # --- Posture
# robot.taskPos = Task ('task_pos')
# robot.taskPos.feature = FeaturePosture('feature_pos')
#
# q = list(robot.dynamic.position.value)
# robot.taskPos.feature.state.value = q
# robot.taskPos.feature.posture.value = q
# robotDim = robot.dynamic.getDimension() # 38
#robot.taskPos.feature.selectDof(6,True)
#robot.taskPos.feature.selectDof(7,True)
#robot.taskPos.feature.selectDof(8,True)
#robot.taskPos.feature.selectDof(9,True)
#robot.taskPos.feature.selectDof(10,True)
#robot.taskPos.feature.selectDof(11,True)
#robot.taskPos.feature.selectDof(12,True)
#robot.taskPos.feature.selectDof(13,True)
#robot.taskPos.feature.selectDof(14,True)
#robot.taskPos.feature.selectDof(15,True)
#robot.taskPos.feature.selectDof(16,True)
#robot.taskPos.feature.selectDof(17,True)
#robot.taskPos.feature.selectDof(18,True)
#robot.taskPos.feature.selectDof(19,True)
#robot.taskPos.feature.selectDof(20,True)
#robot.taskPos.feature.selectDof(21,True)
#robot.taskPos.feature.selectDof(22,True)
#robot.taskPos.feature.selectDof(23,True)
#robot.taskPos.feature.selectDof(24,True)
#robot.taskPos.feature.selectDof(25,True)
#robot.taskPos.feature.selectDof(26,True)
#robot.taskPos.feature.selectDof(27,True)
#robot.taskPos.feature.selectDof(28,True)
#robot.taskPos.feature.selectDof(29,True)
#robot.taskPos.feature.selectDof(30,True)
#robot.taskPos.feature.selectDof(31,True)
#robot.taskPos.feature.selectDof(32,True)
#robot.taskPos.feature.selectDof(33,True)
#robot.taskPos.feature.selectDof(34,True)
#robot.taskPos.feature.selectDof(35,True)
#robot.taskPos.feature.selectDof(36,True)
#robot.taskPos.feature.selectDof(37,True)
#robot.taskPos.controlGain.value = 100.0
#robot.taskPos.add(robot.taskPos.feature.name)
#plug(robot.dynamic.position, robot.taskPos.feature.state)
# --- SOT solver
robot
.
sot
=
SOT
(
'sot'
)
robot
.
sot
.
setSize
(
robot
.
dynamic
.
getDimension
())
# --- Plug SOT control to device through control manager
plug
(
robot
.
sot
.
control
,
robot
.
cm
.
ctrl_sot_input
)
plug
(
robot
.
cm
.
u_safe
,
robot
.
device
.
control
)
robot
.
sot
.
push
(
robot
.
taskUpperBody
.
name
)
robot
.
sot
.
push
(
robot
.
contactRF
.
task
.
name
)
robot
.
sot
.
push
(
robot
.
contactLF
.
task
.
name
)
robot
.
sot
.
push
(
robot
.
taskComH
.
task
.
name
)
robot
.
sot
.
push
(
robot
.
taskCom
.
task
.
name
)
robot
.
sot
.
push
(
robot
.
keepWaist
.
task
.
name
)
# robot.sot.push(robot.taskPos.name)
# robot.device.control.recompute(0) # this crashes as it employs joint sensors which are not ready yet
# --- Fix robot.dynamic inputs
plug
(
robot
.
device
.
velocity
,
robot
.
dynamic
.
velocity
)
robot
.
dvdt
=
Derivator_of_Vector
(
"dv_dt"
)
robot
.
dvdt
.
dt
.
value
=
dt
plug
(
robot
.
device
.
velocity
,
robot
.
dvdt
.
sin
)
plug
(
robot
.
dvdt
.
sout
,
robot
.
dynamic
.
acceleration
)
# -------------------------- PLOTS --------------------------
# --- ROS PUBLISHER
robot
.
publisher
=
create_rospublish
(
robot
,
'robot_publisher'
)
create_topic
(
robot
.
publisher
,
robot
.
device
,
'state'
,
robot
=
robot
,
data_type
=
'vector'
)
create_topic
(
robot
.
publisher
,
robot
.
base_estimator
,
'q'
,
robot
=
robot
,
data_type
=
'vector'
)
#create_topic(robot.publisher, robot.stf, 'q', robot = robot, data_type='vector')
create_topic
(
robot
.
publisher
,
robot
.
comTrajGen
,
'x'
,
robot
=
robot
,
data_type
=
'vector'
)
# generated CoM
create_topic
(
robot
.
publisher
,
robot
.
comTrajGen
,
'dx'
,
robot
=
robot
,
data_type
=
'vector'
)
# generated CoM velocity
create_topic
(
robot
.
publisher
,
robot
.
comTrajGen
,
'ddx'
,
robot
=
robot
,
data_type
=
'vector'
)
# generated CoM acceleration
create_topic
(
robot
.
publisher
,
robot
.
wp
,
'comDes'
,
robot
=
robot
,
data_type
=
'vector'
)
# desired CoM
create_topic
(
robot
.
publisher
,
robot
.
cdc_estimator
,
'c'
,
robot
=
robot
,
data_type
=
'vector'
)
# estimated CoM
create_topic
(
robot
.
publisher
,
robot
.
cdc_estimator
,
'dc'
,
robot
=
robot
,
data_type
=
'vector'
)
# estimated CoM velocity
create_topic
(
robot
.
publisher
,
robot
.
com_admittance_control
,
'comRef'
,
robot
=
robot
,
data_type
=
'vector'
)
# reference CoM
create_topic
(
robot
.
publisher
,
robot
.
dynamic
,
'com'
,
robot
=
robot
,
data_type
=
'vector'
)
# resulting SOT CoM
create_topic
(
robot
.
publisher
,
robot
.
dcm_control
,
'dcmDes'
,
robot
=
robot
,
data_type
=
'vector'
)
# desired DCM
create_topic
(
robot
.
publisher
,
robot
.
estimator
,
'dcm'
,
robot
=
robot
,
data_type
=
'vector'
)
# estimated DCM
create_topic
(
robot
.
publisher
,
robot
.
zmpTrajGen
,
'x'
,
robot
=
robot
,
data_type
=
'vector'
)
# generated ZMP
create_topic
(
robot
.
publisher
,
robot
.
wp
,
'zmpDes'
,
robot
=
robot
,
data_type
=
'vector'
)
# desired ZMP
create_topic
(
robot
.
publisher
,
robot
.
dynamic
,
'zmp'
,
robot
=
robot
,
data_type
=
'vector'
)
# SOT ZMP
create_topic
(
robot
.
publisher
,
robot
.
zmp_estimator
,
'zmp'
,
robot
=
robot
,
data_type
=
'vector'
)
# estimated ZMP
create_topic
(
robot
.
publisher
,
robot
.
dcm_control
,
'zmpRef'
,
robot
=
robot
,
data_type
=
'vector'
)
# reference ZMP
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic
(
robot
.
publisher
,
robot
.
waistTrajGen
,
'x'
,
robot
=
robot
,
data_type
=
'vector'
)
# desired waist orientation
create_topic
(
robot
.
publisher
,
robot
.
lfTrajGen
,
'x'
,
robot
=
robot
,
data_type
=
'vector'
)
# desired left foot pose
create_topic
(
robot
.
publisher
,
robot
.
rfTrajGen
,
'x'
,
robot
=
robot
,
data_type
=
'vector'
)
# desired right foot pose
create_topic
(
robot
.
publisher
,
robot
.
ftc
,
'left_foot_force_out'
,
robot
=
robot
,
data_type
=
'vector'
)
# calibrated left wrench
create_topic
(
robot
.
publisher
,
robot
.
ftc
,
'right_foot_force_out'
,
robot
=
robot
,
data_type
=
'vector'
)
# calibrated right wrench
create_topic
(
robot
.
publisher
,
robot
.
dynamic
,
'LF'
,
robot
=
robot
,
data_type
=
'matrixHomo'
)
# left foot
create_topic
(
robot
.
publisher
,
robot
.
dynamic
,
'RF'
,
robot
=
robot
,
data_type
=
'matrixHomo'
)
# right foot
# --- TRACER
robot
.
tracer
=
TracerRealTime
(
"com_tracer"
)
robot
.
tracer
.
setBufferSize
(
80
*
(
2
**
20
))
robot
.
tracer
.
open
(
'/tmp'
,
'dg_'
,
'.dat'
)
robot
.
device
.
after
.
addSignal
(
'{0}.triger'
.
format
(
robot
.
tracer
.
name
))
addTrace
(
robot
.
tracer
,
robot
.
wp
,
'comDes'
)
# desired CoM
addTrace
(
robot
.
tracer
,
robot
.
cdc_estimator
,
'c'
)
# estimated CoM
addTrace
(
robot
.
tracer
,
robot
.
cdc_estimator
,
'dc'
)
# estimated CoM velocity
addTrace
(
robot
.
tracer
,
robot
.
com_admittance_control
,
'comRef'
)
# reference CoM
addTrace
(
robot
.
tracer
,
robot
.
dynamic
,
'com'
)
# resulting SOT CoM
addTrace
(
robot
.
tracer
,
robot
.
dcm_control
,
'dcmDes'
)
# desired DCM
addTrace
(
robot
.
tracer
,
robot
.
estimator
,
'dcm'
)
# estimated DCM
addTrace
(
robot
.
tracer
,
robot
.
dcm_control
,
'zmpDes'
)
# desired ZMP
addTrace
(
robot
.
tracer
,
robot
.
dynamic
,
'zmp'
)
# SOT ZMP
addTrace
(
robot
.
tracer
,
robot
.
zmp_estimator
,
'zmp'
)
# estimated ZMP
addTrace
(
robot
.
tracer
,
robot
.
dcm_control
,
'zmpRef'
)
# reference ZMP
addTrace
(
robot
.
tracer
,
robot
.
ftc
,
'left_foot_force_out'
)
# calibrated left wrench
addTrace
(
robot
.
tracer
,
robot
.
ftc
,
'right_foot_force_out'
)
# calibrated right wrench
addTrace
(
robot
.
tracer
,
robot
.
dynamic
,
'LF'
)
# left foot
addTrace
(
robot
.
tracer
,
robot
.
dynamic
,
'RF'
)
# right foot
robot
.
tracer
.
start
()
scripts/appli_online_walking.py
deleted
100644 → 0
View file @
651da839
# flake8: noqa
import
sys
from
math
import
sqrt
import
numpy
as
np
from
dynamic_graph
import
plug
from
dynamic_graph.sot.core
import
SOT
,
Derivator_of_Vector
from
dynamic_graph.sot.core
import
FeaturePosture
,
MatrixHomoToPoseQuaternion
from
dynamic_graph.sot.core
import
Task
,
RPYToMatrix
from
dynamic_graph.sot.core.matrix_util
import
matrixToTuple
# RPYToMatrix deuxieme pour essai ajout waistattitudeabsolute
from
dynamic_graph.sot.core.meta_tasks_kine
import
MetaTaskKine6d
from
dynamic_graph.sot.core.meta_tasks_kine
import
MetaTaskKineCom
,
gotoNd
from
dynamic_graph.sot.dynamic_pinocchio
import
DynamicPinocchio
from
dynamic_graph.tracer_real_time
import
TracerRealTime
from
rospkg
import
RosPack
import
sot_talos_balance.talos.base_estimator_conf
as
base_estimator_conf
import
sot_talos_balance.talos.control_manager_conf
as
cm_conf
import
sot_talos_balance.talos.ft_calibration_conf
as
ft_conf
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
*
from
dynamic_graph.sot.pattern_generator
import
PatternGenerator
# 09.04.20 est a 100 dans dcmZmpControl_file, used to be 10
cm_conf
.
CTRL_MAX
=
100.0
# temporary hack
robot
.
timeStep
=
robot
.
device
.
getTimeStep
()
dt
=
robot
.
timeStep
# --- Pendulum parameters
robot_name
=
'robot'
robot
.
dynamic
.
com
.
recompute
(
0
)
robotDim
=
robot
.
dynamic
.
getDimension
()