I have the following Python script to calculate the joint position. I want to plot also the joint velocity and accelerations
"""
This file demonstrates the use of the Cartesian planner to generate a trajectory
"""
import sys
import os
if os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "src") not in sys.path:
    sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "src"))
import numpy as np
import time
## pin
import pinocchio as pin
from pinocchio.visualize import MeshcatVisualizer
from pinocchio.robot_wrapper import RobotWrapper 
##  POLYTHESIS
from polythesis.core.utils import generate_mod_file
from polythesis.robot.robot_loader import loadABB_IRB7600325, loadABB_IRB7600325v2, loadABB_IRB4600255, loadABB_IRB4600255v2
# from polythesis.ik.differential_ik import DifferentialIk, DifferentialIkOptions
from polythesis.ik.mydifferential_ik import DLS_Options, DLS_Ik
# from polythesis.planning.cartesian_planner import (CartesianPlanner,CartesianPlannerOptions)
from polythesis.planning.my_cartesian_planner import (CartesianPlanner,CartesianPlannerOptions)
from polythesis.visualization.meshcat_utils import visualize_frames
from polythesis.plot.plot_results import PlotResults
import polythesis.EGM.Core as egm
import matplotlib.pyplot as plt

##   Add access if it is not in the system path.


# CONSTANTS
VERBOSE=True                      # Print out debug information
VISUAL=True                       # Visualize the robot in MeshCat
PLOT=True                         # Plot results
# Constants for the simulation
dt=0.04                          # [s] Time step for the simulation
SIMU_REAL_TIME=True               # Set to True if you want to run the simulation in real time
ndt=10                            # [s] Number of integration steps for each control loop
# Constants for the REAL robot controller
CONTROL_REAL_ROBOT=False           # Set to True if you want to control the real robotABB
IP_CONTROLLER='172.18.206.202'    # IP address of the ABB controller
PORT_CONTROLLER=6511              # Port of the ABB controller
# Constants for the robot max values
MAX_LINEAR_VELOCITY=0.4           # [m/s] Maximum TCP linear velocity
MAX_LINEAR_ACCELERATION=0.4       # [m/s^2] Maximum TCP linear acceleration 
MAX_ANGULAR_VELOCITY=0.5          # [rad/s] Maximum TCP angular velocity
MAX_ANGULAR_ACCELERATION=0.4      # [rad/s^2] Maximum TCP angular acceleration
# Other constants
LINE_WIDTH = 80

# Load your robot's URDF model
robot = loadABB_IRB7600325v2() # Available models: loadABB_IRB4600255,loadABB_IRB4600255v2, loadABB_IRB7600325,loadABB_IRB7600325v2
model = robot.model
data = robot.data

# Retrieve joint limits from the robot model
lower_limits = model.lowerPositionLimit
upper_limits = model.upperPositionLimit

# Specify the end-effector frame name as defined in your URDF
end_effector_frame_name = 'tool0'  # Replace with your end-effector frame name
frame_id = model.getFrameId(end_effector_frame_name)

# Initial joint configuration q0 (robot's home position)
# Ensure the initial configuration is within joint limits
q0 = pin.neutral(model)
q0 = np.minimum(np.maximum(q0, lower_limits), upper_limits)

# Time parameters
dt = 0.01  # Time step (seconds)
T = 8.0    # Total time for the motion (seconds)
num_steps = int(T / dt)

## FOR THE ABB IRB 7600-325
start_TCP_pose = pin.SE3(pin.rpy.rpyToMatrix(np.array([0,np.pi/2,0])), np.array([2.21, -1.0, 2.02]))
end_TCP_pose = pin.SE3(pin.rpy.rpyToMatrix(np.array([0,np.pi/2,0])), np.array([2.21, +1.0, 2.02]))

# Function to interpolate between two poses
def interpolate_pose(start_pose, end_pose, alpha):
    # Linear interpolation for position
    position = (1 - alpha) * start_pose.translation + alpha * end_pose.translation
    # Spherical linear interpolation (slerp) for orientation
    quat_start = pin.Quaternion(start_pose.rotation)
    quat_end = pin.Quaternion(end_pose.rotation)
    quat_interp = quat_start.slerp(alpha, quat_end)
    orientation = quat_interp.matrix()
    return pin.SE3(orientation, position)

# Function to compute error between current and desired poses
def compute_pose_error(current_pose, desired_pose):
    # Position error
    e_translation = desired_pose.translation - current_pose.translation
    # Orientation error (using log of rotation matrix)
    R_current = current_pose.rotation
    R_desired = desired_pose.rotation
    R_error = R_desired @ R_current.T
    e_rotation = pin.log3(R_error)
    # Concatenate translation and rotation errors
    e = np.concatenate((e_translation, e_rotation))
    return e

# Initialize joint configuration
q = q0.copy()
q_list = [q.copy()]        # List to record joint positions
time_list = [0.0]          # List to record time steps

# Main control loop
for i in range(num_steps):
    # Compute time and interpolation factor
    t = i * dt
    alpha = t / T  # Interpolation ratio from 0 to 1

    # Interpolate to get desired TCP pose at current time
    x_desired = interpolate_pose(start_TCP_pose, end_TCP_pose, alpha)
    position_desired = x_desired.translation
    orientation_desired = pin.Quaternion(x_desired.rotation)

    # Compute current TCP pose using forward kinematics
    pin.forwardKinematics(model, data, q)
    pin.updateFramePlacement(model, data, frame_id)
    oMf = data.oMf[frame_id]  # Current transform of the end-effector frame
    x_current = pin.SE3(oMf.rotation, oMf.translation)

    # Compute error between current and desired poses
    e = compute_pose_error(x_current, x_desired)

    # Compute the Jacobian of the end-effector
    J = pin.computeFrameJacobian(model, data, q, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
    J6 = J[:6, :]  # Use the first 6 rows for the spatial velocity

    # Compute SVD of the Jacobian
    U, S, Vh = np.linalg.svd(J6, full_matrices=False)

    # Determine the damping factor lambda based on singular values
    singular_threshold = 0.05
    # Minimum singular value
    S_min = S[-1]
    # If near singularity, increase damping
    if S_min < singular_threshold:
        lambda_damping = (1 - (S_min / singular_threshold)) * 0.1
    else:
        lambda_damping = 0.0  # No damping needed
    # Small regularization to avoid division by zero
    lambda_damping = max(lambda_damping, 1e-6)

    # Compute damped singular values
    S_damped_inv = np.array([s / (s**2 + lambda_damping**2) for s in S])

    # Compute damped pseudo-inverse of the Jacobian
    J_damped_pinv = Vh.T @ np.diag(S_damped_inv) @ U.T

    # Compute joint update
    delta_q = J_damped_pinv @ e

    # Optional: Limit the maximum joint update step to avoid too large movements
    max_delta_q = 0.05  # Maximum joint update (radians)
    norm_delta_q = np.linalg.norm(delta_q)
    if norm_delta_q > max_delta_q:
        delta_q *= max_delta_q / norm_delta_q

    # Update joint positions using integration
    q_new = pin.integrate(model, q, delta_q)

    # **Apply joint limits**
    q_new = np.minimum(np.maximum(q_new, lower_limits), upper_limits)

    # Check if the joint update was feasible
    if not np.allclose(q_new, q + delta_q, atol=1e-6):
        # Joint limits have modified the joint update, which may affect the end-effector pose
        # Consider recomputing delta_q with joint limits projection (advanced topic)
        pass  # For this simple implementation, we proceed with the limited q_new

    # Update joint positions
    q = q_new

    # Record the joint positions and time
    q_list.append(q.copy())
    time_list.append(t + dt)

    # (Optional) Print progress
    if i % 100 == 0:
        print(f"Step {i}/{num_steps}, Time {t:.2f}s, Damping {lambda_damping:.4f}")

# The q_list now contains the joint configurations over time, considering joint limits
#wait
# Convert q_list to a numpy array for easier indexing
q_array = np.array(q_list)  # Shape: (num_steps+1, num_joints)

# Plot each joint's values over time
plt.figure(figsize=(10, 6))
num_joints = q_array.shape[1]
for joint_index in range(num_joints):
    plt.plot(time_list, q_array[:, joint_index], label=f"Joint {joint_index + 1}")

plt.xlabel("Time [s]")
plt.ylabel("Joint Angles [rad]")
plt.title("Joint Angles Over Time")
plt.legend()
plt.grid(True)
plt.show()

for idx in range(q_array.shape[0]):
    robot.viz.display(q_array[idx,:])
    time.sleep(dt)

input("Press 'q' to exit the program...")