diff --git a/Go2Py/sim/mujoco.py b/Go2Py/sim/mujoco.py index 1830f8a..39f5ae5 100644 --- a/Go2Py/sim/mujoco.py +++ b/Go2Py/sim/mujoco.py @@ -210,7 +210,7 @@ class Go2Sim: g_in_body = R.T @ np.array([0.0, 0.0, -1.0]).reshape(3, 1) return g_in_body - def getLidarData(self): + def getLaserScan(self, max_range=30): t, q = self.getPose() world_R_body = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix() pnt = t.copy() @@ -227,10 +227,11 @@ class Go2Sim: geomid=geomid, dist=dist, nray=nray, - cutoff=mujoco.mjMAXVAL, + cutoff=max_range#mujoco.mjMAXVAL, ) pcd = dist.reshape(-1, 1) * vec - return {"pcd": pcd, "geomid": geomid, "dist": dist} + idx = np.where(np.logical_and(dist!=-1, dist=1.17.3 and <1.25.0 is required for this version of SciPy (detected version 1.26.4\n", - " warnings.warn(f\"A NumPy version >={np_minversion} and <{np_maxversion}\"\n" - ] - } - ], + "outputs": [], "source": [ "from Go2Py.sim.mujoco import Go2Sim\n", "import numpy as np" @@ -80,17 +71,9 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n" - ] - } - ], + "outputs": [], "source": [ "robot = Go2Sim(mode='highlevel')\n", "robot.standUpReset()" @@ -98,36 +81,9 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "frq: 0.06607245464787131 Hz\n", - "frq: 33.44393324455997 Hz\n", - "frq: 51.28137914170436 Hz\n", - "frq: 57.72269242943451 Hz\n", - "frq: 55.13307744886692 Hz\n", - "frq: 54.144503969534625 Hz\n", - "frq: 54.06006238238857 Hz\n", - "frq: 54.02315846417393 Hz\n", - "frq: 52.84362243612357 Hz\n", - "frq: 56.53615139914811 Hz\n", - "frq: 57.84608594913664 Hz\n", - "frq: 54.998610054811046 Hz\n", - "frq: 52.402598700649676 Hz\n", - "frq: 55.65542315755951 Hz\n", - "frq: 53.71872078279691 Hz\n", - "frq: 57.66079652465597 Hz\n", - "frq: 53.88919724534896 Hz\n", - "frq: 56.06533798505567 Hz\n", - "frq: 57.76641692375496 Hz\n", - "frq: 55.159904785702075 Hz\n" - ] - } - ], + "outputs": [], "source": [ "import time\n", "robot.standUpReset\n", @@ -138,30 +94,33 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'pcd': array([[-1.00000000e+00, -0.00000000e+00, -0.00000000e+00],\n", - " [-9.99981138e-01, -6.14188251e-03, -0.00000000e+00],\n", - " [-9.99924555e-01, -1.22835333e-02, -0.00000000e+00],\n", - " ...,\n", - " [-9.99924555e-01, 1.22835333e-02, -0.00000000e+00],\n", - " [-9.99981138e-01, 6.14188251e-03, -0.00000000e+00],\n", - " [-1.00000000e+00, 2.44929360e-16, -0.00000000e+00]]),\n", - " 'geomid': array([-1, -1, -1, ..., -1, -1, -1], dtype=int32),\n", - " 'dist': array([-1., -1., -1., ..., -1., -1., -1.])}" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ - "robot.getLidarData()" + "robot.getLaserScan()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import mujoco\n", + "mujoco.mjMAXVAL" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import matplotlib.pyplot as plt\n", + "lidar = robot.getLaserScan(max_range=3.)\n", + "idx = np.where(lidar['dist']!=-1)[0]\n", + "plt.plot(lidar['dist'][idx])" ] } ],