diff --git a/main.py b/main.py index bc07698..05909dc 100644 --- a/main.py +++ b/main.py @@ -6,28 +6,28 @@ PORT = 9090 config = { "pre_grasp_position": { - 'pos': [0.14726859839859208, 0.5302551637478977, 0.565792932407347], - 'ori': [0.1305294928324298, -0.34660161091966263, 0.8293216816453197, 0.4183956538514503] + 'pos': [0.11468976722774939, 0.49820734702047975, 0.232304304821942], + 'ori': [0.03139065917169165, 0.007114594087226847, 0.7337638657217594, 0.6786417305388762] }, "grasp_position": { - 'pos': [0.15928907070579643, 0.5270829242433969, 0.6510703359003733], - 'ori': [0.15023706706737835, -0.32373703396446113, 0.8482916258752815, 0.39118342754871005] + 'pos': [0.10651987287035074, 0.4918349678759202, 0.12068363327771679], + 'ori': [0.0461769536371005, 0.019808331049769666, 0.7271651527969424, 0.68462117958154] }, "post_grasp_position": { - 'pos': [0.08296640360052251, 0.5270826520836475, 0.4622718470958212], - 'ori': [0.15024595915643826, -0.3237308590654784, 0.8482945564918141, 0.3911787674097941] + 'pos': [0.10584392070929988, 0.4941985576613782, 0.1688168818859202], + 'ori': [0.04549168126833617, 0.01984700639803424, 0.7237928131621344, 0.6882300246917887] }, "pre_insert_position": { - 'pos': [0.08296640360052251, 0.5270826520836475, 0.4622718470958212], - 'ori': [0.15024595915643826, -0.3237308590654784, 0.8482945564918141, 0.3911787674097941] + 'pos': [0.24027684741261485, 0.49126338168373945, 0.39655250532732794], + 'ori': [0.1964966354829754, -0.025599877633873055, 0.6813871431453161, 0.7045887308673521] }, "insert_position": { - "pos": [0.6, 0.0, 0.2], - "ori_offset": [10, -5, 0], + 'pos': [0.27300632591107804, 0.48041141969089585, 0.3049992290184565], + 'ori': [0.17245191209515515, 6.719202049935031e-05, 0.7033203128819105, 0.689638217464526] }, "post_insert_position": { - "pos": [0.6, 0.0, 0.2], - "ori_offset": [10, -5, 0], + 'pos': [0.2904632222294866, 0.4869030095129388, 0.1625443919495984], + 'ori': [0.20018806753589566, 0.023937708375556675, 0.688796013797962, 0.6963560691988031] }, } @@ -37,10 +37,17 @@ def main(): #robot.set_joint_maxvelc(3) #robot.set_end_max_line_velc(0.6) robot.set_end_max_line_acc(0.8) - robot.move_linear_cartesian(config["pre_grasp_position"]) -# robot.move_to_pose(config["pre_grasp_position"]) -# robot.move_to_pose(config["grasp_position"]) -# robot.move_to_pose(config["post_grasp_position"]) + + robot.move_to_pose_joint(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["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.close() diff --git a/node.py b/node.py index e1ea2dc..cbfa356 100644 --- a/node.py +++ b/node.py @@ -2,7 +2,7 @@ import rclpy from rclpy.node import Node -from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Float64MultiArray from sensor_msgs.msg import JointState from utils import RobotClient import time @@ -14,7 +14,7 @@ class RobotControllerNode(Node): super().__init__("robot_controller_node") self.pub = self.create_publisher( - Float32MultiArray, "/forward_position_controller/commands", 10 + Float64MultiArray, "/forward_position_controller/commands", 10 ) self.js_sub = self.create_subscription( JointState, "/joint_states", self.js_callback, 10 @@ -39,7 +39,7 @@ class RobotControllerNode(Node): self.last_js = msg def send_gripper_command(self, target_pose: float, tolerance: float = 0.01, timeout: float = 3.0): - msg = Float32MultiArray() + msg = Float64MultiArray() msg.data = [target_pose] self.pub.publish(msg) self.get_logger().info(f"Sent gripper pose: {target_pose}") @@ -71,6 +71,7 @@ class RobotControllerNode(Node): return False def run_sequence(self): + print(f"STEP #{self.sequence_step}") if self.sequence_step == 0: self.get_logger().info("Moving to pre-grasp") self.robot_client.move_to_pose_joint(config["pre_grasp_position"]) @@ -81,19 +82,40 @@ class RobotControllerNode(Node): self.robot_client.move_to_pose_cart(config["grasp_position"]) self.sequence_step += 1 - elif self.sequence_step == 3: - if self.send_gripper_command(target_pose=1.0, timeout=10.0): + elif self.sequence_step == 2: + if self.send_gripper_command(target_pose=0.582, timeout=10.0, tolerance=0.1): + self.get_logger().info("Gripper closed") + self.sequence_step += 1 + + elif self.sequence_step == 3: + 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: + 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.sequence_step += 1 + + elif self.sequence_step == 7: + if self.send_gripper_command(target_pose=0.54, timeout=10.0): self.get_logger().info("Gripper closed") self.sequence_step += 1 - # elif self.sequence_step == 3: - # 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 >= 3: # Измените на последний шаг + 1 + elif self.sequence_step >= 8: # Измените на последний шаг + 1 self.get_logger().info("Sequence complete") self.robot_client.close() self.destroy_timer(self.timer)