Skip to content

Commit

Permalink
feat: add go1
Browse files Browse the repository at this point in the history
However, It doesn't have any controller yet.
You can add walk these ways controller here
  • Loading branch information
mqjinwon committed Apr 17, 2024
1 parent 5870a65 commit 23032cf
Show file tree
Hide file tree
Showing 4 changed files with 136 additions and 5 deletions.
8 changes: 8 additions & 0 deletions exts/stride.simulator/config/go1_cfg.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@

sensor:
imu:
update_frequency: 250

lidar:
update_frequency: 25
prim_path: /World/go1/lidar/lidar_PhysX
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@
AccelStamped,
)


# set environment variable to use ROS2
os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp"
os.environ["ROS_DOMAIN_ID"] = "15"
Expand Down
18 changes: 14 additions & 4 deletions exts/stride.simulator/stride/simulator/extension.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,9 @@
from omni import ui

from stride.simulator.interfaces.stride_sim_interface import StrideInterface
from stride.simulator.vehicles.quadrupedrobot.anymalc import AnymalC, AnymalCConfig

# from stride.simulator.vehicles.quadrupedrobot.anymalc import AnymalC, AnymalCConfig
from stride.simulator.vehicles.quadrupedrobot.go1 import Go1Config, Go1
from stride.simulator.params import SIMULATION_ENVIRONMENTS

import asyncio
Expand Down Expand Up @@ -60,14 +62,22 @@ def on_environment():
def on_simulation():
async def respawn():

self._anymal_config = AnymalCConfig()
self._go1_config = Go1Config()

self._anymal = AnymalC(
self._anymal = Go1(
id=0,
init_pos=[0.0, 0.0, 0.7],
init_orientation=[0.0, 0.0, 0.0, 1.0],
config=self._anymal_config,
config=self._go1_config,
)
# self._anymal_config = AnymalCConfig()

# self._anymal = AnymalC(
# id=0,
# init_pos=[0.0, 0.0, 0.7],
# init_orientation=[0.0, 0.0, 0.0, 1.0],
# config=self._anymal_config,
# )

self._current_tasks = self._stride_sim.world.get_current_tasks()
await self._stride_sim.world.reset_async()
Expand Down
114 changes: 114 additions & 0 deletions exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/go1.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
from stride.simulator.vehicles.quadrupedrobot.quadrupedrobot import (
QuadrupedRobot,
QuadrupedRobotConfig,
)

# Mavlink interface
# from stride.simulator.logic.backends.mavlink_backend import MavlinkBackend

# Get the location of the asset
# from stride.simulator.backends import LoggerBackend
from stride.simulator.backends import ROS2Backend
from stride.simulator.params import ROBOTS
from stride.simulator.vehicles.sensors.imu import Imu

# from stride.simulator.vehicles.sensors.lidar import Lidar

from stride.simulator.vehicles.controllers.anymal_controller import AnyamlController

import yaml
import os


class Go1Config(QuadrupedRobotConfig):
"""
Go1 configuration class
"""

def __init__(self):

super().__init__()

self.vehicle_name = "go1"

# Get the path to the "" directory
stridesim_dir = os.path.abspath(__file__)
for _ in range(5):
stridesim_dir = os.path.dirname(stridesim_dir)

# Stage prefix of the vehicle when spawning in the world
self.stage_prefix = "/World/Go1"

# The USD file that describes the visual aspect of the vehicle
self.usd_file = ROBOTS["go1"]

# read config file
with open(
stridesim_dir + "/config/go1_cfg.yaml", "r", encoding="utf-8"
) as file:
self.config = yaml.safe_load(file)

# The default sensors for a Go1
self.sensors = [
Imu(self.config["sensor"]["imu"]),
# Lidar(self.config["sensor"]["lidar"]),
] # pylint: disable=use-list-literal FIXME

# The backends for actually sending commands to the vehicle.
# It can also be a ROS2 backend or your own custom Backend implementation!
self.backends = [
ROS2Backend(self.vehicle_name)
] # pylint: disable=use-list-literal


class Go1(QuadrupedRobot):
"""Go1 class - It is a child class of QuadrupedRobot class to implement a Go1 robot in the simulator."""

def __init__(self, id: int, init_pos, init_orientation, config=Go1Config()):

if init_pos is None:
init_pos = [0.0, 0.0, 0.5]
if init_orientation is None:
init_orientation = [0.0, 0.0, 0.0, 1.0]

super().__init__(
config.stage_prefix,
config.usd_file,
id,
init_pos,
init_orientation,
config=config,
)

self.controller = AnyamlController()

def update_sensors(self, dt: float):
"""Callback that is called at every physics steps and will call the sensor.update method to generate new
sensor data. For each data that the sensor generates, the backend.update_sensor method will also be called for
every backend. For example, if new data is generated for an IMU and we have a MavlinkBackend, then the
update_sensor method will be called for that backend so that this data can latter be sent thorugh mavlink.
Args:
dt (float): The time elapsed between the previous and current function calls (s).
"""

# Call the update method for the sensor to update its values internally (if applicable)
for sensor in self._sensors:
try:
sensor_data = sensor.update(self._state, dt)
except Exception as e: # pylint: disable=broad-except
print(f"Error updating sensor: {e}")
continue

if sensor_data is not None:
for backend in self._backends:
backend.update_sensor(sensor.sensor_type, sensor_data)

def initialize(self, physics_sim_view=None) -> None:
"""[summary]
initialize the dc interface, set up drive mode
"""
super().initialize(physics_sim_view=physics_sim_view)
self.get_articulation_controller().set_effort_modes("force")
self.get_articulation_controller().switch_control_mode("effort")

0 comments on commit 23032cf

Please sign in to comment.