movefasta/aubo-old-sdk-test
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

poses without special fingers

Browse source
This commit is contained in:
Falloimitator 2025-05-28 18:05:19 +03:00
parent 82c92827ea
commit 2357251393
3 changed files with 99 additions and 39 deletions
Download patch file Download diff file Expand all files Collapse all files

60
main.py
View file

@ -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()

67
node.py
View file

@ -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,59 +88,90 @@ 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)

11
utils.py
View file

@ -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()