diff --git a/Go2Py/assets/mujoco/go2.xml b/Go2Py/assets/mujoco/go2.xml index fe8f8d1..d4f9687 100644 --- a/Go2Py/assets/mujoco/go2.xml +++ b/Go2Py/assets/mujoco/go2.xml @@ -78,33 +78,7 @@ - - - - - - - - - - - - - - - - - - - - - - - - + @@ -132,33 +106,35 @@ - - + - - - - - + + + + + - + - + - + - + - + + @@ -186,6 +162,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Go2Py/robot/interface/dds.py b/Go2Py/robot/interface/dds.py index 55261ff..6c5a45d 100644 --- a/Go2Py/robot/interface/dds.py +++ b/Go2Py/robot/interface/dds.py @@ -142,7 +142,7 @@ class GO2Real(): self.highcmd.twist.angular.z = _ω_z self.highcmd_publisher.publish(self.highcmd) - def setCommandsLow(self, q, dq, kp, kd, tau_ff): + def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff): assert q.size == dq.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12" lowcmd = Go2pyLowCmd_( q, diff --git a/Go2Py/sim/mujoco.py b/Go2Py/sim/mujoco.py index f401870..922324b 100644 --- a/Go2Py/sim/mujoco.py +++ b/Go2Py/sim/mujoco.py @@ -35,27 +35,64 @@ class Go2Sim: self.renderer = None self.render = render self.step_counter = 0 + + self.q0 = np.array([-0.03479636, 1.26186061, -2.81310153, + 0.03325212, 1.25883281, -2.78329301, + -0.34708387, 1.27193761, -2.8052032 , + 0.32040933, 1.27148342, -2.81436563]) + self.pos0 = np.array([0., 0., 0.1]) + self.rot0 = np.array([1., 0., 0., 0.]) + self.reset() + mujoco.mj_step(self.model, self.data) + self.viewer.sync() def reset(self): - self.q_nominal = np.array( - 12*[0.] + self.q_nominal = np.hstack( + [self.pos0.squeeze(), self.rot0.squeeze(), self.q0.squeeze()] ) - for i in range(12): - self.data.qpos[i] = self.q_nominal[i] + self.data.qpos = self.q_nominal + self.data.qvel = np.zeros(18) + + def resetStanding(self): + self.q0 = np.array([ 0.00901526, 0.77832842, -1.56065452, + -0.00795561, 0.76754963, -1.56634164, + -0.05375515, 0.76681757, -1.53601146, + 0.06183922, 0.75422204, -1.53229916]) + self.pos0 = np.array([0., 0., 0.33]) + self.rot0 = np.array([1., 0., 0., 0.]) + self.reset() + mujoco.mj_step(self.model, self.data) + self.viewer.sync() - self.data.qpos[7] = 0.0 - self.data.qpos[8] = 0.0 + def resetSitting(self): + self.q0 = np.array([-0.03479636, 1.26186061, -2.81310153, + 0.03325212, 1.25883281, -2.78329301, + -0.34708387, 1.27193761, -2.8052032 , + 0.32040933, 1.27148342, -2.81436563]) + self.pos0 = np.array([0., 0., 0.1]) + self.rot0 = np.array([1., 0., 0., 0.]) + self.reset() + mujoco.mj_step(self.model, self.data) + self.viewer.sync() - def step(self, tau): + def getJointStates(self): + return self.data.qpos[7:], self.data.qvel[6:] + + def getPose(self): + return self.data.qpos[:3], self.data.qpos[3:7] + + def setCommands(self, q_des, dq_des, kp, kd, 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) + self.data.ctrl[:] = tau.squeeze() + + def step(self): self.step_counter += 1 - self.data.ctrl[:] = tau mujoco.mj_step(self.model, self.data) # Render every render_ds_ratio steps (60Hz GUI update) if self.render and (self.step_counter%self.render_ds_ratio)==0: self.viewer.sync() - return self.data.qpos, self.data.qvel - - def close(self): self.viewer.close() diff --git a/examples/mujoco_test.ipynb b/examples/mujoco_test.ipynb index 8d5a6f6..4c0281a 100644 --- a/examples/mujoco_test.ipynb +++ b/examples/mujoco_test.ipynb @@ -2,7 +2,7 @@ "cells": [ { "cell_type": "code", - "execution_count": 1, + "execution_count": 3, "metadata": {}, "outputs": [], "source": [ @@ -12,69 +12,93 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 4, "metadata": {}, "outputs": [], "source": [ - "env = Go2Sim()" + "robot = Go2Sim()\n", + "robot.resetStanding()" ] }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "ename": "KeyboardInterrupt", + "evalue": "", + "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: " + ] + } + ], + "source": [ + "import mujoco\n", + "import time\n", + "q,dq = robot.getJointStates()\n", + "robot.resetSitting()\n", + "for i in range(100000):\n", + " q,dq = robot.getJointStates()\n", + " tau = 20*np.eye(12)@(env.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": [ - "False" + "(11,)" ] }, - "execution_count": 3, + "execution_count": 9, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "env.step_counter+=1\n", - "env.render and (env.step_counter%env.render_ds_ratio)==0\n" + "dq.shape" ] }, { "cell_type": "code", - "execution_count": 10, + "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "import mujoco\n", - "import time\n", - "freq_list = []\n", - "for i in range(100000):\n", - " tick = time.time()\n", - " q,dq = env.step(np.zeros(12))\n", - " tock = time.time()\n", - " freq_list.append(1/(tock-tick))" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Using matplotlib backend: TkAgg\n" - ] - } - ], "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, + "metadata": {}, + "outputs": [], + "source": [] } ], "metadata": {