{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "6aa391a8-0a92-43a9-94c1-7cc5d185b1b1",
   "metadata": {},
   "source": [
    "© Copyright, 2025 G. Schaer.\n",
    "\n",
    "SPDX-License-Identifier: GPL-3.0-only"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9bd6dd9a-a0f6-4248-89d4-657c318e0d79",
   "metadata": {},
   "source": [
    "# Tutorial 1: External Forces and Torques"
   ]
  },
  {
   "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 a `condynsate` `Project` in which external forces are applied to the center of mass of a a cube. 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 0."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "b2b6dc1e-8420-4242-91a3-e73422ce713e",
   "metadata": {},
   "outputs": [],
   "source": [
    "from condynsate import Project\n",
    "from condynsate import __assets__ as assets"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dea83d70-5877-4057-b588-983d28579958",
   "metadata": {},
   "source": [
    "## Initializing the Project Class"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "507fb740-3a04-4ff7-af5e-fbbd3c73f962",
   "metadata": {},
   "source": [
    "Here we initialize the `Project` in the same way as Tutorial 0."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "c80e5845-e2f6-40df-be51-d13b49a1bac8",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7028/static/\n"
     ]
    }
   ],
   "source": [
    "# Create the project\n",
    "proj = Project()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "4bcd701f-d09a-4ba5-b805-8c6511cf628c",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Turn off the grid and axes\n",
    "proj.visualizer.set_axes(False) # Returns 0 on success\n",
    "proj.visualizer.set_grid(False) # Returns 0 on success"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "5288ec30-bebf-4adc-a1f9-8e23e5ad7d7e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load a fixed plane as the ground object\n",
    "ground = proj.load_urdf(assets['plane_medium.urdf'], fixed=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "f9f24b1a-0258-451d-b3d9-d4cd20462f0a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Load a cube object that is resting on the ground plane\n",
    "cube = proj.load_urdf(assets['cube.urdf'])\n",
    "cube.set_initial_state(position=(0, 0, 0.5))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "57eb0633-4e59-430b-a5cd-1ace9254970d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Refresh the visualizer to show changes to the cube's position\n",
    "proj.refresh_visualizer() # Returns 0 on success"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "420433c6-c1d2-4702-aba4-52295bf753e2",
   "metadata": {},
   "source": [
    "## Running a Simulation Loop"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "da509ec5-56c1-4bc4-a9d0-0db238bd069a",
   "metadata": {},
   "source": [
    "Similary to Tutorial 0, we start the simulation loop by calling `condynsate.Project.reset`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "7712fce5-69c9-44f0-859c-7a7a4e37ba7b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "proj.reset() # Returns 0 on success"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a8a12c6a-122d-4d64-aab3-f5fbe90df6ec",
   "metadata": {},
   "source": [
    "In each step of the loop we take 4 steps\n",
    "1. Calculate the cube's height above the ground\n",
    "2. Apply an upward force (in the +z world direction) to the center of mass of the cube proportional to the cube's height\n",
    "3. Apply an external torque to the cube\n",
    "4. Take a single simulation step"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d996aff5-516c-4bb6-8251-17472f29e8b9",
   "metadata": {},
   "source": [
    "To get state information about the cube, we access the cube's `state` attribute. The cube's `state` attribute has the following child attributes:\n",
    "\n",
    "    -----------------------------------------------------------------------------\n",
    "    | condynsate.simulator.dataclasses.BodyState                                |\n",
    "    -----------------------------------------------------------------------------\n",
    "    Attributes\n",
    "    ----------\n",
    "    position : 3 tuple of floats\n",
    "        The (x,y,z) position in world coordinates.\n",
    "    orientation : 4 tuple of floats\n",
    "        The wxyz quaternion representation of the orientation in world\n",
    "        coordinates.\n",
    "    ypr : 3 tuple of floats\n",
    "        The (z-y'-x' Tait–Bryan) Euler angles in radians ordered as\n",
    "        (yaw, pitch, roll).\n",
    "    velocity : 3 tuple of floats\n",
    "        The (x,y,z) velocity in world coordinates.\n",
    "    omega : 3 tuple of floats\n",
    "        The (x,y,z) angular velocity in world coordinates.\n",
    "    velocity_in_body : 3 tuple of floats\n",
    "        The (x,y,z) velocity in body coordinates.\n",
    "    omega_in_body : 3 tuple of floats\n",
    "        The (x,y,z) angular velocity in body coordinates.\n",
    "\n",
    "To get the cube's position in world coordinates, we access the `position` child attribute. Based on this position and the length of the cube, we can then calculate how high the cube is above the ground plane."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ecf057ee-0517-439c-827c-e3015df1d279",
   "metadata": {},
   "source": [
    "External forces are applied to simulator objects with the `condynsate.simulator.objects.Body.apply_force` function\n",
    "\n",
    "    -----------------------------------------------------------------------------\n",
    "    | condynsate.simulator.objects.Body.apply_force                             |\n",
    "    -----------------------------------------------------------------------------\n",
    "    Applies force to the center of mass of the body.\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    force : 3 tuple of floats\n",
    "        The force being applied to the center of mass.\n",
    "    **kwargs\n",
    "\n",
    "    Keyword Args\n",
    "    ------------\n",
    "    body : bool, optional\n",
    "        A Boolean flag that indicates if the force argument is in\n",
    "        body coordinates (True), or in world coordinates (False).\n",
    "        The default is False.\n",
    "    draw_arrow : bool, optional\n",
    "        A Boolean flag that indicates if an arrow should be drawn\n",
    "        to represent the applied force. The default is False.\n",
    "    arrow_scale : float, optional\n",
    "        The scaling factor, relative to the size of the applied force,\n",
    "        that is used to size the force arrow. The default is 1.0.\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    ret_code : int\n",
    "        0 if successful, -1 if something went wrong."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "79a4de03-c599-4c8b-822f-9a2573d66c89",
   "metadata": {},
   "source": [
    "External torques are applied to simulator objects with the `condynsate.simulator.objects.Body.apply_torque` function\n",
    "\n",
    "    -----------------------------------------------------------------------------\n",
    "    | condynsate.simulator.objects.Body.apply_torque                            |\n",
    "    -----------------------------------------------------------------------------\n",
    "    Applies external torque to the body.\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    torque : 3 tuple of floats\n",
    "        The torque being applied.\n",
    "    **kwargs\n",
    "\n",
    "    Keyword Args\n",
    "    ------------\n",
    "    body : bool, optional\n",
    "        A Boolean flag that indicates if the torque argument is in\n",
    "        body coordinates (True), or in world coordinates (False).\n",
    "        The default is False.\n",
    "    draw_arrow : bool, optional\n",
    "        A Boolean flag that indicates if an arrow should be drawn\n",
    "        to represent the applied torque. The default is False.\n",
    "    arrow_scale : float, optional\n",
    "        The scaling factor, relative to the size of the applied torque,\n",
    "        that is used to size the torque arrow. The default is 1.0.\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    ret_code : int\n",
    "        0 if successful, -1 if something went wrong."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "5de2c636-cf08-4007-8f67-3d8e40605f36",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Run a 10 second simulation\n",
    "while proj.simtime <= 10.:\n",
    "    \n",
    "    # Get the cube's height above the ground\n",
    "    position = cube.state.position\n",
    "    height = position[2] - 0.5\n",
    "\n",
    "    # Calculate an upward force proportional to the cube's height and apply it to the cube\n",
    "    upward_force = 9.81 + (1.0-height) * 0.1\n",
    "    cube.apply_force((0, 0, upward_force), body=False, draw_arrow=True, arrow_scale=0.15)\n",
    "\n",
    "    # Apply a small external torque to the cube\n",
    "    cube.apply_torque((0.02, 0.02, 0.02), body=False, draw_arrow=True, arrow_scale=55.0)\n",
    "\n",
    "    # Take a single simulation step\n",
    "    proj.step(real_time=True, # Run the simulation in real time\n",
    "              stable_step=False # Dynamically adjust the refresh rate for best total run time\n",
    "             )"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b8141388-0859-4c2c-bf9e-b2f958872b94",
   "metadata": {},
   "source": [
    "Note that after `condynsate.Project.step` is called, the forces and torques applied in the previous simulation step will not be applied in the next step. Therefore, we must call the `apply_force` and `apply_torque` functions before every time step in which we want to apply external inputs"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cdc72244-8841-43ed-947f-b07004decb85",
   "metadata": {},
   "source": [
    "Finally, we ensure all children threads exit gracefully."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "9bdff44b-aaab-4fe6-bff8-dcf85e1eb55d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "proj.terminate() # Returns 0 on success"
   ]
  },
  {
   "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 simulation loop so that the force applied is also porportional to the magnitude of the velocity of the cube."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "73e7b4e4-6932-496c-a264-229c26d512a8",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.14.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}