def attempt_upwind(self): # control angle: groundspeed = self.body.getLinearVel() airspeed = np.array(groundspeed) - np.array(wind) airspeed_mag = np.linalg.norm(airspeed) # calculate orientation of velocity vel_ori = np.arctan2(self.body.getLinearVel()[1],self.body.getLinearVel()[0]) body_ori = self.get_body_orientation() body_ori_dot = self.body.getAngularVel()[2] gain = -1*airspeed_mag damping = 200 visual_slip = flymath.fix_angular_rollover(vel_ori-body_ori) self.visual_slip_lowpassed += 0.9*(visual_slip - self.visual_slip_lowpassed) control = gain*self.visual_slip_lowpassed - damping*body_ori_dot self.body.addRelTorque((0,0,control)) # control vel vel = np.linalg.norm(np.array(self.body.getLinearVel())) body_ori_vec = np.array([np.cos(self.get_body_orientation()), np.sin(self.get_body_orientation()), 0]) vel_sign = np.sign(np.dot(np.array(self.body.getLinearVel()), body_ori_vec)) self.thrust = -300*(vel_sign*vel- (self.vel_desired+self.vel_desired_addition)) groundspeed = self.body.getLinearVel() airspeed = np.array(groundspeed) - np.array(wind) self.airspeed_upwind = np.linalg.norm(airspeed) print self.vel_desired_addition
def get_body_orientation(self): # 2D sign = np.sign(self.body.getQuaternion()[3]) if sign == 0: sign = 1 woundup = (np.arccos(self.body.getQuaternion()[0])*2)*sign+np.pi/2. return flymath.fix_angular_rollover(woundup)
def hover(self): groundspeed = self.body.getLinearVel() airspeed = np.array(groundspeed) - np.array(wind) airspeed_mag = np.linalg.norm(airspeed) # calculate orientation of velocity vel_ori = np.arctan2(self.body.getLinearVel()[1],self.body.getLinearVel()[0]) body_ori = self.get_body_orientation() body_ori_dot = self.body.getAngularVel()[2] gain = -10*airspeed_mag damping = 50 visual_slip = flymath.fix_angular_rollover(vel_ori-body_ori) self.visual_slip_lowpassed += 0.9*(visual_slip - self.visual_slip_lowpassed) control = gain*self.visual_slip_lowpassed - damping*body_ori_dot self.body.addRelTorque((0,0,control))
def apply_forces(self, wind=(0,0,0)): if self.joy is None: return thrust = self.joystick_thrust + self.thrust slip = self.joystick_slip + self.slip yaw = self.joystick_yaw + self.yaw antenna_left_vel = self.joystick_antenna_left_vel + self.antenna_left_vel antenna_right_vel = self.joystick_antenna_right_vel + self.antenna_right_vel ori = self.get_body_orientation() force = (np.cos(ori)*thrust, np.sin(ori)*thrust, 0) # Add random slip angle rand = self.rand_slip_offset #0#(np.random.random()*2 -1)*np.pi/2. self.slipangle = 0#np.pi/2.*0.3#np.abs(rand) #self.rand_slip_offset # Get airspeed (for gain scheduling) groundspeed = self.body.getLinearVel() airspeed = np.array(groundspeed) - np.array(wind) airspeed_mag = np.linalg.norm(airspeed) # Calculate antenna controls force_drag_l, force_drag_r = self.apply_aero_arista(wind=wind, apply_forces=False) antenna_difference = -100000*(force_drag_l + force_drag_r) - 1*self.antenna_difference_super_lowpassed self.antenna_difference_lowpassed += 1*(antenna_difference - self.antenna_difference_lowpassed) #self.antenna_difference_super_lowpassed += .9*(antenna_difference - self.antenna_difference_super_lowpassed) #control = -0.5*airspeed_mag*self.antenna_difference_lowpassed # gain scheduled proportional controller # set control limits #if np.abs(control) > np.pi/2.: # control = np.sign(control)*np.pi/2. #antenna_left_vel += -2000*antenna_difference #antenna_right_vel += -2000*antenna_difference #print control # Apply forces control = 0 self.body.addRelForce((thrust*np.sin(flymath.fix_angular_rollover(self.slipangle+control)),thrust*np.abs(np.cos(flymath.fix_angular_rollover(self.slipangle+control))),0)) self.body.addRelForce((slip,0,0)) self.body.addRelTorque((0,0,yaw)) antenna_left_vel = 10000000*force_drag_l antenna_right_vel = 10000000*force_drag_r vel_ori = np.arctan2(self.body.getLinearVel()[1],self.body.getLinearVel()[0]) #print self.joint_antenna_l_head.getAngle() + config['antenna_initial_angle'], self.get_arista_orientation_l() - self.get_body_orientation()# - vel_ori self.joint_antenna_l_head.setParam(ode.ParamVel, antenna_left_vel) self.joint_antenna_r_head.setParam(ode.ParamVel, antenna_right_vel) # Visual controllers: if self.joy.buttons[12]: self.attempt_upwind() if self.joy.buttons[13]: self.attempt_cast(-1) if self.joy.buttons[15]: self.attempt_cast(1) ## Control locked hinges # control head self.joint_head_body.addTorque(-1000*self.joint_head_body.getAngle()) # control arista self.joint_arista_antenna_l.addTorque(-100*self.joint_arista_antenna_l.getAngle()) self.joint_arista_antenna_r.addTorque(-100*self.joint_arista_antenna_r.getAngle()) # Apply aerodynamics self.apply_aero_body(wind=wind) self.apply_aero_arista(wind=wind)