def compute_formation_positions(self): # (dX, dY) sign for each position in tier tier_poses_sign = [(-1, -1), (-1, 1), (1, 1), (1, -1)] for i in range(self._n_agents): if rospy.is_shutdown(): break # Initialize agent formation goal self._agents_goals[i] = Position() # Find tier information # i=0 -> tier=0, i=1,2,3,4 -> tier = 1, i=5,6,7,8 -> tier = 2 ... tier_num = ceil(i / 4.0) tier_pos = i % 4 # Position in the tier square_length = 2 * sin(self.theta) * self.tier_dist * tier_num # Compute formation position z_dist = -1 * tier_num * self.tier_dist x_dist = tier_poses_sign[tier_pos][0] * square_length / 2 y_dist = tier_poses_sign[tier_pos][1] * square_length / 2 # information from center center_dist, theta, center_height = compute_info_from_center( [x_dist, y_dist, z_dist]) self._center_dist[i] = center_dist self._angle[i] = theta self._center_height[i] = center_height
def compute_formation_positions(self): for i in range(self._n_agents): if rospy.is_shutdown(): break # Initialize agent formation goal self._agents_goals[i] = Position() # Compute formation position z_dist = 0 if i == 0: x_dist = 0 y_dist = 0 else: angle = i * self.angle_between_agents x_dist = self._scale * cos(angle) y_dist = self._scale * sin(angle) # Compute information from center center_dist, theta, center_height = compute_info_from_center( [x_dist, y_dist, z_dist]) self._center_dist[i] = center_dist self._angle[i] = theta self._center_height[i] = center_height return self._agents_goals
def compute_formation_positions(self): for i in range(self._n_agents): if rospy.is_shutdown(): break # Initialize agent formation goal self._agents_goals[i] = Position() # Compute formation position # TODO: Compute agent position from center # Compute information from center center_dist, theta, center_height = compute_info_from_center( [x_dist, y_dist, z_dist]) self._center_dist[i] = center_dist self._angle[i] = theta self._center_height[i] = center_height return self._agents_goals
def compute_formation_positions(self): center_offset = self._scale/2 for i in range(self._n_agents): if rospy.is_shutdown(): break # Initialize agent formation goal self._agents_goals[i] = Position() # Compute formation position z_dist = 0 x_dist = 0 y_dist = self.agents_dist*i - center_offset # Compute information from center center_dist, theta, center_height = compute_info_from_center([x_dist, y_dist, z_dist]) self._center_dist[i] = center_dist self._angle[i] = theta self._center_height[i] = center_height
def compute_formation_positions(self): for i in range(self._n_agents): if rospy.is_shutdown(): break # Initialize agent formation goal self._agents_goals[i] = Position() # Find row number # i = 0 -> row = 0, i = 1 -> row = 1, i = 2 -> row = 1, i = 3 -> row = 2 ... row_num = ceil(i/2.0) # Find if above or below center sign = -1 if i % 2 == 0: sign = 1 x_dist = -self.dist*row_num * cos(self.theta/2) y_dist = self.dist*row_num * sin(self.theta/2) * sign center_dist, theta, center_height = compute_info_from_center([x_dist, y_dist, 0]) self._center_dist[i] = center_dist self._angle[i] = theta self._center_height[i] = center_height