{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "68cded33-ba30-428a-9c9f-93fb43dd03a6",
   "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 3: Link 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 a `condynsate` `Project` in which forces are applied to specific links of an articulated body. We will cover:\n",
    "1. Applying forces to individual links of a .urdf object.\n",
    "2. Measuring the state of individual links 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 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": "846403b0-0ac0-4cd8-bc64-0ce4aa5e7d60",
   "metadata": {},
   "source": [
    "## Initializing the Project Class"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6e8b4d04-e298-491d-bffe-1e4f87f5dbaf",
   "metadata": {},
   "source": [
    "Here we initialize the `Project` in the same way as Tutorial 2."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "54974e36-d9c3-417a-b12a-fb9259c6f76a",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7030/static/\n"
     ]
    }
   ],
   "source": [
    "# Create the project\n",
    "proj = Project()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "d15cee85-9874-453b-82c3-25726a344d32",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "proj.visualizer.set_axes(False) # Returns 0 on success"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "5431e17b-4926-44c0-b544-f652d7ee5f1f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load a pendulum object and set its initial position to resting on the ground\n",
    "pendulum = proj.load_urdf(assets['pendulum.urdf'], \n",
    "                          fixed=True\n",
    "                         )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "399f40d8-fc5f-4b26-8a07-5c4fdbc5ea7c",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Set the initial angle of the pendulum arm\n",
    "pendulum.joints['base_to_arm'].set_initial_state(angle=0.1745) # All angles in condynsate are radians"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "1f58e78f-780c-4604-878d-5b4e162aa364",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Refresh the visualizer to show changes to the pendulums's position and angle\n",
    "proj.refresh_visualizer() # Returns 0 on success"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d7c03a28-215d-4b7c-a41c-062592923084",
   "metadata": {},
   "source": [
    "## Running a Simulation Loop"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "516a7db7-7e24-4522-86f1-27698df7ff98",
   "metadata": {},
   "source": [
    "Similary to Tutorial 0, we start the simulation loop by calling `condynsate.Project.reset`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "d8de6343-31a0-4525-8791-bb7bdb130e49",
   "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": "2f536c1e-bd87-4454-9348-e343c4adb618",
   "metadata": {},
   "source": [
    "In each step of the loop we take 3 steps\n",
    "1. Get the pendulum's mass's position\n",
    "2. Apply a force to the pendulum mass proportional to the mass's position \n",
    "3. Take a single simulation step"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d3aca127-7eaf-4be1-b7ff-1f18038628fb",
   "metadata": {},
   "source": [
    "To access the links of a body, we use the `links` attribute. The `links` attribute is a dictionary that indexes link names (as defined in the .urdf files from which bodies are loaded) to members of the `condynsate.simulator.objects.Link` class."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e127fedf-9fe5-48a6-b7e3-f768c1fd2fe7",
   "metadata": {},
   "source": [
    "To get state information about links, we access a links's `state` attribute. The `state` attribute has the following child attributes:\n",
    "\n",
    "    -----------------------------------------------------------------------------\n",
    "    | condynsate.simulator.dataclasses.LinkState                                |\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 pendulum's joint angle, we access the `angle` child attribute."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "88aa3ab7-00d3-46f0-aa69-dca04d50e821",
   "metadata": {},
   "source": [
    "To apply a force to a link, we call the `condynsate.simulator.objects.Link.apply_force` function\n",
    "\n",
    "    -----------------------------------------------------------------------------\n",
    "    | condynsate.simulator.objects.Link.apply_force                             |\n",
    "    -----------------------------------------------------------------------------\n",
    "    Applies force to the center of mass of a link.\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": "code",
   "execution_count": 8,
   "id": "5ccf9fb0-4c0f-41d8-94e7-77e36a7aa8be",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Run a 10 second simulation\n",
    "while proj.simtime <= 10.:\n",
    "    \n",
    "    # Get the pendulum's mass's x position\n",
    "    link_state = pendulum.links['mass'].state\n",
    "    x_position = link_state.position[0]\n",
    "\n",
    "    # Apply a force to the pendulum's mass\n",
    "    force = (-20.0 * x_position, 0, 0)\n",
    "    pendulum.links['mass'].apply_force(force, draw_arrow=True, arrow_scale=0.25)\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": "7d4d047e-286a-4fab-a79b-839c5f62bec3",
   "metadata": {},
   "source": [
    "Note that exactly like the previous tutorial, after `condynsate.Project.step` is called, the forces applied in the previous simulation step will not be applied in the next step. Therefore, we must call `apply_force` function before every time step in which we want to apply link forces."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cfab525b-b921-43ee-91d6-44a8a4edf317",
   "metadata": {},
   "source": [
    "Finally, we ensure all children threads exit gracefully."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "a95f4782-c4bd-4ab1-9b4f-bc3310d079f8",
   "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 to implement a PD controller where the force is applied in the body coordinates of the mass, instead of world coordinates."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c5603d1b-3706-47ce-bf20-545514f6a31a",
   "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
}