pyrcf.components.controllers.segway_pid_balance_controller ========================================================== .. py:module:: pyrcf.components.controllers.segway_pid_balance_controller .. autoapi-nested-parse:: Adapted from wheel_controller from Stéphane Caron's upkie repo. (https://github.com/upkie/upkie/blob/main/pid_balancer/wheel_controller.py). Most of the comments from the original code still included. # License from upkie repo code included below: # SPDX-License-Identifier: Apache-2.0 # Copyright 2022 Stéphane Caron # Copyright 2023 Inria Classes ------- .. autoapisummary:: pyrcf.components.controllers.segway_pid_balance_controller.SegwayPIDBalanceController Module Contents --------------- .. py:class:: SegwayPIDBalanceController(pinocchio_interface: pyrcf.utils.kinematics_dynamics.pinocchio_interface.PinocchioInterface, gains: Gains = Gains(), air_return_period: float = 1.0, fall_pitch: float = 1.0, max_ground_velocity: float = 2.0, max_integral_error_velocity: float = 10.0, max_target_accel: float = 0.3, max_target_distance: float = 1.5, max_target_velocity: float = 0.6, max_yaw_accel: float = 10.0, max_yaw_velocity: float = 1.0, turning_deadband: float = 0.015, turning_decision_time: float = 0.2, wheel_radius: float = 0.06, wheel_distance: float = 0.3048, wheel_ee_ids: List[int] = [0, 1], wheel_joint_ids: List[int] = [2, 5], wheel_joint_directions: List[int] = [1, -1], joint_torque_limits: List[float] = [-1.0, 1.0]) Bases: :py:obj:`pyrcf.components.controllers.controller_base.ControllerBase` A simple PD balance controller for a 2-wheel segway type robot. .. py:class:: Gains Gains for this controller. :param pitch_damping: Pitch error (normalized) damping gain. Corresponds to the proportional term of the velocity PI controller, equivalent to the derivative term of the acceleration PD controller. :param pitch_stiffness: Pitch error (normalized) stiffness gain. Corresponds to the integral term of the velocity PI controller, equivalent to the proportional term of the acceleration PD controller. :param footprint_position_damping: Position error (normalized) damping gain. Corresponds to the proportional term of the velocity PI controller, equivalent to the derivative term of the acceleration PD controller. :param footprint_position_stiffness: Position error (normalized) stiffness gain. Corresponds to the integral term of the velocity PI controller, equivalent to the proportional term of the acceleration PD controller. :param joint_kp: Joint position tracking gain (for non-wheel joints). :param joint_kd: Wheel joint velocity gain (only for wheel joints) :param turning_gain_scale: joint velocity additional gains when there is turning probability (yaw command). .. py:attribute:: pitch_damping :type: float :value: 10.0 .. py:attribute:: pitch_stiffness :type: float :value: 20.0 .. py:attribute:: footprint_position_damping :type: float :value: 5.0 .. py:attribute:: footprint_position_stiffness :type: float :value: 2.0 .. py:attribute:: joint_kp :type: float :value: 200.0 .. py:attribute:: joint_kd :type: float :value: 2.0 .. py:attribute:: turning_gain_scale :type: float :value: 2.0 .. py:attribute:: air_return_period .. py:attribute:: fall_pitch .. py:attribute:: gains .. py:attribute:: integral_error_velocity :value: 0.0 .. py:attribute:: max_ground_velocity .. py:attribute:: max_integral_error_velocity .. py:attribute:: max_target_accel .. py:attribute:: max_target_distance .. py:attribute:: max_target_velocity .. py:attribute:: max_yaw_accel .. py:attribute:: max_yaw_velocity .. py:attribute:: pitch :value: 0.0 .. py:attribute:: target_ground_position :value: 0.0 .. py:attribute:: target_ground_velocity :value: 0.0 .. py:attribute:: target_yaw_position :value: 0.0 .. py:attribute:: target_yaw_velocity :value: 0.0 .. py:attribute:: turning_deadband .. py:attribute:: turning_decision_time .. py:attribute:: turning_probability :value: 0.0 .. py:attribute:: wheel_radius .. py:attribute:: wheel_ee_ids .. py:attribute:: wheel_distance .. py:attribute:: wheel_joint_ids .. py:attribute:: torque_lims .. py:attribute:: wheel_joint_directions .. py:attribute:: _pin .. py:attribute:: kp_ground_vel .. py:attribute:: ki_ground_vel .. py:attribute:: _reference_joint_pos :value: None .. py:attribute:: _cmd :type: pyrcf.core.types.RobotCmd :value: None .. py:method:: compute_wheel_velocities(robot_state: pyrcf.core.types.RobotState, local_plan: pyrcf.core.types.LocalMotionPlan, dt: float = None) -> numpy.ndarray .. py:method:: update(robot_state: pyrcf.core.types.RobotState, local_plan: pyrcf.core.types.LocalMotionPlan, t: float, dt: float) -> pyrcf.core.types.RobotCmd The main control update method to be called in the loop. This method will be called after the global plan and local plan are generated in the loop. This method should be implemented by any controller implementation. :param robot_state: The current state information from the robot. :type robot_state: RobotState :param local_plan: the latest local plan generated by the local planner used in the loop. :type local_plan: LocalMotionPlan :param t: the current time signature of the control loop. Defaults to None (controllers may or may not need this). :type t: float, optional :param dt: the time since the last control loop. Defaults to None (controllers may or may not need this). :type dt: float, optional :raises NotImplementedError: Raised if this method is not implemented by the child class. :returns: The output control command to be sent to the robot. :rtype: RobotCmd