motion_stack.core.utils package

Submodules

motion_stack.core.utils.csv module

simple csv utilities

motion_stack.core.utils.csv.update_csv(file_path, new_str, new_float)[source]
Return type:

Tuple[str, Optional[str]]

Parameters:

  • new_str (str)

  • new_float (float)

motion_stack.core.utils.csv.csv_to_dict(file_path)[source]
Return type:

Optional[Dict[str, float]]

motion_stack.core.utils.hypersphere_clamp module

Vectorized functions to clamp onto an hypershpere

Author: Elian NEPPEL Lab: SRL, Moonshot team

motion_stack.core.utils.hypersphere_clamp.SAMPLING_STEP = 0.01

Type:    float

will sample every 0.01 for a unit hypersphere if you use the radii, it is equivalent sampling every 0.01 * radii

motion_stack.core.utils.hypersphere_clamp.ORD = inf

Type:    float

Order of the norm for clamping

motion_stack.core.utils.hypersphere_clamp.clamp_to_unit_hs(start, end, sampling_step=0.01, norm_ord=inf)[source]

Finds the farthest point on the segment that is inside the unit hypersphere.

Parameters:

  • start (NDArray[Shape[N], float64]) – start of the segment

  • end (NDArray[Shape[N], float64]) – end of the segment

  • sampling_step (float) – distance between each sample.

  • norm_ord (int or numpy inf) – order of the distance/norm used to create the hypersphere

Returns:

Farthest point on the segment that is inside the unit hypersphere

Return type:

NDArray[Shape[N], float64]

motion_stack.core.utils.hypersphere_clamp.clamp_to_sqewed_hs(center, start, end, radii, norm_ord=inf)[source]

Finds the farthest point on the segment that is inside the sqewed hypersphere.

radii of the hypersphere in each dimensions is computed by streching the space in each dimension, then computing relative to the unit hypersphere, then unstreching.

Return type:

NDArray[Shape[N], floating]

Parameters:

  • center (NDArray[Shape[N], floating])

  • start (NDArray[Shape[N], floating])

  • end (NDArray[Shape[N], floating])

  • radii (NDArray[Shape[N], floating])

motion_stack.core.utils.hypersphere_clamp.fuse_xyz_quat(pose: motion_stack.core.utils.pose.XyzQuat[nptyping.ndarray.NDArray[nptyping.base_meta_classes.Shape[3], numpy.floating], quaternion.quaternion]) → nptyping.ndarray.NDArray[nptyping.base_meta_classes.Shape[7], numpy.floating][source]
motion_stack.core.utils.hypersphere_clamp.fuse_xyz_quat(pose: List[motion_stack.core.utils.pose.XyzQuat[nptyping.ndarray.NDArray[nptyping.base_meta_classes.Shape[3], numpy.floating], quaternion.quaternion]])
motion_stack.core.utils.hypersphere_clamp.fuse_xyz_quat(pose)

Fuses XyzQuat objects into a flat numpy array.

  • If input is a single XyzQuat, returns a (7,) fused array.

  • If input is a list, returns a flat (n*7,) fused array.

Parameters:

pose (XyzQuat[NDArray[Shape[3], floating], quaternion] | List[XyzQuat[NDArray[Shape[3], floating], quaternion]]) – A single or list of XyzQuat objects containing position and orientation.

Returns:

The fused representation(s) as (7,) for a single input or (n*7,) for batched input.

Return type:

NDArray[Shape[7 n], floating]

motion_stack.core.utils.hypersphere_clamp.unfuse_xyz_quat(arr)[source]

Unpacks a fused 7D array back into XYZ and Quaternion components.

Parameters:

arr (NDArray[Shape[7 n], floating]) – Flattened fused representation(s).

Returns:

The unfused position(s) and orientation(s).

Return type:

List[XyzQuat[NDArray[Shape[3], floating], quaternion]]

motion_stack.core.utils.hypersphere_clamp.clamp_multi_xyz_quat(center, start, end, radii, norm_ord=inf)[source]

wrapper for clamp_to_sqewed_hs specialized in several 3D coordinate + one quaternion.

The math for the quaternion is wrong (lerp instead of slerp). So: Center and start quat should not be opposite from each-other. Precision goes down if they are far appart. But it’s not so bad.

Parameters:

  • center (List[XyzQuat[NDArray[Shape[3], floating], quaternion]]) – center from which not to diverge

  • start (List[XyzQuat[NDArray[Shape[3], floating], quaternion]]) – start point of the interpolation

  • end (List[XyzQuat[NDArray[Shape[3], floating], quaternion]]) – end point of the interpolation

  • radii (List[XyzQuat[float, float]] | XyzQuat[float, float]) – allowed divergence for coord and quat

  • norm_ord (int or numpy inf) – order of the distance/norm used to create the hypersphere.

Returns:

Futhest point on the start-end segment that is inside the hypersphere of center center and radii radii.

Return type:

List[XyzQuat[NDArray[Shape[3], floating], quaternion]]

motion_stack.core.utils.hypersphere_clamp.clamp_xyz_quat(center, start, end, radii, norm_ord=2)[source]

wrapper for clamp_to_sqewed_hs specialized in one 3D coordinate + one quaternion.

The math for the quaternion is wrong (lerp instead of slerp). So: Center and start quat should not be opposite from each-other. Precision goes down if they are far appart. But it’s not so bad.

Parameters:

  • center (XyzQuat[NDArray[Shape[3], floating], quaternion]) – center from which not to diverge

  • start (XyzQuat[NDArray[Shape[3], floating], quaternion]) – start point of the interpolation

  • end (XyzQuat[NDArray[Shape[3], floating], quaternion]) – end point of the interpolation

  • radii (XyzQuat[float, float]) – allowed divergence for coord and quat

  • norm_ord (int or numpy inf) – order of the distance/norm used to create the hypersphere.

Returns:

Futhest point on the start-end segment that is inside the hypersphere of center center and radii radii.

Return type:

XyzQuat[NDArray[Shape[3], floating], quaternion]

motion_stack.core.utils.joint_mapper module

motion_stack.core.utils.joint_mapper.operate_sub_shapers(shaper1, shaper2, op)[source]
Return type:

Optional[Callable[[float], float]]

Parameters:

  • shaper1 (Callable[[float], float] | None)

  • shaper2 (Callable[[float], float] | None)

  • op (Callable[[float, float], float])

motion_stack.core.utils.joint_mapper.eggify_shapers(inner, outer)[source]
Return type:

Optional[Callable[[float], float]]

Parameters:

  • inner (Callable[[float], float] | None)

  • outer (Callable[[float], float] | None)

class motion_stack.core.utils.joint_mapper.Shaper(position=None, velocity=None, effort=None)[source]

Bases: object

Holds and applies functions to position, velocity and effort fields.

If None, the indentity is used.

Parameters:

  • position (Callable[[float], float] | None)

  • velocity (Callable[[float], float] | None)

  • effort (Callable[[float], float] | None)

position = None

Type:    Optional[Callable[[float], float]]

velocity = None

Type:    Optional[Callable[[float], float]]

effort = None

Type:    Optional[Callable[[float], float]]

motion_stack.core.utils.joint_mapper.reverse_dict(d)[source]
Return type:

Dict

Parameters:

d (Dict)

motion_stack.core.utils.joint_mapper.remap_names(states, mapping)[source]
Parameters:

  • states (List[JState])

  • mapping (Dict[str, str])

motion_stack.core.utils.joint_mapper.apply_shaper(state, shaper)[source]
Parameters:
motion_stack.core.utils.joint_mapper.shape_states(states, mapping)[source]
Parameters:

motion_stack.core.utils.joint_state module

class motion_stack.core.utils.joint_state.JState(name, time=None, position=None, velocity=None, effort=None)[source]

Bases: object

Parameters:

  • name (str)

  • time (Time | None)

  • position (float | None)

  • velocity (float | None)

  • effort (float | None)

name

Type:    str

time = None

Type:    Optional[Time]

position = None

Type:    Optional[float]

velocity = None

Type:    Optional[float]

effort = None

Type:    Optional[float]

getattr(name: Literal['name']) → str[source]
getattr(name: Literal['time']) → motion_stack.core.utils.time.Time
getattr(name: Literal['position', 'velocity', 'effort']) → float
getattr(name)
Return type:

Any

Parameters:

name (str)

copy()[source]
Return type:

JState

property is_initialized

bool

Type:

rtype

motion_stack.core.utils.joint_state.js_from_dict_list(dil)[source]
Return type:

List[JState]

Parameters:

dil (Dict[Literal['position', 'velocity', 'effort', 'name', 'time'], ~typing.List])

motion_stack.core.utils.joint_state.impose_state(onto, fromm)[source]
Return type:

JState

Parameters:
motion_stack.core.utils.joint_state.js_changed(j1, j2, delta)[source]
Return type:

bool

Parameters:
motion_stack.core.utils.joint_state.js_diff(j1, j2)[source]
Return type:

JState

Parameters:

motion_stack.core.utils.math module

motion_stack.core.utils.math.qt_normalize(q)[source]
Parameters:

q (quaternion)

motion_stack.core.utils.math.qt_repr(q)[source]
Return type:

str

Parameters:

q (quaternion)

motion_stack.core.utils.math.angle_with_unit_quaternion(q)[source]
motion_stack.core.utils.math.patch_numpy_display_light(floating_points=2)[source]
Parameters:

floating_points (int)

motion_stack.core.utils.pose module

motion_stack.core.utils.pose.T1 = TypeVar(T1)

Type:    TypeVar

Invariant TypeVar.

motion_stack.core.utils.pose.T2 = TypeVar(T2)

Type:    TypeVar

Invariant TypeVar.

class motion_stack.core.utils.pose.XyzQuat(xyz, quat)[source]

Bases: Generic[~T1, ~T2]

Tuplelike containing spatial and rotation data

Parameters:

  • xyz (T1)

  • quat (T2)

xyz

Type:    ~T1

quat

Type:    ~T2

classmethod from_tuple(tup)[source]
Return type:

Self

Parameters:

tup (Tuple[T1, T2])

class motion_stack.core.utils.pose.Pose(time, xyz, quat)[source]

Bases: object

Parameters:

  • time (Time)

  • xyz (NDArray[Shape[3], floating])

  • quat (quaternion)

time

Type:    Time

xyz

Type:    NDArray[Shape[3], floating]

quat

Type:    quaternion

close2zero(atol=(1, 0.017453292519943295))[source]
Return type:

bool

Parameters:

atol (Tuple[float, float])

copy()[source]
class motion_stack.core.utils.pose.VelPose(time, lin, rvec)[source]

Bases: object

Parameters:

  • time (Time)

  • lin (NDArray[Shape[3], floating])

  • rvec (NDArray[Shape[3], floating])

time

Type:    Time

lin

Type:    NDArray[Shape[3], floating]

rvec

Type:    NDArray[Shape[3], floating]

motion_stack.core.utils.printing module

class motion_stack.core.utils.printing.TCOL[source]

Bases: object

Colors for the terminal

HEADER = '\x1b[95m'

Type:    str

OKBLUE = '\x1b[94m'

Type:    str

OKCYAN = '\x1b[96m'

Type:    str

OKGREEN = '\x1b[92m'

Type:    str

WARNING = '\x1b[33;20m'

Type:    str

FAIL = '\x1b[91m'

Type:    str

ENDC = '\x1b[0m'

Type:    str

BOLD = '\x1b[1m'

Type:    str

UNDERLINE = '\x1b[4m'

Type:    str

motion_stack.core.utils.printing.list_cyanize(l, default_color=None)[source]

Makes each element of a list cyan.

Parameters:

  • l (Iterable) – Iterable

  • default_color (str | None) – color to go back to outise of the cyan

Return type:

str

Returns:

Return type:

str

Parameters:

  • l (Iterable)

  • default_color (str | None)

motion_stack.core.utils.robot_parsing module

Uses rtb to parse the robot URDF data

motion_stack.core.utils.robot_parsing.replace_incompatible_char_ros2(string_to_correct)[source]

Sanitizes strings for use by ros2.

replace character that cannot be used for Ros2 Topics by _ inserts “WARN” in front if topic starts with incompatible char

Return type:

str

Parameters:

string_to_correct (str)

motion_stack.core.utils.robot_parsing.get_limit(joint)[source]

Returns the limits of a joint from rtb parsing

Return type:

Tuple[float, float]

Parameters:

joint (Joint)

motion_stack.core.utils.robot_parsing.make_ee(ee_string, number_default=0)[source]
Return type:

Union[None, str, int]

Parameters:

  • ee_string (str)

  • number_default (int)

motion_stack.core.utils.robot_parsing.joint_by_joint_fk(et_chain, joint_names)[source]
Return type:

List[Tuple[str, NDArray]]

Parameters:

  • et_chain (ETS)

  • joint_names (List[str])

motion_stack.core.utils.robot_parsing.load_set_urdf_raw(urdf, end_effector_name=None, start_effector_name=None)[source]

Enables calling load_set_urdf with the full urdf string instead of the path

Return type:

Tuple[Robot, ETS, List[str], List[Joint], Optional[Link]]

Parameters:

  • urdf (str)

  • end_effector_name (str | int | None)

  • start_effector_name (str | None)

motion_stack.core.utils.robot_parsing.load_set_urdf(urdf_path, end_effector_name=None, start_effector_name=None)[source]

I am so sorry. This works to parse the urdf I don’t have time to explain

Note

will change, I hate this

This is terrible and still in the code

Parameters:

  • urdf_path (str)

  • end_effector_name (str | int | None)

  • start_effector_name (str | None)

Return type:

Tuple[Robot, ETS, List[str], List[Joint], Link | None]

Returns:

Return type:

Tuple[Robot, ETS, List[str], List[Joint], Optional[Link]]

Parameters:

  • urdf_path (str)

  • end_effector_name (str | int | None)

  • start_effector_name (str | None)

motion_stack.core.utils.static_executor module

Defines an executor to be replaced by ros2 or other one.

For now the executor have minimal responsibility, limited to time and launch parameters.

motion_stack.core.utils.static_executor.default_param = [('urdf', <class 'str'>, ''), ('leg_number', <class 'int'>, 0), ('end_effector_name', <class 'str'>, ''), ('start_effector_name', <class 'str'>, ''), ('mvmt_update_rate', <class 'float'>, 10.0), ('joint_buffer', typing.List[float], [0.5, 0.0008726646259971648, 0.00017453292519943296, 1.7453292519943296e-05]), ('angle_syncer_delta', <class 'float'>, 0.17453292519943295), ('add_joints', typing.List[str], ['']), ('ignore_limits', <class 'bool'>, False), ('limit_margin', <class 'float'>, 0.0), ('speed_mode', <class 'bool'>, False), ('control_rate', <class 'float'>, 30.0)]

Type:    List[Tuple[str, type, Any]]

The parameters of the motion stack in python form, with their type and default value.

 1 default_param: List[Tuple[str, type, Any]] = [
 2     ("urdf", str, ""),
 3     #: raw urdf string
 4     ("leg_number", int, 0),
 5     # number associated with a leg, similar to a workspace
 6     ("end_effector_name", str, ""),
 7     # end effector associated with a leg, the kinematic chain used for IK will
 8     # go from the root link of the URDF (usually base_link) to the end effector
 9     # link (specified in this parameter). the URDF will be parsed to find this
10     # link name. Make sure it exists. you can also provide a number (as a string)
11     # instead of a link_name. If you do this the Nth longest kinematic path
12     # (sequence of link where each link is connected to exactly one other link)
13     # from the root of the URDF will be used for IK Basically, if you use only
14     # one limb, set this as "0", and it will pick the right ee.
15     ("start_effector_name", str, ""),
16     # setting this manually, works with the motion stack, but not for Rviz and
17     # ros2's tf, so be carefull. In ros, the baselink must be the root of the
18     # tf tree, it cannot have a parent and there can only be one baselink.
19     # Leaving this empty and properly setting your URDF baselink is
20     # recommended.
21     ("mvmt_update_rate", float, 10.0),
22     # update rate used through out the stack
23     (
24         "joint_buffer",
25         List[float],
26         [
27             0.5, # time: seconds
28             np.deg2rad(0.05), # position: rad
29             np.deg2rad(0.01), # velocity: rad/s
30             np.deg2rad(0.001), # effort: N.m
31         ],
32     ),
33     # lvl1 incorporates a joint state buffer to not repeat states that are
34     # identical. If the difference between current and previously sent state
35     # exceed any of those values, a state message is sent.
36     ("angle_syncer_delta", float, np.deg2rad(20)),
37     # The difference command-sensor cannot be further than this delta.
38     # Small values slow down execution but are safer.
39     # Using zero disables this feature.
40     ("add_joints", List[str], [""]),
41     # manually adds joints for lvl1 if they are not in the urdf
42     ("ignore_limits", bool, False),
43     # joint limits set in the URDF will be ignored
44     ("limit_margin", float, 0.0),
45     # adds a additional margin to the limits of the URDF (in rad)
46     ("speed_mode", bool, False),
47     # lvl1 will send speed commands to the motors, using angle readings as
48     # feedback for a PID.
49     ("control_rate", float, 30.0),  # update rate for speed mode PID only
50 ]
motion_stack.core.utils.static_executor.extract_inner_type(list_type)[source]

Extracts the inner type from a typing.List, such as List[float].

Parameters:

list_type (type) – A type hint, e.g., List[float].

Returns:

The inner type of the list, e.g., float.

class motion_stack.core.utils.static_executor.Spinner[source]

Bases: ABC

alias = ''

Type:    str

abstract now()[source]
Return type:

Time

abstract error(*args, **kwargs)[source]
Return type:

None

abstract warn(*args, **kwargs)[source]
Return type:

None

abstract info(*args, **kwargs)[source]
Return type:

None

abstract debug(*args, **kwargs)[source]
Return type:

None

abstract get_parameter(name, value_type, default=None)[source]
Return type:

Any

Parameters:

  • name (str)

  • value_type (type)

class motion_stack.core.utils.static_executor.PythonSpinner[source]

Bases: Spinner

now()[source]
Return type:

Time

change_time(time)[source]
Parameters:

time (Time)

error(*args)[source]
Return type:

None

warn(*args)[source]
Return type:

None

info(*args)[source]
Return type:

None

debug(*args)[source]
Return type:

None

class motion_stack.core.utils.static_executor.FlexNode(spinner)[source]

Bases: object

Parameters:

spinner (Spinner)

spinner

Type:    Spinner

must be initialized and spinning already

startup_time

Type:    Time

now()[source]
Return type:

Time

error(*args)[source]
warn(*args)[source]
info(*args)[source]
debug(*args)[source]
property ms_param

Dict[str, Any]

Type:

rtype

motion_stack.core.utils.time module

class motion_stack.core.utils.time.Time(value=None, *, sec=None, nano=None)[source]

Bases: int

nano()[source]

Return the time as nanoseconds.

sec()[source]

Return the time as fractional seconds.