@@ -1,3 +1,5 @@
"""TODO: Add docstring."""
import enum
import pyarrow as pa
@@ -22,8 +24,7 @@ def wrap_joints_and_values(
joints: Union[list[str], pa.Array],
values: Union[int, list[int], pa.Array],
) -> pa.StructArray:
"""
Wraps joints and their corresponding values into a structured array.
"""Wrap joints and their corresponding values into a structured array.
:param joints: A list, numpy array, or pyarrow array of joint names.
:type joints: Union[list[str], np.array, pa.Array]
@@ -39,7 +40,7 @@ def wrap_joints_and_values(
:raises ValueError: If lengths of joints and values do not match.
Example:
--------
-------
joints = ["shoulder_pan", "shoulder_lift", "elbow_flex"]
values = [100, 200, 300]
struct_array = wrap_joints_and_values(joints, values)
@@ -52,8 +53,8 @@ def wrap_joints_and_values(
struct_array = wrap_joints_and_values(joints, value)
This example broadcasts the single integer value to all joints and wraps them into a structured array.
"""
"""
if isinstance(values, int):
values = [values] * len(joints)
@@ -74,11 +75,15 @@ def wrap_joints_and_values(
class TorqueMode(enum.Enum):
"""TODO: Add docstring."""
ENABLED = pa.scalar(1, pa.uint32())
DISABLED = pa.scalar(0, pa.uint32())
class OperatingMode(enum.Enum):
"""TODO: Add docstring."""
VELOCITY = pa.scalar(1, pa.uint32())
POSITION = pa.scalar(3, pa.uint32())
EXTENDED_POSITION = pa.scalar(4, pa.uint32())
@@ -152,8 +157,10 @@ MODEL_CONTROL_TABLE = {
class DynamixelBus:
"""TODO: Add docstring."""
def __init__(self, port: str, description: dict[str, (int, str)]):
"""TODO: Add docstring."""
self.port = port
self.descriptions = description
self.motor_ctrl = {}
@@ -184,9 +191,11 @@ class DynamixelBus:
self.group_writers = {}
def close(self):
"""TODO: Add docstring."""
self.port_handler.closePort()
def write(self, data_name: str, data: pa.StructArray):
"""TODO: Add docstring."""
motor_ids = [
self.motor_ctrl[motor_name.as_py()]["id"]
for motor_name in data.field("joints")
@@ -255,6 +264,7 @@ class DynamixelBus:
)
def read(self, data_name: str, motor_names: pa.Array) -> pa.StructArray:
"""TODO: Add docstring."""
motor_ids = [
self.motor_ctrl[motor_name.as_py()]["id"] for motor_name in motor_names
]
@@ -298,31 +308,41 @@ class DynamixelBus:
return wrap_joints_and_values(motor_names, values)
def write_torque_enable(self, torque_mode: pa.StructArray):
"""TODO: Add docstring."""
self.write("Torque_Enable", torque_mode)
def write_operating_mode(self, operating_mode: pa.StructArray):
"""TODO: Add docstring."""
self.write("Operating_Mode", operating_mode)
def read_position(self, motor_names: pa.Array) -> pa.StructArray:
"""TODO: Add docstring."""
return self.read("Present_Position", motor_names)
def read_velocity(self, motor_names: pa.Array) -> pa.StructArray:
"""TODO: Add docstring."""
return self.read("Present_Velocity", motor_names)
def read_current(self, motor_names: pa.Array) -> pa.StructArray:
"""TODO: Add docstring."""
return self.read("Present_Current", motor_names)
def write_goal_position(self, goal_position: pa.StructArray):
"""TODO: Add docstring."""
self.write("Goal_Position", goal_position)
def write_goal_current(self, goal_current: pa.StructArray):
"""TODO: Add docstring."""
self.write("Goal_Current", goal_current)
def write_position_p_gain(self, position_p_gain: pa.StructArray):
"""TODO: Add docstring."""
self.write("Position_P_Gain", position_p_gain)
def write_position_i_gain(self, position_i_gain: pa.StructArray):
"""TODO: Add docstring."""
self.write("Position_I_Gain", position_i_gain)
def write_position_d_gain(self, position_d_gain: pa.StructArray):
"""TODO: Add docstring."""
self.write("Position_D_Gain", position_d_gain)