Refactors simulation an raspberry pi project

simulation/.gitignore

@@ -0,0 +1,5 @@
+/venv/
+/.idea/
+*.egg-info
+*.DS_Store
+/dist

simulation/GymEnvs/env_randomizer_base.py

@@ -0,0 +1,27 @@
+"""Abstract base class for environment randomizer.
+Source: https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py
+"""
+
+
+import abc
+
+
+class EnvRandomizerBase(object):
+  """Abstract base class for environment randomizer.
+
+  An EnvRandomizer is called in environment.reset(). It will
+  randomize physical parameters of the objects in the simulation.
+  The physical parameters will be fixed for that episode and be
+  randomized again in the next environment.reset().
+  """
+
+  __metaclass__ = abc.ABCMeta
+
+  @abc.abstractmethod
+  def randomize_env(self, env):
+    """Randomize the simulated_objects in the environment.
+
+    Args:
+      env: The environment to be randomized.
+    """
+    pass

simulation/GymEnvs/heightfield.py

@@ -0,0 +1,138 @@
+"""
+CODE BASED ON EXAMPLE FROM:
+@misc{coumans2017pybullet,
+  title={Pybullet, a python module for physics simulation in robotics, games and machine learning},
+  author={Coumans, Erwin and Bai, Yunfei},
+  url={www.pybullet.org},
+  year={2017},
+}
+
+Example: heightfield.py
+https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/heightfield.py
+"""
+
+import pybullet as p
+import pybullet_data as pd
+
+textureId = -1
+
+useProgrammatic = 0
+useTerrainFromPNG = 1
+useDeepLocoCSV = 2
+updateHeightfield = False
+
+heightfieldSource = useProgrammatic
+numHeightfieldRows = 256
+numHeightfieldColumns = 256
+import random
+random.seed(10)
+
+
+class HeightField():
+    def __init__(self):
+        self.hf_id = 0
+        self.terrainShape = 0
+        self.heightfieldData = [0] * numHeightfieldRows * numHeightfieldColumns
+
+    def _generate_field(self, env, heightPerturbationRange=0.08):
+
+        env.pybullet_client.setAdditionalSearchPath(pd.getDataPath())
+
+        env.pybullet_client.configureDebugVisualizer(
+            env.pybullet_client.COV_ENABLE_RENDERING, 0)
+        heightPerturbationRange = heightPerturbationRange
+        if heightfieldSource == useProgrammatic:
+            for j in range(int(numHeightfieldColumns / 2)):
+                for i in range(int(numHeightfieldRows / 2)):
+                    height = random.uniform(0, heightPerturbationRange)
+                    self.heightfieldData[2 * i +
+                                         2 * j * numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + 1 +
+                                         2 * j * numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + (2 * j + 1) *
+                                         numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + 1 + (2 * j + 1) *
+                                         numHeightfieldRows] = height
+
+            terrainShape = env.pybullet_client.createCollisionShape(
+                shapeType=env.pybullet_client.GEOM_HEIGHTFIELD,
+                meshScale=[.07, .07, 1.6],
+                heightfieldTextureScaling=(numHeightfieldRows - 1) / 2,
+                heightfieldData=self.heightfieldData,
+                numHeightfieldRows=numHeightfieldRows,
+                numHeightfieldColumns=numHeightfieldColumns)
+            terrain = env.pybullet_client.createMultiBody(0, terrainShape)
+            env.pybullet_client.resetBasePositionAndOrientation(
+                terrain, [0, 0, 0.0], [0, 0, 0, 1])
+            env.pybullet_client.changeDynamics(terrain,
+                                               -1,
+                                               lateralFriction=1.0)
+
+        if heightfieldSource == useDeepLocoCSV:
+            terrainShape = env.pybullet_client.createCollisionShape(
+                shapeType=env.pybullet_client.GEOM_HEIGHTFIELD,
+                meshScale=[.5, .5, 2.5],
+                fileName="heightmaps/ground0.txt",
+                heightfieldTextureScaling=128)
+            terrain = env.pybullet_client.createMultiBody(0, terrainShape)
+            env.pybullet_client.resetBasePositionAndOrientation(
+                terrain, [0, 0, 0], [0, 0, 0, 1])
+            env.pybullet_client.changeDynamics(terrain,
+                                               -1,
+                                               lateralFriction=1.0)
+
+        if heightfieldSource == useTerrainFromPNG:
+            terrainShape = env.pybullet_client.createCollisionShape(
+                shapeType=env.pybullet_client.GEOM_HEIGHTFIELD,
+                meshScale=[.05, .05, 1.8],
+                fileName="heightmaps/wm_height_out.png")
+            textureId = env.pybullet_client.loadTexture(
+                "heightmaps/gimp_overlay_out.png")
+            terrain = env.pybullet_client.createMultiBody(0, terrainShape)
+            env.pybullet_client.changeVisualShape(terrain,
+                                                  -1,
+                                                  textureUniqueId=textureId)
+            env.pybullet_client.resetBasePositionAndOrientation(
+                terrain, [0, 0, 0.1], [0, 0, 0, 1])
+            env.pybullet_client.changeDynamics(terrain,
+                                               -1,
+                                               lateralFriction=1.0)
+
+        self.hf_id = terrainShape
+        self.terrainShape = terrainShape
+        print("TERRAIN SHAPE: {}".format(terrainShape))
+
+        env.pybullet_client.changeVisualShape(terrain,
+                                              -1,
+                                              rgbaColor=[1, 1, 1, 1])
+
+        env.pybullet_client.configureDebugVisualizer(
+            env.pybullet_client.COV_ENABLE_RENDERING, 1)
+
+    def UpdateHeightField(self, heightPerturbationRange=0.08):
+        if heightfieldSource == useProgrammatic:
+            for j in range(int(numHeightfieldColumns / 2)):
+                for i in range(int(numHeightfieldRows / 2)):
+                    height = random.uniform(
+                        0, heightPerturbationRange)  # +math.sin(time.time())
+                    self.heightfieldData[2 * i +
+                                         2 * j * numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + 1 +
+                                         2 * j * numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + (2 * j + 1) *
+                                         numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + 1 + (2 * j + 1) *
+                                         numHeightfieldRows] = height
+            #GEOM_CONCAVE_INTERNAL_EDGE may help avoid getting stuck at an internal (shared) edge of the triangle/heightfield.
+            #GEOM_CONCAVE_INTERNAL_EDGE is a bit slower to build though.
+            #flags = p.GEOM_CONCAVE_INTERNAL_EDGE
+            flags = 0
+            self.terrainShape = p.createCollisionShape(
+                shapeType=p.GEOM_HEIGHTFIELD,
+                flags=flags,
+                meshScale=[.05, .05, 1],
+                heightfieldTextureScaling=(numHeightfieldRows - 1) / 2,
+                heightfieldData=self.heightfieldData,
+                numHeightfieldRows=numHeightfieldRows,
+                numHeightfieldColumns=numHeightfieldColumns,
+                replaceHeightfieldIndex=self.terrainShape)

simulation/GymEnvs/spot_bezier_env.py

@@ -0,0 +1,266 @@
+""" This file implements the gym environment of SpotMicro with Bezier Curve.
+"""
+
+import math
+import time
+import numpy as np
+import pybullet_data
+from gym import spaces
+import spot
+from gym.envs.registration import register
+
+from simulation.GymEnvs.spot_gym_env import spotGymEnv
+
+SENSOR_NOISE_STDDEV = spot.SENSOR_NOISE_STDDEV
+
+# Register as OpenAI Gym Environment
+register(
+    id="SpotMicroEnv-v1",
+    entry_point="spotmicro.GymEnvs.spot_bezier_env:spotBezierEnv",
+    max_episode_steps=1000,
+)
+
+
+class spotBezierEnv(spotGymEnv):
+    """The gym environment for spot.
+
+    It simulates the locomotion of spot, a quadruped robot. The state space
+    include the angles, velocities and torques for all the motors and the action
+    space is the desired motor angle for each motor. The reward function is based
+    on how far spot walks in 1000 steps and penalizes the energy
+    expenditure.
+
+    """
+
+    metadata = {"render.modes": ["human", "rgb_array"], "video.frames_per_second": 50}
+
+    def __init__(
+        self,
+        distance_weight=1.0,
+        rotation_weight=0.0,
+        energy_weight=0.000,
+        shake_weight=0.00,
+        drift_weight=0.0,
+        rp_weight=10.0,
+        rate_weight=0.03,
+        urdf_root=pybullet_data.getDataPath(),
+        urdf_version=None,
+        distance_limit=float("inf"),
+        observation_noise_stdev=SENSOR_NOISE_STDDEV,
+        self_collision_enabled=True,
+        motor_velocity_limit=np.inf,
+        pd_control_enabled=False,
+        leg_model_enabled=False,
+        accurate_motor_model_enabled=False,
+        remove_default_joint_damping=False,
+        motor_kp=2.0,
+        motor_kd=0.03,
+        control_latency=0.0,
+        pd_latency=0.0,
+        torque_control_enabled=False,
+        motor_overheat_protection=False,
+        hard_reset=False,
+        on_rack=False,
+        render=True,
+        num_steps_to_log=1000,
+        action_repeat=1,
+        control_time_step=None,
+        forward_reward_cap=float("inf"),
+        reflection=True,
+        log_path=None,
+        desired_velocity=0.5,
+        desired_rate=0.0,
+        lateral=False,
+        draw_foot_path=False,
+        height_field=False,
+        AutoStepper=True,
+        action_dim=14,
+        contacts=True,
+    ):
+
+        super(spotBezierEnv, self).__init__(
+            distance_weight=distance_weight,
+            rotation_weight=rotation_weight,
+            energy_weight=energy_weight,
+            shake_weight=shake_weight,
+            drift_weight=drift_weight,
+            rp_weight=rp_weight,
+            rate_weight=rate_weight,
+            urdf_root=urdf_root,
+            urdf_version=urdf_version,
+            distance_limit=distance_limit,
+            observation_noise_stdev=observation_noise_stdev,
+            self_collision_enabled=self_collision_enabled,
+            motor_velocity_limit=motor_velocity_limit,
+            pd_control_enabled=pd_control_enabled,
+            leg_model_enabled=leg_model_enabled,
+            accurate_motor_model_enabled=accurate_motor_model_enabled,
+            remove_default_joint_damping=remove_default_joint_damping,
+            motor_kp=motor_kp,
+            motor_kd=motor_kd,
+            control_latency=control_latency,
+            pd_latency=pd_latency,
+            torque_control_enabled=torque_control_enabled,
+            motor_overheat_protection=motor_overheat_protection,
+            hard_reset=hard_reset,
+            on_rack=on_rack,
+            render=render,
+            num_steps_to_log=num_steps_to_log,
+            action_repeat=action_repeat,
+            control_time_step=control_time_step,
+            forward_reward_cap=forward_reward_cap,
+            reflection=reflection,
+            log_path=log_path,
+            desired_velocity=desired_velocity,
+            desired_rate=desired_rate,
+            lateral=lateral,
+            draw_foot_path=draw_foot_path,
+            height_field=height_field,
+            AutoStepper=AutoStepper,
+            contacts=contacts,
+        )
+
+        # Residuals + Clearance Height + Penetration Depth
+        action_high = np.array([self._action_bound] * action_dim)
+        self.action_space = spaces.Box(-action_high, action_high)
+        print("Action SPACE: {}".format(self.action_space))
+
+        self.prev_pos = np.array([0.0, 0.0, 0.0])
+
+        self.yaw = 0.0
+
+    def pass_joint_angles(self, ja):
+        """For executing joint angles"""
+        self.ja = ja
+
+    def step(self, action):
+        """Step forward the simulation, given the action.
+
+        Args:
+          action: A list of desired motor angles for eight motors.
+          smach: the bezier state machine containing simulated
+                 random controll inputs
+
+        Returns:
+          observations: The angles, velocities and torques of all motors.
+          reward: The reward for the current state-action pair.
+          done: Whether the episode has ended.
+          info: A dictionary that stores diagnostic information.
+
+        Raises:
+          ValueError: The action dimension is not the same as the number of motors.
+          ValueError: The magnitude of actions is out of bounds.
+        """
+        # Discard all but joint angles
+        action = self.ja
+
+        self._last_base_position = self.spot.GetBasePosition()
+        self._last_base_orientation = self.spot.GetBaseOrientation()
+        # print("ACTION:")
+        # print(action)
+        if self._is_render:
+            # Sleep, otherwise the computation takes less time than real time,
+            # which will make the visualization like a fast-forward video.
+            time_spent = time.time() - self._last_frame_time
+            self._last_frame_time = time.time()
+            time_to_sleep = self.control_time_step - time_spent
+            if time_to_sleep > 0:
+                time.sleep(time_to_sleep)
+            base_pos = self.spot.GetBasePosition()
+            # Keep the previous orientation of the camera set by the user.
+            [yaw, pitch, dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11]
+            self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch, base_pos)
+
+        action = self._transform_action_to_motor_command(action)
+        self.spot.Step(action)
+        # NOTE: SMACH is passed to the reward method
+        reward = self._reward()
+        done = self._termination()
+        self._env_step_counter += 1
+
+        # DRAW FOOT PATH
+        if self.draw_foot_path:
+            self.DrawFootPath()
+
+        return np.array(self._get_observation()), reward, done, {}
+
+    def return_state(self):
+        return np.array(self._get_observation())
+
+    def return_yaw(self):
+        return self.yaw
+
+    def _reward(self):
+        # get observation
+        obs = self._get_observation()
+
+        orn = self.spot.GetBaseOrientation()
+
+        # Return StepVelocity with the sign of StepLength
+        DesiredVelicty = math.copysign(
+            self.spot.StepVelocity / 4.0, self.spot.StepLength
+        )
+
+        fwd_speed = self.spot.prev_lin_twist[0]  # vx
+        lat_speed = self.spot.prev_lin_twist[1]  # vy
+
+        # DEBUG
+        lt, at = self.spot.GetBaseTwist()
+
+        # ONLY WORKS FOR MOVING PURELY FORWARD
+        pos = self.spot.GetBasePosition()
+
+        forward_reward = pos[0] - self.prev_pos[0]
+
+        # yaw_rate = obs[4]
+
+        rot_reward = 0.0
+
+        roll, pitch, yaw = self._pybullet_client.getEulerFromQuaternion(
+            [orn[0], orn[1], orn[2], orn[3]]
+        )
+
+        # if yaw < 0.0:
+        #     yaw += np.pi
+        # else:
+        #     yaw -= np.pi
+
+        # For auto correct
+        self.yaw = yaw
+
+        # penalty for nonzero PITCH and YAW(hidden) ONLY
+        # NOTE: Added Yaw mult
+        rp_reward = -(abs(obs[0]) + abs(obs[1]))
+
+        # print("YAW: {}".format(yaw))
+        # print("RP RWD: {:.2f}".format(rp_reward))
+        # print("ROLL: {} \t PITCH: {}".format(obs[0], obs[1]))
+
+        # penalty for nonzero acc(z) - UNRELIABLE ON IMU
+        shake_reward = 0
+
+        # penalty for nonzero rate (x,y,z)
+        rate_reward = -(abs(obs[2]) + abs(obs[3]))
+
+        # print("RATES: {}".format(obs[2:5]))
+
+        drift_reward = -abs(pos[1])
+        energy_reward = (
+            -np.abs(np.dot(self.spot.GetMotorTorques(), self.spot.GetMotorVelocities()))
+            * self._time_step
+        )
+        reward = (
+            self._distance_weight * forward_reward
+            + self._rotation_weight * rot_reward
+            + self._energy_weight * energy_reward
+            + self._drift_weight * drift_reward
+            + self._shake_weight * shake_reward
+            + self._rp_weight * rp_reward
+            + self._rate_weight * rate_reward
+        )
+        self._objectives.append(
+            [forward_reward, energy_reward, drift_reward, shake_reward]
+        )
+        # print("REWARD: ", reward)
+        # NOTE: return yaw for automatic correction (not part of RL)
+        return reward

simulation/GymEnvs/spot_env_randomizer.py

@@ -0,0 +1,90 @@
+"""
+CODE BASED ON EXAMPLE FROM:
+@misc{coumans2017pybullet,
+  title={Pybullet, a python module for physics simulation in robotics, games and machine learning},
+  author={Coumans, Erwin and Bai, Yunfei},
+  url={www.pybullet.org},
+  year={2017},
+}
+
+Example: minitaur_env_randomizer.py
+https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer.py
+"""
+
+import numpy as np
+import simulation.GymEnvs.env_randomizer_base as env_randomizer_base
+
+# Relative range.
+spot_BASE_MASS_ERROR_RANGE = (-0.2, 0.2)  # 0.2 means 20%
+spot_LEG_MASS_ERROR_RANGE = (-0.2, 0.2)  # 0.2 means 20%
+# Absolute range.
+BATTERY_VOLTAGE_RANGE = (7.0, 8.4)  # Unit: Volt
+MOTOR_VISCOUS_DAMPING_RANGE = (0, 0.01)  # Unit: N*m*s/rad (torque/angular vel)
+spot_LEG_FRICTION = (0.8, 1.5)  # Unit: dimensionless
+
+
+class SpotEnvRandomizer(env_randomizer_base.EnvRandomizerBase):
+    """A randomizer that change the spot_gym_env during every reset."""
+
+    def __init__(
+        self,
+        spot_base_mass_err_range=spot_BASE_MASS_ERROR_RANGE,
+        spot_leg_mass_err_range=spot_LEG_MASS_ERROR_RANGE,
+        battery_voltage_range=BATTERY_VOLTAGE_RANGE,
+        motor_viscous_damping_range=MOTOR_VISCOUS_DAMPING_RANGE,
+    ):
+        self._spot_base_mass_err_range = spot_base_mass_err_range
+        self._spot_leg_mass_err_range = spot_leg_mass_err_range
+        self._battery_voltage_range = battery_voltage_range
+        self._motor_viscous_damping_range = motor_viscous_damping_range
+
+        np.random.seed(0)
+
+    def randomize_env(self, env):
+        self._randomize_spot(env.spot)
+
+    def _randomize_spot(self, spot):
+        """Randomize various physical properties of spot.
+
+        It randomizes the mass/inertia of the base, mass/inertia of the legs,
+        friction coefficient of the feet, the battery voltage and the motor damping
+        at each reset() of the environment.
+
+        Args:
+          spot: the spot instance in spot_gym_env environment.
+        """
+        base_mass = spot.GetBaseMassFromURDF()
+        # print("BM: ", base_mass)
+        randomized_base_mass = np.random.uniform(
+            np.array([base_mass]) * (1.0 + self._spot_base_mass_err_range[0]),
+            np.array([base_mass]) * (1.0 + self._spot_base_mass_err_range[1]),
+        )
+        spot.SetBaseMass(randomized_base_mass[0])
+
+        leg_masses = spot.GetLegMassesFromURDF()
+        leg_masses_lower_bound = np.array(leg_masses) * (
+            1.0 + self._spot_leg_mass_err_range[0]
+        )
+        leg_masses_upper_bound = np.array(leg_masses) * (
+            1.0 + self._spot_leg_mass_err_range[1]
+        )
+        randomized_leg_masses = [
+            np.random.uniform(leg_masses_lower_bound[i], leg_masses_upper_bound[i])
+            for i in range(len(leg_masses))
+        ]
+        spot.SetLegMasses(randomized_leg_masses)
+
+        randomized_battery_voltage = np.random.uniform(
+            BATTERY_VOLTAGE_RANGE[0], BATTERY_VOLTAGE_RANGE[1]
+        )
+        spot.SetBatteryVoltage(randomized_battery_voltage)
+
+        randomized_motor_damping = np.random.uniform(
+            MOTOR_VISCOUS_DAMPING_RANGE[0], MOTOR_VISCOUS_DAMPING_RANGE[1]
+        )
+        spot.SetMotorViscousDamping(randomized_motor_damping)
+
+        randomized_foot_friction = np.random.uniform(
+            spot_LEG_FRICTION[0], spot_LEG_FRICTION[1]
+        )
+        spot.SetFootFriction(randomized_foot_friction)

simulation/GymEnvs/spot_gym_env.py

@@ -0,0 +1,785 @@
+"""
+CODE BASED ON EXAMPLE FROM:
+@misc{coumans2017pybullet,
+  title={Pybullet, a python module for physics simulation in robotics, games and machine learning},
+  author={Coumans, Erwin and Bai, Yunfei},
+  url={www.pybullet.org},
+  year={2017},
+}
+
+Example: minitaur_gym_env.py
+https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py
+"""
+
+import math
+import time
+import gym
+import numpy as np
+import pybullet
+import pybullet_data
+from gym import spaces
+from gym.utils import seeding
+from pkg_resources import parse_version
+import spot
+import pybullet_utils.bullet_client as bullet_client
+from gym.envs.registration import register
+from simulation.GymEnvs.heightfield import HeightField
+from OpenLoopSM.SpotOL import BezierStepper
+from raspberry_pi.src.Kinematics import LieAlgebra as LA
+from simulation.GymEnvs.spot_env_randomizer import SpotEnvRandomizer
+
+NUM_SUBSTEPS = 5
+NUM_MOTORS = 12
+MOTOR_ANGLE_OBSERVATION_INDEX = 0
+MOTOR_VELOCITY_OBSERVATION_INDEX = MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS
+MOTOR_TORQUE_OBSERVATION_INDEX = MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS
+BASE_ORIENTATION_OBSERVATION_INDEX = MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS
+ACTION_EPS = 0.01
+OBSERVATION_EPS = 0.01
+RENDER_HEIGHT = 720
+RENDER_WIDTH = 960
+SENSOR_NOISE_STDDEV = spot.SENSOR_NOISE_STDDEV
+DEFAULT_URDF_VERSION = "default"
+NUM_SIMULATION_ITERATION_STEPS = 1000
+
+spot_URDF_VERSION_MAP = {DEFAULT_URDF_VERSION: spot.Spot}
+
+# Register as OpenAI Gym Environment
+register(
+    id="SpotMicroEnv-v0",
+    entry_point="spotmicro.spot_gym_env:spotGymEnv",
+    max_episode_steps=1000,
+)
+
+
+def convert_to_list(obj):
+    try:
+        iter(obj)
+        return obj
+    except TypeError:
+        return [obj]
+
+
+class spotGymEnv(gym.Env):
+    """The gym environment for spot.
+
+    It simulates the locomotion of spot, a quadruped robot. The state space
+    include the angles, velocities and torques for all the motors and the action
+    space is the desired motor angle for each motor. The reward function is based
+    on how far spot walks in 1000 steps and penalizes the energy
+    expenditure.
+
+    """
+
+    metadata = {"render.modes": ["human", "rgb_array"], "video.frames_per_second": 50}
+
+    def __init__(
+        self,
+        distance_weight=1.0,
+        rotation_weight=1.0,
+        energy_weight=0.0005,
+        shake_weight=0.005,
+        drift_weight=2.0,
+        rp_weight=0.1,
+        rate_weight=0.1,
+        urdf_root=pybullet_data.getDataPath(),
+        urdf_version=None,
+        distance_limit=float("inf"),
+        observation_noise_stdev=SENSOR_NOISE_STDDEV,
+        self_collision_enabled=True,
+        motor_velocity_limit=np.inf,
+        pd_control_enabled=False,
+        leg_model_enabled=False,
+        accurate_motor_model_enabled=False,
+        remove_default_joint_damping=False,
+        motor_kp=2.0,
+        motor_kd=0.03,
+        control_latency=0.0,
+        pd_latency=0.0,
+        torque_control_enabled=False,
+        motor_overheat_protection=False,
+        hard_reset=False,
+        on_rack=False,
+        render=True,
+        num_steps_to_log=1000,
+        action_repeat=1,
+        control_time_step=None,
+        forward_reward_cap=float("inf"),
+        reflection=True,
+        log_path=None,
+        desired_velocity=0.5,
+        desired_rate=0.0,
+        lateral=False,
+        draw_foot_path=False,
+        height_field=False,
+        height_field_iters=2,
+        AutoStepper=False,
+        contacts=True,
+    ):
+        """Initialize the spot gym environment.
+
+        Args:
+          urdf_root: The path to the urdf data folder.
+          urdf_version: [DEFAULT_URDF_VERSION] are allowable
+            versions. If None, DEFAULT_URDF_VERSION is used.
+          distance_weight: The weight of the distance term in the reward.
+          energy_weight: The weight of the energy term in the reward.
+          shake_weight: The weight of the vertical shakiness term in the reward.
+          drift_weight: The weight of the sideways drift term in the reward.
+          distance_limit: The maximum distance to terminate the episode.
+          observation_noise_stdev: The standard deviation of observation noise.
+          self_collision_enabled: Whether to enable self collision in the sim.
+          motor_velocity_limit: The velocity limit of each motor.
+          pd_control_enabled: Whether to use PD controller for each motor.
+          leg_model_enabled: Whether to use a leg motor to reparameterize the action
+            space.
+          accurate_motor_model_enabled: Whether to use the accurate DC motor model.
+          remove_default_joint_damping: Whether to remove the default joint damping.
+          motor_kp: proportional gain for the accurate motor model.
+          motor_kd: derivative gain for the accurate motor model.
+          control_latency: It is the delay in the controller between when an
+            observation is made at some point, and when that reading is reported
+            back to the Neural Network.
+          pd_latency: latency of the PD controller loop. PD calculates PWM based on
+            the motor angle and velocity. The latency measures the time between when
+            the motor angle and velocity are observed on the microcontroller and
+            when the true state happens on the motor. It is typically (0.001-
+            0.002s).
+          torque_control_enabled: Whether to use the torque control, if set to
+            False, pose control will be used.
+          motor_overheat_protection: Whether to shutdown the motor that has exerted
+            large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
+            (OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in spot.py for more
+            details.
+          hard_reset: Whether to wipe the simulation and load everything when reset
+            is called. If set to false, reset just place spot back to start
+            position and set its pose to initial configuration.
+          on_rack: Whether to place spot on rack. This is only used to debug
+            the walking gait. In this mode, spot's base is hanged midair so
+            that its walking gait is clearer to visualize.
+          render: Whether to render the simulation.
+          num_steps_to_log: The max number of control steps in one episode that will
+            be logged. If the number of steps is more than num_steps_to_log, the
+            environment will still be running, but only first num_steps_to_log will
+            be recorded in logging.
+          action_repeat: The number of simulation steps before actions are applied.
+          control_time_step: The time step between two successive control signals.
+          env_randomizer: An instance (or a list) of EnvRandomizer(s). An
+            EnvRandomizer may randomize the physical property of spot, change
+              the terrrain during reset(), or add perturbation forces during step().
+          forward_reward_cap: The maximum value that forward reward is capped at.
+            Disabled (Inf) by default.
+          log_path: The path to write out logs. For the details of logging, refer to
+            spot_logging.proto.
+        Raises:
+          ValueError: If the urdf_version is not supported.
+        """
+        # Sense Contacts
+        self.contacts = contacts
+        # Enable Auto Stepper State Machine
+        self.AutoStepper = AutoStepper
+        # Enable Rough Terrain or Not
+        self.height_field = height_field
+        self.draw_foot_path = draw_foot_path
+        # DRAWING FEET PATH
+        self.prev_feet_path = np.array(
+            [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]
+        )
+
+        # CONTROL METRICS
+        self.desired_velocity = desired_velocity
+        self.desired_rate = desired_rate
+        self.lateral = lateral
+
+        # Set up logging.
+        self._log_path = log_path
+        # @TODO fix logging
+
+        # NUM ITERS
+        self._time_step = 0.01
+        self._action_repeat = action_repeat
+        self._num_bullet_solver_iterations = 300
+        self.logging = None
+        if pd_control_enabled or accurate_motor_model_enabled:
+            self._time_step /= NUM_SUBSTEPS
+            self._num_bullet_solver_iterations /= NUM_SUBSTEPS
+            self._action_repeat *= NUM_SUBSTEPS
+        # PD control needs smaller time step for stability.
+        if control_time_step is not None:
+            self.control_time_step = control_time_step
+        else:
+            # Get Control Timestep
+            self.control_time_step = self._time_step * self._action_repeat
+        # TODO: Fix the value of self._num_bullet_solver_iterations.
+        self._num_bullet_solver_iterations = int(
+            NUM_SIMULATION_ITERATION_STEPS / self._action_repeat
+        )
+
+        # URDF
+        self._urdf_root = urdf_root
+        self._self_collision_enabled = self_collision_enabled
+        self._motor_velocity_limit = motor_velocity_limit
+        self._observation = []
+        self._true_observation = []
+        self._objectives = []
+        self._objective_weights = [
+            distance_weight,
+            energy_weight,
+            drift_weight,
+            shake_weight,
+        ]
+        self._env_step_counter = 0
+        self._num_steps_to_log = num_steps_to_log
+        self._is_render = render
+        self._last_base_position = [0, 0, 0]
+        self._last_base_orientation = [0, 0, 0, 1]
+        self._distance_weight = distance_weight
+        self._rotation_weight = rotation_weight
+        self._energy_weight = energy_weight
+        self._drift_weight = drift_weight
+        self._shake_weight = shake_weight
+        self._rp_weight = rp_weight
+        self._rate_weight = rate_weight
+        self._distance_limit = distance_limit
+        self._observation_noise_stdev = observation_noise_stdev
+        self._action_bound = 1
+        self._pd_control_enabled = pd_control_enabled
+        self._leg_model_enabled = leg_model_enabled
+        self._accurate_motor_model_enabled = accurate_motor_model_enabled
+        self._remove_default_joint_damping = remove_default_joint_damping
+        self._motor_kp = motor_kp
+        self._motor_kd = motor_kd
+        self._torque_control_enabled = torque_control_enabled
+        self._motor_overheat_protection = motor_overheat_protection
+        self._on_rack = on_rack
+        self._cam_dist = 1.0
+        self._cam_yaw = 0
+        self._cam_pitch = -30
+        self._forward_reward_cap = forward_reward_cap
+        self._hard_reset = True
+        self._last_frame_time = 0.0
+        self._control_latency = control_latency
+        self._pd_latency = pd_latency
+        self._urdf_version = urdf_version
+        self._ground_id = None
+        self._reflection = reflection
+        # @TODO fix logging
+        self._episode_proto = None
+        if self._is_render:
+            self._pybullet_client = bullet_client.BulletClient(
+                connection_mode=pybullet.GUI
+            )
+        else:
+            self._pybullet_client = bullet_client.BulletClient()
+        if self._urdf_version is None:
+            self._urdf_version = DEFAULT_URDF_VERSION
+        self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
+        self.seed()
+        # Only update after HF has been generated
+        self.height_field = False
+        self.reset()
+        observation_high = self.spot.GetObservationUpperBound() + OBSERVATION_EPS
+        observation_low = self.spot.GetObservationLowerBound() - OBSERVATION_EPS
+        action_dim = NUM_MOTORS
+        action_high = np.array([self._action_bound] * action_dim)
+        self.action_space = spaces.Box(-action_high, action_high)
+        self.observation_space = spaces.Box(observation_low, observation_high)
+        self.viewer = None
+        self._hard_reset = hard_reset  # This assignment need to be after reset()
+        self.goal_reached = False
+        # Generate HeightField or not
+        self.height_field = height_field
+        self.hf = HeightField()
+        if self.height_field:
+            # Do 3x for extra roughness
+            for i in range(height_field_iters):
+                self.hf._generate_field(self)
+
+    def set_env_randomizer(self, env_randomizer):
+        self._env_randomizer = env_randomizer
+
+    def configure(self, args):
+        self._args = args
+
+    def reset(
+        self,
+        initial_motor_angles=None,
+        reset_duration=1.0,
+        desired_velocity=None,
+        desired_rate=None,
+    ):
+        # Use Autostepper
+        if self.AutoStepper:
+            self.StateMachine = BezierStepper(dt=self._time_step)
+            # Shuffle order of states
+            self.StateMachine.reshuffle()
+
+        self._pybullet_client.configureDebugVisualizer(
+            self._pybullet_client.COV_ENABLE_RENDERING, 0
+        )
+        if self._hard_reset:
+            self._pybullet_client.resetSimulation()
+            self._pybullet_client.setPhysicsEngineParameter(
+                numSolverIterations=int(self._num_bullet_solver_iterations)
+            )
+            self._pybullet_client.setTimeStep(self._time_step)
+            self._ground_id = self._pybullet_client.loadURDF(
+                "%s/plane.urdf" % self._urdf_root
+            )
+            if self._reflection:
+                self._pybullet_client.changeVisualShape(
+                    self._ground_id, -1, rgbaColor=[1, 1, 1, 0.8]
+                )
+                self._pybullet_client.configureDebugVisualizer(
+                    self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION, self._ground_id
+                )
+            self._pybullet_client.setGravity(0, 0, -9.81)
+            acc_motor = self._accurate_motor_model_enabled
+            motor_protect = self._motor_overheat_protection
+            if self._urdf_version not in spot_URDF_VERSION_MAP:
+                raise ValueError(
+                    "%s is not a supported urdf_version." % self._urdf_version
+                )
+            else:
+                self.spot = spot_URDF_VERSION_MAP[self._urdf_version](
+                    pybullet_client=self._pybullet_client,
+                    action_repeat=self._action_repeat,
+                    urdf_root=self._urdf_root,
+                    time_step=self._time_step,
+                    self_collision_enabled=self._self_collision_enabled,
+                    motor_velocity_limit=self._motor_velocity_limit,
+                    pd_control_enabled=self._pd_control_enabled,
+                    accurate_motor_model_enabled=acc_motor,
+                    remove_default_joint_damping=self._remove_default_joint_damping,
+                    motor_kp=self._motor_kp,
+                    motor_kd=self._motor_kd,
+                    control_latency=self._control_latency,
+                    pd_latency=self._pd_latency,
+                    observation_noise_stdev=self._observation_noise_stdev,
+                    torque_control_enabled=self._torque_control_enabled,
+                    motor_overheat_protection=motor_protect,
+                    on_rack=self._on_rack,
+                    np_random=self.np_random,
+                    contacts=self.contacts,
+                )
+        self.spot.Reset(
+            reload_urdf=False,
+            default_motor_angles=initial_motor_angles,
+            reset_time=reset_duration,
+        )
+
+        if desired_velocity is not None:
+            self.desired_velocity = desired_velocity
+        if desired_rate is not None:
+            self.desired_rate = desired_rate
+
+        self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
+        self._env_step_counter = 0
+        self._last_base_position = [0, 0, 0]
+        self._last_base_orientation = [0, 0, 0, 1]
+        self._objectives = []
+        self._pybullet_client.resetDebugVisualizerCamera(
+            self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0]
+        )
+        self._pybullet_client.configureDebugVisualizer(
+            self._pybullet_client.COV_ENABLE_RENDERING, 1
+        )
+        return self._get_observation()
+
+    def seed(self, seed=None):
+        self.np_random, seed = seeding.np_random(seed)
+        return [seed]
+
+    def _transform_action_to_motor_command(self, action):
+        if self._leg_model_enabled:
+            for i, action_component in enumerate(action):
+                if not (
+                    -self._action_bound - ACTION_EPS
+                    <= action_component
+                    <= self._action_bound + ACTION_EPS
+                ):
+                    raise ValueError(
+                        "{}th action {} out of bounds.".format(i, action_component)
+                    )
+            action = self.spot.ConvertFromLegModel(action)
+        return action
+
+    def step(self, action):
+        """Step forward the simulation, given the action.
+
+        Args:
+          action: A list of desired motor angles for eight motors.
+
+        Returns:
+          observations: The angles, velocities and torques of all motors.
+          reward: The reward for the current state-action pair.
+          done: Whether the episode has ended.
+          info: A dictionary that stores diagnostic information.
+
+        Raises:
+          ValueError: The action dimension is not the same as the number of motors.
+          ValueError: The magnitude of actions is out of bounds.
+        """
+        self._last_base_position = self.spot.GetBasePosition()
+        self._last_base_orientation = self.spot.GetBaseOrientation()
+        # print("ACTION:")
+        # print(action)
+        if self._is_render:
+            # Sleep, otherwise the computation takes less time than real time,
+            # which will make the visualization like a fast-forward video.
+            time_spent = time.time() - self._last_frame_time
+            self._last_frame_time = time.time()
+            time_to_sleep = self.control_time_step - time_spent
+            if time_to_sleep > 0:
+                time.sleep(time_to_sleep)
+            base_pos = self.spot.GetBasePosition()
+            # Keep the previous orientation of the camera set by the user.
+            [yaw, pitch, dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11]
+            self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch, base_pos)
+
+        action = self._transform_action_to_motor_command(action)
+        self.spot.Step(action)
+        reward = self._reward()
+        done = self._termination()
+        self._env_step_counter += 1
+
+        # DRAW FOOT PATH
+        if self.draw_foot_path:
+            self.DrawFootPath()
+        return np.array(self._get_observation()), reward, done, {}
+
+    def render(self, mode="rgb_array", close=False):
+        if mode != "rgb_array":
+            return np.array([])
+        base_pos = self.spot.GetBasePosition()
+        view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
+            cameraTargetPosition=base_pos,
+            distance=self._cam_dist,
+            yaw=self._cam_yaw,
+            pitch=self._cam_pitch,
+            roll=0,
+            upAxisIndex=2,
+        )
+        proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
+            fov=60,
+            aspect=float(RENDER_WIDTH) / RENDER_HEIGHT,
+            nearVal=0.1,
+            farVal=100.0,
+        )
+        (_, _, px, _, _) = self._pybullet_client.getCameraImage(
+            width=RENDER_WIDTH,
+            height=RENDER_HEIGHT,
+            renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL,
+            viewMatrix=view_matrix,
+            projectionMatrix=proj_matrix,
+        )
+        rgb_array = np.array(px)
+        rgb_array = rgb_array[:, :, :3]
+        return rgb_array
+
+    def DrawFootPath(self):
+        # Get Foot Positions
+        FL = self._pybullet_client.getLinkState(
+            self.spot.quadruped, self.spot._foot_id_list[0]
+        )[0]
+        FR = self._pybullet_client.getLinkState(
+            self.spot.quadruped, self.spot._foot_id_list[1]
+        )[0]
+        BL = self._pybullet_client.getLinkState(
+            self.spot.quadruped, self.spot._foot_id_list[2]
+        )[0]
+        BR = self._pybullet_client.getLinkState(
+            self.spot.quadruped, self.spot._foot_id_list[3]
+        )[0]
+
+        lifetime = 3.0  # sec
+        self._pybullet_client.addUserDebugLine(
+            self.prev_feet_path[0], FL, [1, 0, 0], lifeTime=lifetime
+        )
+        self._pybullet_client.addUserDebugLine(
+            self.prev_feet_path[1], FR, [0, 1, 0], lifeTime=lifetime
+        )
+        self._pybullet_client.addUserDebugLine(
+            self.prev_feet_path[2], BL, [0, 0, 1], lifeTime=lifetime
+        )
+        self._pybullet_client.addUserDebugLine(
+            self.prev_feet_path[3], BR, [1, 1, 0], lifeTime=lifetime
+        )
+
+        self.prev_feet_path[0] = FL
+        self.prev_feet_path[1] = FR
+        self.prev_feet_path[2] = BL
+        self.prev_feet_path[3] = BR
+
+    def get_spot_motor_angles(self):
+        """Get the spot's motor angles.
+
+        Returns:
+          A numpy array of motor angles.
+        """
+        return np.array(
+            self._observation[
+                MOTOR_ANGLE_OBSERVATION_INDEX : MOTOR_ANGLE_OBSERVATION_INDEX
+                + NUM_MOTORS
+            ]
+        )
+
+    def get_spot_motor_velocities(self):
+        """Get the spot's motor velocities.
+
+        Returns:
+          A numpy array of motor velocities.
+        """
+        return np.array(
+            self._observation[
+                MOTOR_VELOCITY_OBSERVATION_INDEX : MOTOR_VELOCITY_OBSERVATION_INDEX
+                + NUM_MOTORS
+            ]
+        )
+
+    def get_spot_motor_torques(self):
+        """Get the spot's motor torques.
+
+        Returns:
+          A numpy array of motor torques.
+        """
+        return np.array(
+            self._observation[
+                MOTOR_TORQUE_OBSERVATION_INDEX : MOTOR_TORQUE_OBSERVATION_INDEX
+                + NUM_MOTORS
+            ]
+        )
+
+    def get_spot_base_orientation(self):
+        """Get the spot's base orientation, represented by a quaternion.
+
+        Returns:
+          A numpy array of spot's orientation.
+        """
+        return np.array(self._observation[BASE_ORIENTATION_OBSERVATION_INDEX:])
+
+    def is_fallen(self):
+        """Decide whether spot has fallen.
+
+        If the up directions between the base and the world is larger (the dot
+        product is smaller than 0.85) or the base is very low on the ground
+        (the height is smaller than 0.13 meter), spot is considered fallen.
+
+        Returns:
+          Boolean value that indicates whether spot has fallen.
+        """
+        orientation = self.spot.GetBaseOrientation()
+        rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation)
+        local_up = rot_mat[6:]
+        pos = self.spot.GetBasePosition()
+        #  or pos[2] < 0.13
+        return np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.55
+
+    def _termination(self):
+        position = self.spot.GetBasePosition()
+        distance = math.sqrt(position[0] ** 2 + position[1] ** 2)
+        return self.is_fallen() or distance > self._distance_limit
+
+    def _reward(self):
+        """NOTE: reward now consists of:
+        roll, pitch at desired 0
+        acc (y,z) = 0
+        FORWARD-BACKWARD: rate(x,y,z) = 0
+        --> HIDDEN REWARD: x(+-) velocity reference, not incl. in obs
+        SPIN: acc(x) = 0, rate(x,y) = 0, rate (z) = rate reference
+        Also include drift, energy vanilla rewards
+        """
+        current_base_position = self.spot.GetBasePosition()
+
+        # get observation
+        obs = self._get_observation()
+        # forward_reward = current_base_position[0] - self._last_base_position[0]
+
+        # # POSITIVE FOR FORWARD, NEGATIVE FOR BACKWARD | NOTE: HIDDEN
+        # GETTING TWIST IN BODY FRAME
+        pos = self.spot.GetBasePosition()
+        orn = self.spot.GetBaseOrientation()
+        roll, pitch, yaw = self._pybullet_client.getEulerFromQuaternion(
+            [orn[0], orn[1], orn[2], orn[3]]
+        )
+        rpy = LA.RPY(roll, pitch, yaw)
+        R, _ = LA.TransToRp(rpy)
+        T_wb = LA.RpToTrans(R, np.array([pos[0], pos[1], pos[2]]))
+        T_bw = LA.TransInv(T_wb)
+        Adj_Tbw = LA.Adjoint(T_bw)
+
+        Vw = np.concatenate((self.spot.prev_ang_twist, self.spot.prev_lin_twist))
+        Vb = np.dot(Adj_Tbw, Vw)
+
+        # New Twist in Body Frame
+        # POSITIVE FOR FORWARD, NEGATIVE FOR BACKWARD | NOTE: HIDDEN
+        fwd_speed = -Vb[3]  # vx
+        lat_speed = -Vb[4]  # vy
+        # fwd_speed = self.spot.prev_lin_twist[0]
+        # lat_speed = self.spot.prev_lin_twist[1]
+        # print("FORWARD SPEED: {} \t STATE SPEED: {}".format(
+        #     fwd_speed, self.desired_velocity))
+        # self.desired_velocity = 0.4
+
+        # Modification for lateral/fwd rewards
+        reward_max = 1.0
+        # FORWARD
+        if not self.lateral:
+            # f(x)=-(x-desired))^(2)*((1/desired)^2)+1
+            # to make sure that at 0vel there is 0 reawrd.
+            # also squishes allowable tolerance
+            forward_reward = reward_max * np.exp(
+                -((fwd_speed - self.desired_velocity) ** 2) / (0.1)
+            )
+        # LATERAL
+        else:
+            forward_reward = reward_max * np.exp(
+                -((lat_speed - self.desired_velocity) ** 2) / (0.1)
+            )
+
+        yaw_rate = obs[4]
+
+        rot_reward = reward_max * np.exp(-((yaw_rate - self.desired_rate) ** 2) / (0.1))
+
+        # Make sure that for forward-policy there is the appropriate rotation penalty
+        if self.desired_velocity != 0:
+            self._rotation_weight = self._rate_weight
+            rot_reward = -abs(obs[4])
+        elif self.desired_rate != 0:
+            forward_reward = 0.0
+
+        # penalty for nonzero roll, pitch
+        rp_reward = -(abs(obs[0]) + abs(obs[1]))
+        # print("ROLL: {} \t PITCH: {}".format(obs[0], obs[1]))
+
+        # penalty for nonzero acc(z)
+        shake_reward = -abs(obs[4])
+
+        # penalty for nonzero rate (x,y,z)
+        rate_reward = -(abs(obs[2]) + abs(obs[3]))
+
+        # drift_reward = -abs(current_base_position[1] -
+        #                     self._last_base_position[1])
+
+        # this penalizes absolute error, and does not penalize correction
+        # NOTE: for side-side, drift reward becomes in x instead
+        drift_reward = -abs(current_base_position[1])
+
+        # If Lateral, change drift reward
+        if self.lateral:
+            drift_reward = -abs(current_base_position[0])
+
+        # shake_reward = -abs(current_base_position[2] -
+        #                     self._last_base_position[2])
+        self._last_base_position = current_base_position
+        energy_reward = (
+            -np.abs(np.dot(self.spot.GetMotorTorques(), self.spot.GetMotorVelocities()))
+            * self._time_step
+        )
+        reward = (
+            self._distance_weight * forward_reward
+            + self._rotation_weight * rot_reward
+            + self._energy_weight * energy_reward
+            + self._drift_weight * drift_reward
+            + self._shake_weight * shake_reward
+            + self._rp_weight * rp_reward
+            + self._rate_weight * rate_reward
+        )
+        self._objectives.append(
+            [forward_reward, energy_reward, drift_reward, shake_reward]
+        )
+        # print("REWARD: ", reward)
+        return reward
+
+    def get_objectives(self):
+        return self._objectives
+
+    @property
+    def objective_weights(self):
+        """Accessor for the weights for all the objectives.
+
+        Returns:
+          List of floating points that corresponds to weights for the objectives in
+          the order that objectives are stored.
+        """
+        return self._objective_weights
+
+    def _get_observation(self):
+        """Get observation of this environment, including noise and latency.
+
+        spot class maintains a history of true observations. Based on the
+        latency, this function will find the observation at the right time,
+        interpolate if necessary. Then Gaussian noise is added to this observation
+        based on self.observation_noise_stdev.
+
+        Returns:
+          The noisy observation with latency.
+        """
+
+        self._observation = self.spot.GetObservation()
+        return self._observation
+
+    def _get_realistic_observation(self):
+        """Get the observations of this environment.
+
+        It includes the angles, velocities, torques and the orientation of the base.
+
+        Returns:
+          The observation list. observation[0:8] are motor angles. observation[8:16]
+          are motor velocities, observation[16:24] are motor torques.
+          observation[24:28] is the orientation of the base, in quaternion form.
+        """
+        self._observation = self.spot.RealisticObservation()
+        return self._observation
+
+    if parse_version(gym.__version__) < parse_version("0.9.6"):
+        _render = render
+        _reset = reset
+        _seed = seed
+        _step = step
+
+    def set_time_step(self, control_step, simulation_step=0.001):
+        """Sets the time step of the environment.
+
+        Args:
+          control_step: The time period (in seconds) between two adjacent control
+            actions are applied.
+          simulation_step: The simulation time step in PyBullet. By default, the
+            simulation step is 0.001s, which is a good trade-off between simulation
+            speed and accuracy.
+        Raises:
+          ValueError: If the control step is smaller than the simulation step.
+        """
+        if control_step < simulation_step:
+            raise ValueError(
+                "Control step should be larger than or equal to simulation step."
+            )
+        self.control_time_step = control_step
+        self._time_step = simulation_step
+        self._action_repeat = int(round(control_step / simulation_step))
+        self._num_bullet_solver_iterations = (
+            NUM_SIMULATION_ITERATION_STEPS / self._action_repeat
+        )
+        self._pybullet_client.setPhysicsEngineParameter(
+            numSolverIterations=self._num_bullet_solver_iterations
+        )
+        self._pybullet_client.setTimeStep(self._time_step)
+        self.spot.SetTimeSteps(
+            action_repeat=self._action_repeat, simulation_step=self._time_step
+        )
+
+    @property
+    def pybullet_client(self):
+        return self._pybullet_client
+
+    @property
+    def ground_id(self):
+        return self._ground_id
+
+    @ground_id.setter
+    def ground_id(self, new_ground_id):
+        self._ground_id = new_ground_id
+
+    @property
+    def env_step_counter(self):
+        return self._env_step_counter

simulation/OpenLoopSM/SpotOL.py

@@ -0,0 +1,253 @@
+""" Open Loop Controller for Spot Micro. Takes GUI params or uses default
+"""
+import numpy as np
+from random import shuffle
+import copy
+# Ensuring totally random seed every step!
+np.random.seed()
+
+FB = 0
+LAT = 1
+ROT = 2
+COMBI = 3
+
+FWD = 0
+ALL = 1
+
+
+class BezierStepper():
+    def __init__(self,
+                 pos=np.array([0.0, 0.0, 0.0]),
+                 orn=np.array([0.0, 0.0, 0.0]),
+                 StepLength=0.04,
+                 LateralFraction=0.0,
+                 YawRate=0.0,
+                 StepVelocity=0.001,
+                 ClearanceHeight=0.045,
+                 PenetrationDepth=0.003,
+                 episode_length=5000,
+                 dt=0.01,
+                 num_shuffles=2,
+                 mode=FWD):
+        self.pos = pos
+        self.orn = orn
+        self.desired_StepLength = StepLength
+        self.StepLength = StepLength
+        self.StepLength_LIMITS = [-0.05, 0.05]
+        self.LateralFraction = LateralFraction
+        self.LateralFraction_LIMITS = [-np.pi / 2.0, np.pi / 2.0]
+        self.YawRate = YawRate
+        self.YawRate_LIMITS = [-1.0, 1.0]
+        self.StepVelocity = StepVelocity
+        self.StepVelocity_LIMITS = [0.1, 1.5]
+        self.ClearanceHeight = ClearanceHeight
+        self.ClearanceHeight_LIMITS = [0.0, 0.04]
+        self.PenetrationDepth = PenetrationDepth
+        self.PenetrationDepth_LIMITS = [0.0, 0.02]
+
+        self.mode = mode
+
+        self.dt = dt
+
+        # Keep track of state machine
+        self.time = 0
+        # Decide how long to stay in each phase based on maxtime
+        self.max_time = episode_length
+        """ States
+                1: FWD/BWD
+                2: Lat
+                3: Rot
+                4: Combined
+        """
+        self.order = [FB, LAT, ROT, COMBI]
+        # Shuffles list in place so the order of states is unpredictable
+        # NOTE: increment num_shuffles by episode num (cap at 10
+        # and reset or someting) for some forced randomness
+        for _ in range(num_shuffles):
+            shuffle(self.order)
+
+        # Forward/Backward always needs to be first!
+        self.reshuffle()
+
+        # Current State
+        self.current_state = self.order[0]
+
+        # Divide by number of states (see RL_SM())
+        self.time_per_episode = int(self.max_time / len(self.order))
+
+    def ramp_up(self):
+        if self.StepLength < self.desired_StepLength:
+            self.StepLength += self.desired_StepLength * self.dt
+
+    def reshuffle(self):
+        self.time = 0
+        # Make sure FWD/BWD is always first state
+        FB_index = self.order.index(FB)
+        if FB_index != 0:
+            what_was_in_zero = self.order[0]
+            self.order[0] = FB
+            self.order[FB_index] = what_was_in_zero
+
+    def which_state(self):
+        # Ensuring totally random seed every step!
+        np.random.seed()
+        if self.time > self.max_time:
+            # Combined
+            self.current_state = COMBI
+            self.time = 0
+        else:
+            index = int(self.time / self.time_per_episode)
+
+            if index > len(self.order) - 1:
+                index = len(self.order) - 1
+
+            self.current_state = self.order[index]
+
+    def StateMachine(self):
+        """
+        State Machined used for training robust RL on top of OL gait.
+
+        STATES:
+            Forward/Backward: All Default Values.
+                              Can have slow changes to
+                              StepLength(+-) and Velocity
+
+            Lateral: As above (fwd or bwd random) with added random
+                     slow changing LateralFraction param
+
+            Rotating: As above except with YawRate
+
+            Combined: ALL changeable values may change!
+                            StepLength
+                            StepVelocity
+                            LateralFraction
+                            YawRate
+
+        NOTE: the RL is solely responsible for modulating Clearance Height
+              and Penetration Depth
+        """
+        if self.mode is ALL:
+            self.which_state()
+
+            if self.current_state == FB:
+                # print("FORWARD/BACKWARD")
+                self.FB()
+            elif self.current_state == LAT:
+                # print("LATERAL")
+                self.LAT()
+            elif self.current_state == ROT:
+                # print("ROTATION")
+                self.ROT()
+            elif self.current_state == COMBI:
+                # print("COMBINED")
+                self.COMBI()
+
+        return self.return_bezier_params()
+
+    def return_bezier_params(self):
+        # First, Clip Everything
+        self.StepLength = np.clip(self.StepLength, self.StepLength_LIMITS[0],
+                                  self.StepLength_LIMITS[1])
+        self.StepVelocity = np.clip(self.StepVelocity,
+                                    self.StepVelocity_LIMITS[0],
+                                    self.StepVelocity_LIMITS[1])
+        self.LateralFraction = np.clip(self.LateralFraction,
+                                       self.LateralFraction_LIMITS[0],
+                                       self.LateralFraction_LIMITS[1])
+        self.YawRate = np.clip(self.YawRate, self.YawRate_LIMITS[0],
+                               self.YawRate_LIMITS[1])
+        self.ClearanceHeight = np.clip(self.ClearanceHeight,
+                                       self.ClearanceHeight_LIMITS[0],
+                                       self.ClearanceHeight_LIMITS[1])
+        self.PenetrationDepth = np.clip(self.PenetrationDepth,
+                                        self.PenetrationDepth_LIMITS[0],
+                                        self.PenetrationDepth_LIMITS[1])
+        # Then, return
+        # FIRST COPY TO AVOID OVERWRITING
+        pos = copy.deepcopy(self.pos)
+        orn = copy.deepcopy(self.orn)
+        StepLength = copy.deepcopy(self.StepLength)
+        LateralFraction = copy.deepcopy(self.LateralFraction)
+        YawRate = copy.deepcopy(self.YawRate)
+        StepVelocity = copy.deepcopy(self.StepVelocity)
+        ClearanceHeight = copy.deepcopy(self.ClearanceHeight)
+        PenetrationDepth = copy.deepcopy(self.PenetrationDepth)
+
+        return pos, orn, StepLength, LateralFraction,\
+            YawRate, StepVelocity,\
+            ClearanceHeight, PenetrationDepth
+
+    def FB(self):
+        """
+        Here, we can modulate StepLength and StepVelocity
+        """
+        # The maximum update amount for these element
+        StepLength_DELTA = self.dt * (self.StepLength_LIMITS[1] -
+                                      self.StepLength_LIMITS[0]) / (6.0)
+        StepVelocity_DELTA = self.dt * (self.StepVelocity_LIMITS[1] -
+                                        self.StepVelocity_LIMITS[0]) / (2.0)
+
+        # Add either positive or negative or zero delta for each
+        # NOTE: 'High' is open bracket ) so the max is 1
+        if self.StepLength < -self.StepLength_LIMITS[0] / 2.0:
+            StepLength_DIRECTION = np.random.randint(-1, 3, 1)[0]
+        elif self.StepLength > self.StepLength_LIMITS[1] / 2.0:
+            StepLength_DIRECTION = np.random.randint(-2, 2, 1)[0]
+        else:
+            StepLength_DIRECTION = np.random.randint(-1, 2, 1)[0]
+        StepVelocity_DIRECTION = np.random.randint(-1, 2, 1)[0]
+
+        # Now, modify modifiable params AND CLIP
+        self.StepLength += StepLength_DIRECTION * StepLength_DELTA
+        self.StepLength = np.clip(self.StepLength, self.StepLength_LIMITS[0],
+                                  self.StepLength_LIMITS[1])
+        self.StepVelocity += StepVelocity_DIRECTION * StepVelocity_DELTA
+        self.StepVelocity = np.clip(self.StepVelocity,
+                                    self.StepVelocity_LIMITS[0],
+                                    self.StepVelocity_LIMITS[1])
+
+    def LAT(self):
+        """
+        Here, we can modulate StepLength and LateralFraction
+        """
+        # The maximum update amount for these element
+        LateralFraction_DELTA = self.dt * (self.LateralFraction_LIMITS[1] -
+                                           self.LateralFraction_LIMITS[0]) / (
+                                               2.0)
+
+        # Add either positive or negative or zero delta for each
+        # NOTE: 'High' is open bracket ) so the max is 1
+        LateralFraction_DIRECTION = np.random.randint(-1, 2, 1)[0]
+
+        # Now, modify modifiable params AND CLIP
+        self.LateralFraction += LateralFraction_DIRECTION * LateralFraction_DELTA
+        self.LateralFraction = np.clip(self.LateralFraction,
+                                       self.LateralFraction_LIMITS[0],
+                                       self.LateralFraction_LIMITS[1])
+
+    def ROT(self):
+        """
+        Here, we can modulate StepLength and YawRate
+        """
+        # The maximum update amount for these element
+        # no dt since YawRate is already mult by dt
+        YawRate_DELTA = (self.YawRate_LIMITS[1] -
+                         self.YawRate_LIMITS[0]) / (2.0)
+
+        # Add either positive or negative or zero delta for each
+        # NOTE: 'High' is open bracket ) so the max is 1
+        YawRate_DIRECTION = np.random.randint(-1, 2, 1)[0]
+
+        # Now, modify modifiable params AND CLIP
+        self.YawRate += YawRate_DIRECTION * YawRate_DELTA
+        self.YawRate = np.clip(self.YawRate, self.YawRate_LIMITS[0],
+                               self.YawRate_LIMITS[1])
+
+    def COMBI(self):
+        """
+        Here, we can modify all the parameters
+        """
+
+        self.FB()
+        self.LAT()
+        self.ROT()

simulation/motor.py

@@ -0,0 +1,98 @@
+"""This file implements an accurate motor model.
+Source: https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/minitaur/envs/motor.py
+"""
+import numpy as np
+
+VOLTAGE_CLIPPING = 8.4
+OBSERVED_TORQUE_LIMIT = 5.7
+MOTOR_VOLTAGE = 7.4
+MOTOR_RESISTANCE = 0.086
+MOTOR_TORQUE_CONSTANT = 0.00954
+MOTOR_VISCOUS_DAMPING = 0.0
+MOTOR_SPEED_LIMIT = 9.5
+
+
+class MotorModel(object):
+    """The accurate motor model, which is based on the physics of DC motors.
+
+  The motor model support two types of control: position control and torque
+  control. In position control mode, a desired motor angle is specified, and a
+  torque is computed based on the internal motor model. When the torque control
+  is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the
+  torque.
+
+  The internal motor model takes the following factors into consideration:
+  pd gains, viscous friction, back-EMF voltage and current-torque profile.
+  """
+    def __init__(self, torque_control_enabled=False, kp=1.2, kd=0):
+        self._torque_control_enabled = torque_control_enabled
+        self._kp = kp
+        self._kd = kd
+        self._resistance = MOTOR_RESISTANCE
+        self._voltage = MOTOR_VOLTAGE
+        self._torque_constant = MOTOR_TORQUE_CONSTANT
+        self._viscous_damping = MOTOR_VISCOUS_DAMPING
+        self._current_table = [0, 10, 20, 30, 40, 50, 60]
+        self._torque_table = [0, 1, 1.9, 2.45, 3.0, 3.25, 3.5]
+
+    def set_voltage(self, voltage):
+        self._voltage = voltage
+
+    def get_voltage(self):
+        return self._voltage
+
+    def set_viscous_damping(self, viscous_damping):
+        self._viscous_damping = viscous_damping
+
+    def get_viscous_dampling(self):
+        return self._viscous_damping
+
+    def convert_to_torque(self, motor_commands, current_motor_angle,
+                          current_motor_velocity):
+        """Convert the commands (position control or torque control) to torque.
+
+    Args:
+      motor_commands: The desired motor angle if the motor is in position
+        control mode. The pwm signal if the motor is in torque control mode.
+      current_motor_angle: The motor angle at the current time step.
+      current_motor_velocity: The motor velocity at the current time step.
+    Returns:
+      actual_torque: The torque that needs to be applied to the motor.
+      observed_torque: The torque observed by the sensor.
+    """
+        if self._torque_control_enabled:
+            pwm = motor_commands
+        else:
+            pwm = (-self._kp * (current_motor_angle - motor_commands) -
+                   self._kd * current_motor_velocity)
+        pwm = np.clip(pwm, -1.0, 1.0)
+        return self._convert_to_torque_from_pwm(pwm, current_motor_velocity)
+
+    def _convert_to_torque_from_pwm(self, pwm, current_motor_velocity):
+        """Convert the pwm signal to torque.
+
+    Args:
+      pwm: The pulse width modulation.
+      current_motor_velocity: The motor velocity at the current time step.
+    Returns:
+      actual_torque: The torque that needs to be applied to the motor.
+      observed_torque: The torque observed by the sensor.
+    """
+        observed_torque = np.clip(
+            self._torque_constant * (pwm * self._voltage / self._resistance),
+            -OBSERVED_TORQUE_LIMIT, OBSERVED_TORQUE_LIMIT)
+
+        # Net voltage is clipped at 50V by diodes on the motor controller.
+        voltage_net = np.clip(
+            pwm * self._voltage -
+            (self._torque_constant + self._viscous_damping) *
+            current_motor_velocity, -VOLTAGE_CLIPPING, VOLTAGE_CLIPPING)
+        current = voltage_net / self._resistance
+        current_sign = np.sign(current)
+        current_magnitude = np.absolute(current)
+
+        # Saturate torque based on empirical current relation.
+        actual_torque = np.interp(current_magnitude, self._current_table,
+                                  self._torque_table)
+        actual_torque = np.multiply(current_sign, actual_torque)
+        return actual_torque, observed_torque

simulation/pybullet_data/__init__.py

@@ -0,0 +1,6 @@
+import os
+
+
+def getDataPath():
+  resdir = os.path.join(os.path.dirname(__file__))
+  return resdir

simulation/pybullet_data/model/spot.urdf

@@ -0,0 +1,710 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from spot.urdf.xacro                | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="spot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <material name="orange">
+    <color rgba="1.0 0.5 0.0 1"/>
+  </material>
+  <material name="black">
+    <color rgba="0.1 0.1 0.1 1"/>
+  </material>
+  <material name="grey">
+    <color rgba="0.6 0.6 0.6 1"/>
+  </material>
+  <!-- Left Leg: prefix is front or rear -->
+  <!--   <xacro:macro name="LeftLeg" params="prefix reflect">
+    <link name="${prefix}_Left_Leg">
+        <visual>
+            <geometry>
+                <box size="${leglen} 0.1 0.2"/>
+            </geometry>
+            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
+            <material name="white"/>
+        </visual>
+        <collision>
+            <geometry>
+                <box size="${leglen} 0.1 0.2"/>
+            </geometry>
+            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
+        </collision>
+        <xacro:default_inertial mass="10"/>
+    </link>
+
+    <joint name="base_to_${prefix}_leg" type="fixed">
+        <parent link="base_link"/>
+        <child link="${prefix}_leg"/>
+        <origin xyz="0 ${reflect*(width+.02)} 0.25" />
+    </joint>
+</xacro:macro> -->
+  <!-- Robot description -->
+  <!-- STATIC Links -->
+  <link name="base_link">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/MAINBODY.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.114 0.2 0.1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.6"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.0025" ixy="0" ixz="0" iyx="0" iyy="0.0011498" iyz="0" izx="0" izy="0" izz="0.0026498"/>
+    </inertial>
+  </link>
+  <!-- m to mm -->
+  <link name="battery">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.15 0.035 -0.022"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/Battery.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.048 0.145 0.026"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.4"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000723366666667" ixy="0" ixz="0" iyx="0" iyy="9.93333333333e-05" iyz="0" izx="0" izy="0" izz="0.000777633333333"/>
+    </inertial>
+  </link>
+  <!-- m to mm -->
+  <link name="chassis_left">
+    <visual>
+      <origin rpy="0 1.57079632679 -1.57079632679" xyz="0.016 0.003 0"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/Chassis_Left_Side.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57079632679 -1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.06 0.15 0.006"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="1.878e-05" ixy="0" ixz="0" iyx="0" iyy="3.03e-06" iyz="0" izx="0" izy="0" izz="2.175e-05"/>
+    </inertial>
+  </link>
+  <!-- m to mm -->
+  <link name="chassis_right">
+    <visual>
+      <origin rpy="0 1.57079632679 1.57079632679" xyz="-0.016 -0.003 0"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/Chassis_Right_Side.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57079632679 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.06 0.15 0.006"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="1.878e-05" ixy="0" ixz="0" iyx="0" iyy="3.03e-06" iyz="0" izx="0" izy="0" izz="2.175e-05"/>
+    </inertial>
+  </link>
+  <!-- m to mm -->
+  <link name="front">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/Front.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0.16 0 0.006"/>
+      <geometry>
+        <box size="0.114 0.045 0.085"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="7.70833333333e-05" ixy="0" ixz="0" iyx="0" iyy="0.000168508333333" iyz="0" izx="0" izy="0" izz="0.000125175"/>
+    </inertial>
+  </link>
+  <!-- m to mm -->
+  <link name="back">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/Back.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="-0.16 0 0.006"/>
+      <geometry>
+        <box size="0.114 0.075 0.085"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000107083333333" ixy="0" ixz="0" iyx="0" iyy="0.000168508333333" iyz="0" izx="0" izy="0" izz="0.000155175"/>
+    </inertial>
+  </link>
+  <!-- m to mm -->
+  <link name="front_bracket">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/Front_Bracket.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0.1 0 0"/>
+      <geometry>
+        <box size="0.025 0.075 0.085"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000107083333333" ixy="0" ixz="0" iyx="0" iyy="6.54166666667e-05" iyz="0" izx="0" izy="0" izz="5.20833333333e-05"/>
+    </inertial>
+  </link>
+  <!-- m to mm -->
+  <link name="back_bracket">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/Back_Bracket.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="-0.1 0 0"/>
+      <geometry>
+        <box size="0.025 0.075 0.085"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000107083333333" ixy="0" ixz="0" iyx="0" iyy="6.54166666667e-05" iyz="0" izx="0" izy="0" izz="5.20833333333e-05"/>
+    </inertial>
+  </link>
+  <!-- m to mm -->
+  <link name="front_left_hip">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.25 -0.0044 -0.067"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_HIP.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0.015 0.01 -0.01"/>
+      <geometry>
+        <box size="0.05 0.065 0.065"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.2"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000140833333333" ixy="0" ixz="0" iyx="0" iyy="0.000112083333333" iyz="0" izx="0" izy="0" izz="0.000112083333333"/>
+    </inertial>
+  </link>
+  <joint name="motor_front_left_hip" type="revolute">
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="0.0915 0.0394 0.022"/>
+    <parent link="front_bracket"/>
+    <child link="front_left_hip"/>
+    <limit effort="1000.0" lower="-1.04" upper="1.04" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="front_left_upper_leg">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.252 -0.05 -0.055"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_UPPER_LEG.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 0.02 -0.05"/>
+      <geometry>
+        <box size="0.035 0.035 0.11"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.13"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000144354166667" ixy="0" ixz="0" iyx="0" iyy="0.000144354166667" iyz="0" izx="0" izy="0" izz="2.65416666667e-05"/>
+    </inertial>
+  </link>
+  <joint name="motor_front_left_upper_leg" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0.0015 0.045 -0.0087"/>
+    <parent link="front_left_hip"/>
+    <child link="front_left_upper_leg"/>
+    <limit effort="1000.0" lower="-1.57079632679" upper="2.59" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="front_left_lower_leg">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.0535"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_LOWER_LEG.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 0 -0.06"/>
+      <geometry>
+        <box size="0.035 0.035 0.12"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="6.51041666667e-05" ixy="0" ixz="0" iyx="0" iyy="6.51041666667e-05" iyz="0" izx="0" izy="0" izz="1.02083333333e-05"/>
+    </inertial>
+  </link>
+  <joint name="motor_front_left_lower_leg" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="-0.005 0.018 -0.109"/>
+    <parent link="front_left_upper_leg"/>
+    <child link="front_left_lower_leg"/>
+    <limit effort="1000.0" lower="-2.9" upper="1.57079632679" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="front_left_foot">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.18"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_FOOT.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.18"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_FOOT.stl" scale="1 1 1"/>
+      </geometry>
+      <contact_coefficients mu="10.1"/>
+    </collision>
+    <inertial>
+      <mass value="0.005"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="3.52083333333e-06" ixy="0" ixz="0" iyx="0" iyy="2.80208333333e-06" iyz="0" izx="0" izy="0" izz="2.80208333333e-06"/>
+    </inertial>
+  </link>
+  <joint name="front_left_leg_foot" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 -0.1265"/>
+    <parent link="front_left_lower_leg"/>
+    <child link="front_left_foot"/>
+  </joint>
+  <link name="back_left_hip">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.25 -0.0044 -0.067"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_HIP.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0.015 0.01 -0.01"/>
+      <geometry>
+        <box size="0.05 0.065 0.065"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.2"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000140833333333" ixy="0" ixz="0" iyx="0" iyy="0.000112083333333" iyz="0" izx="0" izy="0" izz="0.000112083333333"/>
+    </inertial>
+  </link>
+  <joint name="motor_back_left_hip" type="revolute">
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="-0.1365 0.0394 0.022"/>
+    <parent link="back_bracket"/>
+    <child link="back_left_hip"/>
+    <limit effort="1000.0" lower="-1.04" upper="1.04" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="back_left_upper_leg">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.252 -0.05 -0.055"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_UPPER_LEG.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 0.02 -0.05"/>
+      <geometry>
+        <box size="0.035 0.035 0.11"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.13"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000144354166667" ixy="0" ixz="0" iyx="0" iyy="0.000144354166667" iyz="0" izx="0" izy="0" izz="2.65416666667e-05"/>
+    </inertial>
+  </link>
+  <joint name="motor_back_left_upper_leg" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0.0015 0.045 -0.0087"/>
+    <parent link="back_left_hip"/>
+    <child link="back_left_upper_leg"/>
+    <limit effort="1000.0" lower="-1.57079632679" upper="2.59" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="back_left_lower_leg">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.0535"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_LOWER_LEG.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 0 -0.06"/>
+      <geometry>
+        <box size="0.035 0.035 0.12"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="6.51041666667e-05" ixy="0" ixz="0" iyx="0" iyy="6.51041666667e-05" iyz="0" izx="0" izy="0" izz="1.02083333333e-05"/>
+    </inertial>
+  </link>
+  <joint name="motor_back_left_lower_leg" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="-0.005 0.018 -0.109"/>
+    <parent link="back_left_upper_leg"/>
+    <child link="back_left_lower_leg"/>
+    <limit effort="1000.0" lower="-2.9" upper="1.57079632679" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="back_left_foot">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.18"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_FOOT.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.18"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/LEFT_FOOT.stl" scale="1 1 1"/>
+      </geometry>
+      <contact_coefficients mu="10.1"/>
+    </collision>
+    <inertial>
+      <mass value="0.005"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="3.52083333333e-06" ixy="0" ixz="0" iyx="0" iyy="2.80208333333e-06" iyz="0" izx="0" izy="0" izz="2.80208333333e-06"/>
+    </inertial>
+  </link>
+  <joint name="back_left_leg_foot" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 -0.1265"/>
+    <parent link="back_left_lower_leg"/>
+    <child link="back_left_foot"/>
+  </joint>
+  <link name="front_right_hip">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.25 0.074 -0.067"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_HIP.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0.015 -0.01 -0.01"/>
+      <geometry>
+        <box size="0.05 0.065 0.065"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.2"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000140833333333" ixy="0" ixz="0" iyx="0" iyy="0.000112083333333" iyz="0" izx="0" izy="0" izz="0.000112083333333"/>
+    </inertial>
+  </link>
+  <joint name="motor_front_right_hip" type="revolute">
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="0.0915 -0.0394 0.022"/>
+    <parent link="front_bracket"/>
+    <child link="front_right_hip"/>
+    <limit effort="1000.0" lower="-1.04" upper="1.04" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="front_right_upper_leg">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.252 0.12 -0.055"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_UPPER_LEG.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 -0.02 -0.05"/>
+      <geometry>
+        <box size="0.035 0.035 0.11"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.13"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000144354166667" ixy="0" ixz="0" iyx="0" iyy="0.000144354166667" iyz="0" izx="0" izy="0" izz="2.65416666667e-05"/>
+    </inertial>
+  </link>
+  <joint name="motor_front_right_upper_leg" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0.0015 -0.045 -0.0087"/>
+    <parent link="front_right_hip"/>
+    <child link="front_right_upper_leg"/>
+    <limit effort="1000.0" lower="-1.57079632679" upper="2.59" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="front_right_lower_leg">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.0535"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_LOWER_LEG.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 0 -0.06"/>
+      <geometry>
+        <box size="0.035 0.035 0.12"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="6.51041666667e-05" ixy="0" ixz="0" iyx="0" iyy="6.51041666667e-05" iyz="0" izx="0" izy="0" izz="1.02083333333e-05"/>
+    </inertial>
+  </link>
+  <joint name="motor_front_right_lower_leg" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="-0.005 -0.018 -0.109"/>
+    <parent link="front_right_upper_leg"/>
+    <child link="front_right_lower_leg"/>
+    <limit effort="1000.0" lower="-2.9" upper="1.57079632679" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="front_right_foot">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.18"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_FOOT.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.18"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_FOOT.stl" scale="1 1 1"/>
+      </geometry>
+      <contact_coefficients mu="10.1"/>
+    </collision>
+    <inertial>
+      <mass value="0.005"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="3.52083333333e-06" ixy="0" ixz="0" iyx="0" iyy="2.80208333333e-06" iyz="0" izx="0" izy="0" izz="2.80208333333e-06"/>
+    </inertial>
+  </link>
+  <joint name="front_right_leg_foot" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 -0.1265"/>
+    <parent link="front_right_lower_leg"/>
+    <child link="front_right_foot"/>
+  </joint>
+  <link name="back_right_hip">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.25 0.074 -0.067"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_HIP.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0.015 -0.01 -0.01"/>
+      <geometry>
+        <box size="0.05 0.065 0.065"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.2"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000140833333333" ixy="0" ixz="0" iyx="0" iyy="0.000112083333333" iyz="0" izx="0" izy="0" izz="0.000112083333333"/>
+    </inertial>
+  </link>
+  <joint name="motor_back_right_hip" type="revolute">
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="-0.1365 -0.0394 0.022"/>
+    <parent link="back_bracket"/>
+    <child link="back_right_hip"/>
+    <limit effort="1000.0" lower="-1.04" upper="1.04" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="back_right_upper_leg">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.252 0.12 -0.055"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_UPPER_LEG.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 -0.02 -0.05"/>
+      <geometry>
+        <box size="0.035 0.035 0.11"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.13"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="0.000144354166667" ixy="0" ixz="0" iyx="0" iyy="0.000144354166667" iyz="0" izx="0" izy="0" izz="2.65416666667e-05"/>
+    </inertial>
+  </link>
+  <joint name="motor_back_right_upper_leg" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0.0015 -0.045 -0.0087"/>
+    <parent link="back_right_hip"/>
+    <child link="back_right_upper_leg"/>
+    <limit effort="1000.0" lower="-1.57079632679" upper="2.59" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="back_right_lower_leg">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.0535"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_LOWER_LEG.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="0 0 -0.06"/>
+      <geometry>
+        <box size="0.035 0.035 0.12"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="6.51041666667e-05" ixy="0" ixz="0" iyx="0" iyy="6.51041666667e-05" iyz="0" izx="0" izy="0" izz="1.02083333333e-05"/>
+    </inertial>
+  </link>
+  <joint name="motor_back_right_lower_leg" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="-0.005 -0.018 -0.109"/>
+    <parent link="back_right_upper_leg"/>
+    <child link="back_right_lower_leg"/>
+    <limit effort="1000.0" lower="-2.9" upper="1.57079632679" velocity="0.7"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="back_right_foot">
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.18"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_FOOT.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.18"/>
+      <geometry>
+        <mesh filename="package://mini_ros/stl/OpenQuadruped/RIGHT_FOOT.stl" scale="1 1 1"/>
+      </geometry>
+      <contact_coefficients mu="10.1"/>
+    </collision>
+    <inertial>
+      <mass value="0.005"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- Inertial Axes -->
+      <inertia ixx="3.52083333333e-06" ixy="0" ixz="0" iyx="0" iyy="2.80208333333e-06" iyz="0" izx="0" izy="0" izz="2.80208333333e-06"/>
+    </inertial>
+  </link>
+  <joint name="back_right_leg_foot" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 -0.1265"/>
+    <parent link="back_right_lower_leg"/>
+    <child link="back_right_foot"/>
+  </joint>
+  <joint name="base_battery" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.01 0 -0.0225"/>
+    <parent link="base_link"/>
+    <child link="battery"/>
+  </joint>
+  <joint name="base_left" type="fixed">
+    <origin rpy="0 0 0" xyz="0.00 0.052 0"/>
+    <parent link="base_link"/>
+    <child link="chassis_left"/>
+  </joint>
+  <joint name="base_right" type="fixed">
+    <origin rpy="0 0 0" xyz="0.00 -0.052 0"/>
+    <parent link="base_link"/>
+    <child link="chassis_right"/>
+  </joint>
+  <joint name="base_front" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="front"/>
+  </joint>
+  <joint name="base_back" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="back"/>
+  </joint>
+  <joint name="base_front_bracket" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="front_bracket"/>
+  </joint>
+  <joint name="base_back_bracket" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="back_bracket"/>
+  </joint>
+</robot>

simulation/pybullet_data/model/stl/LICENSE.txt

@@ -0,0 +1,3 @@
+Spotmicro - robot dog (http://www.thingiverse.com/thing:3445283) by KDY0523 is licensed under the Creative Commons - Attribution license.
+http://creativecommons.org/licenses/by/3.0/
+

simulation/pybullet_data/model/stl/README.txt

@@ -0,0 +1,92 @@
+                   .:                     :,                                          
+,:::::::: ::`      :::                   :::                                          
+,:::::::: ::`      :::                   :::                                          
+.,,:::,,, ::`.:,   ... .. .:,     .:. ..`... ..`   ..   .:,    .. ::  .::,     .:,`   
+   ,::    :::::::  ::, :::::::  `:::::::.,:: :::  ::: .::::::  ::::: ::::::  .::::::  
+   ,::    :::::::: ::, :::::::: ::::::::.,:: :::  ::: :::,:::, ::::: ::::::, :::::::: 
+   ,::    :::  ::: ::, :::  :::`::.  :::.,::  ::,`::`:::   ::: :::  `::,`   :::   ::: 
+   ,::    ::.  ::: ::, ::`  :::.::    ::.,::  :::::: ::::::::: ::`   :::::: ::::::::: 
+   ,::    ::.  ::: ::, ::`  :::.::    ::.,::  .::::: ::::::::: ::`    ::::::::::::::: 
+   ,::    ::.  ::: ::, ::`  ::: ::: `:::.,::   ::::  :::`  ,,, ::`  .::  :::.::.  ,,, 
+   ,::    ::.  ::: ::, ::`  ::: ::::::::.,::   ::::   :::::::` ::`   ::::::: :::::::. 
+   ,::    ::.  ::: ::, ::`  :::  :::::::`,::    ::.    :::::`  ::`   ::::::   :::::.  
+                                ::,  ,::                               ``             
+                                ::::::::                                              
+                                 ::::::                                               
+                                  `,,`
+
+
+http://www.thingiverse.com/thing:3445283
+Spotmicro - robot dog by KDY0523 is licensed under the Creative Commons - Attribution license.
+http://creativecommons.org/licenses/by/3.0/
+
+# Summary
+
+I designed Spotmicro inspired by the Spotmini of Boston Dynamics.
+
+It works on the basis of the Arduino mega, and if you use a different board, you have to redesign the 'plate' file yourself and print the non-mega file instead of the regular file.
+
+The ultra sonic sensor can be used for mapping or obstacle avoidance.
+
+When you attach the servo horn to the 3D printed parts, you must use the HOTGLUE.
+
+THERE IS NO CODE YET, SO YOU HAVE TO WRITE IT YOURSELF.
+
+Assembly video part 1 : https://youtu.be/03RR-mz2hwA
+
+Assembly video part 2 : https://youtu.be/LV5vvmhwtxM
+                           
+Instagram : https://www.instagram.com/kim.d.yeon/
+
+To make this, you need the following...
+
+_Electronics_
+12 × MG 996 R servo motor
+1 × Arduino Mega
+2 × HC-SR04 Ultrasonic sensor
+1 × HC-06 Bluetooth module
+1 × MPU-6050 Gyro sensor
+1 × I2C 16x2 LCD Module
+1 × Rleil rocker switch RL3-4
+7.4v Battery
+
+_Screws, Nuts and Bearings_
+8 × 'M5×15'
+40 × 'M4×20'
+8 × 'M4×15'
+48 × 'M4 nut'
+4 × 'M3×20'
+28 × 'M3×10'
+16 × 'M3 nut'
+8 × 'F625zz Flange ball bearing'
+
+
+
+Made by Deok-yeon Kim
+
+# Print Settings
+
+Printer Brand: Creality
+Printer: Ender 3
+Infill: 10~20%
+Filament_brand: .
+Filament_color: Yellow, Black, Gray
+Filament_material: PLA, Flexible
+
+# Post-Printing
+
+![Alt text](https://cdn.thingiverse.com/assets/5d/9f/90/dd/a9/complete_1.jpg)
+
+![Alt text](https://cdn.thingiverse.com/assets/e0/6f/f7/86/7d/complete_4.jpg)
+
+![Alt text](https://cdn.thingiverse.com/assets/3d/1a/c2/52/17/complete_front.jpg)
+
+![Alt text](https://cdn.thingiverse.com/assets/fc/ee/68/ac/75/complete_back.jpg)
+
+![Alt text](https://cdn.thingiverse.com/assets/67/d1/49/80/50/complete_top.jpg)
+
+![Alt text](https://cdn.thingiverse.com/assets/13/bf/32/0e/d4/complete_bottom.jpg)
+
+![Alt text](https://cdn.thingiverse.com/assets/93/fc/7f/81/17/frame_3.jpg)
+
+![Alt text](https://cdn.thingiverse.com/assets/4e/0b/38/9c/80/frame-1.jpg)

simulation/pybullet_data/plane.mtl

@@ -0,0 +1,15 @@
+newmtl Material
+  Ns 10.0000
+  Ni 1.5000
+  d 1.0000
+  Tr 0.0000
+  Tf 1.0000 1.0000 1.0000 
+  illum 2
+  Ka 0.0000 0.0000 0.0000
+  Kd 0.5880 0.5880 0.5880
+  Ks 0.0000 0.0000 0.0000
+  Ke 0.0000 0.0000 0.0000
+  map_Ka cube.tga
+  map_Kd checker_blue.png
+  
+  

simulation/pybullet_data/plane.obj

@@ -0,0 +1,18 @@
+# Blender v2.66 (sub 1) OBJ File: ''
+# www.blender.org
+mtllib plane.mtl
+o Plane
+v 15.000000 -15.000000 0.000000
+v 15.000000 15.000000 0.000000
+v -15.000000 15.000000 0.000000
+v -15.000000  -15.000000 0.000000
+
+vt 15.000000 0.000000
+vt 15.000000 15.000000
+vt 0.000000 15.000000
+vt 0.000000 0.000000
+
+usemtl Material
+s off
+f 1/1 2/2 3/3
+f 1/1 3/3 4/4

simulation/pybullet_data/plane.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+  <contact>
+      <lateral_friction value="1"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="plane.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -5"/>
+      <geometry>
+	 	<box size="30 30 10"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

simulation/pybullet_data/plane100.obj

@@ -0,0 +1,22 @@
+# Blender v2.66 (sub 1) OBJ File: ''
+# www.blender.org
+mtllib plane.mtl
+o Plane
+v 100.000000 -100.000000 0.000000
+v 100.000000 100.000000 0.000000
+v -100.000000 100.000000 0.000000
+v -100.000000  -100.000000 0.000000
+
+vt 100.000000 0.000000
+vt 100.000000 100.000000
+vt 0.000000 100.000000
+vt 0.000000 0.000000
+
+
+
+usemtl Material
+s off
+f 1/1 2/2 3/3
+f 1/1 3/3 4/4
+
+

simulation/pybullet_data/plane100.urdf

@@ -0,0 +1,26 @@
+<?xml version="0.0" ?>
+<robot name="floor">
+  <link name="floor">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="plane100.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<box size="200 200 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

simulation/pybullet_data/plane_implicit.urdf

@@ -0,0 +1,31 @@
+<?xml version="1.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+	<contact>
+      <lateral_friction value="0.9"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="plane100.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!--<origin rpy="0 0 0" xyz="0 0 -5"/>-->
+      <geometry>
+	      <plane normal="0 0 1"/>
+	 			<!--<box size="100 100 10"/>-->
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

simulation/pybullet_data/plane_stadium.sdf

@@ -0,0 +1,44 @@
+<sdf version='1.6'>
+	<world name='default'>
+	<gravity>0 0 -9.8</gravity>
+		<model name='floor_obj'>
+			<static>1</static>
+			<pose frame=''>0 0 0 0 0 0</pose>
+			<link name='floor'>
+			<inertial>
+			<mass>0</mass>
+			<inertia>
+			<ixx>0.166667</ixx>
+			<ixy>0</ixy>
+			<ixz>0</ixz>
+			<iyy>0.166667</iyy>
+			<iyz>0</iyz>
+			<izz>0.166667</izz>
+			</inertia>
+			</inertial>
+			<collision name='collision_1'>
+			<geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+			  </collision>
+			<visual name='visual'>
+				<geometry>
+				<mesh>
+					<scale>1 1 1</scale>
+					<uri>plane100.obj</uri>
+				</mesh>
+				</geometry>
+			<material>
+				<ambient>1 1 1 1</ambient>
+				<diffuse>1 1 1 1</diffuse>
+				<specular>.5 .5 .5 1</specular>
+				<emissive>0 0 0 0</emissive>
+			 </material>
+			 </visual>
+			 </link>
+			</model>
+	</world>
+</sdf>

simulation/pybullet_data/plane_transparent.mtl

@@ -0,0 +1,14 @@
+newmtl Material
+  Ns 10.0000
+  Ni 1.5000
+  d 1.0000
+  Tr 0.0000
+  Tf 1.0000 1.0000 1.0000 
+  illum 2
+  Ka 0.0000 0.0000 0.0000
+  Kd 0.5880 0.5880 0.5880
+  Ks 0.0000 0.0000 0.0000
+  Ke 0.0000 0.0000 0.0000
+  map_Kd checker_blue.png
+  
+  

simulation/pybullet_data/plane_transparent.obj

@@ -0,0 +1,18 @@
+# Blender v2.66 (sub 1) OBJ File: ''
+# www.blender.org
+mtllib plane_transparent.mtl
+o Plane
+v 15.000000 -15.000000 0.000000
+v 15.000000 15.000000 0.000000
+v -15.000000 15.000000 0.000000
+v -15.000000  -15.000000 0.000000
+
+vt 15.000000 0.000000
+vt 15.000000 15.000000
+vt 0.000000 15.000000
+vt 0.000000 0.000000
+
+usemtl Material
+s off
+f 1/1 2/2 3/3
+f 1/1 3/3 4/4

simulation/pybullet_data/plane_transparent.urdf

@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+  <contact>
+      <lateral_friction value="1"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="plane_transparent.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 .7"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -5"/>
+      <geometry>
+	 	<box size="30 30 10"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

simulation/pybullet_data/spot.urdf

@@ -0,0 +1,736 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from spot.urdf.xacro                | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="spot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <material name="orange">
+        <color rgba="1.0 0.5 0.0 1" />
+    </material>
+    <material name="black">
+        <color rgba="0.1 0.1 0.1 1" />
+    </material>
+    <material name="grey">
+        <color rgba="0.6 0.6 0.6 1" />
+    </material>
+    <!-- Left Leg: prefix is front or rear -->
+    <!--   <xacro:macro name="LeftLeg" params="prefix reflect">
+    <link name="${prefix}_Left_Leg">
+        <visual>
+            <geometry>
+                <box size="${leglen} 0.1 0.2"/>
+            </geometry>
+            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
+            <material name="white"/>
+        </visual>
+        <collision>
+            <geometry>
+                <box size="${leglen} 0.1 0.2"/>
+            </geometry>
+            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
+        </collision>
+        <xacro:default_inertial mass="10"/>
+    </link>
+
+    <joint name="base_to_${prefix}_leg" type="fixed">
+        <parent link="base_link"/>
+        <child link="${prefix}_leg"/>
+        <origin xyz="0 ${reflect*(width+.02)} 0.25" />
+    </joint>
+</xacro:macro> -->
+    <!-- Robot description -->
+    <!-- STATIC Links -->
+    <link name="base_link">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/MAINBODY.stl" scale="1 1 1" />
+            </geometry>
+            <material name="orange" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 0 0" />
+            <geometry>
+                <box size="0.114 0.2 0.1" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.6" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.0025" ixy="0" ixz="0" iyx="0" iyy="0.0011498" iyz="0" izx="0" izy="0"
+                izz="0.0026498" />
+        </inertial>
+    </link>
+    <!-- m to mm -->
+    <link name="battery">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.15 0.035 -0.022" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/Battery.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 0 0" />
+            <geometry>
+                <box size="0.048 0.145 0.026" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.4" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000723366666667" ixy="0" ixz="0" iyx="0" iyy="9.93333333333e-05" iyz="0"
+                izx="0" izy="0" izz="0.000777633333333" />
+        </inertial>
+    </link>
+    <!-- m to mm -->
+    <link name="chassis_left">
+        <visual>
+            <origin rpy="0 1.57079632679 -1.57079632679" xyz="0.016 0.003 0" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/Chassis_Left_Side.stl"
+                    scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 1.57079632679 -1.57079632679" xyz="0 0 0" />
+            <geometry>
+                <box size="0.06 0.15 0.006" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.01" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="1.878e-05" ixy="0" ixz="0" iyx="0" iyy="3.03e-06" iyz="0" izx="0" izy="0"
+                izz="2.175e-05" />
+        </inertial>
+    </link>
+    <!-- m to mm -->
+    <link name="chassis_right">
+        <visual>
+            <origin rpy="0 1.57079632679 1.57079632679" xyz="-0.016 -0.003 0" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/Chassis_Right_Side.stl"
+                    scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 1.57079632679 1.57079632679" xyz="0 0 0" />
+            <geometry>
+                <box size="0.06 0.15 0.006" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.01" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="1.878e-05" ixy="0" ixz="0" iyx="0" iyy="3.03e-06" iyz="0" izx="0" izy="0"
+                izz="2.175e-05" />
+        </inertial>
+    </link>
+    <!-- m to mm -->
+    <link name="front">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/Front.stl" scale="1 1 1" />
+            </geometry>
+            <material name="orange" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0.16 0 0.006" />
+            <geometry>
+                <box size="0.114 0.045 0.085" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="7.70833333333e-05" ixy="0" ixz="0" iyx="0" iyy="0.000168508333333" iyz="0"
+                izx="0" izy="0" izz="0.000125175" />
+        </inertial>
+    </link>
+    <!-- m to mm -->
+    <link name="back">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/Back.stl" scale="1 1 1" />
+            </geometry>
+            <material name="orange" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="-0.16 0 0.006" />
+            <geometry>
+                <box size="0.114 0.075 0.085" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000107083333333" ixy="0" ixz="0" iyx="0" iyy="0.000168508333333" iyz="0"
+                izx="0" izy="0" izz="0.000155175" />
+        </inertial>
+    </link>
+    <!-- m to mm -->
+    <link name="front_bracket">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/Front_Bracket.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0.1 0 0" />
+            <geometry>
+                <box size="0.025 0.075 0.085" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000107083333333" ixy="0" ixz="0" iyx="0" iyy="6.54166666667e-05" iyz="0"
+                izx="0" izy="0" izz="5.20833333333e-05" />
+        </inertial>
+    </link>
+    <!-- m to mm -->
+    <link name="back_bracket">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.16 0.035 -0.045" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/Back_Bracket.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="-0.1 0 0" />
+            <geometry>
+                <box size="0.025 0.075 0.085" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000107083333333" ixy="0" ixz="0" iyx="0" iyy="6.54166666667e-05" iyz="0"
+                izx="0" izy="0" izz="5.20833333333e-05" />
+        </inertial>
+    </link>
+    <!-- m to mm -->
+    <link name="front_left_hip">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.25 -0.0044 -0.067" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_HIP.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0.015 0.01 -0.01" />
+            <geometry>
+                <box size="0.05 0.065 0.065" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.2" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000140833333333" ixy="0" ixz="0" iyx="0" iyy="0.000112083333333" iyz="0"
+                izx="0" izy="0" izz="0.000112083333333" />
+        </inertial>
+    </link>
+    <joint name="motor_front_left_hip" type="revolute">
+        <axis xyz="1 0 0" />
+        <origin rpy="0 0 0" xyz="0.0915 0.0394 0.022" />
+        <parent link="front_bracket" />
+        <child link="front_left_hip" />
+        <limit effort="1000.0" lower="-1.04" upper="1.04" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="front_left_upper_leg">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.252 -0.05 -0.055" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_UPPER_LEG.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 0.02 -0.05" />
+            <geometry>
+                <box size="0.035 0.035 0.11" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.13" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000144354166667" ixy="0" ixz="0" iyx="0" iyy="0.000144354166667" iyz="0"
+                izx="0" izy="0" izz="2.65416666667e-05" />
+        </inertial>
+    </link>
+    <joint name="motor_front_left_upper_leg" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0.0015 0.045 -0.0087" />
+        <parent link="front_left_hip" />
+        <child link="front_left_upper_leg" />
+        <limit effort="1000.0" lower="-1.57079632679" upper="2.59" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="front_left_lower_leg">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.0535" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_LOWER_LEG.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 0 -0.06" />
+            <geometry>
+                <box size="0.035 0.035 0.12" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.05" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="6.51041666667e-05" ixy="0" ixz="0" iyx="0" iyy="6.51041666667e-05" iyz="0"
+                izx="0" izy="0" izz="1.02083333333e-05" />
+        </inertial>
+    </link>
+    <joint name="motor_front_left_lower_leg" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.005 0.018 -0.109" />
+        <parent link="front_left_upper_leg" />
+        <child link="front_left_lower_leg" />
+        <limit effort="1000.0" lower="-2.9" upper="1.57079632679" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="front_left_foot">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.18" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_FOOT.stl" scale="1 1 1" />
+            </geometry>
+            <material name="orange" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.18" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_FOOT.stl" scale="1 1 1" />
+            </geometry>
+            <contact_coefficients mu="10.1" />
+        </collision>
+        <inertial>
+            <mass value="0.005" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="3.52083333333e-06" ixy="0" ixz="0" iyx="0" iyy="2.80208333333e-06" iyz="0"
+                izx="0" izy="0" izz="2.80208333333e-06" />
+        </inertial>
+    </link>
+    <joint name="front_left_leg_foot" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 -0.1265" />
+        <parent link="front_left_lower_leg" />
+        <child link="front_left_foot" />
+    </joint>
+    <link name="back_left_hip">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.25 -0.0044 -0.067" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_HIP.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0.015 0.01 -0.01" />
+            <geometry>
+                <box size="0.05 0.065 0.065" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.2" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000140833333333" ixy="0" ixz="0" iyx="0" iyy="0.000112083333333" iyz="0"
+                izx="0" izy="0" izz="0.000112083333333" />
+        </inertial>
+    </link>
+    <joint name="motor_back_left_hip" type="revolute">
+        <axis xyz="1 0 0" />
+        <origin rpy="0 0 0" xyz="-0.1365 0.0394 0.022" />
+        <parent link="back_bracket" />
+        <child link="back_left_hip" />
+        <limit effort="1000.0" lower="-1.04" upper="1.04" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="back_left_upper_leg">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.252 -0.05 -0.055" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_UPPER_LEG.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 0.02 -0.05" />
+            <geometry>
+                <box size="0.035 0.035 0.11" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.13" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000144354166667" ixy="0" ixz="0" iyx="0" iyy="0.000144354166667" iyz="0"
+                izx="0" izy="0" izz="2.65416666667e-05" />
+        </inertial>
+    </link>
+    <joint name="motor_back_left_upper_leg" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0.0015 0.045 -0.0087" />
+        <parent link="back_left_hip" />
+        <child link="back_left_upper_leg" />
+        <limit effort="1000.0" lower="-1.57079632679" upper="2.59" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="back_left_lower_leg">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.0535" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_LOWER_LEG.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 0 -0.06" />
+            <geometry>
+                <box size="0.035 0.035 0.12" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.05" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="6.51041666667e-05" ixy="0" ixz="0" iyx="0" iyy="6.51041666667e-05" iyz="0"
+                izx="0" izy="0" izz="1.02083333333e-05" />
+        </inertial>
+    </link>
+    <joint name="motor_back_left_lower_leg" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.005 0.018 -0.109" />
+        <parent link="back_left_upper_leg" />
+        <child link="back_left_lower_leg" />
+        <limit effort="1000.0" lower="-2.9" upper="1.57079632679" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="back_left_foot">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.18" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_FOOT.stl" scale="1 1 1" />
+            </geometry>
+            <material name="orange" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 -0.068 0.18" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/LEFT_FOOT.stl" scale="1 1 1" />
+            </geometry>
+            <contact_coefficients mu="10.1" />
+        </collision>
+        <inertial>
+            <mass value="0.005" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="3.52083333333e-06" ixy="0" ixz="0" iyx="0" iyy="2.80208333333e-06" iyz="0"
+                izx="0" izy="0" izz="2.80208333333e-06" />
+        </inertial>
+    </link>
+    <joint name="back_left_leg_foot" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 -0.1265" />
+        <parent link="back_left_lower_leg" />
+        <child link="back_left_foot" />
+    </joint>
+    <link name="front_right_hip">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.25 0.074 -0.067" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_HIP.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0.015 -0.01 -0.01" />
+            <geometry>
+                <box size="0.05 0.065 0.065" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.2" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000140833333333" ixy="0" ixz="0" iyx="0" iyy="0.000112083333333" iyz="0"
+                izx="0" izy="0" izz="0.000112083333333" />
+        </inertial>
+    </link>
+    <joint name="motor_front_right_hip" type="revolute">
+        <axis xyz="1 0 0" />
+        <origin rpy="0 0 0" xyz="0.0915 -0.0394 0.022" />
+        <parent link="front_bracket" />
+        <child link="front_right_hip" />
+        <limit effort="1000.0" lower="-1.04" upper="1.04" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="front_right_upper_leg">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.252 0.12 -0.055" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_UPPER_LEG.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 -0.02 -0.05" />
+            <geometry>
+                <box size="0.035 0.035 0.11" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.13" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000144354166667" ixy="0" ixz="0" iyx="0" iyy="0.000144354166667" iyz="0"
+                izx="0" izy="0" izz="2.65416666667e-05" />
+        </inertial>
+    </link>
+    <joint name="motor_front_right_upper_leg" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0.0015 -0.045 -0.0087" />
+        <parent link="front_right_hip" />
+        <child link="front_right_upper_leg" />
+        <limit effort="1000.0" lower="-1.57079632679" upper="2.59" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="front_right_lower_leg">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.0535" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_LOWER_LEG.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 0 -0.06" />
+            <geometry>
+                <box size="0.035 0.035 0.12" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.05" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="6.51041666667e-05" ixy="0" ixz="0" iyx="0" iyy="6.51041666667e-05" iyz="0"
+                izx="0" izy="0" izz="1.02083333333e-05" />
+        </inertial>
+    </link>
+    <joint name="motor_front_right_lower_leg" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.005 -0.018 -0.109" />
+        <parent link="front_right_upper_leg" />
+        <child link="front_right_lower_leg" />
+        <limit effort="1000.0" lower="-2.9" upper="1.57079632679" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="front_right_foot">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.18" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_FOOT.stl" scale="1 1 1" />
+            </geometry>
+            <material name="orange" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.18" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_FOOT.stl" scale="1 1 1" />
+            </geometry>
+            <contact_coefficients mu="10.1" />
+        </collision>
+        <inertial>
+            <mass value="0.005" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="3.52083333333e-06" ixy="0" ixz="0" iyx="0" iyy="2.80208333333e-06" iyz="0"
+                izx="0" izy="0" izz="2.80208333333e-06" />
+        </inertial>
+    </link>
+    <joint name="front_right_leg_foot" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 -0.1265" />
+        <parent link="front_right_lower_leg" />
+        <child link="front_right_foot" />
+    </joint>
+    <link name="back_right_hip">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.25 0.074 -0.067" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_HIP.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0.015 -0.01 -0.01" />
+            <geometry>
+                <box size="0.05 0.065 0.065" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.2" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000140833333333" ixy="0" ixz="0" iyx="0" iyy="0.000112083333333" iyz="0"
+                izx="0" izy="0" izz="0.000112083333333" />
+        </inertial>
+    </link>
+    <joint name="motor_back_right_hip" type="revolute">
+        <axis xyz="1 0 0" />
+        <origin rpy="0 0 0" xyz="-0.1365 -0.0394 0.022" />
+        <parent link="back_bracket" />
+        <child link="back_right_hip" />
+        <limit effort="1000.0" lower="-1.04" upper="1.04" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="back_right_upper_leg">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.252 0.12 -0.055" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_UPPER_LEG.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 -0.02 -0.05" />
+            <geometry>
+                <box size="0.035 0.035 0.11" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.13" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="0.000144354166667" ixy="0" ixz="0" iyx="0" iyy="0.000144354166667" iyz="0"
+                izx="0" izy="0" izz="2.65416666667e-05" />
+        </inertial>
+    </link>
+    <joint name="motor_back_right_upper_leg" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0.0015 -0.045 -0.0087" />
+        <parent link="back_right_hip" />
+        <child link="back_right_upper_leg" />
+        <limit effort="1000.0" lower="-1.57079632679" upper="2.59" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="back_right_lower_leg">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.0535" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_LOWER_LEG.stl" scale="1 1 1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="0 0 -0.06" />
+            <geometry>
+                <box size="0.035 0.035 0.12" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.05" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="6.51041666667e-05" ixy="0" ixz="0" iyx="0" iyy="6.51041666667e-05" iyz="0"
+                izx="0" izy="0" izz="1.02083333333e-05" />
+        </inertial>
+    </link>
+    <joint name="motor_back_right_lower_leg" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.005 -0.018 -0.109" />
+        <parent link="back_right_upper_leg" />
+        <child link="back_right_lower_leg" />
+        <limit effort="1000.0" lower="-2.9" upper="1.57079632679" velocity="0.7" />
+        <joint_properties damping="0.0" friction="0.0" />
+    </joint>
+    <link name="back_right_foot">
+        <visual>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.18" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_FOOT.stl" scale="1 1 1" />
+            </geometry>
+            <material name="orange" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 1.57079632679" xyz="-0.247 0.138 0.18" />
+            <geometry>
+                <mesh filename="package://model/stl/OpenQuadruped/RIGHT_FOOT.stl" scale="1 1 1" />
+            </geometry>
+            <contact_coefficients mu="10.1" />
+        </collision>
+        <inertial>
+            <mass value="0.005" />
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <!-- Inertial Axes -->
+            <inertia ixx="3.52083333333e-06" ixy="0" ixz="0" iyx="0" iyy="2.80208333333e-06" iyz="0"
+                izx="0" izy="0" izz="2.80208333333e-06" />
+        </inertial>
+    </link>
+    <joint name="back_right_leg_foot" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 -0.1265" />
+        <parent link="back_right_lower_leg" />
+        <child link="back_right_foot" />
+    </joint>
+    <joint name="base_battery" type="fixed">
+        <origin rpy="0 0 0" xyz="-0.01 0 -0.0225" />
+        <parent link="base_link" />
+        <child link="battery" />
+    </joint>
+    <joint name="base_left" type="fixed">
+        <origin rpy="0 0 0" xyz="0.00 0.052 0" />
+        <parent link="base_link" />
+        <child link="chassis_left" />
+    </joint>
+    <joint name="base_right" type="fixed">
+        <origin rpy="0 0 0" xyz="0.00 -0.052 0" />
+        <parent link="base_link" />
+        <child link="chassis_right" />
+    </joint>
+    <joint name="base_front" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0" />
+        <parent link="base_link" />
+        <child link="front" />
+    </joint>
+    <joint name="base_back" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0" />
+        <parent link="base_link" />
+        <child link="back" />
+    </joint>
+    <joint name="base_front_bracket" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0" />
+        <parent link="base_link" />
+        <child link="front_bracket" />
+    </joint>
+    <joint name="base_back_bracket" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0" />
+        <parent link="base_link" />
+        <child link="back_bracket" />
+    </joint>
+</robot>

simulation/run_simulation.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python
+
+import numpy as np
+import copy
+import sys
+
+sys.path.append("../../..")
+
+from GymEnvs.spot_bezier_env import spotBezierEnv
+from util.gui import GUI
+from raspberry_pi.src.Kinematics.SpotKinematics import SpotModel
+from raspberry_pi.src.GaitGenerator.Bezier import BezierGait
+
+
+class GaitState:
+    def __init__(self) -> None:
+        self.step_length = 0.0
+        self.yaw_rate = 0
+        self.lateral_fraction = 0
+        self.step_velocity = 0.001
+        self.swing_period = 0.2
+        self.clearance_height = 0.045
+        self.penetration_depth = 0.003
+        self.contacts = [False] * 4
+
+        self.target_step_length = 0
+        self.target_yaw_rate = 0
+        self.target_lateral_fraction = 0
+
+    def update_gait_state(self, dt):
+        self.step_length = self.step_length * (1 - dt) + self.target_step_length * dt
+        self.lateral_fraction = (
+            self.lateral_fraction * (1 - dt) + self.target_lateral_fraction * dt
+        )
+        self.yaw_rate = self.yaw_rate * (1 - dt) + self.target_yaw_rate * dt
+
+
+class BodyState:
+    def __init__(self) -> None:
+        self.position = np.array([0, 0, 0])
+        self.rotation = np.array([0, 0, 0])
+        self.worldFeetPositions = {}
+
+
+class Gait:
+    def __init__(
+        self,
+        env: spotBezierEnv,
+        gui: GUI,
+        bodyState: BodyState,
+        gaitState: GaitState,
+        spotModel: SpotModel,
+        bezierGait: BezierGait,
+    ) -> None:
+        self.env = env
+        self.gui = gui
+        self.body_state = bodyState
+        self.gait_state = gaitState
+        self.spot = spotModel
+        self.bezier_gait = bezierGait
+
+        self.state = self.env.reset()
+        self.action = self.env.action_space.sample()
+        self.body_state.worldFeetPositions = copy.deepcopy(self.spot.WorldToFoot)
+
+        self.dt = 0.01
+
+    def step(self):
+        self.gait_state.update_gait_state(self.dt)
+        self.gui.UserInput(self.body_state, self.gait_state)
+        self.gait_state.contacts = self.state[-4:]
+        self.body_state.worldFeetPositions = copy.deepcopy(self.spot.WorldToFoot)
+
+        self.bezier_gait.generate_trajectory(self.body_state, self.gait_state, self.dt)
+
+        self.update_environment()
+
+        self.state, _, done, _ = self.env.step(self.action)
+        if done:
+            print("DONE")
+            return True
+
+    def update_environment(self):
+        joint_angles = self.spot.IK(
+            self.body_state.rotation,
+            self.body_state.position,
+            self.body_state.worldFeetPositions,
+        )
+        self.env.pass_joint_angles(joint_angles.reshape(-1))
+
+
+if __name__ == "__main__":
+    env = spotBezierEnv(
+        render=True,
+        on_rack=False,
+        height_field=False,
+        draw_foot_path=False,
+    )
+    gui = GUI(env.spot.quadruped)
+    bodyState = BodyState()
+    gaitState = GaitState()
+    spot = SpotModel()
+    bezierGait = BezierGait()
+
+    gait = Gait(env, gui, bodyState, gaitState, spot, bezierGait)
+
+    while True:
+        done = gait.step()
+        if done:
+            gait.env.close()
+            break

simulation/simulator.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+
+import numpy as np
+import copy
+
+from .GymEnvs.spot_bezier_env import spotBezierEnv
+from .util.gui import GUI
+from .Kinematics.SpotKinematics import SpotModel
+from .GaitGenerator.Bezier import BezierGait
+
+
+class GaitState:
+    def __init__(self) -> None:
+        self.step_length = 0.0
+        self.yaw_rate = 0
+        self.lateral_fraction = 0
+        self.step_velocity = 0.001
+        self.swing_period = 0.2
+        self.clearance_height = 0.045
+        self.penetration_depth = 0.003
+        self.contacts = [False] * 4
+
+        self.target_step_length = 0
+        self.target_yaw_rate = 0
+        self.target_lateral_fraction = 0
+
+    def update_gait_state(self, dt):
+        self.step_length = self.step_length * (1 - dt) + self.target_step_length * dt
+        self.lateral_fraction = (
+            self.lateral_fraction * (1 - dt) + self.target_lateral_fraction * dt
+        )
+        self.yaw_rate = self.yaw_rate * (1 - dt) + self.target_yaw_rate * dt
+
+
+class BodyState:
+    def __init__(self) -> None:
+        self.position = np.array([0, 0, 0])
+        self.rotation = np.array([0, 0, 0])
+        self.worldFeetPositions = {}
+
+
+class Simulator:
+    def __init__(
+        self,
+        env: spotBezierEnv,
+        gui: GUI,
+        bodyState: BodyState,
+        gaitState: GaitState,
+        spotModel: SpotModel,
+        bezierGait: BezierGait,
+    ) -> None:
+        self.env = env
+        self.gui = gui
+        self.body_state = bodyState
+        self.gait_state = gaitState
+        self.spot = spotModel
+        self.bezier_gait = bezierGait
+
+        self.state = self.env.reset()
+        self.action = self.env.action_space.sample()
+        self.body_state.worldFeetPositions = copy.deepcopy(self.spot.WorldToFoot)
+
+        self.dt = 0.01
+
+    def step(self, model, command):
+        self.gait_state.update_gait_state(self.dt)
+        self.gui.UserInput(self.body_state, self.gait_state)
+        self.gait_state.contacts = self.state[-4:]
+        self.body_state.worldFeetPositions = copy.deepcopy(self.spot.WorldToFoot)
+
+        self.bezier_gait.generate_trajectory(self.body_state, self.gait_state, self.dt)
+
+        self.update_environment()
+
+        self.state, _, done, _ = self.env.step(self.action)
+        if done:
+            print("DONE")
+            return True
+
+    def update_environment(self):
+        joint_angles = self.spot.IK(
+            self.body_state.rotation,
+            self.body_state.position,
+            self.body_state.worldFeetPositions,
+        )
+        self.env.pass_joint_angles(joint_angles.reshape(-1))
+
+
+if __name__ == "__main__":
+    env = spotBezierEnv(
+        render=True,
+        on_rack=False,
+        height_field=False,
+        draw_foot_path=False,
+        env_randomizer=None,
+    )
+    gui = GUI(env.spot.quadruped)
+    bodyState = BodyState()
+    gaitState = GaitState()
+    spot = SpotModel()
+    bezierGait = BezierGait()
+
+    gait = Simulator(env, gui, bodyState, gaitState, spot, bezierGait)
+
+    while True:
+        done = gait.step()
+        if done:
+            gait.env.close()
+            break

simulation/util/action_mapper.py

@@ -0,0 +1,16 @@
+STATIC_ACTIONS_MAP = {
+    'gallop': ('rex_gym/policies/galloping/balanced', 'model.ckpt-20000000'),
+    'walk': ('rex_gym/policies/walking/alternating_legs', 'model.ckpt-16000000'),
+    'standup': ('rex_gym/policies/standup', 'model.ckpt-10000000')
+}
+
+DYNAMIC_ACTIONS_MAP = {
+    'turn': ('rex_gym/policies/turn', 'model.ckpt-16000000')
+}
+
+ACTIONS_TO_ENV_NAMES = {
+    'gallop': 'RexReactiveEnv',
+    'walk': 'RexWalkEnv',
+    'turn': 'RexTurnEnv',
+    'standup': 'RexStandupEnv'
+}

simulation/util/bullet_client.py

@@ -0,0 +1,54 @@
+"""A wrapper for pybullet to manage different clients."""
+from __future__ import absolute_import
+from __future__ import division
+import functools
+import inspect
+import pybullet
+
+
+class BulletClient(object):
+    """A wrapper for pybullet to manage different clients."""
+
+    def __init__(self, connection_mode=None):
+        """Creates a Bullet client and connects to a simulation.
+
+    Args:
+      connection_mode:
+        `None` connects to an existing simulation or, if fails, creates a
+          new headless simulation,
+        `pybullet.GUI` creates a new simulation with a GUI,
+        `pybullet.DIRECT` creates a headless simulation,
+        `pybullet.SHARED_MEMORY` connects to an existing simulation.
+    """
+        self._shapes = {}
+
+        if connection_mode is None:
+            self._client = pybullet.connect(pybullet.SHARED_MEMORY)
+            if self._client >= 0:
+                return
+            else:
+                connection_mode = pybullet.DIRECT
+        self._client = pybullet.connect(connection_mode)
+
+    def __del__(self):
+        """Clean up connection if not already done."""
+        try:
+            pybullet.disconnect(physicsClientId=self._client)
+        except pybullet.error:
+            pass
+
+    def __getattr__(self, name):
+        """Inject the client id into Bullet functions."""
+        attribute = getattr(pybullet, name)
+        if inspect.isbuiltin(attribute):
+            if name not in [
+                "invertTransform",
+                "multiplyTransforms",
+                "getMatrixFromQuaternion",
+                "getEulerFromQuaternion",
+                "computeViewMatrixFromYawPitchRoll",
+                "computeProjectionMatrixFOV",
+                "getQuaternionFromEuler",
+            ]:  # A temporary hack for now.
+                attribute = functools.partial(attribute, physicsClientId=self._client)
+        return attribute

simulation/util/gui.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+
+import pybullet as pb
+import time
+import numpy as np
+import sys
+
+
+class GUI:
+    def __init__(self, quadruped):
+
+        time.sleep(0.5)
+
+        self.cyaw = 0
+        self.cpitch = -7
+        self.cdist = 0.66
+
+        self.xId = pb.addUserDebugParameter("x", -0.10, 0.10, 0.)
+        self.yId = pb.addUserDebugParameter("y", -0.10, 0.10, 0.)
+        self.zId = pb.addUserDebugParameter("z", -0.055, 0.17, 0.)
+        self.rollId = pb.addUserDebugParameter("roll", -np.pi / 4, np.pi / 4, 0.0)
+        self.pitchId = pb.addUserDebugParameter("pitch", -np.pi / 4, np.pi / 4, 0.0)
+        self.yawId = pb.addUserDebugParameter("yaw", -np.pi / 4, np.pi / 4, 0.)
+        self.StepLengthID = pb.addUserDebugParameter("Step Length", -0.1, 0.1, 0.1)
+        self.YawRateId = pb.addUserDebugParameter("Yaw Rate", -2.0, 2.0, 0.)
+        self.LateralFractionId = pb.addUserDebugParameter(
+            "Lateral Fraction", -np.pi / 2.0, np.pi / 2.0, 0.0
+        )
+        self.StepVelocityId = pb.addUserDebugParameter(
+            "Step Velocity", 0.001, 3.0, 0.001
+        )
+        self.SwingPeriodId = pb.addUserDebugParameter("Swing Period", 0.1, 0.4, 0.2)
+
+        self.ClearanceHeightId = pb.addUserDebugParameter(
+            "Clearance Height", 0.0, 0.1, 0.045
+        )
+        self.PenetrationDepthId = pb.addUserDebugParameter(
+            "Penetration Depth", 0.0, 0.05, 0.003
+        )
+
+        self.quadruped = quadruped
+
+    def UserInput(self, bodyState, gaitState):
+
+        quadruped_pos, _ = pb.getBasePositionAndOrientation(self.quadruped)
+        pb.resetDebugVisualizerCamera(cameraDistance=self.cdist,
+                                      cameraYaw=self.cyaw,
+                                      cameraPitch=self.cpitch,
+                                      cameraTargetPosition=quadruped_pos)
+        keys = pb.getKeyboardEvents()
+        # Keys to change camera
+        if keys.get(100):  # D
+            self.cyaw += 1
+        if keys.get(97):  # A
+            self.cyaw -= 1
+        if keys.get(99):  # C
+            self.cpitch += 1
+        if keys.get(102):  # F
+            self.cpitch -= 1
+        if keys.get(122):  # Z
+            self.cdist += .01
+        if keys.get(120):  # X
+            self.cdist -= .01
+        if keys.get(27):  # ESC
+            pb.disconnect()
+            sys.exit()
+
+        # Read Robot Transform from GUI
+        bodyState.position = np.array(
+            [
+                pb.readUserDebugParameter(self.xId),
+                pb.readUserDebugParameter(self.yId),
+                pb.readUserDebugParameter(self.zId),
+            ]
+        )
+        bodyState.rotation = np.array(
+            [
+                pb.readUserDebugParameter(self.rollId),
+                pb.readUserDebugParameter(self.pitchId),
+                pb.readUserDebugParameter(self.yawId),
+            ]
+        )
+        gaitState.target_step_length = pb.readUserDebugParameter(self.StepLengthID)
+        gaitState.target_yaw_rate = pb.readUserDebugParameter(self.YawRateId)
+        gaitState.target_lateral_fraction = pb.readUserDebugParameter(
+            self.LateralFractionId
+        )
+        gaitState.step_velocity = pb.readUserDebugParameter(self.StepVelocityId)
+        gaitState.clearance_height = pb.readUserDebugParameter(self.ClearanceHeightId)
+        gaitState.penetration_depth = pb.readUserDebugParameter(self.PenetrationDepthId)
+        gaitState.swing_period = pb.readUserDebugParameter(self.SwingPeriodId)