{ "cells": [ { "cell_type": "markdown", "id": "9bd6dd9a-a0f6-4248-89d4-657c318e0d79", "metadata": {}, "source": [ "# Tutorial 02: Joint Forces" ] }, { "cell_type": "markdown", "id": "3c18bd9e-7470-4b09-b7a7-fac8bb2dec17", "metadata": {}, "source": [ "## Tutorial Description" ] }, { "cell_type": "markdown", "id": "b7e48aef-41be-4597-be37-1bd196bc97a5", "metadata": {}, "source": [ "This tutorial covers creating the backend of a project using condynsate that applies torques to specific joints of a .URDF\n", "object. In this tutorial, we will cover:\n", "1. Applying torques to continuous joints of a .URDF object.\n", "2. Measuring the position and velocity of joints of a .URDF object." ] }, { "cell_type": "markdown", "id": "be64a904-72de-4411-8a77-37a630f0c3a4", "metadata": {}, "source": [ "## Imports" ] }, { "cell_type": "markdown", "id": "5ca5c838-ffae-45eb-9e60-0ad1e0ac8698", "metadata": {}, "source": [ "To begin, we import the same modules for the same reasons as tutorial 00." ] }, { "cell_type": "code", "execution_count": 1, "id": "b2b6dc1e-8420-4242-91a3-e73422ce713e", "metadata": {}, "outputs": [], "source": [ "from condynsate import Simulator as con_sim\n", "from condynsate import __assets__ as assets" ] }, { "cell_type": "markdown", "id": "dea83d70-5877-4057-b588-983d28579958", "metadata": {}, "source": [ "## Building the Project Class" ] }, { "cell_type": "markdown", "id": "507fb740-3a04-4ff7-af5e-fbbd3c73f962", "metadata": {}, "source": [ "We now create a `Project` class with `__init__` and `run` functions. In `__init__` a pendulum is loaded using the same technique as tutorial 00. In `run`, we cover how to read the state of the joint of the pendulum and apply torques to the joint based on its state." ] }, { "cell_type": "code", "execution_count": 2, "id": "098a10b0-6cec-4676-8cd2-1ac6b47ad94b", "metadata": {}, "outputs": [], "source": [ "class Project():\n", " def __init__(self):\n", " # Create a instance of the simulator\n", " self.s = con_sim(animation = False,\n", " keyboard = False)\n", " \n", " # Load the pendulum in the orientation we want\n", " self.pendulum = self.s.load_urdf(urdf_path = assets['pendulum'],\n", " position = [0., 0., 0.05],\n", " yaw = 1.571,\n", " wxyz_quaternion = [1., 0., 0., 0],\n", " fixed = True,\n", " update_vis = True)\n", "\n", " '''\n", " ##################################################################\n", " After loading our pendulum, we want to set its joint so that it \n", " is at a slight angle. This is done using the \n", " condynsate.simulator.set_joint_position function. It has 5 \n", " arguments:\n", " urdf_obj : URDF_Obj\n", " A URDF_Obj that contains that joint whose position is being \n", " set.\n", " joint_name : string\n", " The name of the joint whose position is set. The joint name \n", " is\n", " specified in the .urdf file.\n", " position : float, optional\n", " The position in rad to be applied to the joint.\n", " The default is 0..\n", " physics : bool, optional\n", " A boolean flag that indicates whether physics based \n", " poisiton controller will be used to change joint position \n", " or whether the joint position will be reset immediately \n", " to the target position with zero end velocity. The \n", " default is False. \n", " initial_cond : bool, optional\n", " A boolean flag that indicates whether the position set \n", " is an initial condition of the system. If it is an \n", " initial condition, when the simulation is reset using \n", " backspace, the joint position will be set again.\n", " \n", " Note that because this is an intial condition, we set the \n", " initial_cond flag to true.\n", " ##################################################################\n", " '''\n", " # Set the initial angle of the pendulum joint\n", " self.s.set_joint_position(urdf_obj = self.pendulum,\n", " joint_name = 'chassis_to_arm',\n", " position = 0.175,\n", " initial_cond = True,\n", " physics = False)\n", "\n", "\n", " def run(self, max_time=None):\n", " '''\n", " ##################################################################\n", " This run function does all the same basic functions as in \n", " tutorial 00 but with the added functionality of joint torques \n", " forces to the pendulum during the simulation loop\n", " ##################################################################\n", " '''\n", " # Reset the simulator.\n", " self.s.reset()\n", "\n", " # Run the simulation loop until done\n", " while(not self.s.is_done):\n", " '''\n", " ##############################################################\n", " First we want to measure the state (position, and velocity) \n", " of the pendulum joint. The name of the pendulum joint set in \n", " the .URDF file is \"chassis_to_arm\". For a given URDF object \n", " with joints that has already been loaded into the physics \n", " environment, we can measure this by using the \n", " condynsate.simulator.get_joint_state function. There are two \n", " arguments to this function:\n", " 1) urdf_obj : URDF_Obj\n", " A URDF_Obj whose joint state is being measured.\n", " 2) joint_name : string\n", " The name of the joint whose state is measured. The \n", " joint name is specified in the .urdf file.\n", " \n", " state, which is returned by\n", " condynsate.simulator.get_joint_state has the following form:\n", " state : dictionary with the following keys\n", " 'position' : float\n", " The position value of this joint.\n", " 'velocity' : float\n", " The velocity value of this joint.\n", " 'reaction force' : list shape(3,)\n", " These are the joint reaction forces. Only read if \n", " a torque sensor is enabled for this joint.\n", " 'reaction torque' : list shape(3,)\n", " These are the joint reaction torques. Only read if\n", " a torque sensor is enabled for this joint.\n", " 'applied torque' : float\n", " This is the motor torque applied during the last\n", " stepSimulation. Note that this only applies in\n", " VELOCITY_CONTROL and POSITION_CONTROL. If you use\n", " TORQUE_CONTROL then the applied joint motor torque is\n", " exactly what you provide, so there is no need to \n", " report it separately.\n", " ##############################################################\n", " '''\n", " # Get the pendulum joint (\"chassis_to_arm\") state\n", " state = self.s.get_joint_state(urdf_obj = self.pendulum,\n", " joint_name = 'chassis_to_arm')\n", " \n", " \n", " '''\n", " ##############################################################\n", " Suppose we wanted to apply a returning torque whenever the \n", " pendulum angle is above some threshold. To do this, we would \n", " need to first measure the angle of the pendulum. We can do \n", " this by extracting the position (angle for a continuous \n", " joint) from its state.\n", " ##############################################################\n", " '''\n", " # Extract angle of the pendulum joint\n", " angle = 180. * state['position'] / 3.141592654\n", " \n", " \n", " '''\n", " ##############################################################\n", " Now we write an if statement that applies a returning torque\n", " to the joint if its angle is greater than 5.0 degrees. To \n", " apply a torque about a joint, we use the \n", " condynsate.simulator.set_joint_torque function. This function\n", " has six arguments:\n", " urdf_obj : URDF_Obj\n", " A URDF_Obj that contains that joint whose torque is \n", " being set.\n", " joint_name : string\n", " The name of the joint whose torque is set. The joint \n", " name is specified in the .urdf file\n", " torque : float, optional\n", " The torque in NM to be applied to the joint. The \n", " default is 0..\n", " show_arrow : bool, optional\n", " A boolean flag that indicates whether an arrow will \n", " be rendered on the link to visualize the applied \n", " torque. The default is False.\n", " arrow_scale : float, optional\n", " The scaling factor that determines the size of the \n", " arrow. The default is 0.1.\n", " arrow_offset : float, optional\n", " The amount to offset the drawn arrow along the \n", " joint axis. The default is 0.0.\n", " \n", " In this case, we want to draw the torque arrow so we set \n", " show_arrow to True and adjust arrow_scale and arrow_offset \n", " until the size and position of the arrow look correct, \n", " respectively. \n", " ##############################################################\n", " '''\n", " # If positive angle, apply negative torque\n", " if angle > 5.:\n", " self.s.set_joint_torque(urdf_obj = self.pendulum,\n", " joint_name = 'chassis_to_arm',\n", " torque = -10,\n", " show_arrow = True,\n", " arrow_scale = 0.05,\n", " arrow_offset = 0.05)\n", "\n", " # If negative angle, apply positive torque\n", " elif angle < -5.:\n", " self.s.set_joint_torque(urdf_obj = self.pendulum,\n", " joint_name = 'chassis_to_arm',\n", " torque = 10,\n", " show_arrow = True,\n", " arrow_scale = 0.05,\n", " arrow_offset = 0.05)\n", "\n", " # If low angle magnitude, apply no torque\n", " else:\n", " self.s.set_joint_torque(urdf_obj = self.pendulum,\n", " joint_name = 'chassis_to_arm',\n", " torque = 0.0,\n", " show_arrow = True)\n", "\n", " '''\n", " ##############################################################\n", " As usual, at the bottom of the run function we step the \n", " simulation.\n", " ##############################################################\n", " '''\n", " ret_code = self.s.step(max_time=max_time)" ] }, { "cell_type": "markdown", "id": "420433c6-c1d2-4702-aba4-52295bf753e2", "metadata": {}, "source": [ "## Running the Project Class" ] }, { "cell_type": "markdown", "id": "da509ec5-56c1-4bc4-a9d0-0db238bd069a", "metadata": {}, "source": [ "Now that we have made the `Project` class, we can test it by initializing it and then calling the `run` function." ] }, { "cell_type": "code", "execution_count": 3, "id": "a9461fb2-213e-4b6e-92c0-d0fb1ad00176", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "You can open the visualizer by visiting the following URL:\n", "http://127.0.0.1:7004/static/\n" ] } ], "source": [ "# Create an instance of the Project class. \n", "proj = Project()\n", "\n", "# Run the simulation.\n", "proj.run(max_time = 5.0)" ] }, { "cell_type": "markdown", "id": "36d1e2de-e3f4-4dfb-a530-766a4420f876", "metadata": {}, "source": [ "## Challenge" ] }, { "cell_type": "markdown", "id": "5ff6e017-b96d-4668-879d-5ebaf083ae88", "metadata": {}, "source": [ "This tutorial is now complete. For an added challenge, think of how you would modify the Project.run() to implement a PD controller. " ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.13.7" } }, "nbformat": 4, "nbformat_minor": 5 }