I want to implement in Python to convert this inverse kinematic algorithm and Cartesian linear path trajectory planning to be an optimal control algorithm for a robot in order to deal with singularities:
""" Utilities for differential IK. """

import numpy as np
import pinocchio
import time

from ..core.utils import (
    check_collisions_at_state,
    check_within_limits,
    get_random_state,
    get_random_collision_free_state,
)
from ..visualization.meshcat_utils import visualize_frame

VIZ_INITIAL_RENDER_TIME = 0.5
VIZ_SLEEP_TIME = 0.025


class DifferentialIkOptions:
    """Options for differential IK."""

    def __init__(
        self,
        max_iters=200,
        max_retries=10,
        max_translation_error=1e-3,
        max_rotation_error=1e-3,
        damping=1e-3,
        min_step_size=0.1,
        max_step_size=0.5,
        ignore_joint_indices=[],
        rng_seed=None,
    ):
        """
        Initializes a set of differential IK options.

        Parameters
        ----------
            max_iters : int
                Maximum number of iterations per try.
            max_retries : int
                Maximum number of retries with random restarts.
                If set to 0, only the initial state provided will be used.
            max_translation_error : float
                Maximum translation error, in meters, to consider IK solved.
            max_rotation_error : float
                Maximum rotation error, in radians, to consider IK solved.
            damping : float
                Damping value, between 0 and 1, for the Jacobian pseudoinverse.
                Setting this to a nonzero value is using Levenberg-Marquardt.
            min_step_size : float
                Minimum gradient step size, between 0 and 1, based on ratio of current distance to target to initial distance to target.
                To use a fixed step size, set both minimum and maximum values to be equal.
            max_step_size : float
                Maximum gradient step size, between 0 and 1, based on ratio of current distance to target to initial distance to target.
                To use a fixed step size, set both minimum and maximum values to be equal.
            ignore_joint_indices : list[int], optional
                A list of joints to ignore changing when solving IK.
                TODO: This should eventually be done through a concept of joint groups.
            rng_seed : int, optional
                Sets the seed for random number generation. Use to generate deterministic results.
        """
        self.max_iters = max_iters
        self.max_retries = max_retries
        self.max_translation_error = max_translation_error
        self.max_rotation_error = max_rotation_error
        self.damping = damping
        self.min_step_size = min_step_size
        self.max_step_size = max_step_size
        self.ignore_joint_indices = ignore_joint_indices
        self.rng_seed = rng_seed


class DifferentialIk:
    """
    Differential IK solver.

    This is a numerical IK solver that uses the manipulator's Jacobian to take first-order steps towards a solution.
    It contains several of the common options such as damped least squares (Levenberg-Marquardt), random restarts, and nullspace projection.

    Some good resources:
      * https://motion.cs.illinois.edu/RoboticSystems/InverseKinematics.html
      * https://homes.cs.washington.edu/~todorov/courses/cseP590/06_JacobianMethods.pdf
      * https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf
    """

    def __init__(
        self,
        model,
        collision_model=None,
        data=None,
        collision_data=None,
        visualizer=None,
        options=DifferentialIkOptions(),
    ):
        """
        Creates an instance of a DifferentialIk solver.

        Parameters
        ----------
            model : `pinocchio.Model`
                The model to use for this solver.
            collision_model : `pinocchio.Model`, optional
                The model to use for collision checking. If None, no collision checking takes place.
            data : `pinocchio.Data`, optional
                The model data to use for this solver. If None, data is created automatically.
            collision_data : `pinocchio.GeometryData`, optional
                The collision_model data to use for this solver. If None, data is created automatically.
            visualizer : `pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer`, optional
                The visualizer to use for this solver.
            options : `DifferentialIkOptions`, optional
                The options to use for solving IK. If not specified, default options are used.
        """
        self.model = model
        self.collision_model = collision_model

        if not data:
            data = model.createData()
        self.data = data
        if not collision_data and self.collision_model is not None:
            collision_data = collision_model.createData()
        self.collision_data = collision_data

        self.visualizer = visualizer
        self.options = options

    def solve(
        self,
        target_frame,
        target_tform,
        init_state=None,
        nullspace_components=[],
        verbose=False,
    ):
        """
        Solves an IK query.

        Parameters
        ----------
            target_frame : str
                The name of the target frame in the model.
            target_tform : `pinocchio.SE3`
                The desired transformation of the target frame in the model.
            init_state : array-like, optional
                The initial state to solve from. If not specified, a random initial state will be selected.
            nullspace_components : list[function], optional
                An optional list of nullspace components to use when solving.
                These components must take the form `lambda model, q: component(model, q, <other_args>)`.
            verbose : bool, optional
                If True, prints additional information to the console.

        Returns
        -------
            array-like or None
                A list of joint configuration values with the solution, if one was found. Otherwise, returns None.
        """
        np.random.seed(self.options.rng_seed)
        target_frame_id = self.model.getFrameId(target_frame)

        # Create a random initial state, if not specified
        if init_state is None:
            init_state = get_random_state(self.model)
        if self.visualizer:
            self.visualizer.displayFrames(True, frame_ids=[target_frame_id])
            visualize_frame(self.visualizer, "ik_target_pose", target_tform)

        # Initialize IK
        solved = False
        n_tries = 0
        q_cur = init_state
        q_old = init_state
        initial_error_norm = None

        if self.visualizer:
            self.visualizer.display(q_cur)
            time.sleep(VIZ_INITIAL_RENDER_TIME)  # Needed to render

        while n_tries <= self.options.max_retries:
            n_iters = 0
            while n_iters < self.options.max_iters:
                # Compute forward kinematics at the current state
                pinocchio.framesForwardKinematics(self.model, self.data, q_cur)
                cur_tform = self.data.oMf[target_frame_id]

                # Check the error using actInv
                error = target_tform.actInv(cur_tform)
                error = -pinocchio.log(error).vector
                if (
                    np.linalg.norm(error[:3]) < self.options.max_translation_error
                    and np.linalg.norm(error[3:]) < self.options.max_rotation_error
                ):
                    # Wrap to the range -/+ pi, and then check joint limits and collision.
                    #q_cur = (q_cur + np.pi) % (2 * np.pi) - np.pi
                    if check_within_limits(self.model, q_cur):
                        if self.collision_model is not None:
                            if check_collisions_at_state(
                                self.model,
                                self.collision_model,
                                q_cur,
                                self.data,
                                self.collision_data,
                            ):
                                if verbose:
                                    print(
                                        "Solved within joint limits, but in collision."
                                    )
                            else:
                                solved = True
                                if verbose:
                                    print(
                                        "Solved, within joint limits, and collision-free!"
                                    )
                        else:
                            solved = True
                            if verbose:
                                print("Solved within joint limits!")
                    else:
                        if verbose:
                            print("Solved, but outside joint limits.")
                    break

                # Calculate the Jacobian
                J = pinocchio.computeFrameJacobian(
                    self.model,
                    self.data,
                    q_cur,
                    target_frame_id,
                    pinocchio.ReferenceFrame.LOCAL,
                )

                # Solve for the gradient using damping and nullspace components,
                # as specified
                jjt = J.dot(J.T) + self.options.damping**2 * np.eye(6)

                # Compute the gradient descent step size
                error_norm = np.linalg.norm(error)
                if not initial_error_norm:
                    initial_error_norm = error_norm
                alpha = self.options.min_step_size + (
                    1.0 - error_norm / initial_error_norm
                ) * (self.options.max_step_size - self.options.min_step_size)

                # Gradient descent step
                if not nullspace_components:
                    q_step = alpha * J.T @ np.linalg.solve(jjt, error)
                else:
                    nullspace_term = sum(
                        [comp(self.model, q_cur) for comp in nullspace_components]
                    )
                    q_step = alpha * (
                        J.T @ (np.linalg.solve(jjt, error - J @ (nullspace_term)))
                        + nullspace_term
                    )

                # Zero out the values for the ignored indices before returning.
                for idx in self.options.ignore_joint_indices:
                    q_step[idx] = 0.0

                q_cur += q_step
                n_iters += 1

                if self.visualizer:
                    self.visualizer.display(q_cur)
                    time.sleep(VIZ_SLEEP_TIME)

            # Check results at the end of this try
            if solved:
                if verbose:
                    print(f"Solved in {n_tries+1} tries.")
                break
            else:
                if self.collision_model is not None:
                    q_cur = get_random_collision_free_state(
                        self.model, self.collision_model
                    )
                else:
                    q_cur = get_random_state(self.model)
                n_tries += 1
                if verbose:
                    print(f"Retry {n_tries}")

        # Check final results
        if solved:
            return q_cur
        else:
            return None

""" Utilities for Cartesian (or task space) motion planning. """

import numpy as np
import pinocchio
from scipy.spatial.transform import Rotation, Slerp

from pyroboplan.trajectory.trapezoidal_velocity import TrapezoidalVelocityTrajectory


class CartesianPlannerOptions:
    """Options for Cartesian planning."""

    def __init__(
        self,
        use_trapezoidal_scaling=True,
        max_linear_velocity=1.0,
        max_linear_acceleration=1.0,
        max_angular_velocity=1.0,
        max_angular_acceleration=1.0,
    ):
        """
        Initializes a set of Cartesian planning options.

        Parameters
        ----------
            use_trapezoidal_scaling : bool
                If True, uses trapezoidal velocity time scaling.
                Otherwise, uses linear time scaling.
            max_linear_velocity : float
                The maximum linear velocity magnitude, in meters per second.
            max_linear_acceleration : float
                The maximum linear acceleration magnitude, in meters per second squared.
            max_angular_velocity : float
                The maximum angular velocity magnitude, in radians per second.
            max_angular_acceleration : float
                The maximum angular acceleration magnitude, in radians per second squared.
        """
        self.use_trapezoidal_scaling = use_trapezoidal_scaling

        if max_linear_velocity <= 0:
            raise ValueError("Max linear velocity must be positive.")
        self.max_linear_velocity = max_linear_velocity
        if max_linear_acceleration <= 0:
            raise ValueError("Max linear acceleration must be positive.")
        self.max_linear_acceleration = max_linear_acceleration
        if max_angular_velocity <= 0:
            raise ValueError("Max angular velocity must be positive.")
        self.max_angular_velocity = max_angular_velocity
        if max_angular_acceleration <= 0:
            raise ValueError("Max angular acceleration must be positive.")
        self.max_angular_acceleration = max_angular_acceleration


class CartesianPlanner:
    """
    Cartesian (or task space) motion planner.

    This tool will plan a Cartesian trajectory through a set of poses,
    and then use an inverse kinematics solver to incrementally solve for valid
    robot joint configurations to achieve this task-space trajectory.

    Some good resources:
      * Slides: https://www.diag.uniroma1.it/~deluca/rob1_en/14_TrajectoryPlanningCartesian.pdf
      * PickNik blog: https://picknik.ai/cartesian%20planners/moveit/motion%20planning/2021/01/07/guide-to-cartesian-planners-in-moveit.html
    """

    def __init__(
        self, model, target_frame, tforms, ik_solver, options=CartesianPlannerOptions()
    ):
        """
        Initializes a Cartesian motion planner instance.

        Parameters
        ----------
            model : `pinocchio.Model`
                The model to use for motion planning.
            target_frame : str
                The name of the target frame in the model defining the Cartesian motion.
            tforms : list[`pinocchio.SE3`]
                A list of pose waypoints, in SE3, for the target frame to move through.
            ik_solver : any
                Any valid inverse kinematics solver from this library.
            options : `pyroboplan.planning.cartesian_planner.CartesianPlannerOptions`, optional
                A set of Cartesian planning options.
                If not set, a default set of options will be used.
        """
        self.model = model
        self.target_frame = target_frame
        self.tforms = tforms
        self.ik_solver = ik_solver
        self.options = options

        # Generate the segments
        cur_time = 0.0
        self.segments = []
        for idx in range(1, len(tforms)):
            tform_start = tforms[idx - 1]
            tform_end = tforms[idx]

            # Figure out a time scaling.
            tform_diff = tform_end.actInv(tform_start)
            linear_distance = np.linalg.norm(tform_diff.translation)
            angular_distance = Rotation.from_matrix(tform_diff.rotation).magnitude()

            if options.use_trapezoidal_scaling:
                # Use trapezoidal scaling limited by both the linear and angular velocities and accelerations.
                if linear_distance > 0:
                    normalized_linear_accel = (
                        options.max_linear_acceleration / linear_distance
                    )
                    normalized_linear_vel = (
                        options.max_linear_velocity / linear_distance
                    )
                else:
                    normalized_linear_accel = normalized_linear_vel = np.inf

                if angular_distance > 0:
                    normalized_angular_accel = (
                        options.max_angular_acceleration / angular_distance
                    )
                    normalized_angular_vel = (
                        options.max_angular_velocity / angular_distance
                    )
                else:
                    normalized_angular_accel = normalized_angular_vel = np.inf

                max_normalized_accel = min(
                    normalized_linear_accel, normalized_angular_accel
                )
                max_normalized_vel = min(normalized_linear_vel, normalized_angular_vel)

                traj = TrapezoidalVelocityTrajectory(
                    np.array([0.0, 1.0]), max_normalized_vel, max_normalized_accel
                )
                final_time = cur_time + traj.segment_times[-1]
                self.segments.append((traj, final_time))
            else:
                # Use constant velocity at the specified max
                constant_vel_time = max(
                    linear_distance / options.max_linear_velocity,
                    angular_distance / options.max_angular_velocity,
                )
                final_time = cur_time + constant_vel_time
                self.segments.append((None, final_time))

            cur_time = final_time

    def generate(self, q_init, dt):
        """
        Generates a set of joint trajectories from the Cartesian plan at a specified sample time.

        Parameters
        ----------
            q_init : array-like
                The starting joint configuration for the robot.
                This is used as the initial condition for solving inverse kinematics.
            dt : float
                The sample time to use for trajectory generation, in seconds.

        Returns
        -------
            tuple(bool, array-like, array-like)
                The tuple of (success, t_vec, q_vec) corresponding to a success metric,
                the vector of times, and vector of joint positions, respectively.
        """
        self.generated_tforms = []

        t_start = 0
        t_final = self.segments[-1][1]
        t_vec = np.arange(t_start, t_final, dt)
        if t_vec[-1] != t_final:
            t_vec = np.append(t_vec, t_final)
        num_pts = len(t_vec)

        q = np.zeros([self.model.nq, num_pts])
        q[:, 0] = q_init

        q_cur = q_init
        idx = 0
        segment_idx = 0
        t_prev = 0
        t_final_cur = 0

        while idx < num_pts:
            t = t_vec[idx]

            traj_cur, t_final_cur = self.segments[segment_idx]
            tform_prev = self.tforms[segment_idx]
            tform_next = self.tforms[segment_idx + 1]
            slerp = Slerp(
                [0.0, 1.0],
                Rotation.concatenate(
                    [
                        Rotation.from_matrix(tform_prev.rotation),
                        Rotation.from_matrix(tform_next.rotation),
                    ]
                ),
            )

            if t <= t_final_cur:
                # Find the normalized interpolation time.
                dt = t - t_prev
                if self.options.use_trapezoidal_scaling:
                    dt = min(dt, traj_cur.segment_times[-1])  # Avoids numerical issues?
                    alpha, _, _ = traj_cur.evaluate(dt)
                else:
                    alpha = dt / (t_final_cur - t_prev)

                # Interpolate position and rotation separately.
                p = (
                    alpha * tform_next.translation
                    + (1.0 - alpha) * tform_prev.translation
                )
                R = slerp(alpha).as_matrix()
                tform = pinocchio.SE3(R, p)
                self.generated_tforms.append(tform)

                # Solve IK.
                q_sol = self.ik_solver.solve(self.target_frame, tform, init_state=q_cur)
                if q_sol is not None:
                    q[:, idx] = q_sol
                else:
                    print(f"Failed to solve on point {idx + 1} out of {num_pts}")
                    return False, t_vec, q

                idx += 1
            else:
                segment_idx += 1
                t_prev = t_final_cur

        return True, t_vec, q