import mujoco as mj
import mujoco.viewer
import time,os
import numpy as np
import matplotlib.pyplot as plt
from spatialmath import SO3

# Load the XML model
model = mj.MjModel.from_xml_path(r"FurturaPendulum.xml")
data = mj.MjData(model)

#extract id from model for semantics. 
joint1_name = "joint1"
joint1_id = model.joint(joint1_name).id

joint2_name = "joint2"
joint2_id = model.joint(joint2_name).id

end_effector_body_id = model.body(name="end-effector").id
actuator_id = model.actuator(name="joint1-actuator").id

frame1_body_id = model.body(name="frame1").id

#Declare lists to hold the data. 
heights = []
times = []
body_lin_vels_endeffector = []


spatial_ang_vels_endeffector = []
spatial_ang_vels_frame_1 = []


#Unsure from problem what pose is supposed to be used as the inital condition, so assuming the home position.
# pose of Q1 part 1
# theta1 = 5 * np.pi / 6
# theta2 = -3 * np.pi / 7

# pose of home position
theta1 = np.pi / 2
theta2 = 0

#Move the model to the inital theta values
data.qpos[joint1_id] = theta1
data.qpos[joint2_id] = theta2
mj.mj_forward(model, data)

# Start the simulation loop. 
time_simulated = 0
time_to_simulate = 15#s

show_in_real_time = True
with mujoco.viewer.launch_passive(model, data) as viewer:
    steps = 0
    while viewer.is_running():
        # Set current control input
        data.ctrl[actuator_id] = -15 * np.sign(data.qvel[1].copy())

        # Step simulation
        mujoco.mj_step(model, data)

        if show_in_real_time:
            #update viewer
            viewer.sync()
            #sleep so you can watch the simulation in real time
            time.sleep(0.002)
        time_simulated += .002
        steps += 1
        #now we are ready to record data for this step
        heights.append(data.xpos[end_effector_body_id][2])
        times.append(time_simulated)
        body_lin_vels_endeffector.append(data.sensordata[:3].copy())
        R0c = data.xmat[frame1_body_id].reshape(3,3)
        spatial_ang_vels_endeffector.append(np.array(R0c) @ np.array(data.sensordata[3:6].copy()))
        spatial_ang_vels_frame_1.append(np.array(R0c) @ np.array(data.sensordata[6:9].copy()))

        # Stop the simulation after is has simulated the desired time
        if time_simulated > time_to_simulate:
            break
    viewer.close()



print("Done Simulating")

Done Simulating

#Plot the data and save the plots. 
plots_dir = "plots"
if not os.path.exists(plots_dir):
    os.makedirs(plots_dir)


#Heights
plt.figure()
plt.ylim(0,2.1)
plt.xlabel("Time (s)")
plt.ylabel("Height of End-Effector (m)")
plt.title("Height of End-Effector vs. Time")
plt.plot(times, heights)
plt.savefig(plots_dir + '\\heights.png')



#Body linear velocicy of endeffector vs time
body_lin_vels_endeffector_magnitudes = [np.linalg.norm(v) for v in body_lin_vels_endeffector]
body_lin_vels_endeffector_normalized = [v / np.linalg.norm(v) if np.linalg.norm(v) > 0 else np.array([0, 0, 0]) for v in body_lin_vels_endeffector]

fig, axs = plt.subplots(2,2)
fig.suptitle("Body Linear Velocity of End-Effector vs. Time")
axs[0,0].set_title("Magnitude vs. time")
axs[0,0].set_xlabel("Time (s)")
axs[0,0].set_ylabel("Magnitude (m/s)")
axs[0,0].plot(times, body_lin_vels_endeffector_magnitudes)

axs[0,1].set_title("Norm X")
axs[0,1].set_ylim(-1.1, 1.1)
axs[0,1].set_xlabel("Time (s)")
axs[0,1].set_ylabel("X velocity (m/s)")
axs[0,1].plot(times, [a[0] for a in body_lin_vels_endeffector_normalized])


axs[1,0].set_title("Norm Y")
axs[1,0].set_ylim(-1.1, 1.1)
axs[1,0].set_xlabel("Time (s)")
axs[1,0].set_ylabel("Y velocity (m/s)")
axs[1,0].plot(times, [a[1] for a in body_lin_vels_endeffector_normalized])


axs[1,1].set_title("Norm Z")
axs[1,1].set_ylim(-1.1, 1.1)
axs[1,1].set_xlabel("Time (s)")
axs[1,1].set_ylabel("Z velocity (m/s)")
axs[1,1].plot(times, [a[2] for a in body_lin_vels_endeffector_normalized])


plt.tight_layout()
plt.savefig(plots_dir + '\\bodylinearvelocity.png')



#spatial ang velcoity of end effector vs spatial angvelocity of frame 1
spatial_ang_vels_endeffector_magnitudes = [np.linalg.norm(v) for v in spatial_ang_vels_endeffector]
spatial_ang_vels_endeffector_normalized = [v / np.linalg.norm(v) if np.linalg.norm(v) > 0 else np.array([0, 0, 0]) for v in spatial_ang_vels_endeffector]

spatial_ang_vels_frame1_magnitudes = [np.linalg.norm(v) for v in spatial_ang_vels_frame_1]
spatial_ang_vels_frame1_normalized = [v / np.linalg.norm(v) if np.linalg.norm(v) > 0 else np.array([0, 0, 0]) for v in spatial_ang_vels_frame_1]

fig, axs = plt.subplots(2,2)
fig.suptitle("Spatial Angular Velocity of End-Effector vs Spatial Angular Velocity of Frame 1")
axs[0,0].set_title("Magnitude")
axs[0,0].set_xlabel("Frame1 (m/s)")
axs[0,0].set_ylabel("End effector (m/s)")
axs[0,0].plot(spatial_ang_vels_frame1_magnitudes, spatial_ang_vels_endeffector_magnitudes)

axs[0,1].set_title("Norm X")
axs[0,1].set_xlim(-1.1, 1.1)
axs[0,1].set_ylim(-1.1, 1.1)
axs[0,1].set_xlabel("Frame1_X (m/s)")
axs[0,1].set_ylabel("End Effector_X (m/s)")
axs[0,1].plot([a[0] for a in spatial_ang_vels_frame1_normalized], [a[0] for a in spatial_ang_vels_endeffector_normalized])


axs[1,0].set_title("Norm Y")
axs[1,0].set_xlim(-1.1, 1.1)
axs[1,0].set_ylim(-1.1, 1.1)
axs[1,0].set_xlabel("Frame1_Y (m/s)")
axs[1,0].set_ylabel("End Effector_Y (m/s)")
axs[1,0].plot([a[1] for a in spatial_ang_vels_frame1_normalized], [a[1] for a in spatial_ang_vels_endeffector_normalized])


axs[1,1].set_title("Norm Z")
axs[1,1].set_xlim(-1.1, 1.1)
axs[1,1].set_ylim(-1.1, 1.1)
axs[1,1].set_xlabel("Frame1_Z (m/s)")
axs[1,1].set_ylabel("End Effector_Z (m/s)")
axs[1,1].plot([a[2] for a in spatial_ang_vels_frame1_normalized], [a[2] for a in spatial_ang_vels_endeffector_normalized])



plt.tight_layout()
plt.savefig(plots_dir + '\\spatialEndvsFrame1.png')

The Kernel crashed while executing code in the current cell or a previous cell. 

Please review the code in the cell(s) to identify a possible cause of the failure. 

Click <a href='https://aka.ms/vscodeJupyterKernelCrash'>here</a> for more info. 

View Jupyter <a href='command:jupyter.viewOutput'>log</a> for further details.

Question 4¶