Skip to content

Commit 166b4b1

Browse files
committed
improved documentation of robot state
1 parent 645f470 commit 166b4b1

File tree

3 files changed

+858
-809
lines changed

3 files changed

+858
-809
lines changed

cri_lib/robot_state.py

Lines changed: 91 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,8 @@
33

44

55
class RobotMode(Enum):
6+
"""Enum of possible robot modes for jogging, `FSM` does not support jogging."""
7+
68
JOINT = "joint"
79
CARTBASE = "cartbase"
810
CARTTOOL = "carttool"
@@ -11,6 +13,8 @@ class RobotMode(Enum):
1113

1214

1315
class KinematicsState(Enum):
16+
"""Enum of possible states of kinematics"""
17+
1418
NO_ERROR = 0
1519
JOINT_MIN = 13
1620
JOINT_MAX = 14
@@ -27,19 +31,25 @@ class KinematicsState(Enum):
2731

2832

2933
class OperationMode(Enum):
34+
"""Enum of possible operartion modes"""
35+
3036
NOT_ENABLED = -1
3137
NORMAL = 0
3238
MANUAL = 1
3339
MOTION_NOT_ALLOWED = 2
3440

3541

3642
class RunState(Enum):
43+
"""Enum of possible run states"""
44+
3745
STOPPED = 0
3846
PAUSED = 1
3947
RUNNING = 2
4048

4149

4250
class ReplayMode(Enum):
51+
"""Enum of possible replay modes"""
52+
4353
SINGLE = 0
4454
REPEAT = 1
4555
STEP = 2
@@ -48,6 +58,8 @@ class ReplayMode(Enum):
4858

4959
@dataclass
5060
class ErrorStates:
61+
"""error states of axes, multiple errors can apply"""
62+
5163
over_temp: bool = False
5264
estop_lowv: bool = False
5365
motor_not_enabled: bool = False
@@ -60,6 +72,8 @@ class ErrorStates:
6072

6173
@dataclass
6274
class RobotCartesianPosition:
75+
"""Represents the cartesian position of a robot"""
76+
6377
X: float = 0.0
6478
Y: float = 0.0
6579
Z: float = 0.0
@@ -70,13 +84,17 @@ class RobotCartesianPosition:
7084

7185
@dataclass
7286
class PlatformCartesianPosition:
87+
"""Represents the cartesian position of a platform"""
88+
7389
X: float = 0.0
7490
Y: float = 0.0
7591
RZ: float = 0.0
7692

7793

7894
@dataclass
7995
class JointsState:
96+
"""Represents the joints state of a robot"""
97+
8098
A1: float = 0.0
8199
A2: float = 0.0
82100
A3: float = 0.0
@@ -97,6 +115,8 @@ class JointsState:
97115

98116
@dataclass
99117
class PosVariable:
118+
"""Represents a position variable"""
119+
100120
X: float = 0.0
101121
Y: float = 0.0
102122
Z: float = 0.0
@@ -116,6 +136,8 @@ class PosVariable:
116136

117137
@dataclass
118138
class OperationInfo:
139+
"""Operation statistics sent by the robot controler"""
140+
119141
program_starts_total: int = 0
120142
up_time_complete: float = 0.0
121143
up_time_enabled: float = 0.0
@@ -126,13 +148,18 @@ class OperationInfo:
126148

127149

128150
class ReferencingAxisState(Enum):
151+
"""Enum of possible referencing states of an axis"""
152+
129153
NOT_REFERENCED = 0
130154
REFERENCED = 1
131155
REFERENCING = 2
132156

157+
133158
@dataclass
134159
class ReferencingState:
135-
global_state:ReferencingAxisState = ReferencingAxisState.NOT_REFERENCED
160+
"""Represents the overall referencing state of the robot."""
161+
162+
global_state: ReferencingAxisState = ReferencingAxisState.NOT_REFERENCED
136163
mandatory: bool = True
137164
ref_prog_enabled: bool = False
138165
ref_prog_running: bool = False
@@ -149,7 +176,7 @@ class ReferencingState:
149176
E4: ReferencingAxisState = ReferencingAxisState.NOT_REFERENCED
150177
E5: ReferencingAxisState = ReferencingAxisState.NOT_REFERENCED
151178
E6: ReferencingAxisState = ReferencingAxisState.NOT_REFERENCED
152-
179+
153180

154181
@dataclass
155182
class RobotState:
@@ -158,83 +185,144 @@ class RobotState:
158185
"""
159186

160187
mode: RobotMode = RobotMode.JOINT
188+
"""Robot execution mode"""
189+
161190
joints_set_point: JointsState = field(default_factory=JointsState)
191+
"""Set point of robot joints"""
192+
162193
joints_current: JointsState = field(default_factory=JointsState)
194+
"""Actual values of robot joints"""
163195

164196
position_robot: RobotCartesianPosition = field(
165197
default_factory=RobotCartesianPosition
166198
)
199+
"""Current cartesian position of robot"""
200+
167201
position_platform: PlatformCartesianPosition = field(
168202
default_factory=PlatformCartesianPosition
169203
)
204+
"""Current cartesian position of platform"""
170205

171206
cart_speed_mm_per_s: float = 0.0
207+
"""current speed of the cart in mm/s"""
172208

173209
override: float = 100.0
210+
"""global robot speed override"""
174211

175212
din: list[bool] = field(default_factory=lambda: [False] * 64)
213+
"""digital ins"""
214+
176215
dout: list[bool] = field(default_factory=lambda: [False] * 64)
216+
"""digital outs"""
177217

178218
emergency_stop_ok = False
219+
"""`True` if emergency stop circuit is closed"""
220+
179221
main_relay = False
222+
"""`True` if main power relay is closed"""
180223

181224
supply_voltage: float = 0.0
225+
"""current supply voltage"""
226+
182227
battery_percent: float = 0.0
228+
"""battery percent of mobile platform"""
183229

184230
current_total = 0.0
231+
"""total current drawn by robot"""
185232

186233
current_joints: list[float] = field(default_factory=lambda: [0.0] * 16)
234+
"""current drawn by individual axes"""
187235

188236
kinematics_state: KinematicsState = KinematicsState.MOTION_NOT_ALLOWED
237+
"""global kinematics state"""
189238

190239
operation_mode: OperationMode = OperationMode.NOT_ENABLED
240+
"""global operation mode"""
191241

192242
global_signals: list[bool] = field(default_factory=lambda: [False] * 128)
243+
"""global signals"""
193244

194245
frame_name: str = ""
246+
"""name of currently active frame"""
247+
195248
frame_position_current: RobotCartesianPosition = field(
196249
default_factory=RobotCartesianPosition
197250
)
251+
"""position in currently active frame"""
198252

199253
main_main_program: str = ""
254+
"""main program of main interpreter"""
255+
200256
main_current_program: str = ""
257+
"""currently active program of main interpreter, can be different than main_main_program in case of sub programm"""
201258

202259
logic_main_program: str = ""
260+
"""main programm of logic interpreter"""
261+
203262
logic_current_program: str = ""
263+
"""currently active program of logic interpreter, can be different than logic_main_program in case of sub programm"""
204264

205265
main_commands_count: int = 0
266+
"""total number of commands in main program"""
267+
206268
logic_commands_count: int = 0
269+
"""total number of commands in logic program"""
207270

208271
main_current_command: int = 0
272+
"""index of currently executed command in main interpreter"""
273+
209274
logic_current_command: int = 0
275+
"""index of currently executed command in logic interpreter"""
210276

211277
main_runstate: RunState = RunState.STOPPED
278+
"""runstate of main interpreter"""
279+
212280
logic_runstate: RunState = RunState.STOPPED
281+
"""runstate of logic interpreter"""
213282

214283
main_replay_mode: ReplayMode = ReplayMode.SINGLE
284+
"""replay mode of main interpreter"""
285+
215286
logic_replay_mode: ReplayMode = ReplayMode.SINGLE
287+
"""replay mode of logic interpreter"""
216288

217289
error_states: list[ErrorStates] = field(
218290
default_factory=lambda: [ErrorStates()] * 16
219291
)
292+
"""error states of individual axes"""
220293

221294
cycle_time: float = 0.0
295+
"""cycle time of robot control loop"""
296+
222297
workload: float = 0.0
298+
"""workload of robot control cpu"""
223299

224300
gripper_state: float = 0.0
301+
"""current opening value of the gripper"""
225302

226303
variabels: dict[str : [PosVariable | float]] = field(default_factory=dict)
304+
"""variables saved in robot controller"""
227305

228306
operation_info: OperationInfo = field(default_factory=OperationInfo)
307+
"""operation statistics of robot controller"""
229308

230309
active_control: bool = False
310+
"""indicates whether the connection has active control of the robot"""
231311

232312
robot_control_version: str = ""
313+
"""version of robot control software"""
233314

234315
robot_configuration: str = ""
316+
"""configuration of robot"""
317+
235318
robot_type: str = ""
319+
"""type of robot"""
320+
236321
gripper_type: str = ""
322+
"""type of gripper"""
237323

238324
project_file: str = ""
325+
"""currently active project file"""
239326

240-
referencing_state: ReferencingState = field(default_factory=ReferencingState)
327+
referencing_state: ReferencingState = field(default_factory=ReferencingState)
328+
"""individual referencing state of all axes"""

0 commit comments

Comments
 (0)