def InitSimEnv():
global quadruped
global pybulletDebug
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
pybulletDebug = pybulletDebug()
p.setGravity(0, 0, -9.81)
p.loadURDF("plane.urdf")
init_position = [0, 0, 0.5]
init_orn = [0, 0, 0, 1]
path = rospkg.RosPack().get_path("quadruped_ctrl")
print("current path = " + path)
quadruped = p.loadURDF(path + "/src/quadruped_robot.urdf",
init_position, init_orn, useFixedBase=False)
# quadruped = p.loadURDF("mini_cheetah/mini_cheetah.urdf",
# init_position, init_orn, useFixedBase=False)
p.resetDebugVisualizerCamera(0.2, 45, -30, [1, -1, 1])
p.setRealTimeSimulation(1)
startPos = [0.02, -0.78, 1.74, 0.02, -0.78,
1.74, -0.02, -0.78, 1.74, -0.02, -0.78, 1.74]
footFR_index = 3
footFL_index = 7
footHR_index = 11
footHL_index = 15
for i in range(0, footFR_index):
p.setJointMotorControl2(
quadruped, i, p.POSITION_CONTROL, startPos[i]) # , force = 20)
for i in range(footFR_index + 1, footFL_index):
p.setJointMotorControl2(
quadruped, i, p.POSITION_CONTROL, startPos[i-1]) # , force = 20)
for i in range(footFL_index + 1, footHR_index):
p.setJointMotorControl2(
quadruped, i, p.POSITION_CONTROL, startPos[i-2]) # , force = 20)
for i in range(footHR_index + 1, footHL_index):
p.setJointMotorControl2(
quadruped, i, p.POSITION_CONTROL, startPos[i - 3]) # , force = 20)
paramIds = []
time.sleep(0.5)
for j in range(p.getNumJoints(boxId)):
# p.changeDynamics(boxId, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(boxId, j)
print(info)
jointName = info[1]
jointType = info[2]
jointIds.append(j)
footFR_index = 3
footFL_index = 7
footBR_index = 11
footBL_index = 15
pybulletDebug = pybulletDebug()
robotKinematics = robotKinematics()
joystick = Joystick('/dev/input/event18') #need to specify the event route
arduino = ArduinoSerial('/dev/ttyACM1') #need to specify the serial port
trot = trotGait()
#robot properties
"""initial safe position"""
#angles
targetAngs = np.array([
0,
np.pi / 4,
-np.pi / 2,
0, #BR
0,
np.pi / 4,
-np.pi / 2,
def InitSimEnv():
global quadruped
global pybulletDebug
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
pybulletDebug = pybulletDebug()
p.setGravity(0, 0, -9.81)
p.loadURDF("plane.urdf")
init_position = [0, 0, 0.5]
init_orientation = p.getQuaternionFromEuler(
[math.pi / 2.0, 0, math.pi / 2.0])
quadruped = p.loadURDF("laikago/laikago_toes_limits.urdf",
init_position,
init_orientation,
useFixedBase=True)
# quadruped = p.loadURDF("mini_cheetah/mini_cheetah.urdf",
# init_position, init_orn, useFixedBase=False)
p.resetDebugVisualizerCamera(0.2, 45, -30, [1, -1, 1])
p.setRealTimeSimulation(1)
# startPos = [0.02, -0.78, 1.74, 0.02, -0.78,
# 1.74, -0.02, -0.78, 1.74, -0.02, -0.78, 1.74]
startPos = [[
-0.13469, 0.19749, -0.98728, -0.29301, -0.20471, -1.23984, -0.23355,
0.42011, -1.21791, -0.18938, 0.26441, -0.94834
],
[
-0.14512, 0.26054, -0.95961, -0.29102, -0.36855, -1.18856,
-0.27333, 0.31933, -1.19782, -0.16195, 0.32940, -0.92682
],
[
-0.15306, 0.30261, -0.90729, -0.25868, -0.48532, -1.10858,
-0.30688, 0.20205, -1.13190, -0.13235, 0.37537, -0.87545
],
[
-0.16337, 0.32781, -0.83305, -0.22920, -0.55950, -1.03486,
-0.33973, 0.07726, -1.03591, -0.10455, 0.40886, -0.80869
],
[
-0.16965, 0.34171, -0.74659, -0.23311, -0.60282, -0.94829,
-0.36301, -0.03726, -0.92726, -0.07940, 0.43395, -0.73493
],
[
-0.17625, 0.35017, -0.65978, -0.19650, -0.61928, -0.86229,
-0.37492, -0.13853, -0.82065, -0.06165, 0.46502, -0.67749
],
[
-0.17989, 0.35836, -0.58379, -0.11653, -0.58522, -0.80736,
-0.37301, -0.22084, -0.73131, -0.05417, 0.51236, -0.66087
],
[
-0.18044, 0.36833, -0.54932, -0.08977, -0.53380, -0.76223,
-0.36188, -0.28908, -0.66566, -0.05688, 0.56185, -0.67457
],
[
-0.17787, 0.39126, -0.61386, -0.09028, -0.47221, -0.71330,
-0.33558, -0.32496, -0.62603, -0.06434, 0.61170, -0.71491
],
[
-0.16933, 0.42446, -0.72625, -0.08722, -0.39813, -0.70583,
-0.30456, -0.33030, -0.61354, -0.07068, 0.66141, -0.78657
],
[
-0.17410, 0.44456, -0.86169, -0.08785, -0.30924, -0.75960,
-0.27382, -0.31084, -0.62046, -0.07733, 0.69563, -0.87640
],
[
-0.18090, 0.44405, -1.01111, -0.09562, -0.21583, -0.83420,
-0.26601, -0.24525, -0.67550, -0.08763, 0.71085, -0.97991
],
[
-0.19220, 0.40517, -1.15108, -0.10542, -0.11626, -0.90293,
-0.27122, -0.14297, -0.76737, -0.10005, 0.70747, -1.08610
],
[
-0.20451, 0.30068, -1.25100, -0.12191, -0.01085, -0.96244,
-0.27744, -0.03505, -0.84941, -0.11237, 0.67923, -1.17895
],
[
-0.21398, 0.14288, -1.31734, -0.13083, 0.09313, -1.00631,
-0.27792, 0.05942, -0.90905, -0.12975, 0.62727, -1.25950
],
[
-0.20933, -0.03553, -1.31434, -0.13410, 0.18246, -1.01992,
-0.26713, 0.15417, -0.95626, -0.15533, 0.55131, -1.31573
],
[
-0.18514, -0.20865, -1.24941, -0.13137, 0.24918, -1.00930,
-0.24675, 0.23442, -0.97250, -0.18472, 0.44756, -1.32951
],
[
-0.15200, -0.35830, -1.14094, -0.12812, 0.29595, -0.98447,
-0.22266, 0.30016, -0.96332, -0.21647, 0.32512, -1.29471
],
[
-0.10030, -0.47457, -0.99850, -0.12894, 0.32647, -0.93602,
-0.19455, 0.35509, -0.93407, -0.24689, 0.19552, -1.21388
],
[
-0.07595, -0.54361, -0.90392, -0.13124, 0.34496, -0.86624,
-0.16334, 0.39909, -0.89050, -0.27480, 0.06162, -1.09754
],
[
-0.07796, -0.59147, -0.83841, -0.13472, 0.35335, -0.78171,
-0.13330, 0.43503, -0.83421, -0.29635, -0.06430, -0.96968
],
[
-0.02444, -0.57695, -0.76569, -0.14084, 0.35431, -0.68742,
-0.11056, 0.45942, -0.76450, -0.31035, -0.17816, -0.84233
],
[
0.03306, -0.51857, -0.74178, -0.14370, 0.34841, -0.59691,
-0.09653, 0.48009, -0.70593, -0.31628, -0.27009, -0.73351
],
[
0.03693, -0.45715, -0.71912, -0.14406, 0.33574, -0.54469,
-0.09372, 0.50782, -0.68224, -0.31207, -0.34200, -0.66205
],
[
0.02213, -0.38793, -0.73939, -0.14644, 0.34990, -0.58431,
-0.10378, 0.53688, -0.68518, -0.29773, -0.36876, -0.64115
],
[
-0.00440, -0.30834, -0.79874, -0.15446, 0.38900, -0.69914,
-0.12304, 0.56939, -0.71815, -0.27581, -0.36458, -0.64736
],
[
-0.01870, -0.22990, -0.84639, -0.17108, 0.43025, -0.84662,
-0.14480, 0.60458, -0.77773, -0.24323, -0.34274, -0.65512
],
[
-0.03190, -0.13705, -0.89790, -0.19256, 0.44180, -1.00410,
-0.16929, 0.62965, -0.84436, -0.21962, -0.29603, -0.68556
],
[
-0.05247, -0.04078, -0.94286, -0.22764, 0.38617, -1.12246,
-0.19891, 0.64651, -0.92152, -0.21120, -0.20773, -0.75772
],
[
-0.06905, 0.04676, -0.96850, -0.26272, 0.27126, -1.20372,
-0.23094, 0.65590, -1.00493, -0.20474, -0.09638, -0.84458
],
[
-0.08727, 0.13061, -0.98216, -0.30963, 0.11052, -1.24698,
-0.26064, 0.64843, -1.08140, -0.19889, 0.00708, -0.92231
],
[
-0.09822, 0.20722, -0.98374, -0.34176, -0.06244, -1.25238,
-0.28866, 0.62110, -1.14684, -0.18915, 0.09785, -0.97609
],
[
-0.10972, 0.27104, -0.96950, -0.35302, -0.23058, -1.21682,
-0.31460, 0.57537, -1.20075, -0.16983, 0.18096, -1.01378
]]
footFR_index = 3
footFL_index = 7
footHR_index = 11
footHL_index = 15
while 1:
for ii in range(33):
# print(ii)
for i in range(0, footFR_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
startPos[ii][i]) # , force = 20)
for i in range(footFR_index + 1, footFL_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
startPos[ii][i - 1]) # , force = 20)
for i in range(footFL_index + 1, footHR_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
startPos[ii][i - 2]) # , force = 20)
for i in range(footHR_index + 1, footHL_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
startPos[ii][i - 3]) # , force = 20)
time.sleep(0.05)
pos = p.getLinkState(quadruped, 3)
print([pos[0][0], pos[0][1], pos[0][2]])
def InitSimEnv():
global quadruped
global pybulletDebug
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
pybulletDebug = pybulletDebug()
p.setGravity(0, 0, -9.81)
p.loadURDF("plane.urdf")
init_position = [0, 0, 0.5]
init_orn = [0, 0, 0, 1]
path = rospkg.RosPack().get_path("quadruped_ctrl")
print("current path = " + path)
quadruped = p.loadURDF(path + "/src/quadruped_robot.urdf",
init_position,
init_orn,
useFixedBase=True)
# quadruped = p.loadURDF("mini_cheetah/mini_cheetah.urdf",
# init_position, init_orn, useFixedBase=False)
p.resetDebugVisualizerCamera(0.2, 45, -30, [1, -1, 1])
p.setRealTimeSimulation(1)
# startPos = [0.02, -0.78, 1.74, 0.02, -0.78,
# 1.74, -0.02, -0.78, 1.74, -0.02, -0.78, 1.74]
startPos = [[
0.18964, -0.73940, 1.69547, -0.37464, -0.29397, 1.96607, 0.29970,
-0.95008, 1.94149, -0.24429, -0.81545, 1.66544
],
[
0.19949, -0.80721, 1.66951, -0.37832, -0.12916, 1.91088,
0.33957, -0.85016, 1.92731, -0.21613, -0.88191, 1.63938
],
[
0.20642, -0.85550, 1.61857, -0.34610, -0.01919, 1.82495,
0.37164, -0.73796, 1.86622, -0.18533, -0.93072, 1.58316
],
[
0.21550, -0.88837, 1.54637, -0.31481, 0.04758, 1.74697,
0.40253, -0.62099, 1.77531, -0.15635, -0.96748, 1.51184
],
[
0.22052, -0.91000, 1.46168, -0.31476, 0.08178, 1.66104,
0.42368, -0.51515, 1.67112, -0.13026, -0.99580, 1.43397
],
[
0.22615, -0.92584, 1.37707, -0.27118, 0.08824, 1.57191,
0.43360, -0.42188, 1.56801, -0.11232, -1.02931, 1.37358
],
[
0.22917, -0.94010, 1.30293, -0.18317, 0.04696, 1.50833,
0.42996, -0.34557, 1.48027, -0.10574, -1.07751, 1.35554
],
[
0.22962, -0.95276, 1.26910, -0.15075, -0.00975, 1.46059,
0.41762, -0.28079, 1.41427, -0.10993, -1.12702, 1.36941
],
[
0.22800, -0.97116, 1.33172, -0.14652, -0.07690, 1.41282,
0.38997, -0.24627, 1.37125, -0.11927, -1.17571, 1.41057
],
[
0.22118, -0.99583, 1.44078, -0.14077, -0.15305, 1.40554,
0.35776, -0.24047, 1.35384, -0.12789, -1.22201, 1.48284
],
[
0.22850, -1.00623, 1.57583, -0.14100, -0.23939, 1.45947,
0.32602, -0.25845, 1.35541, -0.13672, -1.25113, 1.57357
],
[
0.23876, -0.99325, 1.72599, -0.14943, -0.32874, 1.53531,
0.31844, -0.32069, 1.40807, -0.14916, -1.25980, 1.67890
],
[
0.25440, -0.93976, 1.86820, -0.15995, -0.42477, 1.60576,
0.32497, -0.41793, 1.49933, -0.16381, -1.24861, 1.78763
],
[
0.27125, -0.82003, 1.97062, -0.17727, -0.52778, 1.66814,
0.33259, -0.52184, 1.58147, -0.17812, -1.21201, 1.88332
],
[
0.28657, -0.64624, 2.03815, -0.18683, -0.63046, 1.71369,
0.33416, -0.61365, 1.64069, -0.19756, -1.15141, 1.96764
],
[
0.28634, -0.46001, 2.03275, -0.19013, -0.72083, 1.72798,
0.32412, -0.70626, 1.68583, -0.22502, -1.06808, 2.02866
],
[
0.26361, -0.28961, 1.96213, -0.18694, -0.79000, 1.71696,
0.30363, -0.78626, 1.69870, -0.25504, -0.96059, 2.04712
],
[
0.22901, -0.14991, 1.84738, -0.18303, -0.83992, 1.69156,
0.27888, -0.85325, 1.68560, -0.28578, -0.83993, 2.01648
],
[
0.17246, -0.04759, 1.69747, -0.18277, -0.87546, 1.64321,
0.24977, -0.91023, 1.65176, -0.31368, -0.71754, 1.93918
],
[
0.14546, 0.01343, 1.59947, -0.18368, -0.90037, 1.57393,
0.21744, -0.95641, 1.60309, -0.33834, -0.59423, 1.82637
],
[
0.14625, 0.05630, 1.53407, -0.18574, -0.91589, 1.49039,
0.18635, -0.99487, 1.54188, -0.35692, -0.47916, 1.70202
],
[
0.08696, 0.03533, 1.45460, -0.19057, -0.92447, 1.39800,
0.16265, -1.02268, 1.46853, -0.36842, -0.37531, 1.57808
],
[
0.02524, -0.02486, 1.42246, -0.19235, -0.92522, 1.30923,
0.14818, -1.04633, 1.40775, -0.37249, -0.29138, 1.47207
],
[
0.01809, -0.08829, 1.39887, -0.19203, -0.91603, 1.25805,
0.14576, -1.07576, 1.38352, -0.36733, -0.22371, 1.40124
],
[
0.03155, -0.15717, 1.42138, -0.19504, -0.92790, 1.29732,
0.15673, -1.10610, 1.38790, -0.35252, -0.19737, 1.37822
],
[
0.05823, -0.23421, 1.48508, -0.20509, -0.96025, 1.41172,
0.17745, -1.13891, 1.42353, -0.33010, -0.20016, 1.38054
],
[
0.07251, -0.31049, 1.53516, -0.22492, -0.99256, 1.56046,
0.20117, -1.17279, 1.48591, -0.29644, -0.22063, 1.38287
],
[
0.08583, -0.40099, 1.58901, -0.25047, -0.99258, 1.72080,
0.22765, -1.19597, 1.55581, -0.27224, -0.26497, 1.40904
],
[
0.10666, -0.49583, 1.63750, -0.28958, -0.92583, 1.84487,
0.25957, -1.21016, 1.63709, -0.26425, -0.34894, 1.47905
],
[
0.12330, -0.58344, 1.66599, -0.32899, -0.79866, 1.93160,
0.29429, -1.21589, 1.72513, -0.25872, -0.45517, 1.56423
],
[
0.14159, -0.66864, 1.68271, -0.38174, -0.62570, 1.98066,
0.32652, -1.20391, 1.80623, -0.25407, -0.55389, 1.64068
],
[
0.15252, -0.74730, 1.68610, -0.42034, -0.44236, 1.98782,
0.35677, -1.17144, 1.87639, -0.24517, -0.64147, 1.69284
],
[
0.16384, -0.81449, 1.67367, -0.43698, -0.26971, 1.95002,
0.38470, -1.11989, 1.93491, -0.22632, -0.72205, 1.72749
]]
footFR_index = 3
footFL_index = 7
footHR_index = 11
footHL_index = 15
while 1:
for ii in range(33):
print(ii)
for i in range(0, footFR_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
startPos[ii][i]) # , force = 20)
for i in range(footFR_index + 1, footFL_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
startPos[ii][i - 1]) # , force = 20)
for i in range(footFL_index + 1, footHR_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
startPos[ii][i - 2]) # , force = 20)
for i in range(footHR_index + 1, footHL_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
startPos[ii][i - 3]) # , force = 20)
time.sleep(0.05)
