carm 0.0.20250822__py3-none-any.whl → 0.1.20251010__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- carm/carm.py +247 -28
- carm/carm_dds.py +46 -12
- carm/carm_ros2.py +3 -3
- {carm-0.0.20250822.dist-info → carm-0.1.20251010.dist-info}/METADATA +1 -1
- carm-0.1.20251010.dist-info/RECORD +9 -0
- carm-0.0.20250822.dist-info/RECORD +0 -9
- {carm-0.0.20250822.dist-info → carm-0.1.20251010.dist-info}/WHEEL +0 -0
- {carm-0.0.20250822.dist-info → carm-0.1.20251010.dist-info}/entry_points.txt +0 -0
- {carm-0.0.20250822.dist-info → carm-0.1.20251010.dist-info}/top_level.txt +0 -0
carm/carm.py
CHANGED
|
@@ -10,20 +10,20 @@ class Carm:
|
|
|
10
10
|
self.last_msg = None
|
|
11
11
|
self.ws = websocket.WebSocketApp(
|
|
12
12
|
addr, # 测试用的公开WebSocket服务
|
|
13
|
-
on_open = lambda ws: self.
|
|
14
|
-
on_close = lambda ws, code, close_msg: self.
|
|
15
|
-
on_message= lambda ws, msg: self.
|
|
13
|
+
on_open = lambda ws: self.__on_open(ws),
|
|
14
|
+
on_close = lambda ws, code, close_msg: self.__on_close(ws, code, close_msg),
|
|
15
|
+
on_message= lambda ws, msg: self.__on_message(ws, msg),
|
|
16
16
|
)
|
|
17
17
|
|
|
18
18
|
self.ops = {
|
|
19
|
-
"webSendRobotState": lambda msg: self.
|
|
20
|
-
"taskFinished": lambda msg: self.
|
|
19
|
+
"webSendRobotState": lambda msg: self.__cbk_status(msg),
|
|
20
|
+
"taskFinished": lambda msg: self.__cbk_taskfinish(msg),
|
|
21
21
|
"onCarmError": lambda msg: print("Error:", msg)
|
|
22
22
|
}
|
|
23
23
|
self.res_pool = {}
|
|
24
24
|
self.task_pool = {}
|
|
25
25
|
|
|
26
|
-
self.reader = threading.Thread(target=self.
|
|
26
|
+
self.reader = threading.Thread(target=self.__recv_loop, daemon=True).start()
|
|
27
27
|
self.open_ready = threading.Event()
|
|
28
28
|
self.open_ready.wait()
|
|
29
29
|
self.limit = self.get_limits()["params"]
|
|
@@ -32,12 +32,22 @@ class Carm:
|
|
|
32
32
|
|
|
33
33
|
@property
|
|
34
34
|
def version(self):
|
|
35
|
+
"""
|
|
36
|
+
Get the version of the arm.
|
|
37
|
+
Returns:
|
|
38
|
+
dict: The version of the arm.
|
|
39
|
+
"""
|
|
35
40
|
return self.request({"command":"getArmIntrinsicProperties",
|
|
36
41
|
"arm_index":0,
|
|
37
42
|
"type":"version"})
|
|
38
43
|
|
|
39
44
|
|
|
40
45
|
def get_limits(self):
|
|
46
|
+
"""
|
|
47
|
+
获取主要配置参数,包括关节限位、关节最大速度、加速度、加加速度等。
|
|
48
|
+
Returns:
|
|
49
|
+
dict: The limits of the arm.
|
|
50
|
+
"""
|
|
41
51
|
return self.request({"command":"getJointParams",
|
|
42
52
|
"arm_index":0,
|
|
43
53
|
"type":"version"})
|
|
@@ -58,16 +68,42 @@ class Carm:
|
|
|
58
68
|
return self.set_control_mode(1)
|
|
59
69
|
|
|
60
70
|
def set_servo_enable(self, enable=True):
|
|
71
|
+
"""
|
|
72
|
+
Set the servo enable of the arm.
|
|
73
|
+
Args:
|
|
74
|
+
enable (bool): The servo enable to set.
|
|
75
|
+
Returns:
|
|
76
|
+
dict: The response from the arm.
|
|
77
|
+
"""
|
|
61
78
|
return self.request({"command":"setServoEnable",
|
|
62
79
|
"arm_index":0,
|
|
63
80
|
"enable":enable})
|
|
64
81
|
|
|
65
82
|
def set_control_mode(self, mode=1):
|
|
83
|
+
"""
|
|
84
|
+
Set the control mode of the arm.
|
|
85
|
+
Args:
|
|
86
|
+
mode (int): The control mode to set.
|
|
87
|
+
* 0-IDLE空闲模式
|
|
88
|
+
* 1-点位控制模式
|
|
89
|
+
* 2-MIT控制模式
|
|
90
|
+
* 3-关节拖动模式
|
|
91
|
+
Returns:
|
|
92
|
+
dict: The response from the arm.
|
|
93
|
+
"""
|
|
66
94
|
return self.request({"command":"setControlMode",
|
|
67
95
|
"arm_index":0,
|
|
68
96
|
"mode":mode})
|
|
69
97
|
|
|
70
98
|
def set_end_effector(self, pos, tau):
|
|
99
|
+
"""
|
|
100
|
+
Set the end effector of the arm.
|
|
101
|
+
Args:
|
|
102
|
+
pos (float): The position of the end effector. 0-0.08m
|
|
103
|
+
tau (float): The torque of the end effector. 0-9.0N.m
|
|
104
|
+
Returns:
|
|
105
|
+
dict: The response from the arm.
|
|
106
|
+
"""
|
|
71
107
|
pos = self.__clip(pos, 0, 0.08)
|
|
72
108
|
tau = self.__clip(tau, 0, 9)
|
|
73
109
|
|
|
@@ -77,6 +113,13 @@ class Carm:
|
|
|
77
113
|
"tau": tau})
|
|
78
114
|
|
|
79
115
|
def get_tool_coordinate(self, tool):
|
|
116
|
+
"""
|
|
117
|
+
获取指定工具的坐标系(工具末端相对法兰的位姿关系).
|
|
118
|
+
Args:
|
|
119
|
+
tool (int): The tool index.
|
|
120
|
+
Returns:
|
|
121
|
+
dict: The tool coordinate of the arm.
|
|
122
|
+
"""
|
|
80
123
|
return self.request({"command":"getCoordinate",
|
|
81
124
|
"arm_index":0,
|
|
82
125
|
"type": "tool",
|
|
@@ -84,12 +127,31 @@ class Carm:
|
|
|
84
127
|
|
|
85
128
|
|
|
86
129
|
def set_collision_config(self, flag = True, level = 10):
|
|
130
|
+
"""
|
|
131
|
+
设置碰撞检测配置.
|
|
132
|
+
Args:
|
|
133
|
+
flag (bool): 是否开启碰撞检测.
|
|
134
|
+
level (int): 碰撞检测等级. 灵敏度等级 0-2,0最高,2最低
|
|
135
|
+
Returns:
|
|
136
|
+
dict: The response from the arm.
|
|
137
|
+
"""
|
|
87
138
|
return self.request({"command":"setCollisionConfig",
|
|
88
139
|
"arm_index":0,
|
|
89
140
|
"flag": flag,
|
|
90
141
|
"level": level})
|
|
91
142
|
|
|
92
143
|
def stop(self, type=0):
|
|
144
|
+
"""
|
|
145
|
+
停止carm.
|
|
146
|
+
Args:
|
|
147
|
+
type (int): 停止类型.
|
|
148
|
+
* 0-暂停
|
|
149
|
+
* 1-停止
|
|
150
|
+
* 2-禁用
|
|
151
|
+
* 3-紧急停止
|
|
152
|
+
Returns:
|
|
153
|
+
dict: The response from the arm.
|
|
154
|
+
"""
|
|
93
155
|
stop_id = ["SIG_ARM_PAUSE", "SIG_ARM_STOP",
|
|
94
156
|
"SIG_ARM_DISABLE","SIG_EMERGENCY_STOP"]
|
|
95
157
|
return self.request({"command":"stopSignals",
|
|
@@ -98,57 +160,119 @@ class Carm:
|
|
|
98
160
|
"step_cnt":5})
|
|
99
161
|
|
|
100
162
|
def stop_task(self, at_once=False):
|
|
163
|
+
"""
|
|
164
|
+
停止当前任务.
|
|
165
|
+
Args:
|
|
166
|
+
at_once (bool): 是否立即停止任务.
|
|
167
|
+
Returns:
|
|
168
|
+
dict: The response from the arm.
|
|
169
|
+
"""
|
|
101
170
|
return self.request({"command":"stopSignals",
|
|
102
171
|
"arm_index":0,
|
|
103
172
|
"stop_id": "SIG_TASK_STOP",
|
|
104
173
|
"stop_at_once": at_once})
|
|
105
174
|
|
|
106
175
|
def recover(self):
|
|
176
|
+
"""
|
|
177
|
+
恢复carm.
|
|
178
|
+
Returns:
|
|
179
|
+
dict: The response from the arm.
|
|
180
|
+
"""
|
|
107
181
|
return self.request({"command":"stopSignals",
|
|
108
182
|
"arm_index":0,
|
|
109
183
|
"stop_id": "SIG_ARM_RECOVER",
|
|
110
184
|
"step_cnt": 5})
|
|
111
185
|
|
|
112
186
|
def clean_carm_error(self):
|
|
187
|
+
"""
|
|
188
|
+
清除carm错误.
|
|
189
|
+
Returns:
|
|
190
|
+
dict: The response from the arm.
|
|
191
|
+
"""
|
|
113
192
|
return self.request({"command":"setControllerErrorReset",
|
|
114
193
|
"arm_index":0})
|
|
115
194
|
|
|
116
|
-
def set_speed_level(self, level, response_level):
|
|
195
|
+
def set_speed_level(self, level = 5.0, response_level = 20):
|
|
196
|
+
"""
|
|
197
|
+
Set the speed level of the arm.
|
|
198
|
+
Args:
|
|
199
|
+
level (float): The speed level to set. range from 0 to 10. 0 is the slowest, 10 is the fastest.
|
|
200
|
+
response_level (int): The response level to set, after how many cicles to set. From 10 to 100.
|
|
201
|
+
Returns:
|
|
202
|
+
dict: The response from the arm.
|
|
203
|
+
"""
|
|
117
204
|
return self.request({"command":"setSpeedLevel",
|
|
118
205
|
"arm_index":0,
|
|
119
206
|
"level":level,
|
|
120
207
|
"response_level":response_level})
|
|
121
|
-
|
|
208
|
+
|
|
122
209
|
def set_debug(self, flag):
|
|
210
|
+
"""
|
|
211
|
+
设置调试模式.
|
|
212
|
+
Args:
|
|
213
|
+
flag (bool): 是否开启调试模式.
|
|
214
|
+
Returns:
|
|
215
|
+
dict: The response from the arm.
|
|
216
|
+
"""
|
|
123
217
|
return self.request({"command":"setDebugMode",
|
|
124
218
|
"arm_index":0,
|
|
125
219
|
"trigger":flag})
|
|
126
220
|
|
|
127
221
|
def get_urdf(self):
|
|
222
|
+
"""
|
|
223
|
+
获取urdf模型.
|
|
224
|
+
Returns:
|
|
225
|
+
dict: The urdf model of the arm.
|
|
226
|
+
"""
|
|
128
227
|
return self.request({"command":"getArmIntrinsicProperties",
|
|
129
228
|
"arm_index":0,
|
|
130
229
|
"type":"urdf"})
|
|
131
230
|
@property
|
|
132
231
|
def joint_pos(self):
|
|
232
|
+
"""
|
|
233
|
+
获取当前关节位置.
|
|
234
|
+
Returns:
|
|
235
|
+
list: The joint position of the arm.
|
|
236
|
+
"""
|
|
133
237
|
return self.state["arm"][0]["reality"]["pose"]
|
|
134
238
|
|
|
135
239
|
@property
|
|
136
240
|
def joint_vel(self):
|
|
241
|
+
"""
|
|
242
|
+
获取当前关节速度.
|
|
243
|
+
Returns:
|
|
244
|
+
list: The joint velocity of the arm.
|
|
245
|
+
"""
|
|
137
246
|
return self.state["arm"][0]["reality"]["vel"]
|
|
138
247
|
|
|
139
248
|
@property
|
|
140
249
|
def joint_tau(self):
|
|
250
|
+
"""
|
|
251
|
+
获取当前关节力矩.
|
|
252
|
+
Returns:
|
|
253
|
+
list: The joint torque of the arm.
|
|
254
|
+
"""
|
|
141
255
|
return self.state["arm"][0]["reality"]["torque"]
|
|
142
256
|
|
|
143
257
|
@property
|
|
144
258
|
def cart_pose(self):
|
|
259
|
+
"""
|
|
260
|
+
获取当前笛卡尔位置.
|
|
261
|
+
Returns:
|
|
262
|
+
list: The cartesian position of the arm.
|
|
263
|
+
"""
|
|
145
264
|
return self.state["arm"][0]["pose"]
|
|
146
265
|
|
|
147
266
|
@property
|
|
148
267
|
def gripper_state(self):
|
|
268
|
+
"""
|
|
269
|
+
获取当前夹爪状态.
|
|
270
|
+
Returns:
|
|
271
|
+
dict: The gripper state of the arm.
|
|
272
|
+
"""
|
|
149
273
|
return self.state["arm"][0]["gripper"]
|
|
150
274
|
|
|
151
|
-
def
|
|
275
|
+
def __clip_joints(self, joints):
|
|
152
276
|
lower = self.limit['limit_lower']
|
|
153
277
|
upper = self.limit["limit_upper"]
|
|
154
278
|
for i,v in enumerate(joints):
|
|
@@ -156,7 +280,15 @@ class Carm:
|
|
|
156
280
|
return joints
|
|
157
281
|
|
|
158
282
|
def track_joint(self, pos, end_effector = -1):
|
|
159
|
-
|
|
283
|
+
"""
|
|
284
|
+
关节轨迹跟踪.
|
|
285
|
+
Args:
|
|
286
|
+
pos (list): The joint position to track.
|
|
287
|
+
end_effector (float, optional): The end effector width. -1 means not set.
|
|
288
|
+
Returns:
|
|
289
|
+
dict: The response from the arm.
|
|
290
|
+
"""
|
|
291
|
+
pos = self.__clip_joints(pos)
|
|
160
292
|
req = {"command":"trajectoryTrackingTasks",
|
|
161
293
|
"task_id":"TASK_TRACKING",
|
|
162
294
|
"arm_index":0,
|
|
@@ -169,6 +301,14 @@ class Carm:
|
|
|
169
301
|
return self.request(req)
|
|
170
302
|
|
|
171
303
|
def track_pose(self, pos, end_effector = -1):
|
|
304
|
+
"""
|
|
305
|
+
笛卡尔轨迹跟踪.
|
|
306
|
+
Args:
|
|
307
|
+
pos (list): The cartesian position to track.
|
|
308
|
+
end_effector (float, optional): The end effector width. -1 means not set.
|
|
309
|
+
Returns:
|
|
310
|
+
dict: The response from the arm.
|
|
311
|
+
"""
|
|
172
312
|
req = {"command":"trajectoryTrackingTasks",
|
|
173
313
|
"task_id":"TASK_TRACKING",
|
|
174
314
|
"arm_index":0,
|
|
@@ -181,7 +321,18 @@ class Carm:
|
|
|
181
321
|
return self.request(req)
|
|
182
322
|
|
|
183
323
|
def move_joint(self, pos, tm=-1, sync=True, user=0, tool=0):
|
|
184
|
-
|
|
324
|
+
"""
|
|
325
|
+
关节到点运动
|
|
326
|
+
Args:
|
|
327
|
+
pos (list): The joint position to move.
|
|
328
|
+
tm (float, optional): The time to move. -1 means not set.
|
|
329
|
+
sync (bool, optional): Whether to wait for the movement to finish.
|
|
330
|
+
user (int, optional): The user index.
|
|
331
|
+
tool (int, optional): The tool index.
|
|
332
|
+
Returns:
|
|
333
|
+
dict: The response from the arm.
|
|
334
|
+
"""
|
|
335
|
+
pos = self.__clip_joints(pos)
|
|
185
336
|
|
|
186
337
|
res = self.request({"command":"webRecieveTasks",
|
|
187
338
|
"task_id":"TASK_MOVJ",
|
|
@@ -191,11 +342,22 @@ class Carm:
|
|
|
191
342
|
"data":{"user":user,"tool":tool, "target_pos": pos, "speed":100}})
|
|
192
343
|
|
|
193
344
|
if sync and res["recv"]=="Task_Recieve":
|
|
194
|
-
self.
|
|
345
|
+
self.__wait_task(res["task_key"])
|
|
195
346
|
|
|
196
347
|
return res
|
|
197
348
|
|
|
198
349
|
def move_pose(self, pos, tm=-1, sync=True, user=0, tool=0):
|
|
350
|
+
"""
|
|
351
|
+
笛卡尔到点运动
|
|
352
|
+
Args:
|
|
353
|
+
pos (list): The cartesian position to move.
|
|
354
|
+
tm (float, optional): The time to move. -1 means not set.
|
|
355
|
+
sync (bool, optional): Whether to wait for the movement to finish.
|
|
356
|
+
user (int, optional): The user index.
|
|
357
|
+
tool (int, optional): The tool index.
|
|
358
|
+
Returns:
|
|
359
|
+
dict: The response from the arm.
|
|
360
|
+
"""
|
|
199
361
|
res = self.request({"command":"webRecieveTasks",
|
|
200
362
|
"task_id":"TASK_MOVJ",
|
|
201
363
|
"task_level":"Task_General",
|
|
@@ -204,25 +366,42 @@ class Carm:
|
|
|
204
366
|
"data":{"user":user,"tool":tool, "target_pos": pos, "speed":100}})
|
|
205
367
|
|
|
206
368
|
if sync and res["recv"]=="Task_Recieve":
|
|
207
|
-
self.
|
|
369
|
+
self.__wait_task(res["task_key"])
|
|
208
370
|
|
|
209
371
|
return res
|
|
210
372
|
|
|
211
373
|
|
|
212
374
|
def move_line(self, pos, speed=50, sync=True, user=0, tool=0):
|
|
375
|
+
"""
|
|
376
|
+
笛卡尔直线运动
|
|
377
|
+
Args:
|
|
378
|
+
pos (list): The cartesian position to move.
|
|
379
|
+
speed (float, optional): The speed to move. 0 - 100%.
|
|
380
|
+
sync (bool, optional): Whether to wait for the movement to finish.
|
|
381
|
+
user (int, optional): The user index.
|
|
382
|
+
tool (int, optional): The tool index.
|
|
383
|
+
Returns:
|
|
384
|
+
dict: The response from the arm.
|
|
385
|
+
"""
|
|
386
|
+
ret = self.invK(pos, self.joint_pos, user, tool) # check IK first
|
|
387
|
+
if ret["recv"] != "Task_Recieve":
|
|
388
|
+
print("Inverse kinematics failed:", ret)
|
|
389
|
+
return ret
|
|
390
|
+
|
|
391
|
+
pos = ret["data"]["joint1"]
|
|
213
392
|
res = self.request({"command":"webRecieveTasks",
|
|
214
393
|
"task_id":"TASK_MOVL",
|
|
215
394
|
"task_level":"Task_General",
|
|
216
395
|
"arm_index":0,
|
|
217
|
-
"point_type":{"space":
|
|
218
|
-
"data":{"user":user,"tool":tool, "
|
|
396
|
+
"point_type":{"space":0},
|
|
397
|
+
"data":{"user":user,"tool":tool, "point": pos, "speed":speed}})
|
|
219
398
|
|
|
220
399
|
if sync and res["recv"]=="Task_Recieve":
|
|
221
|
-
self.
|
|
400
|
+
self.__wait_task(res["task_key"])
|
|
222
401
|
|
|
223
402
|
return res
|
|
224
403
|
|
|
225
|
-
def
|
|
404
|
+
def __wait_task(self, task_key):
|
|
226
405
|
event = threading.Event()
|
|
227
406
|
self.task_pool[task_key] = {"event":event}
|
|
228
407
|
event.wait()
|
|
@@ -246,14 +425,43 @@ class Carm:
|
|
|
246
425
|
def move_pvt(self, target_traj, gripper_pos = [], stamps = [], is_sync=True):
|
|
247
426
|
pass # TODO
|
|
248
427
|
|
|
428
|
+
def set_redundancy_tau(self, redundancy_tau, end_effector=None):
|
|
429
|
+
"""
|
|
430
|
+
设置冗余关节的力矩
|
|
431
|
+
Args:
|
|
432
|
+
redundancy_tau (list): The redundancy joint torque.
|
|
433
|
+
end_effector (int, optional): The end effector index.
|
|
434
|
+
Returns:
|
|
435
|
+
dict: The response from the arm.
|
|
436
|
+
"""
|
|
437
|
+
return self.request({"command":"setRedundancyTau",
|
|
438
|
+
"arm_index":0,
|
|
439
|
+
"is_master":True,
|
|
440
|
+
"redundancy_tau":redundancy_tau})
|
|
441
|
+
|
|
249
442
|
def invK(self, cart_pose, ref_joints, user=0, tool=0):
|
|
443
|
+
"""
|
|
444
|
+
逆解
|
|
445
|
+
Args:
|
|
446
|
+
cart_pose (list): The cartesian position to move.
|
|
447
|
+
ref_joints (list): The reference joint position.
|
|
448
|
+
user (int, optional): The user index.
|
|
449
|
+
tool (int, optional): The tool index.
|
|
450
|
+
Returns:
|
|
451
|
+
dict: The response from the arm.
|
|
452
|
+
"""
|
|
250
453
|
if not type(cart_pose[0]) is list:
|
|
251
454
|
cart_pose = [cart_pose, ]
|
|
455
|
+
ref_joints = [ref_joints, ]
|
|
252
456
|
assert(len(cart_pose) == len(ref_joints))
|
|
457
|
+
data = {"user":user,"tool":tool,"point_cnt":len(cart_pose)}
|
|
458
|
+
for i in range(len(ref_joints)):
|
|
459
|
+
data[f"point{i+1}"] = cart_pose[i]
|
|
460
|
+
data[f"refer{i+1}"] = ref_joints[i]
|
|
253
461
|
return self.request({"command":"getKinematics",
|
|
254
462
|
"task_id":"inverse",
|
|
255
463
|
"arm_index":0,
|
|
256
|
-
"data":
|
|
464
|
+
"data":data})
|
|
257
465
|
|
|
258
466
|
def request(self, req):
|
|
259
467
|
event = threading.Event()
|
|
@@ -261,43 +469,54 @@ class Carm:
|
|
|
261
469
|
req["task_key"] = task_key
|
|
262
470
|
self.res_pool[task_key] = {"req":req,"event":event}
|
|
263
471
|
|
|
264
|
-
self.
|
|
472
|
+
self.__send(req)
|
|
265
473
|
|
|
266
474
|
event.wait()
|
|
267
475
|
return self.res_pool.pop(task_key)["res"]
|
|
268
476
|
|
|
269
|
-
def
|
|
477
|
+
def __send(self, msg):
|
|
270
478
|
self.ws.send(json.dumps(msg))
|
|
271
479
|
|
|
272
|
-
def
|
|
480
|
+
def __cbk_status(self, message):
|
|
273
481
|
self.state = message
|
|
274
482
|
|
|
275
|
-
|
|
483
|
+
if not "arm" in message:
|
|
484
|
+
return
|
|
485
|
+
|
|
486
|
+
arm_json = message["arm"][0]
|
|
487
|
+
if "last_task_key" not in arm_json:
|
|
488
|
+
return
|
|
489
|
+
|
|
490
|
+
task = arm_json["last_task_key"]
|
|
491
|
+
self.task_pool[task]["event"].set()
|
|
492
|
+
|
|
493
|
+
|
|
494
|
+
def __cbk_taskfinish(self, message):
|
|
276
495
|
task = message["task_key"]
|
|
277
496
|
self.task_pool[task]["event"].set()
|
|
278
497
|
|
|
279
|
-
def
|
|
498
|
+
def __on_open(self, ws):
|
|
280
499
|
self.open_ready.set()
|
|
281
500
|
print("Connected successfully.")
|
|
282
501
|
|
|
283
|
-
def
|
|
502
|
+
def __on_close(self, ws, code, close_msg):
|
|
284
503
|
print("Disconnected, please check your --addr",code, close_msg)
|
|
285
504
|
|
|
286
|
-
def
|
|
505
|
+
def __on_message(self, ws, message):
|
|
287
506
|
msg = json.loads(message)
|
|
288
507
|
self.last_msg = msg
|
|
289
508
|
cmd = msg["command"]
|
|
290
|
-
op = self.ops.get(cmd, lambda msg: self.
|
|
509
|
+
op = self.ops.get(cmd, lambda msg: self.__response_op(msg))
|
|
291
510
|
op(msg)
|
|
292
511
|
|
|
293
|
-
def
|
|
512
|
+
def __response_op(self, res):
|
|
294
513
|
id = res.get("task_key","")
|
|
295
514
|
data = self.res_pool[id]
|
|
296
515
|
data["res"] = res
|
|
297
516
|
data["event"].set() # notify request thread
|
|
298
517
|
|
|
299
518
|
|
|
300
|
-
def
|
|
519
|
+
def __recv_loop(self):
|
|
301
520
|
print("Recv loop started.")
|
|
302
521
|
self.ws.run_forever()
|
|
303
522
|
|
carm/carm_dds.py
CHANGED
|
@@ -13,24 +13,46 @@ class ArmDriver:
|
|
|
13
13
|
self.args = args
|
|
14
14
|
|
|
15
15
|
self.arm = carm.Carm(args.addr)
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
self.arm.set_control_mode(1)
|
|
16
|
+
self.arm.set_control_mode(args.mode)
|
|
17
|
+
|
|
18
|
+
self.arm.set_speed_level(args.speed)
|
|
20
19
|
|
|
21
20
|
self.pub_joint = messenger.advertise(args.joint_topic, 0)
|
|
22
21
|
self.pub_end = messenger.advertise(args.end_topic, 0)
|
|
22
|
+
self.pub_joint_ik = messenger.advertise(args.joint_cmd_ik_topic, 0)
|
|
23
23
|
|
|
24
24
|
self.sub_joint = messenger.subscribe(args.joint_cmd_topic, 0, lambda msg:self.joint_callback(msg))
|
|
25
25
|
self.sub_end = messenger.subscribe(args.end_cmd_topic, 0, lambda msg:self.end_callback(msg))
|
|
26
26
|
|
|
27
27
|
def end_callback(self, msg):
|
|
28
|
-
position =
|
|
29
|
-
|
|
28
|
+
position = msg["position"]
|
|
29
|
+
if type(position) != tuple:
|
|
30
|
+
position = np.frombuffer(msg["position"],dtype=np.float64).tolist()
|
|
31
|
+
|
|
32
|
+
if position > 7:
|
|
33
|
+
self.arm.track_pose(position[:7], position[7])
|
|
34
|
+
else:
|
|
35
|
+
self.arm.track_pose(position)
|
|
36
|
+
|
|
37
|
+
if self.args.enable_ik:
|
|
38
|
+
ret = self.arm.invK(position, ref_joints=self.arm.joint_pos)
|
|
39
|
+
if ret["recv"] != "Task_Recieve":
|
|
40
|
+
print("Inverse kinematics failed:", ret)
|
|
41
|
+
return
|
|
30
42
|
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
43
|
+
pos = ret["data"]["joint1"]
|
|
44
|
+
self.pub_joint_ik.publish({"header": msg["header"], "position": pos})
|
|
45
|
+
|
|
46
|
+
|
|
47
|
+
def joint_callback(self, msg):
|
|
48
|
+
position = msg["position"]
|
|
49
|
+
if type(position) != tuple:
|
|
50
|
+
position = np.frombuffer(msg["position"],dtype=np.float64).tolist()
|
|
51
|
+
|
|
52
|
+
if position > 6:
|
|
53
|
+
self.arm.track_joint(position[:6], position[6])
|
|
54
|
+
else:
|
|
55
|
+
self.arm.track_joint(position)
|
|
34
56
|
|
|
35
57
|
def loop(self):
|
|
36
58
|
while True:
|
|
@@ -39,8 +61,12 @@ class ArmDriver:
|
|
|
39
61
|
nanosec = int((stamp - sec) * 1e9)
|
|
40
62
|
header = {"stamp": {"sec": sec, "nanosec": nanosec},"frame_id": "base_link"}
|
|
41
63
|
|
|
42
|
-
end_msg = {"header": header,
|
|
43
|
-
|
|
64
|
+
end_msg = {"header": header,
|
|
65
|
+
"position": self.arm.cart_pose + [self.arm.gripper_state["gripper_pos"],]}
|
|
66
|
+
joints_msg = {"header": header,
|
|
67
|
+
"position": self.arm.joint_pos + [self.arm.gripper_state["gripper_pos"],],
|
|
68
|
+
"velocity": self.arm.joint_vel + [self.arm.gripper_state["gripper_vel"],],
|
|
69
|
+
"effort": self.arm.joint_tau + [self.arm.gripper_state["gripper_tau"],]}
|
|
44
70
|
|
|
45
71
|
self.pub_joint.publish(joints_msg)
|
|
46
72
|
self.pub_end.publish(end_msg)
|
|
@@ -66,6 +92,7 @@ def driver_main(args):
|
|
|
66
92
|
transfer = ros2.Transfer({"node": "carm_driver" + args.device,
|
|
67
93
|
"publishers":[[args.joint_topic, "sensor_msgs/msg/JointState",10],
|
|
68
94
|
[args.end_topic, "sensor_msgs/msg/JointState",10],
|
|
95
|
+
[args.joint_cmd_ik_topic, "sensor_msgs/msg/JointState",10],
|
|
69
96
|
],
|
|
70
97
|
"subscriptions":[[args.joint_cmd_topic, "sensor_msgs/msg/JointState",10],
|
|
71
98
|
[args.end_cmd_topic, "sensor_msgs/msg/JointState",10]]})
|
|
@@ -78,11 +105,13 @@ def main():
|
|
|
78
105
|
parser.add_argument("--cmd", type=str, default="", help="Send command instead of start driver, support enable,disable,remote")
|
|
79
106
|
parser.add_argument("--device", type=str, default="carm", help="device name, used as topic prefix")
|
|
80
107
|
parser.add_argument("--dds", type=str, default="svar_messenger_ros2", help="the dds plugin, default is ros2, options: svar_zbus, svar_lcm")
|
|
81
|
-
parser.add_argument("--
|
|
108
|
+
parser.add_argument("--mode", type=int, default=1, help="The arm control mode: POSITION:1, MIT:2, MASTER:3")
|
|
82
109
|
parser.add_argument("--joint_topic", type=str, default="", help="the joints status topic")
|
|
83
110
|
parser.add_argument("--end_topic", type=str, default="", help="the joints status topic")
|
|
84
111
|
parser.add_argument("--joint_cmd_topic", type=str, default="", help="the joints cmd topic")
|
|
112
|
+
parser.add_argument("--joint_cmd_ik_topic", type=str, default="", help="the joints cmd ik topic, this means both end_cmd_topic will be transformed with ik")
|
|
85
113
|
parser.add_argument("--end_cmd_topic", type=str, default="", help="the end cmd topic")
|
|
114
|
+
parser.add_argument("--speed", type=float, default=50.0, help="the speed level")
|
|
86
115
|
|
|
87
116
|
args = parser.parse_args()
|
|
88
117
|
|
|
@@ -94,6 +123,11 @@ def main():
|
|
|
94
123
|
args.joint_cmd_topic = "/"+args.device + "/joints_cmd"
|
|
95
124
|
if args.end_cmd_topic == "":
|
|
96
125
|
args.end_cmd_topic = "/"+args.device + "/end_cmd"
|
|
126
|
+
if args.joint_cmd_ik_topic == "":
|
|
127
|
+
args.joint_cmd_ik_topic = "/"+args.device + "/joints_cmd_ik"
|
|
128
|
+
args.__dict__["enable_ik"] = False
|
|
129
|
+
else:
|
|
130
|
+
args.__dict__["enable_ik"] = True
|
|
97
131
|
|
|
98
132
|
if args.cmd == "":
|
|
99
133
|
driver_main(args)
|
carm/carm_ros2.py
CHANGED
|
@@ -46,9 +46,9 @@ class ArmDriver(Node):
|
|
|
46
46
|
while True:
|
|
47
47
|
joint_msg = JointState() # list of string
|
|
48
48
|
joint_msg.header.stamp = self.get_clock().now().to_msg()
|
|
49
|
-
joint_msg.position = self.arm.joint_pos
|
|
50
|
-
joint_msg.velocity = self.arm.joint_vel
|
|
51
|
-
joint_msg.effort = self.arm.joint_tau
|
|
49
|
+
joint_msg.position = np.array(self.arm.joint_pos).tolist()
|
|
50
|
+
joint_msg.velocity = np.array(self.arm.joint_vel).tolist()
|
|
51
|
+
joint_msg.effort = np.array(self.arm.joint_tau).tolist()
|
|
52
52
|
joint_msg.position.append(self.arm.gripper_state["gripper_pos"])
|
|
53
53
|
self.pub_joint.publish(joint_msg)
|
|
54
54
|
|
|
@@ -0,0 +1,9 @@
|
|
|
1
|
+
carm/__init__.py,sha256=lfNjXYpO8FjQVunCqiaUzjsNUnyuhcyO8Je0ixh1RJY,24
|
|
2
|
+
carm/carm.py,sha256=jep6tWFUYl-8jw-qixC_awV5gdTEyicBU2K7oU7qNGs,17973
|
|
3
|
+
carm/carm_dds.py,sha256=DfejpbrW_qWq36ZQpyWVRdfDsUCkScHXOx2fP-8i3kY,5711
|
|
4
|
+
carm/carm_ros2.py,sha256=nox040kSIjpwT9s-crH9D0yGYvFPNfkWNrzj4tqf4I8,3766
|
|
5
|
+
carm-0.1.20251010.dist-info/METADATA,sha256=QeLknGXuES8_Ld8fsX5AOVUKlgUztdVXoOr4lzJiGGM,389
|
|
6
|
+
carm-0.1.20251010.dist-info/WHEEL,sha256=iAkIy5fosb7FzIOwONchHf19Qu7_1wCWyFNR5gu9nU0,91
|
|
7
|
+
carm-0.1.20251010.dist-info/entry_points.txt,sha256=qsJWC39p-s4UO3KZ8BfV2hikeMX693mahkYbR3w67Lk,76
|
|
8
|
+
carm-0.1.20251010.dist-info/top_level.txt,sha256=nZ1bTssUAnAqc2CMka3erGx_2LsdZrSkqtjNf9Ce_d8,5
|
|
9
|
+
carm-0.1.20251010.dist-info/RECORD,,
|
|
@@ -1,9 +0,0 @@
|
|
|
1
|
-
carm/__init__.py,sha256=lfNjXYpO8FjQVunCqiaUzjsNUnyuhcyO8Je0ixh1RJY,24
|
|
2
|
-
carm/carm.py,sha256=8PaBSryLOW9RPzz8o5KUqHiGdPONGiw9HuCJvdWMQgg,10978
|
|
3
|
-
carm/carm_dds.py,sha256=1LTwExF3fuRfjI_Ev8P5TVrv62mxiFRwPdEmeyySbM8,4107
|
|
4
|
-
carm/carm_ros2.py,sha256=IxQD4pglxCH2Vbfj6mm2DlV8KgkqMnS0k1-mIICV8I8,3709
|
|
5
|
-
carm-0.0.20250822.dist-info/METADATA,sha256=RhO7IwJGvpxCOhaOrqDCcSyPYFpnPms2KHqlLqHAJSo,389
|
|
6
|
-
carm-0.0.20250822.dist-info/WHEEL,sha256=iAkIy5fosb7FzIOwONchHf19Qu7_1wCWyFNR5gu9nU0,91
|
|
7
|
-
carm-0.0.20250822.dist-info/entry_points.txt,sha256=qsJWC39p-s4UO3KZ8BfV2hikeMX693mahkYbR3w67Lk,76
|
|
8
|
-
carm-0.0.20250822.dist-info/top_level.txt,sha256=nZ1bTssUAnAqc2CMka3erGx_2LsdZrSkqtjNf9Ce_d8,5
|
|
9
|
-
carm-0.0.20250822.dist-info/RECORD,,
|
|
File without changes
|
|
File without changes
|
|
File without changes
|