Isaacsim and Mujoco simulator classes are added.
This commit is contained in:
Rooholla-KhorramBakht
parent
caeaeca1dd
commit
f2ee4f1526
|
|
@ -0,0 +1,9 @@
|
|||
import os
|
||||
import platform
|
||||
import sys
|
||||
|
||||
ASSETS_PATH = os.path.join(os.path.dirname(__file__), "assets")
|
||||
GO2_USD_PATH = os.path.join(os.path.dirname(__file__), "assets/usd/go2.usd")
|
||||
GO2_ISAACSIM_CFG_PATH = os.path.join(
|
||||
os.path.dirname(__file__), "sim/isaacsim/sim_config.yaml"
|
||||
)
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics")
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
# Unitree Go2 Description (MJCF)
|
||||
|
||||
Requires MuJoCo 2.2.2 or later.
|
||||
|
||||
## Overview
|
||||
|
||||
This package contains a simplified robot description (MJCF) of the [Go2
|
||||
Quadruped Robot](https://www.unitree.com/go2/) developed by [Unitree
|
||||
Robotics](https://www.unitree.com/). It is derived from the [publicly available
|
||||
URDF
|
||||
description](https://github.com/unitreerobotics/unitree_ros/tree/master/robots/go2_description).
|
||||
|
||||
<p float="left">
|
||||
<img src="go2.png" width="400">
|
||||
</p>
|
||||
|
||||
## URDF → MJCF derivation steps
|
||||
|
||||
1. Converted the DAE [mesh
|
||||
files](https://github.com/unitreerobotics/unitree_mujoco/tree/main/data/a1/meshes)
|
||||
to OBJ format using [Blender](https://www.blender.org/).
|
||||
- When exporting, ensure "up axis" is `+Z`, and "forward axis" is `+Y`.
|
||||
2. Processed `.obj` files with [`obj2mjcf`](https://github.com/kevinzakka/obj2mjcf).
|
||||
3. Added `<mujoco> <compiler discardvisual="false" strippath="false" fusestatic="false"/> </mujoco>` to the URDF's
|
||||
`<robot>` clause in order to preserve visual geometries.
|
||||
4. Loaded the URDF into MuJoCo and saved a corresponding MJCF.
|
||||
5. Added a `<freejoint/>` to the base.
|
||||
6. Manually edited the MJCF to extract common properties into the `<default>` section.
|
||||
7. Softened the contacts of the feet to approximate the effect of rubber and
|
||||
increased `impratio` to reduce slippage.
|
||||
8. Added `scene.xml` which includes the robot, with a textured groundplane, skybox, and haze.
|
||||
|
||||
## License
|
||||
|
||||
This model is released under a [BSD-3-Clause License](LICENSE).
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 1.8 MiB |
|
|
@ -0,0 +1,211 @@
|
|||
<mujoco model="go2">
|
||||
<compiler angle="radian" meshdir="assets" autolimits="true"/>
|
||||
|
||||
<option cone="elliptic" impratio="100"/>
|
||||
|
||||
<default>
|
||||
<default class="go2">
|
||||
<geom friction="0.6" margin="0.001" condim="1"/>
|
||||
<joint axis="0 1 0" damping="0" armature="0.01" frictionloss="0.2"/>
|
||||
<motor ctrlrange="-23.7 23.7"/>
|
||||
<default class="abduction">
|
||||
<joint axis="1 0 0" range="-1.0472 1.0472"/>
|
||||
</default>
|
||||
<default class="hip">
|
||||
<default class="front_hip">
|
||||
<joint range="-1.5708 3.4907"/>
|
||||
</default>
|
||||
<default class="back_hip">
|
||||
<joint range="-0.5236 4.5379"/>
|
||||
</default>
|
||||
</default>
|
||||
<default class="knee">
|
||||
<joint range="-2.7227 -0.83776"/>
|
||||
<motor ctrlrange="-45.43 45.43"/>
|
||||
</default>
|
||||
<default class="visual">
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
|
||||
</default>
|
||||
<default class="collision">
|
||||
<geom group="3"/>
|
||||
<default class="foot">
|
||||
<geom size="0.022" pos="-0.002 0 -0.213" priority="1" solimp="0.015 1 0.031" condim="6"
|
||||
friction="0.8 0.02 0.01"/>
|
||||
</default>
|
||||
</default>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<asset>
|
||||
<material name="metal" rgba=".9 .95 .95 1"/>
|
||||
<material name="black" rgba="0 0 0 1"/>
|
||||
<material name="white" rgba="1 1 1 1"/>
|
||||
<material name="gray" rgba="0.671705 0.692426 0.774270 1"/>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
|
||||
|
||||
<mesh file="base_0.obj"/>
|
||||
<mesh file="base_1.obj"/>
|
||||
<mesh file="base_2.obj"/>
|
||||
<mesh file="base_3.obj"/>
|
||||
<mesh file="base_4.obj"/>
|
||||
<mesh file="hip_0.obj"/>
|
||||
<mesh file="hip_1.obj"/>
|
||||
<mesh file="thigh_0.obj"/>
|
||||
<mesh file="thigh_1.obj"/>
|
||||
<mesh file="thigh_mirror_0.obj"/>
|
||||
<mesh file="thigh_mirror_1.obj"/>
|
||||
<mesh file="calf_0.obj"/>
|
||||
<mesh file="calf_1.obj"/>
|
||||
<mesh file="calf_mirror_0.obj"/>
|
||||
<mesh file="calf_mirror_1.obj"/>
|
||||
<mesh file="foot.obj"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
|
||||
<body name="base" pos="0 0 0.445" childclass="go2">
|
||||
<inertial pos="0.021112 0 -0.005366" quat="-0.000543471 0.713435 -0.00173769 0.700719" mass="6.921"
|
||||
diaginertia="0.107027 0.0980771 0.0244531"/>
|
||||
<freejoint/>
|
||||
<geom mesh="base_0" material="black" class="visual"/>
|
||||
<geom mesh="base_1" material="black" class="visual"/>
|
||||
<geom mesh="base_2" material="black" class="visual"/>
|
||||
<geom mesh="base_3" material="white" class="visual"/>
|
||||
<geom mesh="base_4" material="gray" class="visual"/>
|
||||
<geom size="0.1881 0.04675 0.057" type="box" class="collision"/>
|
||||
<geom size="0.05 0.045" pos="0.285 0 0.01" type="cylinder" class="collision"/>
|
||||
<geom size="0.047" pos="0.293 0 -0.06" class="collision"/>
|
||||
<site name="imu" pos="-0.02557 0 0.04232"/>
|
||||
<body name="FL_hip" pos="0.1934 0.0465 0">
|
||||
<inertial pos="-0.0054 0.00194 -0.000105" quat="0.497014 0.499245 0.505462 0.498237" mass="0.678"
|
||||
diaginertia="0.00088403 0.000596003 0.000479967"/>
|
||||
<joint name="FL_hip_joint" class="abduction"/>
|
||||
<geom mesh="hip_0" material="metal" class="visual"/>
|
||||
<geom mesh="hip_1" material="gray" class="visual"/>
|
||||
<geom size="0.046 0.02" pos="0 0.08 0" quat="1 1 0 0" type="cylinder" class="collision"/>
|
||||
<body name="FL_thigh" pos="0 0.0955 0">
|
||||
<inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623" mass="1.152"
|
||||
diaginertia="0.00594973 0.00584149 0.000878787"/>
|
||||
<joint name="FL_thigh_joint" class="front_hip"/>
|
||||
<geom mesh="thigh_0" material="metal" class="visual"/>
|
||||
<geom mesh="thigh_1" material="gray" class="visual"/>
|
||||
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
|
||||
<body name="FL_calf" pos="0 0 -0.213">
|
||||
<inertial pos="0.00629595 -0.000622121 -0.141417" quat="0.710672 0.00154099 -0.00450087 0.703508"
|
||||
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
|
||||
<joint name="FL_calf_joint" class="knee"/>
|
||||
<geom mesh="calf_0" material="gray" class="visual"/>
|
||||
<geom mesh="calf_1" material="black" class="visual"/>
|
||||
<geom size="0.012 0.06" pos="0.008 0 -0.06" quat="0.994493 0 -0.104807 0" type="cylinder" class="collision"/>
|
||||
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
|
||||
<geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
|
||||
<geom name="FL" class="foot"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="FR_hip" pos="0.1934 -0.0465 0">
|
||||
<inertial pos="-0.0054 -0.00194 -0.000105" quat="0.498237 0.505462 0.499245 0.497014" mass="0.678"
|
||||
diaginertia="0.00088403 0.000596003 0.000479967"/>
|
||||
<joint name="FR_hip_joint" class="abduction"/>
|
||||
<geom mesh="hip_0" material="metal" class="visual" quat="4.63268e-05 1 0 0"/>
|
||||
<geom mesh="hip_1" material="gray" class="visual" quat="4.63268e-05 1 0 0"/>
|
||||
<geom size="0.046 0.02" pos="0 -0.08 0" quat="0.707107 0.707107 0 0" type="cylinder" class="collision"/>
|
||||
<body name="FR_thigh" pos="0 -0.0955 0">
|
||||
<inertial pos="-0.00374 0.0223 -0.0327" quat="0.551623 -0.0200632 0.0847635 0.829533" mass="1.152"
|
||||
diaginertia="0.00594973 0.00584149 0.000878787"/>
|
||||
<joint name="FR_thigh_joint" class="front_hip"/>
|
||||
<geom mesh="thigh_mirror_0" material="metal" class="visual"/>
|
||||
<geom mesh="thigh_mirror_1" material="gray" class="visual"/>
|
||||
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
|
||||
<body name="FR_calf" pos="0 0 -0.213">
|
||||
<inertial pos="0.00629595 0.000622121 -0.141417" quat="0.703508 -0.00450087 0.00154099 0.710672"
|
||||
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
|
||||
<joint name="FR_calf_joint" class="knee"/>
|
||||
<geom mesh="calf_mirror_0" material="gray" class="visual"/>
|
||||
<geom mesh="calf_mirror_1" material="black" class="visual"/>
|
||||
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0" type="cylinder" class="collision"/>
|
||||
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
|
||||
<geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
|
||||
<geom name="FR" class="foot"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="RL_hip" pos="-0.1934 0.0465 0">
|
||||
<inertial pos="0.0054 0.00194 -0.000105" quat="0.505462 0.498237 0.497014 0.499245" mass="0.678"
|
||||
diaginertia="0.00088403 0.000596003 0.000479967"/>
|
||||
<joint name="RL_hip_joint" class="abduction"/>
|
||||
<geom mesh="hip_0" material="metal" class="visual" quat="4.63268e-05 0 1 0"/>
|
||||
<geom mesh="hip_1" material="gray" class="visual" quat="4.63268e-05 0 1 0"/>
|
||||
<geom size="0.046 0.02" pos="0 0.08 0" quat="0.707107 0.707107 0 0" type="cylinder" class="collision"/>
|
||||
<body name="RL_thigh" pos="0 0.0955 0">
|
||||
<inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623" mass="1.152"
|
||||
diaginertia="0.00594973 0.00584149 0.000878787"/>
|
||||
<joint name="RL_thigh_joint" class="back_hip"/>
|
||||
<geom mesh="thigh_0" material="metal" class="visual"/>
|
||||
<geom mesh="thigh_1" material="gray" class="visual"/>
|
||||
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
|
||||
<body name="RL_calf" pos="0 0 -0.213">
|
||||
<inertial pos="0.00629595 -0.000622121 -0.141417" quat="0.710672 0.00154099 -0.00450087 0.703508"
|
||||
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
|
||||
<joint name="RL_calf_joint" class="knee"/>
|
||||
<geom mesh="calf_0" material="gray" class="visual"/>
|
||||
<geom mesh="calf_1" material="black" class="visual"/>
|
||||
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0" type="cylinder" class="collision"/>
|
||||
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
|
||||
<geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
|
||||
<geom name="RL" class="foot"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="RR_hip" pos="-0.1934 -0.0465 0">
|
||||
<inertial pos="0.0054 -0.00194 -0.000105" quat="0.499245 0.497014 0.498237 0.505462" mass="0.678"
|
||||
diaginertia="0.00088403 0.000596003 0.000479967"/>
|
||||
<joint name="RR_hip_joint" class="abduction"/>
|
||||
<geom mesh="hip_0" material="metal" class="visual" quat="2.14617e-09 4.63268e-05 4.63268e-05 -1"/>
|
||||
<geom mesh="hip_1" material="gray" class="visual" quat="2.14617e-09 4.63268e-05 4.63268e-05 -1"/>
|
||||
<geom size="0.046 0.02" pos="0 -0.08 0" quat="0.707107 0.707107 0 0" type="cylinder" class="collision"/>
|
||||
<body name="RR_thigh" pos="0 -0.0955 0">
|
||||
<inertial pos="-0.00374 0.0223 -0.0327" quat="0.551623 -0.0200632 0.0847635 0.829533" mass="1.152"
|
||||
diaginertia="0.00594973 0.00584149 0.000878787"/>
|
||||
<joint name="RR_thigh_joint" class="back_hip"/>
|
||||
<geom mesh="thigh_mirror_0" material="metal" class="visual"/>
|
||||
<geom mesh="thigh_mirror_1" material="gray" class="visual"/>
|
||||
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
|
||||
<body name="RR_calf" pos="0 0 -0.213">
|
||||
<inertial pos="0.00629595 0.000622121 -0.141417" quat="0.703508 -0.00450087 0.00154099 0.710672"
|
||||
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
|
||||
<joint name="RR_calf_joint" class="knee"/>
|
||||
<geom mesh="calf_mirror_0" material="gray" class="visual"/>
|
||||
<geom mesh="calf_mirror_1" material="black" class="visual"/>
|
||||
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0" type="cylinder" class="collision"/>
|
||||
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
|
||||
<geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
|
||||
<geom name="RR" class="foot"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<motor class="abduction" name="FR_hip" joint="FR_hip_joint"/>
|
||||
<motor class="hip" name="FR_thigh" joint="FR_thigh_joint"/>
|
||||
<motor class="knee" name="FR_calf" joint="FR_calf_joint"/>
|
||||
<motor class="abduction" name="FL_hip" joint="FL_hip_joint"/>
|
||||
<motor class="hip" name="FL_thigh" joint="FL_thigh_joint"/>
|
||||
<motor class="knee" name="FL_calf" joint="FL_calf_joint"/>
|
||||
<motor class="abduction" name="RR_hip" joint="RR_hip_joint"/>
|
||||
<motor class="hip" name="RR_thigh" joint="RR_thigh_joint"/>
|
||||
<motor class="knee" name="RR_calf" joint="RR_calf_joint"/>
|
||||
<motor class="abduction" name="RL_hip" joint="RL_hip_joint"/>
|
||||
<motor class="hip" name="RL_thigh" joint="RL_thigh_joint"/>
|
||||
<motor class="knee" name="RL_calf" joint="RL_calf_joint"/>
|
||||
</actuator>
|
||||
|
||||
<keyframe>
|
||||
<key name="home" qpos="0 0 0.27 1 0 0 0 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8"
|
||||
ctrl="0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
<mujoco model="go2 scene">
|
||||
<include file="go2.xml"/>
|
||||
|
||||
<statistic center="0 0 0.1" extent="0.8"/>
|
||||
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
|
||||
<rgba haze="0.15 0.25 0.35 1"/>
|
||||
<global azimuth="-130" elevation="-20"/>
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
|
||||
markrgb="0.8 0.8 0.8" width="300" height="300"/>
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
|
|
@ -8,7 +8,7 @@ Stephen Brawner (brawner@gmail.com)
|
|||
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot name="go2_description">
|
||||
<link name="base">
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.021112 0 -0.005366"/>
|
||||
<mass value="6.921"/>
|
||||
|
|
@ -45,7 +45,7 @@ Stephen Brawner (brawner@gmail.com)
|
|||
</link>
|
||||
<joint dont_collapse="true" name="Head_upper_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.285 0 0.01"/>
|
||||
<parent link="base"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="Head_upper"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
|
@ -92,7 +92,7 @@ Stephen Brawner (brawner@gmail.com)
|
|||
</link>
|
||||
<joint name="FL_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.1934 0.0465 0"/>
|
||||
<parent link="base"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="FL_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
|
|
@ -238,7 +238,7 @@ Stephen Brawner (brawner@gmail.com)
|
|||
</link>
|
||||
<joint name="FR_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.1934 -0.0465 0"/>
|
||||
<parent link="base"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="FR_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
|
|
@ -384,7 +384,7 @@ Stephen Brawner (brawner@gmail.com)
|
|||
</link>
|
||||
<joint name="RL_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.1934 0.0465 0"/>
|
||||
<parent link="base"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="RL_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
|
|
@ -530,7 +530,7 @@ Stephen Brawner (brawner@gmail.com)
|
|||
</link>
|
||||
<joint name="RR_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.1934 -0.0465 0"/>
|
||||
<parent link="base"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="RR_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
|
|
@ -652,7 +652,7 @@ Stephen Brawner (brawner@gmail.com)
|
|||
<child link="RR_foot"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="imu">
|
||||
<link name="imu_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0"/>
|
||||
|
|
@ -661,8 +661,8 @@ Stephen Brawner (brawner@gmail.com)
|
|||
</link>
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.02557 0 0.04232"/>
|
||||
<parent link="base"/>
|
||||
<child link="imu"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="imu_link"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="radar">
|
||||
|
|
@ -674,7 +674,7 @@ Stephen Brawner (brawner@gmail.com)
|
|||
</link>
|
||||
<joint name="radar_joint" type="fixed">
|
||||
<origin rpy="0 2.8782 0" xyz="0.28945 0 -0.046825"/>
|
||||
<parent link="base"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="radar"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,124 @@
|
|||
import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
from B1Py import B1_ISAACSIM_CFG_PATH
|
||||
from B1Py.lcm_types.unitree_lowlevel import UnitreeLowCommand, UnitreeLowState
|
||||
from B1Py.sim.utils import LCMBridgeClient, NumpyMemMapDataPipe, load_config
|
||||
|
||||
|
||||
class B1IsaacSim:
|
||||
"""
|
||||
Class for communication with the simulated Franka Emika FR3 robot in Isaac Sim.
|
||||
It uses LCM to send joint velocity to the simulated robot and receive joint states
|
||||
(joint angle, velocity, and torque) and camera images from the robot.
|
||||
"""
|
||||
|
||||
def __init__(self, robot_id="b1"):
|
||||
"""
|
||||
@param robot_id: (str) The name of the robot in the Isaac Sim scene.
|
||||
"""
|
||||
self.lcm_bridge = LCMBridgeClient(robot_name=robot_id)
|
||||
self.cfg = load_config(B1_ISAACSIM_CFG_PATH)
|
||||
self.camera_pipes = {}
|
||||
for camera in self.cfg["cameras"]:
|
||||
for type in camera["type"]:
|
||||
if type == "rgb":
|
||||
shape = (camera["resolution"][1], camera["resolution"][0], 4)
|
||||
else:
|
||||
shape = (camera["resolution"][1], camera["resolution"][0])
|
||||
|
||||
self.camera_pipes[camera["name"] + "_" + type] = NumpyMemMapDataPipe(
|
||||
camera["name"] + "_" + type, force=False, shape=shape
|
||||
)
|
||||
print(camera["name"] + "_" + type)
|
||||
|
||||
def LCMThreadFunc(self):
|
||||
"""
|
||||
Function that runs on a separate thread and handles LCM communication.
|
||||
`lc.handle()` function is called when there are data available to be processed.
|
||||
"""
|
||||
while self.running:
|
||||
rfds, wfds, efds = select.select([self.lc.fileno()], [], [], 0.5)
|
||||
if rfds: # Handle only if there are data in the interface file
|
||||
self.lc.handle()
|
||||
|
||||
def getStates(self):
|
||||
"""
|
||||
Get the current joint angle, velocity, and torque of the robot.
|
||||
|
||||
@return: (dict or None) The joint state {'q': (numpy.ndarray, shape=(n,)),
|
||||
'dq': (numpy.ndarray, shape=(n,)), 'T': (numpy.ndarray, shape=(n,))}
|
||||
if the latest update was received less than 0.2 seconds ago;
|
||||
otherwise, return None.
|
||||
"""
|
||||
state = self.lcm_bridge.getStates(timeout=0.1)
|
||||
if state is not None:
|
||||
q = np.array(state.q)
|
||||
dq = np.array(state.dq)
|
||||
tau_est = np.array(state.tau_est)
|
||||
contact_state = np.array(state.contact_state)
|
||||
accel = np.array(state.accel)
|
||||
gyro = np.array(state.gyro)
|
||||
temperature = np.array(state.temperature)
|
||||
quaternion = np.array(state.quaternion)
|
||||
rpy = np.array(state.rpy)
|
||||
gravity = np.array(state.gravity)
|
||||
gt_pos = np.array(state.gt_pos)
|
||||
gt_quat = np.array(state.gt_quat)
|
||||
gt_lin_vel = np.array(state.gt_lin_vel)
|
||||
gt_ang_vel = np.array(state.gt_ang_vel)
|
||||
return {
|
||||
"q": q,
|
||||
"dq": dq,
|
||||
"tau_est": tau_est,
|
||||
"contact_state": contact_state,
|
||||
"accel": accel,
|
||||
"gyro": gyro,
|
||||
"temperature": temperature,
|
||||
"quaternion": quaternion,
|
||||
"rpy": rpy,
|
||||
"gravity": gravity,
|
||||
"gt_pos": gt_pos,
|
||||
"gt_quat": gt_quat,
|
||||
"gt_lin_vel": gt_lin_vel,
|
||||
"gt_ang_vel": gt_ang_vel,
|
||||
}
|
||||
|
||||
else:
|
||||
return None
|
||||
|
||||
def readCameras(self):
|
||||
data = {key: pipe.read() for key, pipe in self.camera_pipes.items()}
|
||||
return data
|
||||
|
||||
def sendCommands(self, kp, kd, q_des, dq_des, tau_ff):
|
||||
"""
|
||||
Send a joint velocity command to the robot.
|
||||
@param kp: (numpy.ndarray, shape=(n,)) The proportional gain for the joint position control.
|
||||
@param kd: (numpy.ndarray, shape=(n,)) The derivative gain for the joint position control.
|
||||
@param q_des: (numpy.ndarray, shape=(n,)) The desired joint position.
|
||||
@param dq_des: (numpy.ndarray, shape=(n,)) The desired joint velocity.
|
||||
@param tau_ff: (numpy.ndarray, shape=(n,)) The feedforward torque.
|
||||
"""
|
||||
cmd_lcm = UnitreeLowCommand()
|
||||
cmd_lcm.tau_ff = tau_ff.tolist()
|
||||
cmd_lcm.kp = kp.tolist()
|
||||
cmd_lcm.kd = kd.tolist()
|
||||
cmd_lcm.q_des = q_des.tolist()
|
||||
cmd_lcm.dq_des = dq_des.tolist()
|
||||
self.lcm_bridge.sendCommands(cmd_lcm)
|
||||
|
||||
def close(self):
|
||||
"""
|
||||
Stop the LCM thread, unsubscribe from the LCM topic,
|
||||
and effectively shut down the interface.
|
||||
"""
|
||||
self.lcm_bridge.close()
|
||||
|
||||
def reset(self):
|
||||
time.sleep(0.3)
|
||||
for i in range(100):
|
||||
time.sleep(0.01)
|
||||
self.sendCommands(np.zeros(9))
|
||||
state = self.getStates()
|
||||
|
|
@ -0,0 +1,272 @@
|
|||
from collections import deque
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
from omni.isaac.core.articulations import Articulation
|
||||
from omni.isaac.core.utils.prims import define_prim
|
||||
from omni.isaac.sensor import ContactSensor, IMUSensor
|
||||
|
||||
import Go2Py
|
||||
from Go2Py.sim.isaacsim.lcm_types.unitree_lowlevel import UnitreeLowState
|
||||
|
||||
|
||||
class UnitreeGo2(Articulation):
|
||||
def __init__(
|
||||
self,
|
||||
prim_path: str,
|
||||
name: str = "go2_quadruped",
|
||||
physics_dt: Optional[float] = 1 / 400.0,
|
||||
position: Optional[np.ndarray] = None,
|
||||
orientation: Optional[np.ndarray] = None,
|
||||
usd_path: Optional[str] = None,
|
||||
) -> None:
|
||||
"""
|
||||
[Summary]
|
||||
|
||||
initialize robot, set up sensors and interfaces
|
||||
|
||||
Args:
|
||||
prim_path {str} -- prim path of the robot on the stage
|
||||
name {str} -- name of the quadruped
|
||||
physics_dt {float} -- physics downtime of the controller
|
||||
position {np.ndarray} -- position of the robot
|
||||
orientation {np.ndarray} -- orientation of the robot
|
||||
"""
|
||||
self._prim_path = prim_path
|
||||
prim = define_prim(self._prim_path, "Xform")
|
||||
prim.GetReferences().AddReference(Go2Py.GO2_USD_PATH)
|
||||
self.usd_path = usd_path
|
||||
if self.usd_path is None:
|
||||
prim.GetReferences().AddReference(Go2Py.GO2_USD_PATH)
|
||||
else:
|
||||
prim.GetReferences().AddReference(self.usd_path)
|
||||
|
||||
super().__init__(
|
||||
prim_path=self._prim_path,
|
||||
name=name,
|
||||
position=position,
|
||||
orientation=orientation,
|
||||
)
|
||||
|
||||
# contact sensor setup
|
||||
self.feet_order = ["FL", "RL", "FR", "RR"]
|
||||
self.feet_path = [
|
||||
self._prim_path + "/FL_foot",
|
||||
self._prim_path + "/FR_foot",
|
||||
self._prim_path + "/RL_foot",
|
||||
self._prim_path + "/RR_foot",
|
||||
]
|
||||
|
||||
self.color = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1), (1, 1, 0, 1)]
|
||||
self._contact_sensors = [None] * 4
|
||||
for i in range(4):
|
||||
self._contact_sensors[i] = ContactSensor(
|
||||
prim_path=self.feet_path[i] + "/sensor",
|
||||
min_threshold=0,
|
||||
max_threshold=1000000,
|
||||
radius=0.043,
|
||||
dt=physics_dt,
|
||||
)
|
||||
|
||||
self.foot_force = np.zeros(4)
|
||||
self.enable_foot_filter = True
|
||||
self._FILTER_WINDOW_SIZE = 20
|
||||
self._foot_filters = [deque(), deque(), deque(), deque()]
|
||||
# imu sensor setup
|
||||
self.imu_path = self._prim_path + "/imu_link"
|
||||
self._imu_sensor = IMUSensor(
|
||||
prim_path=self.imu_path + "/imu_sensor",
|
||||
name="imu",
|
||||
dt=physics_dt,
|
||||
translation=np.array([0, 0, 0]),
|
||||
orientation=np.array([1, 0, 0, 0]),
|
||||
)
|
||||
self.accel = np.zeros((3,))
|
||||
self.gyro = np.zeros((3,))
|
||||
|
||||
# Translation maps between Isaac and Bullet
|
||||
# self.bullet_joint_order = ['FL_hip_joint', 'FL_thigh_joint', 'FL_calf_joint',
|
||||
# 'RL_hip_joint', 'RL_thigh_joint', 'RL_calf_joint',
|
||||
# 'FR_hip_joint', 'FR_thigh_joint', 'FR_calf_joint',
|
||||
# 'RR_hip_joint', 'RR_thigh_joint', 'RR_calf_joint']
|
||||
|
||||
self.bullet_joint_order = [
|
||||
"FL_hip_joint",
|
||||
"FL_thigh_joint",
|
||||
"FL_calf_joint",
|
||||
"FR_hip_joint",
|
||||
"FR_thigh_joint",
|
||||
"FR_calf_joint",
|
||||
"RL_hip_joint",
|
||||
"RL_thigh_joint",
|
||||
"RL_calf_joint",
|
||||
"RR_hip_joint",
|
||||
"RR_thigh_joint",
|
||||
"RR_calf_joint",
|
||||
]
|
||||
self.isaac_joint_order = [
|
||||
"FL_hip_joint",
|
||||
"FR_hip_joint",
|
||||
"RL_hip_joint",
|
||||
"RR_hip_joint",
|
||||
"FL_thigh_joint",
|
||||
"FR_thigh_joint",
|
||||
"RL_thigh_joint",
|
||||
"RR_thigh_joint",
|
||||
"FL_calf_joint",
|
||||
"FR_calf_joint",
|
||||
"RL_calf_joint",
|
||||
"RR_calf_joint",
|
||||
]
|
||||
|
||||
self.isaac_name_2_index = {s: i for i, s in enumerate(self.isaac_joint_order)}
|
||||
self.bullet_name_2_index = {s: i for i, s in enumerate(self.bullet_joint_order)}
|
||||
|
||||
self.to_bullet_index = np.array(
|
||||
[self.isaac_name_2_index[id] for id in self.bullet_joint_order]
|
||||
)
|
||||
self.to_isaac_index = np.array(
|
||||
[self.bullet_name_2_index[id] for id in self.isaac_joint_order]
|
||||
)
|
||||
self.tau_est = np.zeros((12,))
|
||||
self.state = UnitreeLowState()
|
||||
self.init_pos = np.array([0.0, 0.0, 0.6])
|
||||
self.init_quat = np.array([0.0, 0.0, 0.0, 1.0])
|
||||
self.init_joint_pos = np.array(
|
||||
[0.2, 0.8, -1.5, -0.2, 0.8, -1.5, 0.2, 1.0, -1.6, -0.2, 1.0, -1.6]
|
||||
)
|
||||
return
|
||||
|
||||
def toIsaacOrder(self, x):
|
||||
return x[self.to_isaac_index, ...]
|
||||
|
||||
def toBulletOrder(self, x):
|
||||
return x[self.to_bullet_index, ...]
|
||||
|
||||
def setState(self, pos, quat, q) -> None:
|
||||
"""[Summary]
|
||||
|
||||
Set the kinematic state of the robot.
|
||||
|
||||
Args:
|
||||
pos {ndarray} -- The position of the robot (x, y, z)
|
||||
quat {ndarray} -- The orientation of the robot (qx, qy, qz, qw)
|
||||
q {ndarray} -- Joint angles of the robot in standard Pinocchio order
|
||||
|
||||
Raises:
|
||||
RuntimeError: When the DC Toolbox interface has not been configured.
|
||||
"""
|
||||
self.set_world_pose(position=pos, orientation=quat[[3, 0, 1, 2]])
|
||||
self.set_linear_velocity(np.zeros((3,)))
|
||||
self.set_angular_velocity(np.zeros((3,)))
|
||||
|
||||
self.set_joint_positions(
|
||||
positions=np.asarray(self.toIsaacOrder(q), dtype=np.float32)
|
||||
)
|
||||
self.set_joint_velocities(
|
||||
velocities=np.asarray(self.toIsaacOrder(np.zeros((12,))), dtype=np.float32)
|
||||
)
|
||||
self.set_joint_efforts(np.zeros_like(q))
|
||||
return
|
||||
|
||||
def initialize(self, physics_sim_view=None) -> None:
|
||||
"""[summary]
|
||||
|
||||
initialize dc interface, set up drive mode and initial robot state
|
||||
|
||||
"""
|
||||
super().initialize(physics_sim_view=physics_sim_view)
|
||||
self.get_articulation_controller().set_effort_modes("force")
|
||||
self.get_articulation_controller().switch_control_mode("effort")
|
||||
self.setState(self.init_pos, self.init_quat, self.init_joint_pos)
|
||||
for i in range(4):
|
||||
self._contact_sensors[i].initialize()
|
||||
return
|
||||
|
||||
def readContactSensor(self) -> None:
|
||||
"""[summary]
|
||||
|
||||
Updates processed contact sensor data from the robot feet, store them in member variable foot_force
|
||||
"""
|
||||
# Order: FL, RL, FR, RR
|
||||
for i in range(len(self.feet_path)):
|
||||
frame = self._contact_sensors[i].get_current_frame()
|
||||
if "force" in frame:
|
||||
if self.enable_foot_filter:
|
||||
self._foot_filters[i].append(frame["force"])
|
||||
if len(self._foot_filters[i]) > self._FILTER_WINDOW_SIZE:
|
||||
self._foot_filters[i].popleft()
|
||||
self.foot_force[i] = np.mean(self._foot_filters[i])
|
||||
|
||||
else:
|
||||
self.foot_force[i] = frame["force"]
|
||||
return self.foot_force
|
||||
|
||||
def readIMUSensor(self) -> None:
|
||||
"""[summary]
|
||||
|
||||
Updates processed imu sensor data from the robot body, store them in member variable base_lin and ang_vel
|
||||
"""
|
||||
frame = self._imu_sensor.get_current_frame()
|
||||
self.accel = frame["lin_acc"]
|
||||
self.gyro = frame["ang_vel"]
|
||||
return self.accel, self.gyro
|
||||
|
||||
def readStates(self):
|
||||
contact_forces = self.readContactSensor()
|
||||
accel, gyro = self.readIMUSensor()
|
||||
|
||||
# joint pos and vel from the DC interface
|
||||
joint_state = super().get_joints_state()
|
||||
joint_pos = self.toBulletOrder(joint_state.positions)
|
||||
joint_vel = self.toBulletOrder(joint_state.velocities)
|
||||
|
||||
# base frame
|
||||
base_pose = self.get_world_pose()
|
||||
base_pos = base_pose[0]
|
||||
base_quat = base_pose[1][[1, 2, 3, 0]]
|
||||
base_lin_vel = self.get_linear_velocity()
|
||||
base_ang_vel = self.get_angular_velocity()
|
||||
|
||||
# assign to state objects
|
||||
self.state.accel = accel.tolist()
|
||||
self.state.gyro = gyro.tolist()
|
||||
self.state.q = joint_pos.tolist()
|
||||
self.state.dq = joint_vel.tolist()
|
||||
self.state.tau_est = self.tau_est
|
||||
self.state.quaternion = base_quat.tolist()
|
||||
self.state.gt_pos = base_pos.tolist()
|
||||
self.state.gt_quat = base_quat.tolist()
|
||||
self.state.gt_lin_vel = base_lin_vel.tolist()
|
||||
self.state.gt_ang_vel = base_ang_vel.tolist()
|
||||
self.state.contact_state = contact_forces.tolist()
|
||||
# projected_gravity = pp.SO3(base_quat).matrix().T @ np.array(
|
||||
# [0.0, 0.0, -1.0]
|
||||
# ).reshape(3, 1)
|
||||
# self.state.gravity = (
|
||||
# projected_gravity.squeeze().numpy().astype(np.float32).tolist()
|
||||
# )
|
||||
return self.state
|
||||
|
||||
def setCommands(self, cmd):
|
||||
"""[summary]
|
||||
sets the joint torques
|
||||
Argument:
|
||||
action {np.ndarray} -- Joint torque command
|
||||
"""
|
||||
q = np.array(self.state.q)
|
||||
dq = np.array(self.state.dq)
|
||||
kp = np.array(cmd.kp)
|
||||
kd = np.array(cmd.kd)
|
||||
tau_ff = np.array(cmd.tau_ff)
|
||||
q_des = np.array(cmd.q_des)
|
||||
dq_des = np.array(cmd.dq_des)
|
||||
tau = (q_des - q) * kp + (dq_des - dq) * kd + tau_ff
|
||||
self.set_joint_efforts(np.asarray(self.toIsaacOrder(tau), dtype=np.float32))
|
||||
self.tau_est = tau
|
||||
return
|
||||
|
||||
def step(self, cmd):
|
||||
self.readStates()
|
||||
self.setCommands(cmd)
|
||||
return self.state
|
||||
|
|
@ -0,0 +1,213 @@
|
|||
# launch Isaac Sim before any other imports
|
||||
# default first two lines in any standalone application
|
||||
import pickle
|
||||
import sys
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
from omni.isaac.kit import SimulationApp
|
||||
|
||||
# sys.path.append("/home/vr-station/projects/lcm/build/python")
|
||||
|
||||
|
||||
simulation_app = SimulationApp({"headless": False}) # we can also run as headless.
|
||||
|
||||
# import cv2
|
||||
from omni.isaac.core import World
|
||||
from omni.isaac.core.utils.nucleus import get_assets_root_path
|
||||
from omni.isaac.core.utils.prims import define_prim, get_prim_at_path
|
||||
from omni.isaac.sensor import RotatingLidarPhysX
|
||||
|
||||
import Go2Py
|
||||
from Go2Py.sim.isaacsim.lcm_types.unitree_lowlevel import UnitreeLowCommand
|
||||
from Go2Py.sim.isaacsim.go2 import UnitreeGo2
|
||||
from Go2Py.sim.isaacsim.utils import AnnotatorManager
|
||||
from Go2Py.sim.utils import (
|
||||
NumpyMemMapDataPipe,
|
||||
load_config,
|
||||
simulationManager,
|
||||
)
|
||||
|
||||
cfg = load_config(Go2Py.GO2_ISAACSIM_CFG_PATH)
|
||||
robots = cfg["robots"]
|
||||
cameras = cfg["cameras"]
|
||||
env_cfg = cfg["environment"]
|
||||
|
||||
PHYSICS_DT = 1 / 200
|
||||
RENDERING_DT = 1 / 200
|
||||
|
||||
# create the world
|
||||
world = World(
|
||||
physics_dt=cfg["environment"]["simulation_dt"],
|
||||
rendering_dt=cfg["environment"]["rendering_dt"],
|
||||
)
|
||||
|
||||
assets_root_path = get_assets_root_path()
|
||||
if assets_root_path is None:
|
||||
print("Could not find Isaac Sim assets folder")
|
||||
|
||||
prim = get_prim_at_path(env_cfg["prim_path"])
|
||||
if not prim.IsValid():
|
||||
prim = define_prim(env_cfg["prim_path"], "Xform")
|
||||
asset_path = (
|
||||
assets_root_path + env_cfg["usd_path"]
|
||||
if env_cfg["buildin"] is True
|
||||
else env_cfg["usd_path"]
|
||||
)
|
||||
prim.GetReferences().AddReference(asset_path)
|
||||
|
||||
breakpoint()
|
||||
go2 = world.scene.add(
|
||||
UnitreeGo2(
|
||||
prim_path=robots[0]["prim_path"],
|
||||
usd_path=(robots[0]["usd_path"] if robots[0]["usd_path"] != "" else None),
|
||||
name=robots[0]["name"],
|
||||
position=np.array(robots[0]["position"]),
|
||||
physics_dt=cfg["environment"]["simulation_dt"],
|
||||
)
|
||||
)
|
||||
|
||||
# # Add Lidar
|
||||
# lidar = world.scene.add(
|
||||
# RotatingLidarPhysX(
|
||||
# prim_path="/World/Env/GO2/imu_link/lidar",
|
||||
# name="lidar",
|
||||
# translation=[0.16, 0.0, 0.14],
|
||||
# orientation=[0.0, 0.0, 0.0, 1.0],
|
||||
# )
|
||||
# )
|
||||
|
||||
# lidar.add_depth_data_to_frame()
|
||||
# lidar.add_point_cloud_data_to_frame()
|
||||
# lidar.set_rotation_frequency(0)
|
||||
# lidar.set_resolution([0.4, 2])
|
||||
# # lidar.enable_visualization()
|
||||
# lidar.prim.GetAttribute("highLod").Set(True)
|
||||
# lidar.prim.GetAttribute("highLod").Set(True)
|
||||
|
||||
# lidar_data_pipe = NumpyMemMapDataPipe(
|
||||
# "lidar_data_pipe", force=True, dtype="float32", shape=(900, 16, 3)
|
||||
# )
|
||||
|
||||
world.reset()
|
||||
go2.initialize()
|
||||
breakpoint()
|
||||
# Add cameras
|
||||
print("Adding cameras")
|
||||
ann = AnnotatorManager(world)
|
||||
|
||||
for camera in cameras:
|
||||
ann.registerCamera(
|
||||
camera["prim_path"],
|
||||
camera["name"],
|
||||
translation=camera["translation"], # xyz
|
||||
orientation=camera["orientation"], # xyzw
|
||||
resolution=camera["resolution"],
|
||||
)
|
||||
for type in camera["type"]:
|
||||
ann.registerAnnotator(type, camera["name"])
|
||||
|
||||
ann.setClippingRange(camera["name"], 0.2, 1000000.0)
|
||||
ann.setFocalLength(camera["name"], 28)
|
||||
|
||||
# Add the shared memory data channels for image and LiDAR data
|
||||
print("Creating shared memory data pipes")
|
||||
camera_pipes = {}
|
||||
for camera in cameras:
|
||||
for type in camera["type"]:
|
||||
if type == "pointcloud":
|
||||
pipe = NumpyMemMapDataPipe(
|
||||
camera["name"] + "_" + type,
|
||||
force=True,
|
||||
dtype="float32",
|
||||
shape=(camera["resolution"][1] * camera["resolution"][0], 3),
|
||||
)
|
||||
camera_pipes[camera["name"] + "_" + type] = pipe
|
||||
elif type == "rgb":
|
||||
pipe = NumpyMemMapDataPipe(
|
||||
camera["name"] + "_" + type,
|
||||
force=True,
|
||||
dtype="uint8",
|
||||
shape=(camera["resolution"][1], camera["resolution"][0], 4),
|
||||
)
|
||||
camera_pipes[camera["name"] + "_" + type] = pipe
|
||||
elif type == "distance_to_camera":
|
||||
pipe = NumpyMemMapDataPipe(
|
||||
camera["name"] + "_" + type,
|
||||
force=True,
|
||||
dtype="uint8",
|
||||
shape=(camera["resolution"][1], camera["resolution"][0]),
|
||||
)
|
||||
camera_pipes[camera["name"] + "_" + type] = pipe
|
||||
|
||||
# Store simulation hyperparamters in shared memory
|
||||
print("Storing simulation hyperparamters in shared memory")
|
||||
|
||||
meta_data = {
|
||||
"camera_names": [camera["name"] for camera in cameras],
|
||||
"camera_types": [camera["type"] for camera in cameras],
|
||||
"camera_resolutions": [camera["resolution"] for camera in cameras],
|
||||
"camera_intrinsics": [
|
||||
ann.getCameraIntrinsics(camera["name"]) for camera in cameras
|
||||
],
|
||||
"camera_extrinsics": [
|
||||
ann.getCameraExtrinsics(camera["name"]) for camera in cameras
|
||||
],
|
||||
}
|
||||
with open("/dev/shm/fr3_sim_meta_data.pkl", "wb") as f:
|
||||
pickle.dump(meta_data, f)
|
||||
|
||||
lcm_server = LCMBridgeServer(robot_name="b1")
|
||||
cmd_stamp = time.time()
|
||||
cmd_stamp_old = cmd_stamp
|
||||
|
||||
cmd = UnitreeLowCommand()
|
||||
cmd.kd = 12 * [2.5]
|
||||
cmd.kp = 12 * [100]
|
||||
cmd.dq_des = 12 * [0]
|
||||
cmd.q_des = b1.init_joint_pos
|
||||
|
||||
# sim_manager = simulationManager(
|
||||
# robot=go2,
|
||||
# lcm_server=lcm_server,
|
||||
# default_cmd=cmd,
|
||||
# physics_dt=PHYSICS_DT,
|
||||
# lcm_timeout=1e-4,
|
||||
# )
|
||||
counter = 0
|
||||
while simulation_app.is_running():
|
||||
# sim_manager.step(counter*PHYSICS_DT)
|
||||
# Step the world with rendering 50 times per second
|
||||
# sim_manager.step(counter * PHYSICS_DT)
|
||||
if counter % 4 == 0:
|
||||
world.step(render=True)
|
||||
pc = lidar.get_current_frame()["point_cloud"]
|
||||
lidar_data_pipe.write(pc, match_length=True)
|
||||
|
||||
# Push the sensor data to the shared memory pipes
|
||||
for camera in cameras:
|
||||
for type in camera["type"]:
|
||||
data = ann.getData(f"{camera['name']}:{type}")
|
||||
if type == "pointcloud":
|
||||
payload = data["data"]
|
||||
if payload.shape[0]:
|
||||
camera_pipes[camera["name"] + "_" + type].write(
|
||||
np.zeros(
|
||||
(camera["resolution"][1] * camera["resolution"][0], 3)
|
||||
),
|
||||
match_length=True,
|
||||
)
|
||||
camera_pipes[camera["name"] + "_" + type].write(
|
||||
payload, match_length=True
|
||||
)
|
||||
else:
|
||||
if data.shape[0]:
|
||||
camera_pipes[camera["name"] + "_" + type].write(
|
||||
data, match_length=False
|
||||
)
|
||||
|
||||
else:
|
||||
world.step(render=False)
|
||||
|
||||
counter += 1
|
||||
simulation_app.close() # close Isaac Sim
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
package unitree_lowlevel;
|
||||
struct UnitreeLowCommand {
|
||||
double stamp;
|
||||
float tau_ff[12];
|
||||
float q_des[12];
|
||||
float dq_des[12];
|
||||
float kp[12];
|
||||
float kd[12];
|
||||
}
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
package unitree_lowlevel;
|
||||
struct UnitreeLowState {
|
||||
double stamp;
|
||||
float q[12];
|
||||
float dq[12];
|
||||
float tau_est[12];
|
||||
float contact_state[4];
|
||||
float accel[3];
|
||||
float gyro[3];
|
||||
float temperature;
|
||||
float quaternion[4];
|
||||
float rpy[3];
|
||||
float gravity[3];
|
||||
float gt_pos[3];
|
||||
float gt_quat[4];
|
||||
float gt_lin_vel[3];
|
||||
float gt_ang_vel[3];
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
#!/bin/bash
|
||||
|
||||
|
||||
GREEN='\033[0;32m'
|
||||
NC='\033[0m' # No Color
|
||||
|
||||
echo -e "${GREEN} Starting LCM type generation...${NC}"
|
||||
# Clean
|
||||
rm -r unitree_lowlevel
|
||||
# Make
|
||||
lcm-gen -xp *.lcm
|
||||
echo -e "${GREEN} Done with LCM type generation${NC}"
|
||||
|
|
@ -0,0 +1,193 @@
|
|||
/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
|
||||
* BY HAND!!
|
||||
*
|
||||
* Generated by lcm-gen
|
||||
**/
|
||||
|
||||
#ifndef __unitree_lowlevel_UnitreeLowCommand_hpp__
|
||||
#define __unitree_lowlevel_UnitreeLowCommand_hpp__
|
||||
|
||||
#include <lcm/lcm_coretypes.h>
|
||||
|
||||
|
||||
namespace unitree_lowlevel
|
||||
{
|
||||
|
||||
class UnitreeLowCommand
|
||||
{
|
||||
public:
|
||||
double stamp;
|
||||
|
||||
float tau_ff[12];
|
||||
|
||||
float q_des[12];
|
||||
|
||||
float dq_des[12];
|
||||
|
||||
float kp[12];
|
||||
|
||||
float kd[12];
|
||||
|
||||
public:
|
||||
/**
|
||||
* Encode a message into binary form.
|
||||
*
|
||||
* @param buf The output buffer.
|
||||
* @param offset Encoding starts at this byte offset into @p buf.
|
||||
* @param maxlen Maximum number of bytes to write. This should generally be
|
||||
* equal to getEncodedSize().
|
||||
* @return The number of bytes encoded, or <0 on error.
|
||||
*/
|
||||
inline int encode(void *buf, int offset, int maxlen) const;
|
||||
|
||||
/**
|
||||
* Check how many bytes are required to encode this message.
|
||||
*/
|
||||
inline int getEncodedSize() const;
|
||||
|
||||
/**
|
||||
* Decode a message from binary form into this instance.
|
||||
*
|
||||
* @param buf The buffer containing the encoded message.
|
||||
* @param offset The byte offset into @p buf where the encoded message starts.
|
||||
* @param maxlen The maximum number of bytes to read while decoding.
|
||||
* @return The number of bytes decoded, or <0 if an error occurred.
|
||||
*/
|
||||
inline int decode(const void *buf, int offset, int maxlen);
|
||||
|
||||
/**
|
||||
* Retrieve the 64-bit fingerprint identifying the structure of the message.
|
||||
* Note that the fingerprint is the same for all instances of the same
|
||||
* message type, and is a fingerprint on the message type definition, not on
|
||||
* the message contents.
|
||||
*/
|
||||
inline static int64_t getHash();
|
||||
|
||||
/**
|
||||
* Returns "UnitreeLowCommand"
|
||||
*/
|
||||
inline static const char* getTypeName();
|
||||
|
||||
// LCM support functions. Users should not call these
|
||||
inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
|
||||
inline int _getEncodedSizeNoHash() const;
|
||||
inline int _decodeNoHash(const void *buf, int offset, int maxlen);
|
||||
inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
|
||||
};
|
||||
|
||||
int UnitreeLowCommand::encode(void *buf, int offset, int maxlen) const
|
||||
{
|
||||
int pos = 0, tlen;
|
||||
int64_t hash = getHash();
|
||||
|
||||
tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = this->_encodeNoHash(buf, offset + pos, maxlen - pos);
|
||||
if (tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
int UnitreeLowCommand::decode(const void *buf, int offset, int maxlen)
|
||||
{
|
||||
int pos = 0, thislen;
|
||||
|
||||
int64_t msg_hash;
|
||||
thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &msg_hash, 1);
|
||||
if (thislen < 0) return thislen; else pos += thislen;
|
||||
if (msg_hash != getHash()) return -1;
|
||||
|
||||
thislen = this->_decodeNoHash(buf, offset + pos, maxlen - pos);
|
||||
if (thislen < 0) return thislen; else pos += thislen;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
int UnitreeLowCommand::getEncodedSize() const
|
||||
{
|
||||
return 8 + _getEncodedSizeNoHash();
|
||||
}
|
||||
|
||||
int64_t UnitreeLowCommand::getHash()
|
||||
{
|
||||
static int64_t hash = static_cast<int64_t>(_computeHash(NULL));
|
||||
return hash;
|
||||
}
|
||||
|
||||
const char* UnitreeLowCommand::getTypeName()
|
||||
{
|
||||
return "UnitreeLowCommand";
|
||||
}
|
||||
|
||||
int UnitreeLowCommand::_encodeNoHash(void *buf, int offset, int maxlen) const
|
||||
{
|
||||
int pos = 0, tlen;
|
||||
|
||||
tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->stamp, 1);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->tau_ff[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->q_des[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->dq_des[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->kp[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->kd[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
int UnitreeLowCommand::_decodeNoHash(const void *buf, int offset, int maxlen)
|
||||
{
|
||||
int pos = 0, tlen;
|
||||
|
||||
tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->stamp, 1);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->tau_ff[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->q_des[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->dq_des[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->kp[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->kd[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
int UnitreeLowCommand::_getEncodedSizeNoHash() const
|
||||
{
|
||||
int enc_size = 0;
|
||||
enc_size += __double_encoded_array_size(NULL, 1);
|
||||
enc_size += __float_encoded_array_size(NULL, 12);
|
||||
enc_size += __float_encoded_array_size(NULL, 12);
|
||||
enc_size += __float_encoded_array_size(NULL, 12);
|
||||
enc_size += __float_encoded_array_size(NULL, 12);
|
||||
enc_size += __float_encoded_array_size(NULL, 12);
|
||||
return enc_size;
|
||||
}
|
||||
|
||||
uint64_t UnitreeLowCommand::_computeHash(const __lcm_hash_ptr *)
|
||||
{
|
||||
uint64_t hash = 0x3fa55736ccae518dLL;
|
||||
return (hash<<1) + ((hash>>63)&1);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
"""LCM type definitions
|
||||
This file automatically generated by lcm.
|
||||
DO NOT MODIFY BY HAND!!!!
|
||||
"""
|
||||
|
||||
try:
|
||||
import cStringIO.StringIO as BytesIO
|
||||
except ImportError:
|
||||
from io import BytesIO
|
||||
import struct
|
||||
|
||||
|
||||
class UnitreeLowCommand:
|
||||
__slots__ = ["stamp", "tau_ff", "q_des", "dq_des", "kp", "kd"]
|
||||
|
||||
__typenames__ = ["double", "float", "float", "float", "float", "float"]
|
||||
|
||||
__dimensions__ = [None, [12], [12], [12], [12], [12]]
|
||||
|
||||
def __init__(self):
|
||||
self.stamp = 0.0
|
||||
self.tau_ff = [0.0 for dim0 in range(12)]
|
||||
self.q_des = [0.0 for dim0 in range(12)]
|
||||
self.dq_des = [0.0 for dim0 in range(12)]
|
||||
self.kp = [0.0 for dim0 in range(12)]
|
||||
self.kd = [0.0 for dim0 in range(12)]
|
||||
|
||||
def encode(self):
|
||||
buf = BytesIO()
|
||||
buf.write(UnitreeLowCommand._get_packed_fingerprint())
|
||||
self._encode_one(buf)
|
||||
return buf.getvalue()
|
||||
|
||||
def _encode_one(self, buf):
|
||||
buf.write(struct.pack(">d", self.stamp))
|
||||
buf.write(struct.pack(">12f", *self.tau_ff[:12]))
|
||||
buf.write(struct.pack(">12f", *self.q_des[:12]))
|
||||
buf.write(struct.pack(">12f", *self.dq_des[:12]))
|
||||
buf.write(struct.pack(">12f", *self.kp[:12]))
|
||||
buf.write(struct.pack(">12f", *self.kd[:12]))
|
||||
|
||||
def decode(data):
|
||||
if hasattr(data, "read"):
|
||||
buf = data
|
||||
else:
|
||||
buf = BytesIO(data)
|
||||
if buf.read(8) != UnitreeLowCommand._get_packed_fingerprint():
|
||||
raise ValueError("Decode error")
|
||||
return UnitreeLowCommand._decode_one(buf)
|
||||
|
||||
decode = staticmethod(decode)
|
||||
|
||||
def _decode_one(buf):
|
||||
self = UnitreeLowCommand()
|
||||
self.stamp = struct.unpack(">d", buf.read(8))[0]
|
||||
self.tau_ff = struct.unpack(">12f", buf.read(48))
|
||||
self.q_des = struct.unpack(">12f", buf.read(48))
|
||||
self.dq_des = struct.unpack(">12f", buf.read(48))
|
||||
self.kp = struct.unpack(">12f", buf.read(48))
|
||||
self.kd = struct.unpack(">12f", buf.read(48))
|
||||
return self
|
||||
|
||||
_decode_one = staticmethod(_decode_one)
|
||||
|
||||
def _get_hash_recursive(parents):
|
||||
if UnitreeLowCommand in parents:
|
||||
return 0
|
||||
tmphash = (0x3FA55736CCAE518D) & 0xFFFFFFFFFFFFFFFF
|
||||
tmphash = (
|
||||
((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)
|
||||
) & 0xFFFFFFFFFFFFFFFF
|
||||
return tmphash
|
||||
|
||||
_get_hash_recursive = staticmethod(_get_hash_recursive)
|
||||
_packed_fingerprint = None
|
||||
|
||||
def _get_packed_fingerprint():
|
||||
if UnitreeLowCommand._packed_fingerprint is None:
|
||||
UnitreeLowCommand._packed_fingerprint = struct.pack(
|
||||
">Q", UnitreeLowCommand._get_hash_recursive([])
|
||||
)
|
||||
return UnitreeLowCommand._packed_fingerprint
|
||||
|
||||
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
|
||||
|
||||
def get_hash(self):
|
||||
"""Get the LCM hash of the struct"""
|
||||
return struct.unpack(">Q", UnitreeLowCommand._get_packed_fingerprint())[0]
|
||||
|
|
@ -0,0 +1,274 @@
|
|||
/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
|
||||
* BY HAND!!
|
||||
*
|
||||
* Generated by lcm-gen
|
||||
**/
|
||||
|
||||
#ifndef __unitree_lowlevel_UnitreeLowState_hpp__
|
||||
#define __unitree_lowlevel_UnitreeLowState_hpp__
|
||||
|
||||
#include <lcm/lcm_coretypes.h>
|
||||
|
||||
|
||||
namespace unitree_lowlevel
|
||||
{
|
||||
|
||||
class UnitreeLowState
|
||||
{
|
||||
public:
|
||||
double stamp;
|
||||
|
||||
float q[12];
|
||||
|
||||
float dq[12];
|
||||
|
||||
float tau_est[12];
|
||||
|
||||
float contact_state[4];
|
||||
|
||||
float accel[3];
|
||||
|
||||
float gyro[3];
|
||||
|
||||
float temperature;
|
||||
|
||||
float quaternion[4];
|
||||
|
||||
float rpy[3];
|
||||
|
||||
float gravity[3];
|
||||
|
||||
float gt_pos[3];
|
||||
|
||||
float gt_quat[4];
|
||||
|
||||
float gt_lin_vel[3];
|
||||
|
||||
float gt_ang_vel[3];
|
||||
|
||||
public:
|
||||
/**
|
||||
* Encode a message into binary form.
|
||||
*
|
||||
* @param buf The output buffer.
|
||||
* @param offset Encoding starts at this byte offset into @p buf.
|
||||
* @param maxlen Maximum number of bytes to write. This should generally be
|
||||
* equal to getEncodedSize().
|
||||
* @return The number of bytes encoded, or <0 on error.
|
||||
*/
|
||||
inline int encode(void *buf, int offset, int maxlen) const;
|
||||
|
||||
/**
|
||||
* Check how many bytes are required to encode this message.
|
||||
*/
|
||||
inline int getEncodedSize() const;
|
||||
|
||||
/**
|
||||
* Decode a message from binary form into this instance.
|
||||
*
|
||||
* @param buf The buffer containing the encoded message.
|
||||
* @param offset The byte offset into @p buf where the encoded message starts.
|
||||
* @param maxlen The maximum number of bytes to read while decoding.
|
||||
* @return The number of bytes decoded, or <0 if an error occurred.
|
||||
*/
|
||||
inline int decode(const void *buf, int offset, int maxlen);
|
||||
|
||||
/**
|
||||
* Retrieve the 64-bit fingerprint identifying the structure of the message.
|
||||
* Note that the fingerprint is the same for all instances of the same
|
||||
* message type, and is a fingerprint on the message type definition, not on
|
||||
* the message contents.
|
||||
*/
|
||||
inline static int64_t getHash();
|
||||
|
||||
/**
|
||||
* Returns "UnitreeLowState"
|
||||
*/
|
||||
inline static const char* getTypeName();
|
||||
|
||||
// LCM support functions. Users should not call these
|
||||
inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
|
||||
inline int _getEncodedSizeNoHash() const;
|
||||
inline int _decodeNoHash(const void *buf, int offset, int maxlen);
|
||||
inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
|
||||
};
|
||||
|
||||
int UnitreeLowState::encode(void *buf, int offset, int maxlen) const
|
||||
{
|
||||
int pos = 0, tlen;
|
||||
int64_t hash = getHash();
|
||||
|
||||
tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = this->_encodeNoHash(buf, offset + pos, maxlen - pos);
|
||||
if (tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
int UnitreeLowState::decode(const void *buf, int offset, int maxlen)
|
||||
{
|
||||
int pos = 0, thislen;
|
||||
|
||||
int64_t msg_hash;
|
||||
thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &msg_hash, 1);
|
||||
if (thislen < 0) return thislen; else pos += thislen;
|
||||
if (msg_hash != getHash()) return -1;
|
||||
|
||||
thislen = this->_decodeNoHash(buf, offset + pos, maxlen - pos);
|
||||
if (thislen < 0) return thislen; else pos += thislen;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
int UnitreeLowState::getEncodedSize() const
|
||||
{
|
||||
return 8 + _getEncodedSizeNoHash();
|
||||
}
|
||||
|
||||
int64_t UnitreeLowState::getHash()
|
||||
{
|
||||
static int64_t hash = static_cast<int64_t>(_computeHash(NULL));
|
||||
return hash;
|
||||
}
|
||||
|
||||
const char* UnitreeLowState::getTypeName()
|
||||
{
|
||||
return "UnitreeLowState";
|
||||
}
|
||||
|
||||
int UnitreeLowState::_encodeNoHash(void *buf, int offset, int maxlen) const
|
||||
{
|
||||
int pos = 0, tlen;
|
||||
|
||||
tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->stamp, 1);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->q[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->dq[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->tau_est[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->contact_state[0], 4);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->accel[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->gyro[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->temperature, 1);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->quaternion[0], 4);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->rpy[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->gravity[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->gt_pos[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->gt_quat[0], 4);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->gt_lin_vel[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_encode_array(buf, offset + pos, maxlen - pos, &this->gt_ang_vel[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
int UnitreeLowState::_decodeNoHash(const void *buf, int offset, int maxlen)
|
||||
{
|
||||
int pos = 0, tlen;
|
||||
|
||||
tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->stamp, 1);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->q[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->dq[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->tau_est[0], 12);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->contact_state[0], 4);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->accel[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->gyro[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->temperature, 1);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->quaternion[0], 4);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->rpy[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->gravity[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->gt_pos[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->gt_quat[0], 4);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->gt_lin_vel[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
tlen = __float_decode_array(buf, offset + pos, maxlen - pos, &this->gt_ang_vel[0], 3);
|
||||
if(tlen < 0) return tlen; else pos += tlen;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
int UnitreeLowState::_getEncodedSizeNoHash() const
|
||||
{
|
||||
int enc_size = 0;
|
||||
enc_size += __double_encoded_array_size(NULL, 1);
|
||||
enc_size += __float_encoded_array_size(NULL, 12);
|
||||
enc_size += __float_encoded_array_size(NULL, 12);
|
||||
enc_size += __float_encoded_array_size(NULL, 12);
|
||||
enc_size += __float_encoded_array_size(NULL, 4);
|
||||
enc_size += __float_encoded_array_size(NULL, 3);
|
||||
enc_size += __float_encoded_array_size(NULL, 3);
|
||||
enc_size += __float_encoded_array_size(NULL, 1);
|
||||
enc_size += __float_encoded_array_size(NULL, 4);
|
||||
enc_size += __float_encoded_array_size(NULL, 3);
|
||||
enc_size += __float_encoded_array_size(NULL, 3);
|
||||
enc_size += __float_encoded_array_size(NULL, 3);
|
||||
enc_size += __float_encoded_array_size(NULL, 4);
|
||||
enc_size += __float_encoded_array_size(NULL, 3);
|
||||
enc_size += __float_encoded_array_size(NULL, 3);
|
||||
return enc_size;
|
||||
}
|
||||
|
||||
uint64_t UnitreeLowState::_computeHash(const __lcm_hash_ptr *)
|
||||
{
|
||||
uint64_t hash = 0x63fba9ef911d1605LL;
|
||||
return (hash<<1) + ((hash>>63)&1);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,163 @@
|
|||
"""LCM type definitions
|
||||
This file automatically generated by lcm.
|
||||
DO NOT MODIFY BY HAND!!!!
|
||||
"""
|
||||
|
||||
try:
|
||||
import cStringIO.StringIO as BytesIO
|
||||
except ImportError:
|
||||
from io import BytesIO
|
||||
import struct
|
||||
|
||||
|
||||
class UnitreeLowState:
|
||||
__slots__ = [
|
||||
"stamp",
|
||||
"q",
|
||||
"dq",
|
||||
"tau_est",
|
||||
"contact_state",
|
||||
"accel",
|
||||
"gyro",
|
||||
"temperature",
|
||||
"quaternion",
|
||||
"rpy",
|
||||
"gravity",
|
||||
"gt_pos",
|
||||
"gt_quat",
|
||||
"gt_lin_vel",
|
||||
"gt_ang_vel",
|
||||
]
|
||||
|
||||
__typenames__ = [
|
||||
"double",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
"float",
|
||||
]
|
||||
|
||||
__dimensions__ = [
|
||||
None,
|
||||
[12],
|
||||
[12],
|
||||
[12],
|
||||
[4],
|
||||
[3],
|
||||
[3],
|
||||
None,
|
||||
[4],
|
||||
[3],
|
||||
[3],
|
||||
[3],
|
||||
[4],
|
||||
[3],
|
||||
[3],
|
||||
]
|
||||
|
||||
def __init__(self):
|
||||
self.stamp = 0.0
|
||||
self.q = [0.0 for dim0 in range(12)]
|
||||
self.dq = [0.0 for dim0 in range(12)]
|
||||
self.tau_est = [0.0 for dim0 in range(12)]
|
||||
self.contact_state = [0.0 for dim0 in range(4)]
|
||||
self.accel = [0.0 for dim0 in range(3)]
|
||||
self.gyro = [0.0 for dim0 in range(3)]
|
||||
self.temperature = 0.0
|
||||
self.quaternion = [0.0 for dim0 in range(4)]
|
||||
self.rpy = [0.0 for dim0 in range(3)]
|
||||
self.gravity = [0.0 for dim0 in range(3)]
|
||||
self.gt_pos = [0.0 for dim0 in range(3)]
|
||||
self.gt_quat = [0.0 for dim0 in range(4)]
|
||||
self.gt_lin_vel = [0.0 for dim0 in range(3)]
|
||||
self.gt_ang_vel = [0.0 for dim0 in range(3)]
|
||||
|
||||
def encode(self):
|
||||
buf = BytesIO()
|
||||
buf.write(UnitreeLowState._get_packed_fingerprint())
|
||||
self._encode_one(buf)
|
||||
return buf.getvalue()
|
||||
|
||||
def _encode_one(self, buf):
|
||||
buf.write(struct.pack(">d", self.stamp))
|
||||
buf.write(struct.pack(">12f", *self.q[:12]))
|
||||
buf.write(struct.pack(">12f", *self.dq[:12]))
|
||||
buf.write(struct.pack(">12f", *self.tau_est[:12]))
|
||||
buf.write(struct.pack(">4f", *self.contact_state[:4]))
|
||||
buf.write(struct.pack(">3f", *self.accel[:3]))
|
||||
buf.write(struct.pack(">3f", *self.gyro[:3]))
|
||||
buf.write(struct.pack(">f", self.temperature))
|
||||
buf.write(struct.pack(">4f", *self.quaternion[:4]))
|
||||
buf.write(struct.pack(">3f", *self.rpy[:3]))
|
||||
buf.write(struct.pack(">3f", *self.gravity[:3]))
|
||||
buf.write(struct.pack(">3f", *self.gt_pos[:3]))
|
||||
buf.write(struct.pack(">4f", *self.gt_quat[:4]))
|
||||
buf.write(struct.pack(">3f", *self.gt_lin_vel[:3]))
|
||||
buf.write(struct.pack(">3f", *self.gt_ang_vel[:3]))
|
||||
|
||||
def decode(data):
|
||||
if hasattr(data, "read"):
|
||||
buf = data
|
||||
else:
|
||||
buf = BytesIO(data)
|
||||
if buf.read(8) != UnitreeLowState._get_packed_fingerprint():
|
||||
raise ValueError("Decode error")
|
||||
return UnitreeLowState._decode_one(buf)
|
||||
|
||||
decode = staticmethod(decode)
|
||||
|
||||
def _decode_one(buf):
|
||||
self = UnitreeLowState()
|
||||
self.stamp = struct.unpack(">d", buf.read(8))[0]
|
||||
self.q = struct.unpack(">12f", buf.read(48))
|
||||
self.dq = struct.unpack(">12f", buf.read(48))
|
||||
self.tau_est = struct.unpack(">12f", buf.read(48))
|
||||
self.contact_state = struct.unpack(">4f", buf.read(16))
|
||||
self.accel = struct.unpack(">3f", buf.read(12))
|
||||
self.gyro = struct.unpack(">3f", buf.read(12))
|
||||
self.temperature = struct.unpack(">f", buf.read(4))[0]
|
||||
self.quaternion = struct.unpack(">4f", buf.read(16))
|
||||
self.rpy = struct.unpack(">3f", buf.read(12))
|
||||
self.gravity = struct.unpack(">3f", buf.read(12))
|
||||
self.gt_pos = struct.unpack(">3f", buf.read(12))
|
||||
self.gt_quat = struct.unpack(">4f", buf.read(16))
|
||||
self.gt_lin_vel = struct.unpack(">3f", buf.read(12))
|
||||
self.gt_ang_vel = struct.unpack(">3f", buf.read(12))
|
||||
return self
|
||||
|
||||
_decode_one = staticmethod(_decode_one)
|
||||
|
||||
def _get_hash_recursive(parents):
|
||||
if UnitreeLowState in parents:
|
||||
return 0
|
||||
tmphash = (0x63FBA9EF911D1605) & 0xFFFFFFFFFFFFFFFF
|
||||
tmphash = (
|
||||
((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)
|
||||
) & 0xFFFFFFFFFFFFFFFF
|
||||
return tmphash
|
||||
|
||||
_get_hash_recursive = staticmethod(_get_hash_recursive)
|
||||
_packed_fingerprint = None
|
||||
|
||||
def _get_packed_fingerprint():
|
||||
if UnitreeLowState._packed_fingerprint is None:
|
||||
UnitreeLowState._packed_fingerprint = struct.pack(
|
||||
">Q", UnitreeLowState._get_hash_recursive([])
|
||||
)
|
||||
return UnitreeLowState._packed_fingerprint
|
||||
|
||||
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
|
||||
|
||||
def get_hash(self):
|
||||
"""Get the LCM hash of the struct"""
|
||||
return struct.unpack(">Q", UnitreeLowState._get_packed_fingerprint())[0]
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
"""LCM package __init__.py file
|
||||
This file automatically generated by lcm-gen.
|
||||
DO NOT MODIFY BY HAND!!!!
|
||||
"""
|
||||
|
||||
from .UnitreeLowCommand import UnitreeLowCommand
|
||||
from .UnitreeLowState import UnitreeLowState
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
environment:
|
||||
simulation_dt: 0.01
|
||||
rendering_dt: 0.02
|
||||
prim_path: "/World/Env"
|
||||
buildin: true
|
||||
usd_path: "/Isaac/Environments/Grid/default_environment.usd" # /Isaac/Environments/Simple_Warehouse/full_warehouse.usd
|
||||
synchronous_mode: false # if true, the simulator will wait for the control commands from the LCM channel for 0.1 seconds
|
||||
|
||||
robots:
|
||||
- prim_path: "/World/Env/GO2"
|
||||
usd_path: ""
|
||||
name: "go2"
|
||||
usd_file: "assets/usd/go2.usd"
|
||||
position: [0.0, 0.0, 0.8]
|
||||
orientation: [0.0, 0.0, 0.0, 1.0]
|
||||
|
||||
cameras:
|
||||
- prim_path: "/World/Env/GO2/imu_link"
|
||||
name: "test_camera"
|
||||
type: ["rgb", "distance_to_camera"]
|
||||
resolution: [1280, 720]
|
||||
interstice_parameters:
|
||||
K: [1000, 1000, 500, 500]
|
||||
D: [0.1, 0.2, 0.1, 0.0]
|
||||
translation: [0.0, 0.0, 0.0] # x, y, z
|
||||
orientation: [0.0, 0.0, 0.0, 1.0] # qx, qy, qz, qw
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
import numpy as np
|
||||
import omni.replicator.core as rep
|
||||
from omni.isaac.core.prims import BaseSensor
|
||||
from omni.isaac.core.prims.xform_prim import XFormPrim
|
||||
from pxr import UsdGeom
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
|
||||
class AnnotatorManager:
|
||||
def __init__(self, world):
|
||||
self.annotators = {}
|
||||
self.render_products = {}
|
||||
self.cameras = {}
|
||||
self.annotator_types = [
|
||||
"rgb",
|
||||
"distance_to_camera",
|
||||
"pointcloud",
|
||||
"semantic_segmentation",
|
||||
]
|
||||
self.camera_prims = {}
|
||||
self.camera_info = {}
|
||||
self.world = world
|
||||
|
||||
def registerCamera(
|
||||
self, parent_prim, name, translation, orientation, resolution=(1024, 600)
|
||||
):
|
||||
"""
|
||||
Register a camera in the scene. The camera will be attached to the parent_prim.
|
||||
parent_prim: The prim path of the parent prim. The camera will be attached to this prim.
|
||||
name: The name of the camera.
|
||||
translation: The translation of the camera relative to the parent prim in the form of (x, y, z).
|
||||
orientation: The orientation of the camera relative to the parent prim in the form of (x, y, z, w).
|
||||
resolution: The resolution of the camera in the form of (width, height).
|
||||
"""
|
||||
qx, qy, qz, qw = tuple(orientation)
|
||||
xformprim = XFormPrim(
|
||||
prim_path=parent_prim + "/" + name,
|
||||
name=name,
|
||||
translation=tuple(translation),
|
||||
orientation=(qw, qx, qy, qz), # Omniverse core convention (w, x, y, z)
|
||||
)
|
||||
self.camera_prims[name] = xformprim
|
||||
self.cameras[name] = self.world.stage.DefinePrim(
|
||||
xformprim.prim_path + "/" + name, "Camera"
|
||||
)
|
||||
UsdGeom.Xformable(self.cameras[name]).AddTranslateOp().Set((0.0, 0.0, 0.0))
|
||||
# Rotate around x for 180 degrees to make the convention consistent with opencv/ROS
|
||||
UsdGeom.Xformable(self.cameras[name]).AddRotateXYZOp().Set((180.0, 0.0, 0.0))
|
||||
self.camera_info[name] = {
|
||||
"translation": translation,
|
||||
"orientation": orientation,
|
||||
"resolution": resolution,
|
||||
}
|
||||
self.render_products[name] = rep.create.render_product(
|
||||
str(self.cameras[name].GetPrimPath()), resolution
|
||||
)
|
||||
|
||||
def registerAnnotator(self, type, camera_name):
|
||||
assert (
|
||||
type in self.annotator_types
|
||||
), "Requested replicator type not available. Choose from: " + str(
|
||||
self.annotator_types
|
||||
)
|
||||
assert (
|
||||
camera_name in self.cameras.keys()
|
||||
), "The requested camera is not registered. Please register the camera first."
|
||||
self.annotators[camera_name + ":" + type] = rep.AnnotatorRegistry.get_annotator(
|
||||
type
|
||||
)
|
||||
self.annotators[camera_name + ":" + type].attach(
|
||||
[self.render_products[camera_name]]
|
||||
)
|
||||
|
||||
def getData(self, annotator_name):
|
||||
assert (
|
||||
annotator_name in self.annotators.keys()
|
||||
), "The requested annotator is not registered. Please register the annotator first."
|
||||
return self.annotators[annotator_name].get_data()
|
||||
|
||||
def getStreams(self):
|
||||
return list(self.annotators.keys())
|
||||
|
||||
def setFocalLength(self, name, value):
|
||||
assert (
|
||||
name in self.cameras.keys()
|
||||
), "The requested camera is not registered. Please register the camera first."
|
||||
self.cameras[name].GetAttribute("focalLength").Set(value)
|
||||
|
||||
def setClippingRange(self, name, min=0.2, max=1000000.0):
|
||||
assert (
|
||||
name in self.cameras.keys()
|
||||
), "The requested camera is not registered. Please register the camera first."
|
||||
self.cameras[name].GetAttribute("clippingRange").Set((min, max))
|
||||
|
||||
# TODO: implement these functions
|
||||
def getCameraIntrinsics(self, name):
|
||||
return None
|
||||
|
||||
def getCameraExtrinsics(self, name):
|
||||
return None
|
||||
|
||||
def getCameraPose(self, name):
|
||||
"""
|
||||
Get the camera pose in the world frame
|
||||
@param name: (str) The name of the camera
|
||||
@return: (t, q) The camera pose (np.array([x, y, z]), np.array([qw, qx, qy, qz]))
|
||||
"""
|
||||
t, q = self.camera_prims[name].get_world_pose()
|
||||
return t, q
|
||||
|
||||
def setCameraPose(self, name, t, q):
|
||||
"""
|
||||
Get the camera pose in the world frame
|
||||
@param name: (str) The name of the camera
|
||||
@param t: (mp.array([x, y, z])) The translation of the camera in the world frame
|
||||
@param q: (mp.array([qw, qx, qy, qz])) The orientation of the camera in the world frame
|
||||
@return: None
|
||||
"""
|
||||
self.camera_prims[name].set_local_pose(t, q)
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
import time
|
||||
from copy import deepcopy
|
||||
import matplotlib.pyplot as plt
|
||||
import mujoco
|
||||
import mujoco.viewer
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
from pinocchio.robot_wrapper import RobotWrapper
|
||||
from Go2Py import ASSETS_PATH
|
||||
import os
|
||||
|
||||
class Go2Sim:
|
||||
def __init__(self, render=True, dt=0.002):
|
||||
|
||||
self.model = mujoco.MjModel.from_xml_path(
|
||||
os.path.join(ASSETS_PATH, 'mujoco/go2.xml')
|
||||
)
|
||||
self.data = mujoco.MjData(self.model)
|
||||
self.dt = dt
|
||||
_render_dt = 1/60
|
||||
self.render_ds_ratio = max(1, _render_dt//dt)
|
||||
|
||||
if render:
|
||||
self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
|
||||
self.render = True
|
||||
self.viewer.cam.distance = 3.0
|
||||
self.viewer.cam.azimuth = 90
|
||||
self.viewer.cam.elevation = -45
|
||||
self.viewer.cam.lookat[:] = np.array([0.0, -0.25, 0.824])
|
||||
else:
|
||||
self.render = False
|
||||
|
||||
self.model.opt.gravity[2] = -9.81
|
||||
self.model.opt.timestep = dt
|
||||
self.renderer = None
|
||||
self.render = render
|
||||
self.step_counter = 0
|
||||
|
||||
def reset(self):
|
||||
self.q_nominal = np.array(
|
||||
12*[0.]
|
||||
)
|
||||
for i in range(12):
|
||||
self.data.qpos[i] = self.q_nominal[i]
|
||||
|
||||
self.data.qpos[7] = 0.0
|
||||
self.data.qpos[8] = 0.0
|
||||
|
||||
def step(self, tau):
|
||||
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()
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
import select
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
from Go2Py.sim.isaacsim.lcm_types.unitree_lowlevel import UnitreeLowCommand, UnitreeLowState
|
||||
|
||||
|
||||
def load_config(file_path):
|
||||
with open(file_path, "r") as file:
|
||||
config = yaml.safe_load(file)
|
||||
return config
|
||||
|
||||
class NumpyMemMapDataPipe:
|
||||
def __init__(self, channel_name, force=False, dtype="uint8", shape=(640, 480, 3)):
|
||||
self.channel_name = channel_name
|
||||
self.force = force
|
||||
self.dtype = dtype
|
||||
self.shape = shape
|
||||
if force:
|
||||
self.shm = np.memmap(
|
||||
"/dev/shm/" + channel_name, dtype=self.dtype, mode="w+", shape=shape
|
||||
)
|
||||
else:
|
||||
self.shm = np.memmap(
|
||||
"/dev/shm/" + channel_name, dtype=self.dtype, mode="r+", shape=shape
|
||||
)
|
||||
|
||||
def write(self, data, match_length=True):
|
||||
if match_length:
|
||||
self.shm[: data.shape[0], ...] = data
|
||||
else:
|
||||
assert (
|
||||
data.shape == self.shape
|
||||
), "The data and the shape of the shared memory must match"
|
||||
self.shm[:] = data
|
||||
|
||||
def read(self):
|
||||
return self.shm.copy()
|
||||
|
||||
|
||||
class simulationManager:
|
||||
def __init__(self, robot, lcm_server, default_cmd, physics_dt, lcm_timeout=0.01):
|
||||
self.robot = robot
|
||||
self.lcm_server = lcm_server
|
||||
self.missed_ticks = 0
|
||||
self.default_cmd = default_cmd
|
||||
self.robot.initialize()
|
||||
self.reset_required = True
|
||||
self.lcm_timeout = lcm_timeout
|
||||
self.physics_dt = physics_dt
|
||||
self.robot.setCommands(self.default_cmd)
|
||||
|
||||
def step(self, timestamp):
|
||||
# Read the robot's state and send it to the LCM client
|
||||
state = self.robot.readStates()
|
||||
state.stamp = timestamp
|
||||
self.lcm_server.sendStates(state)
|
||||
# Wait for a response from the LCM client timeout=0.1 second
|
||||
lcm_cmd = self.lcm_server.getCommands(timeout=self.lcm_timeout)
|
||||
if lcm_cmd is not None:
|
||||
self.missed_ticks = 0
|
||||
# Reset the robot if the communication has been off for too long
|
||||
if self.reset_required:
|
||||
self.robot.initialize()
|
||||
self.robot.setCommands(self.default_cmd)
|
||||
self.reset_required = False
|
||||
self.robot.setCommands(lcm_cmd)
|
||||
else:
|
||||
self.missed_ticks += 1
|
||||
if self.missed_ticks > 0.2 / (
|
||||
self.lcm_timeout + self.physics_dt
|
||||
): # stopped for more than a second?
|
||||
self.reset_required = True
|
||||
self.robot.setCommands(self.default_cmd)
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"import numpy as np"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"env = Go2Sim()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/plain": [
|
||||
"False"
|
||||
]
|
||||
},
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"env.step_counter+=1\n",
|
||||
"env.render and (env.step_counter%env.render_ds_ratio)==0\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 10,
|
||||
"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)"
|
||||
]
|
||||
}
|
||||
],
|
||||
"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.9.18"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
|
|
@ -0,0 +1,180 @@
|
|||
#!/bin/bash
|
||||
|
||||
# Note: This shell script is inspired by the Orbit simulator: https://isaac-B1Py.github.io/
|
||||
#==
|
||||
# Configurations
|
||||
#==
|
||||
|
||||
# Exits if error occurs
|
||||
set -e
|
||||
|
||||
# Set tab-spaces
|
||||
tabs 4
|
||||
|
||||
# get source directory
|
||||
export GO2PY_PATH="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
|
||||
|
||||
#==
|
||||
# Helper functions
|
||||
#==
|
||||
|
||||
# extract the python from isaacsim
|
||||
extract_python_exe() {
|
||||
# Check if IsaacSim directory manually specified
|
||||
# Note: for manually build isaacsim, this: _build/linux-x86_64/release
|
||||
if [ ! -z ${ISAACSIM_PATH} ];
|
||||
then
|
||||
# Use local build
|
||||
build_path=${ISAACSIM_PATH}
|
||||
else
|
||||
# Use TeamCity build
|
||||
build_path=${GO2PY_PATH}/_isaac_sim
|
||||
fi
|
||||
# check if using conda
|
||||
if ! [[ -z "${CONDA_PREFIX}" ]]; then
|
||||
# use conda python
|
||||
local python_exe=${CONDA_PREFIX}/bin/python
|
||||
else
|
||||
# use python from kit
|
||||
local python_exe=${build_path}/python.sh
|
||||
fi
|
||||
# check if there is a python path available
|
||||
if [ ! -f "${python_exe}" ]; then
|
||||
echo "[ERROR] No python executable found at path: ${build_path}" >&2
|
||||
exit 1
|
||||
fi
|
||||
# return the result
|
||||
echo ${python_exe}
|
||||
}
|
||||
|
||||
# check if input directory is a python extension and install the module
|
||||
install_B1Py_extension() {
|
||||
# retrieve the python executable
|
||||
python_exe=$(extract_python_exe)
|
||||
# if the directory contains setup.py then install the python module
|
||||
if [ -f "$1/setup.py" ];
|
||||
then
|
||||
echo -e "\t module: $1"
|
||||
${python_exe} -m pip install --editable $1
|
||||
fi
|
||||
}
|
||||
|
||||
# update the vscode settings from template and isaac sim settings
|
||||
update_vscode_settings() {
|
||||
echo "[INFO] Setting up vscode settings..."
|
||||
# retrieve the python executable
|
||||
python_exe=$(extract_python_exe)
|
||||
# run the setup script
|
||||
${python_exe} ${GO2PY_PATH}/.vscode/tools/setup_vscode.py
|
||||
}
|
||||
|
||||
# print the usage description
|
||||
print_help () {
|
||||
echo -e "\nusage: $(basename "$0") [-h] [-i] [-e] [-f] [-p] [-s] [-v] [-d] [-c] -- Utility to manage extensions in Orbit."
|
||||
echo -e "\noptional arguments:"
|
||||
echo -e "\t-h, --help Display the help content."
|
||||
echo -e "\t-i, --install Install the extensions inside Isaac B1Py."
|
||||
echo -e "\t-f, --format Run pre-commit to format the code and check lints."
|
||||
echo -e "\t-p, --python Run the python executable (python.sh) provided by Isaac Sim."
|
||||
# echo -e "\t-s, --sim Run the simulator executable (isaac-sim.sh) provided by Isaac Sim."
|
||||
echo -e "\t-s, --sim Run the B1Py Isaac Sim simulation node."
|
||||
echo -e "\t-v, --vscode Generate the VSCode settings file from template."
|
||||
echo -e "\n" >&2
|
||||
}
|
||||
|
||||
#==
|
||||
# Main
|
||||
#==
|
||||
|
||||
# check argument provided
|
||||
if [ -z "$*" ]; then
|
||||
echo "[Error] No arguments provided." >&2;
|
||||
print_help
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# pass the arguments
|
||||
while [[ $# -gt 0 ]]; do
|
||||
# read the key
|
||||
case "$1" in
|
||||
-i|--install)
|
||||
# install the python packages for supported reinforcement learning frameworks
|
||||
echo "[INFO] Installing extra requirements such as learning frameworks..."
|
||||
python_exe=$(extract_python_exe)
|
||||
# install the rl-frameworks specified
|
||||
${python_exe} -m pip install -e ${GO2PY_PATH}
|
||||
shift # past argument
|
||||
;;
|
||||
-f|--format)
|
||||
# reset the python path to avoid conflicts with pre-commit
|
||||
# this is needed because the pre-commit hooks are installed in a separate virtual environment
|
||||
# and it uses the system python to run the hooks
|
||||
if [ -n "${CONDA_DEFAULT_ENV}" ]; then
|
||||
cache_pythonpath=${PYTHONPATH}
|
||||
export PYTHONPATH=""
|
||||
fi
|
||||
# run the formatter over the repository
|
||||
# check if pre-commit is installed
|
||||
if ! command -v pre-commit &>/dev/null; then
|
||||
echo "[INFO] Installing pre-commit..."
|
||||
pip install pre-commit
|
||||
fi
|
||||
# always execute inside the Orbit directory
|
||||
echo "[INFO] Formatting the repository..."
|
||||
cd ${GO2PY_PATH}
|
||||
pre-commit run --all-files
|
||||
cd - > /dev/null
|
||||
# set the python path back to the original value
|
||||
if [ -n "${CONDA_DEFAULT_ENV}" ]; then
|
||||
export PYTHONPATH=${cache_pythonpath}
|
||||
fi
|
||||
shift # past argument
|
||||
# exit neatly
|
||||
break
|
||||
;;
|
||||
-p|--python)
|
||||
# run the python provided by isaacsim
|
||||
python_exe=$(extract_python_exe)
|
||||
echo "[INFO] Using python from: ${python_exe}"
|
||||
shift # past argument
|
||||
${python_exe} $@
|
||||
# exit neatly
|
||||
break
|
||||
;;
|
||||
# -s|--sim)
|
||||
# # run the simulator exe provided by isaacsim
|
||||
# isaacsim_exe=$(extract_isaacsim_exe)
|
||||
# echo "[INFO] Running isaac-sim from: ${isaacsim_exe}"
|
||||
# shift # past argument
|
||||
# ${isaacsim_exe} --ext-folder ${GO2PY_PATH}/source/extensions $@
|
||||
# # exit neatly
|
||||
# break
|
||||
# ;;
|
||||
|
||||
-s|--sim)
|
||||
# run the simulator exe provided by isaacsim
|
||||
python_exe=$(extract_python_exe)
|
||||
# install the rl-frameworks specified
|
||||
${python_exe} ${GO2PY_PATH}/B1Py/sim/isaac/isaacsim_node.py
|
||||
# exit neatly
|
||||
break
|
||||
;;
|
||||
|
||||
-v|--vscode)
|
||||
# update the vscode settings
|
||||
update_vscode_settings
|
||||
shift # past argument
|
||||
# exit neatly
|
||||
break
|
||||
;;
|
||||
-h|--help)
|
||||
print_help
|
||||
exit 1
|
||||
;;
|
||||
*) # unknown option
|
||||
echo "[Error] Invalid argument provided: $1"
|
||||
print_help
|
||||
exit 1
|
||||
;;
|
||||
esac
|
||||
done
|
||||
Loading…
Reference in New Issue