def __get_force_reach(self):
ori = self.inputs["in_body_orientation"].astype(np.float32)
pos = self.inputs["in_body_position"].astype(np.float32)
target_pos = self.inputs["in_target_position"].astype(np.float32)
self.target_pos = target_pos
body_euler_yaw = transformations.euler_from_quaternion(ori)[2]
direction = self.target_pos - pos
distance = np.linalg.norm(direction)
dir_world_pos = (- math.cos(body_euler_yaw), -math.sin(body_euler_yaw))
angle = util.angle2D_sign((direction[0], direction[1]), dir_world_pos)
if self.last_angle is None:
self.last_angle = angle
if self.last_angle - angle > math.pi:
anguler_vel_yaw = 0
elif self.last_angle - angle < -math.pi:
anguler_vel_yaw = 0
else:
anguler_vel_yaw = self.last_angle - angle
self.last_angle = angle
if math.fabs(angle) < math.pi / 18.0 or distance < 0.3:
return self.__get_force_straight(self.target_pos)
else:
power = 1.5*angle - 200.0*anguler_vel_yaw
return power, -power
def __get_force_reach(self, input_dat):
ori = input_dat[0]
pos = input_dat[1]
vel = input_dat[2]
self.target_pos = input_dat[3]
body_euler_yaw = transformations.euler_from_quaternion(ori)[2]
direction = self.target_pos - pos
distance = np.linalg.norm(direction)
dir_world_pos = (-math.cos(body_euler_yaw), -math.sin(body_euler_yaw))
angle = util.angle2D_sign((direction[0], direction[1]), dir_world_pos)
if self.last_angle is None:
self.last_angle = angle
if self.last_angle - angle > math.pi:
anguler_vel_yaw = 0
elif self.last_angle - angle < -math.pi:
anguler_vel_yaw = 0
else:
anguler_vel_yaw = self.last_angle - angle
self.last_angle = angle
if math.fabs(angle) < math.pi / 18.0 or distance < 0.3:
return self.__get_force_straight(ori, pos, vel, self.target_pos)
else:
power = 1.5 * angle - 200.0 * anguler_vel_yaw
return (power, -power)
def __get_force_reach(self):
ori = self.inputs["in_body_orientation"].astype(np.float32)
pos = self.inputs["in_body_position"].astype(np.float32)
target_pos = self.inputs["in_target_position"].astype(np.float32)
self.target_pos = target_pos
body_euler_yaw = transformations.euler_from_quaternion(ori)[2]
direction = self.target_pos - pos
distance = np.linalg.norm(direction)
dir_world_pos = (-math.cos(body_euler_yaw), -math.sin(body_euler_yaw))
angle = util.angle2D_sign((direction[0], direction[1]), dir_world_pos)
if self.last_angle is None:
self.last_angle = angle
if self.last_angle - angle > math.pi:
anguler_vel_yaw = 0
elif self.last_angle - angle < -math.pi:
anguler_vel_yaw = 0
else:
anguler_vel_yaw = self.last_angle - angle
self.last_angle = angle
if math.fabs(angle) < math.pi / 18.0 or distance < 0.3:
return self.__get_force_straight(self.target_pos)
else:
power = 1.5 * angle - 200.0 * anguler_vel_yaw
return power, -power
def __get_force_reach(self, input_dat):
ori = input_dat[0]
pos = input_dat[1]
vel = input_dat[2]
self.target_pos = input_dat[3]
body_euler_yaw = transformations.euler_from_quaternion(ori)[2]
direction = self.target_pos - pos
distance = np.linalg.norm(direction)
dir_world_pos = (- math.cos(body_euler_yaw), -math.sin(body_euler_yaw))
angle = util.angle2D_sign((direction[0], direction[1]), dir_world_pos)
if self.last_angle is None:
self.last_angle = angle
if self.last_angle - angle > math.pi:
anguler_vel_yaw = 0
elif self.last_angle - angle < -math.pi:
anguler_vel_yaw = 0
else:
anguler_vel_yaw = self.last_angle - angle
self.last_angle = angle
if math.fabs(angle) < math.pi / 18.0 or distance < 0.3:
return self.__get_force_straight(ori, pos, vel, self.target_pos)
else:
power = 1.5*angle - 200.0*anguler_vel_yaw
return (power, -power)
