mirror of
https://github.com/deepmodeling/Uni-Lab-OS
synced 2026-04-26 15:09:56 +00:00
修改移液部分代码，如果没有10ul枪头，则不使用10ul移液枪
This commit is contained in:
q434343
@@ -467,43 +467,58 @@ set_status(command_json)
|
||||
#### 2.1.9 核心方法详解：`pick_and_place`
|
||||
|
||||
```python
|
||||
def pick_and_place(self, command: str)
|
||||
def pick_and_place(
|
||||
self,
|
||||
option: str,
|
||||
move_group: str,
|
||||
status: str,
|
||||
resource: Optional[str] = None,
|
||||
x_distance: Optional[float] = None,
|
||||
y_distance: Optional[float] = None,
|
||||
lift_height: Optional[float] = None,
|
||||
retry: Optional[int] = None,
|
||||
speed: Optional[float] = None,
|
||||
target: Optional[str] = None,
|
||||
constraints: Optional[Sequence[float]] = None,
|
||||
) -> None:
|
||||
```
|
||||
|
||||
这是 `MoveitInterface` 最复杂的方法，实现了完整的抓取-放置工作流。它动态构建一个**有序函数列表** (`function_list`)，然后顺序执行。
|
||||
|
||||
**JSON 指令格式（完整参数）：**
|
||||
**破坏性变更（注册表 / 客户端）**：`pick_and_place` 在注册表中由 **SendCmd**（单字段 `command` JSON 字符串）改为 **UniLabJsonCommand**，goal 为与上表同名的**结构化字段**。云端与其它调用方需按 schema 提交 goal，而不再把整段 JSON 塞进 `command`。
|
||||
|
||||
**动作 goal 示例（字段与旧版 JSON 一致，现为结构化 goal）：**
|
||||
|
||||
```json
|
||||
{
|
||||
"option": "pick", // *必须: pick/place/side_pick/side_place
|
||||
"move_group": "arm", // *必须: MoveIt2 规划组名
|
||||
"status": "pick_station_A", // *必须: 在 joint_poses 中的目标状态名
|
||||
"resource": "beaker_1", // 要操作的资源名称
|
||||
"target": "custom_link", // pick 时资源附着的目标 link (默认末端执行器)
|
||||
"lift_height": 0.05, // 抬升高度 (米)
|
||||
"x_distance": 0.1, // X 方向水平移动距离 (米)
|
||||
"y_distance": 0.0, // Y 方向水平移动距离 (米)
|
||||
"speed": 0.5, // 运动速度因子 (0.1~1.0)
|
||||
"retry": 10, // 规划失败重试次数
|
||||
"constraints": [0, 0, 0, 0.5, 0, 0] // 各关节约束容差 (>0 时生效)
|
||||
"option": "pick",
|
||||
"move_group": "arm",
|
||||
"status": "pick_station_A",
|
||||
"resource": "beaker_1",
|
||||
"target": "custom_link",
|
||||
"lift_height": 0.05,
|
||||
"x_distance": 0.1,
|
||||
"y_distance": 0.0,
|
||||
"speed": 0.5,
|
||||
"retry": 10,
|
||||
"constraints": [0, 0, 0, 0.5, 0, 0]
|
||||
}
|
||||
```
|
||||
|
||||
##### 阶段 1：指令解析与动作类型判定
|
||||
##### 阶段 1：参数与动作类型判定
|
||||
|
||||
```
|
||||
pick_and_place(command_json)
|
||||
pick_and_place(option, move_group, status, ...)
|
||||
│
|
||||
├── JSON 解析
|
||||
├── 校验 option ∈ move_option，否则直接 return
|
||||
├── 动作类型判定:
|
||||
│ move_option = ["pick", "place", "side_pick", "side_place"]
|
||||
│ 0 1 2 3
|
||||
│ option_index = move_option.index(cmd["option"])
|
||||
│ option_index = move_option.index(option)
|
||||
│ place_flag = option_index % 2 ← 0=pick类, 1=place类
|
||||
│
|
||||
├── 提取运动参数:
|
||||
│ config = {speed, retry, move_group} ← 从 cmd_dict 中按需提取
|
||||
│ config = { move_group }；若 speed/retry 非 None 则写入 config
|
||||
│
|
||||
└── 获取目标关节位姿:
|
||||
joint_positions_ = joint_poses[move_group][status]
|
||||
@@ -515,7 +530,7 @@ pick_and_place(command_json)
|
||||
根据 place_flag 决定资源 TF 操作:
|
||||
|
||||
if pick 类 (place_flag == 0):
|
||||
if "target" 已指定:
|
||||
if target is not None:
|
||||
function_list += [resource_manager(resource, target)] ← 挂到自定义 link
|
||||
else:
|
||||
function_list += [resource_manager(resource, end_effector)] ← 挂到末端执行器
|
||||
@@ -527,7 +542,7 @@ pick_and_place(command_json)
|
||||
##### 阶段 3：构建关节约束
|
||||
|
||||
```
|
||||
if "constraints" 存在于指令中:
|
||||
if constraints is not None:
|
||||
for i, tolerance in enumerate(constraints):
|
||||
if tolerance > 0:
|
||||
JointConstraint(
|
||||
@@ -546,7 +561,7 @@ if "constraints" 存在于指令中:
|
||||
这是最复杂的场景，涉及 FK/IK 计算和多段运动拼接：
|
||||
|
||||
```
|
||||
if "lift_height" 存在:
|
||||
if lift_height is not None:
|
||||
│
|
||||
├── Step 1: FK 计算 → 获取目标关节配置对应的末端位姿
|
||||
│ retval = compute_fk(joint_positions_) ← 可能需要重试
|
||||
@@ -562,12 +577,12 @@ if "lift_height" 存在:
|
||||
│ function_list += [moveit_task(position=pose_lifted, ...)]
|
||||
│
|
||||
├── Step 4 (可选): 水平移动
|
||||
│ if "x_distance":
|
||||
│ if x_distance is not None:
|
||||
│ deep_pose = copy(pose_lifted)
|
||||
│ deep_pose[0] += x_distance
|
||||
│ function_list = [moveit_task(pose_lifted)] + function_list
|
||||
│ function_list += [moveit_task(deep_pose)]
|
||||
│ elif "y_distance":
|
||||
│ elif y_distance is not None:
|
||||
│ 类似处理 Y 方向
|
||||
│
|
||||
├── Step 5: IK 预计算 → 将末端位姿转换为安全的关节配置
|
||||
@@ -585,7 +600,7 @@ if "lift_height" 存在:
|
||||
##### 阶段 4B：无 `lift_height` 的简单流程
|
||||
|
||||
```
|
||||
else (无 lift_height):
|
||||
else (lift_height is None):
|
||||
│
|
||||
└── 直接关节运动到目标位姿
|
||||
function_list = [moveit_joint_task(joint_positions_)] + function_list
|
||||
@@ -600,10 +615,10 @@ for i, func in enumerate(function_list):
|
||||
│ i == 0: cartesian_flag = False ← 第一步用自由空间规划（大范围移动）
|
||||
│ i > 0: cartesian_flag = True ← 后续用笛卡尔直线规划（精确控制）
|
||||
│
|
||||
├── result = func() ← 执行动作
|
||||
├── re = func() ← 执行动作
|
||||
│
|
||||
└── if not result:
|
||||
return failure ← 任一步骤失败即中止
|
||||
└── if not re:
|
||||
return（无返回值，不构造 SendCmd.Result）← 任一步骤失败即中止
|
||||
```
|
||||
|
||||
##### 完整 pick 流程示例（含 lift_height + x_distance）
|
||||
@@ -657,10 +672,10 @@ for i, func in enumerate(function_list):
|
||||
| `.move_to_pose(...)` | `moveit_task` L129-137 | 笛卡尔空间运动规划与执行 |
|
||||
| `.wait_until_executed()` | `moveit_task` L138, `moveit_joint_task` L157 | 阻塞等待运动完成 |
|
||||
| `.move_to_configuration(...)` | `moveit_joint_task` L156 | 关节空间运动规划与执行 |
|
||||
| `.compute_fk(...)` | `pick_and_place` L244, `moveit_joint_task` L160 | 正运动学：关节角 → 末端位姿 |
|
||||
| `.compute_ik(...)` | `pick_and_place` L298-300 | 逆运动学：末端位姿 → 关节角（含约束） |
|
||||
| `.end_effector_name` | `pick_and_place` L218 | 获取末端执行器 link 名 |
|
||||
| `.joint_names` | `pick_and_place` L232, L308, L313 | 获取关节名列表 |
|
||||
| `.compute_fk(...)` | `pick_and_place`, `moveit_joint_task` | 正运动学：关节角 → 末端位姿 |
|
||||
| `.compute_ik(...)` | `pick_and_place` | 逆运动学：末端位姿 → 关节角（含约束） |
|
||||
| `.end_effector_name` | `pick_and_place` | 获取末端执行器 link 名 |
|
||||
| `.joint_names` | `pick_and_place` | 获取关节名列表 |
|
||||
|
||||
---
|
||||
|
||||
@@ -668,11 +683,11 @@ for i, func in enumerate(function_list):
|
||||
|
||||
| 场景 | 处理方式 |
|
||||
|------|---------|
|
||||
| FK 计算失败 | 最多重试 `retry` 次（每次间隔 0.1s），超时返回 `result.success = False` |
|
||||
| FK 计算失败 | 最多重试 `retry` 次（每次间隔 0.1s），超时则提前 `return`（无返回值） |
|
||||
| IK 计算失败 | 同上 |
|
||||
| 运动规划失败 | 在 `moveit_task` / `moveit_joint_task` 中最多重试 `retry+1` 次 |
|
||||
| 动作序列中任一步失败 | `pick_and_place` 立即中止并返回 `result.success = False` |
|
||||
| 未知异常 | `pick_and_place` 和 `set_status` 捕获 Exception，重置 `cartesian_flag`，返回失败 |
|
||||
| 动作序列中任一步失败 | `pick_and_place` 立即中止并 `return`（不返回 `SendCmd.Result`） |
|
||||
| 未知异常 | `pick_and_place` 捕获 Exception，打印并重置 `cartesian_flag`（`set_status` 仍返回 SendCmd.Result） |
|
||||
|
||||
---
|
||||
|
||||
@@ -702,7 +717,8 @@ for i, func in enumerate(function_list):
|
||||
```
|
||||
外部系统 (base_device_node)
|
||||
│
|
||||
│ JSON 指令字符串
|
||||
│ set_position/set_status: JSON 指令字符串（SendCmd.command）
|
||||
│ pick_and_place: UniLabJsonCommand 结构化 goal → Python 关键字参数
|
||||
▼
|
||||
┌── MoveitInterface ──────────────────────────────────────────────────┐
|
||||
│ │
|
||||
@@ -710,7 +726,7 @@ for i, func in enumerate(function_list):
|
||||
│ │
|
||||
│ set_status(cmd) ──→ moveit_joint_task() ──→ MoveIt2.move_to_config│
|
||||
│ │
|
||||
│ pick_and_place(cmd) │
|
||||
│ pick_and_place(option, move_group, status, ...) │
|
||||
│ │ │
|
||||
│ ├─ MoveIt2.compute_fk() ─── /compute_fk service ──→ move_group │
|
||||
│ ├─ MoveIt2.compute_ik() ─── /compute_ik service ──→ move_group │
|
||||
@@ -963,7 +979,7 @@ robotic_arm.SCARA_with_slider.moveit.virtual:
|
||||
module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
|
||||
type: python
|
||||
action_value_mappings:
|
||||
pick_and_place: ... # SendCmd Action（JSON 指令）
|
||||
pick_and_place: ... # UniLabJsonCommand（结构化 goal，与 Python 签名一致）
|
||||
set_position: ... # SendCmd Action
|
||||
set_status: ... # SendCmd Action
|
||||
auto-moveit_task: ... # 自动发现的方法（UniLabJsonCommand）
|
||||
|
||||
@@ -1196,6 +1196,27 @@ class PRCXI9300Handler(LiquidHandlerAbstract):
|
||||
none_keys=none_keys,
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _tip_rack_is_10ul_range(rack: TipRack) -> bool:
|
||||
"""判断 tip 盒是否为 10µL 量程（对应右头）；优先用孔位上 prototype tip 的 maximal_volume。"""
|
||||
children = getattr(rack, "children", None) or []
|
||||
if children:
|
||||
spot = children[0]
|
||||
tr = getattr(spot, "tracker", None)
|
||||
tip = None
|
||||
if tr is not None:
|
||||
tip = getattr(tr, "_tip", None) or getattr(tr, "tip", None)
|
||||
if tip is None:
|
||||
tip = getattr(spot, "tip", None)
|
||||
mv = getattr(tip, "maximal_volume", None) if tip is not None else None
|
||||
if mv is not None:
|
||||
try:
|
||||
return float(mv) <= 10.0
|
||||
except (TypeError, ValueError):
|
||||
pass
|
||||
ident = f"{getattr(rack, 'model', '') or ''} {type(rack).__name__}".lower()
|
||||
return "10ul" in ident
|
||||
|
||||
async def transfer_liquid(
|
||||
self,
|
||||
sources: Sequence[Container],
|
||||
@@ -1230,19 +1251,20 @@ class PRCXI9300Handler(LiquidHandlerAbstract):
|
||||
|
||||
_asp_list = asp_vols if isinstance(asp_vols, list) else [asp_vols]
|
||||
_dis_list = dis_vols if isinstance(dis_vols, list) else [dis_vols]
|
||||
if all(v <= 10.0 for v in _asp_list) and all(v <= 10.0 for v in _dis_list):
|
||||
use_channels = [1]
|
||||
mix_vol = max(min(mix_vol,10),0) if mix_vol is not None else None
|
||||
sources = await self._resolve_to_plr_resources(sources)
|
||||
targets = await self._resolve_to_plr_resources(targets)
|
||||
tip_racks = list(await self._resolve_to_plr_resources(tip_racks))
|
||||
change_slots = []
|
||||
change_slots.append(sources[0].parent)
|
||||
change_slots.append(targets[0].parent)
|
||||
if isinstance(tip_racks[0], TipRack):
|
||||
tip_rack = tip_racks[0]
|
||||
else:
|
||||
tip_rack = tip_racks[0].parent
|
||||
small_vols = all(v <= 10.0 for v in _asp_list) and all(v <= 10.0 for v in _dis_list)
|
||||
if small_vols and self._tip_rack_is_10ul_range(tip_rack):
|
||||
use_channels = [1]
|
||||
mix_vol = max(min(mix_vol, 10), 0) if mix_vol is not None else None
|
||||
change_slots = []
|
||||
change_slots.append(sources[0].parent)
|
||||
change_slots.append(targets[0].parent)
|
||||
|
||||
change_slots.append(tip_rack)
|
||||
|
||||
|
||||
@@ -2,6 +2,7 @@ import json
|
||||
import time
|
||||
from copy import deepcopy
|
||||
from pathlib import Path
|
||||
from typing import Optional, Sequence
|
||||
|
||||
from moveit_msgs.msg import JointConstraint, Constraints
|
||||
from rclpy.action import ActionClient
|
||||
@@ -171,173 +172,160 @@ class MoveitInterface:
|
||||
|
||||
return True
|
||||
|
||||
def pick_and_place(self, command: str):
|
||||
def pick_and_place(
|
||||
self,
|
||||
option: str,
|
||||
move_group: str,
|
||||
status: str,
|
||||
resource: Optional[str] = None,
|
||||
x_distance: Optional[float] = None,
|
||||
y_distance: Optional[float] = None,
|
||||
lift_height: Optional[float] = None,
|
||||
retry: Optional[int] = None,
|
||||
speed: Optional[float] = None,
|
||||
target: Optional[str] = None,
|
||||
constraints: Optional[Sequence[float]] = None,
|
||||
) -> None:
|
||||
"""
|
||||
Using MoveIt to make the robotic arm pick or place materials to a target point.
|
||||
使用 MoveIt 完成抓取/放置等序列（pick/place/side_pick/side_place）。
|
||||
|
||||
Args:
|
||||
command: A JSON-formatted string that includes option, target, speed, lift_height, mt_height
|
||||
|
||||
*option (string) : Action type: pick/place/side_pick/side_place
|
||||
*move_group (string): The move group moveit will plan
|
||||
*status(string) : Target pose
|
||||
resource(string) : The target resource
|
||||
x_distance (float) : The distance to the target in x direction(meters)
|
||||
y_distance (float) : The distance to the target in y direction(meters)
|
||||
lift_height (float) : The height at which the material should be lifted(meters)
|
||||
retry (float) : Retry times when moveit plan fails
|
||||
speed (float) : The speed of the movement, speed > 0
|
||||
Returns:
|
||||
None
|
||||
必选：option, move_group, status。
|
||||
可选：resource, x_distance, y_distance, lift_height, retry, speed, target, constraints。
|
||||
无返回值；失败时提前 return 或打印异常。
|
||||
"""
|
||||
result = SendCmd.Result()
|
||||
|
||||
try:
|
||||
cmd_str = str(command).replace("'", '"')
|
||||
cmd_dict = json.loads(cmd_str)
|
||||
if option not in self.move_option:
|
||||
raise ValueError(f"Invalid option: {option}")
|
||||
|
||||
if cmd_dict["option"] in self.move_option:
|
||||
option_index = self.move_option.index(cmd_dict["option"])
|
||||
place_flag = option_index % 2
|
||||
option_index = self.move_option.index(option)
|
||||
place_flag = option_index % 2
|
||||
|
||||
config = {}
|
||||
function_list = []
|
||||
config: dict = {"move_group": move_group}
|
||||
if speed is not None:
|
||||
config["speed"] = speed
|
||||
if retry is not None:
|
||||
config["retry"] = retry
|
||||
|
||||
status = cmd_dict["status"]
|
||||
joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
|
||||
function_list = []
|
||||
joint_positions_ = self.joint_poses[move_group][status]
|
||||
|
||||
config.update({k: cmd_dict[k] for k in ["speed", "retry", "move_group"] if k in cmd_dict})
|
||||
# 夹取 / 放置：绑定 resource 与 parent
|
||||
if not place_flag:
|
||||
if target is not None:
|
||||
function_list.append(lambda r=resource, t=target: self.resource_manager(r, t))
|
||||
else:
|
||||
ee = self.moveit2[move_group].end_effector_name
|
||||
function_list.append(lambda r=resource: self.resource_manager(r, ee))
|
||||
else:
|
||||
function_list.append(lambda r=resource: self.resource_manager(r, "world"))
|
||||
|
||||
# 夹取
|
||||
if not place_flag:
|
||||
if "target" in cmd_dict.keys():
|
||||
function_list.append(lambda: self.resource_manager(cmd_dict["resource"], cmd_dict["target"]))
|
||||
else:
|
||||
function_list.append(
|
||||
lambda: self.resource_manager(
|
||||
cmd_dict["resource"], self.moveit2[cmd_dict["move_group"]].end_effector_name
|
||||
joint_constraint_msgs: list = []
|
||||
if constraints is not None:
|
||||
for i, c in enumerate(constraints):
|
||||
v = float(c)
|
||||
if v > 0:
|
||||
joint_constraint_msgs.append(
|
||||
JointConstraint(
|
||||
joint_name=self.moveit2[move_group].joint_names[i],
|
||||
position=joint_positions_[i],
|
||||
tolerance_above=v,
|
||||
tolerance_below=v,
|
||||
weight=1.0,
|
||||
)
|
||||
)
|
||||
else:
|
||||
function_list.append(lambda: self.resource_manager(cmd_dict["resource"], "world"))
|
||||
|
||||
constraints = []
|
||||
if "constraints" in cmd_dict.keys():
|
||||
if lift_height is not None:
|
||||
retval = None
|
||||
attempts = config.get("retry", 10)
|
||||
while retval is None and attempts > 0:
|
||||
retval = self.moveit2[move_group].compute_fk(joint_positions_)
|
||||
time.sleep(0.1)
|
||||
attempts -= 1
|
||||
if retval is None:
|
||||
raise ValueError("Failed to compute forward kinematics")
|
||||
pose = [retval.pose.position.x, retval.pose.position.y, retval.pose.position.z]
|
||||
quaternion = [
|
||||
retval.pose.orientation.x,
|
||||
retval.pose.orientation.y,
|
||||
retval.pose.orientation.z,
|
||||
retval.pose.orientation.w,
|
||||
]
|
||||
|
||||
for i in range(len(cmd_dict["constraints"])):
|
||||
v = float(cmd_dict["constraints"][i])
|
||||
if v > 0:
|
||||
constraints.append(
|
||||
JointConstraint(
|
||||
joint_name=self.moveit2[cmd_dict["move_group"]].joint_names[i],
|
||||
position=joint_positions_[i],
|
||||
tolerance_above=v,
|
||||
tolerance_below=v,
|
||||
weight=1.0,
|
||||
)
|
||||
)
|
||||
function_list = [
|
||||
lambda: self.moveit_task(
|
||||
position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z],
|
||||
quaternion=quaternion,
|
||||
**config,
|
||||
cartesian=self.cartesian_flag,
|
||||
)
|
||||
] + function_list
|
||||
|
||||
if "lift_height" in cmd_dict.keys():
|
||||
retval = None
|
||||
retry = config.get("retry", 10)
|
||||
while retval is None and retry > 0:
|
||||
retval = self.moveit2[cmd_dict["move_group"]].compute_fk(joint_positions_)
|
||||
time.sleep(0.1)
|
||||
retry -= 1
|
||||
if retval is None:
|
||||
result.success = False
|
||||
return result
|
||||
pose = [retval.pose.position.x, retval.pose.position.y, retval.pose.position.z]
|
||||
quaternion = [
|
||||
retval.pose.orientation.x,
|
||||
retval.pose.orientation.y,
|
||||
retval.pose.orientation.z,
|
||||
retval.pose.orientation.w,
|
||||
]
|
||||
pose[2] += float(lift_height)
|
||||
function_list.append(
|
||||
lambda p=pose.copy(), q=quaternion, cfg=config: self.moveit_task(
|
||||
position=p, quaternion=q, **cfg, cartesian=self.cartesian_flag
|
||||
)
|
||||
)
|
||||
end_pose = list(pose)
|
||||
|
||||
if x_distance is not None or y_distance is not None:
|
||||
if x_distance is not None:
|
||||
deep_pose = deepcopy(pose)
|
||||
deep_pose[0] += float(x_distance)
|
||||
elif y_distance is not None:
|
||||
deep_pose = deepcopy(pose)
|
||||
deep_pose[1] += float(y_distance)
|
||||
|
||||
function_list = [
|
||||
lambda: self.moveit_task(
|
||||
position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z],
|
||||
quaternion=quaternion,
|
||||
**config,
|
||||
cartesian=self.cartesian_flag,
|
||||
lambda p=pose.copy(), q=quaternion, cfg=config: self.moveit_task(
|
||||
position=p, quaternion=q, **cfg, cartesian=self.cartesian_flag
|
||||
)
|
||||
] + function_list
|
||||
|
||||
pose[2] += float(cmd_dict["lift_height"])
|
||||
function_list.append(
|
||||
lambda: self.moveit_task(
|
||||
position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
|
||||
lambda dp=deep_pose.copy(), q=quaternion, cfg=config: self.moveit_task(
|
||||
position=dp, quaternion=q, **cfg, cartesian=self.cartesian_flag
|
||||
)
|
||||
)
|
||||
end_pose = pose
|
||||
end_pose = list(deep_pose)
|
||||
|
||||
if "x_distance" in cmd_dict.keys() or "y_distance" in cmd_dict.keys():
|
||||
if "x_distance" in cmd_dict.keys():
|
||||
deep_pose = deepcopy(pose)
|
||||
deep_pose[0] += float(cmd_dict["x_distance"])
|
||||
elif "y_distance" in cmd_dict.keys():
|
||||
deep_pose = deepcopy(pose)
|
||||
deep_pose[1] += float(cmd_dict["y_distance"])
|
||||
retval_ik = None
|
||||
attempts_ik = config.get("retry", 10)
|
||||
while retval_ik is None and attempts_ik > 0:
|
||||
retval_ik = self.moveit2[move_group].compute_ik(
|
||||
position=end_pose,
|
||||
quat_xyzw=quaternion,
|
||||
constraints=Constraints(joint_constraints=joint_constraint_msgs),
|
||||
)
|
||||
time.sleep(0.1)
|
||||
attempts_ik -= 1
|
||||
if retval_ik is None:
|
||||
raise ValueError("Failed to compute inverse kinematics")
|
||||
position_ = [
|
||||
retval_ik.position[retval_ik.name.index(i)] for i in self.moveit2[move_group].joint_names
|
||||
]
|
||||
jn = self.moveit2[move_group].joint_names
|
||||
function_list = [
|
||||
lambda pos=position_, names=jn, cfg=config: self.moveit_joint_task(
|
||||
joint_positions=pos, joint_names=names, **cfg
|
||||
)
|
||||
] + function_list
|
||||
else:
|
||||
function_list = [lambda cfg=config, jp=joint_positions_: self.moveit_joint_task(**cfg, joint_positions=jp)] + function_list
|
||||
|
||||
function_list = [
|
||||
lambda: self.moveit_task(
|
||||
position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
|
||||
)
|
||||
] + function_list
|
||||
function_list.append(
|
||||
lambda: self.moveit_task(
|
||||
position=deep_pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
|
||||
)
|
||||
)
|
||||
end_pose = deep_pose
|
||||
|
||||
retval_ik = None
|
||||
retry = config.get("retry", 10)
|
||||
while retval_ik is None and retry > 0:
|
||||
retval_ik = self.moveit2[cmd_dict["move_group"]].compute_ik(
|
||||
position=end_pose, quat_xyzw=quaternion, constraints=Constraints(joint_constraints=constraints)
|
||||
)
|
||||
time.sleep(0.1)
|
||||
retry -= 1
|
||||
if retval_ik is None:
|
||||
result.success = False
|
||||
return result
|
||||
position_ = [
|
||||
retval_ik.position[retval_ik.name.index(i)]
|
||||
for i in self.moveit2[cmd_dict["move_group"]].joint_names
|
||||
]
|
||||
function_list = [
|
||||
lambda: self.moveit_joint_task(
|
||||
joint_positions=position_,
|
||||
joint_names=self.moveit2[cmd_dict["move_group"]].joint_names,
|
||||
**config,
|
||||
)
|
||||
] + function_list
|
||||
for i in range(len(function_list)):
|
||||
if i == 0:
|
||||
self.cartesian_flag = False
|
||||
else:
|
||||
function_list = [
|
||||
lambda: self.moveit_joint_task(**config, joint_positions=joint_positions_)
|
||||
] + function_list
|
||||
self.cartesian_flag = True
|
||||
|
||||
for i in range(len(function_list)):
|
||||
if i == 0:
|
||||
self.cartesian_flag = False
|
||||
else:
|
||||
self.cartesian_flag = True
|
||||
|
||||
re = function_list[i]()
|
||||
if not re:
|
||||
print(i, re)
|
||||
result.success = False
|
||||
return result
|
||||
result.success = True
|
||||
re = function_list[i]()
|
||||
if not re:
|
||||
print(i, re)
|
||||
raise ValueError(f"Failed to execute moveit task: {i}")
|
||||
|
||||
except Exception as e:
|
||||
print(e)
|
||||
self.cartesian_flag = False
|
||||
result.success = False
|
||||
|
||||
return result
|
||||
raise e
|
||||
|
||||
def set_status(self, command: str):
|
||||
"""
|
||||
|
||||
@@ -173,48 +173,64 @@ robotic_arm.SCARA_with_slider.moveit.virtual:
|
||||
type: object
|
||||
type: UniLabJsonCommand
|
||||
pick_and_place:
|
||||
feedback:
|
||||
status: status
|
||||
goal:
|
||||
command: command
|
||||
feedback: {}
|
||||
goal: {}
|
||||
goal_default:
|
||||
command: ''
|
||||
constraints: null
|
||||
lift_height: null
|
||||
move_group: null
|
||||
option: null
|
||||
resource: null
|
||||
retry: null
|
||||
speed: null
|
||||
status: null
|
||||
target: null
|
||||
x_distance: null
|
||||
y_distance: null
|
||||
handles: {}
|
||||
placeholder_keys: {}
|
||||
result:
|
||||
return_info: return_info
|
||||
success: success
|
||||
result: {}
|
||||
schema:
|
||||
description: ''
|
||||
description: pick_and_place 显式参数（UniLabJsonCommand）
|
||||
properties:
|
||||
feedback:
|
||||
additionalProperties: false
|
||||
feedback: {}
|
||||
goal:
|
||||
properties:
|
||||
constraints:
|
||||
items:
|
||||
type: number
|
||||
type: array
|
||||
lift_height:
|
||||
type: string
|
||||
move_group:
|
||||
type: string
|
||||
option:
|
||||
type: string
|
||||
resource:
|
||||
type: string
|
||||
retry:
|
||||
type: string
|
||||
speed:
|
||||
type: string
|
||||
status:
|
||||
type: string
|
||||
title: SendCmd_Feedback
|
||||
type: object
|
||||
goal:
|
||||
additionalProperties: false
|
||||
properties:
|
||||
command:
|
||||
target:
|
||||
type: string
|
||||
title: SendCmd_Goal
|
||||
type: object
|
||||
result:
|
||||
additionalProperties: false
|
||||
properties:
|
||||
return_info:
|
||||
x_distance:
|
||||
type: string
|
||||
success:
|
||||
type: boolean
|
||||
title: SendCmd_Result
|
||||
y_distance:
|
||||
type: string
|
||||
required:
|
||||
- option
|
||||
- move_group
|
||||
- status
|
||||
type: object
|
||||
result: {}
|
||||
required:
|
||||
- goal
|
||||
title: SendCmd
|
||||
title: pick_and_place参数
|
||||
type: object
|
||||
type: SendCmd
|
||||
type: UniLabJsonCommand
|
||||
set_position:
|
||||
feedback:
|
||||
status: status
|
||||
|
||||
@@ -684,48 +684,64 @@ linear_motion.toyo_xyz.sim:
|
||||
type: object
|
||||
type: UniLabJsonCommand
|
||||
pick_and_place:
|
||||
feedback:
|
||||
status: status
|
||||
goal:
|
||||
command: command
|
||||
feedback: {}
|
||||
goal: {}
|
||||
goal_default:
|
||||
command: ''
|
||||
constraints: null
|
||||
lift_height: null
|
||||
move_group: null
|
||||
option: null
|
||||
resource: null
|
||||
retry: null
|
||||
speed: null
|
||||
status: null
|
||||
target: null
|
||||
x_distance: null
|
||||
y_distance: null
|
||||
handles: {}
|
||||
placeholder_keys: {}
|
||||
result:
|
||||
return_info: return_info
|
||||
success: success
|
||||
result: {}
|
||||
schema:
|
||||
description: ''
|
||||
description: pick_and_place 显式参数（UniLabJsonCommand）
|
||||
properties:
|
||||
feedback:
|
||||
additionalProperties: false
|
||||
feedback: {}
|
||||
goal:
|
||||
properties:
|
||||
constraints:
|
||||
items:
|
||||
type: number
|
||||
type: array
|
||||
lift_height:
|
||||
type: string
|
||||
move_group:
|
||||
type: string
|
||||
option:
|
||||
type: string
|
||||
resource:
|
||||
type: string
|
||||
retry:
|
||||
type: string
|
||||
speed:
|
||||
type: string
|
||||
status:
|
||||
type: string
|
||||
title: SendCmd_Feedback
|
||||
type: object
|
||||
goal:
|
||||
additionalProperties: false
|
||||
properties:
|
||||
command:
|
||||
target:
|
||||
type: string
|
||||
title: SendCmd_Goal
|
||||
type: object
|
||||
result:
|
||||
additionalProperties: false
|
||||
properties:
|
||||
return_info:
|
||||
x_distance:
|
||||
type: string
|
||||
success:
|
||||
type: boolean
|
||||
title: SendCmd_Result
|
||||
y_distance:
|
||||
type: string
|
||||
required:
|
||||
- option
|
||||
- move_group
|
||||
- status
|
||||
type: object
|
||||
result: {}
|
||||
required:
|
||||
- goal
|
||||
title: SendCmd
|
||||
title: pick_and_place参数
|
||||
type: object
|
||||
type: SendCmd
|
||||
type: UniLabJsonCommand
|
||||
set_position:
|
||||
feedback:
|
||||
status: status
|
||||
|
||||
@@ -543,7 +543,12 @@ class BaseROS2DeviceNode(Node, Generic[T]):
|
||||
}
|
||||
)
|
||||
tree_response: SerialCommand.Response = await client.call_async(request)
|
||||
uuid_maps = json.loads(tree_response.response)
|
||||
_raw_resp = tree_response.response if tree_response else ""
|
||||
if _raw_resp:
|
||||
uuid_maps = json.loads(_raw_resp)
|
||||
else:
|
||||
uuid_maps = {}
|
||||
self._lab_logger.warning("Resource tree add 返回空响应，跳过 UUID 映射")
|
||||
self.resource_tracker.loop_update_uuid(input_resources, uuid_maps)
|
||||
self._lab_logger.info(f"Resource tree added. UUID mapping: {len(uuid_maps)} nodes")
|
||||
# 这里created_resources不包含parent_resource
|
||||
@@ -1729,10 +1734,25 @@ class BaseROS2DeviceNode(Node, Generic[T]):
|
||||
try:
|
||||
function_args[arg_name] = self._convert_resources_sync(resource_data["uuid"])[0]
|
||||
except Exception as e:
|
||||
self.lab_logger().error(
|
||||
f"转换ResourceSlot参数 {arg_name} 失败: {e}\n{traceback.format_exc()}"
|
||||
# UUID 在资源树中不存在，尝试从传入的完整 dict 直接构建 PLR 资源
|
||||
self.lab_logger().warning(
|
||||
f"UUID查询 {arg_name} 失败，尝试从传入数据直接构建: {e}"
|
||||
)
|
||||
raise JsonCommandInitError(f"ResourceSlot参数转换失败: {arg_name}")
|
||||
try:
|
||||
fallback_tree = ResourceTreeSet.from_raw_dict_list([resource_data])
|
||||
if len(fallback_tree.trees) == 0:
|
||||
raise
|
||||
plr_list = fallback_tree.to_plr_resources()
|
||||
if not plr_list:
|
||||
raise
|
||||
plr_res = plr_list[0]
|
||||
figured = self.resource_tracker.figure_resource(plr_res, try_mode=True)
|
||||
function_args[arg_name] = figured[0] if figured else plr_res
|
||||
except Exception:
|
||||
self.lab_logger().error(
|
||||
f"转换ResourceSlot参数 {arg_name} 失败（含回退）: {e}\n{traceback.format_exc()}"
|
||||
)
|
||||
raise JsonCommandInitError(f"ResourceSlot参数转换失败: {arg_name}")
|
||||
|
||||
# 处理 ResourceSlot 列表
|
||||
elif isinstance(arg_type, tuple) and len(arg_type) == 2:
|
||||
@@ -1744,10 +1764,25 @@ class BaseROS2DeviceNode(Node, Generic[T]):
|
||||
uuids = [r["uuid"] for r in resource_list if isinstance(r, dict) and "id" in r]
|
||||
function_args[arg_name] = self._convert_resources_sync(*uuids) if uuids else []
|
||||
except Exception as e:
|
||||
self.lab_logger().error(
|
||||
f"转换ResourceSlot列表参数 {arg_name} 失败: {e}\n{traceback.format_exc()}"
|
||||
self.lab_logger().warning(
|
||||
f"UUID查询列表 {arg_name} 失败，尝试从传入数据直接构建: {e}"
|
||||
)
|
||||
raise JsonCommandInitError(f"ResourceSlot列表参数转换失败: {arg_name}")
|
||||
try:
|
||||
dict_items = [r for r in resource_list if isinstance(r, dict) and "id" in r]
|
||||
fallback_tree = ResourceTreeSet.from_raw_dict_list(dict_items)
|
||||
if len(fallback_tree.trees) == 0:
|
||||
raise
|
||||
plr_list = fallback_tree.to_plr_resources()
|
||||
resolved = []
|
||||
for plr_res in plr_list:
|
||||
figured = self.resource_tracker.figure_resource(plr_res, try_mode=True)
|
||||
resolved.append(figured[0] if figured else plr_res)
|
||||
function_args[arg_name] = resolved
|
||||
except Exception:
|
||||
self.lab_logger().error(
|
||||
f"转换ResourceSlot列表参数 {arg_name} 失败（含回退）: {e}\n{traceback.format_exc()}"
|
||||
)
|
||||
raise JsonCommandInitError(f"ResourceSlot列表参数转换失败: {arg_name}")
|
||||
|
||||
# todo: 默认反报送
|
||||
return function(**function_args)
|
||||
|
||||
@@ -28,7 +28,7 @@
|
||||
"matrix_id": "",
|
||||
"simulator": false,
|
||||
"channel_num": 2,
|
||||
"step_mode": true,
|
||||
"step_mode": false,
|
||||
"calibration_points": {
|
||||
"line_1": [[452.07,21.19], [313.88,21.19], [176.87,21.19], [39.08,21.19]],
|
||||
"line_2": [[451.37,116.68], [313.28,116.88], [176.58,116.69], [38.58,117.18]],
|
||||
|
||||
Reference in New Issue
Block a user