diff --git a/main.py b/main.py index 05909dc..96ee3ef 100644 --- a/main.py +++ b/main.py @@ -5,47 +5,65 @@ HOST = "192.168.3.2" PORT = 9090 config = { + "init": { + 'pos': [0.09810244822368663, 0.3313196269764149, 0.44544069305226175], 'ori': [0.8029422807647959, -0.4820363460296128, 0.034976080376991056, -0.3488571751707414] + }, "pre_grasp_position": { - 'pos': [0.11468976722774939, 0.49820734702047975, 0.232304304821942], - 'ori': [0.03139065917169165, 0.007114594087226847, 0.7337638657217594, 0.6786417305388762] + 'pos': [0.076875580075771, 0.4985114141223675, 0.2578368487288887], 'ori': [0.6837207910111557, -0.7294870127641278, -0.004201289362322976, -0.01889252008713971] }, "grasp_position": { - 'pos': [0.10651987287035074, 0.4918349678759202, 0.12068363327771679], - 'ori': [0.0461769536371005, 0.019808331049769666, 0.7271651527969424, 0.68462117958154] + 'pos': [0.04022607947465449, 0.49870956679172035, 0.12855873351195465], 'ori': [0.7108673879318799, -0.6971760905452062, 0.04979916198276968, -0.07831410481402765] }, "post_grasp_position": { - 'pos': [0.10584392070929988, 0.4941985576613782, 0.1688168818859202], - 'ori': [0.04549168126833617, 0.01984700639803424, 0.7237928131621344, 0.6882300246917887] + 'pos': [0.04138766852425917, 0.49843996951036346, 0.17242170594713332], 'ori': [0.7105716687621472, -0.6972845331909826, 0.050898212328428484, -0.0793193249276688] }, "pre_insert_position": { - 'pos': [0.24027684741261485, 0.49126338168373945, 0.39655250532732794], - 'ori': [0.1964966354829754, -0.025599877633873055, 0.6813871431453161, 0.7045887308673521] + 'pos': [0.15531457163779136, 0.4887644805574635, 0.30996257822689316], 'ori': [0.7096793154895238, -0.6750311908027418, 0.12151729153083345, -0.1610022001985184] }, "insert_position": { - 'pos': [0.27300632591107804, 0.48041141969089585, 0.3049992290184565], - 'ori': [0.17245191209515515, 6.719202049935031e-05, 0.7033203128819105, 0.689638217464526] + 'pos': [0.18819735499444024, 0.4933238342577449, 0.2803404963272801], 'ori': [0.7110943169204221, -0.6510710114660861, 0.06550934464000123, -0.2572157387036094] }, "post_insert_position": { - 'pos': [0.2904632222294866, 0.4869030095129388, 0.1625443919495984], - 'ori': [0.20018806753589566, 0.023937708375556675, 0.688796013797962, 0.6963560691988031] + 'pos': [0.20150600619054684, 0.4986867571185973, 0.24683169991220366], 'ori': [0.6922110693388366, -0.6725304109660972, 0.0757396109325311, -0.2506196184386724] + }, + "move_away_position": { + 'pos': [0.06646318981938315, 0.28008362744706417, 0.3081809630089793], 'ori': [0.9557005454147172, 0.023550142689031436, 0.010835030563343097, -0.29319696517193] + }, + "joint_move_away_position": { + 'pos': [-0.03705798520615846, 0.2948740118379424, 0.1380593922055155], 'ori': [0.7044778798061612, -0.7037920747711072, 0.051086947698884985, -0.07601155259569223] + }, + "final_moveaway": { + 'pos': [0.06232913033349278, 0.28220623060555067, 0.1939445619236099], 'ori': [0.6689183667946061, -0.6855402199272477, 0.10599550652863488, -0.267110048524341] + }, + "joint_speed_test": { + 'pos': [-0.014667855657250158, 0.21178473592030767, 0.6099448870104303], 'ori': [0.8973159098098845, -0.2554666640875886, 0.044440682660961045, -0.35719177939399194] }, } def main(): robot = RobotClient(HOST, PORT) - #robot.set_joint_maxacc(3) - #robot.set_joint_maxvelc(3) - #robot.set_end_max_line_velc(0.6) - robot.set_end_max_line_acc(0.8) + robot.set_joint_maxacc(1) + robot.set_joint_maxvelc(1) + robot.set_end_max_line_velc(1) + robot.set_end_max_line_acc(1) + robot.set_end_max_angle_acc(1) + robot.set_end_max_angle_velc(1) - robot.move_to_pose_joint(config["pre_grasp_position"]) + robot.move_to_pose_joint(config["final_moveaway"]) + robot.move_to_pose_cart(config["pre_grasp_position"]) robot.move_to_pose_cart(config["grasp_position"]) - robot.move_to_pose_joint(config["post_grasp_position"]) - robot.move_to_pose_joint(config["pre_insert_position"]) + robot.move_to_pose_cart(config["post_grasp_position"]) + robot.move_to_pose_cart(config["pre_insert_position"]) robot.move_to_pose_cart(config["insert_position"]) robot.move_to_pose_cart(config["post_insert_position"]) - robot.move_to_pose_cart(config["insert_position"]) - robot.move_to_pose_joint(config["pre_insert_position"]) + robot.move_to_pose_cart(config["final_moveaway"]) + + # robot.move_to_pose_joint(config["final_moveaway"]) + # robot.move_to_pose_joint(config["joint_speed_test"]) + # robot.move_to_pose_joint(config["final_moveaway"]) + # robot.move_to_pose_joint(config["joint_speed_test"]) + # robot.move_to_pose_joint(config["final_moveaway"]) + # robot.move_to_pose_joint(config["joint_speed_test"]) robot.close() diff --git a/node.py b/node.py index e668028..c1b446e 100644 --- a/node.py +++ b/node.py @@ -46,7 +46,7 @@ class RobotControllerNode(Node): start_time = time.time() while rclpy.ok(): - rclpy.spin_once(self, timeout_sec=0.1) + # rclpy.spin_once(self, timeout_sec=0.1) joint_name_list = list(self.last_js.name) if self.last_js.name else [] joint_position_list = ( @@ -88,58 +88,89 @@ class RobotControllerNode(Node): def run_sequence(self): print(f"STEP #{self.sequence_step}") if self.sequence_step == 0: + self.robot_client.set_joint_maxacc(1) + self.robot_client.set_joint_maxvelc(1) + self.robot_client.set_end_max_line_velc(1) + self.robot_client.set_end_max_line_acc(1) self.get_logger().info("Moving to pre-grasp") self.robot_client.move_to_pose_joint(config["pre_grasp_position"]) self.sequence_step += 1 elif self.sequence_step == 1: - self.get_logger().info("Moving to grasp") - self.robot_client.move_to_pose_cart(config["grasp_position"]) - self.sequence_step += 1 - - elif self.sequence_step == 2: if not self.waiting_for_gripper: self.waiting_for_gripper = True threading.Thread( target=self.handle_gripper_step, - args=(0.582, 10.0, 0.1), + args=(0.45, 10.0, 0.1), daemon=True, ).start() self.sequence_step += 1 + elif self.sequence_step == 2: + self.get_logger().info("Moving to grasp") + self.robot_client.move_to_pose_cart(config["grasp_position"]) + self.sequence_step += 1 + elif self.sequence_step == 3: + if not self.waiting_for_gripper: + self.waiting_for_gripper = True + threading.Thread( + target=self.handle_gripper_step, + args=(0.6, 10.0, 0.1), + daemon=True, + ).start() + self.sequence_step += 1 + + elif self.sequence_step == 4: self.get_logger().info("Moving to post-grasp") self.robot_client.move_to_pose_cart(config["post_grasp_position"]) self.sequence_step += 1 - elif self.sequence_step == 4: + elif self.sequence_step == 5: self.get_logger().info("Moving to pre_insert_position") self.robot_client.move_to_pose_cart(config["pre_insert_position"]) self.sequence_step += 1 - elif self.sequence_step == 5: - self.get_logger().info("Moving to insert_position") - self.robot_client.move_to_pose_joint(config["insert_position"]) - self.sequence_step += 1 - elif self.sequence_step == 6: - self.get_logger().info("Moving to post_insert_position") - self.robot_client.move_to_pose_joint(config["post_insert_position"]) + self.get_logger().info("Moving to insert_position") + self.robot_client.move_to_pose_cart(config["insert_position"]) self.sequence_step += 1 elif self.sequence_step == 7: + self.get_logger().info("Moving to post_insert_position") + self.robot_client.move_to_pose_cart(config["post_insert_position"]) + self.sequence_step += 1 + + elif self.sequence_step == 8: + # if not self.waiting_for_gripper: + # self.waiting_for_gripper = True + # threading.Thread( + # target=self.handle_gripper_step, args=(0.56, 10.0), daemon=True + # ).start() + self.sequence_step += 1 + + elif self.sequence_step == 9: + self.get_logger().info("Moving away") if not self.waiting_for_gripper: self.waiting_for_gripper = True threading.Thread( - target=self.handle_gripper_step, args=(0.54, 10.0), daemon=True + target=self.handle_gripper_step, args=(0.56, 10.0), daemon=True ).start() - self.sequence_step += 1 + self.robot_client.move_to_pose_cart(config["final_moveaway"]) + self.sequence_step += 1 - elif self.sequence_step >= 8: + elif self.sequence_step == 10: + self.get_logger().info("Moving to init position") + self.robot_client.move_to_pose_joint(config["init"]) + self.sequence_step += 1 + + elif self.sequence_step >= 11: self.get_logger().info("Sequence complete") self.robot_client.close() self.destroy_timer(self.timer) + + def main(args=None): rclpy.init(args=args) diff --git a/utils.py b/utils.py index a15c916..cad8c0a 100644 --- a/utils.py +++ b/utils.py @@ -249,6 +249,17 @@ class RobotClient: if ret.get("msg") != "succ": raise RuntimeError(f"set_end_max_line_acc failed: {ret}") + def set_end_max_angle_acc(self, acc: float): + cmd = {"command": "set_end_max_angle_acc", "end_maxacc": acc} + ret = self._send_single_json(cmd) + if ret.get("msg") != "succ": + raise RuntimeError(f"set_end_max_line_acc failed: {ret}") + + def set_end_max_angle_velc(self, vel: float): + cmd = {"command": "set_end_max_angle_velc", "end_maxvelc": vel} + ret = self._send_single_json(cmd) + if ret.get("msg") != "succ": + raise RuntimeError(f"set_end_max_line_acc failed: {ret}") def close(self): self.sock.close()