timor.utilities.trajectory ========================== .. py:module:: timor.utilities.trajectory Classes ------- .. autoapisummary:: timor.utilities.trajectory.Trajectory Functions --------- .. autoapisummary:: timor.utilities.trajectory.greedy_ik_trajectory Module Contents --------------- .. py:class:: Trajectory .. autoapi-inheritance-diagram:: timor.utilities.trajectory.Trajectory :parts: 1 :private-bases: Holds an ordered set of states (can be any one of these): * robot configuration q + change * poses each with or without time information. .. py:method:: __add__(other) Appends other trajectory to self. .. py:method:: __eq__(other) Check if two trajectories are equal. .. py:method:: __getitem__(item) Get sub-trajectory via indexing. .. py:method:: __getstate__() Custom get state, e.g. for pickle / deepcopy. .. py:method:: __iter__() Explicit iterator over trajectory. .. py:method:: __len__() Return number of samples in this trajectory .. py:method:: __post_init__() Check consistency such as same size for t, q, dq, ddq, and / or pose. .. py:method:: __setstate__(state) Custom set state, e.g. for pickle / deepcopy. .. py:attribute:: _allowed_deviation_in_time :type: float :value: 1e-12 .. py:attribute:: _ddq :type: Union[timor.utilities.dtypes.Lazy, numpy.ndarray] .. py:method:: _deduce_package_dir(filepath, content) :staticmethod: Logic for deducing the package directory if the JSONable object is loaded from a file. :param filepath: The path to the file. :param content: The content of the file parsed into a dictionary. :note: This method can be overridden by subclasses to provide custom package directory resolution; e.g. Task.py. .. py:attribute:: _dq :type: Union[timor.utilities.dtypes.Lazy, numpy.ndarray] .. py:method:: add_tolerance(tolerance) Add a tolerance to each pose of this trajectory. :param tolerance: The tolerance to add; if iterable each Tolerance is added to pose with same index. .. py:property:: ddq :type: numpy.ndarray The joint accelerations. .. py:property:: dof Number of degrees of freedom encoded by this trajectory. .. py:property:: dq :type: numpy.ndarray The joint velocities. .. py:method:: empty() :classmethod: Create and return an empty, timed, q-based trajectory .. py:method:: fk_trajectory(robot) Calculate a forward kinematic trajectory for this joint trajectory. I.e., execute this trajectory defining a joint space movement on the provided assembly and create a new trajectory of the end-effector movement and its poses over time :param robot: The robot to calculate the FKs with. :return: The end-effector trajectory that follows the joint trajectory. .. py:method:: from_json_data(d, **kwargs) :classmethod: Load from dict .. py:method:: from_json_file(filepath, *args, **kwargs) :classmethod: Factory method to load a class instance from a json file. :param filepath: The path to the json file. :param args: Additional arguments to pass to the from_json_data factory method of the specific class. :param kwargs: Additional arguments to pass to the from_json_data factory method of the specific class. .. py:method:: from_json_string(s, *args, **kwargs) :classmethod: Create from a json string. .. py:method:: from_mstraj(via_points, dt, t_acc, qd_max, **kwargs) :classmethod: Create multi-segment multi-axis trajectory. See: https://petercorke.github.io/robotics-toolbox-python/arm_trajectory.html?highlight=mstraj :param via_points: Via points to connect with PTP trajectory. Start at first row, stop at last :param dt: sample time in seconds :param t_acc: acceleration time per PTP segment :param qd_max: maximum speed in each axis :param kwargs: Additional key word arguments piped directly into the mstraj function .. py:method:: get_at_time(time) Returns state at desired time. .. py:attribute:: goal2time :type: Dict[str, float] .. py:property:: has_ddq :type: bool Does this trajectory enforce a configuration .. py:property:: has_dq :type: bool Does this trajectory enforce a configuration .. py:property:: has_goals :type: bool Does this trajectory refer to goal2time .. py:property:: has_poses :type: bool Does this trajectory enforce a pose. .. py:property:: has_q :type: bool Does this trajectory enforce a configuration .. py:property:: is_timed :type: bool Does this trajectory enforce time. .. py:method:: plot(show_goals_reached = False, show_figure = True) Plot this trajectory :param show_goals_reached: If set to true, the time at which goals are reached is highlighted in the plot :param show_figure: Toggle call to plt.show() which might disable some figure manipulations. :return: Figure with a plot of this trajectory. .. py:attribute:: pose :type: Optional[numpy.ndarray] :value: None .. py:method:: pose_interpolation(T_start, T_end, steps) :classmethod: Create a trajectory that interpolates between two poses. :param T_start: The start pose. :param T_end: The end pose. :param steps: The number of steps to interpolate. :return: The interpolated trajectory. .. py:attribute:: q :type: Optional[numpy.ndarray] :value: None .. py:method:: stationary(time, *, q = None, pose = None, sample_time = None) :classmethod: Create a trajectory that stays in place for time seconds. :param time: Time period to stay at this configuration. If sample time is given will be rounded up to next multiple of sample time. :param q: Configuration to stay in (mutually exclusive with pose). :param pose: Tolerated pose to stay in (mutually exclusive with configuration). :param sample_time: If given, the time at which to sample the stationary trajectory; otherwise only 0 and time are sampled. :return: The stationary trajectory, composed of two configurations / pose and a zero velocity / acceleration .. py:attribute:: t :type: Optional[Union[numpy.ndarray, float]] :value: None .. py:method:: to_json_data() Transform into a jsonable dict. .. py:method:: to_json_file(save_at, *args, **kwargs) Writes the instance to a json file. :param save_at: File location or folder to write the class to. .. py:method:: to_json_string() Return the json string representation. .. py:method:: visualize(viz, *, name_prefix = None, scale = 0.33, num_markers = 2, line_color = None, robot = None) Visualize trajectory and add num_markers triads to show orientation. :param viz: The visualizer to show the trajectory in :param name_prefix: Name of the visualized trajectory. :param scale: size of the orientation markers. :param num_markers: how many orientation markers to show. :param line_color: An optional color (3x1 vector) for each line-segment (total len(self)-1 x 3); default red; if single color vector given is broadcasted to all line segments. :param robot: A robot to evaluate a joint space trajectory on; plots the end-effector movement .. py:function:: greedy_ik_trajectory(trajectory, robot, retries = 10, **ik_kwargs) Calculate an inverse kinematic trajectory for this end-effector trajectory greedily. Greedily as in path planning, use the first best IK guess and try to move along the trajectory continuing from it. On failure retries with another random initial IK guess. :param trajectory: The end-effector trajectory to calculate the IKs for. :param robot: The robot to calculate the IKs with. :param tolerance: The tolerance for the end-effector poses (global or per step). :param retries: How many times to retry the IK calculation. :param ik_kwargs: Additional keyword arguments to pass to the IK solver; allow_random_restart will only affect finding the IK solution of the first step and ignored afterward. :return: The joint trajectory that follows the end-effector trajectory. :raises ValueError: If no valid IK trajectory could be found.