devbox
Guff_9hys
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

Sim2Real两个源代码

作者：Guff_9hys | 2024-07-12 22:29:54

赞

踩

sim2real

1.AXS_baseline.py

  1. # %%
  2. import warnings
  3.  
  4. warnings.filterwarnings("ignore")
  5. from airbot.backend import Arm, Camera, Base, Gripper
  6. import os
  7. import numpy as np
  8. import copy
  9. from airbot.backend.utils.utils import camera2base, armbase2world
  10. from airbot.lm import Detector, Segmentor
  11. from airbot.grasp.graspmodel import GraspPredictor
  12. from PIL import Image
  13. import time
  14. import cv2
  15. from airbot.example.utils.draw import draw_bbox, obb2poly
  16. from airbot.example.utils.vis_depth import vis_image_and_depth
  17. from scipy.spatial.transform import Rotation
  18. from threading import Thread, Lock
  19. import math
  20.  
  21. from airbot.lm.utils import depth2cloud
  22.  
  23. os.environ['LM_CONFIG'] = "/root/Workspace/AXS_baseline/ICRA2024-Sim2Real-AXS/local.yaml"
  24. os.environ['CKPT_DIR'] = '/root/Workspace/AXS_baseline/ckpt'
  25.  
  26.  
  27.  
  28. class Solution:
  29.  
  30.     CAMERA_SHIFT = np.array([-0.093, 0, 0.07]), np.array([0.5, -0.5, 0.5, -0.5])
  31.  
  32.     BEFORE_MW_BASE_POSE = (np.array([
  33.         -0.02596185755302699,
  34.         -0.5065354084074988,
  35.         -0.17365354084074988,
  36.     ]), np.array([
  37.         0.00179501,
  38.         0.06667202,
  39.         0.04863613,
  40.         0.99658725,
  41.     ]))
  42.  
  43.     ARM_POSE_TO_MICROWAVE = (np.array([0.67335, 0.2, -0.07678]), np.array([0.0, 0.0, 0.0, 1.0]))
  44.  
  45.     POSE_CLOSE_MICROWAVE = (np.array([-0.05931371154832413, 0.10621326743872146, 0.052710225767309826]), np.array([0.00476639,  0.08115882, 0.05789078, 0.99500713]))
  46.  
  47.     ARM_POSE_CLOSE_MICROWAVE = (np.array([0.47335, 0.1, 0.05]), np.array([0.0, 0.0, 0.0, 1.0]))
  48.  
  49.     ARM_POSE_CLOSE_MICROWAVE_END = (np.array([0.4, -0.55, 0.05]), np.array([0.0, 0.0, 0.0, 1.0]))
  50.  
  51.     POSE_TO_BOWL = (np.array([-0.01226429675289562, 0.11259263609492792, -0.042619463119529605]),
  52.                     np.array([-0.05418989389554988, 0.056031992518506414, 0.707, 0.707]))
  53.     
  54.     ARM_POSE_TO_LOWER_CABINET = (np.array([0.62335, 0.25, -0.05678]), np.array([0.0, 0.0, 0.0, 1.0]))
  55.  
  56.     ARM_POSE_PUT_LOWER = (np.array([0.62335, 0.25, -0.09678]), np.array([0.0, 0.0, 0.0, 1.0]))
  57.  
  58.     ARM_POSE_TO_UPPER_CABINET = (np.array([0.62335, 0.25, 0.22]), np.array([0.0, 0.0, 0.0, 1.0]))
  59.  
  60.     ARM_POSE_PUT_UPPER = (np.array([0.62335, 0.25, 0.17]), np.array([0.0, 0.0, 0.0, 1.0]))
  61.  
  62.     POSE_OPEN_CAB = (np.array([-0.2741637241544763, 0.3241416117180577, -0.07743623649227918]), np.array([-0.026456952502619244, 0.022510511678367467, 0.6642090190392874, 0.7467393692280592]))
  63.     
  64.     ARM_POSE_STANDARD_MOVING = (np.array([0.3225, 0.00, 0.219]), np.array([0.0, 0.0, 0.0, 1.0]))
  65.  
  66.     GRASP_POSE_1 = (np.array([0.345502073568463, 0.49365995419306763, 0.07947950001408821]), np.array(
  67.         [-0.0051582, 0.09461529, 0.05903872, 0.99374834]))
  68.  
  69.     GRASP_POSE_2 = (np.array([0.40524870062559315, 0.4479303912730242, 0.004671858854359251]), np.array(
  70.         [-0.04836788, 0.0417043, 0.66597635, 0.74323402]))
  71.  
  72.     OBSERVE_ARM_POSE_1 = (np.array([
  73.         0.2565699,
  74.         0.2,
  75.         0.171663168,
  76.     ]), np.array([
  77.         -0.13970062182177911,
  78.         0.6487791800204252,
  79.         0.032918235938941776,
  80.         0.7473190092439113,
  81.     ]))
  82.     
  83.     OBSERVE_ARM_POSE_2 = (np.array([
  84.         0.2565699,
  85.         -0.2,
  86.         0.171663168,
  87.     ]), np.array([
  88.         -0.13970062182177911,
  89.         0.6487791800204252,
  90.         0.032918235938941776,
  91.         0.7473190092439113,
  92.     ]))
  93.     END_POSE = (np.array([-1.2317611908186263, -0.9177336410440479, -0.2374505629662929]), 
  94.                 np.array([0.006592923324957343, -0.012942241749323641, 0.014944697147015459, 0.9997828203003477]))
  95.  
  96.     def __init__(self):
  97.         self.arm = Arm(backend='ros')
  98.  
  99.         self.base = Base(backend='ros')
  100.  
  101.         self.gripper = Gripper(backend='ros')
  102.         self.gripper.open()
  103.  
  104.         self.camera = Camera(backend='ros')
  105.  
  106.         self.detector = Detector(model='grounding-dino')
  107.         # self.detector = Detector(model='yolo-v7')
  108.         self.segmentor = Segmentor(model='segment-anything')
  109.         self.grasper = GraspPredictor(model='graspnet')
  110.  
  111.         self.image_lock = Lock()
  112.         self.result_lock = Lock()
  113.         self.prompt_lock = Lock()
  114.         self.running = True
  115.         self.prompt = 'sky'
  116.         self.update_once()
  117.         self.t_vis = Thread(target=self.vis, daemon=True)
  118.         self.t_vis.start()
  119.         self.t_update = Thread(target=self.update, daemon=True)
  120.         self.t_update.start()
  121.  
  122.     @property
  123.     def image(self):
  124.         with self.image_lock:
  125.             return copy.deepcopy(self._image)
  126.  
  127.     @image.setter
  128.     def image(self, value):
  129.         with self.image_lock:
  130.             self._image = copy.deepcopy(value)
  131.  
  132.     @property
  133.     def prompt(self):
  134.         with self.prompt_lock:
  135.             return copy.deepcopy(self._prompt)
  136.  
  137.     @prompt.setter
  138.     def prompt(self, value):
  139.         with self.prompt_lock:
  140.             self._prompt = copy.deepcopy(value)
  141.  
  142.     @property
  143.     def depth(self):
  144.         with self.image_lock:
  145.             return copy.deepcopy(self._depth)
  146.  
  147.     @depth.setter
  148.     def depth(self, value):
  149.         with self.image_lock:
  150.             self._depth = copy.deepcopy(value)
  151.  
  152.     @property
  153.     def bbox(self):
  154.         with self.result_lock:
  155.             return copy.deepcopy(self._bbox)
  156.  
  157.     @bbox.setter
  158.     def bbox(self, value):
  159.         with self.result_lock:
  160.             self._bbox = copy.deepcopy(value)
  161.  
  162.     @property
  163.     def mask(self):
  164.         with self.result_lock:
  165.             return copy.deepcopy(self._mask)
  166.  
  167.     @mask.setter
  168.     def mask(self, value):
  169.         with self.result_lock:
  170.             self._mask = copy.deepcopy(value)
  171.  
  172.     def update_once(self):
  173.         with self.image_lock, self.result_lock:
  174.             self._image = copy.deepcopy(self.camera.get_rgb())
  175.             self._depth = copy.deepcopy(self.camera.get_depth())
  176.             self._det_result = self.detector.infer(self._image, self._prompt)
  177.             self._bbox = self._det_result['bbox'].numpy().astype(int)
  178.             self._sam_result = self.segmentor.infer(self._image, self._bbox[None, :2][:, [1, 0]])
  179.             self._mask = self._sam_result['mask']
  180.  
  181.     def update(self):
  182.         while self.running:
  183.             self.update_once()
  184.             time.sleep(0.005)
  185.  
  186.     def vis(self):
  187.         try:
  188.             # Infinite loop to display images
  189.             while self.running:
  190.                 image_draw = self.image
  191.                 image_draw = image_draw * (self.mask[:, :, None].astype(np.uint8) * 0.75 + 0.25)
  192.                 image_draw = draw_bbox(image_draw, obb2poly(self.bbox[None, ...]).astype(int))
  193.                 image_draw = image_draw.astype(np.uint8)
  194.                 image_show = cv2.cvtColor(image_draw, cv2.COLOR_RGB2BGR)
  195.                 cv2.imshow('RGB', image_show)
  196.                 _flag = np.any(self.mask)
  197.                 if _flag == True:
  198.                     cv2.putText(image_show,
  199.                                 f"det score: {self._det_result['score']}, sam score: {self._sam_result['score']}",
  200.                                 (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
  201.                 cv2.imshow('RGB', image_show)
  202.                 if cv2.waitKey(1) & 0xFF == ord('q'):
  203.                     break
  204.  
  205.         except KeyboardInterrupt:
  206.             print("Exiting due to user interruption.")
  207.         finally:
  208.             cv2.destroyAllWindows()
  209.  
  210.     @staticmethod
  211.     def _bbox2mask(image, bbox):
  212.         mask = np.zeros_like(image[:, :, 0], dtype=bool)
  213.         mask[
  214.             bbox[0] - bbox[2] // 2:bbox[0] + bbox[2] // 2,
  215.             bbox[1] - bbox[3] // 2:bbox[1] + bbox[3] // 2,
  216.         ] = True
  217.         return mask
  218.  
  219.     @staticmethod
  220.     def base_cloud(image, depth, intrinsic, shift, end_pose):
  221.         cam_cloud = depth2cloud(depth, intrinsic)
  222.         cam_cloud = np.copy(np.concatenate((cam_cloud, image), axis=2))
  223.         return camera2base(cam_cloud, shift, end_pose)
  224.  
  225.     def grasp(self):
  226.         # find the center of self.mask
  227.         # center_cam = np.array(np.nonzeros(self.mask)).mean(axis=1)
  228.         # center_frame = self.base_cloud[center_cam]
  229.  
  230.         # self.base.move_to(*self.OPERATE_BASE_POSE_1)
  231.         # self.arm.move_end_to_pose(*self.OPERATE_ARM_POSE)
  232.         # time.sleep(3)
  233.         # method = input('method')
  234.         method = "2"
  235.         with self.image_lock, self.result_lock:
  236.             _depth = copy.deepcopy(self._depth)
  237.             _image = copy.deepcopy(self._image)
  238.             _bbox = copy.deepcopy(self._bbox)
  239.             _mask = copy.deepcopy(self._mask)
  240.  
  241.         cloud = self.base_cloud(_image, _depth, self.camera.INTRINSIC, self.CAMERA_SHIFT, self.arm.end_pose)
  242.  
  243.         if method == "1":
  244.             direction = cloud[_bbox[0] - _bbox[2] // 2, _bbox[1]][:3] - self.arm.end_pose[0]
  245.             direction = direction / np.linalg.norm(direction)
  246.             grasp_position = cloud[_bbox[0] - _bbox[2] // 2 + 9, _bbox[1]][:3] - 0.12 * direction
  247.             grasp_rotation = Rotation.from_euler('xyz', [0, np.pi / 2, np.pi / 2], degrees=False).as_quat()
  248.         elif method == "2":
  249.             grasp_position = cloud[ _bbox[0], _bbox[1] - _bbox[3] // 2 + 8][:3]
  250.             grasp_position[2] = -0.165
  251.             grasp_rotation = Rotation.from_euler('xyz', [0, np.pi / 2, 0], degrees=False).as_quat()
  252.         else:
  253.             bbox_mask = self._bbox2mask(_image, _bbox)
  254.             (grasp_position, grasp_rotation), _ = self.grasper.infer(cloud, bbox_mask)
  255.  
  256.             grasp_rotation = Rotation.from_euler("yz", [np.pi / 2, np.pi / 2], degrees=False).as_quat()
  257.  
  258.         self.arm.move_end_to_pose(grasp_position, grasp_rotation)
  259.         time.sleep(2)
  260.         self.gripper.close()
  261.         time.sleep(4)
  262.         self.arm.move_end_to_pose(*self.ARM_POSE_STANDARD_MOVING)
  263.  
  264.     @staticmethod
  265.     def _vis_grasp(cloud, position, rotation):
  266.         import open3d as o3d
  267.         from graspnetAPI.grasp import GraspGroup
  268.         o3d_cloud = o3d.geometry.PointCloud()
  269.         cloud = copy.deepcopy(cloud)
  270.         o3d_cloud.points = o3d.utility.Vector3dVector(cloud[:, :, :3].reshape(-1, 3).astype(np.float32))
  271.         o3d_cloud.colors = o3d.utility.Vector3dVector(cloud[:, :, 3:].reshape(-1, 3).astype(np.float32) / 255.)
  272.         gg = GraspGroup(
  273.             np.array([1., 0.06, 0.01, 0.06, *Rotation.from_quat(rotation).as_matrix().flatten(), *position,
  274.                       0]).reshape(1, -1))
  275.         coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5, origin=[0, 0, 0])
  276.         o3d.visualization.draw_geometries([o3d_cloud, *gg.to_open3d_geometry_list(), coordinate_frame])
  277.  
  278.     def place_microwave(self):
  279.         self.base.move_to(*self.BEFORE_MW_BASE_POSE, 'world', False)
  280.         self.arm.move_end_to_pose(*self.ARM_POSE_TO_MICROWAVE)
  281.         input_pose = (np.array([0.25, 0.0, 0.0]), np.array([0.0, 0.0, 0.0, 1.0]))  
  282.         self.base.move_to(*input_pose, 'robot', True)
  283.         self.gripper.open()
  284.         time.sleep(2)
  285.         output_pose = (np.array([-0.3, 0.0, 0.0]), np.array([0.0, 0.0, 0.0, 1.0]))  
  286.         self.base.move_to(*output_pose, 'robot', True)
  287.  
  288.     def close_microwave(self):
  289.         self.arm.move_end_to_pose(*self.ARM_POSE_STANDARD_MOVING)
  290.         self.base.move_to(*self.POSE_CLOSE_MICROWAVE, 'world', False)
  291.         self.arm.move_end_to_pose(*self.ARM_POSE_CLOSE_MICROWAVE)
  292.         self.arm.move_end_to_pose(*self.ARM_POSE_CLOSE_MICROWAVE_END)
  293.         output_pose1 = (np.array([-0.25, 0.0, 0.0]), np.array([0.0, 0.0, 0.0, 1.0]))  
  294.         self.base.move_to(*output_pose1, 'robot', True)
  295.         self.arm.move_end_to_pose(*self.ARM_POSE_STANDARD_MOVING)
  296.  
  297.  
  298.     def place_bowl_lower(self):
  299.         self.arm.move_end_to_pose(*self.ARM_POSE_STANDARD_MOVING)
  300.         self.base.move_to(*self.POSE_TO_BOWL, 'world', False)
  301.         self.arm.move_end_to_pose(*self.ARM_POSE_TO_LOWER_CABINET)
  302.         input_pose = (np.array([0.35, 0.0, 0.0]), np.array([0.0, 0.0, 0.0, 1.0]))
  303.         self.base.move_to(*input_pose, 'robot', True)
  304.         self.gripper.open()
  305.         self.arm.move_end_to_pose(*self.ARM_POSE_PUT_LOWER)
  306.         time.sleep(2)
  307.         output_pose = (np.array([- 0.35, 0.0, 0.0]), np.array([0.0, 0.0, 0.0, 1.0]))
  308.         self.base.move_to(*output_pose, 'robot', True)
  309.         self.arm.move_end_to_pose(*self.ARM_POSE_STANDARD_MOVING)
  310.  
  311.     def place_bowl_upper(self):
  312.         self.arm.move_end_to_pose(*self.ARM_POSE_STANDARD_MOVING)
  313.         self.base.move_to(*self.POSE_TO_BOWL,'world', False)
  314.         self.arm.move_end_to_pose(*self.ARM_POSE_TO_UPPER_CABINET)
  315.         input_pose = (np.array([0.35, 0.0, 0.0]), np.array([0.0, 0.0, 0.0, 1.0]))
  316.         self.base.move_to(*input_pose, 'robot', True)
  317.         self.gripper.open()
  318.         self.arm.move_end_to_pose(*self.ARM_POSE_PUT_UPPER)
  319.         time.sleep(2)
  320.         output_pose = (np.array([-0.35, 0.0, 0.0]), np.array([0.0, 0.0, 0.0, 1.0]))
  321.         self.base.move_to(*output_pose, 'robot', True)
  322.         self.arm.move_end_to_pose(*self.ARM_POSE_STANDARD_MOVING)
  323.  
  324.     def lookforonce(self, det_th, sam_th):
  325.         with self.image_lock, self.result_lock:
  326.             _rgb = copy.deepcopy(self.camera.get_rgb())
  327.             _depth = copy.deepcopy(self.camera.get_depth())
  328.             _det_result = copy.deepcopy(self._det_result)
  329.             _sam_result = copy.deepcopy(self._sam_result)
  330.         _bbox = _det_result['bbox'].numpy().astype(int)
  331.         _mask = _sam_result['mask']
  332.         if np.any(_mask) is False:
  333.             print(f"direction {direction} not Found")
  334.             
  335.         print("det score:", _det_result['score'])
  336.         print("sam score:", _sam_result['score'])
  337.         if _det_result['score'] > det_th and _sam_result['score'] > sam_th:
  338.             print(f"Found the {self._prompt}")
  339.             centerpoint = depth2cloud(_depth, self.camera.INTRINSIC, organized=True)[_bbox[0] // 1, _bbox[1] // 1]
  340.             centerpoint = camera2base(centerpoint, self.CAMERA_SHIFT, self.arm.end_pose)
  341.             centerpoint = (armbase2world(centerpoint, (self.base.position, self.base.rotation)).squeeze())
  342.             object_rgb = _rgb[_bbox[0] - np.int32(_bbox[2]/4):_bbox[0] + np.int32(_bbox[2]/4), _bbox[1] - np.int32(_bbox[3]/4):_bbox[1] + np.int32(_bbox[3]/4)]
  343.             mean_rgb = (np.mean(np.mean(object_rgb, axis=0), axis=0).astype(int))
  344.             print('-' * 50)
  345.             print('centerpoint is', centerpoint)
  346.             print('object rgb is', mean_rgb)
  347.             print('-' * 50)
  348.  
  349.             return centerpoint, mean_rgb
  350.  
  351. if __name__ == '__main__':
  352.     s = Solution()
  353.  
  354.     s.base.move_to(*s.POSE_OPEN_CAB, 'world', False)
  355.     time.sleep(1)
  356.     POS_DOOR_HANDLE = np.array([0.30353946626186371, 1.230472918510437, 0])
  357.     centerp_car = np.linalg.inv(np.array(Rotation.from_quat(s.base.rotation).as_matrix())).dot((POS_DOOR_HANDLE-s.base.position))
  358.     ARM_POSE_DOOR_HANDLE = (np.array([
  359.                     centerp_car[0] - 0.2975 - 0.01,
  360.                     centerp_car[1] + 0.17309 - 0.01,
  361.                     0.2,
  362.                 ]), np.array([
  363.                     0.0,
  364.                     0.0,
  365.                     0.0,
  366.                     1.0,
  367.                 ]))
  368.     s.arm.move_end_to_pose(*ARM_POSE_DOOR_HANDLE)
  369.     s.gripper.close()
  370.     time.sleep(5)
  371.     
  372.     for i in range(7):
  373.         d = 0.1 * (i + 1)
  374.         new_ori = np.array([0, 0, 0, 1])
  375.         new_pos = ARM_POSE_DOOR_HANDLE[0] + np.array([-0.4*math.sin(d), 0.4-0.4*math.cos(d), 0])
  376.         r = Rotation.from_euler("xyz", np.array([0, 0, -d]), degrees=False)
  377.         new_ori = r.as_quat()
  378.         s.arm.move_end_to_pose(new_pos, np.array(new_ori))
  379.         time.sleep(0.5)
  380.     
  381.     s.gripper.open()
  382.     time.sleep(3)
  383.     s.arm.move_end_to_pose(np.array([0.3225, 0.00, 0.219]), np.array([0.0, 0.0, 0.0, 1.0]))
  384.     s.arm.move_end_to_pose(np.array([0.3225, -0.25, 0.219]), np.array([0.0, 0.0, 0.0, 1.0]))
  385.     s.arm.move_end_to_pose(np.array([0.5615004168820418, -0.2, 0.35123932220414126]), np.array([0.0, 0.0, 0.2953746452532359, 0.9547541169761965]))
  386.     s.arm.move_end_to_pose(np.array([0.6015004168820418, -0.15, 0.35123932220414126]), np.array([0.0, 0.0, 0.2953746452532359, 0.9547541169761965]))
  387.  
  388.     s.arm.move_end_to_pose(np.array([0.4882092425581316, 0.2917225555849343, 0.3515424067641672]), np.array([0.0, 0.0, 0.6045684271573144, 0.7957869908463996]))
  389.     back_pose = (np.array([-0.05, 0.0, 0.0]), np.array([0.0, 0.0, 0.0, 1.0]))  
  390.     s.base.move_to(*back_pose, 'robot', True)
  391.     s.arm.move_end_to_pose(*s.ARM_POSE_STANDARD_MOVING)
  392.  
  393.     s._prompt = 'white mug'
  394.     cp = None
  395.     s.base.move_to(*s.GRASP_POSE_1, 'world', False)
  396.     look_num = 0
  397.     while cp is None:
  398.         for direction in [1, 2]:
  399.             if direction == 1:
  400.                 s.arm.move_end_to_pose(*s.OBSERVE_ARM_POSE_1)
  401.             else:
  402.                 s.arm.move_end_to_pose(*s.OBSERVE_ARM_POSE_2)
  403.             cp = s.lookforonce(0.65,0.65)
  404.             if cp is not None:
  405.                 break
  406.         look_num += 1
  407.         if look_num>3:
  408.             break
  409.     if cp is not None:
  410.         centerpoint, object_mean_rgb = cp    
  411.         centerp_car = np.linalg.inv(np.array(Rotation.from_quat(s.base.rotation).as_matrix())).dot((centerpoint-s.base.position))
  412.         OBSERVE_ARM_POSE_TOP = (np.array([
  413.                     centerp_car[0]- 0.2975 - 0.05,
  414.                     centerp_car[1] + 0.17309,
  415.                     0.018713334665877806,
  416.                 ]), np.array([
  417.                     -0.13970062182177911,
  418.                     0.6487791800204252,
  419.                     0.032918235938941776,
  420.                     0.7473190092439113,
  421.                 ]))
  422.         s.arm.move_end_to_pose(*OBSERVE_ARM_POSE_TOP)
  423.         time.sleep(1)
  424.         s.grasp()
  425.         s.place_microwave()
  426.     s.close_microwave()
  427.  
  428.  
  429.     obj_rgb = []
  430.     for j in range(5):
  431.             s._prompt = 'bowl'
  432.             cp = None
  433.             s.base.move_to(*s.GRASP_POSE_1, 'world', False)
  434.             look_num = []
  435.             while cp is None:
  436.                 for direction in [1, 2]:
  437.                     if direction == 1:
  438.                         s.arm.move_end_to_pose(*s.OBSERVE_ARM_POSE_1)
  439.                     else:
  440.                         s.arm.move_end_to_pose(*s.OBSERVE_ARM_POSE_2)
  441.                     cp = s.lookforonce(0.6, 0.6)
  442.                     if cp is not None:
  443.                         break
  444.                 look_num.append(1)
  445.                 if len(look_num)>2:
  446.                     break
  447.             if len(look_num)>2:
  448.                 break
  449.             centerpoint, object_mean_rgb = cp    
  450.             centerp_car = np.linalg.inv(np.array(Rotation.from_quat(s.base.rotation).as_matrix())).dot((centerpoint-s.base.position))
  451.             OBSERVE_ARM_POSE_TOP = (np.array([
  452.                         centerp_car[0]- 0.2975 - 0.05,
  453.                         centerp_car[1] + 0.17309,
  454.                         0.018713334665877806,
  455.                     ]), np.array([
  456.                         -0.13970062182177911,
  457.                         0.6487791800204252,
  458.                         0.032918235938941776,
  459.                         0.7473190092439113,
  460.                     ]))
  461.             s.arm.move_end_to_pose(*OBSERVE_ARM_POSE_TOP)
  462.             time.sleep(1)
  463.             s.grasp()
  464.             obj_rgb.append(object_mean_rgb)
  465.             if j != 0:
  466.                 print("color", abs(sum(obj_rgb[j]-obj_rgb[0])))
  467.             if j == 0:
  468.                 s.place_bowl_lower()
  469.             elif abs(sum(obj_rgb[j]-obj_rgb[0]))>30:
  470.                 s.place_bowl_upper()
  471.             else:
  472.                 s.place_bowl_lower()
  473.     obj_rgb_2 = []
  474.     for j in range(5):
  475.         s._prompt = 'bowl'
  476.         cp = None
  477.         s.base.move_to(*s.GRASP_POSE_2, 'world', False)
  478.         look_num = []
  479.         while cp is None:
  480.             for direction in [1, 2]:
  481.                 if direction == 1:
  482.                     s.arm.move_end_to_pose(*s.OBSERVE_ARM_POSE_1)
  483.                 else:
  484.                     s.arm.move_end_to_pose(*s.OBSERVE_ARM_POSE_2)
  485.                 cp = s.lookforonce(0.6, 0.6)
  486.                 if cp is not None:
  487.                     break
  488.                 look_num.append(1)
  489.             if len(look_num)>2:
  490.                 break
  491.         if len(look_num)>2:
  492.             break
  493.         centerpoint, object_mean_rgb = cp    
  494.         centerp_car = np.linalg.inv(np.array(Rotation.from_quat(s.base.rotation).as_matrix())).dot((centerpoint-s.base.position))
  495.         OBSERVE_ARM_POSE_TOP = (np.array([
  496.                     centerp_car[0]- 0.2975 - 0.05,
  497.                     centerp_car[1] + 0.17309,
  498.                     0.018713334665877806,
  499.                 ]), np.array([
  500.                     -0.13970062182177911,
  501.                     0.6487791800204252,
  502.                     0.032918235938941776,
  503.                     0.7473190092439113,
  504.                 ]))
  505.         s.arm.move_end_to_pose(*OBSERVE_ARM_POSE_TOP)
  506.         time.sleep(1)
  507.         s.grasp()
  508.         obj_rgb_2.append(object_mean_rgb)
  509.         if len(obj_rgb) != 0:
  510.             if abs(sum(obj_rgb_2[j]-obj_rgb[0]))>30:
  511.                 s.place_bowl_upper()
  512.             else:
  513.                 s.place_bowl_lower()
  514.         else:
  515.             if j == 0:
  516.                 s.place_bowl_lower()
  517.             elif abs(sum(obj_rgb_2[j]-obj_rgb_2[0]))>30:
  518.                 s.place_bowl_upper()
  519.             else:
  520.                 s.place_bowl_lower()
  521.     print("finish the task")
  522.     s.base.move_to(*s.END_POSE, 'world', False) 
  523.     time.sleep(20)

2.ros_base_control.py

  1. from . import transformations, CoordinateTools
  2.  
  3. import numpy as np
  4. import time
  5. from typing import Union
  6. from threading import Thread
  7.  
  8.  
  9. class BaseControlTools(object):
  10.     """机器人底盘运动控制工具类：
  11.     pose(tuple)：
  12.         0:position：（x,y,z）
  13.         1:rotation：（r,p,y）（欧拉角，按xyz顺序，r:[-pi,pi],p:[-pi/2,pi/2],y:[-pi,pi]）(函数参数设置也可使用四元数，按xyzw顺序)
  14.     velocity(tuple)：
  15.         0:linear：（x,y,z）
  16.         1:angular：（r,p,y）（与rotation对应）
  17.     """
  18.  
  19.     coor = CoordinateTools()
  20.     _TEST_ = False
  21.  
  22.     @staticmethod
  23.     def target_pose_to_velocity(
  24.         posi_or_rota: Union[float, np.ndarray], kp, limits: tuple, dead_zone: float
  25.     ) -> Union[float, np.ndarray]:
  26.         """根据目标在机器人坐标系下的位置或姿态和给定参数计算机器人的线速度或角速度"""
  27.         if limits[0] > limits[1]:
  28.             limits = (limits[1], limits[0])
  29.         if dead_zone > limits[0]:
  30.             dead_zone = limits[0]
  31.         if isinstance(posi_or_rota, np.ndarray):
  32.             raw_velocity_norm = np.linalg.norm(posi_or_rota)
  33.         else:
  34.             raw_velocity_norm = abs(posi_or_rota)
  35.         target_linear_velocity_norm = kp * raw_velocity_norm
  36.         if target_linear_velocity_norm == 0:
  37.             target_velocity = posi_or_rota
  38.         elif target_linear_velocity_norm > limits[1]:
  39.             target_velocity = limits[1] / raw_velocity_norm * posi_or_rota
  40.         elif target_linear_velocity_norm < dead_zone:
  41.             target_velocity *= 0.0
  42.         elif target_linear_velocity_norm < limits[0]:
  43.             target_velocity = limits[0] / raw_velocity_norm * posi_or_rota
  44.         else:
  45.             target_velocity = (
  46.                 target_linear_velocity_norm / raw_velocity_norm * posi_or_rota
  47.             )
  48.         return target_velocity
  49.  
  50.     @staticmethod
  51.     def min_rotation_move(distance: float, direcition: float) -> tuple:
  52.         """将机器人从当前方向轴按最小方向转向目标方向轴，使得机器人的方向轴（如x轴）与目标方向共线（同向或反向）。
  53.         输入：
  54.             distance: 机器人与目标点的距离 >= 0
  55.             direction: 机器人与目标点的方向 [-pi, pi]
  56.         输出：
  57.             distance: 机器人与目标点的距离，若转向后机器人方向轴与目标方向同向则为正，反向则为负
  58.             direction: 机器人与目标点的方向 [-pi/2, pi/2]
  59.         """
  60.         if direcition < -np.pi / 2:
  61.             direcition += np.pi
  62.             distance *= -1
  63.         elif direcition > np.pi / 2:
  64.             direcition -= np.pi
  65.             distance *= -1
  66.         return distance, direcition
  67.  
  68.     @classmethod
  69.     def composit_velocity(
  70.         cls, target_pose, current_pose, kp, limits, dead_zone
  71.     ) -> tuple:
  72.         """根据目标位姿和当前位姿及给定参数计算机器人的线速度和角速度"""
  73.         raw_linear_velocity, raw_angular_velocity = cls.coor.to_robot_coordinate(
  74.             target_pose, current_pose
  75.         )
  76.         target_linear_velocity = cls.target_pose_to_velocity(
  77.             raw_linear_velocity, kp[0], limits[0], dead_zone[0]
  78.         )
  79.         target_angular_velocity = cls.target_pose_to_velocity(
  80.             raw_angular_velocity, kp[1], limits[1], dead_zone[1]
  81.         )
  82.  
  83.         return target_linear_velocity, target_angular_velocity
  84.  
  85.     @classmethod
  86.     def three_stage_control(
  87.         self, target_pose, current_pose, tolerance, kp, limits, dead_zone
  88.     ) -> tuple:
  89.         """三阶段底盘位置-速度控制法(先旋转调整方向，再平移调整位置，然后再选择调整姿态；根据误差反馈动态调整)"""
  90.         target_linear_velocity = np.zeros(3, dtype=np.float64)
  91.         target_angular_velocity = np.zeros(3, dtype=np.float64)
  92.         stage, bias = self.get_stage_and_bias(
  93.             target_pose, current_pose, tolerance[:2], tolerance[2], improve=False
  94.         )
  95.         if stage in [1, 3]:
  96.             target_angular_velocity[2] = self.target_pose_to_velocity(
  97.                 bias, kp[1], limits[1], dead_zone[1]
  98.             )
  99.         elif stage == 2:
  100.             target_linear_velocity[0] = self.target_pose_to_velocity(
  101.                 bias, kp[0], limits[0], dead_zone[0]
  102.             )
  103.         return target_linear_velocity, target_angular_velocity
  104.  
  105.     @classmethod
  106.     def get_stage_and_bias(
  107.         cls,
  108.         target_pose: tuple,
  109.         current_pose: tuple,
  110.         pose_tolerance: tuple,
  111.         direction_tolerance: Union[float, tuple, None] = None,
  112.         improve=False,
  113.         last_stage=None,
  114.         new_target=1,
  115.         avoid_321=False,
  116.         same_ignore=False,
  117.         avoid_swing=False,
  118.     ) -> tuple:
  119.         """获得三阶段控制法的当前阶段及其相关偏差量"""
  120.         position_error, rotation_error = cls.coor.get_pose_error_in_axis(
  121.             target_pose, current_pose
  122.         )  # rotation_error范围为[-pi, pi]
  123.         position_distance, rotation_distance = cls.coor.norm(
  124.             position_error
  125.         ), cls.coor.norm(
  126.             rotation_error
  127.         )  # 计算位置以及姿态（优弧范围内）的距离
  128.         same_target = same_ignore and new_target == 0
  129.         if cls._TEST_:
  130.             print("position_error:", position_error)
  131.             print("rotation_error:", rotation_error)
  132.             print("position_distance:", position_distance)
  133.             print("rotation_distance:", rotation_distance)
  134.             print("new_target:", new_target)
  135.             print("same_target:", same_target)
  136.         if (
  137.             position_distance <= pose_tolerance[0]
  138.             or (avoid_321 and last_stage == 3 and new_target == 0)
  139.             or (last_stage == -1 and same_target)
  140.         ):
  141.             if rotation_distance <= pose_tolerance[1]:
  142.                 return -1, 0  # end and stop
  143.             else:
  144.                 return 3, rotation_error[2]
  145.         else:
  146.             pose_in_robot = cls.coor.to_robot_coordinate(target_pose, current_pose)
  147.             position_in_robot = pose_in_robot[0]
  148.             direction_error = cls.coor.get_spherical(position_in_robot)[2]
  149.             # 劣弧处理成优弧（position_distance这时带有正负了）
  150.             position_distance, direction_error = cls.min_rotation_move(
  151.                 position_distance, direction_error
  152.             )
  153.             if cls._TEST_:
  154.                 print("direction_error:", direction_error)
  155.             direction_error_abs = abs(direction_error)
  156.             if improve:  # 使用改进三阶段控制法
  157.                 if (
  158.                     ((last_stage in [None, 1, -1]) or not same_target)
  159.                     and direction_error_abs > direction_tolerance[0]
  160.                 ) or direction_error_abs > direction_tolerance[1]:
  161.                     return 1, direction_error
  162.                 elif direction_error_abs <= 0:  # 完全对齐后才开始绝对直线
  163.                     return 2, position_distance
  164.                 elif direction_error_abs <= direction_tolerance[1]:
  165.                     if avoid_swing:
  166.                         direction_error = 0  # 不再修正方向，避免来回摆动
  167.                     if position_distance < 0:
  168.                         direction_error *= -1  # 倒车修正
  169.                     return 1.5, (position_distance, direction_error)  # 1.5的bias是个tuple
  170.             else:
  171.                 if (
  172.                     direction_tolerance is None
  173.                     and direction_error_abs <= pose_tolerance[2]
  174.                 ) or (direction_error_abs <= direction_tolerance):
  175.                     return 2, position_distance
  176.                 else:
  177.                     return 1, direction_error
  178.  
  179.     @classmethod
  180.     def three_stage_control_improved(
  181.         self,
  182.         target_pose,
  183.         current_pose,
  184.         pose_tolerance,
  185.         direction_tolerance,
  186.         kp,
  187.         limits,
  188.         dead_zone,
  189.         enhance=1,
  190.         last_stage=None,
  191.         new_target=1,
  192.         avoid_321=False,
  193.         same_ignore=False,
  194.         avoid_swing=False,
  195.     ) -> tuple:
  196.         """改进三阶段底盘位置-速度控制法(增加单轴移动+旋转叠加阶段)"""
  197.         target_linear_velocity = np.zeros(3, dtype=np.float64)
  198.         target_angular_velocity = np.zeros(3, dtype=np.float64)
  199.         stage, bias = self.get_stage_and_bias(
  200.             target_pose,
  201.             current_pose,
  202.             pose_tolerance,
  203.             direction_tolerance,
  204.             improve=True,
  205.             last_stage=last_stage,
  206.             new_target=new_target,
  207.             avoid_321=avoid_321,
  208.             same_ignore=same_ignore,
  209.             avoid_swing=avoid_swing,
  210.         )
  211.         if self._TEST_:
  212.             print("stage:", stage)
  213.         if stage in [1, 1.5, 3]:
  214.             if stage == 1.5:
  215.                 bias_r = bias[1]
  216.             else:
  217.                 bias_r = bias
  218.             target_angular_velocity[2] = (
  219.                 self.target_pose_to_velocity(bias_r, kp[1], limits[1], dead_zone[1])
  220.                 / enhance
  221.             )
  222.         if stage in [1.5, 2]:
  223.             if stage == 1.5:
  224.                 bias_t = bias[0]
  225.             else:
  226.                 bias_t = bias
  227.             target_linear_velocity[0] = (
  228.                 self.target_pose_to_velocity(bias_t, kp[0], limits[0], dead_zone[0])
  229.                 * enhance
  230.             )
  231.         return (target_linear_velocity, target_angular_velocity), stage
  232.  
  233.     @staticmethod
  234.     def four_steering_wheel_ik(
  235.         linear_vx, linear_vy, angular_vz, width, lenth, radius=1, reduction=1
  236.     ) -> tuple:
  237.         """四舵轮底盘的逆运动学（给定目标速度和相关参数，计算四轮转向和轮速）"""
  238.  
  239.         r = np.hypot(lenth, width)
  240.  
  241.         # 各轮的合成分速度
  242.         A = linear_vy - angular_vz * lenth / r  # 右轮的y方向分速度
  243.         B = linear_vy + angular_vz * lenth / r  # 左轮的y方向分速度
  244.         C = linear_vx - angular_vz * width / r  # 前轮的x方向分速度
  245.         D = linear_vx + angular_vz * width / r  # 后轮的x方向分速度
  246.  
  247.         # 各轮合成速度大小
  248.         ws1 = np.sqrt(B**2 + C**2)
  249.         ws2 = np.sqrt(B**2 + D**2)
  250.         ws3 = np.sqrt(A**2 + D**2)
  251.         ws4 = np.sqrt(A**2 + C**2)
  252.  
  253.         # 各轮合成速度方向
  254.         wa1 = np.arctan2(B, C)
  255.         wa2 = np.arctan2(B, D)
  256.         wa3 = np.arctan2(A, D)
  257.         wa4 = np.arctan2(A, C)
  258.  
  259.         # 车轮自身转速缩放因子（减速比/轮半径）
  260.         rotating_factor = reduction / radius
  261.  
  262.         # 各轮转速大小（rotating_factor为1时为线速度）
  263.         speeds = np.array([ws1, ws2, ws3, ws4], dtype=np.float64) * rotating_factor
  264.         # 各轮旋转角度
  265.         angles = np.array([wa1, wa2, wa3, wa4], dtype=np.float64)
  266.  
  267.         return speeds, angles
  268.  
  269.  
  270. class BaseControl(object):
  271.     """机器人底盘运动控制顶层类"""
  272.  
  273.     _TEST_ = False
  274.  
  275.     def __init__(self) -> None:
  276.         self._move_kp = (100, 100)
  277.         self._velocity_dead_zone = (0.0, 0.0)
  278.         self._linear_limits = (0.2, 0.3)
  279.         self._angular_limits = (0.17, 0.18)
  280.         self._direction_tolerance = 0.2
  281.         self._direction_tolerance_improved = (0.1, 0.3)
  282.         self._wait_tolerance = (0.1, 0.1)
  283.         self._improve_enhance = 1
  284.         self._last_stage = None
  285.         self._wait_timeout = 5
  286.         self._wait_period = 0.01
  287.         self._move_stop = True
  288.         self._move_method = "three_stage_improved"
  289.         self._current_position = self._current_rotation = None
  290.         BaseControlTools._TEST_ = self._TEST_
  291.         self._tools = BaseControlTools()
  292.         self._muilti_avoid = {"set": False, "get": False}
  293.         self._new_target = 1
  294.         self._avoid_321 = False
  295.         self._same_ignore = False
  296.         self._position_target = np.zeros(3, dtype=np.float64)
  297.         self._rotation_target = np.zeros(3, dtype=np.float64)
  298.         self._last_orientation_cmd = np.zeros(4, dtype=np.float64)
  299.         self._last_pose_ref = "world"
  300.  
  301.     def move_to(self, position: np.ndarray, rotation: np.ndarray) -> bool:
  302.         """设置并移动机器人到指定的目标位姿"""
  303.         self.set_target_pose(position, rotation)
  304.         result = self.move()
  305.         return result
  306.  
  307.     def shift_pose(self, axis: int, target: float) -> bool:
  308.         """在机器人当前位姿的基础上，沿指定轴移动指定距离"""
  309.         position, rotation = self.get_current_world_pose()
  310.         if axis < 3:
  311.             position[axis] += target
  312.         else:
  313.             rotation[axis-3] += target
  314.         result = self.move_to(position, rotation)
  315.         return result
  316.  
  317.     def set_target_pose(
  318.         self, position: np.ndarray, rotation: np.ndarray, ref: str = "world"
  319.     ) -> None:
  320.         """
  321.         设置机器人在世界（ref=world）/自身（ref=robot）当前坐标系下的目标位姿；
  322.         默认为世界坐标系下的目标位姿；
  323.         """
  324.         len_rotation = len(rotation)
  325.         # 目标相同是否忽略
  326.         if self._avoid_321:
  327.             if self._last_pose_ref == ref and (position == self._position_target).all():
  328.                 if len_rotation == 4:
  329.                     if (rotation == self._last_orientation_cmd).all():
  330.                         return
  331.                 elif (rotation == self._rotation_target).all():
  332.                     return
  333.             else:
  334.                 self._new_target += 1
  335.         # 重复设置目标位姿时的警告
  336.         if self._muilti_avoid["set"]:
  337.             print("Warning: set_target_pose is called before the last is finished!")
  338.             return
  339.         else:
  340.             self._muilti_avoid["set"] = True
  341.         # 设置目标位姿
  342.         if ref == "robot":
  343.             position, rotation = self._tools.coor.to_world_coordinate(
  344.                 (position, rotation), self.get_current_world_pose()
  345.             )
  346.         else:
  347.             if ref != "world":
  348.                 print("ref is not 'world' or 'robot', by default 'world' is used!")
  349.                 ref = "world"
  350.             if len_rotation == 4:
  351.                 self._last_orientation_cmd = rotation
  352.                 rotation = transformations.euler_from_quaternion(rotation)
  353.                 rotation = np.array(rotation, dtype=np.float64)
  354.         self._last_pose_ref = ref
  355.         self._position_target, self._rotation_target = position, rotation
  356.         self._muilti_avoid["set"] = False
  357.  
  358.     def get_target_pose(self) -> tuple:
  359.         """获取机器人在世界坐标系下的目标位姿"""
  360.         return self._position_target, self._rotation_target
  361.  
  362.     def get_velocity_cmd(self, ignore_stop=False) -> tuple:
  363.         """获取机器人的速度指令"""
  364.         if self._muilti_avoid["get"]:
  365.             print("Warning: get_velocity_cmd is called before the last is finished!")
  366.             return self._vel_cmd
  367.         else:
  368.             self._muilti_avoid["get"] = True
  369.         if self._move_stop and not ignore_stop:
  370.             self._vel_cmd = (np.array((0, 0, 0)), np.array((0, 0, 0)))
  371.         elif self._move_method == "three_stage":
  372.             self._three_stage_control()
  373.         elif self._move_method == "three_stage_improved":
  374.             self._three_stage_control_improved()
  375.         else:
  376.             self._composit_velocity()
  377.         self._muilti_avoid["get"] = False
  378.         if self._new_target > 0:
  379.             self._new_target -= 1
  380.         return self._vel_cmd
  381.  
  382.     def move(self, time_out=None) -> bool:
  383.         """移动机器人到最新设置的目标位姿"""
  384.         self._move_stop = False
  385.         start_time = time.time()
  386.         self.get_velocity_cmd()  # 防止第一次速度指令为0
  387.         wait_time = time_out if time_out is not None else self._wait_timeout
  388.         while (self._vel_cmd[0] != 0).any() and (self._vel_cmd[1] != 0).any():
  389.             if (time.time() - start_time) > wait_time:
  390.                 print(
  391.                     "Move timeout, the robot may be stuck! The motion will be stopped and the program will continue!"
  392.                 )
  393.                 self._move_stop = True
  394.                 return False
  395.             time.sleep(self._wait_period)
  396.         self._move_stop = True
  397.         return True
  398.  
  399.     def set_wait_tolerance(
  400.         self, position: float, orientation: float, timeout: float, frequency: float
  401.     ):
  402.         """设置等待的误差容限和超时时间，以及等待的刷新频率"""
  403.         self._wait_tolerance = np.array([position, orientation], dtype=np.float64)
  404.         self._wait_timeout = timeout
  405.         self._wait_period = 1 / frequency
  406.  
  407.     def avoid_321(self, avoid: bool = True, same_ignore: bool = True):
  408.         """避免3阶段旋转误差造成的3阶段中、3阶段结束后相同命令引起的退化"""
  409.         self._avoid_321 = avoid
  410.         self._same_ignore = same_ignore
  411.  
  412.     def avoid_swing(self, avoid: bool = True):
  413.         self._avoid_swing = avoid
  414.  
  415.     def set_move_kp(self, tarns: float, rotat: float):
  416.         """设置机器人运动控制的比例增益"""
  417.         self._move_kp = (tarns, rotat)
  418.  
  419.     def set_velocity_limits(self, linear: tuple, angular: tuple):
  420.         """设置机器人运动的线速度和角速度的上下限"""
  421.         self._linear_limits = linear
  422.         self._angular_limits = angular
  423.  
  424.     def set_velocity_dead_zone(self, linear_dead_zone: float, angular_dead_zone: float):
  425.         """设置机器人运动的线速度和角速度死区"""
  426.         self._velocity_dead_zone = (linear_dead_zone, angular_dead_zone)
  427.  
  428.     def set_direction_tolerance(self, tolerance: Union[float, tuple, list]):
  429.         """设置机器人运动的方向容限"""
  430.         if isinstance(tolerance, (tuple, list)):
  431.             self._direction_tolerance_improved = tolerance
  432.         else:
  433.             self._direction_tolerance = tolerance
  434.  
  435.     def set_improve_enhance(self, enhance: float):
  436.         """设置机器人运动的方向容限"""
  437.         self._improve_enhance = enhance
  438.  
  439.     def set_move_method(self, method: str):
  440.         """设置机器人运动的控制方法"""
  441.         self._move_method = method
  442.  
  443.     def _composit_velocity(self):
  444.         velocity = self._tools.composit_velocity(
  445.             self.get_target_pose(),
  446.             self.get_current_world_pose(),
  447.             self._move_kp,
  448.             (self._linear_limits, self._angular_limits),
  449.             self._velocity_dead_zone,
  450.         )
  451.         self._vel_cmd = velocity
  452.         return velocity
  453.  
  454.     def _three_stage_control(self):
  455.         velocity = self._tools.three_stage_control(
  456.             self.get_target_pose(),
  457.             self.get_current_world_pose(),
  458.             list(self._wait_tolerance) + [self._direction_tolerance],
  459.             self._move_kp,
  460.             (self._linear_limits, self._angular_limits),
  461.             self._velocity_dead_zone,
  462.         )
  463.         self._vel_cmd = velocity
  464.         return velocity
  465.  
  466.     def _three_stage_control_improved(self) -> tuple:
  467.         velocity, self._last_stage = self._tools.three_stage_control_improved(
  468.             self.get_target_pose(),
  469.             self.get_current_world_pose(),
  470.             self._wait_tolerance,
  471.             self._direction_tolerance_improved,
  472.             self._move_kp,
  473.             (self._linear_limits, self._angular_limits),
  474.             self._velocity_dead_zone,
  475.             enhance=self._improve_enhance,
  476.             last_stage=self._last_stage,
  477.             new_target=self._new_target,
  478.             avoid_321=self._avoid_321,
  479.             same_ignore=self._same_ignore,
  480.             avoid_swing=self._avoid_swing,
  481.         )
  482.         if self._TEST_:
  483.             print("target_rotation:", self.get_target_pose()[1])
  484.             print("current_rotation:", self.get_current_world_pose()[1])
  485.             print("target_position:", self.get_target_pose()[0])
  486.             print("current_position:", self.get_current_world_pose()[0])
  487.             print("velocity cmd:", velocity)
  488.             print(" ")
  489.         self._vel_cmd = velocity
  490.         return velocity
  491.  
  492.     def _get_wait_error(self) -> np.ndarray:
  493.         """得到机器人当前与目标位姿的误差（欧式距离）"""
  494.         wait_error = self._tools.coor.get_pose_distance(
  495.             self.get_target_pose(), self.get_current_world_pose()
  496.         )
  497.         return wait_error
  498.  
  499.     def set_control_handel(self, hanle, frequency=None):
  500.         """设置外部控制函数，将启动一个线程来循环执行该函数"""
  501.  
  502.         def thread_handle():
  503.             while True:
  504.                 hanle(self.get_velocity_cmd())
  505.                 if frequency is not None:
  506.                     time.sleep(1 / frequency)
  507.  
  508.         Thread(target=thread_handle, daemon=True).start()
  509.  
  510.     def set_current_pose_handle(self, handle):
  511.         """设置获得当前位姿的外部句柄函数调用(重写get_current_world_pose)"""
  512.         self.get_current_world_pose = handle
  513.  
  514.     def set_current_world_pose(self, position, rotation):
  515.         """设置机器人当前位姿"""
  516.         if len(rotation) == 4:
  517.             rotation = transformations.euler_from_quaternion(rotation)
  518.             rotation = np.array(rotation, dtype=np.float64)
  519.         self._current_position, self._current_rotation = position, rotation
  520.  
  521.     def get_current_world_pose(self) -> tuple:
  522.         """
  523.         获取机器人在世界坐标系下的位置和姿态；
  524.         若未设置过当前位姿，则返回目标位姿（不论目标位姿是否设置过）；
  525.         """
  526.         if self._current_position is None or self._current_rotation is None:
  527.             return self._position_target, self._rotation_target
  528.         else:
  529.             return self._current_position, self._current_rotation
  530.  
  531.     def set_model_four_steering_wheel(self, width, lenth, radius=1, reduction=1):
  532.         """设置四舵轮机器人底盘模型"""
  533.         self.chasis_width = width
  534.         self.chasis_lenth = lenth
  535.         self.wheel_radius = radius
  536.         self.wheel_reduction = reduction
  537.  
  538.     def ik_four_steering_wheel(self, linear_vx, linear_vy, angular_vz):
  539.         """四舵轮底盘的逆运动学（给定目标速度和相关参数，计算四轮转向和轮速）"""
  540.         speeds, angles = self._tools.four_steering_wheel_ik(
  541.             linear_vx,
  542.             linear_vy,
  543.             angular_vz,
  544.             self.chasis_width,
  545.             self.chasis_lenth,
  546.             self.wheel_radius,
  547.             self.wheel_reduction,
  548.         )
  549.         return speeds, angles
  550.  
  551.     def pose_ik_four_steering_wheel(self):
  552.         """根据当前位姿和目标位姿，直接解算得到四轮转角和转速"""
  553.         vel = self._composit_velocity()
  554.         speeds, angles = self.ik_four_steering_wheel(vel[0][0], vel[0][1], vel[1][2])
  555.         return speeds, angles
  556.  
  557.  
  558. if __name__ == "__main__":
  559.     import rospy
  560.     from geometry_msgs.msg import Pose, Twist
  561.     from . import to_ros_msgs
  562.  
  563.     rospy.init_node("test_base_control")
  564.  
  565.     base_control = BaseControl()
  566.     base_control.set_move_method("three_stage_improved")
  567.     base_control.set_move_kp(1.5, 0.2)
  568.     base_control.set_velocity_limits((0.0, 0.3), (0.0, 0.18))
  569.     base_control.set_velocity_dead_zone(0.0, 0.0)
  570.     base_control.set_direction_tolerance(0.2)
  571.     base_control.set_wait_tolerance(0.1, 0.1, 60, 200)
  572.     base_control.set_improve_enhance(2)
  573.  
  574.     # 注册机器人底盘外部控制接口
  575.     pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
  576.  
  577.     def vel_cmd_pub(raw_cmd):
  578.         pub.publish(to_ros_msgs.to_Twist(raw_cmd))
  579.  
  580.     base_control.set_control_handel(vel_cmd_pub, frequency=200)
  581.  
  582.     # 实现机器人当前位姿更新接口
  583.     def pose_cb(msg: Pose):
  584.         euler = transformations.euler_from_quaternion(
  585.             [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]
  586.         )
  587.         base_control.set_current_world_pose(
  588.             np.array(
  589.                 [msg.position.x, msg.position.y, msg.position.z], dtype=np.float64
  590.             ),
  591.             np.array(euler, dtype=np.float64),
  592.         )
  593.  
  594.     rospy.Subscriber("/airbot/pose", Pose, pose_cb)
  595.  
  596.     rospy.sleep(1)
  597.     target_pose0 = base_control.get_current_world_pose()
  598.     target_pose1 = (target_pose0[0], np.array([0.0, 0.0, 1.5], dtype=np.float64))
  599.     target_pose2 = (np.array([-2.5, -4, 0.5], dtype=np.float64), target_pose1[1])
  600.  
  601.     base_control.set_target_pose(*target_pose2)
  602.     print("*************************************")
  603.     print("Start moving!")
  604.     base_control.move()
  605.     print("Move finished!")
  606.     print("*************************************")
  607.     rospy.sleep(0.5)

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Guff_9hys/article/detail/816297
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号