def __init__(self, pose=TransformStamped( header=Header(frame_id='panda_link8'), child_frame_id='target', transform=Transform(rotation=Quaternion( *tf.quaternion_about_axis(numpy.pi / 4, [0, 0, 1])), translation=Vector3(0, 0, 0.105)))): self.robot = RobotModel() self.robot._add(Joint(pose)) # add a fixed end-effector transform self.pub = rospy.Publisher('/target_joint_states', JointState, queue_size=10) self.joint_msg = JointState() self.joint_msg.name = [j.name for j in self.robot.active_joints] self.joint_msg.position = numpy.asarray([ (j.min + j.max) / 2 + 0.1 * (j.max - j.min) * random.uniform(0, 1) for j in self.robot.active_joints ]) self.target_link = pose.child_frame_id self.T, self.J = self.robot.fk( self.target_link, dict(zip(self.joint_msg.name, self.joint_msg.position))) self.im_server = MyInteractiveMarkerServer("controller", self.T)
class Controller(object): def __init__(self, pose=TransformStamped( header=Header(frame_id='panda_link8'), child_frame_id='target', transform=Transform(rotation=Quaternion( *tf.quaternion_about_axis(numpy.pi / 4, [0, 0, 1])), translation=Vector3(0, 0, 0.105)))): self.robot = RobotModel() self.robot._add(Joint(pose)) # add a fixed end-effector transform self.pub = rospy.Publisher('/target_joint_states', JointState, queue_size=10) self.joint_msg = JointState() self.joint_msg.name = [j.name for j in self.robot.active_joints] self.joint_msg.position = numpy.asarray([ (j.min + j.max) / 2 + 0.1 * (j.max - j.min) * random.uniform(0, 1) for j in self.robot.active_joints ]) self.target_link = pose.child_frame_id self.T, self.J = self.robot.fk( self.target_link, dict(zip(self.joint_msg.name, self.joint_msg.position))) self.im_server = MyInteractiveMarkerServer("controller", self.T) def actuate(self, q_delta): self.joint_msg.position += q_delta.ravel() self.pub.publish(self.joint_msg) self.T, self.J = self.robot.fk( self.target_link, dict(zip(self.joint_msg.name, self.joint_msg.position))) def solve(self, J, error): return numpy.linalg.pinv(J, 0.01).dot(error) @staticmethod def position_error(T_tgt, T_cur): return T_tgt[0:3, 3] - T_cur[0:3, 3] def position_control(self, target): v = self.position_error(target, self.T) q_delta = self.solve(self.J[0:3, :], v) self.actuate(q_delta)
def draw_robot_with_center_of_mass(robot_name=None, read_state_string=None): assert robot_name is not None or read_state_string is not None if read_state_string is None: read_state_string = RealRobotClient().read_state() assert 'nan' not in read_state_string angles = ReadStateParser(read_state_string).get_all_joint_radians() painter = Painter(azimuth=0, elevation=0) robot_model = RobotModel(painter) center_of_mass = compute_center_of_mass_from_read_state_string(angles, painter, robot_model,\ draw_robot=True, draw_center_of_mass=True) # robot_model.draw_coordinates() # draw coordinates lastly because we want them on top layer robot_model.show()
def __init__(self, hip_height, torso_angle, lift_off_slope, step_height, step_length, put_down_slope): self.hip_height = hip_height self.torso_angle = torso_angle self.lift_off_slope = lift_off_slope self.step_height = step_height self.step_length = step_length self.put_down_slope = put_down_slope self.upper_limp_part = UpperLimbWhileWalking(torso_angle, hip_height) self.hip_offset = self.upper_limp_part.hip_offset self.painter = Painter() self.robot_model = RobotModel(self.painter)
__author__ = 'zhao' from inverse_kinematics.lower_limp import InverseKinematicsLeg from util.painter import Painter from util.point import Point from robot_model import RobotModel is_left = False # target_position = Point((-8.0, 7.25, -44.699999999999989)) target_position = Point((-8.0, 7.25, -44.7)) painter = Painter() robot_model = RobotModel(painter) r_hip_transversal_radians = 0 r_hip_frontal_radians, r_hip_lateral_radians, r_knee_lateral_radians, r_ankle_lateral_radians, r_ankle_frontal_radians\ = InverseKinematicsLeg(is_left=is_left, hip_transversal_radians=r_hip_transversal_radians, target_position=target_position, robot_model=robot_model, painter=painter).get_angles() # r_hip_frontal_radians, r_hip_lateral_radians, r_knee_lateral_radians, r_ankle_lateral_radians, r_ankle_frontal_radians = [0] * 5 # painter.draw_point(target_position) robot_model.draw_neck_and_head() robot_model.draw_torso() robot_model.draw_left_arm() robot_model.draw_right_arm() robot_model.draw_left_lower_limp() robot_model.draw_right_lower_limp( right_hip_transversal_radians=r_hip_transversal_radians, right_hip_frontal_radians=r_hip_frontal_radians, right_hip_lateral_radians=r_hip_lateral_radians, right_knee_lateral_radians=r_knee_lateral_radians, right_ankle_lateral_radians=r_ankle_lateral_radians, right_ankle_frontal_radians=r_ankle_frontal_radians)
def main(gx=8, gy=2): show_animation = True print(__file__ + " start!!") max_v = 2 min_v = 0 max_w = pi / 3 * 2 max_acc_v = 0.7 max_acc_w = pi / 3.0 * 5 init_x = 0.0 init_y = 6 init_yaw = pi / 8.0 robot_radius = 0.3 ob_radius = 0.3 goal_radius = 0.3 pursuitor_model = RobotModel(max_v, min_v, max_w, max_acc_v, max_acc_w, init_x, init_y, init_yaw) target_model = RobotModel(max_v, min_v, max_w, max_acc_v, max_acc_w, gx, gy, -pi / 2.0) # obstacles [x(m) y(m), ....] ob = np.array([[0, 2], [4.0, 2.0], [5.0, 4.0], [5.0, 6.0], [7.0, 9.0]]) config = ConfigAPF(2, 0, math.pi / 3, 0.7, math.pi / 3) traj = np.array(pursuitor_model.state) traj_u = np.array([0, 0]) traj_target = np.array(target_model.state) for i in range(2000): logging.info("") logging.info(i) u = apf_control(config, pursuitor_model.state, target_model.state, ob) traj_u = np.vstack((traj_u, u)) print(u) u[1] = pursuitor_model.rot_to_angle(u[1]) # u[0]=0 x = pursuitor_model.motion(u, config.dt) traj = np.vstack((traj, x)) # store state history goal = target_model.motion([1.8, pi / 5], config.dt) traj_target = np.vstack((traj_target, goal)) # print(traj) if show_animation: plt.cla() plt.plot(x[0], x[1], "xr") plt.plot(goal[0], goal[1], "xb") plt.plot(ob[:, 0], ob[:, 1], "ok") plot_arrow(x[0], x[1], x[2]) plt.axis("equal") plt.grid(True) plt.pause(0.0001) # check goal if math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2) <= (pursuitor_model.robot_radius + target_model.robot_radius): print("Goal!!") break # check collision collision = False for obstacle in ob: if math.sqrt((x[0] - obstacle[0])**2 + (x[1] - obstacle[1])**2) <= ( pursuitor_model.robot_radius + config.obstacle_radius): collision = True if collision: print("Collision!") break prefix = "apf_flaw/4_" np.save(prefix + "traj.npy", traj) np.save(prefix + "traj_target.npy", traj_target) np.save(prefix + "traj_u.npy", traj_u) print("Done") if show_animation: plt.plot(traj[:, 0], traj[:, 1], "-r") plt.plot(traj_target[:, 0], traj_target[:, 1], "-g") fig = plt.gcf() # fig.set_size_inches(6,6) plt.xlabel('X(m)') plt.ylabel('Y(m)') # plt.axis("auto") plt.tight_layout() print(plt.xlim((-1.488585497312541, 14.14373130420207))) print(plt.ylim((0.4404926013282211, 12.089089959876205))) plt.figure() plt.plot(np.arange(0, len(traj)) * config.dt, traj_u[:, 1], label=r'Input $\theta$') plt.plot(np.arange(0, len(traj)) * config.dt, traj[:, 2], label=r'Actual $\theta$') plt.xlabel('T(s)') plt.ylabel(r'$\theta$(rad)') plt.legend() plt.show()
import matplotlib.image as mpimg from grid_map import GridMap from robot_model import RobotModel dr = mpimg.imread('/home/kai/Pictures/dr.png') global_grid_map = GridMap(dr, 1.0) global_grid_map.print() robot_model = RobotModel(2.0) controls = robot_model.get_controls((639, 200), global_grid_map) print(controls)
def setUp(self): self.painter = Painter() self.r = RobotModel(Painter())
from util.painter import Painter from util.point import Point from robot_model import RobotModel from robot_spec.position_data import * from inverse_kinematics.arm import InverseKinematicsArm from inverse_kinematics.lower_limp import InverseKinematicsLeg from inverse_kinematics.head import move_head from robot_client_server.robot_clients import RealRobotClient from joint import * painter = Painter(azimuth=0, elevation=50) r = RobotModel(painter) def inverse_kinematics(head_target_position, l_arm_target_position, r_arm_target_position, l_leg_target_position, l_hip_transversal_radians, r_leg_target_position, r_hip_transversal_radians, do_actuate): # move kinematic chains neck_transversal_radians, neck_lateral_radians\ = move_head(head_target_position, painter) l_shoulder_lateral_radians, l_shoulder_frontal_radians, l_elbow_lateral_radians\ = InverseKinematicsArm(l_arm_target_position, r, painter, is_left=True).get_angles() r_shoulder_lateral_radians, r_shoulder_frontal_radians, r_elbow_lateral_radians\ = InverseKinematicsArm(r_arm_target_position, r, painter, is_left=False).get_angles() l_hip_frontal_radians, l_hip_lateral_radians, l_knee_lateral_radians, l_ankle_lateral_radians, l_ankle_frontal_radians\ = InverseKinematicsLeg(is_left=True, l_hip_transversal_radians, l_leg_target_position, r, painter).get_angles() r_hip_frontal_radians, r_hip_lateral_radians, r_knee_lateral_radians, r_ankle_lateral_radians, r_ankle_frontal_radians\ = InverseKinematicsLeg(is_left=False, r_hip_transversal_radians, r_leg_target_position, r, painter).get_angles()
def __init__(self): self.max_v = MAX_V self.min_v = MIN_V self.max_w = MAX_W self.max_acc_v = 0.7 self.max_acc_w = pi / 3.0 * 5 self.init_x = 0.0 self.init_y = 6.0 self.init_yaw = pi / 8.0 self.robot_radius = 0.3 self.ob_radius = 0.3 self.target_radius = 0.3 self.dt = 0.1 self.target_init_x = 10 self.target_init_y = 4 self.target_init_yaw = pi / 2.0 self.target_u = None self.pursuitor_model = RobotModel(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, self.init_x, self.init_y, self.init_yaw) self.evader_model = RobotModel(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, self.target_init_x, self.target_init_y, self.target_init_yaw) self.config_dwa = ConfigDWA(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, robot_raduis=self.robot_radius, obstacle_radius=self.ob_radius, dt=self.dt) self.confg_apf = ConfigAPF(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, obstacle_radius=self.ob_radius) self.confg_pn = ConfigPN() """env parameters need reset """ self.ob_list = None # self.ob_list = None self.last_obs = None self.last_pur_state = None self.last_tar_state = None # trajectory self.traj = np.array(self.get_state()) # state done self.collision = False self.catch = False # actions # self.v_reso = 0.1 # self.w_reso = pi/18.0 # # # self.action_table = [] # for v in np.arange(self.min_v, self.max_v+self.v_reso, self.v_reso): # for w in np.arange(-self.max_w, self.max_w+self.w_reso, self.w_reso): # self.action_table.append(np.array([v, w])) # self.action_num = len(self.action_table) self.action_num = 2 self.action_space = spaces.Box(np.array([-1]), np.array( [+1])) # v accelerate, w accelerate # observations = laser, goal_angle, goal_distance self.percept_region = np.array([-4, 4, -2, 6]) # left, right, behind, ahead self.basic_sample_reso = 0.1 # sampling resolution of percept region self.sample_reso_scale = 1.5 # sampling density increases with distance from def cal_samples(sample_length, basic_sample_reso, sample_reso_scale): # get sample number # x = sympy.Symbol('x') # result_dict = sympy.solve( # [basic_sample_reso * (1 - sample_reso_scale ** x) / (1 - sample_reso_scale) # - abs(sample_length)], [x]) # print(result_dict) a = math.log(sample_length / basic_sample_reso * (sample_reso_scale - 1) + 1) b = math.log(sample_reso_scale) # print(str(a)+", "+str(b)) x = a / b return int(x) left_samples = right_samples = cal_samples(abs(self.percept_region[0]), self.basic_sample_reso, self.sample_reso_scale) behind_samples = cal_samples(abs(self.percept_region[2]), self.basic_sample_reso, self.sample_reso_scale) ahead_samples = cal_samples(abs(self.percept_region[3]), self.basic_sample_reso, self.sample_reso_scale) self.sample_map_center = np.array( [left_samples - 1, behind_samples - 1]) self.sample_map_width = left_samples + right_samples - 1 self.sample_map_height = behind_samples + ahead_samples - 1 self.percept_sample_map = [] for i in range(self.sample_map_width): tmp_list_y = [] for j in range(self.sample_map_height): x = self.basic_sample_reso * (1 - self.sample_reso_scale ** abs(i - self.sample_map_center[0])) \ / (1 - self.sample_reso_scale) x = x if i - self.sample_map_center[0] >= 0 else -x y = self.basic_sample_reso * (1 - self.sample_reso_scale ** abs(j - self.sample_map_center[1])) \ / (1 - self.sample_reso_scale) y = y if j - self.sample_map_center[1] >= 0 else -y tmp_list_y.append(np.array([x, y])) self.percept_sample_map.append(tmp_list_y) # self.obs_num = self.pursuitor_model.laser_num + 2 # high = np.hstack((np.zeros(self.pursuitor_model.laser_num)+self.pursuitor_model.laser_max_range, pi, 50)) # low = np.hstack((np.zeros(self.pursuitor_model.laser_num), -pi, 0)) # self.observation_space = spaces.Box(low, high, dtype=np.float) self.obs_num = 2 + 2 self.observation_space = spaces.Box(-np.inf, np.inf, shape=(self.obs_num, ), dtype=float) # seed self.np_random = None self.seed() # time limit self.limit_step = 300 self.count = 0 self.action_plot = 0 # record self.pursuit_strategy = None self.traj_pursuitor = None self.traj_evader = None # intelligent evasion mode self.evasion_mode = False self.config_evasion = ConfigDWA(self.max_v * 0.9, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, robot_raduis=self.robot_radius, obstacle_radius=self.ob_radius, dt=self.dt) self.evasion_route = None self.evasion_route_index = None
class PursuitEnvTorque(gym.Env): metadata = { 'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 2 } def __init__(self): self.max_v = MAX_V self.min_v = MIN_V self.max_w = MAX_W self.max_acc_v = 0.7 self.max_acc_w = pi / 3.0 * 5 self.init_x = 0.0 self.init_y = 6.0 self.init_yaw = pi / 8.0 self.robot_radius = 0.3 self.ob_radius = 0.3 self.target_radius = 0.3 self.dt = 0.1 self.target_init_x = 10 self.target_init_y = 4 self.target_init_yaw = pi / 2.0 self.target_u = None self.pursuitor_model = RobotModel(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, self.init_x, self.init_y, self.init_yaw) self.evader_model = RobotModel(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, self.target_init_x, self.target_init_y, self.target_init_yaw) self.config_dwa = ConfigDWA(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, robot_raduis=self.robot_radius, obstacle_radius=self.ob_radius, dt=self.dt) self.confg_apf = ConfigAPF(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, obstacle_radius=self.ob_radius) self.confg_pn = ConfigPN() """env parameters need reset """ self.ob_list = None # self.ob_list = None self.last_obs = None self.last_pur_state = None self.last_tar_state = None # trajectory self.traj = np.array(self.get_state()) # state done self.collision = False self.catch = False # actions # self.v_reso = 0.1 # self.w_reso = pi/18.0 # # # self.action_table = [] # for v in np.arange(self.min_v, self.max_v+self.v_reso, self.v_reso): # for w in np.arange(-self.max_w, self.max_w+self.w_reso, self.w_reso): # self.action_table.append(np.array([v, w])) # self.action_num = len(self.action_table) self.action_num = 2 self.action_space = spaces.Box(np.array([-1]), np.array( [+1])) # v accelerate, w accelerate # observations = laser, goal_angle, goal_distance self.percept_region = np.array([-4, 4, -2, 6]) # left, right, behind, ahead self.basic_sample_reso = 0.1 # sampling resolution of percept region self.sample_reso_scale = 1.5 # sampling density increases with distance from def cal_samples(sample_length, basic_sample_reso, sample_reso_scale): # get sample number # x = sympy.Symbol('x') # result_dict = sympy.solve( # [basic_sample_reso * (1 - sample_reso_scale ** x) / (1 - sample_reso_scale) # - abs(sample_length)], [x]) # print(result_dict) a = math.log(sample_length / basic_sample_reso * (sample_reso_scale - 1) + 1) b = math.log(sample_reso_scale) # print(str(a)+", "+str(b)) x = a / b return int(x) left_samples = right_samples = cal_samples(abs(self.percept_region[0]), self.basic_sample_reso, self.sample_reso_scale) behind_samples = cal_samples(abs(self.percept_region[2]), self.basic_sample_reso, self.sample_reso_scale) ahead_samples = cal_samples(abs(self.percept_region[3]), self.basic_sample_reso, self.sample_reso_scale) self.sample_map_center = np.array( [left_samples - 1, behind_samples - 1]) self.sample_map_width = left_samples + right_samples - 1 self.sample_map_height = behind_samples + ahead_samples - 1 self.percept_sample_map = [] for i in range(self.sample_map_width): tmp_list_y = [] for j in range(self.sample_map_height): x = self.basic_sample_reso * (1 - self.sample_reso_scale ** abs(i - self.sample_map_center[0])) \ / (1 - self.sample_reso_scale) x = x if i - self.sample_map_center[0] >= 0 else -x y = self.basic_sample_reso * (1 - self.sample_reso_scale ** abs(j - self.sample_map_center[1])) \ / (1 - self.sample_reso_scale) y = y if j - self.sample_map_center[1] >= 0 else -y tmp_list_y.append(np.array([x, y])) self.percept_sample_map.append(tmp_list_y) # self.obs_num = self.pursuitor_model.laser_num + 2 # high = np.hstack((np.zeros(self.pursuitor_model.laser_num)+self.pursuitor_model.laser_max_range, pi, 50)) # low = np.hstack((np.zeros(self.pursuitor_model.laser_num), -pi, 0)) # self.observation_space = spaces.Box(low, high, dtype=np.float) self.obs_num = 2 + 2 self.observation_space = spaces.Box(-np.inf, np.inf, shape=(self.obs_num, ), dtype=float) # seed self.np_random = None self.seed() # time limit self.limit_step = 300 self.count = 0 self.action_plot = 0 # record self.pursuit_strategy = None self.traj_pursuitor = None self.traj_evader = None # intelligent evasion mode self.evasion_mode = False self.config_evasion = ConfigDWA(self.max_v * 0.9, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, robot_raduis=self.robot_radius, obstacle_radius=self.ob_radius, dt=self.dt) self.evasion_route = None self.evasion_route_index = None def seed(self, seed=None): self.np_random, seed = seeding.np_random(seed) return [seed] def step(self, action): # assert self.action_space.contains(action), "%r (%s) invalid" % (action, type(action)) observation, reward, done, info = None, None, None, None total_reward = 0 for _ in range(random.randint(3, 3)): observation, reward, done, info = self.mini_step(action) total_reward += reward self.count += 1 if done: self.record_count = self.count prefix = "scenario3/" # np.save(prefix+"p_strategy.npy", np.array(self.pursuit_strategy)) # np.save(prefix+"p_hybrid.npy", np.array(self.traj_pursuitor)) # np.save(prefix+"e_hybrid.npy", np.array(self.traj_evader)) # # np.save(prefix+"p_dwa.npy", np.array(self.traj_pursuitor)) # np.save(prefix+"e_dwa.npy", np.array(self.traj_evader)) # # np.save(prefix+"p_apf.npy", np.array(self.traj_pursuitor)) # np.save(prefix+"e_apf.npy", np.array(self.traj_evader)) break return observation, total_reward, done, info def mini_step(self, action): self.action_plot = action self._set_action(action) observation = self._get_obs() done = self._is_done(observation) if self.count >= self.limit_step: done = True reward = self._compute_reward(observation) self.last_obs = observation return observation, reward, done, {} def reset(self): self.init_terrain(2) self.init_env_from_list(random.randint(0, 0)) if self.evasion_mode: self.init_evader_route(random.randint(0, 2)) else: self.init_evader_circle(random.randint(2, 2)) # self.init_env_from_list(3, 1) self.pursuitor_model.set_init_state(self.init_x, self.init_y, self.init_yaw) self.evader_model.set_init_state(self.target_init_x, self.target_init_y, self.target_init_yaw) self.count = 0 # trajectory self.traj = np.array(self.get_state()) obs = self._get_obs() self.last_obs = obs self.last_pur_state = self.get_state() self.last_tar_state = self.get_goal() # record self.pursuit_strategy = [] self.traj_pursuitor = [] self.traj_pursuitor.append(self.get_state()) self.traj_evader = [] self.traj_evader.append(self.get_goal()) # intelligent evasion self.keyboard_u = [0.0, 0.0] return obs def close(self): pass def render(self, mode='human'): plt.cla() x = self.get_state() goal = self.get_goal() ob = self.ob_list plt.plot(x[0], x[1], "xr") plt.plot(goal[0], goal[1], "xb") if ob is not None: plt.plot(ob[:, 0], ob[:, 1], "ok") plot_arrow(x[0], x[1], x[2]) # # plot laser # T_robot = self.get_TF() # points = self.scan_to_points(self.last_obs[:self.pursuitor_model.laser_num], self.pursuitor_model.laser_min_angle, # self.pursuitor_model.laser_increment_angle, T_robot) # if len(points)>0: # plt.plot(points[:,0], points[:,1], ".m") plt.axis("equal") plt.grid(True) plt.pause(0.0001) # print(self.last_obs) return np.array([[[1, 1, 1]]], dtype=np.uint8) def scan_to_points(self, laser_ranges, min_angle, increment_angle, T_robot): laser_points = [] for laser_index in range(len(laser_ranges)): laser_range = laser_ranges[laser_index] laser_angle = increment_angle * laser_index + min_angle # only plot reflected if laser_range < self.pursuitor_model.laser_max_range - 2: laser_points.append([ laser_range * cos(laser_angle), laser_range * sin(laser_angle) ]) laser_points = np.array(laser_points) for i, laser_point in enumerate(laser_points): laser_point_tmp = np.matmul(T_robot, np.hstack((laser_point, 1))) laser_points[i] = laser_point_tmp[:2] return laser_points def _get_obs(self): """Returns the observation. """ # laser_ranges = self.pursuitor_model.get_laser_scan(self.ob_list, self.ob_radius) percept_map = self.sample_map() percept_map = np.reshape(percept_map, (1, -1))[0].tolist() goal = self.get_goal() state = self.get_state() goal_angle = atan2(goal[1] - state[1], goal[0] - state[0]) - state[2] goal_angle = self.normlize_angle(goal_angle) goal_distance = np.linalg.norm(goal[:2] - state[:2]) return np.array(state[3:5].tolist() + [goal_angle, goal_distance]) def sample_map(self): """ “exteroceptive information”, sample the percept region. :return: sampling points of percept region, with value representing occupancy (0 is non-free, 255 is free) """ if self.ob_list is None: return np.ones([self.sample_map_width, self.sample_map_height]) # represent obstacle in robot coordinate system T_robot = self.get_TF() ob_r_list = [ np.matmul(T_robot.T, np.hstack((np.array(ob), 1)))[:2] for ob in self.ob_list ] # filter obstacle out of percept region def filter_ob(obstacle_radius, ob_list, percept_region): percept_region_expanded = percept_region + np.array([ -obstacle_radius, obstacle_radius, -obstacle_radius, obstacle_radius ]) def is_in_percept_region(ob): check_ob_center_in = percept_region_expanded[0] <= ob[0] <= percept_region_expanded[1] and \ percept_region_expanded[2] <= ob[1] <= percept_region_expanded[3] return check_ob_center_in filtered_ob_list = list(filter(is_in_percept_region, ob_list)) return filtered_ob_list filtered_ob_list = filter_ob(self.ob_radius, ob_r_list, self.percept_region) if len(filtered_ob_list) is 0: return np.ones([self.sample_map_width, self.sample_map_height]) sampled_map = np.zeros([self.sample_map_width, self.sample_map_height]) for i in range(self.sample_map_width): for j in range(self.sample_map_height): sample_point = self.percept_sample_map[i][j] dis2ob = np.linalg.norm(np.array(filtered_ob_list) - sample_point, axis=1, keepdims=True) if np.any(dis2ob <= self.ob_radius): sampled_map[i, j] = 0 else: sampled_map[i, j] = 1 return sampled_map def _set_action(self, action): """Applies the given action to the simulation. """ u = self.get_state()[3:5] u[0] = self.max_v u[1] = (action[0] - (-1)) / 2 * (self.max_w + self.max_w) - self.max_w # use for differential guidence law self.last_pur_state = self.get_state() self.last_tar_state = self.get_goal() # u=[0,0] self.pursuitor_model.motion(u, self.dt) if self.evasion_mode: # if reach this point if np.linalg.norm( self.evasion_route[self.evasion_route_index, :2] - self.get_goal()[:2]) <= self.robot_radius * 2: self.evasion_route_index = (self.evasion_route_index + 1) % len(self.evasion_route) target_u, _ = dwa_control( self.get_goal(), self.get_goal()[3:5], self.config_evasion, self.evasion_route[self.evasion_route_index, :2], self.ob_list) self.evader_model.motion((target_u), self.dt) else: self.evader_model.motion(self.target_u, self.dt) assert not self.robot_collision_with_obstacle( self.evader_model.state[:2], self.target_radius, self.ob_list, self.ob_radius) x = self.get_state() self.traj = np.vstack((self.traj, x)) # store state history self.traj_pursuitor.append(self.get_state()) self.pursuit_strategy.append(action) self.traj_evader.append(self.get_goal()) def _compute_reward(self, observation): # reward = ((-observation[-1] + self.last_obs[-1])/(self.max_v*self.dt)) ** 3 * 10 reward = -observation[-1] / 30.0 + -abs(observation[-2]) / 15.0 if self.collision: reward = -150 if self.catch: reward = 200 return reward def _is_done(self, observations): self.catch = False self.collision = False if observations[-1] <= self.robot_radius + self.target_radius: self.catch = True # # laser_collision = np.array(observations[:self.pursuitor_model.laser_num]) <= self.robot_radius # if laser_collision.any(): # self.collision = True if self.robot_collision_with_obstacle(self.get_state()[:2], self.robot_radius, self.ob_list, self.ob_radius): self.collision = True return self.catch or self.collision def why_done(self): if self.catch: return 0 elif self.collision: return 1 elif self.record_count >= self.limit_step: return 2 return False def robot_collision_with_obstacle(self, x, x_radius, ob_list, ob_radius): if ob_list is None: return False for ob in ob_list: if np.linalg.norm(x - ob) <= x_radius + ob_radius: return True return False def get_goal(self): return np.array(self.evader_model.state) def get_state(self): return np.array(self.pursuitor_model.state) def normlize_angle(self, angle): norm_angle = angle % 2 * math.pi if norm_angle > math.pi: norm_angle -= 2 * math.pi return norm_angle def get_TF(self): theta = self.get_state()[2] transition = self.get_state()[:2] T_robot = np.array([[cos(theta), -sin(theta), transition[0]], [sin(theta), cos(theta), transition[1]], [0, 0, 1]]) return T_robot def init_terrain(self, num): if num == 0: self.ob_list = np.array([[0, 2], [4.0, 2.0], [-2.0, 4.6], [5.0, 6.0], [7.0, 9.0], [12.0, 12.0], [4.0, 12.0]]) elif num == 1: self.ob_list = np.array([[0, 2], [4.0, 2.0], [-2.0, 4.6], [5.0, 6.0], [7.0, 9.0], [12.0, 12.0], [4.0, 12.0], [3.5, 15.8], [12.1, 17.0], [7.16, 14.6], [8.6, 13.0], [4.42, 10.76], [-3.76, 8.8], [2.0, -1.8], [-0.16, -1.66], [3.1, -5.1], [0.7, 6.5], [4.85, -3.05], [8.0, -0.33], [-0.3, -6.75]]) elif num == 2: self.ob_list = None def init_env_from_list(self, pursuit_init_num): if pursuit_init_num == 0: # pursuit env 0 self.init_x = 2.1 self.init_y = 7.5 self.init_yaw = 0.0 elif pursuit_init_num == 1: # pursuit env 1 self.init_x = 8.0 self.init_y = 6.0 self.init_yaw = 0.0 elif pursuit_init_num == 2: # pursuit env 2 self.init_x = 12.0 self.init_y = 8.0 self.init_yaw = 0.0 elif pursuit_init_num == 3: # pursuit env 3 self.init_x = -3.0 self.init_y = 0.0 self.init_yaw = pi / 8.0 def init_evader_circle(self, evader_init_num): if evader_init_num == 0: # evader env 1 self.target_init_x = 0.0 self.target_init_y = -10.0 self.target_init_yaw = -pi / 2.0 self.target_u = np.array([0.0, -pi / 5 * 2.0]) elif evader_init_num == 1: # evader env 2 self.target_init_x = 0.0 self.target_init_y = 9.0 self.target_init_yaw = pi - 0.3 self.target_u = np.array([0.0, 0.66]) elif evader_init_num == 2: # evader env 4 self.target_init_x = 10.0 self.target_init_y = 12.0 self.target_init_yaw = -pi / 2.0 self.target_u = np.array([0.0, -pi / 5]) def init_evader_route(self, evader_init_num): if evader_init_num == 0: self.evasion_route = np.array([[4.0, 3.5], [6.0, 2.7], [7.55, 6.8], [9.57, 11.2], [9.3, 16.5], [5.44, 16.0], [5.44, 13.23], [5.44, 9.24], [0.0, 4.5], [-2.2, -1.1], [0.0, -5.0]]) elif evader_init_num == 1: self.evasion_route = np.array([[11.64, 8.78], [7.87, 10.94], [5.49, 12.91], [2.78, 13.79], [0.99, 10.71], [2.46, 3.27], [6.91, 2.53]]) elif evader_init_num == 2: self.evasion_route = np.array([ [1.16, 8.15], [7.4, 5.63], [8.84, 0.78], [14.06, 11.86], [13.68, 17.4], ]) self.evasion_route_index = 0 self.target_init_x = self.evasion_route[0, 0] self.target_init_y = self.evasion_route[0, 1] self.target_init_yaw = math.atan2( self.evasion_route[1, 1] - self.evasion_route[0, 1], self.evasion_route[1, 0] - self.evasion_route[0, 0])
max_acc_v = 0.7 max_acc_w = pi / 3.0 * 5 init_x = 0.0 init_y = 6.0 init_yaw = pi / 8.0 robot_radius = 0.3 ob_radius = 0.3 target_radius = 0.3 dt = 0.1 target_init_x = 10 target_init_y = 4 target_init_yaw = pi / 2.0 target_u = None evader_model = RobotModel(max_v, min_v, max_w, max_acc_v, max_acc_w, target_init_x, target_init_y, target_init_yaw) ob_wall=[] top, bottom, left, right = 20.0, -10.0, -10.0, 20.0 # top y=20 for x in np.arange(left, right, 2*ob_radius): ob_wall.append(np.array([x, top])) # bottom y=-10 for x in np.arange(left, right, 2*ob_radius): ob_wall.append(np.array([x, bottom])) # left for y in np.arange(bottom+2*ob_radius, top, 2*ob_radius): ob_wall.append(np.array([left, y])) # right for y in np.arange(bottom, top, 2*ob_radius): ob_wall.append(np.array([right, y]))
from util.painter import Painter from robot_model import RobotModel from joint.joint_angle import * r = RobotModel(Painter()) r.draw_neck_and_head() r.draw_left_arm() # r.draw_left_arm(radians(-30), radians(60), radians(-30)) # r._painter.draw_point(r.draw_left_arm(radians(0), radians(-60), radians(0), color='b-')[-1]) r.draw_right_arm() # r.draw_right_arm(radians(0), radians(30), radians(160), color='b-') # r._painter.draw_point(r.draw_right_arm(radians(0), radians(60), radians(0), color='b-')[-1]) # r.draw_left_lower_limp() # r.draw_right_lower_limp(right_knee_lateral_radians=radians(150)) r.draw_left_lower_limp( # LeftHipTransversal().outward(0).radians, radians(-0), LeftHipFrontal().outward(0).angle, LeftHipLateral().forward(0).radians, LeftKneeLateral().backward(0).radians, # radians(150), LeftAnkleLateral().forward(0).radians, LeftAnkleFrontal().outward(-0).radians) r.draw_right_lower_limp( RightHipTransversal().inward(0).radians, # radians(-0), RightHipFrontal().outward(0).angle,
import time from math import * import numpy as np from a_star_planner import AStarPlanner from robot_model import RobotModel import matplotlib.image as mpimg from grid_map import GridMap rospy.init_node('test_a_star_planner', anonymous=True) vehicle_config = yaml.load( open('/home/kai/catkin_ws/src/drproj/vehicle_config.yaml')) half_length = 0.5 * vehicle_config['length'] half_width = 0.5 * vehicle_config['width'] radius = sqrt(half_length**2 + half_width**2) robot_model = RobotModel(radius) global_planner = AStarPlanner(robot_model) free = mpimg.imread('/home/kai/catkin_ws/src/drproj/free.png') global_map_config = yaml.load(open('/home/kai/catkin_ws/src/drproj/free.yaml')) global_map = GridMap(free, global_map_config['resolution']) global_map.show('rviz_global_grid_map') global_map.print() global_mission = yaml.load( open('/home/kai/catkin_ws/src/drproj/global_mission.yaml')) start_grid_x = int(global_mission['start'][0] / global_map.resolution + 0.5) start_grid_y = int(global_mission['start'][1] / global_map.resolution + 0.5) goal_grid_x = int(global_mission['goal'][0] / global_map.resolution + 0.5) goal_grid_y = int(global_mission['goal'][1] / global_map.resolution + 0.5) start = (start_grid_x, start_grid_y)
def forward_kinematics(robot_name=None, read_state_string=None): assert robot_name is not None or read_state_string is not None if read_state_string is None: read_state_string = RealRobotClient().read_state() assert 'nan' not in read_state_string r = RobotModel(Painter()) angles = ReadStateParser(read_state_string).get_all_joint_radians() r.draw_neck_and_head(neck_transversal_radians=angles['neck_transversal'], neck_lateral_radians=angles['neck_lateral']) r.draw_left_arm(left_shoulder_lateral_radians=angles['left_shoulder_lateral'], left_shoulder_frontal_radians=angles['left_shoulder_frontal'], left_elbow_lateral_radians=angles['left_elbow_lateral']) r.draw_right_arm(right_shoulder_lateral_radians=angles['right_shoulder_lateral'], right_shoulder_frontal_radians=angles['right_shoulder_frontal'], right_elbow_lateral_radians=angles['right_elbow_lateral']) r.draw_left_lower_limp(left_hip_transversal_radians=angles['left_hip_transversal'], left_hip_frontal_radians=angles['left_hip_frontal'], left_hip_lateral_radians=angles['left_hip_lateral'], left_knee_lateral_radians=angles['left_knee_lateral'], left_ankle_lateral_radians=angles['left_ankle_lateral'], left_ankle_frontal_radians=angles['left_ankle_frontal']) r.draw_right_lower_limp(right_hip_transversal_radians=angles['right_hip_transversal'], right_hip_frontal_radians=angles['right_hip_frontal'], right_hip_lateral_radians=angles['right_hip_lateral'], right_knee_lateral_radians=angles['right_knee_lateral'], right_ankle_lateral_radians=angles['right_ankle_lateral'], right_ankle_frontal_radians=angles['right_ankle_frontal']) r.draw_torso() r.draw_coordinates() # draw coordinates lastly because we want them on top layer r.show()
def __init__(self): self.max_v = 2 self.min_v = -0.3 self.max_w = pi / 3.0 * 2 self.max_acc_v = 0.7 self.max_acc_w = pi / 3.0 * 5 self.init_x = 0.0 self.init_y = 6.0 self.init_yaw = pi / 8.0 self.robot_radius = 0.3 self.ob_radius = 0.3 self.target_radius = 0.3 self.dt = 0.1 self.target_init_x = 10 self.target_init_y = 4 self.target_init_yaw = pi / 2.0 self.target_u = None self.pursuitor_model = RobotModel(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, self.init_x, self.init_y, self.init_yaw) self.evader_model = RobotModel(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, self.target_init_x, self.target_init_y, self.target_init_yaw) self.config_dwa = ConfigDWA(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, robot_raduis=self.robot_radius, obstacle_radius=self.ob_radius, dt=self.dt) self.confg_apf = ConfigAPF(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, obstacle_radius=self.ob_radius) self.confg_pn = ConfigPN() """env parameters need reset """ self.ob_list = np.array([[0, 2], [4.0, 2.0], [5.0, 4.0], [5.0, 6.0], [7.0, 9.0], [12.0, 12.0]]) self.last_obs = None self.last_pur_state = None self.last_tar_state = None # trajectory self.traj = np.array(self.get_state()) # state done self.collision = False self.catch = False # actions # self.v_reso = 0.1 # self.w_reso = pi/18.0 # # # self.action_table = [] # for v in np.arange(self.min_v, self.max_v+self.v_reso, self.v_reso): # for w in np.arange(-self.max_w, self.max_w+self.w_reso, self.w_reso): # self.action_table.append(np.array([v, w])) # self.action_num = len(self.action_table) self.action_num = 2 self.action_space = spaces.Discrete(self.action_num) # observations = image self.screen_height = 600 # pixel self.screen_width = 600 # pixel self.state_height = 150 # pixel self.state_width = 150 # pixel self.world_width = 20.0 # m self.observation_space = spaces.Box(low=0, high=255, shape=(self.state_height, self.state_width, 3), dtype=np.uint8) # seed self.np_random = None self.seed() # time limit self.limit_step = 300 self.count = 0 self.action_plot = 0 self.viewer = None
class PursuitEnv2(gym.Env): metadata = { 'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50 } def __init__(self): self.max_v = 2 self.min_v = -0.3 self.max_w = pi / 3.0 * 2 self.max_acc_v = 0.7 self.max_acc_w = pi / 3.0 * 5 self.init_x = 0.0 self.init_y = 6.0 self.init_yaw = pi / 8.0 self.robot_radius = 0.3 self.ob_radius = 0.3 self.target_radius = 0.3 self.dt = 0.1 self.target_init_x = 10 self.target_init_y = 4 self.target_init_yaw = pi / 2.0 self.target_u = None self.pursuitor_model = RobotModel(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, self.init_x, self.init_y, self.init_yaw) self.evader_model = RobotModel(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, self.target_init_x, self.target_init_y, self.target_init_yaw) self.config_dwa = ConfigDWA(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, robot_raduis=self.robot_radius, obstacle_radius=self.ob_radius, dt=self.dt) self.confg_apf = ConfigAPF(self.max_v, self.min_v, self.max_w, self.max_acc_v, self.max_acc_w, obstacle_radius=self.ob_radius) self.confg_pn = ConfigPN() """env parameters need reset """ self.ob_list = np.array([[0, 2], [4.0, 2.0], [5.0, 4.0], [5.0, 6.0], [7.0, 9.0], [12.0, 12.0]]) self.last_obs = None self.last_pur_state = None self.last_tar_state = None # trajectory self.traj = np.array(self.get_state()) # state done self.collision = False self.catch = False # actions # self.v_reso = 0.1 # self.w_reso = pi/18.0 # # # self.action_table = [] # for v in np.arange(self.min_v, self.max_v+self.v_reso, self.v_reso): # for w in np.arange(-self.max_w, self.max_w+self.w_reso, self.w_reso): # self.action_table.append(np.array([v, w])) # self.action_num = len(self.action_table) self.action_num = 2 self.action_space = spaces.Discrete(self.action_num) # observations = image self.screen_height = 600 # pixel self.screen_width = 600 # pixel self.state_height = 150 # pixel self.state_width = 150 # pixel self.world_width = 20.0 # m self.observation_space = spaces.Box(low=0, high=255, shape=(self.state_height, self.state_width, 3), dtype=np.uint8) # seed self.np_random = None self.seed() # time limit self.limit_step = 300 self.count = 0 self.action_plot = 0 self.viewer = None def seed(self, seed=None): self.np_random, seed = seeding.np_random(seed) return [seed] def step(self, action): assert self.action_space.contains( action), "%r (%s) invalid" % (action, type(action)) self.action_plot = action self._set_action(action) observation = self._get_obs() done = self._is_done(observation) if self.count >= self.limit_step: done = True reward = self._compute_reward(observation) self.last_obs = observation self.count += 1 return observation, reward, done, {} def reset(self): self.init_env_from_list(random.randint(0, 3), random.randint(1, 2)) # self.init_env_from_list(3, 1) self.pursuitor_model.set_init_state(self.init_x, self.init_y, self.init_yaw) self.evader_model.set_init_state(self.target_init_x, self.target_init_y, self.target_init_yaw) self.count = 0 # trajectory self.traj = np.array(self.get_state()) obs = self._get_obs() self.last_obs = obs self.last_pur_state = self.get_state() self.last_tar_state = self.get_goal() return obs def close(self): if self.viewer: self.viewer.close() self.viewer = None def render(self, mode='human'): # render image scale = self.screen_width / self.world_width origin = [self.screen_width / 10, self.screen_height / 10] if self.viewer is None: from gym.envs.classic_control import rendering self.viewer = rendering.Viewer(self.screen_width, self.screen_height) # plot robot with orientation pursuit_radius = int(self.robot_radius * scale) pursuit = rendering.make_polygon( [(-pursuit_radius, pursuit_radius), (0, pursuit_radius), (pursuit_radius, 0), (0, -pursuit_radius), (-pursuit_radius, -pursuit_radius)], filled=True) pursuit.set_color(.8, .6, .4) self.pursuit_trans = rendering.Transform() pursuit.add_attr(self.pursuit_trans) self.viewer.add_geom(pursuit) # plot target with orientation evader_radius = int(self.robot_radius * scale) evader = rendering.make_polygon([(-evader_radius, evader_radius), (0, evader_radius), (evader_radius, 0), (0, -evader_radius), (-evader_radius, -evader_radius)]) evader.set_color(.5, .5, .8) self.evader_trans = rendering.Transform() evader.add_attr(self.evader_trans) self.viewer.add_geom(evader) # plot obstacles for ob in self.ob_list: ob_radius = self.ob_radius * scale obstacle = rendering.make_circle(ob_radius) obstacle.set_color(0, 0, 0) obstacle.add_attr( rendering.Transform(translation=(ob[0] * scale + origin[0], ob[1] * scale + origin[1]))) self.viewer.add_geom(obstacle) self.pursuit_trans.set_translation( self.get_state()[0] * scale + origin[0], self.get_state()[1] * scale + origin[1]) self.pursuit_trans.set_rotation(self.get_state()[2]) self.evader_trans.set_translation( self.get_goal()[0] * scale + origin[0], self.get_goal()[1] * scale + origin[1]) self.evader_trans.set_rotation(self.get_goal()[2]) return self.viewer.render(return_rgb_array=mode == 'rgb_array') def scan_to_points(self, laser_ranges, min_angle, increment_angle, T_robot): laser_points = [] for laser_index in range(len(laser_ranges)): laser_range = laser_ranges[laser_index] laser_angle = increment_angle * laser_index + min_angle # only plot reflected if laser_range < self.pursuitor_model.laser_max_range - 2: laser_points.append([ laser_range * cos(laser_angle), laser_range * sin(laser_angle) ]) laser_points = np.array(laser_points) for i, laser_point in enumerate(laser_points): laser_point_tmp = np.matmul(T_robot, np.hstack((laser_point, 1))) laser_points[i] = laser_point_tmp[:2] return laser_points def _get_obs(self): """Returns the observation. """ arr = self.render(mode='rgb_array') arr = imresize(arr, (self.state_height, self.state_width, 3)) return arr def sample_map(self): """ “exteroceptive information”, sample the percept region. :return: sampling points of percept region, with value representing occupancy (0 is non-free, 255 is free) """ # represent obstacle in robot coordinate system T_robot = self.get_TF() ob_r_list = [ np.matmul(T_robot.T, np.hstack((np.array(ob), 1)))[:2] for ob in self.ob_list ] # filter obstacle out of percept region def filter_ob(obstacle_radius, ob_list, percept_region): percept_region_expanded = percept_region + np.array([ -obstacle_radius, obstacle_radius, -obstacle_radius, obstacle_radius ]) def is_in_percept_region(ob): check_ob_center_in = percept_region_expanded[0] <= ob[0] <= percept_region_expanded[1] and \ percept_region_expanded[2] <= ob[1] <= percept_region_expanded[3] return check_ob_center_in filtered_ob_list = list(filter(is_in_percept_region, ob_list)) return filtered_ob_list filtered_ob_list = filter_ob(self.ob_radius, ob_r_list, self.percept_region) sampled_map = np.zeros([self.sample_map_width, self.sample_map_height]) for i in range(self.sample_map_width): for j in range(self.sample_map_height): sample_point = self.percept_sample_map[i][j] dis2ob = np.linalg.norm(np.array(filtered_ob_list) - sample_point, axis=1, keepdims=True) if np.any(dis2ob <= self.ob_radius): sampled_map[i, j] = 0 else: sampled_map[i, j] = 255 return sampled_map def _set_action(self, action): """Applies the given action to the simulation. """ u = [0, 0] if action == 0: u, ltraj = dwa_control(self.get_state(), np.array(self.pursuitor_model.state[3:5]), self.config_dwa, self.get_goal()[:2], self.ob_list) elif action == 1: # u = apf_control(self.confg_apf, self.pursuitor_model.state, self.evader_model.state, self.ob_list) # # # get angle velocity from angle input # u[1] = self.pursuitor_model.rot_to_angle(u[1]) w = pn_control(self.confg_pn, self.get_state(), self.get_goal(), self.last_pur_state, self.last_tar_state) u = [self.max_v, w] self.last_pur_state = self.get_state() self.last_tar_state = self.get_goal() self.pursuitor_model.motion(u, self.dt) self.evader_model.motion(self.target_u, self.dt) assert not self.robot_collision_with_obstacle( self.evader_model.state[:2], self.target_radius, self.ob_list, self.ob_radius) x = self.get_state() self.traj = np.vstack((self.traj, x)) # store state history def _compute_reward(self, observation): # reward = ((-observation[-1] + self.last_obs[-1])/(self.max_v*self.dt)) ** 3 * 10 reward = -observation[-1] / 10.0 if self.collision: reward = -150 if self.catch: reward = 200 return reward def _is_done(self, observations): self.catch = False self.collision = False if np.linalg.norm(self.get_goal()[:2] - self.get_state()[:2] ) <= self.robot_radius + self.target_radius: self.catch = True # # laser_collision = np.array(observations[:self.pursuitor_model.laser_num]) <= self.robot_radius # if laser_collision.any(): # self.collision = True if self.robot_collision_with_obstacle(self.get_state()[:2], self.robot_radius, self.ob_list, self.ob_radius): self.collision = True return self.catch or self.collision def robot_collision_with_obstacle(self, x, x_radius, ob_list, ob_radius): for ob in ob_list: if np.linalg.norm(x - ob) <= x_radius + ob_radius: return True return False def get_goal(self): return np.array(self.evader_model.state) def get_state(self): return np.array(self.pursuitor_model.state) def normlize_angle(self, angle): norm_angle = angle % 2 * math.pi if norm_angle > math.pi: norm_angle -= 2 * math.pi return norm_angle def get_TF(self): theta = self.get_state()[2] transition = self.get_state()[:2] T_robot = np.array([[cos(theta), -sin(theta), transition[0]], [sin(theta), cos(theta), transition[1]], [0, 0, 1]]) return T_robot def init_env_from_list(self, pursuit_init_num, evader_init_num): if pursuit_init_num == 0: # pursuit env 0 self.init_x = 0.0 self.init_y = 6.0 self.init_yaw = 0.0 elif pursuit_init_num == 1: # pursuit env 1 self.init_x = 8.0 self.init_y = 6.0 self.init_yaw = 0.0 elif pursuit_init_num == 2: # pursuit env 2 self.init_x = 12.0 self.init_y = 8.0 self.init_yaw = 0.0 elif pursuit_init_num == 3: # pursuit env 3 self.init_x = 0.0 self.init_y = 0.0 self.init_yaw = pi / 8.0 # if evader_init_num==0: # # evader env 0 # self.target_init_x = 2.0 # self.target_init_y = 11.0 # self.target_init_yaw = -pi/2.0 # self.target_u = np.array([1.8, 0.0]) if evader_init_num == 1: # evader env 1 self.target_init_x = 0.0 self.target_init_y = 10.0 self.target_init_yaw = -pi / 2.0 self.target_u = np.array([1.8, -pi / 5 * 2.0]) elif evader_init_num == 2: # evader env 2 self.target_init_x = 0.0 self.target_init_y = 9.0 self.target_init_yaw = pi - 0.3 self.target_u = np.array([1.8, 0.66])
def forward_kinematics(robot_name=None, read_state_string=None): assert robot_name is not None or read_state_string is not None if read_state_string is None: read_state_string = RealRobotClient().read_state() assert 'nan' not in read_state_string r = RobotModel(Painter()) angles = ReadStateParser(read_state_string).get_all_joint_radians() r.draw_neck_and_head(neck_transversal_radians=angles['neck_transversal'], neck_lateral_radians=angles['neck_lateral']) r.draw_left_arm( left_shoulder_lateral_radians=angles['left_shoulder_lateral'], left_shoulder_frontal_radians=angles['left_shoulder_frontal'], left_elbow_lateral_radians=angles['left_elbow_lateral']) r.draw_right_arm( right_shoulder_lateral_radians=angles['right_shoulder_lateral'], right_shoulder_frontal_radians=angles['right_shoulder_frontal'], right_elbow_lateral_radians=angles['right_elbow_lateral']) r.draw_left_lower_limp( left_hip_transversal_radians=angles['left_hip_transversal'], left_hip_frontal_radians=angles['left_hip_frontal'], left_hip_lateral_radians=angles['left_hip_lateral'], left_knee_lateral_radians=angles['left_knee_lateral'], left_ankle_lateral_radians=angles['left_ankle_lateral'], left_ankle_frontal_radians=angles['left_ankle_frontal']) r.draw_right_lower_limp( right_hip_transversal_radians=angles['right_hip_transversal'], right_hip_frontal_radians=angles['right_hip_frontal'], right_hip_lateral_radians=angles['right_hip_lateral'], right_knee_lateral_radians=angles['right_knee_lateral'], right_ankle_lateral_radians=angles['right_ankle_lateral'], right_ankle_frontal_radians=angles['right_ankle_frontal']) r.draw_torso() r.draw_coordinates( ) # draw coordinates lastly because we want them on top layer r.show()
length * math.cos(yaw), length * math.sin(yaw), head_length=width, head_width=width) plt.plot(x, y) from robot_model import RobotModel if __name__ == '__main__': gx = 10.0 gy = 12.0 print(__file__ + " start!!") # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] # x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0]) motionModel = RobotModel(2, 0, math.pi / 3 * 2, 0.7, math.pi / 3 * 5, 0.0, 0.0, math.pi / 8.0, 0.0, 0.0) x = motionModel.state # goal position [x(m), y(m)] target_model = RobotModel(2, 0, math.pi / 3 * 2, 0.7, math.pi / 3 * 5, gx, gy, -math.pi / 2.0, 0.0, 0.0) goal = target_model.state[:2] # obstacles [x(m) y(m), ....] ob = np.array([[-1, -1], [0, 2], [4.0, 2.0], [5.0, 4.0], [5.0, 5.0], [5.0, 6.0], [5.0, 9.0], [8.0, 9.0], [7.0, 9.0], [12.0, 12.0], [4.0, 12.0], [3.5, 15.8], [12.1, 17.0], [7.16, 14.6], [8.6, 13.0]]) u = np.array([0.0, 0.0]) config = ConfigDWA(2, 0, math.pi / 3 * 2, 0.7, math.pi / 3 * 5) traj = np.array(x)
__author__ = 'zhao' from inverse_kinematics.lower_limp import InverseKinematicsLeg from util.painter import Painter from util.point import Point from robot_model import RobotModel is_left = False # target_position = Point((-8.0, 7.25, -44.699999999999989)) target_position = Point((-8.0, 7.25, -44.7)) painter = Painter() robot_model = RobotModel(painter) r_hip_transversal_radians = 0 r_hip_frontal_radians, r_hip_lateral_radians, r_knee_lateral_radians, r_ankle_lateral_radians, r_ankle_frontal_radians\ = InverseKinematicsLeg(is_left=is_left, hip_transversal_radians=r_hip_transversal_radians, target_position=target_position, robot_model=robot_model, painter=painter).get_angles() # r_hip_frontal_radians, r_hip_lateral_radians, r_knee_lateral_radians, r_ankle_lateral_radians, r_ankle_frontal_radians = [0] * 5 # painter.draw_point(target_position) robot_model.draw_neck_and_head() robot_model.draw_torso() robot_model.draw_left_arm() robot_model.draw_right_arm() robot_model.draw_left_lower_limp() robot_model.draw_right_lower_limp(right_hip_transversal_radians=r_hip_transversal_radians, right_hip_frontal_radians=r_hip_frontal_radians, right_hip_lateral_radians=r_hip_lateral_radians, right_knee_lateral_radians=r_knee_lateral_radians, right_ankle_lateral_radians=r_ankle_lateral_radians, right_ankle_frontal_radians=r_ankle_frontal_radians) robot_model.show()
angles[angle_index] = robot_server_message.joint_angle def start_robot_server_thread(): robot_server_host = "localhost" robot_server_port = 1313 udp_server = socketserver.UDPServer((robot_server_host, robot_server_port), VirtualRobotRequestHandler) Thread(target=udp_server.serve_forever).start() if __name__ == '__main__': start_robot_server_thread() painter = Painter(azimuth=-30, elevation=60) robot_model = RobotModel(painter) # robot_model.draw_initial_pose() robot_model.draw_neck_and_head() RobotMemory.left_arm_joints = robot_model.draw_left_arm() RobotMemory.right_arm_joints = robot_model.draw_right_arm() RobotMemory.torso_corner_positions = robot_model.draw_torso() def func(i): # Note that draw_left/right_lower_limp() is called two times. # The param `draw` is false for the first time, which makes the func only do math work. # If we want super-high performance, refactor those 2 funcs. RobotMemory.left_lower_limp_joints = robot_model.draw_left_lower_limp(*RobotMemory.left_lower_limp_angles, draw=False) RobotMemory.right_lower_limp_joints = robot_model.draw_right_lower_limp(*RobotMemory.right_lower_limp_angles, draw=False)
def start_robot_server_thread(): robot_server_host = "localhost" robot_server_port = 1313 udp_server = socketserver.UDPServer((robot_server_host, robot_server_port), VirtualRobotRequestHandler) Thread(target=udp_server.serve_forever).start() if __name__ == '__main__': start_robot_server_thread() painter = Painter(azimuth=-30, elevation=60) robot_model = RobotModel(painter) # robot_model.draw_initial_pose() robot_model.draw_neck_and_head() RobotMemory.left_arm_joints = robot_model.draw_left_arm() RobotMemory.right_arm_joints = robot_model.draw_right_arm() RobotMemory.torso_corner_positions = robot_model.draw_torso() def func(i): # Note that draw_left/right_lower_limp() is called two times. # The param `draw` is false for the first time, which makes the func only do math work. # If we want super-high performance, refactor those 2 funcs. RobotMemory.left_lower_limp_joints = robot_model.draw_left_lower_limp( *RobotMemory.left_lower_limp_angles, draw=False) RobotMemory.right_lower_limp_joints = robot_model.draw_right_lower_limp(