<jupyter_start><jupyter_text>5 Joint:  b'LF_FOOT10 Joint:  b'RF_FOOT15 Joint:  b'LH_FOOT20 Joint:  b'RH_FOOT<jupyter_code>quadruped_joint_ids = []
active_joint = 0
for j in range(p.getNumJoints(quadruped_id)):
    p.changeDynamics(quadruped_id, j, linearDamping=0, angularDamping=0)
    info = p.getJointInfo(quadruped_id, jointIndex=j)
    joint_type = info[2]
    if joint_type == p.JOINT_PRISMATIC or joint_type == p.JOINT_REVOLUTE:
        quadruped_joint_ids.append(j)
        p.resetJointState(quadruped_id, j, quadruped_joint_angles[active_joint])
        active_joint+=1
for i in quadruped_joint_ids:
#     dyn_info = p.getJointState(quadruped_id, jointIndex=i)
    jnt_info = p.getJointInfo(quadruped_id, jointIndex=i)[12]
#     jnt_info = p.getJointState(quadruped_id, i)[0]
    print(jnt_info)
len(quadruped_joint_ids)<jupyter_output>b'LF_HIP'
b'LF_THIGH'
b'LF_SHANK'
b'RF_HIP'
b'RF_THIGH'
b'RF_SHANK'
b'LH_HIP'
b'LH_THIGH'
b'LH_SHANK'
b'RH_HIP'
b'RH_THIGH'
b'RH_SHANK'<jupyter_text>Inspecting the joint angles=====<jupyter_code>import matplotlib.pyplot as plt
%matplotlib notebook
camTargetPos = [0, 0, 0]
cameraUp = [0, 0, 1]
cameraPos = [1, 1, 1]

pitch = -10.0
roll = 0
yaw = 45
upAxisIndex = 2
# camDistance = 4
camDistance = 2
pixelWidth = 640
pixelHeight = 400
nearPlane = 0.01
farPlane = 100

fov = 60

# For plotting multiple frames
img = np.random.rand(400, 640)
fig, ax = plt.subplots(figsize=(9.6, 6))
image = ax.imshow(img, interpolation='none', animated=True, label="blah")

# for yaw in range(0, 360, 10):
#     p.stepSimulation()
#     viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
#                                                                 roll, upAxisIndex) 
#     aspect = pixelWidth / pixelHeight
#     projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
#     img_arr = p.getCameraImage(pixelWidth,
#                               pixelHeight,
#                               viewMatrix,
#                               projectionMatrix,
#                               shadow=1,
#                               lightDirection=[1, 1, 1],
#                               renderer=p.ER_BULLET_HARDWARE_OPENGL)
[0.03, 0.4, -0.8, -0.03, 0.4, -0.8, 0.03, -0.4, 0.8, -0.03, -0.4, 0.8]
# hip,thig,shank, hip, , thigh,sha, hip, thigh, sha, hip  , thig, shan
# all at once
actions_low = np.asarray([-0.09, -0.2, -1.4, -0.15, -0.2, -1.4, -0.09, -1.0, 0.2, -0.15, -1.0, 0.2])
actions_high = np.asarray([0.15, 1.0, -0.2, 0.09, 1.0, -0.2, 0.15, 0.2, 1.4, 0.09, 0.2, 1.4])
interp_angles = np.linspace(actions_high, actions_low, 25)

# hips only
actions_low = np.asarray([-0.09, 0.4, -0.8, -0.15, 0.4, -0.8, -0.09, -0.4, 0.8, -0.15, -0.4, 0.8])
actions_high = np.asarray([0.15, 0.4, -0.8, 0.09, 0.4, -0.8, 0.15, -0.4, 0.8, 0.09, -0.4, 0.8])
interp_angles_hips = np.linspace(actions_high, actions_low, 25)

# thigh only
actions_low_thigh = np.asarray([0.03, -0.2, -0.8, -0.03, -0.2, -0.8, 0.03, -1.0, 0.8, -0.03, -1.0, 0.8])
actions_high = np.asarray([0.03, 1.0, -0.8, -0.03, 1.0, -0.8, 0.03, 0.2, 0.8, -0.03, 0.2, 0.8])
interp_angles_thigh = np.linspace(actions_high, actions_low, 25)

# shank only
actions_low = np.asarray([0.03, 0.4, -1.4, -0.03, 0.4, -1.4, 0.03, -0.4, 0.2, -0.03, -0.4, 0.2])
actions_high = np.asarray([0.03, 0.4, -0.2, -0.03, 0.4, -0.2, 0.03, -0.4, 1.4, -0.03, -0.4, 1.4])
interp_angles_shank = np.linspace(actions_high, actions_low, 25)

interp_angles = interp_angles_hips
yaw = 90
for joint_angles in interp_angles:
    for i, j in enumerate(quadruped_joint_ids):
        p.resetJointState(quadruped_id, j, joint_angles[i])
        
    p.stepSimulation()
    viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
                                                                roll, upAxisIndex) 
    aspect = pixelWidth / pixelHeight
    projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
    img_arr = p.getCameraImage(pixelWidth,
                              pixelHeight,
                              viewMatrix,
                              projectionMatrix,
                              shadow=1,
                              lightDirection=[1, 1, 1],
                              renderer=p.ER_BULLET_HARDWARE_OPENGL)
    
    
    
    w = img_arr[0]  #width of the image, in pixels
    h = img_arr[1]  #height of the image, in pixels
    rgb = img_arr[2]  #color data RGB
    dep = img_arr[3]  #depth data
    np_img_arr = np.reshape(rgb, (h, w, 4))
    np_img_arr = np_img_arr * (1. / 255.)
    image.set_data(np_img_arr)
    fig.canvas.draw()
    plt.pause(0.005)

# for plotting single frames
# fig, ax = plt.subplots(figsize=(6.4, 4))
# plt.cla()
# p.stepSimulation()
# viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
#                                                             roll, upAxisIndex) 
# aspect = pixelWidth / pixelHeight
# projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
# img_arr = p.getCameraImage(pixelWidth,
#                           pixelHeight,
#                           viewMatrix,
#                           projectionMatrix,
#                           shadow=1,
#                           lightDirection=[1, 1, 1],
#                           renderer=p.ER_BULLET_HARDWARE_OPENGL)
# w = img_arr[0]  #width of the image, in pixels
# h = img_arr[1]  #height of the image, in pixels
# rgb = img_arr[2]  #color data RGB
# dep = img_arr[3]  #depth data
# np_img_arr = np.reshape(rgb, (h, w, 4))
# np_img_arr = np_img_arr * (1. / 255.)
# image = ax.imshow(np_img_arr)
plt.show()
    


LF_stream = []
for foot_index in [5,10,15,20]:
for foot_index in [5]:
    contact = p.getContactPoints(quadruped_id, plane_id, linkIndexA=foot_index)
    if contact == ():
        LF_stream.append(0)
    else:
        LF_stream.append(1)
feet_ids = {'LF':5, 'RF':10, 'LH':15, 'RH':20}
for foot, idx in feet_ids.items():
    foot=5
LF_stream = np.around(np.random.rand(100))
LF_stream
plt.plot(LF_stream)
plt.show()
quadruped_pos, quadruped_orientation = p.getBasePositionAndOrientation(quadruped_id)
quadruped_pos, quadruped_orientation = p.getBasePositionAndOrientation(quadruped_id)
quadruped_orientation = p.getEulerFromQuaternion(quadruped_orientation)
quadruped_linear_vel, quadruped_angular_vel = p.getBaseVelocity(quadruped_id)

joint_states = []
for j in quadruped_joint_ids:
    joint_states.append(p.getJointState(quadruped_id, j)[0])
    joint_states.append(p.getJointState(quadruped_id, j)[1])

obs = np.concatenate([quadruped_pos, quadruped_orientation,
                       quadruped_linear_vel, quadruped_angular_vel, joint_states])<jupyter_output><empty_output><jupyter_text>Investigating early termination: tuning conditions====<jupyter_code>quadruped_pos, quadruped_orientation = p.getBasePositionAndOrientation(quadruped_id)
quadruped_linear_vel, _ = p.getBaseVelocity(quadruped_id)
vel_x = quadruped_linear_vel[0]
quadruped_orientation = p.getEulerFromQuaternion(quadruped_orientation)

done = bool(quadruped_pos[2] < 0.3)
done = bool(done or np.abs(quadruped_orientation[0]) >= np.pi / 4)
done = bool(done or np.abs(quadruped_orientation[1]) >= np.pi / 4)
done = bool(done or np.abs(quadruped_orientation[2]) >= np.pi / 4)
done = bool(done or vel_x > 1)
quadruped_orientation
help(p.setTimeStep)
p.getPhysicsEngineParameters()['fixedTimeStep']
p.resetSimulation
p.getPhysicsEngineParameters()['fixedTimeStep']<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.13602649006622516, 'fraction_numerical': 0.05231788079470199, 'mean_word_length': 2.888259526261586, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 16, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '3', 'dir': 'jupyter-structured-clean-dedup', 'id': '390584', 'license': 'MIT', 'n_tokens_mistral': 3156, 'n_tokens_neox': 2917, 'n_words': 555, 'path': 'gym_anybullet/envs/joint_tests.ipynb', 'repo_name': 'bibbygoodwin/rl-baselines-zoo', 'size': '7550'}