From 3aeab5a52d4a8d7ded76f4ef12d9af1b515c7684 Mon Sep 17 00:00:00 2001 From: Giovanni Fregonese Date: Fri, 21 Jun 2024 00:33:41 +0200 Subject: [PATCH] Add simple notebook example for joint control (#855) --- CHANGELOG.md | 1 + .../yarp_direct_motor_control.ipynb | 415 ++++++++++++++++++ 2 files changed, 416 insertions(+) create mode 100644 examples/BindingsMotorControl/yarp_direct_motor_control.ipynb diff --git a/CHANGELOG.md b/CHANGELOG.md index 56758c9823..8600665b60 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ All notable changes to this project are documented in this file. - Implement `ButterworthLowPassFilter` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/838) - Implement `Conversions::toiDynTreeRot` function (https://github.com/ami-iit/bipedal-locomotion-framework/pull/842) - Create python bindings of `VectorsCollection` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/854) +- Added a simple motor control example (https://github.com/ami-iit/bipedal-locomotion-framework/pull/855) ### Changed - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810) diff --git a/examples/BindingsMotorControl/yarp_direct_motor_control.ipynb b/examples/BindingsMotorControl/yarp_direct_motor_control.ipynb new file mode 100644 index 0000000000..0226ae3a37 --- /dev/null +++ b/examples/BindingsMotorControl/yarp_direct_motor_control.ipynb @@ -0,0 +1,415 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Example joint control with BLF in Python" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Introduction" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Background\n", + "\n", + "The code in this example is written in Python and uses the [Bipedal Locomotion Framework](https://github.com/ami-iit/bipedal-locomotion-framework) (BLF) to control the joints of the [ergoCub](https://ergocub.eu/project) robot in the Gazebo simulator. To run the code in this example you need to have a working installation of the YARP middleware and the Gazebo simulator, as well as the BLF and ergoCub software. An easy way to install all the dependencies is to use the conda environment that can be created with the following command:\n", + "\n", + "```bash\n", + "conda env create -n blf_example_env gazebo-yarp-plugins icub-models ergocub-software bopedal-locomotion-framework jupyter\n", + "```\n", + "\n", + "this environment should contain all the dependencies needed to run the example.\n", + "\n", + "In all terminal windows, activate the conda environment:\n", + "```bash\n", + "conda activate blf_example_env\n", + "```\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import yarp\n", + "import tempfile\n", + "\n", + "import bipedal_locomotion_framework.bindings as blf" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Yarp server\n", + "\n", + "Terminal 1:\n", + "```bash\n", + "export YARP_ROBOT_NAME=\"ergoCubGazeboV1_1\"\n", + "yarpserver --write\n", + "``` \n", + "\n", + "This will start the YARP name server that allows the different processes to communicate with each other." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "At this point we can connect to the yarp network." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "network = yarp.Network()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Gazebo\n", + "\n", + "Terminal 2:\n", + "```bash\n", + "export YARP_ROBOT_NAME=\"ergoCubGazeboV1_1\"\n", + "export YARP_CLOCK=/clock\n", + "gazebo -slibgazebo_yarp_clock.so\n", + "```\n", + "\n", + "After Gazebo is open, drag and drop the robot model 'ergoCubGazeboV1_1' from the left panel Insert tab to the Gazebo world." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Configuration files" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A minimal configuration file is shown in the next cell. It has two groups, one for the robot control boards and one for the sensor bridge. The control board group will be used to intialize the access to the joint motors, while the sensor bridge group will be used to access the joint encoders." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "config = \"\"\"\n", + "[REMOTE_CONTROL_BOARD]\n", + "robot_name ergocubSim\n", + "\n", + "remote_control_boards (left_arm)\n", + "\n", + "joints_list (l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_yaw l_wrist_pitch l_wrist_roll)\n", + "\n", + "positioning_duration 10.0 # seconds\n", + "positioning_tolerance 0.1 # radians\n", + "position_direct_max_admissible_error 0.1 # radians\n", + "\n", + "[SENSOR_BRIDGE]\n", + "check_for_nan false\n", + "stream_joint_states true\n", + "stream_motor_states false\n", + "stream_forcetorque_sensors false\n", + "\"\"\"\n", + "\n", + "tmpfile = tempfile.NamedTemporaryFile(mode='w', delete=False)\n", + "tmpfile.write(config)\n", + "tmpfile.close()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We have now a configuration file in a temporary file, let's use it with the `YarpParametersHandler` class to read the configuration file and print the parameters." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "param_handler = blf.parameters_handler.YarpParametersHandler()\n", + "param_handler.set_from_filename(tmpfile.name)\n", + "\n", + "# add the local prefix to the remote control board group\n", + "# this is needed for the robot control\n", + "param_handler.get_group(\"REMOTE_CONTROL_BOARD\").set_parameter_string(\"local_prefix\", \"example_controller\")\n", + "\n", + "print(param_handler)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Motor Control\n", + "\n", + "To gain access to the robot control board we need to instantiate a PolyDriver object and pass it the configuration file. This will allow us to access the control board remote control interface.\n", + "\n", + "Refs:\n", + " - https://www.yarp.it/latest/classRemoteControlBoardRemapper.html\n", + " - https://ami-iit.github.io/bipedal-locomotion-framework/YarpHelper_8h.html#a2c07f5099140671abde506a8a316e130\n", + " - https://ami-iit.github.io/bipedal-locomotion-framework/structBipedalLocomotion_1_1RobotInterface_1_1PolyDriverDescriptor.html" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "polydrivers = {}\n", + "polydrivers[\"REMOTE_CONTROL_BOARD\"] = blf.robot_interface.construct_remote_control_board_remapper(param_handler.get_group(\"REMOTE_CONTROL_BOARD\"))\n", + "polydrivers[\"REMOTE_CONTROL_BOARD\"].is_valid()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "After instantiating the PolyDriver object we can create a YarpRobotContol object to access the joints specified in the configuration file." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot_control = blf.robot_interface.YarpRobotControl()\n", + "robot_control.initialize(param_handler.get_group(\"REMOTE_CONTROL_BOARD\"))\n", + "robot_control.set_driver(polydrivers[\"REMOTE_CONTROL_BOARD\"].poly)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "At this point we have access to the robot joints, for example to count the number of dofs." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "n_dof = len(robot_control.get_joint_list())\n", + "n_dof" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can set the control mode of the joints to position direct and set the desired position of the joints." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot_control.set_control_mode(blf.robot_interface.YarpRobotControl.PositionDirect)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "joint_values = np.zeros(n_dof)\n", + "control_modes = blf.robot_interface.YarpRobotControl.PositionDirect\n", + "\n", + "robot_control.set_references(joint_values, control_modes)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "If the requested joint position is greater than the parameter `position_direct_max_admissible_error` (in rad) then the `set_reference` function will return false and log an error message.\n", + "To avoid this we can set the parameter `position_direct_max_admissible_error` to a higher value, or pass the third argument of the `set_reference` function which is the current joint position read from the encoders." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can use `check_motion_done` to check if the motion is done. The function returns a tuple of 4 values:\n", + "1) isOk: True if the function executed correctly, False otherwise.\n", + "2) motionDone: True if the motion ended, False otherwise.\n", + "3) isTimeExpired: True if internal timer expired, False otherwise.\n", + "4) info: vector containing the list of the joint whose motion did not finish yet, and the corresponding position error.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot_control.check_motion_done()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Sensor Readings\n", + "\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "To control the robot in closed loop (hopefully that is the goal) we need to read the joint encoders. We can use the `YarpSensorBridge` class to access the joint encoders.\n", + "\n", + "Let's create a `YarpSensorBridge` object and initialize it with the relative group in the configuration file." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "sensor_bridge = blf.robot_interface.YarpSensorBridge()\n", + "\n", + "sensor_bridge.initialize(param_handler.get_group(\"SENSOR_BRIDGE\"))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "To have access to the joint encoders we need to link the `PolyDriver` object to the `YarpSensorBridge` object.\n", + "This will allow us to read encoders of the joints we are controlling and that we defined in the `REMOTE_CONTROL_BOARD` group of the configuration file." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "sensor_bridge.set_drivers_list([polydrivers[\"REMOTE_CONTROL_BOARD\"]])" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "At this point we can read the joint encoders and print them." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "sensor_bridge.advance()\n", + "are_joints_ok, joint_positions, _ = sensor_bridge.get_joint_positions()\n", + "joint_positions" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Example Control" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We now have all the tools to control the robot. We can create a simple control loop that moves the joints to a desired position." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "joint_to_test = \"l_shoulder_pitch\"\n", + "joint_index = robot_control.get_joint_list().index(joint_to_test)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "for i in range(100):\n", + " t = i * 0.1\n", + " # read the joint positions\n", + " sensor_bridge.advance()\n", + " are_joints_ok, joint_positions, _ = sensor_bridge.get_joint_positions()\n", + "\n", + " new_joint_positions = joint_positions.copy()\n", + " # set the references\n", + " new_joint_positions[joint_index] += 0.1 * np.cos(10 * t)\n", + " robot_control.set_references(new_joint_positions, blf.robot_interface.YarpRobotControl.PositionDirect, joint_positions)\n", + " yarp.delay(0.1)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "xbgenv", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.2" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}