Skip to content
Snippets Groups Projects
Select Git revision
  • devel
  • master default protected
  • topic/initial_cmake
  • v2.0.5
  • v2.0.4
  • v2.0.3
  • v2.0.2
  • v2.0.1
  • v2.0.0
  • v1.9.2
  • v1.9.1
  • v1.9.0
  • v1.8.1
  • v1.8.0
  • v1.7.0
  • v1.6.0
  • v1.5.0
  • v1.4.0
  • v1.3.1
  • v1.3.0
  • v1.2.0
  • v1.1.0
  • v1.0.0
23 results
Blame Permalink
Forked from loco-3d / sot-talos-balance
534 commits behind the upstream repository.
appli_dcmComZmpControl.py 2.80 KiB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75 






from sot_talos_balance.create_entities_utils import create_com_admittance_controller, create_dummy_dcm_estimator, create_dcm_com_controller
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT

from dynamic_graph.tracer_real_time import TracerRealTime
from sot_talos_balance.create_entities_utils import addTrace, dump_tracer

robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep;

# --- Dummy estimator
robot.estimator = create_dummy_dcm_estimator(robot)

# --- DCM controller
Kp_dcm = [500.0,500.0,500.0]
Ki_dcm = [1.0,1.0,0.0] # to be set later
robot.dcm_control = create_dcm_com_controller(Kp_dcm,[0.0]*3,dt,robot,robot.estimator.dcm)

# --- Admittance controller
Kp_adm = [20.0,10.0,0.0] # to be set later
robot.com_admittance_control = create_com_admittance_controller([0.0]*3,dt,robot)

# --- 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)
robot.contactLF.keep()
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)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF

# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
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.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control,robot.device.control)

robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0)

# --- TRACER
robot.tracer = TracerRealTime("zmp_tracer")
robot.tracer.setBufferSize(80*(2**20))
robot.tracer.open('/tmp','dg_','.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
robot.device.after.addSignal('{0}.zmpRef'.format(robot.dcm_control.name))
robot.device.after.addSignal('{0}.zmp'.format(robot.dynamic.name)) # why needed?
robot.device.after.addSignal('{0}.comRef'.format(robot.com_admittance_control.name)) # why needed?

addTrace(robot.tracer, robot.dynamic, 'zmp')
addTrace(robot.tracer, robot.dcm_control, 'zmpRef')
addTrace(robot.tracer, robot.estimator, 'dcm')
addTrace(robot.tracer, robot.dynamic, 'com')
addTrace(robot.tracer, robot.com_admittance_control, 'comRef')

# SIMULATION

robot.tracer.start()