fsm modified and tested on robot

Go2Py/robot/interface/dds.py

@@ -30,6 +30,21 @@ class GO2Real():
         if self.mode == 'highlevel':
             raise NotImplementedError('DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.')
         self.simulated = False
+        self.prestanding_q = np.array([ 0.0,  1.26186061, -2.5,
+                                    0.0,  1.25883281, -2.5,
+                                    0.0,  1.27193761, -2.6,  
+                                    0.0,  1.27148342, -2.6])
+
+        self.sitting_q = np.array([-0.02495611,  1.26249647, -2.82826662,
+                                    0.04563564,  1.2505368 , -2.7933557 ,
+                                   -0.30623949,  1.28283751, -2.82314873,  
+                                    0.26400229,  1.29355574, -2.84276843])
+
+        self.standing_q = np.array([ 0.0,  0.77832842, -1.56065452,
+                                     0.0,  0.76754963, -1.56634164,
+                                     0.0,  0.76681757, -1.53601146,  
+                                     0.0,  0.75422204, -1.53229916])
+        self.latest_command_stamp = time.time()
         self.highcmd_topic_name = "rt/go2/twist_cmd"
         self.lowcmd_topic_name = "rt/go2/lowcmd"
         self.lowstate_topic_name = "rt/lowstate"
@@ -47,6 +62,7 @@ class GO2Real():
         self.running = True
         self.lowstate_thread.start()
 
+
     def lowstate_update(self):
         """
         Retrieve the state of the robot
@@ -143,15 +159,16 @@ class GO2Real():
         self.highcmd_publisher.publish(self.highcmd)
 
     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"
+        assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
         lowcmd = Go2pyLowCmd_(
-            q,
+            q_des,
-            dq, 
+            dq_des, 
             kp,
             kd,
             tau_ff
         )
         self.lowcmd_writer.write(lowcmd)
+        self.latest_command_stamp = time.time()
 
     def close(self):
         self.running = False
@@ -171,3 +188,6 @@ class GO2Real():
             scale = 1.0
 
         return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)
+
+    def overheat(self):
+        return False

Go2Py/robot/interface/ros2.py

@@ -67,6 +67,21 @@ class GO2Real(Node):
     ):
         assert mode in ['highlevel', 'lowlevel'], "mode should be either 'highlevel' or 'lowlevel'"
         self.simulated = False
+        self.prestanding_q = np.array([ 0.0,  1.26186061, -2.5,
+                                    0.0,  1.25883281, -2.5,
+                                    0.0,  1.27193761, -2.6,  
+                                    0.0,  1.27148342, -2.6])
+
+        self.sitting_q = np.array([-0.02495611,  1.26249647, -2.82826662,
+                                    0.04563564,  1.2505368 , -2.7933557 ,
+                                   -0.30623949,  1.28283751, -2.82314873,  
+                                    0.26400229,  1.29355574, -2.84276843])
+
+        self.standing_q = np.array([ 0.0,  0.77832842, -1.56065452,
+                                     0.0,  0.76754963, -1.56634164,
+                                     0.0,  0.76681757, -1.53601146,  
+                                     0.0,  0.75422204, -1.53229916])
+        self.latest_command_stamp = time.time()
         self.mode = mode
         self.node_name = "go2py_highlevel_subscriber"
         self.highcmd_topic = "/go2/twist_cmd"
@@ -204,15 +219,17 @@ class GO2Real(Node):
         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"
+        assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
         lowcmd = Go2pyLowCmd()
-        lowcmd.q = q.tolist()
+        lowcmd.q = q_des.tolist()
-        lowcmd.dq = dq.tolist()
+        lowcmd.dq = dq_des.tolist()
         lowcmd.kp = kp.tolist()
         lowcmd.kd = kd.tolist()
         lowcmd.tau = tau_ff.tolist()
         self.lowcmd_publisher.publish(lowcmd)
+        self.latest_command_stamp = time.time()
+
 
     def close(self):
         self.running = False
@@ -234,3 +251,6 @@ class GO2Real(Node):
             scale = 1.0
 
         return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)
+
+    def overheat(self):
+        return False

Go2Py/sim/mujoco.py

@@ -37,10 +37,15 @@ class Go2Sim:
         self.render = render
         self.step_counter = 0
         
-        self.sitting_q = np.array([ 0.0,  1.26186061, -2.81310153,
+        self.prestanding_q = np.array([ 0.0,  1.26186061, -2.5,
-                                    0.0,  1.25883281, -2.78329301,
+                                    0.0,  1.25883281, -2.5,
-                                    0.0,  1.27193761, -2.8052032 ,  
+                                    0.0,  1.27193761, -2.6,  
-                                    0.0,  1.27148342, -2.81436563])
+                                    0.0,  1.27148342, -2.6])
+
+        self.sitting_q = np.array([-0.02495611,  1.26249647, -2.82826662,
+                                    0.04563564,  1.2505368 , -2.7933557 ,
+                                   -0.30623949,  1.28283751, -2.82314873,  
+                                    0.26400229,  1.29355574, -2.84276843])
 
         self.standing_q = np.array([ 0.0,  0.77832842, -1.56065452,
                                      0.0,  0.76754963, -1.56634164,
@@ -89,7 +94,8 @@ class Go2Sim:
         self.viewer.sync()
 
     def getJointStates(self):
-        return self.data.qpos[7:], self.data.qvel[6:]
+        return {"q":self.data.qpos[7:], 
+               "dq":self.data.qvel[6:]}
 
     def getPose(self):
         return self.data.qpos[:3], self.data.qpos[3:7]
@@ -112,7 +118,8 @@ class Go2Sim:
         self.latest_command_stamp = time.time()
         
     def step(self):
-        q, dq = self.getJointStates()
+        state = self.getJointStates()
+        q, dq = state['q'], state['dq']
         tau = np.diag(self.kp)@(self.q_des-q).reshape(12,1)+ \
               np.diag(self.kv)@(self.dq_des-dq).reshape(12,1)+self.tau_ff.reshape(12,1)
         self.data.ctrl[:] = tau.squeeze()

examples/dds_draft.ipynb

@@ -369,7 +369,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.10"
+   "version": "3.8.18"
   }
  },
  "nbformat": 4,

examples/fsm_real.ipynb

@@ -0,0 +1,351 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from Go2Py.robot.interface.dds import GO2Real\n",
+    "import time\n",
+    "robot = GO2Real(mode='lowlevel')"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import threading\n",
+    "from pynput import keyboard\n",
+    "class BaseRemote:\n",
+    "    def __init__(self):\n",
+    "        pass\n",
+    "    def startSeq(self):\n",
+    "        return False\n",
+    "    def standUpDownSeq(self):\n",
+    "        return False\n",
+    "\n",
+    "    def flushStates(self):\n",
+    "        pass\n",
+    "\n",
+    "remote = BaseRemote()\n",
+    "\n",
+    "class KeyboardRemote(BaseRemote):\n",
+    "    def __init__(self):\n",
+    "        super().__init__()\n",
+    "        self.start_seq_flag = False\n",
+    "        self.stand_up_down_seq_flag = False\n",
+    "        self.listener_thread = threading.Thread(target=self._listen_to_keyboard)\n",
+    "        self.listener_thread.daemon = True\n",
+    "        self.listener_thread.start()\n",
+    "\n",
+    "    def _on_press(self, key):\n",
+    "        try:\n",
+    "            if key.char == 's':  # Start sequence\n",
+    "                self.start_seq_flag = True\n",
+    "            elif key.char == 'u':  # Stand up/down sequence\n",
+    "                self.stand_up_down_seq_flag = True\n",
+    "        except AttributeError:\n",
+    "            pass  # Special keys (like space) will be handled here\n",
+    "\n",
+    "    \n",
+    "    def _on_release(self, key):\n",
+    "        try:\n",
+    "            if key.char == 's':  # Start sequence\n",
+    "                self.start_seq_flag = False\n",
+    "            elif key.char == 'u':  # Stand up/down sequence\n",
+    "                self.stand_up_down_seq_flag = False\n",
+    "        except AttributeError:\n",
+    "            pass  # Special keys (like space) will be handled here\n",
+    "\n",
+    "    def _listen_to_keyboard(self):\n",
+    "        with keyboard.Listener(on_press=self._on_press, on_release=self._on_release) as listener:\n",
+    "            listener.join()\n",
+    "\n",
+    "    def startSeq(self):\n",
+    "        if self.start_seq_flag:\n",
+    "            self.start_seq_flag = False\n",
+    "            return True\n",
+    "        return False\n",
+    "\n",
+    "    def standUpDownSeq(self):\n",
+    "        if self.stand_up_down_seq_flag:\n",
+    "            self.stand_up_down_seq_flag = False\n",
+    "            return True\n",
+    "        return False\n",
+    "\n",
+    "    def flushStates(self):\n",
+    "        self.stand_up_down_seq_flag = False\n",
+    "        self.start_seq_flag = False"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "remote = KeyboardRemote()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 4,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import time\n",
+    "class SafetyHypervisor():\n",
+    "    def __init__(self, robot):\n",
+    "        self.robot = robot\n",
+    "\n",
+    "    def unsafe(self):\n",
+    "        return False\n",
+    "    \n",
+    "    def controlTimeout(self):\n",
+    "        if time.time() - robot.latest_command_stamp > 0.1:\n",
+    "            print('controller timeout')\n",
+    "            return True\n",
+    "        else:\n",
+    "            return False \n",
+    "\n",
+    "safety = SafetyHypervisor(robot)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 5,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import threading\n",
+    "import numpy as np\n",
+    "import time\n",
+    "from enum import Enum\n",
+    "\n",
+    "\n",
+    "class FSM:\n",
+    "    def __init__(self, robot, remote, safety_hypervisor, user_controller_callback=None):\n",
+    "        self.robot = robot\n",
+    "        self.remote = remote\n",
+    "        self.remote.flushStates()\n",
+    "        self.safety = safety_hypervisor\n",
+    "        self.user_controller_callback = user_controller_callback\n",
+    "\n",
+    "        self.state = \"damping\"\n",
+    "\n",
+    "        # self.tracking_kp = np.array(4*[200, 200, 350.]).reshape(12)\n",
+    "        # self.tracking_kv = np.array(12*[10.])\n",
+    "\n",
+    "        self.tracking_kp = np.array(4*[150, 150, 150.]).reshape(12)\n",
+    "        self.tracking_kv = np.array(12*[3.])\n",
+    "        self.damping_kv = np.array(12*[2.])\n",
+    "\n",
+    "\n",
+    "        self.tracking_complete = True\n",
+    "        self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), self.damping_kv, np.zeros(12))\n",
+    "        self.fsm_dT = 1./50.\n",
+    "        self.control_dT = 1./400.\n",
+    "        self.dT = self.fsm_dT\n",
+    "\n",
+    "        self.modes = {\"tracking\":self.trackingControlUpdate,\n",
+    "                      \"damping\" :self.dampingControlUpdate,\n",
+    "                      \"user\": self.userControlUpdate,\n",
+    "                     }\n",
+    "        self.setMode(\"damping\")\n",
+    "\n",
+    "        self.running = True\n",
+    "        self.fsm_thread = threading.Thread(target = self.update)\n",
+    "        self.fsm_thread.start()\n",
+    "        # if the robot is a simulation, create a thread for stepping it\n",
+    "        if robot.simulated:\n",
+    "            self.sim_thread = threading.Thread(target=self.simUpdate)\n",
+    "            self.sim_thread.start()\n",
+    "\n",
+    "    def setMode(self, mode):\n",
+    "        assert mode in self.modes.keys(), 'the requested control update mode is not implemented'\n",
+    "        self.updateCommands = self.modes[mode]\n",
+    "        # print(f'setting mode to {mode}')\n",
+    "        \n",
+    "    def moveTo(self, target, duration=0.5):\n",
+    "        # assert self.tracking_complete, 'The previous moveTo command is not completed yet!'\n",
+    "        self.q_start = self.robot.getJointStates()['q']\n",
+    "        self.q_target = target\n",
+    "        self.time = 0.0\n",
+    "        self.move_duration = duration\n",
+    "        self.q_des = lambda t: [self.q_start + np.clip((t)/self.move_duration,0, 1)*(self.q_target - self.q_start), \\\n",
+    "            True if np.clip((t)/self.move_duration,0, 1)==1 else False]  # q_des(t), Movement finished\n",
+    "        self.tracking_complete = False\n",
+    "        self.setMode(\"tracking\")\n",
+    "\n",
+    "    def trackingControlUpdate(self):\n",
+    "        self.time +=self.dT\n",
+    "        q_des, done = self.q_des(self.time)\n",
+    "        self.robot.setCommands(q_des, np.zeros(12), self.tracking_kp, self.tracking_kv, np.zeros(12))\n",
+    "        self.tracking_complete = done\n",
+    "\n",
+    "    def dampingControlUpdate(self):\n",
+    "        self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), self.damping_kv, np.zeros(12))\n",
+    "\n",
+    "    def userControlUpdate(self):\n",
+    "        if self.user_controller_callback is not None:\n",
+    "            user_controller_callback(robot, remote)\n",
+    "\n",
+    "    def simUpdate(self):\n",
+    "        while self.running:\n",
+    "            self.robot.step()\n",
+    "            time.sleep(robot.dt)\n",
+    "    \n",
+    "    def update(self):\n",
+    "        while self.running:\n",
+    "            getattr(self, self.state)()\n",
+    "            time.sleep(self.dT)\n",
+    "            self.updateCommands()\n",
+    "    \n",
+    "    def close(self):\n",
+    "        self.running = False\n",
+    "\n",
+    "    # The following functions each are the states of the FSM\n",
+    "    def damping(self):\n",
+    "        # print('damping')\n",
+    "        if self.remote.standUpDownSeq():\n",
+    "            self.moveTo(robot.prestanding_q, 1)\n",
+    "            self.state = \"pre_standing\"\n",
+    "        else:\n",
+    "            self.setMode(\"damping\")\n",
+    "            self.state = \"damping\"\n",
+    "\n",
+    "    def pre_standing(self):\n",
+    "        # print(\"pre_stance\")\n",
+    "        if self.tracking_complete:\n",
+    "            self.moveTo(robot.standing_q, duration=1.5)\n",
+    "            self.state = 'standing'\n",
+    "        else:\n",
+    "            self.state = \"pre_standing\"\n",
+    "            \n",
+    "    def standing(self):\n",
+    "        # print(\"standing\")\n",
+    "        if self.tracking_complete:\n",
+    "            # self.moveTo(robot.standing_q, duration=1)\n",
+    "            self.state = 'locked_stance'\n",
+    "        else:\n",
+    "            self.state = \"standing\"\n",
+    "\n",
+    "    def locked_stance(self):\n",
+    "        # print(\"locked_stance\")\n",
+    "        if self.remote.startSeq():\n",
+    "            self.setMode(\"user\")\n",
+    "            self.dT = self.control_dT\n",
+    "            self.state = \"user_loop\"\n",
+    "            self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12))\n",
+    "        elif self.remote.standUpDownSeq() or robot.overheat():\n",
+    "            self.moveTo(robot.sitting_q, duration = 1.5)\n",
+    "            self.state = \"sitting\"\n",
+    "\n",
+    "    def user_loop(self):\n",
+    "        # print(\"user_loop\")\n",
+    "        if self.safety.unsafe():\n",
+    "            self.dT = self.fsm_dT\n",
+    "            self.setMode(\"damping\")\n",
+    "        elif self.remote.standUpDownSeq() or self.safety.controlTimeout():\n",
+    "            self.dT = self.fsm_dT\n",
+    "            self.moveTo(robot.standing_q, duration = 1)\n",
+    "            self.timer = time.time()\n",
+    "            self.state = \"switch_back_to_locked_stance\"\n",
+    "        else:\n",
+    "            self.state = \"user_loop\"\n",
+    "\n",
+    "    def sitting(self):\n",
+    "        # print('sitting')\n",
+    "        if self.tracking_complete:\n",
+    "            self.setMode(\"damping\")\n",
+    "            self.state = 'damping'\n",
+    "        else:\n",
+    "            self.state = \"sitting\"\n",
+    "\n",
+    "    def switch_back_to_locked_stance(self):\n",
+    "        if time.time()-self.timer > 0.5:\n",
+    "            # print(\"going back to locked stance\")\n",
+    "            self.state = \"locked_stance\""
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "fsm = FSM(robot, remote, safety)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 7,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "fsm.close()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 8,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "{'q': array([-0.02495611,  1.26249647, -2.82826662,  0.04563564,  1.2505368 ,\n",
+       "        -2.7933557 , -0.30623949,  1.28283751, -2.82314873,  0.26400229,\n",
+       "         1.29355574, -2.84276843]),\n",
+       " 'dq': array([ 0.03875524,  0.0155021 ,  0.01011006,  0.00775105,  0.03875524,\n",
+       "         0.01415409, -0.04263076, -0.00387552,  0.02426415, -0.00387552,\n",
+       "         0.        , -0.02426415]),\n",
+       " 'ddq': array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]),\n",
+       " 'tau_est': array([-0.02473828,  0.04947656, -0.04741504, -0.07421485,  0.        ,\n",
+       "         0.        ,  0.04947656,  0.04947656,  0.        ,  0.02473828,\n",
+       "        -0.07421485,  0.        ]),\n",
+       " 'temperature': array([38., 32., 32., 36., 32., 33., 41., 33., 33., 41., 33., 33.])}"
+      ]
+     },
+     "execution_count": 8,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "robot.getJointStates()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "b1-env",
+   "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.8.18"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

examples/fsm_sim.ipynb

@@ -2,7 +2,7 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -12,7 +12,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -22,7 +22,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -92,7 +92,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -101,19 +101,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 6,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import time\n",
-    "while True:\n",
-    "    print(remote.standUpDownSeq())\n",
-    "    time.sleep(0.1)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -137,16 +125,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 7,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "safety.controlTimeout()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -166,10 +145,15 @@
     "\n",
     "        self.state = \"damping\"\n",
     "\n",
-    "        self.tracking_kp = np.array(4*[200, 200, 350.]).reshape(12)\n",
+    "        # self.tracking_kp = np.array(4*[200, 200, 350.]).reshape(12)\n",
-    "        self.tracking_kv = np.array(12*[10.])\n",
+    "        # self.tracking_kv = np.array(12*[10.])\n",
+    "\n",
+    "        self.tracking_kp = np.array(4*[150, 150, 150.]).reshape(12)\n",
+    "        self.tracking_kv = np.array(12*[3.])\n",
+    "        self.damping_kv = np.array(12*[2.])\n",
+    "\n",
+    "\n",
     "        self.tracking_complete = True\n",
-    "        self.damping_kv = np.array(12*[3.])\n",
     "        self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), self.damping_kv, np.zeros(12))\n",
     "        self.fsm_dT = 1./50.\n",
     "        self.control_dT = 1./400.\n",
@@ -196,7 +180,7 @@
     "        \n",
     "    def moveTo(self, target, duration=0.5):\n",
     "        # assert self.tracking_complete, 'The previous moveTo command is not completed yet!'\n",
-    "        self.q_start = self.robot.getJointStates()[0]\n",
+    "        self.q_start = self.robot.getJointStates()['q']\n",
     "        self.q_target = target\n",
     "        self.time = 0.0\n",
     "        self.move_duration = duration\n",
@@ -236,7 +220,7 @@
     "    def damping(self):\n",
     "        # print('damping')\n",
     "        if self.remote.standUpDownSeq():\n",
-    "            self.moveTo(robot.sitting_q, 1)\n",
+    "            self.moveTo(robot.prestanding_q, 1)\n",
     "            self.state = \"pre_standing\"\n",
     "        else:\n",
     "            self.setMode(\"damping\")\n",
@@ -245,7 +229,7 @@
     "    def pre_standing(self):\n",
     "        # print(\"pre_stance\")\n",
     "        if self.tracking_complete:\n",
-    "            self.moveTo(robot.standing_q, duration=2)\n",
+    "            self.moveTo(robot.standing_q, duration=1.5)\n",
     "            self.state = 'standing'\n",
     "        else:\n",
     "            self.state = \"pre_standing\"\n",
@@ -253,7 +237,7 @@
     "    def standing(self):\n",
     "        # print(\"standing\")\n",
     "        if self.tracking_complete:\n",
-    "            self.moveTo(robot.standing_q, duration=2)\n",
+    "            # self.moveTo(robot.standing_q, duration=1)\n",
     "            self.state = 'locked_stance'\n",
     "        else:\n",
     "            self.state = \"standing\"\n",
@@ -266,7 +250,7 @@
     "            self.state = \"user_loop\"\n",
     "            self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12))\n",
     "        elif self.remote.standUpDownSeq() or robot.overheat():\n",
-    "            self.moveTo(robot.sitting_q, duration = 3)\n",
+    "            self.moveTo(robot.sitting_q, duration = 1.5)\n",
     "            self.state = \"sitting\"\n",
     "\n",
     "    def user_loop(self):\n",
@@ -300,7 +284,30 @@
    "cell_type": "code",
    "execution_count": null,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stderr",
+     "output_type": "stream",
+     "text": [
+      "Exception in thread Thread-9:\n",
+      "Traceback (most recent call last):\n",
+      "  File \"/home/rstaion/miniconda3/envs/b1py/lib/python3.8/threading.py\", line 932, in _bootstrap_inner\n",
+      "    self.run()\n",
+      "  File \"/home/rstaion/miniconda3/envs/b1py/lib/python3.8/threading.py\", line 870, in run\n",
+      "    self._target(*self._args, **self._kwargs)\n",
+      "  File \"/tmp/ipykernel_130541/2701327530.py\", line 76, in simUpdate\n"
+     ]
+    },
+    {
+     "name": "stderr",
+     "output_type": "stream",
+     "text": [
+      "  File \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/sim/mujoco.py\", line 122, in step\n",
+      "    tau = np.diag(self.kp)@(self.q_des-q).reshape(12,1)+ \\\n",
+      "numpy.core._exceptions.UFuncTypeError: ufunc 'subtract' did not contain a loop with signature matching types (dtype('float64'), dtype('<U1')) -> None\n"
+     ]
+    }
+   ],
    "source": [
     "fsm = FSM(robot, remote, safety)"
    ]