Commit d38981af authored by Olivier Stasse's avatar Olivier Stasse Committed by olivier stasse
Browse files

[python] Clean and rename file to test online walking generator.

parent f184d9bd
# flake8: noqa
## File adapted from appli_dcmZmpControl_file.py and appli_dcmZmpControl_online_ISA.py written by Isabelle Maroger
## ALL THINGS ADDED OR CHANGED ARE COMMENTED IN CAPITAL LETTERS
# ADDED
import sys
# END ADDED
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, 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, MetaTaskKineCom, gotoNd
# ERROR ON 09.04 from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio # NOTE: IS IN /integration_tests/robotpkg-test-rc/install/lib/python 2.7/site-packages/dynamic_graph/sot/dynamics_pinocchio
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio # essai sans le 's' de dynamics => PROBLEME REGLE ! =D
from dynamic_graph.sot.core import SOT, Derivator_of_Vector, FeaturePosture
from dynamic_graph.sot.core import MatrixHomoToPoseQuaternion, Task, RPYToMatrix
from dynamic_graph.sot.core.matrix_util import matrixToTuple
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
......@@ -25,18 +22,10 @@ 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 *
# Ajout 24.03.20, pour mise a jour des parametres waist, mauvaise fonction
#from sot_talos_balance.utils.plot_utils import dump_sot_sig
# fin ajout
# ADDED FROM APPLI ONLINE ISABELLE #
#from dynamic_graph.sot.pattern_generator import PatternGenerator #ERROR: "DOESN't EXIST", indeed ...
# TRYING IMPORTING THE PACKAGE FROM DEVEL-SRC
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
# This will only work in simulation ! WARNING !
cm_conf.CTRL_MAX = 100.0
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
......@@ -61,106 +50,16 @@ robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)
# -------------------------- DESIRED TRAJECTORY --------------------------
rospack = RosPack() ## DO I NEED THAT ?
## COMMENTING EVERYTHING RELATED TO READING A PRE-EXISTING FILE
#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)
#
## ADDED: THE PG OF M.NAVEAU FROM ISABELLE'S CODE
# -------------------------- PATTERN GENERATOR --------------------------
robot.pg = PatternGenerator('pg')
# MODIFIED WITH MY PATHS
# Specify the robot model TODO: use get_parameter through ParameterServer
# DO USE FILE ANYMORE
rospack = RosPack()
talos_data_folder = rospack.get_path('talos_data')
robot.pg.setURDFpath(talos_data_folder+'/urdf/talos_reduced_wpg.urdf')
robot.pg.setSRDFpath(talos_data_folder+'/srdf/talos_wpg.srdf')
## END MODIFIED
robot.pg.buildModel()
......@@ -182,7 +81,6 @@ robot.pg.parseCmd(":SetAlgoForZmpTrajectory Naveau")
plug(robot.dynamic.position,robot.pg.position)
plug(robot.dynamic.com, robot.pg.com)
#plug(robot.dynamic.com, robot.pg.comStateSIN)
plug(robot.dynamic.LF, robot.pg.leftfootcurrentpos)
plug(robot.dynamic.RF, robot.pg.rightfootcurrentpos)
robotDim = len(robot.dynamic.velocity.value)
......@@ -203,14 +101,6 @@ robot.pg.parseCmd(':useDynamicFilter true')
robot.pg.velocitydes.value=(0.1,0.0,0.0) # DEFAULT VALUE (0.1,0.0,0.0)
# Ajout 24.03.20 -> pas la bonne fonction
#dump_sot_sig(robot, robot.pg, 'waistattitudematrix', dt) # quel duration ? essai avec 0.1, 1., dt, 0.005
#
# 27.03.20 la fonction
#robot.device.after.addSignal('robot.pg.waistattitudeabsolute')
#robot.pg.displaySignals()
# -------------------------- TRIGGER --------------------------
......@@ -222,39 +112,15 @@ plug(robot.triggerPG.sout,robot.pg.trigger)
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
# #wp.displaySignals()
wp.omega.value = omega
# 31.03.20 essai sur le waist
#robot.waistattitudeabTOmatrixHomo = RPYToMatrix('waistattitudeabTOmatrixHomo') # marche pas, je peux pas rajouter ca dans robot
#waistattitudeabTOmatrixHomo = RPYToMatrix('waistattitudeabTOmatrixHomo') # pas non plus
#waistattitudeabTOmatrixHomo = RPYToMatrix('waistattitudeabTOmatrixHomo')
#plug(robot.pg.waistattitudeabsolute, waistattitudeabTOmatrixHomo.sin)
#plug(waistattitudeabTOmatrixHomo.sout, wp.waist)
# ca tout seul marche pas
#plug(RPYToMatrix(robot.pg.waistattitudeabsolute), wp.waist)
# j etais la dessus apparemment
#robot.waistToMatrix = RPYToMatrix('w2m')
#plug(robot.waistMix.sout, robot.waistToMatrix.sin)
#20.04
#robot.waistToMatrix = RPYToMatrix('w2m')
#plug(robot.pg.waistattitudeabsolute, robot.waistToMatrix.sin) # works
#plug(robot.waistToMatrix.sout, wp.waist)# waistToMatrix sort une matrice de rotation, waist prend une matrixHomo en entree...
# 22.04 after modifying pg.cpp, new way to try and connect the waist
# Plug Pattern generator to the dummy one.
plug(robot.pg.waistattitudematrixabsolute, wp.waist)
#plug(robot.pg.waistattitudematrix, wp.waist)
# fin
plug(robot.pg.leftfootref, wp.footLeft)
plug(robot.pg.rightfootref, wp.footRight)
plug(robot.pg.comref, wp.com)
plug(robot.pg.dcomref, wp.vcom)
plug(robot.pg.ddcomref, wp.acom)
#plug(robot.zmpTrajGen.x, wp.zmp)
robot.wp = wp
......@@ -263,8 +129,6 @@ robot.wp.comDes.recompute(0)
robot.wp.dcmDes.recompute(0)
robot.wp.zmpDes.recompute(0)
## END ADDED
# -------------------------- ESTIMATION --------------------------
# --- Base Estimation
......@@ -279,25 +143,6 @@ 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)
......@@ -458,64 +303,13 @@ robot.taskCom.feature.selec.value = '011'
# --- Waist
# Ajout 24.03.20, se met a jour tous les combien ??? A chaque cycle. Essai de le mettre plus haut, au niveau pg
#dump_sot_sig(robot, robot.pg, 'waistattitudematrix', 0.005) # quel duration ? essai avec 0.1, 1., dt, 0.005
#
robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT', robot.OperationalPointsMap['waist'])
robot.keepWaist.feature.frame('desired')
robot.keepWaist.gain.setConstant(300)
#plug(robot.wp.waist, robot.keepWaist.featureDes.position)
#plug(robot.wp.waist, robot.keepWaist.feature.position)
plug(robot.wp.waistDes, robot.keepWaist.featureDes.position) #de base
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')
......@@ -531,8 +325,6 @@ 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)
......@@ -552,12 +344,10 @@ robot.publisher = create_rospublish(robot, 'robot_publisher')
## ADDED
create_topic(robot.publisher, robot.pg, 'comref', robot = robot, data_type='vector') # desired CoM
create_topic(robot.publisher, robot.pg, 'dcomref', robot = robot, data_type='vector')
#create_topic(robot.publisher, robot.wp, 'waistDes', robot = robot, data_type='matrixHomo')
create_topic(robot.publisher, robot.wp, 'waist', robot = robot, data_type='matrixHomo')
create_topic(robot.publisher, robot.keepWaist.featureDes, 'position', robot = robot, data_type='matrixHomo')
create_topic(robot.publisher, robot.dynamic, 'WT', robot = robot, data_type='matrixHomo')
create_topic(robot.publisher, robot.pg, 'waistattitudematrixabsolute', robot = robot, data_type='matrixHomo') ## que font ces lignes exactement ??
#create_topic(robot.publisher, robot.pg, 'waistattitudeabsolute', robot = robot, data_type='vectorRPY')
create_topic(robot.publisher, robot.pg, 'leftfootref', robot = robot, data_type ='matrixHomo')
create_topic(robot.publisher, robot.wp, 'footLeft', robot = robot, data_type ='matrixHomo')
......@@ -617,29 +407,11 @@ create_topic(robot.publisher, robot.wp, 'footRight', robot = robot, data_type ='
## --- TRACER
robot.tracer = TracerRealTime("com_tracer")
robot.tracer.setBufferSize(80 * (2**20))
#robot.tracer.open('/tmp', 'dg_', '.dat') ## THIS LINE DIFFERENT, TO CHECK, BELOW SOME NAMES DIFFERENT, CHECK AS WELL
#REPLACED BY
robot.tracer.open('/tmp', 'dg_', '.dat')
#END REPLACED
# 24.03.20 QUE FAIT CE TRUC ??
# Info trouvee sur internet: 'Make sure signals are recomputed even if not used in the control graph'
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
# 30.03
#robot.device.after.addSignal('robot.pg.waist...') renvoie 'entity not found'
#robot.device.after.addSignal('pg.waistattitudeabsolute')
# 23.04
#robot.device.after.addSignal('pg.waistattitudematrix') #not needed to update waist values time in the end
addTrace(robot.tracer, robot.pg, 'waistattitudeabsolute')
# fin
addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
......@@ -647,7 +419,7 @@ 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
# REPLACED BY and ADDED
addTrace(robot.tracer, robot.pg, 'comref')
addTrace(robot.tracer, robot.pg, 'dcomref')
addTrace(robot.tracer, robot.pg, 'ddcomref')
......@@ -658,23 +430,9 @@ addTrace(robot.tracer, robot.pg, 'leftfootref')
addTrace(robot.tracer, robot.pg, 'rightfootcontact')
addTrace(robot.tracer, robot.pg, 'leftfootcontact')
addTrace(robot.tracer, robot.pg, 'SupportFoot')
# END REPLACED AND ADDED
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
# FOLLOWING LINES NOT IN ISA'S CODE
#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
# END COMMENTED PART
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
......
'''Test CoM admittance control as described in paper, with a Reference Velocity provided'''
## File adapted from appli_dcmZmpControl_file.py and appli_dcmZmpControl_online_ISA.py written by Isabelle Maroger
## File adapted from appli_dcmZmpControl_file.py and
## appli_dcmZmpControl_online_ISA.py written by Isabelle Maroger
## ALL THINGS ADDED OR CHANGED ARE COMMENTED IN CAPITAL LETTERS
from sys import argv
#from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient
## INSTEAD, ISA WROTE (CHECK DIFFERENCES)
from sot_talos_balance.utils.run_test_utils import *
from time import sleep
## END DIFFERENCE
from dynamic_graph import * # for entity graph display
## NEXT FEW LINES IRRELEVENT AS WISH TO GO ONLINE --> COMMENTED
#try:
# # Python 2
# input = raw_input # noqa
#except NameError:
# pass
#
#test_folder = argv[1] if len(argv) > 1 else 'TestKajita2003WalkingOnSpot64/DSP20SSP780'
#print('Using folder ' + test_folder)
#
#runCommandClient('test_folder = "' + test_folder + '"')
## END DIFFERENCE
run_test('appli_joystick_dcmZmpControl_file.py')
run_test('appli_joystick_dcmZmpControl.py')
run_ft_calibration('robot.ftc')
input("Wait before running the test")
......@@ -43,9 +28,7 @@ runCommandClient('robot.dcm_control.Ki.value = Ki_dcm')
c = ask_for_confirmation('Execute trajectory?')
if c:
print('Executing the trajectory')
# runCommandClient('robot.triggerTrajGen.sin.value = 1') # NAMED DIFFERENTLY IN ISA'S SCRIPT, SEE BELOW
runCommandClient('robot.triggerPG.sin.value = 1')
writeGraph('/local/lscherrer/lscherrer/Scripts/Results/my_dyn_graph.dot')
else:
print('Not executing the trajectory')
......@@ -54,6 +37,4 @@ input("Wait before dumping the data")
runCommandClient('dump_tracer(robot.tracer)')
## ADDED ;)
print('Bye!')
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment