{ "cells": [ { "cell_type": "markdown", "id": "9bd6dd9a-a0f6-4248-89d4-657c318e0d79", "metadata": {}, "source": [ "# Tutorial 01: External 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 in which external forces are applied to the center of mass of an\n", "unjointed .URDF object. We will cover:\n", "1. Appling forces and torques to the center of mass of an object.\n", "2. Measuring the position, orientation, velocity, and rotational velocity of the center of mass of that 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__` solid ground and a sphere are loaded using the same technique as tutorial 00. In `run`, we cover how to read the position and orientation of the sphere and apply external forces based on the sphere's 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 ground\n", " self.ground = self.s.load_urdf(urdf_path = assets['plane_big'],\n", " position = [0., 0., 0.],\n", " wxyz_quaternion = [1., 0., 0., 0],\n", " fixed = True,\n", " update_vis = False)\n", " \n", " # Load the sphere so that it's resting on the ground\n", " self.sphere = self.s.load_urdf(urdf_path = assets['sphere'],\n", " position=[0., 0., 0.5],\n", " wxyz_quaternion=[1., 0., 0., 0],\n", " fixed = False,\n", " update_vis = True)\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 applying external \n", " forces to the sphere 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, orientation,\n", " velocity, and angular vel) about the center of mass of the \n", " sphere. For a given URDF object that has already been loaded \n", " into the physics environment, we can measure this by using \n", " the condynsate.simulator.get_base_state function. There are \n", " two arguments to this function:\n", " 1) urdf_obj: the unique URDF object ID that is returned \n", " when condynsate.simulator.load_urdf is called\n", " 2) body_coords: A boolean flag that indicates whether the\n", " velocity and angular velocity is given in world coords\n", " (False) or body coords (True). World coords are \n", " defined around the axes defined in the URDF file.\n", " In this case, we want the world coords, so we set body_coords\n", " to False\n", " \n", " state, which is returned by \n", " condynsate.simulator.get_base_state has the following form:\n", " state : a dictionary with the following keys:\n", " 'position' : array-like, shape (3,)\n", " The (x,y,z) world coordinates of the base of the \n", " urdf.\n", " 'roll' : float\n", " The Euler angle roll of the base of the urdf\n", " that define the body's orientation in the world. \n", " Rotation of the body about the world's x-axis.\n", " 'pitch' : float\n", " The Euler angle pitch of the base of the urdf\n", " that define the body's orientation in the world. \n", " Rotation of the body about the world's y-axis.\n", " 'yaw' : float\n", " The Euler angle yaw of the base of the urdf\n", " that define the body's orientation in the world. \n", " Rotation of the body about the world's z-axis.\n", " 'R of world in body' : array-like, shape(3,3):\n", " The rotation matrix that takes vectors in world \n", " coordinates to body coordinates. For example, \n", " let V_inB be a 3vector written in body coordinates. \n", " Let V_inW be a 3vector written in world coordinates. \n", " Then: V_inB = R_ofWorld_inBody @ V_inW\n", " 'velocity' : array-like, shape (3,)\n", " The linear velocity of the base of the urdf in \n", " either world coords or body coords. Ordered as \n", " either (vx_inW, vy_inW, vz_inW) or \n", " (vx_inB, vy_inB, vz_inB).\n", " 'angular velocity' : array-like, shape (3,)\n", " The angular velocity of the base of the urdf in\n", " either world coords or body coords. Ordered as either\n", " (wx_inW, wy_inW, wz_inW), or \n", " (wx_inB, wy_inB, wz_inB). When written in world \n", " coordinates, exactly equal to the roll rate, the\n", " pitch rate, and the yaw rate.\n", " ##############################################################\n", " '''\n", " # Get the base state of the sphere\n", " state = self.s.get_base_state(urdf_obj = self.sphere,\n", " body_coords = False)\n", " \n", " '''\n", " ##############################################################\n", " Suppose we wanted to apply an upward force to the center of \n", " mass of the sphere if it is less than 1.0 meters above the \n", " ground. To do this, we would need to first measure its height \n", " off of the ground. We can do this by extracting the height \n", " from its state.\n", " ##############################################################\n", " '''\n", " # Extract the height of the center of mass of the sphere\n", " position = state['position']\n", " height = position[2]\n", " \n", " '''\n", " ##############################################################\n", " Now we write an if statement that applies a force to the\n", " center of mass if the height is less than 1.0 and applies 0 \n", " force if the height is greater than 1.0. To apply a force, we\n", " use the condynsate.simulator.apply_force_to_com function. \n", " This function has six arguments:\n", " urdf_obj : URDF_Obj\n", " A URDF_Obj to which the force is applied.\n", " force : array-like, shape (3,)\n", " The force vector in either world or body coordinates \n", " to apply to the body.\n", " body_coords : bool, optional\n", " A boolean flag that indicates whether force is \n", " given in body coords (True) or world coords (False). \n", " The default is False.\n", " show_arrow : bool, optional\n", " A boolean flag that indicates whether an arrow \n", " will be rendered on the CoM to visualize the applied \n", " force. 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.4.\n", " arrow_offset : float, optional\n", " The amount by which the drawn force arrow will be \n", " offset from the center of mass along the direction \n", " of the applied force. The default is 0.0.\n", " \n", " In this case, we want to draw the force 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", " # Apply an upward force if low\n", " if height <= 1.0:\n", " self.s.apply_force_to_com(urdf_obj = self.sphere,\n", " force = [0.,0.,20.],\n", " body_coords = False,\n", " show_arrow = True,\n", " arrow_scale = 0.05,\n", " arrow_offset = 0.5) \n", " \n", " # Apply no forces if high\n", " else:\n", " self.s.apply_force_to_com(urdf_obj = self.sphere,\n", " force = [0.,0.,0.])\n", " \n", " '''\n", " ##############################################################\n", " Suppose we also wanted to apply a torque about the center of\n", " mass of the sphere near the top of its trajectory. To do this \n", " we would need to measure not only its height, but also its \n", " upward speed. We can do this by extracting the upward speed \n", " from its state.\n", " ##############################################################\n", " '''\n", " # Extract the upward speed of the center of mass of the sphere\n", " velocity = state['velocity']\n", " upward_speed = abs(velocity[2])\n", " \n", " '''\n", " ##############################################################\n", " Now we write an if statement that applies a torque to the\n", " center of mass if the height is more than 1.0 and its upward \n", " speed is less than 2.0 in magnitude. Otherwise it applies 0 \n", " torque. To apply a torque about the center of mass of a URDF \n", " object, we use the \n", " condynsate.simulator.apply_external_torque function. This has \n", " five arguments:\n", " urdf_obj : URDF_Obj\n", " A URDF_Obj to which the torque is applied.\n", " torque : array-like, shape(3,)\n", " The torque vector in world coordinates to apply to \n", " the body.\n", " body_coords : bool, optional\n", " A boolean flag that indicates whether torque is\n", " given in body coords (True) or world coords (False). \n", " The default is False.\n", " show_arrow : bool, optional\n", " A boolean flag that indicates whether an arrow will\n", " be rendered on the com 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 by which the drawn torque arrow will be \n", " offsetnfrom the center of mass along the direction \n", " of the applied torque. 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", " # Apply torque at top of trajectory\n", " if height > 1.0 and upward_speed < 2.0:\n", " self.s.apply_external_torque(urdf_obj = self.sphere,\n", " torque = [0.,0.,0.1],\n", " body_coords = False,\n", " show_arrow = True,\n", " arrow_scale = 25.0,\n", " arrow_offset = 0.0) \n", "\n", " # Apply no torque at bottom of trajectory\n", " else:\n", " self.s.apply_external_torque(urdf_obj = self.sphere,\n", " torque = [0.,0.,0.])\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:7003/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() function so that the force applied is porportional to the magnitude of the velocity of the sphere." ] } ], "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 }