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
'''
class Dog(Supervisor):
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 refresh(self):
'''
Quardpueds robot refreshes state
'''
#Leg part
self.Leg_lf.refresh()
self.Leg_lb.refresh()
self.Leg_rf.refresh()
self.Leg_rb.refresh()
self.touchstate = np.array([
self.Leg_lf.touchstate, self.Leg_rf.touchstate,
self.Leg_lb.touchstate, self.Leg_rb.touchstate
])
inertial = self.imu.getRollPitchYaw() #imu values, we correct here
self.attitude.roll = -inertial[0] # x-axis
self.attitude.pitch = -inertial[1] # z-axis
self.attitude.yaw = inertial[2] # y-axis
pos = self.gps.getValues() #gps values, , we correct here
self.gps_pos.x = -pos[0]
self.gps_pos.y = pos[1]
self.gps_pos.z = -pos[2]
#CoM coordinates in our frame
self.gps_pos_now.x = self.gps_pos.x - self.gps_pos_bias[0]
self.gps_pos_now.y = self.gps_pos.y - self.gps_pos_bias[1]
self.gps_pos_now.z = self.gps_pos.z - self.gps_pos_bias[2]
self.leg_pos_now = np.array([
[
robot.Leg_lf.position.x, robot.Leg_lf.position.y,
robot.Leg_lf.position.z
],
[
robot.Leg_rf.position.x, robot.Leg_rf.position.y,
robot.Leg_rf.position.z
],
[
robot.Leg_lb.position.x, robot.Leg_lb.position.y,
robot.Leg_lb.position.z
],
[
robot.Leg_rb.position.x, robot.Leg_rb.position.y,
robot.Leg_rb.position.z
],
]) #leg position
def forceset(self, force, reaction_force):
'''
Quardpueds torques set
'''
forceclass = Force()
if not (reaction_force == 1 or -1):
raise ValueError('reaction_force must be 1 or -1')
if not np.shape(np.array(force)) == (4, 3):
raise ValueError('force shape must be 4x3')
force_new = []
for i in range(4):
m_force = np.mat([[force[i][0]], [force[i][1]], [force[i][2]]])
m_R = np.mat(
R_Matrix_inv(self.attitude.roll, self.attitude.yaw,
self.attitude.pitch)) #RT
force_new.append(np.squeeze(np.array(m_R * m_force)))
forceclass.x, forceclass.y, forceclass.z = reaction_force * force_new[
0][0], reaction_force * force_new[0][
1], reaction_force * force_new[0][2]
self.Leg_lf.set_force(forceclass)
forceclass.x, forceclass.y, forceclass.z = reaction_force * force_new[
2][0], reaction_force * force_new[2][
1], reaction_force * force_new[2][2]
self.Leg_lb.set_force(forceclass)
forceclass.x, forceclass.y, forceclass.z = reaction_force * force_new[
1][0], reaction_force * force_new[1][
1], reaction_force * force_new[1][2]
self.Leg_rf.set_force(forceclass)
forceclass.x, forceclass.y, forceclass.z = reaction_force * force_new[
3][0], reaction_force * force_new[3][
1], reaction_force * force_new[3][2]
self.Leg_rb.set_force(forceclass)
def forceset_single(self, id, force, reaction_force, *args):
'''
Quardpueds single torque set
'''
forceclass = Force()
if not (reaction_force == 1 or -1):
raise ValueError('reaction_force must be 1 or -1')
m_force = np.mat([[force[0]], [force[1]], [force[2]]])
if args[0] == 'inv': #Whether to transform the coordinate
m_R = np.mat(
R_Matrix_inv(self.attitude.roll, self.attitude.yaw,
self.attitude.pitch))
else:
m_R = np.mat(
R_Matrix(self.attitude.roll, self.attitude.yaw,
self.attitude.pitch))
force_new = np.squeeze(np.array(m_R * m_force))
forceclass.x, forceclass.y, forceclass.z = reaction_force * force_new[
0], reaction_force * force_new[1], reaction_force * force_new[2]
if id == 0:
self.Leg_lf.set_force(forceclass)
elif id == 2:
self.Leg_lb.set_force(forceclass)
elif id == 1:
self.Leg_rf.set_force(forceclass)
elif id == 3:
self.Leg_rb.set_force(forceclass)
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_)
def stand_up(self):
'''
Quardpueds standing initialization function
'''
pos = Pos(0, -self.leg_exp_len, 0)
self.Leg_lf.set_position(pos)
self.Leg_lb.set_position(pos)
self.Leg_rf.set_position(pos)
self.Leg_rb.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 gps_pos_bias_refresh(self):
'''
gps bias
'''
self.gps_pos_bias[0], self.gps_pos_bias[1], self.gps_pos_bias[
2] = self.gps_pos.x, 0, self.gps_pos.z
class Biped(Supervisor):
def __init__(self):
super(Biped, 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)
self.Leg_rf = Leg(module_name['rf'], 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, LF RF respectively
self.leg_loc = np.array([[0,self.Virtual_Height,0.065],[0,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_desire_touch = [1,1]
# Dynamic function
def refresh(self):
'''
Bipeds robot refreshes state
'''
#Leg part
self.Leg_lf.refresh()
self.Leg_rf.refresh()
self.touchstate = np.array([self.Leg_lf.touchstate,self.Leg_rf.touchstate])
inertial = self.imu.getRollPitchYaw() #imu values, we correct here
self.attitude.roll = -inertial[0] # x-axis
self.attitude.pitch = -inertial[1] # z-axis
self.attitude.yaw = inertial[2] # y-axis
pos = self.gps.getValues() #gps values, , we correct here
self.gps_pos.x = -pos[0]
self.gps_pos.y = pos[1]
self.gps_pos.z = -pos[2]
#print(pos)
#CoM coordinates in our frame
self.gps_pos_now.x = self.gps_pos.x - self.gps_pos_bias[0]
self.gps_pos_now.y = self.gps_pos.y - self.gps_pos_bias[1]
self.gps_pos_now.z = self.gps_pos.z - self.gps_pos_bias[2]
self.leg_pos_now = np.array([[robot.Leg_lf.position.x, robot.Leg_lf.position.y, robot.Leg_lf.position.z],
[robot.Leg_rf.position.x, robot.Leg_rf.position.y, robot.Leg_rf.position.z],
]) #leg position
def forceset(self, force,reaction_force):
'''
Bipeds torques set
'''
forceclass = Force()
if not (reaction_force == 1 or -1):
raise ValueError('reaction_force must be 1 or -1')
if not np.shape(np.array(force)) == (2,3):
raise ValueError('force shape must be 2x3')
force_new = []
for i in range(2):
m_force = np.mat([[force[i][0]],[force[i][1]],[force[i][2]]])
m_R = np.mat(R_Matrix_inv(self.attitude.roll,self.attitude.yaw,self.attitude.pitch)) #腿部坐标系需要单独进行转换
force_new.append(np.squeeze(np.array(m_R * m_force)))
forceclass.x,forceclass.y,forceclass.z = reaction_force * force_new[0][0], reaction_force * force_new[0][1], reaction_force * force_new[0][2]
self.Leg_lf.set_force(forceclass)
forceclass.x ,forceclass.y,forceclass.z = reaction_force * force_new[1][0], reaction_force * force_new[1][1], reaction_force * force_new[1][2]
self.Leg_rf.set_force(forceclass)
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