Merge branch 'walking-gait' of https://github.com/runeharlyk/SpotMicroESP32-Leika into walking-gait

.gitignore

@@ -1,8 +1,2 @@
-.pio
-.vscode/.browse.c_cpp.db*
-.vscode/c_cpp_properties.json
-.vscode/launch.json
-.vscode/ipch
-data/
-include/secrets.h
-lib/*
+*.pyc
+spot_env

app/package.json

@@ -23,7 +23,6 @@
 		"@typescript-eslint/parser": "^6.20.0",
 		"autoprefixer": "^10.4.17",
 		"cross-env": "^7.0.3",
-		"dotenv": "^16.4.5",
 		"husky": "^9.0.7",
 		"jsdom": "^24.0.0",
 		"lint-staged": "^15.2.0",

app/pnpm-lock.yaml

@@ -49,9 +49,6 @@ devDependencies:
   cross-env:
     specifier: ^7.0.3
     version: 7.0.3
-  dotenv:
-    specifier: ^16.4.5
-    version: 16.4.5
   husky:
     specifier: ^9.0.7
     version: 9.0.7
@@ -1218,11 +1215,6 @@ packages:
       esutils: 2.0.3
     dev: true
 
-  /dotenv@16.4.5:
-    resolution: {integrity: sha512-ZmdL2rui+eB2YwhsWzjInR8LldtZHGDoQ1ugH85ppHKwpUHL7j7rN0Ti9NCnGiQbhaZ11FpR+7ao1dNsmduNUg==}
-    engines: {node: '>=12'}
-    dev: true
-
   /electron-to-chromium@1.4.649:
     resolution: {integrity: sha512-dq/owIaALxZGqWm5RXpKQ4baX6aDC19e2Z16c8SXYN+I71PyEKjbVqQUgm7kcuk8CRqljTKXbolo0XXDjxnh2w==}
     dev: true

app/vite.config.ts

@@ -1,20 +1,21 @@
-import { defineConfig } from 'vite';
+import { defineConfig, loadEnv } from 'vite';
 import { svelte } from '@sveltejs/vite-plugin-svelte';
 import { viteSingleFile } from 'vite-plugin-singlefile';
 import viteCompression from 'vite-plugin-compression';
 import path from 'path';
-import 'dotenv/config';
-
-const embeddedBuild = process.env.VITE_EMBEDDED_BUILD == 'true';
 
 // https://vitejs.dev/config/
-export default defineConfig({
+export default ({ mode }) => {
+	process.env = { ...process.env, ...loadEnv(mode, process.cwd()) };
+	const embeddedBuild = process.env.VITE_EMBEDDED_BUILD == 'true';
+
+	return defineConfig({
 		plugins: [
 			svelte(),
 			...(embeddedBuild ? [viteSingleFile(), viteCompression({ deleteOriginFile: true })] : [])
 		],
 		build: {
-		outDir: embeddedBuild ? '../data' : './build',
+			outDir: embeddedBuild ? '../esp32/data' : './build',
 			emptyOutDir: true
 		},
 		resolve: {
@@ -26,3 +27,4 @@ export default defineConfig({
 			}
 		}
 	});
+};

esp32/.gitignore

@@ -0,0 +1,8 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
+data/
+include/secrets.h
+lib/*

raspberry_pi/run_spot.py

@@ -0,0 +1,40 @@
+# SCRIPT FOR RUNNING THE RPI ROBOT
+
+import time
+from src.spot import Spot
+from src.kinematics.SpotKinematics import SpotModel
+from src.camera.WebCamera import WebCamera
+from src.imu.IMU import IMU
+from src.hardware_interface.HardwareInterface import HardwareInterface
+from src.controller_interface.WebsocketController import WebsocketController
+from src.controller_interface.SharedControllerState import SharedState, shared_state
+
+
+def main():
+    kinematics = SpotModel()
+    camera = WebCamera()
+    imu = IMU()
+    hardware_interface = HardwareInterface()
+    # shared_controller_state = SharedState()
+    shared_controller_state = SharedState()
+    controller_interface = WebsocketController(shared_controller_state)
+
+    spot = Spot(
+        kinematics, 
+        camera, 
+        imu, 
+        hardware_interface, 
+        controller_interface, 
+        shared_controller_state
+    )
+
+    spot.start()
+
+    try:
+        while True:
+            spot.run()
+    except KeyboardInterrupt:
+        spot.stop()
+
+if __name__ == '__main__':
+    main()

raspberry_pi/simulate.py

@@ -0,0 +1,68 @@
+from os import sys
+import time
+
+from src.spot import Spot
+from src.kinematics.SpotKinematics import SpotModel
+from src.camera.WebCamera import WebCamera
+from src.imu.IMU import IMU
+from src.controller_interface.WebsocketController import WebsocketController
+from src.controller_interface.SharedControllerState import SharedState, shared_state
+
+# from src.camera_server import StreamingServerThread
+
+sys.path.append('../')
+
+from simulation.simulator import Simulator
+from simulation.GymEnvs.spot_bezier_env import spotBezierEnv
+from simulation.sensors.camera import PyBulletCamera
+from simulation.sensors.hardware_interface import PyBulletHardwareInterface
+
+def main():
+    env = spotBezierEnv(
+        render=True,
+        on_rack=False,
+        height_field=False,
+        draw_foot_path=False,
+        urdf_root="..\simulation\pybullet_data"
+    )
+
+    kinematics = SpotModel()
+    # camera = WebCamera()
+    frame_queue = Queue(maxsize=10) 
+    camera = PyBulletCamera(env.spot.quadruped, frame_queue=frame_queue)
+    imu = IMU()
+    hardware_interface = PyBulletHardwareInterface(env)
+    # shared_controller_state = SharedState()
+    controller_interface = WebsocketController(shared_state)
+
+    spot = Spot(
+        kinematics, 
+        camera,
+        frame_queue,
+        imu, 
+        hardware_interface, 
+        controller_interface, 
+        shared_state
+    )
+
+    simulator = Simulator(env, spot)
+
+    spot.start()
+
+    last_time = time.time()
+
+    try:
+        while True:
+            contacts, done = simulator.step()
+            spot.gait_state.contacts = contacts
+            spot.run() # time.time() - last_time
+            last_time = time.time()
+            if done:
+                break
+    except KeyboardInterrupt:
+        print("Program was interrupted")
+    spot.stop()
+
+if __name__ == '__main__':
+    main()
+

raspberry_pi/src/Camera/CameraBase.py

@@ -0,0 +1,10 @@
+from abc import abstractmethod
+
+
+class CameraBase():
+    def __init__(self) -> None:
+        pass
+
+    @abstractmethod
+    def update(self) -> None:
+        raise NotImplementedError

raspberry_pi/src/Camera/WebCamera.py

@@ -0,0 +1,20 @@
+import cv2
+from .CameraBase import CameraBase
+
+
+class WebCamera(CameraBase):
+    def __init__(self, camera_id=0):
+        self.cap = cv2.VideoCapture(camera_id)
+        self._last_frame = None
+        super().__init__()
+
+    def get_image(self):
+        self._last_frame
+
+    def update(self) -> None:
+        ret, frame = self.cap.read()
+        if ret:
+            self._last_frame = frame.copy()
+
+    def __del__(self):
+        self.cap.release()

raspberry_pi/src/GaitGenerator/Bezier.py

@@ -0,0 +1,371 @@
+from enum import Enum
+import numpy as np
+
+
+class Phase(Enum):
+    STANCE = 0
+    SWING = 1
+
+
+def TransToRp(T):
+    T = np.array(T)
+    return T[0:3, 0:3], T[0:3, 3]
+
+
+class BezierGait():
+
+    def __init__(self, leg_phases=[0.0, 0.0, 0.5, 0.5], dt=0.01, t_swing=0.2):
+        self.leg_phases = leg_phases
+        self.prev_foot_pos = np.zeros((4, 3))
+
+        self.num_control_points = 11
+
+        self.dt = dt
+        self.time = 0.0
+        self.touch_down_time = 0.0
+        self.last_touch_down_time = 0.0
+
+        # Trajectory Mode
+        self.phase = Phase.SWING
+
+        # Swing Phase value [0, 1] of Reference Foot
+        self.sw_ref = 0.0
+        self.st_ref = 0.0
+        # Whether Reference Foot has Touched Down
+        self.touch_down = False
+
+        # Stance Time
+        self.t_swing = t_swing
+
+        # Reference Leg
+        self.ref_idx = 0
+
+        # Store all leg phases
+        self.phases = self.leg_phases
+
+    def reset(self):
+        self.prev_foot_pos.fill(0)
+        self.time = 0.0
+        self.touch_down_time = 0.0
+        self.last_touch_down_time = 0.0
+        self.phase = Phase.SWING
+        self.sw_ref = 0.0
+        self.st_ref = 0.0
+        self.touch_down = False
+
+    def get_phase(self, index):
+        """Retrieves the phase of an individual leg.
+
+        NOTE modification
+        from original paper:
+
+        if ti < -t_swing:
+           ti += t_stride
+
+        This is to avoid a phase discontinuity if the user selects
+        a Step Length and Velocity combination that causes t_stance > t_swing.
+
+        :param index: the leg's index, used to identify the required
+                      phase lag
+        :param t_stance: the current user-specified stance period
+        :param t_swing: the swing period (constant, class member)
+        :return: Leg Phase, and StanceSwing (bool) to indicate whether
+                 leg is in stance or swing mode
+        """
+        t_stride = self.t_stance + self.t_swing
+        time_index = self.time_index(index, t_stride)
+
+        if time_index < -self.t_swing:
+            time_index += t_stride
+
+        is_stance_phase = time_index >= 0.0 and time_index <= self.t_stance
+        if is_stance_phase:
+            return self.get_stance_phase(time_index, index)
+
+        return self.get_swing_phase(time_index, index)
+
+    def get_stance_phase(self, time_index, index):
+        leg_phase = time_index / float(self.t_stance)
+        if self.t_stance == 0.0:
+            leg_phase = 0.0
+        if index == self.ref_idx:
+            self.phase = Phase.STANCE
+        return leg_phase, Phase.STANCE
+
+    def get_swing_phase(self, time_index, index):
+        leg_phase = 0.0
+        if time_index >= -self.t_swing and time_index < 0.0:
+            leg_phase = min((time_index + self.t_swing) / self.t_swing, 1.0)
+        elif time_index > self.t_stance and time_index <= self.t_stride:
+            leg_phase = min((time_index - self.t_stance) / self.t_swing, 1.0)
+        # Touchdown at End of Swing
+        leg_phase = min(leg_phase, 1.0)
+        if index == self.ref_idx:
+            self.phase = Phase.SWING
+            self.sw_ref = leg_phase
+            if self.sw_ref >= 0.999:
+                self.touch_down = True
+        return leg_phase, Phase.SWING
+
+    def time_index(self, index, t_stride):
+        """Retrieves the time index for the individual leg
+
+        :param index: the leg's index, used to identify the required
+                      phase lag
+        :param t_stride: the total leg movement period (t_stance + t_swing)
+        :return: the leg's time index
+        """
+        # NOTE: for some reason python's having numerical issues w this
+        # setting to 0 for ref leg by force
+        if index == self.ref_idx:
+            self.leg_phases[index] = 0.0
+        return self.last_touch_down_time - self.leg_phases[index] * t_stride
+
+    def update_clock(self, dt):
+        """Increments the Bezier gait generator's internal clock (self.time)
+
+        :param dt: the time step
+                      phase lag
+        :return: the leg's time index
+        """
+        self.t_stride = self.t_stance + self.t_swing
+        self._check_touch_down()
+        self.last_touch_down_time = self.time - self.touch_down_time
+        if self.last_touch_down_time > self.t_stride:
+            self.last_touch_down_time = self.t_stride
+        elif self.last_touch_down_time < 0.0:
+            self.last_touch_down_time = 0.0
+        self.time += dt
+
+        if self.t_stride < self.t_swing + dt:
+            self.time = 0.0
+            self.last_touch_down_time = 0.0
+            self.touch_down_time = 0.0
+            self.sw_ref = 0.0
+
+    def _check_touch_down(self):
+        """Checks whether a reference leg touchdown
+        has occurred, and whether this warrants
+        resetting the touchdown time
+        """
+        if self.sw_ref >= 0.9 and self.touch_down:
+            self.touch_down_time = self.time
+            self.touch_down = False
+            self.sw_ref = 0.0
+
+    def _binomial(self, n, k):
+        return np.math.factorial(n) / (np.math.factorial(k) * np.math.factorial(n - k))
+
+    def _bern_stein_poly(self, t, n, k, point):
+        return point * self._binomial(n, k) * np.power(t, k) * np.power(1 - t, n - k)
+
+    def _bezier_swing(self, phase, L, lateral_fraction, clearance_height=0.04):
+        STEP = np.array(
+            [-L] * 2 + [-L * 1.5] * 3 + [0.0] * 3 + [L * 1.5] * 2 + [L * 1.4, L]
+        )
+        Z = np.array(
+            [0.0] * 2
+            + [clearance_height * 0.9] * 5
+            + [clearance_height * 1.1] * 3
+            + [0.0] * 2
+        )
+        X, Y = STEP * np.cos(lateral_fraction), STEP * np.sin(lateral_fraction)
+        n = self.num_control_points
+
+        stepX = sum(self._bern_stein_poly(phase, n, i, X[i]) for i in range(n))
+        stepY = sum(self._bern_stein_poly(phase, n, i, Y[i]) for i in range(n))
+        stepZ = sum(self._bern_stein_poly(phase, n, i, Z[i]) for i in range(n))
+
+        return stepX, stepY, stepZ
+
+    def sine_stance(self, phase, L, lateral_fraction, penetration_depth=0.00):
+        """Calculates the step coordinates for the Sinusoidal stance period
+
+        :param phase: current trajectory phase
+        :param L: step length
+        :param lateral_fraction: determines how lateral the movement is
+        :param penetration_depth: foot penetration depth during stance phase
+
+        :returns: X,Y,Z Foot Coordinates relative to unmodified body
+        """
+        # moves from +L to -L
+        step = L * (1.0 - 2.0 * phase)
+        stepX = step * np.cos(lateral_fraction)
+        stepY = step * np.sin(lateral_fraction)
+        stepZ = 0.0
+        if L != 0.0:
+            stepZ = -penetration_depth * np.cos((np.pi * (stepX + stepY)) / (2.0 * L))
+
+        return stepX, stepY, stepZ
+
+    def yaw_circle(self, T_bf, index):
+        """ Calculates the required rotation of the trajectory plane
+            for yaw motion
+
+           :param T_bf: default body-to-foot Vector
+           :param index: the foot index in the container
+           :returns: phi_arc, the plane rotation angle required for yaw motion
+        """
+
+        # Foot Magnitude depending on leg type
+        DefaultBodyToFoot_Magnitude = np.sqrt(T_bf[0]**2 + T_bf[1]**2)
+
+        # Rotation Angle depending on leg type
+        DefaultBodyToFoot_Direction = np.arctan2(T_bf[1], T_bf[0])
+
+        # Previous leg coordinates relative to default coordinates
+        g_xyz = self.prev_foot_pos[index] - np.array([T_bf[0], T_bf[1], T_bf[2]])
+
+        # Modulate Magnitude to keep tracing circle
+        g_mag = np.sqrt((g_xyz[0])**2 + (g_xyz[1])**2)
+        th_mod = np.arctan2(g_mag, DefaultBodyToFoot_Magnitude)
+
+        # Angle Traced by Foot for Rotation
+        phi_arc = np.pi / 2.0 + th_mod
+        phi_arc += DefaultBodyToFoot_Direction * 1 if index == 1 or index == 2 else -1
+
+        return phi_arc
+
+    def swing_step(self, phase, gaitState, T_bf, index):
+        """Calculates the step coordinates for the Bezier (swing) period
+        using a combination of forward and rotational step coordinates
+        initially decomposed from user input of
+        L, lateral_fraction and yaw_rate
+
+        :param phase: current trajectory phase
+        :param L: step length
+        :param lateral_fraction: determines how lateral the movement is
+        :param yaw_rate: the desired body yaw rate
+        :param clearance_height: foot clearance height during swing phase
+        :param T_bf: default body-to-foot Vector
+        :param key: indicates which foot is being processed
+        :param index: the foot index in the container
+
+        :returns: Foot Coordinates relative to unmodified body
+        """
+
+        # Yaw foot angle for tangent-to-circle motion
+        phi_arc = self.yaw_circle(T_bf, index)
+
+        # Get Foot Coordinates for Forward Motion
+        X_delta_lin, Y_delta_lin, Z_delta_lin = self._bezier_swing(
+            phase,
+            gaitState.step_length,
+            gaitState.lateral_fraction,
+            gaitState.clearance_height,
+        )
+
+        X_delta_rot, Y_delta_rot, Z_delta_rot = self._bezier_swing(
+            phase, gaitState.yaw_rate, phi_arc, gaitState.clearance_height
+        )
+
+        coord = np.array(
+            [
+                X_delta_lin + X_delta_rot,
+                Y_delta_lin + Y_delta_rot,
+                Z_delta_lin + Z_delta_rot,
+            ]
+        )
+
+        self.prev_foot_pos[index] = coord
+
+        return coord
+
+    def stance_step(self, phase, gaitState, T_bf, index):
+        """Calculates the step coordinates for the Sine (stance) period
+        using a combination of forward and rotational step coordinates
+        initially decomposed from user input of
+        L, lateral_fraction and yaw_rate
+
+        :param phase: current trajectory phase
+        :param gaitState: current gait state
+        :param T_bf: default body-to-foot Vector
+        :param index: the foot index in the container
+
+        :returns: Foot Coordinates relative to unmodified body
+        """
+
+        # Yaw foot angle for tangent-to-circle motion
+        phi_arc = self.yaw_circle(T_bf, index)
+
+        # Get Foot Coordinates for Forward Motion
+        X_delta_lin, Y_delta_lin, Z_delta_lin = self.sine_stance(
+            phase,
+            gaitState.step_length,
+            gaitState.lateral_fraction,
+            gaitState.penetration_depth,
+        )
+
+        X_delta_rot, Y_delta_rot, Z_delta_rot = self.sine_stance(
+            phase, gaitState.yaw_rate, phi_arc, gaitState.penetration_depth
+        )
+
+        coord = np.array([
+            X_delta_lin + X_delta_rot, Y_delta_lin + Y_delta_rot,
+            Z_delta_lin + Z_delta_rot
+        ])
+
+        self.prev_foot_pos[index] = coord
+
+        return coord
+
+    def foot_step(self, gaitState, body_foot, index):
+        """Calculates the step coordinates in either the Bezier or
+        Sine portion of the trajectory depending on the retrieved phase
+
+        :param T_bf: default body-to-foot Vector
+        :param index: the foot index in the container
+
+        :returns: Foot Coordinates relative to unmodified body
+        """
+        leg_phase, foot_phase = self.get_phase(index)
+        stored_phase = leg_phase
+        if foot_phase == Phase.SWING:
+            stored_phase += 1.0
+
+        # Just for keeping track
+        self.phases[index] = stored_phase
+        if foot_phase == Phase.STANCE:
+            return self.stance_step(leg_phase, gaitState, body_foot, index)
+        elif foot_phase == Phase.SWING:
+            return self.swing_step(leg_phase, gaitState, body_foot, index)
+
+    def generate_trajectory(self, bodyState, gaitState, dt):
+        """Calculates the step coordinates for each foot"""
+        gaitState.yaw_rate *= dt
+
+        self.t_stance = 2.0 * abs(gaitState.step_length) / abs(gaitState.step_velocity)
+        if gaitState.step_velocity == 0.0:
+            self.t_stance = 0.0
+            gaitState.step_length = 0.0
+            self.touch_down = False
+            self.time = 0.0
+            self.last_touch_down_time = 0.0
+
+        # Catch infeasible timestep
+        if self.t_stance < dt:
+            self.t_stance = 0.0
+            gaitState.step_length = 0.0
+            self.touch_down = False
+            self.time = 0.0
+            self.last_touch_down_time = 0.0
+            gaitState.yaw_rate = 0.0
+        self.t_stance = min(self.t_stance, 1.3 * self.t_swing)
+
+        if gaitState.contacts[0] == 1 and self.t_stance > dt:
+            self.touch_down = True
+
+        self.update_clock(dt)
+
+        ref_dS = {"FL": 0.0, "FR": 0.5, "BL": 0.5, "BR": 0.0}
+        for i, (key, Tbf_in) in enumerate(bodyState.worldFeetPositions.items()):
+            self.ref_idx = i if key == "FL" else self.ref_idx
+            self.leg_phases[i] = ref_dS[key]
+            _, leg_feet_positions = TransToRp(Tbf_in)
+            step_coord = (
+                self.foot_step(gaitState, leg_feet_positions, i)
+                if self.t_stance > 0.0
+                else np.array([0.0, 0.0, 0.0])
+            )
+            for j in range(3):
+                bodyState.worldFeetPositions[key][j, 3] += step_coord[j]

raspberry_pi/src/IMU/IMU.py

@@ -0,0 +1,9 @@
+import numpy as np
+from src.imu.IMUBase import IMUBase
+
+class IMU(IMUBase):
+    def __init__(self) -> None:
+        pass
+
+    def read_orientation(self):
+        return np.array([1, 0, 0, 0])

raspberry_pi/src/IMU/IMUBase.py

@@ -0,0 +1,6 @@
+class IMUBase():
+    def __init__(self) -> None:
+        pass
+
+    def read_orientation(self):
+        raise NotImplementedError()

raspberry_pi/src/Kinematics/LegKinematics.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python
+# https://www.researchgate.net/publication/320307716_Inverse_Kinematic_Analysis_Of_A_Quadruped_Robot
+import numpy as np
+
+
+class LegIK():
+    def __init__(self,
+                 legtype="RIGHT",
+                 shoulder_length=0.04,
+                 elbow_length=0.1,
+                 wrist_length=0.125,
+                 hip_lim=[-0.548, 0.548],
+                 shoulder_lim=[-2.17, 0.97],
+                 leg_lim=[-0.1, 2.59]):
+        self.legtype = legtype
+        self.shoulder_length = shoulder_length
+        self.elbow_length = elbow_length
+        self.wrist_length = wrist_length
+        self.hip_lim = hip_lim
+        self.shoulder_lim = shoulder_lim
+        self.leg_lim = leg_lim
+
+    def get_domain(self, x, y, z):
+        """
+        Calculates the leg's Domain and caps it in case of a breach
+
+        :param x,y,z: hip-to-foot distances in each dimension
+        :return: Leg Domain D
+        """
+        D = (y**2 + (-z)**2 - self.shoulder_length**2 +
+             (-x)**2 - self.elbow_length**2 - self.wrist_length**2) / (
+                 2 * self.wrist_length * self.elbow_length)
+        if D > 1 or D < -1:
+            # DOMAIN BREACHED
+            # print("---------DOMAIN BREACH---------")
+            D = np.clip(D, -1.0, 1.0)
+            return D
+        else:
+            return D
+
+    def solve(self, xyz_coord):
+        """
+        Generic Leg Inverse Kinematics Solver
+
+        :param xyz_coord: hip-to-foot distances in each dimension
+        :return: Joint Angles required for desired position
+        """
+        x = xyz_coord[0]
+        y = xyz_coord[1]
+        z = xyz_coord[2]
+        D = self.get_domain(x, y, z)
+        if self.legtype == "RIGHT":
+            return self.RightIK(x, y, z, D)
+        else:
+            return self.LeftIK(x, y, z, D)
+
+    def RightIK(self, x, y, z, D):
+        """
+        Right Leg Inverse Kinematics Solver
+
+        :param x,y,z: hip-to-foot distances in each dimension
+        :param D: leg domain
+        :return: Joint Angles required for desired position
+        """
+        wrist_angle = np.arctan2(-np.sqrt(1 - D**2), D)
+        sqrt_component = y**2 + (-z)**2 - self.shoulder_length**2
+        if sqrt_component < 0.0:
+            # print("NEGATIVE SQRT")
+            sqrt_component = 0.0
+        shoulder_angle = -np.arctan2(z, y) - np.arctan2(
+            np.sqrt(sqrt_component), -self.shoulder_length)
+        elbow_angle = np.arctan2(-x, np.sqrt(sqrt_component)) - np.arctan2(
+            self.wrist_length * np.sin(wrist_angle),
+            self.elbow_length + self.wrist_length * np.cos(wrist_angle))
+        joint_angles = np.array([-shoulder_angle, elbow_angle, wrist_angle])
+        return joint_angles
+
+    def LeftIK(self, x, y, z, D):
+        """
+        Left Leg Inverse Kinematics Solver
+
+        :param x,y,z: hip-to-foot distances in each dimension
+        :param D: leg domain
+        :return: Joint Angles required for desired position
+        """
+        wrist_angle = np.arctan2(-np.sqrt(1 - D**2), D)
+        sqrt_component = y**2 + (-z)**2 - self.shoulder_length**2
+        if sqrt_component < 0.0:
+            print("NEGATIVE SQRT")
+            sqrt_component = 0.0
+        shoulder_angle = -np.arctan2(z, y) - np.arctan2(
+            np.sqrt(sqrt_component), self.shoulder_length)
+        elbow_angle = np.arctan2(-x, np.sqrt(sqrt_component)) - np.arctan2(
+            self.wrist_length * np.sin(wrist_angle),
+            self.elbow_length + self.wrist_length * np.cos(wrist_angle))
+        joint_angles = np.array([-shoulder_angle, elbow_angle, wrist_angle])
+        return joint_angles

raspberry_pi/src/Kinematics/LieAlgebra.py

@@ -0,0 +1,182 @@
+#!/usr/bin/env python
+import numpy as np
+
+# NOTE: Code snippets from Modern Robotics at Northwestern University:
+# See https://github.com/NxRLab/ModernRobotics
+
+
+def RpToTrans(R, p):
+    """
+    Converts a rotation matrix and a position vector into homogeneous
+    transformation matrix
+
+    :param R: A 3x3 rotation matrix
+    :param p: A 3-vector
+    :return: A homogeneous transformation matrix corresponding to the inputs
+
+    Example Input:
+        R = np.array([[1, 0,  0],
+                      [0, 0, -1],
+                      [0, 1,  0]])
+        p = np.array([1, 2, 5])
+
+    Output:
+        np.array([[1, 0,  0, 1],
+                  [0, 0, -1, 2],
+                  [0, 1,  0, 5],
+                  [0, 0,  0, 1]])
+    """
+    return np.r_[np.c_[R, p], [[0, 0, 0, 1]]]
+
+
+def TransToRp(T):
+    """
+    Converts a homogeneous transformation matrix into a rotation matrix
+    and position vector
+
+    :param T: A homogeneous transformation matrix
+    :return R: The corresponding rotation matrix,
+    :return p: The corresponding position vector.
+
+    Example Input:
+        T = np.array([[1, 0,  0, 0],
+                      [0, 0, -1, 0],
+                      [0, 1,  0, 3],
+                      [0, 0,  0, 1]])
+
+    Output:
+        (np.array([[1, 0,  0],
+                   [0, 0, -1],
+                   [0, 1,  0]]),
+         np.array([0, 0, 3]))
+    """
+    T = np.array(T)
+    return T[0:3, 0:3], T[0:3, 3]
+
+
+def TransInv(T):
+    """
+    Inverts a homogeneous transformation matrix
+
+    :param T: A homogeneous transformation matrix
+    :return: The inverse of T
+    Uses the structure of transformation matrices to avoid taking a matrix
+    inverse, for efficiency.
+
+    Example input:
+        T = np.array([[1, 0,  0, 0],
+                      [0, 0, -1, 0],
+                      [0, 1,  0, 3],
+                      [0, 0,  0, 1]])
+    Output:
+        np.array([[1,  0, 0,  0],
+                  [0,  0, 1, -3],
+                  [0, -1, 0,  0],
+                  [0,  0, 0,  1]])
+    """
+    R, p = TransToRp(T)
+    Rt = np.array(R).T
+    return np.r_[np.c_[Rt, -np.dot(Rt, p)], [[0, 0, 0, 1]]]
+
+
+def Adjoint(T):
+    """
+    Computes the adjoint representation of a homogeneous transformation
+    matrix
+
+    :param T: A homogeneous transformation matrix
+    :return: The 6x6 adjoint representation [AdT] of T
+
+    Example Input:
+        T = np.array([[1, 0,  0, 0],
+                      [0, 0, -1, 0],
+                      [0, 1,  0, 3],
+                      [0, 0,  0, 1]])
+    Output:
+        np.array([[1, 0,  0, 0, 0,  0],
+                  [0, 0, -1, 0, 0,  0],
+                  [0, 1,  0, 0, 0,  0],
+                  [0, 0,  3, 1, 0,  0],
+                  [3, 0,  0, 0, 0, -1],
+                  [0, 0,  0, 0, 1,  0]])
+    """
+    R, p = TransToRp(T)
+    return np.r_[np.c_[R, np.zeros((3, 3))], np.c_[np.dot(VecToso3(p), R), R]]
+
+
+def VecToso3(omg):
+    """
+    Converts a 3-vector to an so(3) representation
+
+    :param omg: A 3-vector
+    :return: The skew symmetric representation of omg
+
+    Example Input:
+        omg = np.array([1, 2, 3])
+    Output:
+        np.array([[ 0, -3,  2],
+                  [ 3,  0, -1],
+                  [-2,  1,  0]])
+    """
+    return np.array([[0, -omg[2], omg[1]], [omg[2], 0, -omg[0]],
+                     [-omg[1], omg[0], 0]])
+
+
+def RPY(roll, pitch, yaw):
+    """
+    Creates a Roll, Pitch, Yaw Transformation Matrix
+
+    :param roll: roll component of matrix
+    :param pitch: pitch component of matrix
+    :param yaw: yaw component of matrix
+    :return: The transformation matrix
+
+    Example Input:
+        roll = 0.0
+        pitch = 0.0
+        yaw = 0.0
+    Output:
+        np.array([[1, 0, 0, 0],
+                  [0, 1, 0, 0],
+                  [0, 0, 1, 0],
+                  [0, 0, 0, 1]])
+    """
+    Roll = np.array([[1, 0, 0, 0], [0, np.cos(roll), -np.sin(roll), 0],
+                     [0, np.sin(roll), np.cos(roll), 0], [0, 0, 0, 1]])
+    Pitch = np.array([[np.cos(pitch), 0, np.sin(pitch), 0], [0, 1, 0, 0],
+                      [-np.sin(pitch), 0, np.cos(pitch), 0], [0, 0, 0, 1]])
+    Yaw = np.array([[np.cos(yaw), -np.sin(yaw), 0, 0],
+                    [np.sin(yaw), np.cos(yaw), 0, 0], [0, 0, 1, 0],
+                    [0, 0, 0, 1]])
+    return np.matmul(np.matmul(Roll, Pitch), Yaw)
+
+
+def RotateTranslate(rotation, position):
+    """
+    Creates a Transformation Matrix from a Rotation, THEN, a Translation
+
+    :param rotation: pure rotation matrix
+    :param translation: pure translation matrix
+    :return: The transformation matrix
+    """
+    trans = np.eye(4)
+    trans[0, 3] = position[0]
+    trans[1, 3] = position[1]
+    trans[2, 3] = position[2]
+
+    return np.dot(rotation, trans)
+
+
+def TransformVector(xyz_coord, rotation, translation):
+    """
+    Transforms a vector by a specified Rotation THEN Translation Matrix
+
+    :param xyz_coord: the vector to transform
+    :param rotation: pure rotation matrix
+    :param translation: pure translation matrix
+    :return: The transformed vector
+    """
+    xyz_vec = np.append(xyz_coord, 1.0)
+
+    Transformed = np.dot(RotateTranslate(rotation, translation), xyz_vec)
+    return Transformed[:3]

raspberry_pi/src/Kinematics/SpotKinematics.py

@@ -0,0 +1,225 @@
+#!/usr/bin/env python
+
+import numpy as np
+from collections import OrderedDict
+
+from src.kinematics.LegKinematics import LegIK
+from src.kinematics.LieAlgebra import RpToTrans, TransToRp, TransInv, RPY
+
+
+class SpotModel:
+    def __init__(
+        self,
+        shoulder_length=0.055,
+        elbow_length=0.10652,
+        wrist_length=0.145,
+        hip_x=0.23,
+        hip_y=0.075,
+        foot_x=0.23,
+        foot_y=0.185,
+        height=0.20,
+        com_offset=0.016,
+        shoulder_lim=[-0.548, 0.548],
+        elbow_lim=[-2.17, 0.97],
+        wrist_lim=[-0.1, 2.59],
+    ):
+        """
+        Spot Micro Kinematics
+        """
+        # COM offset in x direction
+        self.com_offset = com_offset
+
+        # Leg Parameters
+        self.shoulder_length = shoulder_length
+        self.elbow_length = elbow_length
+        self.wrist_length = wrist_length
+
+        # Leg Vector desired_positions
+
+        # Distance Between Hips
+        # Length
+        self.hip_x = hip_x
+        # Width
+        self.hip_y = hip_y
+
+        # Distance Between Feet
+        # Length
+        self.foot_x = foot_x
+        # Width
+        self.foot_y = foot_y
+
+        # Body Height
+        self.height = height
+
+        # Joint Parameters
+        self.shoulder_lim = shoulder_lim
+        self.elbow_lim = elbow_lim
+        self.wrist_lim = wrist_lim
+
+        # Dictionary to store Leg IK Solvers
+        self.Legs = OrderedDict()
+        self.Legs["FL"] = LegIK(
+            "LEFT",
+            self.shoulder_length,
+            self.elbow_length,
+            self.wrist_length,
+            self.shoulder_lim,
+            self.elbow_lim,
+            self.wrist_lim,
+        )
+        self.Legs["FR"] = LegIK(
+            "RIGHT",
+            self.shoulder_length,
+            self.elbow_length,
+            self.wrist_length,
+            self.shoulder_lim,
+            self.elbow_lim,
+            self.wrist_lim,
+        )
+        self.Legs["BL"] = LegIK(
+            "LEFT",
+            self.shoulder_length,
+            self.elbow_length,
+            self.wrist_length,
+            self.shoulder_lim,
+            self.elbow_lim,
+            self.wrist_lim,
+        )
+        self.Legs["BR"] = LegIK(
+            "RIGHT",
+            self.shoulder_length,
+            self.elbow_length,
+            self.wrist_length,
+            self.shoulder_lim,
+            self.elbow_lim,
+            self.wrist_lim,
+        )
+
+        # Dictionary to store Hip and Foot Transforms
+
+        # Transform of Hip relative to world frame
+        # With Body Centroid also in world frame
+        Rwb = np.eye(3)
+        self.WorldToHip = OrderedDict()
+
+        self.ph_FL = np.array([self.hip_x / 2.0, self.hip_y / 2.0, 0])
+        self.WorldToHip["FL"] = RpToTrans(Rwb, self.ph_FL)
+
+        self.ph_FR = np.array([self.hip_x / 2.0, -self.hip_y / 2.0, 0])
+        self.WorldToHip["FR"] = RpToTrans(Rwb, self.ph_FR)
+
+        self.ph_BL = np.array([-self.hip_x / 2.0, self.hip_y / 2.0, 0])
+        self.WorldToHip["BL"] = RpToTrans(Rwb, self.ph_BL)
+
+        self.ph_BR = np.array([-self.hip_x / 2.0, -self.hip_y / 2.0, 0])
+        self.WorldToHip["BR"] = RpToTrans(Rwb, self.ph_BR)
+
+        # Transform of Foot relative to world frame
+        # With Body Centroid also in world frame
+        self.WorldToFoot = OrderedDict()
+
+        self.pf_FL = np.array([self.foot_x / 2.0, self.foot_y / 2.0, -self.height])
+        self.WorldToFoot["FL"] = RpToTrans(Rwb, self.pf_FL)
+
+        self.pf_FR = np.array([self.foot_x / 2.0, -self.foot_y / 2.0, -self.height])
+        self.WorldToFoot["FR"] = RpToTrans(Rwb, self.pf_FR)
+
+        self.pf_BL = np.array([-self.foot_x / 2.0, self.foot_y / 2.0, -self.height])
+        self.WorldToFoot["BL"] = RpToTrans(Rwb, self.pf_BL)
+
+        self.pf_BR = np.array([-self.foot_x / 2.0, -self.foot_y / 2.0, -self.height])
+        self.WorldToFoot["BR"] = RpToTrans(Rwb, self.pf_BR)
+
+    def HipToFoot(self, orn, pos, T_bf):
+        """
+        Converts a desired position and orientation wrt Spot's
+        home position, with a desired body-to-foot Transform
+        into a body-to-hip Transform, which is used to extract
+        and return the Hip To Foot Vector.
+
+        :param orn: A 3x1 np.array([]) with Spot's Roll, Pitch, Yaw angles
+        :param pos: A 3x1 np.array([]) with Spot's X, Y, Z coordinates
+        :param T_bf: Dictionary of desired body-to-foot Transforms.
+        :return: Hip To Foot Vector for each of Spot's Legs.
+        """
+
+        # Following steps in attached document: SpotBodyIK.
+        # TODO: LINK DOC
+
+        # Only get Rot component
+        Rb, _ = TransToRp(RPY(orn[0], orn[1], orn[2]))
+        pb = pos
+        T_wb = RpToTrans(Rb, pb)
+
+        # Dictionary to store vectors
+        HipToFoot_List = OrderedDict()
+
+        for i, (key, T_wh) in enumerate(self.WorldToHip.items()):
+            # ORDER: FL, FR, FR, BL, BR
+
+            # Extract vector component
+            _, p_bf = TransToRp(T_bf[key])
+
+            # Step 1, get T_bh for each leg
+            T_bh = np.dot(TransInv(T_wb), T_wh)
+
+            # Step 2, get T_hf for each leg
+
+            # VECTOR ADDITION METHOD
+            _, p_bh = TransToRp(T_bh)
+            p_hf0 = p_bf - p_bh
+
+            # TRANSFORM METHOD
+            T_hf = np.dot(TransInv(T_bh), T_bf[key])
+            _, p_hf1 = TransToRp(T_hf)
+
+            # They should yield the same result
+            if p_hf1.all() != p_hf0.all():
+                print("NOT EQUAL")
+
+            p_hf = p_hf1
+
+            HipToFoot_List[key] = p_hf
+
+        return HipToFoot_List
+
+    def IK(self, orn, pos, T_bf):
+        """
+        Uses HipToFoot() to convert a desired position
+        and orientation wrt Spot's home position into a
+        Hip To Foot Vector, which is fed into the LegIK solver.
+
+        Finally, the resultant joint angles are returned
+        from the LegIK solver for each leg.
+
+        :param orn: A 3x1 np.array([]) with Spot's Roll, Pitch, Yaw angles
+        :param pos: A 3x1 np.array([]) with Spot's X, Y, Z coordinates
+        :param T_bf: Dictionary of desired body-to-foot Transforms.
+        :return: Joint angles for each of Spot's joints.
+        """
+
+        # Following steps in attached document: SpotBodyIK.
+        # TODO: LINK DOC
+
+        # Modify x by com offset
+        pos[0] += self.com_offset
+
+        # 4 legs, 3 joints per leg
+        joint_angles = np.zeros((4, 3))
+
+        # print("T_bf: {}".format(T_bf))
+
+        # Steps 1 and 2 of pipeline here
+        HipToFoot = self.HipToFoot(orn, pos, T_bf)
+
+        for i, (key, p_hf) in enumerate(HipToFoot.items()):
+            # ORDER: FL, FR, FR, BL, BR
+
+            # print("LEG: {} \t HipToFoot: {}".format(key, p_hf))
+
+            # Step 3, compute joint angles from T_hf for each leg
+            joint_angles[i, :] = self.Legs[key].solve(p_hf)
+
+        # print("-----------------------------")
+
+        return joint_angles

raspberry_pi/src/State.py

@@ -0,0 +1,58 @@
+import numpy as np
+from enum import Enum
+
+
+# https://github.com/stanfordroboticsclub/StanfordQuadruped/blob/master/src/State.py
+class State:
+    def __init__(self):
+        self.horizontal_velocity = np.array([0.0, 0.0])
+        self.yaw_rate = 0.0
+        self.height = -0.16
+        self.pitch = 0.0
+        self.roll = 0.0
+        self.activation = 0
+        self.behavior_state = BehaviorState.REST
+
+        self.ticks = 0
+        self.foot_locations = np.zeros((3, 4))
+        self.joint_angles = np.zeros((3, 4))
+
+        self.behavior_state = BehaviorState.REST
+
+
+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 BehaviorState(Enum):
+    DEACTIVATED = -1
+    REST = 0
+    TROT = 1
+    HOP = 2
+    FINISHHOP = 3

raspberry_pi/src/camera_server.py

@@ -0,0 +1,64 @@
+from http.server import BaseHTTPRequestHandler, HTTPServer
+from threading import Thread
+from multiprocessing import Process
+import cv2
+import time
+
+
+class StreamingHandler(BaseHTTPRequestHandler):
+    def do_GET(self):
+        if self.path == '/stream.mjpg':
+            self.send_response(200)
+            self.send_header('Content-type', 'multipart/x-mixed-replace; boundary=frame')
+            self.end_headers()
+            try:
+                while True:
+                    if not self.server.frame_queue.empty():
+                        frame = self.server.frame_queue.get()
+
+                        _, jpeg = cv2.imencode('.jpg', frame)
+                        self.wfile.write(b'--frame\r\n')
+                        self.send_header('Content-Type', 'image/jpeg')
+                        self.send_header('Content-Length', len(jpeg))
+                        self.end_headers()
+                        self.wfile.write(jpeg.tobytes())
+                        self.wfile.write(b'\r\n')
+                    time.sleep(0.1)
+            except Exception as e:
+                print(f"Stream stopped: {e}")
+                raise e
+        else:
+            self.send_error(404)
+            self.end_headers()
+
+class StreamingServer(HTTPServer):
+    def __init__(self, server_address, frame_queue):
+        super().__init__(server_address, StreamingHandler)
+        self.frame_queue = frame_queue
+
+class StreamingServerThread:
+    def __init__(self, frame_queue, port=8080):
+        self.frame_queue = frame_queue
+        self.port = port
+        self.server_thread = None
+
+    def run_server(self, frame_queue):
+        address = ('', self.port)
+        server = StreamingServer(address, frame_queue)
+        print(f"Starting server at http://localhost:{self.port}/stream.mjpg")
+        server.serve_forever()
+
+    def start(self):
+        self.server_thread = Thread(target=self.run_server)
+        self.server_thread.daemon = True
+        self.server_thread.start()
+
+    def start_process(self):
+        self.server_process = Process(target=self.run_server, args=(self.frame_queue,))
+        self.server_process.start()
+
+    def stop(self):
+        if self.server_thread:
+            self.server_thread.join()
+        if self.server_process:
+            self.server_process.join()

raspberry_pi/src/controller_interface/ControllerState.py

@@ -0,0 +1,10 @@
+class ControllerState:
+    def __init__(self) -> None:
+        self.command = 0
+        self.estop = 0
+        self.lx = 0
+        self.ly = 0
+        self.rx = 0
+        self.ry = 0
+        self.height = 0
+        self.speed = 0

raspberry_pi/src/controller_interface/SharedControllerState.py

@@ -0,0 +1,18 @@
+import threading
+
+from src.controller_interface.ControllerState import ControllerState 
+
+class SharedState:
+    def __init__(self):
+        self.lock = threading.Lock()
+        self.state = ControllerState()
+
+    def update_state(self, new_state):
+        with self.lock:
+            self.state = new_state
+
+    def get_latest_state(self):
+        with self.lock:
+            return self.state
+
+shared_state = SharedState()

raspberry_pi/src/controller_interface/WebsocketController.py

@@ -0,0 +1,107 @@
+import asyncio
+from enum import Enum
+import struct
+import threading
+import websockets
+import json
+
+from src.controller_interface.ControllerState import ControllerState
+from src.controller_interface.SharedControllerState import shared_state
+
+clients = {}
+
+class Command(Enum):
+    ESTOP = 0
+    CONTROLLER = 1
+
+def get_controller(buffer):
+    buffer = struct.unpack("<8b", buffer)
+    state = ControllerState()
+    state.command = buffer[0]
+    state.estop = buffer[1]
+    state.lx = buffer[2]
+    state.ly = buffer[3]
+    state.rx = buffer[4]
+    state.ry = buffer[5]
+    state.height = buffer[6]
+    state.speed = buffer[7]
+
+    return state
+
+class WebsocketController:
+    def __init__(self, shared_controller_state=shared_state, host="localhost", port=2096):
+        self.shared_controller_state = shared_controller_state
+        self.host = host
+        self.port = port
+        self.loop = None
+        self.thread = None
+
+    async def handle_message(self, websocket, path):
+        client_id = id(websocket)
+        clients[client_id] = {
+            "websocket": websocket,
+        }
+        try:
+            async for message in websocket:
+                if isinstance(message, bytes):
+                    await self.handle_binary_message(clients[client_id], message)
+                else:
+                    await self.handle_json_message(clients[client_id], message)
+        finally:
+            del clients[client_id]
+
+    async def handle_binary_message(self, client, data):
+        message = get_controller(data)
+        shared_state.update_state(message)
+        command = Command(message.command)
+        if command == Command.ESTOP:
+            await client["websocket"].send(
+                json.dumps({"type": "stop", "data": "Servos stopped"})
+            )
+
+        if command == Command.CONTROLLER:
+            await client["websocket"].send(json.dumps({"type": "echo", "data": message.__dict__}))
+
+
+    async def handle_json_message(self, client, message):
+        data = json.loads(message)
+        if data["type"] in ("stop", "mode_change"):
+            client["model"][data["type"]] = data.get("data", False)
+            await client["websocket"].send(
+                json.dumps(
+                    {"type": data["type"], "data": data.get("data", "Servos stopped")}
+                )
+            )
+
+    def start_server(self):
+        asyncio.set_event_loop(self.loop)
+        start_server = websockets.serve(self.handle_message, self.host, self.port)
+        self.loop.run_until_complete(start_server)
+        self.loop.run_forever()
+
+    def start(self):
+        # Create a new event loop in a new thread
+        if not self.thread or not self.thread.is_alive():
+            self.loop = asyncio.new_event_loop()
+            self.thread = threading.Thread(target=self.start_server)
+            self.thread.start()
+
+    def stop(self):
+        if self.loop:
+            self.loop.call_soon_threadsafe(self.loop.stop)
+        if self.thread and self.thread.is_alive():
+            self.thread.join()
+
+if __name__ == "__main__":
+    server = WebsocketController("localhost", 2096)
+    server.run()
+
+    try:
+        # Your main thread logic here
+        print("WebSocket server is running...")
+        # Example: Keep the main thread doing something or waiting
+        while True:
+            pass
+    except KeyboardInterrupt:
+        print("Stopping WebSocket server...")
+        server.stop()

raspberry_pi/src/hardware_interface/HardwareInterface.py

@@ -0,0 +1,10 @@
+from src.hardware_interface.HardwareInterfaceBase import HardwareInterfaceBase
+
+
+class HardwareInterface(HardwareInterfaceBase):
+    def __init__(self):
+        super().__init__()
+
+    def set_actuator_positions(self, joint_angles):
+        pass
+    

raspberry_pi/src/hardware_interface/HardwareInterfaceBase.py

@@ -0,0 +1,11 @@
+from abc import abstractmethod
+
+class HardwareInterfaceBase:
+    def __init__(self):
+        pass
+
+    @abstractmethod
+    def set_actuator_positions(self, joint_angles):
+        raise NotImplementedError
+    
+

raspberry_pi/src/spot.py

@@ -0,0 +1,70 @@
+
+import numpy as np
+import copy
+import sys
+
+from .camera.CameraBase import CameraBase
+from .camera_server import StreamingServerThread
+from .GaitGenerator.Bezier import BezierGait
+from .State import BodyState, GaitState
+
+sys.path.append("../../..")
+
+def map_value(x, from_low, from_high, to_low, to_high):
+    return ((x - from_low) / (from_high - from_low)) * (to_high - to_low) + to_low
+
+class Spot:
+    def __init__(self, kinematic, camera:CameraBase, frame_queue, imu, hardware_interface, controller_interface, shared_controller_state):
+        self.kinematic = kinematic
+        self.camera = camera
+        self.imu = imu
+        self.hardware_interface = hardware_interface
+        self.controller_interface = controller_interface
+        self.shared_controller_state = shared_controller_state
+
+        self.camera_stream = StreamingServerThread(frame_queue)
+
+        self.body_state = BodyState()
+        self.gait_state = GaitState()
+        self.gait_controller = BezierGait()
+
+        self.body_state.worldFeetPositions = copy.deepcopy(self.kinematic.WorldToFoot)
+
+    def start(self):
+        self.controller_interface.start()
+        self.camera_stream.start_process()
+
+    def stop(self):
+        self.controller_interface.stop()
+        self.camera_stream.stop()
+
+    def joint_angles(self):
+        return self.kinematic.IK(
+            self.body_state.rotation,
+            self.body_state.position,
+            self.body_state.worldFeetPositions,
+        )
+    
+    def handle_input(self, controller):
+        self.gait_state.target_step_length = map_value(max(controller.lx, controller.ly, key=abs), -127, 128, -0.1, 0.1)
+        self.gait_state.target_lateral_fraction = map_value(controller.lx, -127, 128, -np.pi / 2.0, np.pi / 2.0)
+        # self.gait_state.target_yaw_rate = map_value(controller.rx, -127, 128, -2.0, 2.0)
+
+    def run(self, dt=.01):
+        self.camera.update()
+        controller = self.shared_controller_state.get_latest_state()
+        self.handle_input(controller)
+        self.gait_state.update_gait_state(dt)
+
+        self.body_state.worldFeetPositions = copy.deepcopy(self.kinematic.WorldToFoot)
+
+        self.gait_controller.generate_trajectory(self.body_state, self.gait_state, dt)
+
+        joint_angles = self.kinematic.IK(
+            self.body_state.rotation,
+            self.body_state.position,
+            self.body_state.worldFeetPositions,
+        )
+        self.hardware_interface.set_actuator_positions(joint_angles.reshape(-1))
+
+

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
+import random
+
+textureId = -1
+
+useProgrammatic = 0
+useTerrainFromPNG = 1
+useDeepLocoCSV = 2
+updateHeightfield = False
+
+heightfieldSource = useProgrammatic
+numHeightfieldRows = 256
+numHeightfieldColumns = 256
+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
+from gym.envs.registration import register
+
+from simulation.entities import spot
+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 gym.envs.registration import register
+from pkg_resources import parse_version
+import pybullet_utils.bullet_client as bullet_client
+
+import simulation.entities.spot as spot
+from simulation.GymEnvs.heightfield import HeightField
+from raspberry_pi.src.kinematics import LieAlgebra as LA
+from simulation.OpenLoopSM.SpotOL import BezierStepper
+
+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/entities/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)