EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

laserscan range filter is added.

This commit is contained in:
Rooholla-KhorramBakht 2024-05-27 11:47:55 -04:00
parent 62df03d4ae
commit fa5af751c7
2 changed files with 35 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Go2Py/sim/mujoco.py
View File

@ -210,7 +210,7 @@ class Go2Sim:
g_in_body = R.T @ np.array([0.0, 0.0, -1.0]).reshape(3, 1) g_in_body = R.T @ np.array([0.0, 0.0, -1.0]).reshape(3, 1)
return g_in_body return g_in_body
def getLidarData(self): def getLaserScan(self, max_range=30):
t, q = self.getPose() t, q = self.getPose()
world_R_body = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix() world_R_body = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
pnt = t.copy() pnt = t.copy()
@ -227,10 +227,11 @@ class Go2Sim:
geomid=geomid, geomid=geomid,
dist=dist, dist=dist,
nray=nray, nray=nray,
cutoff=mujoco.mjMAXVAL, cutoff=max_range#mujoco.mjMAXVAL,
) )
pcd = dist.reshape(-1, 1) * vec pcd = dist.reshape(-1, 1) * vec
return {"pcd": pcd, "geomid": geomid, "dist": dist} idx = np.where(np.logical_and(dist!=-1, dist<max_range))[0]
return {"pcd": pcd[idx,...], "geomid": geomid[idx,...], "dist": dist[idx,...]}
def overheat(self): def overheat(self):
return False return False

103
examples/02-MuJoCo-sim.ipynb
View File

@ -61,18 +61,9 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [],
{
"name": "stderr",
"output_type": "stream",
"text": [
"/usr/lib/python3/dist-packages/scipy/__init__.py:146: UserWarning: A NumPy version >=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"
]
}
],
"source": [ "source": [
"from Go2Py.sim.mujoco import Go2Sim\n", "from Go2Py.sim.mujoco import Go2Sim\n",
"import numpy as np" "import numpy as np"
@ -80,17 +71,9 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [],
{
"name": "stdout",
"output_type": "stream",
"text": [
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
]
}
],
"source": [ "source": [
"robot = Go2Sim(mode='highlevel')\n", "robot = Go2Sim(mode='highlevel')\n",
"robot.standUpReset()" "robot.standUpReset()"
@ -98,36 +81,9 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [ "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"
]
}
],
"source": [ "source": [
"import time\n", "import time\n",
"robot.standUpReset\n", "robot.standUpReset\n",
@ -138,30 +94,33 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 4, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [ "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"
}
],
"source": [ "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])"
] ]
} }
], ],