reachy2_sdk.orbita.utils

This module defines the utils class functions for Orbita.

View Source

 1"""This module defines the utils class functions for Orbita."""
 2
 3from typing import Any
 4
 5import numpy as np
 6from google.protobuf.wrappers_pb2 import BoolValue, FloatValue, UInt32Value
 7from reachy2_sdk_api.component_pb2 import PIDGains
 8
 9
10def to_position(internal_pos: float) -> float:
11    """Convert an internal angular value in radians to a value in degrees.
12
13    Args:
14        internal_pos: The internal angular value in radians.
15
16    Returns:
17        The corresponding angular value in degrees.
18    """
19    return float(np.rad2deg(internal_pos))
20
21
22def to_internal_position(pos: float) -> Any:
23    """Convert an angular value in degrees to a value in radians.
24
25    The server expects values in radians, so conversion is necessary.
26
27    Args:
28        pos: The angular value in degrees.
29
30    Returns:
31        The corresponding value in radians.
32
33    Raises:
34        TypeError: If the provided value is not of type int or float.
35    """
36    try:
37        return np.deg2rad(pos)
38    except TypeError:
39        raise TypeError(f"Excepted one of: int, float, got {type(pos).__name__}")
40
41
42def unwrapped_pid_value(value: Any) -> Any:
43    """Unwrap the internal PID value from a gRPC protobuf object to a Python value.
44
45    Args:
46        value: The gRPC protobuf object containing the PID values.
47
48    Returns:
49        A tuple representing the unwrapped PID gains (p, i, d).
50    """
51    return (value.p.value, value.i.value, value.d.value)
52
53
54def wrapped_proto_value(value: bool | float | int) -> Any:
55    """Wrap a simple Python value to the corresponding gRPC protobuf type.
56
57    Args:
58        value: The value to be wrapped, which can be a bool, float, or int.
59
60    Returns:
61        The corresponding gRPC protobuf object (BoolValue, FloatValue, or UInt32Value).
62
63    Raises:
64        TypeError: If the provided value is not a supported type.
65    """
66    if isinstance(value, bool):
67        return BoolValue(value=value)
68    if isinstance(value, float):
69        return FloatValue(value=value)
70    if isinstance(value, int):
71        return UInt32Value(value=value)
72    return value
73
74
75def wrapped_pid_value(value: Any) -> Any:
76    """Wrap a simple Python value to the corresponding gRPC protobuf type.
77
78    Args:
79        value: The value to be wrapped, which can be a bool, float, or int.
80
81    Returns:
82        The corresponding gRPC protobuf object (BoolValue, FloatValue, or UInt32Value).
83
84    Raises:
85        TypeError: If the provided value is not a supported type.
86    """
87    return PIDGains(
88        p=FloatValue(value=value[0]),
89        i=FloatValue(value=value[1]),
90        d=FloatValue(value=value[2]),
91    )

def to_position(internal_pos: float) -> float: View Source

11def to_position(internal_pos: float) -> float:
12    """Convert an internal angular value in radians to a value in degrees.
13
14    Args:
15        internal_pos: The internal angular value in radians.
16
17    Returns:
18        The corresponding angular value in degrees.
19    """
20    return float(np.rad2deg(internal_pos))

Convert an internal angular value in radians to a value in degrees.

Arguments:

internal_pos: The internal angular value in radians.

Returns:

The corresponding angular value in degrees.

def to_internal_position(pos: float) -> Any: View Source

23def to_internal_position(pos: float) -> Any:
24    """Convert an angular value in degrees to a value in radians.
25
26    The server expects values in radians, so conversion is necessary.
27
28    Args:
29        pos: The angular value in degrees.
30
31    Returns:
32        The corresponding value in radians.
33
34    Raises:
35        TypeError: If the provided value is not of type int or float.
36    """
37    try:
38        return np.deg2rad(pos)
39    except TypeError:
40        raise TypeError(f"Excepted one of: int, float, got {type(pos).__name__}")

Convert an angular value in degrees to a value in radians.

The server expects values in radians, so conversion is necessary.

Arguments:

pos: The angular value in degrees.

Returns:

The corresponding value in radians.

Raises:

TypeError: If the provided value is not of type int or float.

def unwrapped_pid_value(value: Any) -> Any: View Source

43def unwrapped_pid_value(value: Any) -> Any:
44    """Unwrap the internal PID value from a gRPC protobuf object to a Python value.
45
46    Args:
47        value: The gRPC protobuf object containing the PID values.
48
49    Returns:
50        A tuple representing the unwrapped PID gains (p, i, d).
51    """
52    return (value.p.value, value.i.value, value.d.value)

Unwrap the internal PID value from a gRPC protobuf object to a Python value.

Arguments:

value: The gRPC protobuf object containing the PID values.

Returns:

A tuple representing the unwrapped PID gains (p, i, d).

def wrapped_proto_value(value: bool | float | int) -> Any: View Source

55def wrapped_proto_value(value: bool | float | int) -> Any:
56    """Wrap a simple Python value to the corresponding gRPC protobuf type.
57
58    Args:
59        value: The value to be wrapped, which can be a bool, float, or int.
60
61    Returns:
62        The corresponding gRPC protobuf object (BoolValue, FloatValue, or UInt32Value).
63
64    Raises:
65        TypeError: If the provided value is not a supported type.
66    """
67    if isinstance(value, bool):
68        return BoolValue(value=value)
69    if isinstance(value, float):
70        return FloatValue(value=value)
71    if isinstance(value, int):
72        return UInt32Value(value=value)
73    return value

Wrap a simple Python value to the corresponding gRPC protobuf type.

Arguments:

value: The value to be wrapped, which can be a bool, float, or int.

Returns:

The corresponding gRPC protobuf object (BoolValue, FloatValue, or UInt32Value).

Raises:

TypeError: If the provided value is not a supported type.

def wrapped_pid_value(value: Any) -> Any: View Source

76def wrapped_pid_value(value: Any) -> Any:
77    """Wrap a simple Python value to the corresponding gRPC protobuf type.
78
79    Args:
80        value: The value to be wrapped, which can be a bool, float, or int.
81
82    Returns:
83        The corresponding gRPC protobuf object (BoolValue, FloatValue, or UInt32Value).
84
85    Raises:
86        TypeError: If the provided value is not a supported type.
87    """
88    return PIDGains(
89        p=FloatValue(value=value[0]),
90        i=FloatValue(value=value[1]),
91        d=FloatValue(value=value[2]),
92    )

Wrap a simple Python value to the corresponding gRPC protobuf type.

Arguments:

value: The value to be wrapped, which can be a bool, float, or int.

Returns:

The corresponding gRPC protobuf object (BoolValue, FloatValue, or UInt32Value).

Raises:

TypeError: If the provided value is not a supported type.