From 05ebf000a1897de00e0d44685f50cfe0afbd8ef6 Mon Sep 17 00:00:00 2001 From: JustaGist Date: Fri, 9 Aug 2024 10:15:32 +0100 Subject: [PATCH] create upkie balance controller; rearrange examples and create new for upkie balancing controller --- .../tutorials/02_managed_ctrl_loop_usage.py | 10 +- .../03_component_implementation_intro.py | 141 ++++++++ .../04_minimal_ctrl_loop_upkie_balance.py | 78 ++++ pyrcf/components/controllers/README.md | 2 + pyrcf/components/controllers/__init__.py | 1 + .../segway_pid_balance_controller.py | 338 ++++++++++++++++++ .../ui_reference_generators/__init__.py | 1 + ...polator_position_controller_manipulator.py | 4 +- 8 files changed, 569 insertions(+), 6 deletions(-) create mode 100644 examples/tutorials/03_component_implementation_intro.py create mode 100644 examples/tutorials/04_minimal_ctrl_loop_upkie_balance.py create mode 100644 pyrcf/components/controllers/segway_pid_balance_controller.py diff --git a/examples/tutorials/02_managed_ctrl_loop_usage.py b/examples/tutorials/02_managed_ctrl_loop_usage.py index c5ce301..a9aacba 100644 --- a/examples/tutorials/02_managed_ctrl_loop_usage.py +++ b/examples/tutorials/02_managed_ctrl_loop_usage.py @@ -20,13 +20,15 @@ # control loop. robot = DummyRobot() state_estimator = DummyStateEstimator() - controller = DummyController() - local_planner = DummyLocalPlanner() global_planner = DummyUI() + local_planner = DummyLocalPlanner() + controller = DummyController() # create a controller manager to manage the controllers in the control loop. This is to - # deal with cases where there are multiple controllers in the control loop. In our case - # we don't necessarily need it, but this is required for creating the ControlLoop object. + # deal with cases where there are multiple controllers/localplanners in the control loop. + # In our case, we only have one controller and local planner in this example and we don't + # necessarily need a controller manager, but this is the format expected by the + # SimpleManagedCtrlLoop class we use in this example. controller_manager = SimpleControllerManager() # the simple controller manager manages different 'agents'. In our case, our 'agent' is diff --git a/examples/tutorials/03_component_implementation_intro.py b/examples/tutorials/03_component_implementation_intro.py new file mode 100644 index 0000000..6f0ef49 --- /dev/null +++ b/examples/tutorials/03_component_implementation_intro.py @@ -0,0 +1,141 @@ +"""Here we use actual implementations of control loop components instead of Dummy implementation, +and run a proper balancing controller on a simulated 2-wheeled segway-type robot (upkie). + +It also shows how a RobotInterface instance of any robot can be created in pybullet for +testing different controllers and planners. + +Running this demo: +You can send velocity commands (forward, backward, rotate left, rotate right) to the robot +using keyboard (or joystick if connected) and make it move in the simulation. +""" + +from pyrcf.components.controllers import SegwayPIDBalanceController +from pyrcf.components.robot_interfaces.simulation import PybulletRobot +from pyrcf.components.agents import PlannerControllerAgent +from pyrcf.components.local_planners import BlindForwardingPlanner +from pyrcf.components.state_estimators import DummyStateEstimator +from pyrcf.components.controller_manager import SimpleControllerManager +from pyrcf.components.global_planners.ui_reference_generators import ( + JoystickGlobalPlannerInterface, + KeyboardGlobalPlannerInterface, + DEFAULT_GAMEPAD_MAPPINGS, + DEFAULT_KEYBOARD_MAPPING, +) +from pyrcf.control_loop import SimpleManagedCtrlLoop +from pyrcf.core.logging import logging +from pyrcf.core.exceptions import NotConnectedError + +from pybullet_robot.utils.robot_loader_utils import get_urdf_from_awesome_robot_descriptions + +logging.getLogger().setLevel(logging.DEBUG) + + +if __name__ == "__main__": + # If using MinimalCtrlLoop, only requires defining a robot and a (compatible) controller. + # (You can still add local_planner and global_planner if required.) + + # ** robot interface ** + # get robot urdf from robot descriptions loader. This function allows getting urdf of any robot from + # the robot_descriptions package. See available robots here: + # https://github.com/robot-descriptions/robot_descriptions.py/tree/main?tab=readme-ov-file#descriptions + upkie_urdf = get_urdf_from_awesome_robot_descriptions( + robot_description_pkg_name="upkie_description" + ) + # The PybulletRobot class can create a valid RobotInterface object that can be used in the + # control loop, just by providing a urdf + robot = PybulletRobot( + urdf_path=upkie_urdf, + floating_base=True, # this robot does not have its base fixed in the world + # the `ee_names` arg is used in creating a fully defined kinematics-dynamics + # object (PinocchioInterface) for this robot (retrievable using the `get_pinocchio_interface()` + # method of `PybulletRobot`). If `ee_names` is not provided, end-effector-based + # kinematic funcitons of `PinocchioInterface` is not available, but other generic + # kinematics dynamics functions can still be used. + ee_names=["left_wheel_tire", "right_wheel_tire"], + ) + # NOTE: the PybulletRobot has a helper classmethod to directly load a urdf from + # the robot descriptions package as seen below: + # ``` + # robot = PybulletRobot.fromAwesomeRobotDescriptions( + # robot_description_name="upkie_description", + # floating_base=True, # this robot does not have its base fixed in the world + # ee_names=["left_wheel_tire", "right_wheel_tire"], + # ) + # ``` + + # ** state estimator ** + # all states are directly retrieved from the simulation in `PybulletRobot` class, + # so we don't need a custom state estimator. So we just use the DummyStateEstimator. + # Custom state estimators can be used here if needed. + state_estimator = DummyStateEstimator(squawk=False) + + # ** global planner ** + # We use a user interface as a global planner to generate high-level commands to + # the robot. Here, we use a Joystick interface if available, otherwise create + # a keyboard interface (using pygame window) + try: + # use joystick if available (key bindings defined in DEFAULT_GAMEPAD_MAPPINGS + # in `key_mappings.py`) + global_planner = JoystickGlobalPlannerInterface( + gamepad_mappings=DEFAULT_GAMEPAD_MAPPINGS, check_connection_at_init=True + ) + logging.info("Using Joystick interface.") + except (IndexError, NotConnectedError): + # if joystick not available, use keyboard interface (pygame window should + # be in focus for commands to work) + logging.info("Could not detect joystick. Using Keyboard interface.") + global_planner = KeyboardGlobalPlannerInterface(key_mappings=DEFAULT_KEYBOARD_MAPPING) + + # ** local planner ** + # In this example, we directly pass the velocity targets from the global planner (user + # key input) and does not need a local planner to do anything. So we use the + # `BlindForwardingPlanner` as the local planner in the control loop. This local + # planner simply forwards the references from the GlobalMotionPlan into a + # LocalMotionPlan object (which can then be used by a controller) directly without any + # modification. + local_planner = BlindForwardingPlanner() + + # ** controller ** + # create an instance of ControllerBase to be used in the control loop. + # This controller is capable of balancing a 2-wheel segway type robot. + # (Gains are tuned for this robot, task and simulator.) + controller = SegwayPIDBalanceController( + # the controller is also given access to the mutable `PinocchioInterface` + # object of this robot. The `PinocchioInterface` object is automatically + # updated for the real-time configuration of the robot in the simulation + # in `PybulletRobot` (see `read()` method of `PybulletRobot` for + # implementation). + pinocchio_interface=robot.get_pinocchio_interface(), + gains=SegwayPIDBalanceController.Gains( + pitch_damping=10.0, + pitch_stiffness=50.0, + footprint_position_damping=5.0, + footprint_position_stiffness=2.0, + joint_kp=200.0, + joint_kd=2.0, + turning_gain_scale=1.0, + ), + max_integral_error_velocity=12, + joint_torque_limits=[-1.0, 1.0], + ) + + # ** agent ** + # wrap the local planner and controller into a control 'agent' as in previous example + agent = PlannerControllerAgent(local_planner=local_planner, controller=controller) + + # ** controller manager ** + # create a controller manager to manage all the agents in the control loop + controller_manager = SimpleControllerManager(agents=[agent]) + + # use SimpleManagedCtrlLoop to run all the components + control_loop = SimpleManagedCtrlLoop( + robot_interface=robot, + state_estimator=state_estimator, + controller_manager=controller_manager, + global_planner=global_planner, + ) + + ctrl_loop_rate: float = 200 # 200hz control loop + # start control loop, kill it with Ctrl+C (keyboard interrupt is accepted by the control loop as + # a shutdown signal, and the control loop will try to shut down all components gracefully) + control_loop.run(loop_rate=ctrl_loop_rate) diff --git a/examples/tutorials/04_minimal_ctrl_loop_upkie_balance.py b/examples/tutorials/04_minimal_ctrl_loop_upkie_balance.py new file mode 100644 index 0000000..e5d3576 --- /dev/null +++ b/examples/tutorials/04_minimal_ctrl_loop_upkie_balance.py @@ -0,0 +1,78 @@ +"""Demonstrating the use of `MinimalCtrlLoop` class which is useful when there is only +one agent in the control loop. This runs the exact same components as in the previous +example, and is equivalent demo, but simpler to write. + +The `MinimalCtrlLoop` class is implemented from the `SimpleManagedCtrlLoop` introduced in +previous example, but is simplified for systems with only one controller and local planner +(i.e. one agent) in the control loop. Internally, the `MinimalCtrlLoop` does the creation of +Agent and Controller Manager for you. It also has some helpful defaults for components. +It also automatically connects to joystick or keyboard (as global planner) as available. + +This demo does exactly the same thing as the previous example, but a lot of things are +hidden within the `MinimalCtrlLoop` class' implemenation. + +Running this demo: +You can send velocity commands (forward, backward, rotate left, rotate right) to the robot +using keyboard (or joystick if connected) and make it move in the simulation. +""" + +from pyrcf.components.controllers import SegwayPIDBalanceController +from pyrcf.components.robot_interfaces.simulation import PybulletRobot +from pyrcf.control_loop import MinimalCtrlLoop +from pyrcf.core.logging import logging + +logging.getLogger().setLevel(logging.DEBUG) + + +if __name__ == "__main__": + # If using MinimalCtrlLoop, only requires defining a robot and a (compatible) controller. + # (You can still add local_planner and global_planner if required.) + + # ** robot interface ** + robot = PybulletRobot.fromAwesomeRobotDescriptions( + robot_description_name="upkie_description", + floating_base=True, # this robot does not have its base fixed in the world + ee_names=["left_wheel_tire", "right_wheel_tire"], + ) + + # ** controller ** + controller = SegwayPIDBalanceController( + pinocchio_interface=robot.get_pinocchio_interface(), + gains=SegwayPIDBalanceController.Gains( + pitch_damping=10.0, + pitch_stiffness=50.0, + footprint_position_damping=5.0, + footprint_position_stiffness=2.0, + joint_kp=200.0, + joint_kd=2.0, + turning_gain_scale=1.0, + ), + max_integral_error_velocity=12, + joint_torque_limits=[-1.0, 1.0], + ) + + # create a control loop using MinimalCtrlLoop class. + control_loop: MinimalCtrlLoop = MinimalCtrlLoop.useWithDefaults( + robot_interface=robot, controller=controller + ) + # NOTE: the `useWithDefaults` classmethod automatically uses + # dummy state estimator and forwarding planner (unless specified), as well + # as connects to joystick or keyboard UI automatically (unless a different + # global planner is specified) + + """ + # the above line is equivalent to doing the following: + + control_loop = MinimalCtrlLoop( + robot_interface=robot, + controller=controller, + state_estimator=DummyStateEstimator(squawk=False), + local_planner=BlindForwardingPlanner(), + global_planner=JoystickInterface() # or KeyboardInterface() + ) + """ + + ctrl_loop_rate: float = 200 # 200hz control loop + # start control loop, kill it with Ctrl+C (keyboard interrupt is accepted by the control loop as + # a shutdown signal, and the control loop will try to shut down all components gracefully) + control_loop.run(loop_rate=ctrl_loop_rate) diff --git a/pyrcf/components/controllers/README.md b/pyrcf/components/controllers/README.md index 055cdfb..050646c 100644 --- a/pyrcf/components/controllers/README.md +++ b/pyrcf/components/controllers/README.md @@ -25,6 +25,7 @@ Joint PD controller for joint position and velocity tracking. - Compatible LocalPlanner Classes: - `JointReferenceInterpolator` + - `IKReferenceInterpolator` - `BlindForwardingPlanner` (if applicable) ### `GravityCompensatedPDController` @@ -49,4 +50,5 @@ Joint PD controller for joint position and velocity tracking with active gravity - Compatible LocalPlanner Classes: - `JointReferenceInterpolator` + - `IKReferenceInterpolator` - `BlindForwardingPlanner` (if applicable) diff --git a/pyrcf/components/controllers/__init__.py b/pyrcf/components/controllers/__init__.py index e9ab7d3..4209033 100644 --- a/pyrcf/components/controllers/__init__.py +++ b/pyrcf/components/controllers/__init__.py @@ -3,3 +3,4 @@ from .controller_base import ControllerBase, DummyController from .joint_pd_controller import JointPDController from .gravity_compensated_joint_pd_controller import GravityCompensatedPDController +from .segway_pid_balance_controller import SegwayPIDBalanceController diff --git a/pyrcf/components/controllers/segway_pid_balance_controller.py b/pyrcf/components/controllers/segway_pid_balance_controller.py new file mode 100644 index 0000000..646b630 --- /dev/null +++ b/pyrcf/components/controllers/segway_pid_balance_controller.py @@ -0,0 +1,338 @@ +""" +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 +""" + +from dataclasses import dataclass +from typing import List +import numpy as np + +from .controller_base import ControllerBase +from ...core.types import ControlMode, LocalMotionPlan, RobotCmd, RobotState +from ...utils.math_utils import quat2rpy +from ...utils.filters import abs_bounded_derivative_filter, low_pass_filter +from ...utils.kinematics_dynamics.pinocchio_interface import PinocchioInterface + + +class SegwayPIDBalanceController(ControllerBase): + """A simple PD balance controller for a 2-wheel segway type robot.""" + + @dataclass + class Gains: + """ + Gains for this controller. + + Args: + 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. + 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. + 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. + 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. + joint_kp: Joint position tracking gain (for non-wheel joints). + joint_kd: Wheel joint velocity gain (only for wheel joints) + turning_gain_scale: joint velocity additional gains when there + is turning probability (yaw command). + """ + + pitch_damping: float = 10.0 + pitch_stiffness: float = 20.0 + footprint_position_damping: float = 5.0 + footprint_position_stiffness: float = 2.0 + joint_kp: float = 200.0 + joint_kd: float = 2.0 + turning_gain_scale: float = 2.0 + + def __init__( + self, + 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], + ): + """A simple PD balance controller for a 2-wheel segway type robot. + + Initialize balancer. + + Args: + air_return_period: Cutoff period for resetting integrators while + the robot is in the air, in [s]. + max_ground_velocity: Maximum commanded ground velocity no matter + what, in [m] / [s]. + max_integral_error_velocity: Maximum integral error velocity, in + [m] / [s]. + max_target_accel: Maximum acceleration for the ground + target, in [m] / [s]². This bound does not affect the commanded + ground velocity. + max_target_distance: Maximum distance from the current ground + position to the target, in [m]. + max_target_velocity: Maximum velocity for the ground target, + in [m] / [s]. This bound indirectly affects the commanded + ground velocity. + max_yaw_accel: Maximum yaw angular acceleration in [rad] / [s]². + max_yaw_velocity: Maximum yaw angular velocity in [rad] / [s]. + turning_deadband: Joystick axis value between 0.0 and 1.0 below + which legs stiffen but the turning motion doesn't start. + turning_decision_time: Minimum duration in [s] for the turning + probability to switch from zero to one and converesly. + wheel_radius: Wheel radius in [m]. + """ + assert 0.0 <= turning_deadband <= 1.0 + self.air_return_period = air_return_period + self.fall_pitch = fall_pitch + self.gains = gains + self.integral_error_velocity = 0.0 + self.max_ground_velocity = max_ground_velocity + self.max_integral_error_velocity = max_integral_error_velocity + self.max_target_accel = max_target_accel + self.max_target_distance = max_target_distance + self.max_target_velocity = max_target_velocity + self.max_yaw_accel = max_yaw_accel + self.max_yaw_velocity = max_yaw_velocity + self.pitch = 0.0 + self.target_ground_position = 0.0 + self.target_ground_velocity = 0.0 + self.target_yaw_position = 0.0 + self.target_yaw_velocity = 0.0 + self.turning_deadband = turning_deadband + self.turning_decision_time = turning_decision_time + self.turning_probability = 0.0 + self.wheel_radius = wheel_radius + self.wheel_ee_ids = wheel_ee_ids + self.wheel_distance = wheel_distance + self.wheel_joint_ids = wheel_joint_ids + self.torque_lims = joint_torque_limits + self.wheel_joint_directions = wheel_joint_directions + + self._pin = pinocchio_interface + + self.kp_ground_vel = np.array( + [ + gains.footprint_position_damping, + gains.pitch_damping, + ] + ) + self.ki_ground_vel = np.array( + [ + gains.footprint_position_stiffness, + gains.pitch_stiffness, + ] + ) + + self._reference_joint_pos = None + self._cmd: RobotCmd = None + + def compute_wheel_velocities( + self, + robot_state: RobotState, + local_plan: LocalMotionPlan, + dt: float = None, + ) -> np.ndarray: + self.target_ground_velocity = abs_bounded_derivative_filter( + self.target_ground_velocity, + local_plan.twist.linear[0], + dt, + self.max_target_velocity, + self.max_target_accel, + ) + + turning_intent = abs(local_plan.twist.angular[2] / self.turning_deadband) + self.turning_probability = abs_bounded_derivative_filter( + self.turning_probability, + turning_intent, # might be > 1.0 + dt, + max_output=1.0, # output is <= 1.0 + max_derivative=1.0 / self.turning_decision_time, + ) + + velocity_ratio = (abs(local_plan.twist.angular[2]) - self.turning_deadband) / ( + 1.0 - self.turning_deadband + ) + velocity_ratio = max(0.0, velocity_ratio) + yaw_vel = self.max_yaw_velocity * np.sign(local_plan.twist.angular[2]) * velocity_ratio + turn_hasnt_started = abs(self.target_yaw_velocity) < 0.01 + turn_not_sure_yet = self.turning_probability < 0.99 + if turn_hasnt_started and turn_not_sure_yet: + yaw_vel = 0.0 + self.target_yaw_velocity = abs_bounded_derivative_filter( + self.target_yaw_velocity, + yaw_vel, + dt, + self.max_yaw_velocity, + self.max_yaw_accel, + ) + if abs(self.target_yaw_velocity) > 0.01: # still turning + self.turning_probability = 1.0 + + torso_pitch = quat2rpy(robot_state.state_estimates.pose.orientation)[1] + if abs(torso_pitch) > self.fall_pitch: + self.integral_error_velocity = 0.0 # [m] / [s] + ground_velocity = 0.0 # [m] / [s] + raise RuntimeError(f"Base angle {torso_pitch=:.3} rad denotes a fall") + + # NOTE: this is a weird way of setting ee position reference. It is not transformed to base frame!! + # NOTE: also this is averaging over all end-effector poses (will break if + # the robot has end-effectors other than wheels) + ground_position = np.mean([ee_pose[0][0] for ee_pose in self._pin.get_ee_poses()]) + floor_contact = np.any( + robot_state.state_estimates.contact_states[i] for i in self.wheel_ee_ids + ) + + error = np.array( + [ + self.target_ground_position - ground_position, + 0.0 - torso_pitch, # target pitch is 0.0 rad + ] + ) + + if not floor_contact: + self.integral_error_velocity = low_pass_filter( + self.integral_error_velocity, self.air_return_period, 0.0, dt + ) + # We don't reset self.target_ground_velocity: either takeoff + # detection is a false positive and we should resume close to the + # pre-takeoff state, or the robot is really in the air and the user + # should stop smashing the joystick like a bittern ;p + self.target_ground_position = low_pass_filter( + self.target_ground_position, + self.air_return_period, + ground_position, + dt, + ) + else: # floor_contact: + self.integral_error_velocity += self.ki_ground_vel.dot(error) * dt + self.integral_error_velocity = np.clip( + self.integral_error_velocity, + -self.max_integral_error_velocity, + self.max_integral_error_velocity, + ) + self.target_ground_position += self.target_ground_velocity * dt + self.target_ground_position = np.clip( + self.target_ground_position, + ground_position - self.max_target_distance, + ground_position + self.max_target_distance, + ) + + # Non-minimum phase trick: as per control theory's book, the proper + # feedforward velocity should be ``+self.target_ground_velocity``. + # However, it is with resolute purpose that it sends + # ``-self.target_ground_velocity`` instead! + # + # Try both on the robot, you will see the difference :) + # + # This hack is not purely out of "esprit de contradiction". Changing + # velocity is a non-minimum phase behavior (to accelerate forward, the + # ZMP of the LIPM needs to move backward at first, then forward), and + # our feedback can't realize that (it only takes care of balancing + # around a stationary velocity). + # + # What's left? Our integrator! If we send the opposite of the target + # velocity (or only a fraction of it, although 100% seems to do a good + # job), Upkie will immediately start executing the desired non-minimum + # phase behavior. The error will then grow and the integrator catch up + # so that ``upkie_trick_velocity - self.integral_error_velocity`` + # converges to its proper steady state value (the same value ``0 - + # self.integral_error_velocity`` would have converged to if we had no + # feedforward). + # + # Unconvinced? Try it on the robot. You will feel Upkie's trick ;) + # + upkie_trick_velocity = -self.target_ground_velocity + + ground_velocity = ( + upkie_trick_velocity - self.kp_ground_vel.dot(error) - self.integral_error_velocity + ) + ground_velocity = np.clip( + ground_velocity, -self.max_ground_velocity, self.max_ground_velocity + ) + + left_wheel_velocity = self.wheel_joint_directions[0] * ground_velocity / self.wheel_radius + right_wheel_velocity = self.wheel_joint_directions[1] * ground_velocity / self.wheel_radius + + # Yaw rotation + turning_radius = 0.5 * self.wheel_distance + yaw_to_wheel = turning_radius / self.wheel_radius + + # NOTE: not sure why the target yaw velocities have to be negative (not there in original upkie code) + left_wheel_velocity += yaw_to_wheel * (-self.target_yaw_velocity) + right_wheel_velocity += yaw_to_wheel * (-self.target_yaw_velocity) + + return np.array([left_wheel_velocity, right_wheel_velocity]) + + def update( # pylint: disable=W0222 + self, + robot_state: RobotState, + local_plan: LocalMotionPlan, + t: float, + dt: float, + ) -> RobotCmd: + if self._reference_joint_pos is None: + self._reference_joint_pos = robot_state.joint_states.joint_positions + + if local_plan.control_mode != ControlMode.CONTROL: + return RobotCmd() + + target_vel = np.zeros(robot_state.joint_states.joint_positions.size) + target_vel[self.wheel_joint_ids] = self.compute_wheel_velocities( + robot_state=robot_state, local_plan=local_plan, dt=dt + ) + self._reference_joint_pos[self.wheel_joint_ids] = robot_state.joint_states.joint_positions[ + self.wheel_joint_ids + ] + + kd = self.gains.joint_kd + self.gains.turning_gain_scale * self.turning_probability + + if self._cmd is None: + self._cmd = RobotCmd.createZeros( + dof=robot_state.joint_states.joint_positions.size, + joint_names=robot_state.joint_states.joint_names, + ) + + self._cmd.joint_commands.joint_efforts = self.gains.joint_kp * ( + self._reference_joint_pos - robot_state.joint_states.joint_positions + ) + + self._cmd.joint_commands.joint_efforts += kd * ( + target_vel - robot_state.joint_states.joint_velocities + ) + + self._cmd.joint_commands.joint_efforts = np.clip( + self._cmd.joint_commands.joint_efforts, + self.torque_lims[0], + self.torque_lims[1], + ) + + return self._cmd diff --git a/pyrcf/components/global_planners/ui_reference_generators/__init__.py b/pyrcf/components/global_planners/ui_reference_generators/__init__.py index 6e2d50f..995fa4e 100644 --- a/pyrcf/components/global_planners/ui_reference_generators/__init__.py +++ b/pyrcf/components/global_planners/ui_reference_generators/__init__.py @@ -5,3 +5,4 @@ from .keyboard_interface import KeyboardGlobalPlannerInterface from .pb_gui_iinterface import PybulletGUIGlobalPlannerInterface from .joystick_interface import JoystickGlobalPlannerInterface +from .key_mappings import DEFAULT_GAMEPAD_MAPPINGS, DEFAULT_KEYBOARD_MAPPING diff --git a/test_scripts/ik_interpolator_position_controller_manipulator.py b/test_scripts/ik_interpolator_position_controller_manipulator.py index 6348995..566c3c1 100644 --- a/test_scripts/ik_interpolator_position_controller_manipulator.py +++ b/test_scripts/ik_interpolator_position_controller_manipulator.py @@ -1,4 +1,4 @@ -"""Simple demo showing the use of awesome robots loader, joint position controller, +"""Simple demo showing the use of some implmentations of pyrcf components joint position controller, end-effector reference interpolator (local planner), and pybullet GUI for setting target end-effector poses (implemented as a GlobalPlanner). @@ -15,7 +15,7 @@ 4. The controller is a joint position (and velocity) tracking controller. -5. Extension of this controller that also adds gravity compensation torques to the +5. Also has an extended variant of this controller that adds gravity compensation torques to the robot command, making the joint tracking much better. 6. There is also a debug robot in the simulation (using `PybulletRobotPlanDebugger`)