carm 0.0.20250815__py3-none-any.whl → 0.0.20250911__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 +233 -25
- carm/carm_dds.py +129 -0
- carm/carm_ros2.py +96 -0
- {carm-0.0.20250815.dist-info → carm-0.0.20250911.dist-info}/METADATA +1 -1
- carm-0.0.20250911.dist-info/RECORD +9 -0
- carm-0.0.20250911.dist-info/entry_points.txt +3 -0
- carm-0.0.20250815.dist-info/RECORD +0 -7
- carm-0.0.20250815.dist-info/entry_points.txt +0 -3
- {carm-0.0.20250815.dist-info → carm-0.0.20250911.dist-info}/WHEEL +0 -0
- {carm-0.0.20250815.dist-info → carm-0.0.20250911.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,11 +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"])
|
|
370
|
+
|
|
371
|
+
return res
|
|
372
|
+
|
|
373
|
+
|
|
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"]
|
|
392
|
+
res = self.request({"command":"webRecieveTasks",
|
|
393
|
+
"task_id":"TASK_MOVL",
|
|
394
|
+
"task_level":"Task_General",
|
|
395
|
+
"arm_index":0,
|
|
396
|
+
"point_type":{"space":0},
|
|
397
|
+
"data":{"user":user,"tool":tool, "point": pos, "speed":speed}})
|
|
398
|
+
|
|
399
|
+
if sync and res["recv"]=="Task_Recieve":
|
|
400
|
+
self.__wait_task(res["task_key"])
|
|
208
401
|
|
|
209
402
|
return res
|
|
210
403
|
|
|
211
|
-
def
|
|
404
|
+
def __wait_task(self, task_key):
|
|
212
405
|
event = threading.Event()
|
|
213
406
|
self.task_pool[task_key] = {"event":event}
|
|
214
407
|
event.wait()
|
|
@@ -233,13 +426,28 @@ class Carm:
|
|
|
233
426
|
pass # TODO
|
|
234
427
|
|
|
235
428
|
def invK(self, cart_pose, ref_joints, user=0, tool=0):
|
|
429
|
+
"""
|
|
430
|
+
逆解
|
|
431
|
+
Args:
|
|
432
|
+
cart_pose (list): The cartesian position to move.
|
|
433
|
+
ref_joints (list): The reference joint position.
|
|
434
|
+
user (int, optional): The user index.
|
|
435
|
+
tool (int, optional): The tool index.
|
|
436
|
+
Returns:
|
|
437
|
+
dict: The response from the arm.
|
|
438
|
+
"""
|
|
236
439
|
if not type(cart_pose[0]) is list:
|
|
237
440
|
cart_pose = [cart_pose, ]
|
|
441
|
+
ref_joints = [ref_joints, ]
|
|
238
442
|
assert(len(cart_pose) == len(ref_joints))
|
|
443
|
+
data = {"user":user,"tool":tool,"point_cnt":len(cart_pose)}
|
|
444
|
+
for i in range(len(ref_joints)):
|
|
445
|
+
data[f"point{i+1}"] = cart_pose[i]
|
|
446
|
+
data[f"refer{i+1}"] = ref_joints[i]
|
|
239
447
|
return self.request({"command":"getKinematics",
|
|
240
448
|
"task_id":"inverse",
|
|
241
449
|
"arm_index":0,
|
|
242
|
-
"data":
|
|
450
|
+
"data":data})
|
|
243
451
|
|
|
244
452
|
def request(self, req):
|
|
245
453
|
event = threading.Event()
|
|
@@ -247,43 +455,43 @@ class Carm:
|
|
|
247
455
|
req["task_key"] = task_key
|
|
248
456
|
self.res_pool[task_key] = {"req":req,"event":event}
|
|
249
457
|
|
|
250
|
-
self.
|
|
458
|
+
self.__send(req)
|
|
251
459
|
|
|
252
460
|
event.wait()
|
|
253
461
|
return self.res_pool.pop(task_key)["res"]
|
|
254
462
|
|
|
255
|
-
def
|
|
463
|
+
def __send(self, msg):
|
|
256
464
|
self.ws.send(json.dumps(msg))
|
|
257
465
|
|
|
258
|
-
def
|
|
466
|
+
def __cbk_status(self, message):
|
|
259
467
|
self.state = message
|
|
260
468
|
|
|
261
|
-
def
|
|
469
|
+
def __cbk_taskfinish(self, message):
|
|
262
470
|
task = message["task_key"]
|
|
263
471
|
self.task_pool[task]["event"].set()
|
|
264
472
|
|
|
265
|
-
def
|
|
473
|
+
def __on_open(self, ws):
|
|
266
474
|
self.open_ready.set()
|
|
267
475
|
print("Connected successfully.")
|
|
268
476
|
|
|
269
|
-
def
|
|
477
|
+
def __on_close(self, ws, code, close_msg):
|
|
270
478
|
print("Disconnected, please check your --addr",code, close_msg)
|
|
271
479
|
|
|
272
|
-
def
|
|
480
|
+
def __on_message(self, ws, message):
|
|
273
481
|
msg = json.loads(message)
|
|
274
482
|
self.last_msg = msg
|
|
275
483
|
cmd = msg["command"]
|
|
276
|
-
op = self.ops.get(cmd, lambda msg: self.
|
|
484
|
+
op = self.ops.get(cmd, lambda msg: self.__response_op(msg))
|
|
277
485
|
op(msg)
|
|
278
486
|
|
|
279
|
-
def
|
|
487
|
+
def __response_op(self, res):
|
|
280
488
|
id = res.get("task_key","")
|
|
281
489
|
data = self.res_pool[id]
|
|
282
490
|
data["res"] = res
|
|
283
491
|
data["event"].set() # notify request thread
|
|
284
492
|
|
|
285
493
|
|
|
286
|
-
def
|
|
494
|
+
def __recv_loop(self):
|
|
287
495
|
print("Recv loop started.")
|
|
288
496
|
self.ws.run_forever()
|
|
289
497
|
|
carm/carm_dds.py
ADDED
|
@@ -0,0 +1,129 @@
|
|
|
1
|
+
#!/usr/bin/python3
|
|
2
|
+
import time
|
|
3
|
+
import os
|
|
4
|
+
import argparse
|
|
5
|
+
import svar
|
|
6
|
+
import numpy as np
|
|
7
|
+
import carm
|
|
8
|
+
|
|
9
|
+
messenger = svar.load("svar_messenger").messenger
|
|
10
|
+
|
|
11
|
+
class ArmDriver:
|
|
12
|
+
def __init__(self,args):
|
|
13
|
+
self.args = args
|
|
14
|
+
|
|
15
|
+
self.arm = carm.Carm(args.addr)
|
|
16
|
+
if args.mit:
|
|
17
|
+
self.arm.set_control_mode(3)
|
|
18
|
+
else:
|
|
19
|
+
self.arm.set_control_mode(1)
|
|
20
|
+
|
|
21
|
+
self.arm.set_speed_level(args.speed)
|
|
22
|
+
|
|
23
|
+
self.pub_joint = messenger.advertise(args.joint_topic, 0)
|
|
24
|
+
self.pub_end = messenger.advertise(args.end_topic, 0)
|
|
25
|
+
self.pub_joint_ik = messenger.advertise(args.joint_cmd_ik_topic, 0)
|
|
26
|
+
|
|
27
|
+
self.sub_joint = messenger.subscribe(args.joint_cmd_topic, 0, lambda msg:self.joint_callback(msg))
|
|
28
|
+
self.sub_end = messenger.subscribe(args.end_cmd_topic, 0, lambda msg:self.end_callback(msg))
|
|
29
|
+
|
|
30
|
+
def end_callback(self, msg):
|
|
31
|
+
position = np.frombuffer(msg["position"],dtype=np.float64).tolist()
|
|
32
|
+
self.arm.track_pose(position)
|
|
33
|
+
|
|
34
|
+
if self.args.enable_ik:
|
|
35
|
+
ret = self.arm.invK(position, ref_joints=self.arm.joint_pos)
|
|
36
|
+
if ret["recv"] != "Task_Recieve":
|
|
37
|
+
print("Inverse kinematics failed:", ret)
|
|
38
|
+
return
|
|
39
|
+
|
|
40
|
+
pos = ret["data"]["joint1"]
|
|
41
|
+
self.pub_joint_ik.publish({"header": msg["header"], "position": pos})
|
|
42
|
+
|
|
43
|
+
|
|
44
|
+
def joint_callback(self, msg):
|
|
45
|
+
position = np.frombuffer(msg["position"],dtype=np.float64).tolist()
|
|
46
|
+
self.arm.track_joint(position)
|
|
47
|
+
|
|
48
|
+
def loop(self):
|
|
49
|
+
while True:
|
|
50
|
+
stamp = time.time()
|
|
51
|
+
sec = int(stamp )
|
|
52
|
+
nanosec = int((stamp - sec) * 1e9)
|
|
53
|
+
header = {"stamp": {"sec": sec, "nanosec": nanosec},"frame_id": "base_link"}
|
|
54
|
+
|
|
55
|
+
end_msg = {"header": header, "position": self.arm.cart_pose + [self.arm.gripper_state["gripper_pos"],]}
|
|
56
|
+
joints_msg = {"header": header, "position": self.arm.joint_pos + [self.arm.gripper_state["gripper_pos"],]}
|
|
57
|
+
|
|
58
|
+
self.pub_joint.publish(joints_msg)
|
|
59
|
+
self.pub_end.publish(end_msg)
|
|
60
|
+
|
|
61
|
+
time.sleep(0.005)
|
|
62
|
+
|
|
63
|
+
def send_cmd(args):
|
|
64
|
+
arm = carm.Carm(args.addr)
|
|
65
|
+
|
|
66
|
+
cmd = args.cmd
|
|
67
|
+
|
|
68
|
+
if cmd == "disable":
|
|
69
|
+
arm.set_servo_enable(False)
|
|
70
|
+
if cmd == "remote":
|
|
71
|
+
arm.set_control_mode(3)
|
|
72
|
+
|
|
73
|
+
def driver_main(args):
|
|
74
|
+
print("Starting driver mode...")
|
|
75
|
+
|
|
76
|
+
driver = ArmDriver(args)
|
|
77
|
+
|
|
78
|
+
ros2 = svar.load(args.dds)
|
|
79
|
+
transfer = ros2.Transfer({"node": "carm_driver" + args.device,
|
|
80
|
+
"publishers":[[args.joint_topic, "sensor_msgs/msg/JointState",10],
|
|
81
|
+
[args.end_topic, "sensor_msgs/msg/JointState",10],
|
|
82
|
+
[args.joint_cmd_ik_topic, "sensor_msgs/msg/JointState",10],
|
|
83
|
+
],
|
|
84
|
+
"subscriptions":[[args.joint_cmd_topic, "sensor_msgs/msg/JointState",10],
|
|
85
|
+
[args.end_cmd_topic, "sensor_msgs/msg/JointState",10]]})
|
|
86
|
+
|
|
87
|
+
driver.loop()
|
|
88
|
+
|
|
89
|
+
def main():
|
|
90
|
+
parser = argparse.ArgumentParser()
|
|
91
|
+
parser.add_argument("--addr", type=str, default="ws://localhost:8090", help="Device address, including ip and port")
|
|
92
|
+
parser.add_argument("--cmd", type=str, default="", help="Send command instead of start driver, support enable,disable,remote")
|
|
93
|
+
parser.add_argument("--device", type=str, default="carm", help="device name, used as topic prefix")
|
|
94
|
+
parser.add_argument("--dds", type=str, default="svar_messenger_ros2", help="the dds plugin, default is ros2, options: svar_zbus, svar_lcm")
|
|
95
|
+
parser.add_argument("--mit", action="store_true", help="Enable mit mode")
|
|
96
|
+
parser.add_argument("--joint_topic", type=str, default="", help="the joints status topic")
|
|
97
|
+
parser.add_argument("--end_topic", type=str, default="", help="the joints status topic")
|
|
98
|
+
parser.add_argument("--joint_cmd_topic", type=str, default="", help="the joints cmd topic")
|
|
99
|
+
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")
|
|
100
|
+
parser.add_argument("--end_cmd_topic", type=str, default="", help="the end cmd topic")
|
|
101
|
+
parser.add_argument("--speed", type=float, default=5.0, help="the speed level")
|
|
102
|
+
|
|
103
|
+
args = parser.parse_args()
|
|
104
|
+
|
|
105
|
+
if args.joint_topic == "":
|
|
106
|
+
args.joint_topic = "/"+args.device + "/joints"
|
|
107
|
+
if args.end_topic == "":
|
|
108
|
+
args.end_topic = "/"+args.device + "/end"
|
|
109
|
+
if args.joint_cmd_topic == "":
|
|
110
|
+
args.joint_cmd_topic = "/"+args.device + "/joints_cmd"
|
|
111
|
+
if args.end_cmd_topic == "":
|
|
112
|
+
args.end_cmd_topic = "/"+args.device + "/end_cmd"
|
|
113
|
+
if args.joint_cmd_ik_topic == "":
|
|
114
|
+
args.joint_cmd_ik_topic = "/"+args.device + "/joints_cmd_ik"
|
|
115
|
+
args.__dict__["enable_ik"] = False
|
|
116
|
+
else:
|
|
117
|
+
args.__dict__["enable_ik"] = True
|
|
118
|
+
|
|
119
|
+
if args.cmd == "":
|
|
120
|
+
driver_main(args)
|
|
121
|
+
else:
|
|
122
|
+
send_cmd(args)
|
|
123
|
+
|
|
124
|
+
|
|
125
|
+
# 测试代码
|
|
126
|
+
if __name__ == "__main__":
|
|
127
|
+
main()
|
|
128
|
+
|
|
129
|
+
|
carm/carm_ros2.py
ADDED
|
@@ -0,0 +1,96 @@
|
|
|
1
|
+
#!/usr/bin/python3
|
|
2
|
+
import rclpy
|
|
3
|
+
from rclpy.node import Node
|
|
4
|
+
import time
|
|
5
|
+
import argparse
|
|
6
|
+
|
|
7
|
+
from std_msgs.msg import String, Bool, Int16MultiArray, MultiArrayLayout, MultiArrayDimension, Int8
|
|
8
|
+
from geometry_msgs.msg import Point, Pose, PoseArray
|
|
9
|
+
from sensor_msgs.msg import JointState
|
|
10
|
+
from example_interfaces.srv import AddTwoInts
|
|
11
|
+
import carm
|
|
12
|
+
import threading
|
|
13
|
+
import numpy as np
|
|
14
|
+
|
|
15
|
+
class ArmDriver(Node):
|
|
16
|
+
def __init__(self,args):
|
|
17
|
+
super().__init__('carm_'+args.device)
|
|
18
|
+
self.args = args
|
|
19
|
+
|
|
20
|
+
self.arm = carm.Carm(args.addr)
|
|
21
|
+
if args.mit:
|
|
22
|
+
self.arm.set_control_mode(2)
|
|
23
|
+
else:
|
|
24
|
+
self.arm.set_control_mode(1)
|
|
25
|
+
|
|
26
|
+
|
|
27
|
+
print("version:",self.arm.version)
|
|
28
|
+
print("limits:", self.arm.limit)
|
|
29
|
+
print("state:", self.arm.state)
|
|
30
|
+
|
|
31
|
+
self.pub_joint = self.create_publisher(JointState, args.joint_topic, 10)
|
|
32
|
+
self.pub_end = self.create_publisher(JointState, args.end_topic, 10)
|
|
33
|
+
|
|
34
|
+
self.sub_joint = self.create_subscription(JointState, args.joint_cmd_topic, self.joint_callback, 10)
|
|
35
|
+
self.sub_end = self.create_subscription(JointState, args.end_cmd_topic, self.end_callback, 10)
|
|
36
|
+
|
|
37
|
+
self.worker = threading.Thread(target=self.loop).start()
|
|
38
|
+
|
|
39
|
+
def end_callback(self, msg):
|
|
40
|
+
self.arm.track_pose(list(msg.position))
|
|
41
|
+
|
|
42
|
+
def joint_callback(self, msg):
|
|
43
|
+
self.arm.track_joint(list(msg.position))
|
|
44
|
+
|
|
45
|
+
def loop(self):
|
|
46
|
+
while True:
|
|
47
|
+
joint_msg = JointState() # list of string
|
|
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
|
|
52
|
+
joint_msg.position.append(self.arm.gripper_state["gripper_pos"])
|
|
53
|
+
self.pub_joint.publish(joint_msg)
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
end_msg = JointState() # list of string
|
|
57
|
+
end_msg.header.stamp = self.get_clock().now().to_msg()
|
|
58
|
+
end_msg.position = np.array(self.arm.cart_pose).tolist()
|
|
59
|
+
end_msg.position.append(self.arm.gripper_state["gripper_pos"])
|
|
60
|
+
self.pub_end.publish(end_msg)
|
|
61
|
+
|
|
62
|
+
time.sleep(0.005)
|
|
63
|
+
|
|
64
|
+
def main():
|
|
65
|
+
parser = argparse.ArgumentParser()
|
|
66
|
+
parser.add_argument("--addr", type=str, default="ws://localhost:8090", help="Device address, including ip and port")
|
|
67
|
+
parser.add_argument("--device", type=str, default="carm", help="device name, used as topic prefix")
|
|
68
|
+
parser.add_argument("--dds", type=str, default="svar_messenger_ros2", help="the dds plugin, default is ros2, options: svar_zbus, svar_lcm")
|
|
69
|
+
parser.add_argument("--mit", action="store_true", help="Enable mit mode")
|
|
70
|
+
parser.add_argument("--joint_topic", type=str, default="", help="the joints status topic")
|
|
71
|
+
parser.add_argument("--end_topic", type=str, default="", help="the joints status topic")
|
|
72
|
+
parser.add_argument("--joint_cmd_topic", type=str, default="", help="the joints cmd topic")
|
|
73
|
+
parser.add_argument("--end_cmd_topic", type=str, default="", help="the end cmd topic")
|
|
74
|
+
|
|
75
|
+
args = parser.parse_args()
|
|
76
|
+
|
|
77
|
+
if args.joint_topic == "":
|
|
78
|
+
args.joint_topic = "/"+args.device + "/joints"
|
|
79
|
+
if args.end_topic == "":
|
|
80
|
+
args.end_topic = "/"+args.device + "/end"
|
|
81
|
+
if args.joint_cmd_topic == "":
|
|
82
|
+
args.joint_cmd_topic = "/"+args.device + "/joints_cmd"
|
|
83
|
+
if args.end_cmd_topic == "":
|
|
84
|
+
args.end_cmd_topic = "/"+args.device + "/end_cmd"
|
|
85
|
+
|
|
86
|
+
|
|
87
|
+
rclpy.init(args=None)
|
|
88
|
+
node = ArmDriver(args)
|
|
89
|
+
rclpy.spin(node)
|
|
90
|
+
node.destroy_node()
|
|
91
|
+
rclpy.shutdown()
|
|
92
|
+
|
|
93
|
+
# 测试代码
|
|
94
|
+
if __name__ == "__main__":
|
|
95
|
+
main()
|
|
96
|
+
|
|
@@ -0,0 +1,9 @@
|
|
|
1
|
+
carm/__init__.py,sha256=lfNjXYpO8FjQVunCqiaUzjsNUnyuhcyO8Je0ixh1RJY,24
|
|
2
|
+
carm/carm.py,sha256=yVKWmDov9c69l7ye1jcWwBWluWgarEUbFhU6tSfaVoA,17159
|
|
3
|
+
carm/carm_dds.py,sha256=9IkexhHYrjNx2nzplEa4Kj0KdLayzACPChtIF1G29BM,5137
|
|
4
|
+
carm/carm_ros2.py,sha256=IxQD4pglxCH2Vbfj6mm2DlV8KgkqMnS0k1-mIICV8I8,3709
|
|
5
|
+
carm-0.0.20250911.dist-info/METADATA,sha256=hpgdZE1IgDYSIAC3HU-RMdfVtbVGgYlhcLWKuWyslJA,389
|
|
6
|
+
carm-0.0.20250911.dist-info/WHEEL,sha256=iAkIy5fosb7FzIOwONchHf19Qu7_1wCWyFNR5gu9nU0,91
|
|
7
|
+
carm-0.0.20250911.dist-info/entry_points.txt,sha256=qsJWC39p-s4UO3KZ8BfV2hikeMX693mahkYbR3w67Lk,76
|
|
8
|
+
carm-0.0.20250911.dist-info/top_level.txt,sha256=nZ1bTssUAnAqc2CMka3erGx_2LsdZrSkqtjNf9Ce_d8,5
|
|
9
|
+
carm-0.0.20250911.dist-info/RECORD,,
|
|
@@ -1,7 +0,0 @@
|
|
|
1
|
-
carm/__init__.py,sha256=lfNjXYpO8FjQVunCqiaUzjsNUnyuhcyO8Je0ixh1RJY,24
|
|
2
|
-
carm/carm.py,sha256=cx04M6b5XyQpB8Xw-9sycwmoE4fKi3tBqGjmCmtABAc,10404
|
|
3
|
-
carm-0.0.20250815.dist-info/METADATA,sha256=wZxNVuffF6HYb3_Qm7giI5qpBFU14luJ1XE0XLMddOw,389
|
|
4
|
-
carm-0.0.20250815.dist-info/WHEEL,sha256=iAkIy5fosb7FzIOwONchHf19Qu7_1wCWyFNR5gu9nU0,91
|
|
5
|
-
carm-0.0.20250815.dist-info/entry_points.txt,sha256=N8dt2p7PK_49n8YkzKy8tymcvKS7JEH9Ob6i093vEUg,62
|
|
6
|
-
carm-0.0.20250815.dist-info/top_level.txt,sha256=nZ1bTssUAnAqc2CMka3erGx_2LsdZrSkqtjNf9Ce_d8,5
|
|
7
|
-
carm-0.0.20250815.dist-info/RECORD,,
|
|
File without changes
|
|
File without changes
|