Skip to content

Commit 5c6607d

Browse files
cpr-mabcpr-mab
authored
Merge pull request #30 from CommonplaceRobotics/bugfix_28_joint_order
Fixed #28: Joints in joints_set_point, joints_current, current_joints…
2 parents c06e951 + f7e8287 commit 5c6607d

6 files changed

Lines changed: 406 additions & 19 deletions

File tree

‎cri_lib/cri_controller.py‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -121,9 +121,13 @@ def connect(
121121
# Start sending ALIVEJOG message
122122
self.jog_thread.start()
123123

124+
# Send hello message, this lets the robot control know who we are and what our time is (for logging / troubleshooting)
124125
hello_msg = f'INFO Hello "{application_name}" {application_version} {datetime.now(timezone.utc).strftime(format="%Y-%m-%dT%H:%M:%S")}'
125-
126126
self._send_command(hello_msg)
127+
128+
# Request the axis count, this is needed for interpreting some messages
129+
self._send_command("CONFIG GetAxes")
130+
127131
logger.debug("Connected to %s:%d", host, port)
128132
return True
129133

‎cri_lib/cri_protocol_parser.py‎

Lines changed: 114 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ def __init__(self, robot_state: RobotState, robot_state_lock: Lock):
3232
self.robot_state_lock = robot_state_lock
3333
self.file_list: list = []
3434
self.file_list_lock: Lock = threading.Lock()
35+
self.robot_joint_count = 0
3536

3637
def parse_message(
3738
self, message: str
@@ -122,6 +123,11 @@ def _parse_status(self, parameters: list[str]) -> None:
122123
"""
123124
segment_start_idx = 0
124125

126+
r_cnt = self.robot_state.robot_axes_count
127+
e_cnt = self.robot_state.external_axes_count
128+
t_cnt = self.robot_state.tool_axes_count
129+
p_cnt = self.robot_state.platform_axes_count
130+
125131
while segment_start_idx < len(parameters):
126132
match parameters[segment_start_idx]:
127133
case "MODE":
@@ -132,18 +138,48 @@ def _parse_status(self, parameters: list[str]) -> None:
132138
segment_start_idx += 2
133139

134140
case "POSJOINTSETPOINT":
135-
joints = []
141+
joints = [0.0] * 16 # array of 16 elements
136142
for i in range(16):
137-
joints.append(float(parameters[segment_start_idx + 1 + i]))
143+
# external axes follow immediately after the last robot axis
144+
# therefore we need to reorder the entries
145+
if i < self.robot_state.robot_axes_count: # robot axes
146+
joints[i] = float(parameters[segment_start_idx + 1 + i])
147+
elif i < r_cnt + e_cnt: # external axes
148+
joints[6 + i - r_cnt] = float(
149+
parameters[segment_start_idx + 1 + i]
150+
)
151+
elif i < r_cnt + e_cnt + t_cnt: # tool axes
152+
joints[9 + i - r_cnt - e_cnt] = float(
153+
parameters[segment_start_idx + 1 + i]
154+
)
155+
elif i < r_cnt + e_cnt + t_cnt + p_cnt: # platform axes
156+
joints[12 + i - r_cnt - e_cnt - t_cnt] = float(
157+
parameters[segment_start_idx + 1 + i]
158+
)
138159

139160
with self.robot_state_lock:
140161
self.robot_state.joints_set_point = JointsState(*joints)
141162
segment_start_idx += 17
142163

143164
case "POSJOINTCURRENT":
144-
joints = []
165+
joints = [0.0] * 16 # array of 16 elements
145166
for i in range(16):
146-
joints.append(float(parameters[segment_start_idx + 1 + i]))
167+
# external axes follow immediately after the last robot axis
168+
# therefore we need to reorder the entries
169+
if i < self.robot_state.robot_axes_count: # robot axes
170+
joints[i] = float(parameters[segment_start_idx + 1 + i])
171+
elif i < r_cnt + e_cnt: # external axes
172+
joints[6 + i - r_cnt] = float(
173+
parameters[segment_start_idx + 1 + i]
174+
)
175+
elif i < r_cnt + e_cnt + t_cnt: # tool axes
176+
joints[9 + i - r_cnt - e_cnt] = float(
177+
parameters[segment_start_idx + 1 + i]
178+
)
179+
elif i < r_cnt + e_cnt + t_cnt + p_cnt: # platform axes
180+
joints[12 + i - r_cnt - e_cnt - t_cnt] = float(
181+
parameters[segment_start_idx + 1 + i]
182+
)
147183

148184
with self.robot_state_lock:
149185
self.robot_state.joints_current = JointsState(*joints)
@@ -212,29 +248,55 @@ def _parse_status(self, parameters: list[str]) -> None:
212248
segment_start_idx += 2
213249

214250
case "CURRENTJOINTS":
215-
currents = []
251+
currents = [0.0] * 16 # array of 16 elements
216252
for i in range(16):
217-
currents.append(
218-
float(int(parameters[segment_start_idx + 1 + i])) / 1000
219-
)
253+
# external axes follow immediately after the last robot axis
254+
# therefore we need to reorder the entries
255+
if i < self.robot_state.robot_axes_count: # robot axes
256+
currents[i] = (
257+
float(parameters[segment_start_idx + 1 + i]) / 1000
258+
)
259+
elif i < r_cnt + e_cnt: # external axes
260+
currents[6 + i - r_cnt] = (
261+
float(parameters[segment_start_idx + 1 + i]) / 1000
262+
)
263+
elif i < r_cnt + e_cnt + t_cnt: # tool axes
264+
currents[9 + i - r_cnt - e_cnt] = (
265+
float(parameters[segment_start_idx + 1 + i]) / 1000
266+
)
267+
elif i < r_cnt + e_cnt + t_cnt + p_cnt: # platform axes
268+
currents[12 + i - r_cnt - e_cnt - t_cnt] = (
269+
float(parameters[segment_start_idx + 1 + i]) / 1000
270+
)
220271

221272
with self.robot_state_lock:
222273
self.robot_state.current_joints = currents
223274
segment_start_idx += 17
224275

225276
case "ERROR":
226-
errors = []
277+
errors = [ErrorStates()] * 16
227278
self.robot_state.combined_axes_error = parameters[
228279
segment_start_idx + 1
229280
]
230-
for axis_idx in range(16):
231-
value_int = int(
232-
parameters[segment_start_idx + 2 + axis_idx], base=10
233-
)
281+
for i in range(16):
282+
value_int = int(parameters[segment_start_idx + 2 + i], base=10)
234283
error_bits = []
235-
for i in range(8):
236-
error_bits.append(value_int & (1 << i) != 0)
237-
errors.append(ErrorStates(*error_bits))
284+
for j in range(8):
285+
error_bits.append(value_int & (1 << j) != 0)
286+
287+
# external axes follow immediately after the last robot axis
288+
# therefore we need to reorder the entries
289+
if i < self.robot_state.robot_axes_count: # robot axes
290+
errors[i] = ErrorStates(*error_bits)
291+
elif i < r_cnt + e_cnt: # external axes
292+
errors[6 + i - r_cnt] = ErrorStates(*error_bits)
293+
elif i < r_cnt + e_cnt + t_cnt: # tool axes
294+
errors[9 + i - r_cnt - e_cnt] = ErrorStates(*error_bits)
295+
elif i < r_cnt + e_cnt + t_cnt + p_cnt: # platform axes
296+
errors[12 + i - r_cnt - e_cnt - t_cnt] = ErrorStates(
297+
*error_bits
298+
)
299+
238300
with self.robot_state_lock:
239301
self.robot_state.error_states = errors
240302
segment_start_idx += 18
@@ -532,6 +594,42 @@ def _parse_config(self, parameters: Sequence[str]) -> None:
532594
with self.robot_state_lock:
533595
self.robot_state.project_file = parameters[1]
534596

597+
if parameters[0] == "Axes":
598+
with self.robot_state_lock:
599+
self.robot_state.robot_axes_count = 0
600+
self.robot_state.external_axes_count = 0
601+
self.robot_state.tool_axes_count = 0
602+
self.robot_state.platform_axes_count = 0
603+
604+
# Count axes of each type
605+
# Each axis description follows this format: A1 canid posmin posmax velmax
606+
# Where A is the axis type and 1 is the index within that type
607+
for param in parameters[1:]:
608+
if (
609+
param.startswith("A")
610+
and len(param) == 2
611+
and param[1].isnumeric()
612+
):
613+
self.robot_state.robot_axes_count += 1
614+
if (
615+
param.startswith("E")
616+
and len(param) == 2
617+
and param[1].isnumeric()
618+
):
619+
self.robot_state.external_axes_count += 1
620+
if (
621+
param.startswith("T")
622+
and len(param) == 2
623+
and param[1].isnumeric()
624+
):
625+
self.robot_state.tool_axes_count += 1
626+
if (
627+
param.startswith("P")
628+
and len(param) == 2
629+
and param[1].isnumeric()
630+
):
631+
self.robot_state.platform_axes_count += 1
632+
535633
def _parse_cmderror(self, parameters: Sequence[str]) -> dict[str, str]:
536634
"""Parses a CMDERROR message to notify calling function
537635

‎cri_lib/robot_state.py‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -197,11 +197,23 @@ class RobotState:
197197
mode: RobotMode = RobotMode.JOINT
198198
"""Robot execution mode"""
199199

200+
robot_axes_count: int = 0
201+
"""Number of robot axes"""
202+
external_axes_count: int = 0
203+
"""Number of external axes"""
204+
tool_axes_count: int = 0
205+
"""Number of tool axes"""
206+
platform_axes_count: int = 0
207+
"""Number of mobile platform axes"""
208+
200209
joints_set_point: JointsState = field(default_factory=JointsState)
201-
"""Set point of robot joints"""
210+
"""Target positions of the axes. This position is calculated by the kinematics."""
202211

203212
joints_current: JointsState = field(default_factory=JointsState)
204-
"""Actual values of robot joints"""
213+
"""
214+
Actual hardware positions of the axes. Generally use joints_set_point as
215+
base values for moving the robot to avoid creating a control loop!
216+
"""
205217

206218
position_robot: RobotCartesianPosition = field(
207219
default_factory=RobotCartesianPosition

‎examples/README.md‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
# Examples
2+
This directory contains examples on how to use the CRI Python API.
3+
4+
To run an example copy it to the parent directory, then call it as follows:
5+
```sh
6+
python3 ./NameOfExampleFile.py
7+
```

‎examples/get_joint_position.py‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
import logging
2+
from time import sleep
3+
4+
from cri_lib import CRIController
5+
6+
# 🔹 Configure logging
7+
logging.basicConfig(
8+
level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s"
9+
)
10+
logger = logging.getLogger(__name__)
11+
12+
# CRIController is the main interface for controlling the iRC
13+
controller = CRIController()
14+
15+
# Connect to default iRC IP
16+
# controller.connect("192.168.3.11")
17+
if not controller.connect("127.0.0.1", 3921):
18+
logger.error("Unable to connect to iRC! Ensure the simulator is running.")
19+
quit()
20+
21+
# Acquire active control.
22+
# not necessary for this example
23+
# controller.set_active_control(True)
24+
25+
# 10 times in 500ms intervals
26+
for i in range(0, 10):
27+
sleep(0.5)
28+
j = controller.robot_state.joints_set_point
29+
logger.info(
30+
f"Axis positions: A1={j.A1} A2={j.A2} A3={j.A3} A4={j.A4} A5={j.A5} A6={j.A6} E1={j.E1} E2={j.E2} E3={j.E3} T1={j.G1} T2={j.G2} T3={j.G3} P1={j.P1} P2={j.P2} P3={j.P3} P4={j.P4}"
31+
)
32+
33+
# Disable motors and disconnect
34+
logger.info("Disconnecting...")
35+
controller.close()
36+
37+
logger.info("Script execution completed successfully.")

0 commit comments

Comments
 (0)