sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_coupled_ankle_admittance.py
Go to the documentation of this file.
1 # flake8: noqa
2 from dynamic_graph import plug
3 from dynamic_graph.ros import RosPublish, RosSubscribe
4 from dynamic_graph.sot.core import SOT, FeaturePosture, Task
5 from dynamic_graph.sot.core.meta_tasks_kine import (
6  MetaTaskKine6d,
7  MetaTaskKineCom,
8  gotoNd,
9 )
10 from dynamic_graph.sot.core.operator import Add_of_vector, Selec_of_vector
11 
12 from dynamic_graph.sot_talos_balance.coupled_admittance_controller import (
13  CoupledAdmittanceController,
14 )
15 from dynamic_graph.sot_talos_balance.create_entities_utils import *
16 from dynamic_graph.sot_talos_balance.meta_task_joint import MetaTaskKineJoint
17 
18 N_JOINTS = 32
19 N_CONFIG = N_JOINTS + 6
20 
21 robot.timeStep = robot.device.getTimeStep()
22 timeStep = robot.timeStep
23 
24 RightPitchJoint = 10
25 RightRollJoint = 11
26 LeftPitchJoint = 4
27 LeftRollJoint = 5
28 
29 kSumPitch = [-0.000]
30 kSumRoll = [0.001]
31 kDiffPitch = [-0.000]
32 kDiffRoll = [0.001]
33 tauDesRoll = [0.0]
34 tauDesPitch = [0.0]
35 
36 # ---------- ADMITTANCE CONTROLLERS ----------
37 # --- PITCH
38 robot.tauselecRP = Selec_of_vector("tauselecRP")
39 robot.tauselecRP.selec(RightPitchJoint, RightPitchJoint + 1)
40 plug(robot.device.ptorque, robot.tauselecRP.sin)
41 
42 robot.tauselecLP = Selec_of_vector("tauselecLP")
43 robot.tauselecLP.selec(LeftPitchJoint, LeftPitchJoint + 1)
44 plug(robot.device.ptorque, robot.tauselecLP.sin)
45 
46 robot.pitchController = CoupledAdmittanceController("pitchController")
47 robot.pitchController.kSum.value = kSumPitch
48 robot.pitchController.kDiff.value = kDiffPitch
49 robot.pitchController.tauDesL.value = tauDesPitch
50 robot.pitchController.tauDesR.value = tauDesPitch
51 plug(robot.tauselecRP.sout, robot.pitchController.tauR)
52 plug(robot.tauselecLP.sout, robot.pitchController.tauL)
53 
54 # --- ROLL
55 robot.tauselecRR = Selec_of_vector("tauselecRR")
56 robot.tauselecRR.selec(RightRollJoint, RightRollJoint + 1)
57 plug(robot.device.ptorque, robot.tauselecRR.sin)
58 
59 robot.tauselecLR = Selec_of_vector("tauselecLR")
60 robot.tauselecLR.selec(LeftRollJoint, LeftRollJoint + 1)
61 plug(robot.device.ptorque, robot.tauselecLR.sin)
62 
63 robot.rollController = CoupledAdmittanceController("rollController")
64 robot.rollController.kSum.value = kSumRoll
65 robot.rollController.kDiff.value = kDiffRoll
66 robot.rollController.tauDesL.value = tauDesRoll
67 robot.rollController.tauDesR.value = tauDesRoll
68 plug(robot.tauselecRR.sout, robot.rollController.tauR)
69 plug(robot.tauselecLR.sout, robot.rollController.tauL)
70 
71 # ---------- TASKS ----------
72 # RIGHT PITCH
73 robot.taskRP = MetaTaskKineJoint(robot.dynamic, RightPitchJoint + 6)
74 robot.taskRP.task.controlGain.value = 0
75 robot.taskRP.task.setWithDerivative(True)
76 robot.taskRP.featureDes.errorIN.value = [0.0]
77 plug(robot.pitchController.dqRefR, robot.taskRP.featureDes.errordotIN)
78 
79 # RIGHT ROLL
80 robot.taskRR = MetaTaskKineJoint(robot.dynamic, RightRollJoint + 6)
81 robot.taskRR.task.controlGain.value = 0
82 robot.taskRR.task.setWithDerivative(True)
83 robot.taskRR.featureDes.errorIN.value = [0.0]
84 plug(robot.rollController.dqRefR, robot.taskRR.featureDes.errordotIN)
85 
86 # --- LEFT PITCH
87 robot.taskLP = MetaTaskKineJoint(robot.dynamic, LeftPitchJoint + 6)
88 robot.taskLP.task.controlGain.value = 0
89 robot.taskLP.task.setWithDerivative(True)
90 robot.taskLP.featureDes.errorIN.value = [0.0]
91 plug(robot.pitchController.dqRefL, robot.taskLP.featureDes.errordotIN)
92 
93 # --- LEFT ROLL
94 robot.taskLR = MetaTaskKineJoint(robot.dynamic, LeftRollJoint + 6)
95 robot.taskLR.task.controlGain.value = 0
96 robot.taskLR.task.setWithDerivative(True)
97 robot.taskLR.featureDes.errorIN.value = [0.0]
98 plug(robot.rollController.dqRefL, robot.taskLR.featureDes.errordotIN)
99 
100 # -------------------------- SOT CONTROL --------------------------
101 # --- Posture
102 robot.taskPosture = Task("taskPosture")
103 robot.taskPosture.feature = FeaturePosture("featurePosture")
104 
105 q = list(robot.dynamic.position.value)
106 robot.taskPosture.feature.state.value = q
107 robot.taskPosture.feature.posture.value = q
108 
109 for i in range(6, 38):
110  robot.taskPosture.feature.selectDof(i, True)
111 
112 robot.taskPosture.controlGain.value = 100.0
113 robot.taskPosture.add(robot.taskPosture.feature.name)
114 plug(robot.dynamic.position, robot.taskPosture.feature.state)
115 
116 # --- SOT
117 robot.sot = SOT("sot")
118 robot.sot.setSize(robot.dynamic.getDimension())
119 plug(robot.sot.control, robot.device.control)
120 
121 robot.sot.push(robot.taskRP.task.name)
122 robot.sot.push(robot.taskLP.task.name)
123 robot.sot.push(robot.taskRR.task.name)
124 robot.sot.push(robot.taskLR.task.name)
125 robot.sot.push(robot.taskPosture.name)
126 robot.device.control.recompute(0)
127 
128 # -------------------------- PLOTS --------------------------
129 # --- ROS PUBLISHER
130 robot.publisher = create_rospublish(robot, "robot_publisher")
131 
132 create_topic(
133  robot.publisher, robot.pitchController, "tauL", robot=robot, data_type="vector"
134 )
135 create_topic(
136  robot.publisher, robot.rollController, "tauL", robot=robot, data_type="vector"
137 )
138 create_topic(
139  robot.publisher, robot.pitchController, "tauR", robot=robot, data_type="vector"
140 )
141 create_topic(
142  robot.publisher, robot.rollController, "tauR", robot=robot, data_type="vector"
143 )
144 
145 create_topic(
146  robot.publisher, robot.pitchController, "tauDesL", robot=robot, data_type="vector"
147 )
148 create_topic(
149  robot.publisher, robot.rollController, "tauDesL", robot=robot, data_type="vector"
150 )
151 create_topic(
152  robot.publisher, robot.pitchController, "tauDesR", robot=robot, data_type="vector"
153 )
154 create_topic(
155  robot.publisher, robot.rollController, "tauDesR", robot=robot, data_type="vector"
156 )
157 
158 create_topic(
159  robot.publisher, robot.pitchController, "dqRefL", robot=robot, data_type="vector"
160 )
161 create_topic(
162  robot.publisher, robot.rollController, "dqRefL", robot=robot, data_type="vector"
163 )
164 create_topic(
165  robot.publisher, robot.pitchController, "dqRefR", robot=robot, data_type="vector"
166 )
167 create_topic(
168  robot.publisher, robot.rollController, "dqRefR", robot=robot, data_type="vector"
169 )
170 
171 # # --- ROS SUBSCRIBER
172 robot.subscriber = RosSubscribe("ankle_joint_subscriber")
173 
174 robot.subscriber.add("vector", "tauRP", "/sot/pitchController/tauL")
175 robot.subscriber.add("vector", "tauRR", "/sot/rollController/tauR")
176 robot.subscriber.add("vector", "tauLP", "/sot/pitchController/tauL")
177 robot.subscriber.add("vector", "tauLR", "/sot/rollController/tauR")
178 
179 robot.subscriber.add("vector", "tauDesRP", "/sot/pitchController/tauDesL")
180 robot.subscriber.add("vector", "tauDesRR", "/sot/rollController/tauDesR")
181 robot.subscriber.add("vector", "tauDesLP", "/sot/pitchController/tauDesL")
182 robot.subscriber.add("vector", "tauDesLR", "/sot/rollController/tauDesR")
183 
184 robot.subscriber.add("vector", "dqRefRP", "/sot/pitchController/dqRefL")
185 robot.subscriber.add("vector", "dqRefRR", "/sot/rollController/dqRefR")
186 robot.subscriber.add("vector", "dqRefLP", "/sot/pitchController/dqRefL")
187 robot.subscriber.add("vector", "dqRefLR", "/sot/rollController/dqRefR")
create_entities_utils
sot_talos_balance.create_entities_utils.create_topic
def create_topic(rospub, entity, signalName, robot=None, data_type="vector")
Definition: create_entities_utils.py:454
sot_talos_balance.create_entities_utils.create_rospublish
def create_rospublish(robot, name)
Definition: create_entities_utils.py:446