diff --git a/utils.py b/utils.py index 7594a7c..fd2276d 100644 --- a/utils.py +++ b/utils.py @@ -143,7 +143,7 @@ class RobotClient: def move_to_pose(self, pose: dict): joint_start, _, _ = self.get_current_waypoint() joint_target = self.inverse_kin(joint_start, pose["pos"], pose["ori"]) - self.move_line(joint_target) + self.move_joint(joint_target) def add_waypoint(self, joint_radian: list): cmd = {"command": "add_waypoint", "joint_radian": joint_radian} @@ -205,6 +205,13 @@ class RobotClient: raise RuntimeError(f"move_line failed: {ret}") self.wait_untill_motion_is_done(10.0) + def move_joint(self, joint_radian: list) -> None: + cmd = {"command": "move_joint", "joint_radian": joint_radian} + ret = self._send_single_json(cmd, 10.0) + if ret.get("ret") != 0: + raise RuntimeError(f"move_joint failed: {ret}") + self.wait_untill_motion_is_done(10.0) + def wait_untill_motion_is_done(self, timeout=10.0): """ Blocks until a callback with addr=9/status=0.0 or code=0/text="RobotMoveControlStopDone." is received,