def __init__(self): super(One, self).__init__() self.attitude = Attitude() #body attitude roll pitch yaw self.imu = InertialUnit(module_name['imu']) self.imu.enable(2) self.gps = GPS(module_name['gps']) self.gps.enable(2) self.gps_pos_now = Pos() #my coordinates self.gps_pos = Pos() #gps raw coordinates self.gps_pos_bias = [0., 0., 0.] #temporary variables self.Leg_lf = Leg(module_name['lf'], module_info) #**************Robot attributes************** self.Swing_Lenth = module_info['swing_lenth'] self.Thign_Lenth = module_info['thign_lenth'] self.Calf_Lenth = module_info['calf_lenth'] self.Virtual_Height = module_info['virtual_height'] self.Body_Height = module_info['body_height'] #Robot ankle coordinates in body frame self.leg_loc = np.array([[0, self.Virtual_Height, 0.0]]) self.mg_N = 8.22 * 9.81 #Robot gravity self.leg_desire_touch = [1] #1 means leg is touchdown, 0 is not self.leg_exp_len = 0.2 #Leg extension self.body_exp_y = self.leg_exp_len + self.Body_Height #CoM coordinates
def __init__(self): super(Dog, self).__init__() self.attitude = Attitude() #body attitude roll pitch yaw self.imu = InertialUnit(module_name['imu']) self.imu.enable(2) #500Hz self.gps = GPS(module_name['gps']) self.gps.enable(2) #500Hz self.gps_pos_now = Pos() #my coordinates self.gps_pos = Pos() #gps raw coordinates self.gps_pos_bias = [0, 0, 0] #temporary variables #Leg initialization self.Leg_lf = Leg(module_name['lf'], module_info) self.Leg_lb = Leg(module_name['lb'], module_info) self.Leg_rf = Leg(module_name['rf'], module_info) self.Leg_rb = Leg(module_name['rb'], module_info) #**************Robot attributes************** self.Swing_Lenth = module_info['swing_lenth'] self.Thign_Lenth = module_info['thign_lenth'] self.Calf_Lenth = module_info['calf_lenth'] self.Body_Width = module_info['body_width'] self.Body_Length = module_info['body_length'] self.Virtual_Height = module_info['virtual_height'] self.Body_Height = module_info['body_height'] #Robot ankle coordinates in body frame, LF RF LB RB respectively self.leg_loc = np.array([[-0.13, self.Virtual_Height, 0.065], [-0.13, self.Virtual_Height, -0.065], [0.13, self.Virtual_Height, 0.065], [0.13, self.Virtual_Height, -0.065]]) self.mg_N = 16.44 * 9.81 #Robot gravity self.leg_exp_len = 0.2 #Leg extension self.body_exp_y = self.leg_exp_len + self.Body_Height #CoM coordinates #**************State attributes************** #1 means leg is touchdown, 0 is not self.leg_reset = [1, 1, 1, 1] # Dynamic function '''
def __init__(self, module_name, module_info): self.module_info = module_info self.speed = Speed() #Current speed self.speed_f = Speed() #Desire speed self.position = Pos() #Current position self.position_f = Pos() #Desire position self.torque_f = Torque() #Desire torque self.force = Force() self.force_f = Force() #Desire foot force self.swing_angle = 0 self.thign_angle = 0 self.calf_angle = 0 self.swing_angle_f = 0 #Desire motor position self.thign_angle_f = 0 self.calf_angle_f = 0 self.touchstate = 0 self.swing_motor = Motor(module_name['swing_motor_s']) self.thign_motor = Motor(module_name['thignleg_motor_s']) self.calf_motor = Motor(module_name['calfleg_motor_s']) self.swing_motor.enableTorqueFeedback(2) self.thign_motor.enableTorqueFeedback(2) self.calf_motor.enableTorqueFeedback(2) self.swing_positionsensor = PositionSensor( module_name['swing_positionsensor_s']) self.thign_positionsensor = PositionSensor( module_name['thign_positionsensor_s']) self.calf_positionsensor = PositionSensor( module_name['calf_positionsensor_s']) self.swing_positionsensor.enable(2) self.thign_positionsensor.enable(2) self.calf_positionsensor.enable(2) self.touchsensor = TouchSensor(module_name['foot_touchsensor_s']) self.touchsensor.enable(2)
def stand_up(self): ''' Bipeds standing initialization function ''' pos = Pos(0, -self.leg_exp_len , 0) self.Leg_lf.set_position(pos) self.Leg_rf.set_position(pos) #gps bias self.gps_pos_bias[0], self.gps_pos_bias[1],self.gps_pos_bias[2] = self.gps_pos.x, self.gps_pos.y -self.body_exp_y, self.gps_pos.z
def posset(self, id, pos): ''' Quardpueds leg position set ''' pos_ = Pos(pos[0], pos[1], pos[2]) if id == 0: self.Leg_lf.set_position(pos_) elif id == 2: self.Leg_lb.set_position(pos_) elif id == 1: self.Leg_rf.set_position(pos_) elif id == 3: self.Leg_rb.set_position(pos_)