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)
