NAO_soft_ground.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "martial-prophet",
   "metadata": {},
   "source": [
    "## Simulation on Soft Plane\n",
    "**creating an elastic ground for NAO**\n",
    "<br>V1: Nothing works! "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "id": "sporting-process",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pybullet\n",
    "import time\n",
    "import pybullet_data\n",
    "import numpy as np\n",
    "\n",
    "from qibullet import SimulationManager\n",
    "from qibullet import NaoVirtual"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "distributed-drink",
   "metadata": {},
   "source": [
    "Loading PyBullet Physics Client, Ground Plane"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "id": "bizarre-literacy",
   "metadata": {},
   "outputs": [],
   "source": [
    "phisycsClient = pybullet.connect(pybullet.GUI)\n",
    "pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "id": "lonely-necessity",
   "metadata": {},
   "outputs": [],
   "source": [
    "pybullet.setGravity(0,0,-9.81)\n",
    "planeID = pybullet.loadURDF(\"plane.urdf\",[0,0,0])\n",
    "deformableID = pybullet.loadSoftBody(\"soft_plane.obj\",[0,0,0],collisionMargin = 0.01\n",
    "                                    ,springElasticStiffness = 0.5\n",
    "                                    ,springDampingStiffness = 10)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "quiet-relief",
   "metadata": {},
   "source": [
    "Droping a cylinder on the ground"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 66,
   "id": "amber-panic",
   "metadata": {},
   "outputs": [],
   "source": [
    "stoneID = pybullet.loadURDF(\"stone.urdf\",[0,0,15])\n",
    "pybullet.setRealTimeSimulation(1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "flying-chick",
   "metadata": {},
   "source": [
    "Spawning NAO robot using QiBullet"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "id": "official-quebec",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "simulation_manager = SimulationManager()\n",
    "simulation_manager.setGravity(phisycsClient, [0.0, 0.0, -9.81])\n",
    "\n",
    "nao = simulation_manager.spawnNao(phisycsClient, spawn_ground_plane=False)\n",
    "pybullet.setRealTimeSimulation(1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "characteristic-privilege",
   "metadata": {},
   "source": [
    "Controlling Joints"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 63,
   "id": "future-jenny",
   "metadata": {},
   "outputs": [],
   "source": [
    "nao.setAngles([\"HeadYaw\", \"HeadPitch\"], [0.5, 0.3], [0.1, 0.1])\n",
    "time.sleep(2.0)\n",
    "nao.setAngles(\"HeadYaw\", 0.0, 0.6)\n",
    "nao.setAngles(\"HeadPitch\", 0.0, 0.6)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bizarre-chrome",
   "metadata": {},
   "source": [
    "Getting joint Position and Velocity"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "knowing-anniversary",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.00011758266829786724, -0.0001136376438375301]\n",
      "0.008355050154070556   -0.0004940329118779778   -0.004256912868314794\n"
     ]
    }
   ],
   "source": [
    "angle_list = nao.getAnglesVelocity([\"HeadYaw\", \"HeadPitch\"])\n",
    "print(angle_list)\n",
    "x, y, theta = nao.getPosition() # getting base position\n",
    "print(x, \" \", y , \" \", theta)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "muslim-hindu",
   "metadata": {},
   "source": [
    "Getting IMU Values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "mature-designer",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(8.4292639933623e-05, 3.633190015242385e-05, -5.9711991297524763e-05) [0.0014624198854202792, -0.00011800724918319269, -0.0014358784116215136]\n"
     ]
    }
   ],
   "source": [
    "imu = nao.getImu()\n",
    "nao.subscribeImu(frequency=100)\n",
    "\n",
    "angular_velocity, linear_acceleration = nao.getImuValues()\n",
    "print(angular_velocity, linear_acceleration)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "parliamentary-parcel",
   "metadata": {},
   "source": [
    "Getting Joint limits"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "id": "mounted-representative",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "HeadYaw   ( -2.08567 , 2.08567 )\n",
      "HeadPitch   ( -0.671952 , 0.514872 )\n",
      "LHipYawPitch   ( -1.14529 , 0.740718 )\n",
      "LHipRoll   ( -0.379435 , 0.79046 )\n",
      "LHipPitch   ( -1.53589 , 0.48398 )\n",
      "LKneePitch   ( -0.0923279 , 2.11255 )\n",
      "LAnklePitch   ( -1.18944 , 0.922581 )\n",
      "LAnkleRoll   ( -0.397761 , 0.768992 )\n",
      "RHipYawPitch   ( -1.14529 , 0.740718 )\n",
      "RHipRoll   ( -0.79046 , 0.379435 )\n",
      "RHipPitch   ( -1.53589 , 0.48398 )\n",
      "RKneePitch   ( -0.0923279 , 2.11255 )\n",
      "RAnklePitch   ( -1.1863 , 0.932006 )\n",
      "RAnkleRoll   ( -0.768992 , 0.397761 )\n",
      "LShoulderPitch   ( -2.08567 , 2.08567 )\n",
      "LShoulderRoll   ( -0.314159 , 1.32645 )\n",
      "LElbowYaw   ( -2.08567 , 2.08567 )\n",
      "LElbowRoll   ( -1.54462 , -0.0349066 )\n",
      "LWristYaw   ( -1.82387 , 1.82387 )\n",
      "LHand   ( 0.0 , 1.0 )\n",
      "LFinger21   ( 0.0 , 0.999899 )\n",
      "LFinger22   ( 0.0 , 0.999899 )\n",
      "LFinger23   ( 0.0 , 0.999899 )\n",
      "LFinger11   ( 0.0 , 0.999899 )\n",
      "LFinger12   ( 0.0 , 0.999899 )\n",
      "LFinger13   ( 0.0 , 0.999899 )\n",
      "LThumb1   ( 0.0 , 0.999899 )\n",
      "LThumb2   ( 0.0 , 0.999899 )\n",
      "RShoulderPitch   ( -2.08567 , 2.08567 )\n",
      "RShoulderRoll   ( -1.32645 , 0.314159 )\n",
      "RElbowYaw   ( -2.08567 , 2.08567 )\n",
      "RElbowRoll   ( 0.0349066 , 1.54462 )\n",
      "RWristYaw   ( -1.82387 , 1.82387 )\n",
      "RHand   ( 0.0 , 1.0 )\n",
      "RFinger21   ( 0.0 , 0.999899 )\n",
      "RFinger22   ( 0.0 , 0.999899 )\n",
      "RFinger23   ( 0.0 , 0.999899 )\n",
      "RFinger11   ( 0.0 , 0.999899 )\n",
      "RFinger12   ( 0.0 , 0.999899 )\n",
      "RFinger13   ( 0.0 , 0.999899 )\n",
      "RThumb1   ( 0.0 , 0.999899 )\n",
      "RThumb2   ( 0.0 , 0.999899 )\n"
     ]
    }
   ],
   "source": [
    "for name, joint in nao.joint_dict.items():\n",
    "    print(name,\"  (\",joint.getLowerLimit(), \",\", joint.getUpperLimit(),\")\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "id": "ranging-batman",
   "metadata": {},
   "outputs": [],
   "source": [
    "simulation_manager.stopSimulation(phisycsClient)\n",
    "#pybullet.disconnect()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}