def __get_force_straight(self, ori, pos, vel, target_pos):
body_euler_yaw = transformations.euler_from_quaternion(ori)[2]
vel_forward = -util.rotate2D(vel, -body_euler_yaw)[0]
direction = target_pos - pos
distance = util.rotate2D(direction, -body_euler_yaw)[0]
power = 0.15 * distance - 20 * vel_forward
return (power, power)
def __get_force_straight(self, ori, pos, vel, target_pos):
body_euler_yaw = transformations.euler_from_quaternion(ori)[2]
vel_forward = - util.rotate2D(vel, -body_euler_yaw)[0]
direction = target_pos - pos
distance = util.rotate2D(direction, -body_euler_yaw)[0]
power = 0.15*distance - 20*vel_forward
return (power, power)
def __get_force_straight(self, target_pos):
vel = self.inputs["in_body_velocity"].astype(np.float32)
pos = self.inputs["in_body_position"].astype(np.float32)
ori = self.inputs["in_body_orientation"].astype(np.float32)
body_euler_yaw = transformations.euler_from_quaternion(ori)[2]
vel_forward = - util.rotate2D(vel, -body_euler_yaw)[0]
direction = target_pos - pos
distance = util.rotate2D(direction, -body_euler_yaw)[0]
power = 0.15*distance - 20*vel_forward
return power, power
def __get_force_straight(self, target_pos):
vel = self.inputs["in_body_velocity"].astype(np.float32)
pos = self.inputs["in_body_position"].astype(np.float32)
ori = self.inputs["in_body_orientation"].astype(np.float32)
body_euler_yaw = transformations.euler_from_quaternion(ori)[2]
vel_forward = -util.rotate2D(vel, -body_euler_yaw)[0]
direction = target_pos - pos
distance = util.rotate2D(direction, -body_euler_yaw)[0]
power = 0.15 * distance - 20 * vel_forward
return power, power