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_)