toddlerbot.algorithms package¶

Submodules¶

toddlerbot.algorithms.zmp_planner module¶

Zero Moment Point (ZMP) planning algorithms.

This module implements ZMP-based trajectory planning for bipedal walking, based on “A Generalized ZMP Preview Control for Bipedal Walking” by Kajita et al.

class toddlerbot.algorithms.zmp_planner.ExpPlusPPoly(K: Array | ndarray[Any, dtype[float32]], A: Array | ndarray[Any, dtype[float32]], alpha: Array | ndarray[Any, dtype[float32]], ppoly: PPoly)¶

Bases: object

Exponential plus piecewise polynomial class.

derivative(order: int) → ExpPlusPPoly¶

Computes the derivative of the ExpPlusPPoly object to a specified order.

Parameters:: order (int) – The order of the derivative to compute.
Returns:: A new ExpPlusPPoly object representing the derivative of the specified order.
Return type:: ExpPlusPPoly

value(t: float | Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]¶

Evaluate the value of a piecewise polynomial with an exponential component at a given time.

Parameters:: t (float | ArrayType) – The time or array of times at which to evaluate the function.
Returns:: The evaluated value(s) of the function at the specified time(s).
Return type:: ArrayType

class toddlerbot.algorithms.zmp_planner.PPoly(c: Array | ndarray[Any, dtype[float32]], x: Array | ndarray[Any, dtype[float32]])¶

Bases: object

Piecewise polynomial class.

derivative(order: int = 1) → PPoly¶

Compute the derivative of the piecewise polynomial.

Parameters:: order (int) – The order of the derivative to compute. Defaults to 1.
Returns:: A new piecewise polynomial representing the derivative of the specified order.
Return type:: PPoly

class toddlerbot.algorithms.zmp_planner.ZMPPlanner¶

Bases: object

Zero Moment Point (ZMP) planner class based on the paper “A Generalized ZMP Preview Control for Bipedal Walking” by Kajita et al.

com_acc_to_cop(x: Array | ndarray[Any, dtype[float32]], u: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]¶

Calculates the control output based on the current state and input.

This function computes the control output using the state vector x and the input vector u by applying the transformation matrices C and D. It requires that the planning step has been completed prior to execution.

Parameters:

x (ArrayType) – The current state vector.
u (ArrayType) – The current input vector.

Returns:

The resulting control output vector.

Return type:

ArrayType

Raises:

ValueError – If the planning step has not been completed.

get_desired_zmp(time: float | Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]¶

Retrieves the desired Zero Moment Point (ZMP) at a specified time.

Parameters:: time (float | ArrayType) – The time or array of times at which to retrieve the desired ZMP.
Returns:: The desired ZMP corresponding to the given time.
Return type:: ArrayType
Raises:: ValueError – If the plan has not been initialized by calling the plan method.

get_desired_zmp_traj()¶

Retrieves the desired Zero Moment Point (ZMP) trajectory.

Returns:: A tuple containing the time steps and the desired ZMP trajectory.
Return type:: tuple
Raises:: ValueError – If the plan has not been executed prior to calling this method.

get_nominal_com(time: float | Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]¶

Retrieve the nominal center of mass (CoM) position at a specified time or times.

Parameters:: time (float | ArrayType) – The time or array of times at which to evaluate the CoM position.
Returns:: The nominal CoM position(s) corresponding to the given time(s).
Return type:: ArrayType
Raises:: ValueError – If the plan has not been initialized by calling the plan method.

get_nominal_com_acc(time: float | Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]¶

Calculates the nominal center of mass acceleration at a given time or times.

Parameters:: time (float | ArrayType) – The time or array of times at which to evaluate the center of mass acceleration.
Returns:: The center of mass acceleration at the specified time(s).
Return type:: ArrayType
Raises:: ValueError – If the plan has not been initialized by calling the plan method.

get_nominal_com_vel(time: float | Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]¶

Retrieve the nominal center of mass velocity at a specified time.

Parameters:: time (float | ArrayType) – The time or array of times at which to evaluate the center of mass velocity.
Returns:: The nominal center of mass velocity at the specified time(s).
Return type:: ArrayType
Raises:: ValueError – If the plan has not been called prior to this method.

get_optim_com_acc(time: float | Array | ndarray[Any, dtype[float32]], x: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]¶

Calculates the optimal center of mass acceleration.

This function computes the optimal center of mass (CoM) acceleration based on the current state and time. It requires that a planning step has been executed prior to its invocation.

Parameters:

time (float | ArrayType) – The current time or an array of time values.
x (ArrayType) – The current state vector.

Returns:

The calculated optimal CoM acceleration.

Return type:

ArrayType

Raises:

ValueError – If the planning step has not been executed before calling this function.

plan(time_steps: Array | ndarray[Any, dtype[float32]], zmp_d: List[Array | ndarray[Any, dtype[float32]]], x0: Array | ndarray[Any, dtype[float32]], com_z: float, Qy: Array | ndarray[Any, dtype[float32]], R: Array | ndarray[Any, dtype[float32]])¶

Plans the Center of Mass (CoM) trajectory and control gains for a system based on the desired Zero Moment Point (ZMP) trajectory.

Parameters:

time_steps (ArrayType) – Array of time steps for the trajectory.
zmp_d (List[ArrayType]) – List of desired ZMP positions at each time step.
x0 (ArrayType) – Initial state of the system.
com_z (float) – Height of the Center of Mass.
Qy (ArrayType) – Weighting matrix for the output in the cost function.
R (ArrayType) – Weighting matrix for the control input in the cost function.

This function computes the time-varying linear and constant terms in the value function, solves for parameters of the CoM trajectory, and updates the planned CoM position, velocity, and acceleration.

toddlerbot.algorithms.zmp_walk module¶

class toddlerbot.algorithms.zmp_walk.ZMPWalk(robot: Robot, cycle_time: float, single_double_ratio: float = 2.0, foot_step_height: float = 0.05, control_dt: float = 0.02, control_cost_Q: float = 1.0, control_cost_R: float = 0.1)¶

Bases: object

ZMP-based walking trajectory generator for humanoid robots.

This class implements a Zero Moment Point (ZMP) based walking algorithm that generates stable walking gaits for humanoid robots. It uses linear model predictive control (MPC) to plan Center of Mass (CoM) trajectories and generates corresponding joint trajectories for stable locomotion.

The algorithm supports various walking patterns including forward/backward walking, sideways stepping, and turning, with configurable gait parameters such as cycle time, step height, and single/double support ratios.

build_lookup_table(robot_suffix: str, command_range: List[List[float]] = [[-0.2, 0.4], [-0.2, 0.2], [-0.8, 0.8]], interval: float = 0.05)¶

Builds a lookup table for command references and associated data.

Parameters:

command_range (List[List[float]]) – A list of command ranges for each dimension, where each range is defined by a start and stop value. Defaults to [[-0.2, 0.4], [-0.2, 0.2], [-0.8, 0.8]].
interval (float) – The interval at which to sample command values within each range. Defaults to 0.02.

Returns:

A tuple containing:

A list of tuples representing the command keys.
A list of arrays for the center of mass reference data.
A list of arrays for the leg joint position reference data.
A list of arrays for the stance mask reference data.

Return type:

Tuple[List[Tuple[float, …]], List[ArrayType], List[ArrayType], List[ArrayType]]

compute_foot_trajectories(time_steps: Array | ndarray[Any, dtype[float32]], footsteps: List[Array | ndarray[Any, dtype[float32]]]) → Tuple[Array | ndarray[Any, dtype[float32]], ...]¶

Compute the trajectories for the left and right foot positions and orientations, as well as the torso orientation and stance mask over a series of time steps.

Parameters:

time_steps (ArrayType) – An array of time steps at which the trajectories are computed.
footsteps (List[ArrayType]) – A list of arrays representing the footstep positions and orientations, with each array containing the x, y, and orientation values.

Returns:

A tuple containing arrays for the left foot position trajectory, left foot orientation trajectory, right foot position trajectory, right foot orientation trajectory, torso orientation trajectory, and stance mask trajectory.

Return type:

Tuple[ArrayType, …]

foot_ik(target_foot_pos: Array | ndarray[Any, dtype[float32]], target_foot_ori: Array | ndarray[Any, dtype[float32]], side: str = 'left') → Array | ndarray[Any, dtype[float32]]¶

Calculates the inverse kinematics for a robot’s foot, determining the necessary joint angles to achieve a specified foot position and orientation.

Parameters:

target_foot_pos (ArrayType) – The target position of the foot in 3D space, specified as an array with shape (n, 3), where each row represents the x, y, and z coordinates.
target_foot_ori (ArrayType) – The target orientation of the foot, specified as an array with shape (n, 3), where each row represents the roll, pitch, and yaw angles.
side (str, optional) – The side of the robot for which the calculations are performed. Defaults to “left”.

Returns:

An array of joint angles required to achieve the specified foot position and orientation, with shape (n, 6), where each row contains the angles for hip pitch, hip roll, hip yaw, knee pitch, ankle roll, and ankle pitch.

Return type:

ArrayType

mujoco_replay(sim: MuJoCoSim, leg_joint_pos_ref: List[Array | ndarray[Any, dtype[float32]]], stance_mask_ref: List[Array | ndarray[Any, dtype[float32]]]) → dict¶

plan(path_pos: Array | ndarray[Any, dtype[float32]], path_quat: Array | ndarray[Any, dtype[float32]], command: Array | ndarray[Any, dtype[float32]], total_time: float, rotation_radius: float = 0.1) → Tuple[Array | ndarray[Any, dtype[float32]], Array | ndarray[Any, dtype[float32]], Array | ndarray[Any, dtype[float32]], Array | ndarray[Any, dtype[float32]]]¶

Plans the trajectory for a bipedal robot’s movement based on the given path and command.

Parameters:

path_pos (ArrayType) – The initial position of the path as a 2D array.
path_quat (ArrayType) – The initial orientation of the path in quaternion form.
command (ArrayType) – The desired movement command, including translation and rotation.
total_time (float, optional) – The total time for the planned movement. Defaults to 20.0.
rotation_radius (float, optional) – The radius for rotation movements. Defaults to 0.1.

Returns:

A tuple containing:

The desired Zero Moment Points (ZMPs) as an array.
The trajectory of the center of mass (CoM) as an array.
The reference positions for leg joints as an array.
The stance mask reference indicating foot contact states as an array.

Return type:

Tuple[ArrayType, ArrayType, ArrayType, ArrayType]

solve_ik(left_foot_pos_traj: Array | ndarray[Any, dtype[float32]], left_foot_ori_traj: Array | ndarray[Any, dtype[float32]], right_foot_pos_traj: Array | ndarray[Any, dtype[float32]], right_foot_ori_traj: Array | ndarray[Any, dtype[float32]], com_pose_traj: Array | ndarray[Any, dtype[float32]])¶

Solves the inverse kinematics (IK) for a bipedal robot’s legs based on foot and center of mass (COM) trajectories.

This function computes the joint positions for the left and right legs by adjusting the foot position and orientation trajectories relative to the COM trajectory. It then uses inverse kinematics to determine the necessary joint angles for achieving the desired foot trajectories.

Parameters:

left_foot_pos_traj (ArrayType) – Trajectory of the left foot positions.
left_foot_ori_traj (ArrayType) – Trajectory of the left foot orientations.
right_foot_pos_traj (ArrayType) – Trajectory of the right foot positions.
right_foot_ori_traj (ArrayType) – Trajectory of the right foot orientations.
com_pose_traj (ArrayType) – Trajectory of the center of mass poses.

Returns:

Combined joint position trajectories for both left and right legs.

Return type:

ArrayType

Module contents¶

Algorithms module for motion planning and control in ToddlerBot.

This package contains various algorithmic implementations for: - Zero Moment Point (ZMP) based walking algorithms - Motion planning and trajectory generation - Balance and stability control algorithms - Gait pattern generation and optimization - Inverse kinematics and dynamics solvers

The algorithms support both research and practical applications, providing robust solutions for humanoid robot locomotion and control.