From 9ea07ec6a8d863e27ddeb57e45205f8cded10683 Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Fri, 15 Mar 2024 22:41:55 -0400 Subject: [PATCH] simulator bug fixed --- Go2Py/sim/mujoco.py | 6 +-- examples/mujoco_test.ipynb | 78 +++++++++++--------------------------- 2 files changed, 26 insertions(+), 58 deletions(-) diff --git a/Go2Py/sim/mujoco.py b/Go2Py/sim/mujoco.py index 922324b..5537b1b 100644 --- a/Go2Py/sim/mujoco.py +++ b/Go2Py/sim/mujoco.py @@ -53,7 +53,7 @@ class Go2Sim: self.data.qpos = self.q_nominal self.data.qvel = np.zeros(18) - def resetStanding(self): + def standUp(self): self.q0 = np.array([ 0.00901526, 0.77832842, -1.56065452, -0.00795561, 0.76754963, -1.56634164, -0.05375515, 0.76681757, -1.53601146, @@ -64,7 +64,7 @@ class Go2Sim: mujoco.mj_step(self.model, self.data) self.viewer.sync() - def resetSitting(self): + def sitDown(self): self.q0 = np.array([-0.03479636, 1.26186061, -2.81310153, 0.03325212, 1.25883281, -2.78329301, -0.34708387, 1.27193761, -2.8052032 , @@ -81,7 +81,7 @@ class Go2Sim: def getPose(self): return self.data.qpos[:3], self.data.qpos[3:7] - def setCommands(self, q_des, dq_des, kp, kd, tau_ff): + def setCommands(self, q_des, dq_des, kp, kv, tau_ff): q, dq = self.getJointStates() tau = np.diag(kp)@(q_des-q).reshape(12,1)+ \ np.diag(kv)@(dq_des-dq).reshape(12,1)+tau_ff.reshape(12,1) diff --git a/examples/mujoco_test.ipynb b/examples/mujoco_test.ipynb index 4c0281a..b1bbd52 100644 --- a/examples/mujoco_test.ipynb +++ b/examples/mujoco_test.ipynb @@ -2,7 +2,7 @@ "cells": [ { "cell_type": "code", - "execution_count": 3, + "execution_count": 1, "metadata": {}, "outputs": [], "source": [ @@ -12,9 +12,21 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 2, "metadata": {}, - "outputs": [], + "outputs": [ + { + "ename": "AttributeError", + "evalue": "'Go2Sim' object has no attribute 'resetStanding'", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[2], line 2\u001b[0m\n\u001b[1;32m 1\u001b[0m robot \u001b[38;5;241m=\u001b[39m Go2Sim()\n\u001b[0;32m----> 2\u001b[0m \u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mresetStanding\u001b[49m()\n", + "\u001b[0;31mAttributeError\u001b[0m: 'Go2Sim' object has no attribute 'resetStanding'" + ] + } + ], "source": [ "robot = Go2Sim()\n", "robot.resetStanding()" @@ -22,20 +34,18 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 3, "metadata": {}, "outputs": [ { - "ename": "KeyboardInterrupt", - "evalue": "", + "ename": "NameError", + "evalue": "name 'env' is not defined", "output_type": "error", "traceback": [ "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[5], line 11\u001b[0m\n\u001b[1;32m 8\u001b[0m env\u001b[38;5;241m.\u001b[39msetCommands(np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), tau)\n\u001b[1;32m 9\u001b[0m \u001b[38;5;66;03m# tau[1:]=0\u001b[39;00m\n\u001b[1;32m 10\u001b[0m \u001b[38;5;66;03m# tau[0]=100*(-0.3-q[7])\u001b[39;00m\n\u001b[0;32m---> 11\u001b[0m \u001b[43menv\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mstep\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n", - "File \u001b[0;32m~/projects/rooholla/locomotion/Go2Py/Go2Py/sim/mujoco.py:96\u001b[0m, in \u001b[0;36mstep\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 94\u001b[0m # Render every render_ds_ratio steps (60Hz GUI update)\n\u001b[1;32m 95\u001b[0m if self.render and (self.step_counter%self.render_ds_ratio)==0:\n\u001b[0;32m---> 96\u001b[0m self.viewer.sync()\n\u001b[1;32m 97\u001b[0m \n\u001b[1;32m 98\u001b[0m def close(self):\n", - "File \u001b[0;32m~/miniconda3/envs/b1-env/lib/python3.9/site-packages/mujoco/viewer.py:125\u001b[0m, in \u001b[0;36mHandle.sync\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 123\u001b[0m sim \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_get_sim()\n\u001b[1;32m 124\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m sim \u001b[38;5;129;01mis\u001b[39;00m \u001b[38;5;129;01mnot\u001b[39;00m \u001b[38;5;28;01mNone\u001b[39;00m:\n\u001b[0;32m--> 125\u001b[0m \u001b[43msim\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msync\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n", - "\u001b[0;31mKeyboardInterrupt\u001b[0m: " + "\u001b[0;31mNameError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[3], line 7\u001b[0m\n\u001b[1;32m 5\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m i \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mrange\u001b[39m(\u001b[38;5;241m100000\u001b[39m):\n\u001b[1;32m 6\u001b[0m q,dq \u001b[38;5;241m=\u001b[39m robot\u001b[38;5;241m.\u001b[39mgetJointStates()\n\u001b[0;32m----> 7\u001b[0m tau \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m20\u001b[39m\u001b[38;5;241m*\u001b[39mnp\u001b[38;5;241m.\u001b[39meye(\u001b[38;5;241m12\u001b[39m)\u001b[38;5;241m@\u001b[39m(\u001b[43menv\u001b[49m\u001b[38;5;241m.\u001b[39mq0 \u001b[38;5;241m-\u001b[39m q)\u001b[38;5;241m.\u001b[39mreshape(\u001b[38;5;241m12\u001b[39m,\u001b[38;5;241m1\u001b[39m)\n\u001b[1;32m 8\u001b[0m robot\u001b[38;5;241m.\u001b[39msetCommands(np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), tau)\n\u001b[1;32m 9\u001b[0m env\u001b[38;5;241m.\u001b[39mstep()\n", + "\u001b[0;31mNameError\u001b[0m: name 'env' is not defined" ] } ], @@ -43,56 +53,14 @@ "import mujoco\n", "import time\n", "q,dq = robot.getJointStates()\n", - "robot.resetSitting()\n", + "robot.sitDown()\n", "for i in range(100000):\n", " q,dq = robot.getJointStates()\n", - " tau = 20*np.eye(12)@(env.q0 - q).reshape(12,1)\n", + " tau = 20*np.eye(12)@(robot.q0 - q).reshape(12,1)\n", " robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), tau)\n", " env.step()" ] }, - { - "cell_type": "code", - "execution_count": 9, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(11,)" - ] - }, - "execution_count": 9, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "dq.shape" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "\n", - "import matplotlib.pyplot as plt\n", - "%matplotlib\n", - "_ = plt.hist(freq_list, bins=100)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import mujoco\n", - "dir(mujoco.mjtObj)" - ] - }, { "cell_type": "code", "execution_count": null,