diff --git a/clients/__pycache__/arm_motion_client.cpython-39.pyc b/clients/__pycache__/arm_motion_client.cpython-39.pyc new file mode 100644 index 0000000..5b4267b Binary files /dev/null and b/clients/__pycache__/arm_motion_client.cpython-39.pyc differ diff --git a/clients/__pycache__/base_client.cpython-39.pyc b/clients/__pycache__/base_client.cpython-39.pyc index 806cfb0..51d90ea 100644 Binary files a/clients/__pycache__/base_client.cpython-39.pyc and b/clients/__pycache__/base_client.cpython-39.pyc differ diff --git a/clients/__pycache__/hand_client.cpython-39.pyc b/clients/__pycache__/hand_client.cpython-39.pyc index 8bc2225..967eb3e 100644 Binary files a/clients/__pycache__/hand_client.cpython-39.pyc and b/clients/__pycache__/hand_client.cpython-39.pyc differ diff --git a/clients/__pycache__/movej_client.cpython-39.pyc b/clients/__pycache__/movej_client.cpython-39.pyc index 4ef95f7..c544f9e 100644 Binary files a/clients/__pycache__/movej_client.cpython-39.pyc and b/clients/__pycache__/movej_client.cpython-39.pyc differ diff --git a/clients/arm_motion_client.py b/clients/arm_motion_client.py new file mode 100644 index 0000000..6b53064 --- /dev/null +++ b/clients/arm_motion_client.py @@ -0,0 +1,134 @@ +from __future__ import annotations + +from typing import Iterable, Mapping + +from clients._path_setup import ensure_paths + +ensure_paths() + +from google.protobuf import timestamp_pb2 + +from clients.base_client import RobotClientBase +from cmvr.api import arm_command_pb2 as pb +from cmvr.api import common_pb2 + + +FRAME_BY_NAME = { + "base": pb.ARM_FRAME_BASE, + "tool": pb.ARM_FRAME_TOOL, + "world": pb.ARM_FRAME_WORLD, + "user": pb.ARM_FRAME_USER, +} + + +class ArmMotionClient(RobotClientBase): + def _header(self, device_id: str) -> common_pb2.CommandHeader.Request: + header = common_pb2.CommandHeader.Request() + header.device_id = device_id + ts = timestamp_pb2.Timestamp() + ts.GetCurrentTime() + header.timestamp.CopyFrom(ts) + return header + + def _frame(self, frame: str) -> int: + return FRAME_BY_NAME.get(frame.strip().lower(), pb.ARM_FRAME_BASE) + + def _print_feedback(self, action: str, response) -> None: + header = response.header + print(f"{action} RPC call succeeded") + print(f"Success: {getattr(header, 'success', None)}") + print(f"Error message: {getattr(header, 'error_message', '')}") + print(f"Timestamp: {getattr(header.timestamp, 'seconds', 0)}") + + def movej( + self, + joint_list: Iterable[Mapping[str, float]], + vel: float = 1.0, + acc: float = 0.5, + device_id: str = "right_arm", + ) -> None: + target = pb.JointPositionCommand(position=[float(j["rad"]) for j in joint_list]) + req = pb.MoveJ.Request( + header=self._header(device_id), + target=target, + options=pb.MotionOptions(velocity=vel, acceleration=acc), + ) + self._print_feedback("MoveJ", self.stub.moveJ(req, timeout=10)) + + def movel( + self, + pose: Mapping[str, float], + vel: float = 0.2, + acc: float = 0.2, + blend_radius: float = 0.0, + frame: str = "base", + device_id: str = "right_arm", + ) -> None: + req = pb.MoveL.Request( + header=self._header(device_id), + target=pb.CartesianPose( + x=float(pose["x"]), + y=float(pose["y"]), + z=float(pose["z"]), + rx=float(pose["rx"]), + ry=float(pose["ry"]), + rz=float(pose["rz"]), + ), + options=pb.MotionOptions( + velocity=vel, + acceleration=acc, + blend_radius=blend_radius, + ), + frame=self._frame(frame), + ) + self._print_feedback("MoveL", self.stub.moveL(req, timeout=10)) + + def speedj( + self, + velocity_list: Iterable[Mapping[str, float]], + acc: float = 0.5, + duration: float = 0.2, + device_id: str = "right_arm", + ) -> None: + velocity = pb.JointVelocityCommand( + velocity=[float(j["velocity"]) for j in velocity_list] + ) + req = pb.SpeedJ.Request( + header=self._header(device_id), + velocity=velocity, + acceleration=acc, + duration=duration, + ) + self._print_feedback("SpeedJ", self.stub.speedJ(req, timeout=10)) + + def speedl( + self, + velocity: Mapping[str, float], + acc: float = 0.2, + duration: float = 0.2, + frame: str = "base", + device_id: str = "right_arm", + ) -> None: + req = pb.SpeedL.Request( + header=self._header(device_id), + velocity=pb.CartesianVelocity( + vx=float(velocity["vx"]), + vy=float(velocity["vy"]), + vz=float(velocity["vz"]), + wx=float(velocity["wx"]), + wy=float(velocity["wy"]), + wz=float(velocity["wz"]), + ), + acceleration=acc, + duration=duration, + frame=self._frame(frame), + ) + self._print_feedback("SpeedL", self.stub.speedL(req, timeout=10)) + + def stop_motion(self, device_id: str = "right_arm") -> None: + req = self._header(device_id) + resp = self.stub.stopMotion(req, timeout=10) + print("StopMotion RPC call succeeded") + print(f"Success: {getattr(resp, 'success', None)}") + print(f"Error message: {getattr(resp, 'error_message', '')}") + print(f"Timestamp: {getattr(resp.timestamp, 'seconds', 0)}") diff --git a/clients/base_client.py b/clients/base_client.py index 6925ee4..87ef91b 100644 --- a/clients/base_client.py +++ b/clients/base_client.py @@ -11,7 +11,7 @@ from cmvr.api import arm_service_pb2_grpc class RobotClientBase: - def __init__(self, address="192.168.0.222:50052", timeout=2, retries=1): + def __init__(self, address="192.168.0.233:50052", timeout=2, retries=1): self.address = address self.timeout = timeout self.retries = retries diff --git a/clients/hand_client.py b/clients/hand_client.py index 729203f..75e0951 100644 --- a/clients/hand_client.py +++ b/clients/hand_client.py @@ -104,7 +104,7 @@ if __name__ == "__main__": # ] # client.set_angles(hand_open, device_id="hand2") - client.set_angles(hand_touch, device_id="hand2") + client.set_angles(hand_open, device_id="hand2") # client.set_angles(handshake, device_id="hand1") # Close client diff --git a/ui/__pycache__/app.cpython-313.pyc b/ui/__pycache__/app.cpython-313.pyc index 628103d..fbccee7 100644 Binary files a/ui/__pycache__/app.cpython-313.pyc and b/ui/__pycache__/app.cpython-313.pyc differ diff --git a/ui/__pycache__/run_action.cpython-39.pyc b/ui/__pycache__/run_action.cpython-39.pyc index b0b6771..e2bb397 100644 Binary files a/ui/__pycache__/run_action.cpython-39.pyc and b/ui/__pycache__/run_action.cpython-39.pyc differ diff --git a/ui/app.py b/ui/app.py index 9a0f8d0..2343a36 100644 --- a/ui/app.py +++ b/ui/app.py @@ -171,6 +171,7 @@ class MainWindow(QtWidgets.QMainWindow): self._pose_worker: Optional[PoseWorker] = None self._plot_selected: set[str] = set() self._last_data: Dict[str, Tuple[float, float]] = {} + self._last_pose: Dict[str, float] = {} self._updating_movej_table = False self.urdf: Optional[URDF] = None self.urdf_error: Optional[str] = None @@ -273,7 +274,7 @@ class MainWindow(QtWidgets.QMainWindow): panel = QtWidgets.QWidget() layout = QtWidgets.QVBoxLayout(panel) layout.addWidget(self._build_settings_group()) - layout.addWidget(self._build_movej_group()) + layout.addWidget(self._build_motion_group()) layout.addWidget(self._build_actions_group()) layout.addStretch(1) return panel @@ -284,18 +285,132 @@ class MainWindow(QtWidgets.QMainWindow): layout.addLayout(self._build_form()) return group - def _build_movej_group(self) -> QtWidgets.QGroupBox: - group = QtWidgets.QGroupBox("MoveJ") + def _build_motion_group(self) -> QtWidgets.QGroupBox: + group = QtWidgets.QGroupBox("Motion") layout = QtWidgets.QVBoxLayout(group) + tabs = QtWidgets.QTabWidget() + tabs.addTab(self._build_movej_tab(), "MoveJ") + tabs.addTab(self._build_movel_tab(), "MoveL") + tabs.addTab(self._build_speedj_tab(), "SpeedJ") + tabs.addTab(self._build_speedl_tab(), "SpeedL") + layout.addWidget(tabs) + return group + + def _build_movej_tab(self) -> QtWidgets.QWidget: + tab = QtWidgets.QWidget() + layout = QtWidgets.QVBoxLayout(tab) params = QtWidgets.QFormLayout() params.addRow("MoveJ Vel", self.vel_edit) params.addRow("MoveJ Acc", self.acc_edit) layout.addLayout(params) layout.addWidget(self._build_movej_table()) + movej_actions = QtWidgets.QHBoxLayout() + self.movej_current_btn = QtWidgets.QPushButton("Fill Current Angles") + self.movej_current_btn.clicked.connect(self._on_fill_movej_current_angles) + movej_actions.addWidget(self.movej_current_btn) self.movej_btn = QtWidgets.QPushButton("Send MoveJ") self.movej_btn.clicked.connect(self._on_movej) - layout.addWidget(self.movej_btn) - return group + movej_actions.addWidget(self.movej_btn) + layout.addLayout(movej_actions) + return tab + + def _build_movel_tab(self) -> QtWidgets.QWidget: + tab = QtWidgets.QWidget() + layout = QtWidgets.QVBoxLayout(tab) + form = QtWidgets.QFormLayout() + self.movel_x_edit = self._build_double_spin(-10.0, 10.0, 0.0, decimals=6, step=0.001) + self.movel_y_edit = self._build_double_spin(-10.0, 10.0, 0.0, decimals=6, step=0.001) + self.movel_z_edit = self._build_double_spin(-10.0, 10.0, 0.0, decimals=6, step=0.001) + self.movel_rx_edit = self._build_double_spin(-6.283185, 6.283185, 0.0, decimals=6, step=0.01) + self.movel_ry_edit = self._build_double_spin(-6.283185, 6.283185, 0.0, decimals=6, step=0.01) + self.movel_rz_edit = self._build_double_spin(-6.283185, 6.283185, 0.0, decimals=6, step=0.01) + self.movel_vel_edit = self._build_double_spin(0.001, 10.0, 0.2, decimals=4, step=0.01) + self.movel_acc_edit = self._build_double_spin(0.001, 10.0, 0.2, decimals=4, step=0.01) + self.movel_blend_edit = self._build_double_spin(0.0, 10.0, 0.0, decimals=4, step=0.001) + self.movel_frame_combo = self._build_frame_combo() + form.addRow("X", self.movel_x_edit) + form.addRow("Y", self.movel_y_edit) + form.addRow("Z", self.movel_z_edit) + form.addRow("RX", self.movel_rx_edit) + form.addRow("RY", self.movel_ry_edit) + form.addRow("RZ", self.movel_rz_edit) + form.addRow("Vel", self.movel_vel_edit) + form.addRow("Acc", self.movel_acc_edit) + form.addRow("Blend Radius", self.movel_blend_edit) + form.addRow("Frame", self.movel_frame_combo) + layout.addLayout(form) + movel_actions = QtWidgets.QHBoxLayout() + self.movel_current_btn = QtWidgets.QPushButton("Fill Current Pose") + self.movel_current_btn.clicked.connect(self._on_fill_movel_current_pose) + movel_actions.addWidget(self.movel_current_btn) + self.movel_btn = QtWidgets.QPushButton("Send MoveL") + self.movel_btn.clicked.connect(self._on_movel) + movel_actions.addWidget(self.movel_btn) + layout.addLayout(movel_actions) + return tab + + def _build_speedj_tab(self) -> QtWidgets.QWidget: + tab = QtWidgets.QWidget() + layout = QtWidgets.QVBoxLayout(tab) + form = QtWidgets.QFormLayout() + self.speedj_acc_edit = self._build_double_spin(0.001, 10.0, 0.5, decimals=4, step=0.01) + self.speedj_duration_edit = self._build_double_spin(0.001, 10.0, 0.2, decimals=4, step=0.01) + form.addRow("Acc", self.speedj_acc_edit) + form.addRow("Duration", self.speedj_duration_edit) + layout.addLayout(form) + layout.addWidget(self._build_speedj_table()) + self.speedj_btn = QtWidgets.QPushButton("Send SpeedJ") + self.speedj_btn.clicked.connect(self._on_speedj) + layout.addWidget(self.speedj_btn) + return tab + + def _build_speedl_tab(self) -> QtWidgets.QWidget: + tab = QtWidgets.QWidget() + layout = QtWidgets.QVBoxLayout(tab) + form = QtWidgets.QFormLayout() + self.speedl_vx_edit = self._build_double_spin(-10.0, 10.0, 0.0, decimals=6, step=0.001) + self.speedl_vy_edit = self._build_double_spin(-10.0, 10.0, 0.0, decimals=6, step=0.001) + self.speedl_vz_edit = self._build_double_spin(-10.0, 10.0, 0.0, decimals=6, step=0.001) + self.speedl_wx_edit = self._build_double_spin(-10.0, 10.0, 0.0, decimals=6, step=0.001) + self.speedl_wy_edit = self._build_double_spin(-10.0, 10.0, 0.0, decimals=6, step=0.001) + self.speedl_wz_edit = self._build_double_spin(-10.0, 10.0, 0.0, decimals=6, step=0.001) + self.speedl_acc_edit = self._build_double_spin(0.001, 10.0, 0.2, decimals=4, step=0.01) + self.speedl_duration_edit = self._build_double_spin(0.001, 10.0, 0.2, decimals=4, step=0.01) + self.speedl_frame_combo = self._build_frame_combo() + form.addRow("VX", self.speedl_vx_edit) + form.addRow("VY", self.speedl_vy_edit) + form.addRow("VZ", self.speedl_vz_edit) + form.addRow("WX", self.speedl_wx_edit) + form.addRow("WY", self.speedl_wy_edit) + form.addRow("WZ", self.speedl_wz_edit) + form.addRow("Acc", self.speedl_acc_edit) + form.addRow("Duration", self.speedl_duration_edit) + form.addRow("Frame", self.speedl_frame_combo) + layout.addLayout(form) + self.speedl_btn = QtWidgets.QPushButton("Send SpeedL") + self.speedl_btn.clicked.connect(self._on_speedl) + layout.addWidget(self.speedl_btn) + return tab + + def _build_double_spin( + self, + minimum: float, + maximum: float, + value: float, + decimals: int = 3, + step: float = 0.001, + ) -> QtWidgets.QDoubleSpinBox: + spin = QtWidgets.QDoubleSpinBox() + spin.setDecimals(decimals) + spin.setRange(minimum, maximum) + spin.setSingleStep(step) + spin.setValue(value) + return spin + + def _build_frame_combo(self) -> QtWidgets.QComboBox: + combo = QtWidgets.QComboBox() + combo.addItems(["base", "tool", "world", "user"]) + return combo def _build_actions_group(self) -> QtWidgets.QGroupBox: group = QtWidgets.QGroupBox("Actions") @@ -303,12 +418,15 @@ class MainWindow(QtWidgets.QMainWindow): self.jointstate_toggle = self._build_switch_row("JointState") self.torque_on_btn = QtWidgets.QPushButton("Torque On") self.torque_off_btn = QtWidgets.QPushButton("Torque Off") + self.stop_motion_btn = QtWidgets.QPushButton("Stop Motion") self.jointstate_toggle.toggled.connect(self._on_jointstate_toggle) self.torque_on_btn.clicked.connect(self._on_torque_on) self.torque_off_btn.clicked.connect(self._on_torque_off) + self.stop_motion_btn.clicked.connect(self._on_stop_motion) layout.addLayout(self._wrap_switch_row(self.jointstate_toggle, "Start JointState")) layout.addWidget(self.torque_on_btn) layout.addWidget(self.torque_off_btn) + layout.addWidget(self.stop_motion_btn) return group def _build_switch_row(self, name: str) -> QtWidgets.QCheckBox: @@ -444,6 +562,23 @@ class MainWindow(QtWidgets.QMainWindow): self.movej_table.itemChanged.connect(self._on_movej_angle_changed) return self.movej_table + def _build_speedj_table(self) -> QtWidgets.QWidget: + self.speedj_table = QtWidgets.QTableWidget(len(self.joint_names), 3) + self.speedj_table.setHorizontalHeaderLabels(["Use", "Joint", "Velocity (rad/s)"]) + self.speedj_table.verticalHeader().setVisible(False) + self.speedj_table.setSizePolicy( + QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding + ) + for row, name in enumerate(self.joint_names): + use_item = QtWidgets.QTableWidgetItem() + use_item.setCheckState(QtCore.Qt.Unchecked) + self.speedj_table.setItem(row, 0, use_item) + self.speedj_table.setItem(row, 1, QtWidgets.QTableWidgetItem(name)) + self.speedj_table.setItem(row, 2, QtWidgets.QTableWidgetItem("0.0")) + self.speedj_table.horizontalHeader().setStretchLastSection(True) + self._fit_table_height(self.speedj_table) + return self.speedj_table + def _fit_table_height(self, table: QtWidgets.QTableWidget) -> None: header_height = table.horizontalHeader().height() row_height = table.verticalHeader().defaultSectionSize() @@ -503,6 +638,72 @@ class MainWindow(QtWidgets.QMainWindow): ] self._start_process(args + ["--joint-list", self._encode_joint_list(joint_list)]) + def _on_movel(self) -> None: + args = self._base_args() + [ + "movel", + "--x", + str(self.movel_x_edit.value()), + "--y", + str(self.movel_y_edit.value()), + "--z", + str(self.movel_z_edit.value()), + "--rx", + str(self.movel_rx_edit.value()), + "--ry", + str(self.movel_ry_edit.value()), + "--rz", + str(self.movel_rz_edit.value()), + "--vel", + str(self.movel_vel_edit.value()), + "--acc", + str(self.movel_acc_edit.value()), + "--blend-radius", + str(self.movel_blend_edit.value()), + "--frame", + self.movel_frame_combo.currentText(), + ] + self._start_process(args) + + def _on_speedj(self) -> None: + joint_list = self._get_selected_speedj_joints() + + if not joint_list: + self._append_log("[warn] No joints selected for SpeedJ.") + return + + args = self._base_args() + [ + "speedj", + "--acc", + str(self.speedj_acc_edit.value()), + "--duration", + str(self.speedj_duration_edit.value()), + ] + self._start_process(args + ["--joint-list", self._encode_joint_value_list(joint_list, "velocity")]) + + def _on_speedl(self) -> None: + args = self._base_args() + [ + "speedl", + "--vx", + str(self.speedl_vx_edit.value()), + "--vy", + str(self.speedl_vy_edit.value()), + "--vz", + str(self.speedl_vz_edit.value()), + "--wx", + str(self.speedl_wx_edit.value()), + "--wy", + str(self.speedl_wy_edit.value()), + "--wz", + str(self.speedl_wz_edit.value()), + "--acc", + str(self.speedl_acc_edit.value()), + "--duration", + str(self.speedl_duration_edit.value()), + "--frame", + self.speedl_frame_combo.currentText(), + ] + self._start_process(args) + def _on_get_pose(self) -> None: if self._joint_worker and self._joint_worker.isRunning(): self._append_log("[info] JointState already running.") @@ -531,6 +732,10 @@ class MainWindow(QtWidgets.QMainWindow): args = self._base_args() + ["torque_off"] self._start_process(args) + def _on_stop_motion(self) -> None: + args = self._base_args() + ["stop_motion"] + self._start_process(args) + def _on_jointstate_toggle(self, checked: bool) -> None: if checked: self._on_get_pose() @@ -547,9 +752,42 @@ class MainWindow(QtWidgets.QMainWindow): self._last_data = data self._render_joint_table_and_plot() + def _on_fill_movej_current_angles(self) -> None: + """Copy the latest JointState positions into the MoveJ command table.""" + if not self._last_data: + self._append_log("[warn] No joint state received. Start JointState first.") + return + + self._updating_movej_table = True + try: + filled = 0 + for row, name in enumerate(self.joint_names): + state = self._last_data.get(name) + if state is None: + continue + position = float(state[0]) + deg_item = self.movej_table.item(row, 2) + rad_item = self.movej_table.item(row, 3) + if deg_item is None: + deg_item = QtWidgets.QTableWidgetItem() + self.movej_table.setItem(row, 2, deg_item) + if rad_item is None: + rad_item = QtWidgets.QTableWidgetItem() + self.movej_table.setItem(row, 3, rad_item) + rad_item.setText(f"{position:.6f}") + deg_item.setText(f"{position * 180.0 / 3.141592653589793:.3f}") + filled += 1 + finally: + self._updating_movej_table = False + + self._append_log(f"[info] Filled {filled} MoveJ angle(s) from JointState.") + def _encode_joint_list(self, joint_list: list[dict]) -> str: return ";".join(f"{j['joint_name']}:{j['rad']}" for j in joint_list) + def _encode_joint_value_list(self, joint_list: list[dict], field_name: str) -> str: + return ";".join(f"{j['joint_name']}:{j[field_name]}" for j in joint_list) + def _get_selected_movej_joints(self) -> list[dict]: joint_list = [] for row, name in enumerate(self.joint_names): @@ -563,6 +801,19 @@ class MainWindow(QtWidgets.QMainWindow): joint_list.append({"joint_name": name, "rad": rad}) return joint_list + def _get_selected_speedj_joints(self) -> list[dict]: + joint_list = [] + for row, name in enumerate(self.joint_names): + use_item = self.speedj_table.item(row, 0) + if use_item and use_item.checkState() == QtCore.Qt.Checked: + velocity_item = self.speedj_table.item(row, 2) + try: + velocity = float(velocity_item.text()) if velocity_item else 0.0 + except ValueError: + velocity = 0.0 + joint_list.append({"joint_name": name, "velocity": velocity}) + return joint_list + def _on_movej_angle_changed(self, item: QtWidgets.QTableWidgetItem) -> None: if self._updating_movej_table: return @@ -629,11 +880,39 @@ class MainWindow(QtWidgets.QMainWindow): self._stop_pose_worker() def _update_pose(self, data: Dict[str, float]) -> None: + self._last_pose = dict(data) for key, value in data.items(): if key in self.pose_labels: self.pose_labels[key].setText(f"{key}: {value:.6f}") return + def _on_fill_movel_current_pose(self) -> None: + """Copy the latest GetPose result into the MoveL target fields.""" + if not self._last_pose: + self._append_log("[warn] No pose received. Start GetPose first.") + return + + fields = ( + ("x", self.movel_x_edit), + ("y", self.movel_y_edit), + ("z", self.movel_z_edit), + ("rx", self.movel_rx_edit), + ("ry", self.movel_ry_edit), + ("rz", self.movel_rz_edit), + ) + filled = 0 + for key, field in fields: + value = self._last_pose.get(key) + if value is None: + continue + field.setValue(float(value)) + filled += 1 + + if filled: + self._append_log(f"[info] Filled {filled} MoveL pose value(s) from GetPose.") + else: + self._append_log("[warn] Current pose has no usable position or orientation values.") + def _render_joint_table_and_plot(self) -> None: selected = set(self._plot_selected) if not self._last_data: diff --git a/ui/run_action.py b/ui/run_action.py index ed7a80e..4b6b204 100644 --- a/ui/run_action.py +++ b/ui/run_action.py @@ -10,7 +10,16 @@ def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description="Run robot client actions.") parser.add_argument( "action", - choices=["movej", "get_pose", "torque_on", "torque_off"], + choices=[ + "movej", + "movel", + "speedj", + "speedl", + "stop_motion", + "get_pose", + "torque_on", + "torque_off", + ], help="Action to run", ) parser.add_argument("--host", default="192.168.0.222", help="gRPC server host") @@ -19,48 +28,146 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--interval", type=float, default=0.05, help="Interval seconds") parser.add_argument("--base-link", default="PELVIS_S", help="Base link for pose") parser.add_argument("--ee-link", default="R_FINGER_TIP", help="End-effector link for pose") - parser.add_argument("--vel", type=float, default=1.0, help="MoveJ velocity") - parser.add_argument("--acc", type=float, default=0.5, help="MoveJ acceleration") + parser.add_argument("--vel", type=float, default=1.0, help="Motion velocity") + parser.add_argument("--acc", type=float, default=0.5, help="Motion acceleration") + parser.add_argument("--duration", type=float, default=0.2, help="Speed command duration") + parser.add_argument("--blend-radius", type=float, default=0.0, help="MoveL blend radius") + parser.add_argument( + "--frame", + choices=["base", "tool", "world", "user"], + default="base", + help="Cartesian command frame", + ) parser.add_argument( "--joint-list", default="", - help="MoveJ joint list format: JOINT:RAD;JOINT:RAD", + help="Joint list format: JOINT:VALUE;JOINT:VALUE", ) + parser.add_argument("--x", type=float, default=0.0, help="Cartesian pose x") + parser.add_argument("--y", type=float, default=0.0, help="Cartesian pose y") + parser.add_argument("--z", type=float, default=0.0, help="Cartesian pose z") + parser.add_argument("--rx", type=float, default=0.0, help="Cartesian pose rx") + parser.add_argument("--ry", type=float, default=0.0, help="Cartesian pose ry") + parser.add_argument("--rz", type=float, default=0.0, help="Cartesian pose rz") + parser.add_argument("--vx", type=float, default=0.0, help="Cartesian velocity x") + parser.add_argument("--vy", type=float, default=0.0, help="Cartesian velocity y") + parser.add_argument("--vz", type=float, default=0.0, help="Cartesian velocity z") + parser.add_argument("--wx", type=float, default=0.0, help="Cartesian angular velocity x") + parser.add_argument("--wy", type=float, default=0.0, help="Cartesian angular velocity y") + parser.add_argument("--wz", type=float, default=0.0, help="Cartesian angular velocity z") return parser.parse_args() +def parse_joint_values(value: str, field_name: str) -> list[dict]: + parsed = [] + for item in value.split(";"): + if not item or ":" not in item: + continue + name, raw = item.split(":", 1) + try: + number = float(raw) + except ValueError: + number = 0.0 + parsed.append({"joint_name": name, field_name: number}) + return parsed + + def main() -> int: args = parse_args() address = f"{args.host}:{args.port}" if args.action == "movej": - from clients.movej_client import MoveJClient, test_joint_list + from clients.arm_motion_client import ArmMotionClient + from clients.movej_client import test_joint_list joint_list = test_joint_list if args.joint_list: - parsed = [] - for item in args.joint_list.split(";"): - if not item: - continue - if ":" not in item: - continue - name, rad_str = item.split(":", 1) - try: - rad = float(rad_str) - except ValueError: - rad = 0.0 - parsed.append({"joint_name": name, "rad": rad}) + parsed = parse_joint_values(args.joint_list, "rad") if parsed: joint_list = parsed - client = MoveJClient(address=address) + client = ArmMotionClient(address=address) try: - client.send(joint_list, vel=args.vel, acc=args.acc, device_id=args.device_id) + client.movej(joint_list, vel=args.vel, acc=args.acc, device_id=args.device_id) print(joint_list) finally: client.close() return 0 + if args.action == "movel": + from clients.arm_motion_client import ArmMotionClient + + client = ArmMotionClient(address=address) + try: + client.movel( + pose={ + "x": args.x, + "y": args.y, + "z": args.z, + "rx": args.rx, + "ry": args.ry, + "rz": args.rz, + }, + vel=args.vel, + acc=args.acc, + blend_radius=args.blend_radius, + frame=args.frame, + device_id=args.device_id, + ) + finally: + client.close() + return 0 + + if args.action == "speedj": + from clients.arm_motion_client import ArmMotionClient + + joint_list = parse_joint_values(args.joint_list, "velocity") + client = ArmMotionClient(address=address) + try: + client.speedj( + joint_list, + acc=args.acc, + duration=args.duration, + device_id=args.device_id, + ) + print(joint_list) + finally: + client.close() + return 0 + + if args.action == "speedl": + from clients.arm_motion_client import ArmMotionClient + + client = ArmMotionClient(address=address) + try: + client.speedl( + velocity={ + "vx": args.vx, + "vy": args.vy, + "vz": args.vz, + "wx": args.wx, + "wy": args.wy, + "wz": args.wz, + }, + acc=args.acc, + duration=args.duration, + frame=args.frame, + device_id=args.device_id, + ) + finally: + client.close() + return 0 + + if args.action == "stop_motion": + from clients.arm_motion_client import ArmMotionClient + + client = ArmMotionClient(address=address) + try: + client.stop_motion(device_id=args.device_id) + finally: + client.close() + return 0 + if args.action == "get_pose": from clients.get_pose_client import GetPoseClient