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

.gitignore

@@ -0,0 +1,2 @@
+*.pyc
+spot_env

app/src/components/Controls.svelte

@@ -12,7 +12,7 @@
 	let right: nipplejs.JoystickManager;
 
 	let throttle_timing = 40;
-	let data = new Int8Array(7);
+	let data = new Int8Array($outControllerData.length);
 
 	onMount(() => {
 		left = nipplejs.create({
@@ -46,13 +46,14 @@
 	};
 
 	const updateData = () => {
-		data[0] = 0;
+		data[0] = 1;
-		data[1] = toInt8($input.left.x, -1, 1);
+		data[1] = 0;
-		data[2] = toInt8($input.left.y, -1, 1);
+		data[2] = toInt8($input.left.x, -1, 1);
-		data[3] = toInt8($input.right.x, -1, 1);
+		data[3] = toInt8($input.left.y, -1, 1);
-		data[4] = toInt8($input.right.y, -1, 1);
+		data[4] = toInt8($input.right.x, -1, 1);
-		data[5] = toInt8($input.height, 0, 100);
+		data[5] = toInt8($input.right.y, -1, 1);
-		data[6] = toInt8($input.speed, 0, 100);
+		data[6] = toInt8($input.height, 0, 100);
+		data[7] = toInt8($input.speed, 0, 100);
 
 		outControllerData.set(data);
 	};

app/src/components/Views/Model.svelte

@@ -27,7 +27,8 @@
 
     let settings = {
         'Trace feet':true,
-        'Trace points': 30
+        'Trace points': 30,
+        'Fix camera on robot': true
     }
 
 	onMount(async () => {
@@ -48,6 +49,7 @@
         const visibility = panel.addFolder('Visualization');
         visibility.add(settings, 'Trace feet')
         visibility.add(settings, 'Trace points', 1, 1000, 1)
+        visibility.add(settings, 'Fix camera on robot')
     }
 
 	const cacheModelFiles = async () => {
@@ -142,6 +144,10 @@
 
         renderTraceLines(toes)
 
+        if (settings['Fix camera on robot']) {
+            sceneManager.controls.target = robot.position.clone()
+        }
+
 		robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
 		robot.rotation.z = lerp(robot.rotation.z, degToRad($mpu.heading + 90), 0.1);
 		modelTargetAngles = $servoAngles;

app/src/lib/sceneBuilder.ts

@@ -197,6 +197,7 @@ export default class SceneBuilder {
 
 	public startRenderLoop = () => {
 		this.renderer.setAnimationLoop(() => {
+            this.controls.update();
 			this.renderer.render(this.scene, this.camera);
 			this.handleRobotShadow();
 			if (this.callback) this.callback();

app/src/lib/services/socket-service.ts

@@ -17,10 +17,12 @@ type WebsocketOutData = string | ArrayBufferLike | Blob | ArrayBufferView;
 
 class SocketService {
 	private socket!: WebSocket;
+    private url?:string
 
 	constructor() {}
 
 	public connect(url: string): void {
+        this.url = url
 		this.socket = new WebSocket(url);
 		this.socket.binaryType = 'arraybuffer';
 		this.socket.onopen = () => this.handleConnected();
@@ -50,6 +52,7 @@ class SocketService {
 
 	private handleDisconnected(): void {
 		isConnected.set(false);
+        setTimeout(() => this.connect(this.url as string), 500)
 	}
 
 	private getJsonFromMessage(msg: string): Result<WebSocketJsonMsg, string> {

app/src/lib/stores/model-store.ts

@@ -14,7 +14,7 @@ export type Modes = (typeof modes)[number];
 
 export const mode: Writable<Modes> = writable('idle');
 
-export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0]));
+export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 0, 70, 0]));
 
 export const input: Writable<ControllerInput> = writable({
 	left: { x: 0, y: 0 },

mock/.gitignore

@@ -1 +1,3 @@
-/node_modules
+/node_modules
+__pycache__/
+*.pyc

mock/MotionController.py

@@ -0,0 +1,53 @@
+import copy
+
+
+class GaitState:
+    def __init__(self) -> None:
+        self.step_length = 0.1
+        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 MotionController:
+    def __init__(
+        self,
+        # env: spotBezierEnv,
+        # gui: GUI,
+        # bodyState: BodyState,
+        # gaitState: GaitState,
+        spot_model,
+        gait,
+    ) -> None:
+        self.gait = gait
+        self.gait_state = GaitState()
+        self.spot_model = spot_model
+
+        self.dt = 0.01
+
+    def update_gait_state(self, command):
+        self.gait_state.step_length = abs(command["lx"]) / 255
+
+    def run(self, model, command):
+        self.update_gait_state(command)
+        self.gait_state.contacts = [False] * 4
+        self.body_state.worldFeetPositions = copy.deepcopy(self.spot.WorldToFoot)
+
+        model["transformation"]["world_feet_position"] = self.gait.generate_trajectory(
+            model, self.gait_state, self.dt
+        )

mock/model.py

@@ -0,0 +1,34 @@
+import random
+
+
+model = lambda: {
+    "gait": {
+        "step_length": 0,
+        "yaw_rate": 0,
+        "lateral_fraction": 0,
+        "step_velocity": 0,
+        "swing_period": 0,
+        "clearance_height": 0,
+        "penetration_depth": 0,
+        "contacts": 0,
+    },
+    "transformation": {
+        "world_position": [0, 0, 0],
+        "position": [0, 0, 0],
+        "rotation": [0, 0, 0],
+        "world_feet_positions": {},
+    },
+    "sensors": {
+        "mpu": {
+            "x": 0,
+            "y": 0,
+            "z": 0,
+        },
+        "battery": {
+            "voltage": round(random.uniform(7.6, 8.2), 2),
+            "ampere": round(random.uniform(0.2, 3), 2),
+        },
+    },
+    "logs": ["[2023-02-05 10:00:00] Booting up"],
+    "settings": {"useMetric": True},
+}

mock/server.js

@@ -209,13 +209,14 @@ const updateAngles = (angles) => {
 
 const bufferToController = (buffer) => {
   return {
-    stop: buffer[0],
+    command: buffer[0],
-    lx: buffer[1],
+    stop: buffer[1],
-    ly: buffer[2],
+    lx: buffer[2],
-    rx: buffer[3],
+    ly: buffer[3],
-    ry: buffer[4],
+    rx: buffer[4],
-    h: buffer[5],
+    ry: buffer[5],
-    s: buffer[6],
+    h: buffer[6],
+    s: buffer[7],
   };
 };
 
@@ -276,15 +277,31 @@ const stand = (client) => {
 // https://www.hindawi.com/journals/cin/2016/9853070/
 
 const step = (model, controller, tick) => {
-  const y1 = -100 * Math.sin(-0.05 * tick) - 150;
+  const arc_height = controller.h;
-  const y2 = -100 * Math.sin(-0.05 * tick + Math.PI) - 150;
+  const speed = (controller.s + 128) / 255 / 4;
-  const x1 = Math.abs((tick % 120) - 60) - 60;
+
+  const T_stride_s = 500 / 1000;
+  const overlay = 10 / 100;
+
+  const T_stance_s = T_stride_s * (0.5 + overlay);
+  const T_swing_s = T_stride_s * (0.5 - overlay);
+
+  const x = Math.abs((tick % 200) - 100);
+
+  const y1 = Math.min(
+    Math.max(-arc_height * Math.sin(-speed * tick) - 150, -200),
+    -100
+  );
+  const y2 = Math.min(
+    Math.max(-arc_height * Math.sin(-speed * tick + Math.PI) - 150, -200),
+    -100
+  );
   const Lp = [
     // -50  is minimum
     [100, y1, 100, 1],
     [100, y2, -100, 1],
-    [-100, y2, 100, 1],
+    [-65, y2, 100, 1],
-    [-100, y1, -100, 1],
+    [-65, y1, -100, 1],
   ];
 
   model.servos.angles = kinematic
@@ -336,7 +353,7 @@ const handelController = (ws, buffer) => {
 };
 
 const handleBufferMessage = (ws, buffer) => {
-  if (buffer.length === 6) {
+  if (buffer.length === 8) {
     handelController(ws, buffer);
   }
 };

mock/server.py

@@ -0,0 +1,100 @@
+import asyncio
+from enum import Enum
+import json
+import sys
+import websockets
+from model import model
+from MotionController import GaitState
+from simulator.GaitGenerator.Bezier import BezierGait
+from simulator.GymEnvs.spot_bezier_env import spotBezierEnv
+from simulator.Kinematics.SpotKinematics import SpotModel
+from simulator.util.gui import GUI
+from simulator.simulator import BodyState, Simulator
+import struct
+
+sys.path.append("./simulator/GymEnvs")
+
+clients = {}
+
+env = spotBezierEnv(
+    render=True,
+    on_rack=False,
+    height_field=False,
+    draw_foot_path=False,
+    env_randomizer=None,
+)
+gui = GUI(env.spot.quadruped)
+bodyState = BodyState()
+gaitState = GaitState()
+spot = SpotModel()
+bezierGait = BezierGait()
+simulator = Simulator()
+
+class Command(Enum):
+    ESTOP = 0
+    CONTROLLER = 1
+
+
+def get_controller(buffer):
+    buffer = struct.unpack("<8b", buffer)
+    return {
+        "command": buffer[0],
+        "estop": buffer[1],
+        "lx": buffer[2],
+        "ly": buffer[3],
+        "rx": buffer[4],
+        "ry": buffer[5],
+        "height": buffer[6],
+        "speed": buffer[7],
+    }
+
+
+async def handle_binary_message(client, data):
+    message = get_controller(data)
+    command = Command(message["command"])
+    if command == Command.ESTOP:
+        client["model"]["running"] = False
+        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}))
+
+
+async def handle_json_message(client, message):
+    data = json.loads(message)
+    client = client["clientState"]
+    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")}
+            )
+        )
+
+
+async def handle_message(websocket, path):
+    client_id = id(websocket)
+    clients[client_id] = {
+        "clientState": model(),
+        "websocket": websocket,
+    }
+    try:
+        async for message in websocket:
+            if isinstance(message, bytes):
+                await handle_binary_message(clients[client_id], message)
+            else:
+                await handle_json_message(clients[client_id], message)
+    finally:
+        del clients[client_id]
+
+
+async def main():
+    async with websockets.serve(handle_message, "localhost", 2096):
+        print("Server starting")
+        await asyncio.Future()
+
+
+if __name__ == "__main__":
+    asyncio.run(main())

mock/simulator/.gitignore

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

mock/simulator/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]

mock/simulator/GymEnvs/spot_bezier_env.py

@@ -0,0 +1,273 @@
+""" This file implements the gym environment of SpotMicro with Bezier Curve.
+"""
+
+import math
+import time
+import gym
+import numpy as np
+import pybullet
+import pybullet_data
+from gym import spaces
+from gym.utils import seeding
+from pkg_resources import parse_version
+from .. import spot
+import pybullet_utils.bullet_client as bullet_client
+from gym.envs.registration import register
+from ..spot_gym_env import spotGymEnv
+from ..spot_env_randomizer import SpotEnvRandomizer
+
+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,
+        env_randomizer=SpotEnvRandomizer(),
+        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,
+            env_randomizer=env_randomizer,
+            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

mock/simulator/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

mock/simulator/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]

mock/simulator/Kinematics/SpotKinematics.py

@@ -0,0 +1,224 @@
+#!/usr/bin/env python
+
+import numpy as np
+from .LegKinematics import LegIK
+from .LieAlgebra import RpToTrans, TransToRp, TransInv, RPY, TransformVector
+from collections import OrderedDict
+
+
+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

mock/simulator/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()

mock/simulator/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

mock/simulator/env_tester.py

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

mock/simulator/heightfield.py

@@ -0,0 +1,140 @@
+"""
+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 math
+import time
+
+textureId = -1
+
+useProgrammatic = 0
+useTerrainFromPNG = 1
+useDeepLocoCSV = 2
+updateHeightfield = False
+
+heightfieldSource = useProgrammatic
+numHeightfieldRows = 256
+numHeightfieldColumns = 256
+import random
+random.seed(10)
+
+
+class HeightField():
+    def __init__(self):
+        self.hf_id = 0
+        self.terrainShape = 0
+        self.heightfieldData = [0] * numHeightfieldRows * numHeightfieldColumns
+
+    def _generate_field(self, env, heightPerturbationRange=0.08):
+
+        env.pybullet_client.setAdditionalSearchPath(pd.getDataPath())
+
+        env.pybullet_client.configureDebugVisualizer(
+            env.pybullet_client.COV_ENABLE_RENDERING, 0)
+        heightPerturbationRange = heightPerturbationRange
+        if heightfieldSource == useProgrammatic:
+            for j in range(int(numHeightfieldColumns / 2)):
+                for i in range(int(numHeightfieldRows / 2)):
+                    height = random.uniform(0, heightPerturbationRange)
+                    self.heightfieldData[2 * i +
+                                         2 * j * numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + 1 +
+                                         2 * j * numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + (2 * j + 1) *
+                                         numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + 1 + (2 * j + 1) *
+                                         numHeightfieldRows] = height
+
+            terrainShape = env.pybullet_client.createCollisionShape(
+                shapeType=env.pybullet_client.GEOM_HEIGHTFIELD,
+                meshScale=[.07, .07, 1.6],
+                heightfieldTextureScaling=(numHeightfieldRows - 1) / 2,
+                heightfieldData=self.heightfieldData,
+                numHeightfieldRows=numHeightfieldRows,
+                numHeightfieldColumns=numHeightfieldColumns)
+            terrain = env.pybullet_client.createMultiBody(0, terrainShape)
+            env.pybullet_client.resetBasePositionAndOrientation(
+                terrain, [0, 0, 0.0], [0, 0, 0, 1])
+            env.pybullet_client.changeDynamics(terrain,
+                                               -1,
+                                               lateralFriction=1.0)
+
+        if heightfieldSource == useDeepLocoCSV:
+            terrainShape = env.pybullet_client.createCollisionShape(
+                shapeType=env.pybullet_client.GEOM_HEIGHTFIELD,
+                meshScale=[.5, .5, 2.5],
+                fileName="heightmaps/ground0.txt",
+                heightfieldTextureScaling=128)
+            terrain = env.pybullet_client.createMultiBody(0, terrainShape)
+            env.pybullet_client.resetBasePositionAndOrientation(
+                terrain, [0, 0, 0], [0, 0, 0, 1])
+            env.pybullet_client.changeDynamics(terrain,
+                                               -1,
+                                               lateralFriction=1.0)
+
+        if heightfieldSource == useTerrainFromPNG:
+            terrainShape = env.pybullet_client.createCollisionShape(
+                shapeType=env.pybullet_client.GEOM_HEIGHTFIELD,
+                meshScale=[.05, .05, 1.8],
+                fileName="heightmaps/wm_height_out.png")
+            textureId = env.pybullet_client.loadTexture(
+                "heightmaps/gimp_overlay_out.png")
+            terrain = env.pybullet_client.createMultiBody(0, terrainShape)
+            env.pybullet_client.changeVisualShape(terrain,
+                                                  -1,
+                                                  textureUniqueId=textureId)
+            env.pybullet_client.resetBasePositionAndOrientation(
+                terrain, [0, 0, 0.1], [0, 0, 0, 1])
+            env.pybullet_client.changeDynamics(terrain,
+                                               -1,
+                                               lateralFriction=1.0)
+
+        self.hf_id = terrainShape
+        self.terrainShape = terrainShape
+        print("TERRAIN SHAPE: {}".format(terrainShape))
+
+        env.pybullet_client.changeVisualShape(terrain,
+                                              -1,
+                                              rgbaColor=[1, 1, 1, 1])
+
+        env.pybullet_client.configureDebugVisualizer(
+            env.pybullet_client.COV_ENABLE_RENDERING, 1)
+
+    def UpdateHeightField(self, heightPerturbationRange=0.08):
+        if heightfieldSource == useProgrammatic:
+            for j in range(int(numHeightfieldColumns / 2)):
+                for i in range(int(numHeightfieldRows / 2)):
+                    height = random.uniform(
+                        0, heightPerturbationRange)  # +math.sin(time.time())
+                    self.heightfieldData[2 * i +
+                                         2 * j * numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + 1 +
+                                         2 * j * numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + (2 * j + 1) *
+                                         numHeightfieldRows] = height
+                    self.heightfieldData[2 * i + 1 + (2 * j + 1) *
+                                         numHeightfieldRows] = height
+            #GEOM_CONCAVE_INTERNAL_EDGE may help avoid getting stuck at an internal (shared) edge of the triangle/heightfield.
+            #GEOM_CONCAVE_INTERNAL_EDGE is a bit slower to build though.
+            #flags = p.GEOM_CONCAVE_INTERNAL_EDGE
+            flags = 0
+            self.terrainShape = p.createCollisionShape(
+                shapeType=p.GEOM_HEIGHTFIELD,
+                flags=flags,
+                meshScale=[.05, .05, 1],
+                heightfieldTextureScaling=(numHeightfieldRows - 1) / 2,
+                heightfieldData=self.heightfieldData,
+                numHeightfieldRows=numHeightfieldRows,
+                numHeightfieldColumns=numHeightfieldColumns,
+                replaceHeightfieldIndex=self.terrainShape)

mock/simulator/mini_ros/CMakeLists.txt

@@ -0,0 +1,248 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mini_ros)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-Wall -Wextra -pipe -Wno-psabi)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  message_generation
+  message_runtime
+  roscpp
+  rospy
+  rostest
+  sensor_msgs
+  visualization_msgs
+)
+
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+# Install Python Modules
+catkin_python_setup()
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  MiniCmd.msg
+  JoyButtons.msg
+  IMUdata.msg
+  ContactData.msg
+  AgentData.msg
+  JointAngles.msg
+  JointPulse.msg
+)
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  CalibServo.srv
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+INCLUDE_DIRS include
+LIBRARIES ${PROJECT_NAME}
+CATKIN_DEPENDS geometry_msgs message_generation message_runtime roscpp rospy sensor_msgs visualization_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Eigen3_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/${PROJECT_NAME}/spot.cpp
+  src/${PROJECT_NAME}/teleop.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(spot_sm src/spot_sm.cpp)
+add_executable(teleop_node src/teleop_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+set_target_properties(spot_sm PROPERTIES OUTPUT_NAME spot_sm PREFIX "")
+set_target_properties(teleop_node PROPERTIES OUTPUT_NAME teleop_node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(spot_sm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(teleop_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(
+spot_sm
+# Eigen3::Eigen
+${PROJECT_NAME}
+${catkin_LIBRARIES}
+)
+
+target_link_libraries(
+teleop_node
+# Eigen3::Eigen
+${PROJECT_NAME}
+${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+install(PROGRAMS
+  src/spot_pybullet_interface
+  src/spot_real_interface
+  src/servo_calibration
+  src/sensor_interface
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+install(TARGETS spot_sm teleop_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/${PROJECT_NAME}_test.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
+
+# if (CATKIN_ENABLE_TESTING)
+#     catkin_add_gtest(${PROJECT_NAME}_test tests/${PROJECT_NAME}_test.cpp)
+#     target_link_libraries(${PROJECT_NAME}_test ${catkin_Libraries} gtest_main ${PROJECT_NAME})
+# endif()

mock/simulator/mini_ros/config/control.yaml

@@ -0,0 +1,34 @@
+spot:
+  joint_states_controller:
+      type: joint_state_controller/JointStateController
+      publish_rate: 50
+
+  joint_group_position_controller:
+      type: effort_controllers/JointTrajectoryController
+      joints:
+          - motor_front_left_hip
+          - motor_front_left_upper_leg
+          - motor_front_left_lower_leg
+          - motor_front_right_hip
+          - motor_front_right_upper_leg
+          - motor_front_right_lower_leg
+          - motor_back_left_hip
+          - motor_back_left_upper_leg
+          - motor_back_left_lower_leg
+          - motor_back_right_hip
+          - motor_back_right_upper_leg
+          - motor_back_right_lower_leg
+
+      gains:
+        motor_front_left_hip        : {p: 180, d: 0.9, i: 20}
+        motor_front_left_upper_leg  : {p: 180, d: 0.9, i: 20}
+        motor_front_left_lower_leg  : {p: 180, d: 0.9, i: 20}
+        motor_front_right_hip       : {p: 180, d: 0.9, i: 20}
+        motor_front_right_upper_leg : {p: 180, d: 0.9, i: 20}
+        motor_front_right_lower_leg : {p: 180, d: 0.9, i: 20}
+        motor_back_left_hip         : {p: 180, d: 0.9, i: 20}
+        motor_back_left_upper_leg   : {p: 180, d: 0.9, i: 20}
+        motor_back_left_lower_leg   : {p: 180, d: 0.9, i: 20}
+        motor_back_right_hip        : {p: 180, d: 0.9, i: 20}
+        motor_back_right_upper_leg  : {p: 180, d: 0.9, i: 20}
+        motor_back_right_lower_leg  : {p: 180, d: 0.9, i: 20}

mock/simulator/mini_ros/config/joy_params.yaml

@@ -0,0 +1,12 @@
+# Right Joystick (Push U/D L/R)
+STEPLENGTH_SCALE: 0.05
+# Left Joystick (Push U/D)
+Z_SCALE_CTRL: 0.15
+# Right Joystick (Push U/D L/R) | Left Joystick (Push L/R)
+RPY_SCALE: 0.785
+# Lower Bumpers (Step Velocity [Left Lowers | Right Raises])
+SV_SCALE: 0.05
+# Arrow Pad (U/D for Clearance Height | L/R for Penetration Depth)
+CHPD_SCALE: 0.0005
+# Left Joystick (L/R)
+YAW_SCALE: 1.25

mock/simulator/mini_ros/config/policy_params.yaml

@@ -0,0 +1,14 @@
+# AGENT PARAMS - SCALING and FILTERING
+# Clearance Height and Penetration Depth
+CD_SCALE: 0.05
+# Step Length and Velocity
+SLV_SCALE: 0.05
+# Residuals
+RESIDUALS_SCALE: 0.015
+# Body Height Modulation
+Z_SCALE: 0.035
+# Exponential Filter Amplitude
+alpha: 0.7
+# Added this to avoid filtering residuals
+# -1 for all
+actions_to_filter: 14

mock/simulator/mini_ros/config/spot_params.yaml

@@ -0,0 +1,39 @@
+# MEASURE
+# COM to Shoulder
+shoulder_length: 0.055
+# Shoulder to Elbow
+elbow_length: 0.10652
+# Elbow to Wrist
+wrist_length: 0.145
+# Forward Hip Separation
+hip_x: 0.23
+# Lateral Hip Separation
+hip_y: 0.075
+
+# ADJUSTABLE
+# Stance Length
+foot_x: 0.23
+# Stance Width
+foot_y: 0.185
+# Stance Height
+height: 0.20
+# Adjust for balanced walk
+com_offset: 0.0
+
+# Time Step
+dt: 0.001
+
+# Swing Period (lower is faster)
+Tswing: 0.2
+SwingPeriod_LIMITS: [0.1, 0.3]
+
+# Step Velocity (Using very low value as my main form of speed control is swing period)
+BaseStepVelocity: 0.001
+
+# Foot Clearance Height
+BaseClearanceHeight: 0.035
+ClearanceHeight_LIMITS: [0.0, 0.04]
+
+# Foot Penetration Depth
+BasePenetrationDepth: 0.003
+PenetrationDepth_LIMITS: [0.0, 0.02]

mock/simulator/mini_ros/include/mini_ros/spot.hpp

@@ -0,0 +1,83 @@
+#ifndef SPOT_INCLUDE_GUARD_HPP
+#define SPOT_INCLUDE_GUARD_HPP
+/// \file
+/// \brief Spots library which contains control functionality for Spot Mini Mini.
+#include <vector>
+#include <ros/ros.h>
+
+namespace spot
+{
+
+    /// \brief approximately compare two floating-point numbers using
+    ///        an absolute comparison
+    /// \param d1 - a number to compare
+    /// \param d2 - a second number to compare
+    /// \param epsilon - absolute threshold required for equality
+    /// \return true if abs(d1 - d2) < epsilon
+    /// Note: the fabs function in <cmath> (c++ equivalent of math.h) will
+    /// be useful here
+    // constexpr are all define in .hpp
+    // constexpr allows fcn to be run at compile time and interface with 
+    // static_assert tests.
+    // Note high default epsilon since using controller
+    constexpr bool almost_equal(double d1, double d2, double epsilon=1.0e-1)
+    {
+        if (fabs(d1 - d2) < epsilon)
+        {
+            return true;
+        } else {
+            return false;
+        }
+    }
+
+    enum Motion {Go, Stop};
+    enum Movement {Stepping, Viewing};
+
+    // \brief Struct to store the commanded type of motion, velocity and rate
+    struct SpotCommand
+    {
+        Motion motion = Stop;
+        Movement movement = Viewing;
+        double x_velocity = 0.0;
+        double y_velocity = 0.0;
+        double rate = 0.0;
+        double roll = 0.0;
+        double pitch = 0.0;
+        double yaw = 0.0;
+        double z = 0.0;
+        double faster = 0.0;
+        double slower = 0.0;
+    };
+
+    // \brief Spot class responsible for high-level motion commands
+    class Spot
+    {
+
+    public:
+        // \brief Constructor for Spot class
+        Spot();
+
+        // \brief updates the type and velocity of motion to be commanded to the Spot
+        // \param vx: linear velocity (x)
+        // \param vy: linear velocity (y)
+        // \param z: robot height
+        // \param w: angular velocity
+        // \param wx: step height increase
+        // \param wy: step height decrease
+        void update_command(const double & vx, const double & vy, const double & z,
+                            const double & w, const double & wx, const double & wy);
+
+        // \brief changes the commanded motion from Forward/Backward to Left/Right or vice-versa
+        void switch_movement();
+
+        // \brief returns the Spot's current command (Motion, v,w) for external use
+        // \returns SpotCommand
+        SpotCommand return_command();
+
+    private:
+        SpotCommand cmd;
+    };
+    
+}
+
+#endif

mock/simulator/mini_ros/include/mini_ros/teleop.hpp

@@ -0,0 +1,89 @@
+#ifndef TELEOP_INCLUDE_GUARD_HPP
+#define TELEOP_INCLUDE_GUARD_HPP
+/// \file
+/// \brief Teleoperation Library that converts Joystick commands to motion
+#include <vector>
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/Joy.h>
+#include "mini_ros/JoyButtons.h"
+
+namespace tele
+{
+
+    // \brief Teleop class responsible for convertick Joystick commands into linear and angular velocity
+    class Teleop
+    {
+    public:
+        // \brief Teleop constructor that defines the axes used for control and sets their scaling factor
+        // \param linear_x: joystick axis assigned to linear velocity (x)
+        // \param linear_y: joystick axis assigned to linear velocity (y)
+        // \param linear_z: joystick axis assigned to robot height [overloading]
+        // \param angular: joystick axis assigned to angular velocity
+        // \param l_scale: scaling factor for linear velocity
+        // \param a_scale: scaling factor for angular velocity
+        // \param LB: left bottom bumper axis
+        // \param RB: right bottom bumper axis
+        // \param B_scale: scaling factor for bottom bumpers
+        // \param LT: left top bumper button
+        // \param RT: right top bumper button
+        // \param UD: up/down key on arrow pad
+        // \param LR: left/right key on arrow pad
+        // \param sw: button for switch_trigger
+        // \param es: button for ESTOP
+        Teleop(const int & linear_x, const int & linear_y, const int & linear_z,
+               const int & angular, const double & l_scale, const double & a_scale,
+               const int & LB, const int & RB, const int & B_scale, const int & LT,
+               const int & RT, const int & UD, const int & LR,
+               const int & sw, const int & es);
+
+        // \brief Takes a Joy messages and converts it to linear and angular velocity (Twist)
+        // \param joy: sensor_msgs describing Joystick inputs
+        void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
+
+        // \brief returns the  most recently commanded Twist
+        // \returns: Twist
+        geometry_msgs::Twist return_twist();
+
+        // \brief returns a boolean indicating whether the movement switch trigger has been pressed
+        // \returns: switch_trigger(bool)
+        bool return_trigger();
+
+        // \brief returns whether the E-STOP has been pressed
+        // \returns: ESTOP(bool)
+        bool return_estop();
+
+        /// \brief returns other joystick buttons triggers, arrow pad etc)
+        mini_ros::JoyButtons return_buttons();
+
+    private:
+        // AXES ON JOYSTICK
+        int linear_x_ = 0;
+        int linear_y_ = 0;
+        int linear_z_ = 0;
+        int angular_= 0;
+        int RB_ = 0;
+        int LB_ = 0;
+        // BUTTONS ON JOYSTICK
+        int sw_ = 0;
+        int es_ = 0;
+        int RT_ = 0;
+        int LT_ = 0;
+        int UD_ = 0;
+        int LR_ = 0;
+        // AXIS SCALES
+        double l_scale_, a_scale_, B_scale_;
+        // TWIST
+        geometry_msgs::Twist twist;
+        // TRIGGERS
+        bool switch_trigger = false;
+        bool ESTOP = false;
+        int updown = 0;
+        int leftright = 0;
+        bool left_bump = false;
+        bool right_bump = false;
+    };
+    
+}
+
+#endif

mock/simulator/mini_ros/launch/controller.launch

@@ -0,0 +1,16 @@
+<launch>
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find mini_ros)/config/control.yaml" command="load"/>
+
+  <!-- Load the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" ns="/spot" args="joint_group_position_controller joint_states_controller"/>
+
+
+  <!-- convert joint states to TF transforms for rviz, etc -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
+    <remap from="/joint_states" to="/spot/joint_states" />
+  </node>
+
+</launch>

mock/simulator/mini_ros/launch/spot_calibrate.launch

@@ -0,0 +1,11 @@
+<launch>
+<!-- Sensor Calibration Node -->
+<node name="servo_calibration" pkg="mini_ros" type="servo_calibration" output="screen"/>
+
+<!-- Sensor Interface Node -->
+<node pkg="rosserial_python" type="serial_node.py" name="serial_node">
+    <param name="port" value="/dev/ttyS0"/> <!-- HW Serial -->
+    <param name="baud" value="500000"/>  <!-- must match Teensy -->
+  </node>
+
+</launch>

mock/simulator/mini_ros/launch/spot_gazebo.launch

@@ -0,0 +1,24 @@
+<launch>
+  <!-- Load the urdf into the parameter server from the xacro file-->
+  <param name="robot_description" command="xacro '$(find mini_ros)/urdf/spot.urdf.xacro'" />
+
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+
+  <!-- We reuse the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="paused" value="true"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="headless" value="$(arg headless)"/>
+  </include>
+
+  <!-- Spawn the urdf -->
+  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -x 0.0 -y 0.0 -z 0.26 -model spot" respawn="false" output="screen"/>
+
+  <include file="$(find mini_ros)/launch/controller.launch"/>
+
+</launch>

mock/simulator/mini_ros/launch/spot_move.launch

@@ -0,0 +1,41 @@
+<launch>
+ <!-- Minitaur State Machine Node-->
+  <node name="spot_sm" pkg="mini_ros" type="spot_sm" output="screen">
+    <param name="frequency" value="200.0" type="double"/>
+  </node>
+
+ <!-- Joystick Node -->
+  <node respawn="true" pkg="joy"
+        type="joy_node" name="spot_joy" >
+    <param name="dev" type="string" value="/dev/input/js0" />
+    <param name="deadzone" value="0.05" />
+  </node>
+
+ <!-- Teleop Node -->
+ <node name="spot_teleop" pkg="mini_ros" type="teleop_node" output="screen">
+  <param name="frequency" value="200.0" type="double"/>
+  <param name="axis_linear_x" value="4" type="int"/>
+  <param name="axis_linear_y" value="3" type="int"/>
+  <param name="axis_linear_z" value="1" type="int"/>
+  <param name="axis_angular" value="0" type="int"/>
+  <param name="scale_linear" value="1.0" type="double"/>
+  <param name="scale_angular" value="1.0" type="double"/>
+  <param name="button_switch" value="0" type="int"/>
+  <param name="button_estop" value="1" type="int"/>
+</node>
+
+<!-- Policy Node -->
+<node name="spot_pybullet" pkg="mini_ros" type="spot_pybullet_interface" output="screen"/>
+
+
+<!-- NOTE:
+        Setting Up Joystick:
+          - Get Number (you will see something like jsX): ls /dev/input/
+          - Make available to ROS: sudo chmod a+rw /dev/input/jsX
+          - Make sure <param name="dev" type="string" value="/dev/input/jsX" /> is correct in launch
+
+        You can ignore this msg: [ERROR] [1591631380.406690714]: Couldn't open joystick force feedback!
+        It just means your controller is missing some functionality, but this package doesn't use it. 
+-->
+
+</launch>

mock/simulator/mini_ros/launch/spot_real.launch

@@ -0,0 +1,60 @@
+<launch>
+ <!-- Minitaur State Machine Node-->
+  <node name="spot_sm" pkg="mini_ros" type="spot_sm" output="screen">
+    <param name="frequency" value="200.0" type="double"/>
+  </node>
+
+ <!-- Joystick Node -->
+  <node respawn="true" pkg="joy"
+        type="joy_node" name="spot_joy" >
+    <param name="dev" type="string" value="/dev/input/js0" />
+    <param name="deadzone" value="0.005" />
+  </node>
+
+ <!-- Teleop Node -->
+ <node name="spot_teleop" pkg="mini_ros" type="teleop_node" output="screen">
+  <param name="frequency" value="200.0" type="double"/>
+  <param name="axis_linear_x" value="4" type="int"/>
+  <param name="axis_linear_y" value="3" type="int"/>
+  <param name="axis_linear_z" value="1" type="int"/>
+  <param name="axis_angular" value="0" type="int"/>
+  <param name="scale_linear" value="1.0" type="double"/>
+  <param name="scale_angular" value="1.0" type="double"/>
+  <param name="button_switch" value="0" type="int"/>
+  <param name="button_estop" value="1" type="int"/>
+</node>
+
+<!-- Policy Node -->
+<arg name="agent_num" default="0" doc="Agent Number for ARS (GMBC) Policy. Default (0) Loads no Agent. Policy numbers start at 9 and increase by 10. E.G: 9...19...29..."/> 
+<param name="agent_num" value="$(eval arg('agent_num'))" />
+
+<!-- Spot Params -->
+<rosparam command="load" file="$(find mini_ros)/config/spot_params.yaml" />
+
+<!-- Policy Params -->
+<rosparam command="load" file="$(find mini_ros)/config/policy_params.yaml" />
+
+<!-- Joystick Params -->
+<rosparam command="load" file="$(find mini_ros)/config/joy_params.yaml" />
+
+<!-- the above is equivalent to $(eval agent_num) but I left it in to acknowledge that both options exist -->
+<node name="spot_real" pkg="mini_ros" type="spot_real_interface" output="screen"/>
+
+<!-- Sensor Interface Node -->
+<node pkg="rosserial_python" type="serial_node.py" name="serial_node">
+    <param name="port" value="/dev/ttyS0"/> <!-- HW Serial -->
+    <param name="baud" value="500000"/>  <!-- must match Teensy -->
+  </node>
+
+
+<!-- NOTE:
+        Setting Up Joystick:
+          - Get Number (you will see something like jsX): ls /dev/input/
+          - Make available to ROS: sudo chmod a+rw /dev/input/jsX
+          - Make sure <param name="dev" type="string" value="/dev/input/jsX" /> is correct in launch
+
+        You can ignore this msg: [ERROR] [1591631380.406690714]: Couldn't open joystick force feedback!
+        It just means your controller is missing some functionality, but this package doesn't use it. 
+-->
+
+</launch>

mock/simulator/mini_ros/launch/view_spot.launch

@@ -0,0 +1,21 @@
+<launch>
+  <!-- This launchfile loads a differential drive robot into RViz, whose parameters are set
+       and can be modified in diff_params.yaml -->
+
+  <!-- load the urdf into the parameter server from the xacro file-->
+  <param name="robot_description" command="xacro '$(find mini_ros)/urdf/spot.urdf.xacro'" />
+
+  <!-- The robot_state_publisher reads the urdf from /robot_description parameter
+       and listens to joint information on the /joint_states topic -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+
+  <!-- The joint state publisher will be launched with a gui, read the urdf from /robot_description
+       and publish the joint values on /joint_states. Optional launch using use_jsp_gui:=1 --> 
+  <arg name="use_jsp_gui" default="True" doc="Launch the joint_state_publisher gui to publish joint angles"/>
+  <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>
+  <!-- <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/> -->
+
+  <!-- rviz will enable us to see the robot. REQUIRED NODE - All other nodes terminate when RViz closes -->
+  <node name="rviz" pkg="rviz" type="rviz" required="True" args="-d $(find mini_ros)/rviz/spot.rviz"/>
+
+</launch>

mock/simulator/mini_ros/msg/AgentData.msg

@@ -0,0 +1,14 @@
+float32 action0
+float32 action1
+float32 action2
+float32 action3
+float32 action4
+float32 action5
+float32 action6
+float32 action7
+float32 action8
+float32 action9
+float32 action10
+float32 action11
+float32 action12
+float32 action13

mock/simulator/mini_ros/msg/ContactData.msg

@@ -0,0 +1,4 @@
+bool FL
+bool FR
+bool BL
+bool BR

mock/simulator/mini_ros/msg/IMUdata.msg

@@ -0,0 +1,8 @@
+float32 roll
+float32 pitch
+float32 acc_x
+float32 acc_y
+float32 acc_z
+float32 gyro_x
+float32 gyro_y
+float32 gyro_z

mock/simulator/mini_ros/msg/JointAngles.msg

@@ -0,0 +1,36 @@
+# FL
+# Shoulder or x
+float32 fls
+# Elbow or y
+float32 fle
+# Wrist or z
+float32 flw
+
+# FR
+# Shoulder or x
+float32 frs
+# Elbow or y
+float32 fre
+# Wrist or z
+float32 frw
+
+# BL
+# Shoulder or x
+float32 bls
+# Elbow or y
+float32 ble
+# Wrist or z
+float32 blw
+
+# BR
+# Shoulder or x
+float32 brs
+# Elbow or y
+float32 bre
+# Wrist or z
+float32 brw
+
+# Move Type (for servo smoothness)
+# step is 0
+# view is 1
+bool step_or_view

mock/simulator/mini_ros/msg/JointPulse.msg

@@ -0,0 +1,2 @@
+int8 servo_num
+int32 servo_pulse

mock/simulator/mini_ros/msg/JoyButtons.msg

@@ -0,0 +1,4 @@
+int8 updown
+int8 leftright
+int8 left_bump
+int8 right_bump

mock/simulator/mini_ros/msg/MiniCmd.msg

@@ -0,0 +1,24 @@
+# motion type: Go, Stop
+string motion
+
+# movement type: Stepping, Viewing
+string movement
+
+# linear velocity
+float32 x_velocity
+float32 y_velocity
+
+# angular rate
+float32 rate
+
+# viewing params
+float32 roll
+float32 pitch
+float32 yaw
+
+# robot height
+float32 z
+
+# Step adjust
+float32 faster
+float32 slower

mock/simulator/mini_ros/package.xml

@@ -0,0 +1,83 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mini_ros</name>
+  <version>0.0.0</version>
+  <description>Controls Minitaur Robot using Trained Policies in Pybullet and IRL</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="mauricerahme2020@u.northwestern.edu">mori</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>MIT</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/mini_ros</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>message_runtime</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>visualization_msgs</build_export_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>hector_gazebo_plugins</exec_depend>
+  <exec_depend>hector_sensors_description</exec_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

mock/simulator/mini_ros/policies/mini_ars_Backward_policy

@@ -0,0 +1,67 @@
+(iars_lib.ars
+Policy
+p0
+(dp1
+S'state_dim'
+p2
+I8
+sS'learning_rate'
+p3
+F0.02
+sS'num_deltas'
+p4
+I16
+sS'seed'
+p5
+I0
+sS'episode_steps'
+p6
+I1000
+sS'theta'
+p7
+cnumpy.core.multiarray
+_reconstruct
+p8
+(cnumpy
+ndarray
+p9
+(I0
+tp10
+S'b'
+p11
+tp12
+Rp13
+(I1
+(I8
+I8
+tp14
+cnumpy
+dtype
+p15
+(S'f8'
+p16
+I0
+I1
+tp17
+Rp18
+(I3
+S'<'
+p19
+NNNI-1
+I-1
+I0
+tp20
+bI00
+S'\xb0\xec\x8c\xb5\xb5\'\xc6\xbf\x96k\xf6\xfa\xb7i\xc0?@p&!33\xdf?\x93P\xd3\x97Iv\xb7?\x03\xa5P\xe1\xa1\xae\xc7\xbf\xac\xa5\\[\xa8\x9a\xd2?\x99\x9b\x19l\xe4\xa5\xb0\xbf\x91\xbe}\xb8\xd5\xe4\xc5?!\x19\x91k\xfc=\xd0\xbf\xf1\x9e\x8a\xa1\xf0 \xca?\xa5\xd0\x1a\xe4\x81\xa6\xbd\xbf\x10\xd6\x9a\x8c\x8f"\xd7??\xca[\\\x15\xaa\xd2?\xdc$,QX\x90\xbd?>\xe4H\x1a\x168\xb2?\xcdb\xef\xd9\xca\x1e\xd4\xbf\x0eG=\xf3\x17\x08\xe0?\x0b1xT\x00d\xd6\xbf\x97q\xaaQ\xfb\xba\xa0?\xff\r\x81\x84\xf2\x9e\xaf?\x16u\x04\xe9|^\x92\xbf\x8f\x91e\xed\xbb\xef\xd4?\xb4\xceM\xb9\xbaO\xc8?IjIi{$\xd9\xbf-\xd3Z\xd9\xba\x81\xbe?j\xb0\xa2\x81\xf1\xde\xd7\xbf\xa3\xca\x9c\xf20\x91\xe2\xbfj\x81+\x8dYK\xbe?,Y\x8d\xda\x06\xf4\xae?R\\(-q\xbe\xe1\xbf\xa1\xa9\xe8(\xb5C\xd3\xbfv\xd3\xff\xd3\xdb\x00\xa0?\x96\xd1osV\xaf\xbd?U\xe1\xc8\x86A\x9a\xd5\xbf\xa5\x81\xcd\x03\xaaZ\xc7?\xb2K\x95\x0b\xde\xe2\xe8\xbf\x1d\t\xfd(B\xa1\xc8?\xca\x88\xc0\xfe\x03}\xc0\xbfL+\xfb\x9dQ\xab\x81\xbf\x7fH\xe3\xe0\xfd\\\xb6\xbf\xb5S\x84@\xb9>\xd4?b\x96\xd9\xc6:\x8e\xa9\xbf\x0em`7\xcc\x14\xcb?C\xbaf\x86\xfc\xdb\xd2?\x81\x0b\x8c\r/Sv\xbfe\x00vO\xb8P\xc0?\xdd \xaf\xb1\xa9z\xab?\xa5\xbbH\n\x98\xa9\xab?f\xfa4-\xe4\xfe\xda\xbf\xc8\xe4f$\xfde\xb0\xbf\xcb\x91\xbd3\x1a\x07\xc8\xbf\x0b_~B\xb0\x85z\xbf\x90\x84\xc8\xfeV\x97\xc8?\x18\xbeW\x7f\xedF\xbf\xbfR\xc7\xaf\xb7%E\xb6\xbf\x97\xaf\xb0\x1a\x80C\xdd?\x8d\t\xfa\xe5\xcfC\xc2\xbf\xcfR\xb9?rL\xd1?\x03X2\x0e/\xa2\xa1?+d\xa5V\xa6\xeb\xa3?\x0ch\x95\xc8\xf8l\xd2?\xbe\x8d\x8b\x0e\x92-\xc9?sF<\xa9\x96J\xc7?84\xae\xfc\x8b2\xae?'
+p21
+tp22
+bsS'num_best_deltas'
+p23
+I16
+sS'expl_noise'
+p24
+F0.01
+sS'action_dim'
+p25
+I8
+sb.

mock/simulator/mini_ros/policies/mini_ars_Backward_policy_old

@@ -0,0 +1,67 @@
+(iars_lib.ars
+Policy
+p0
+(dp1
+S'state_dim'
+p2
+I8
+sS'learning_rate'
+p3
+F0.02
+sS'num_deltas'
+p4
+I16
+sS'seed'
+p5
+I1
+sS'episode_steps'
+p6
+I500
+sS'theta'
+p7
+cnumpy.core.multiarray
+_reconstruct
+p8
+(cnumpy
+ndarray
+p9
+(I0
+tp10
+S'b'
+p11
+tp12
+Rp13
+(I1
+(I8
+I8
+tp14
+cnumpy
+dtype
+p15
+(S'f8'
+p16
+I0
+I1
+tp17
+Rp18
+(I3
+S'<'
+p19
+NNNI-1
+I-1
+I0
+tp20
+bI00
+S"&Yz\x9cM6\xc4?Z\xce\x81\x1d \xe2\xb1\xbf\x97h!\n\x93\xd6\xb5?x\xc7Q\xf3\xbdr\xc0\xbf+\xa3\x185\xd8d\xba?^\xdap\xfa\x14{\xb0?\x85R%\xb7\xda\xc6\xc2?\xc7\x9b\x15\xb6e#\xc9\xbf\x19X\xd3\x145\xcb\xb5\xbf\x0c\r7c\x1a\xa1\xcb\xbf\x82_7m\xd4E\xb3?c!F\x07\x14\x18\xa0?^o\x10\xcbu\xba\xc4?*O\xadA\xb3\xf2\x92?zf,\x17\x08\x8b\xc6\xbf\x8cm3\xee\xda\x97\xc2?\x0b^\xea\x80H\xaa\xd0\xbfQ\x81\x9fC2V\xae\xbf\xfb\xaeLr\xf8\x10\xc9\xbfq\xc7+\x98-f\xbd\xbfG\n\xbb\x84\x90\xa7\xa5?|2\xff3/<n?\xe6'D2\xfc`\xd5\xbfeE\xaf#\xfb9\xab\xbf\xdbf\x1a?,\x93\x8f\xbf\xfe\xfd\xe7!&K\xb5\xbf$\x98|\xde\x85\xe0\xd6\xbfd\xb6UI\x0e\x0e\xc4\xbf\x9eg\xd3\x1d\xf8\xb3\xb0?\x19\xa9+*o\xf2\xcc?6HG(\xf1\xdd\xdc?\xa29\xc2\x01\xa4\xf6\x8c\xbf\xf3X\xb1\\\xd4y\xb6?\xa4\x80\x9a\xf8\xc5\x06\xc1?\xfa\xce]\xa0'\x90\xde?D\xbd\xe6\x91\x13e\xd7?\x11\x9d$\xbb\x8f\xaa\xb7?\xd8\x84\xa5\xd9<\xf3\xc8?\xf4\x05\x84\x8d\x92\xce\xc8\xbf#h\xff&3\xb8\xbe\xbf\xdf\xde\xbeAe\xb3\x95\xbf.\x8cmZ^\x98\xbd\xbf%\xb6\xb62\xdd\x9e\xcb?\x00r&|\x8b\xd0\xba\xbf\x0b\xb2u\xf0\xb7\xec\xab?Xo\xb8y\r\x02\xc1?\x84\xb7B\xde\x00U\xb3?\xc9{\xc73Ya\xa0\xbfJ{1\xa9\xed\x9d\xcb\xbf\x10fAr\x19\xf2\xdd\xbf\xc4\xf6\x8a\xb0E\x02\xb3\xbf\x13dt\x9d\x8d\x17\x9a?}\xf0\xd0\x8f\x8c\x8c\xd3\xbf\x1c\x84\xf0\xcajt\xc2?A\xb7\xa1\xa2\x05%\xca?/\xa2\xdc\xbb\x0e\x06\xc3?\x12\x85\xf5\xd3\x89\xca\xc1\xbf\x81~O+\xc7\xdc\xb1?c\x94\xb3\x1e\xbb\xc7\xc0?\xfb\xde\x96F\\\xd2\xb1\xbfWB/\x12\x08\x84\xb3\xbfC\xc9\x8d\x90I\xed\xd6\xbf\x1e\xb8\x06#L\x87\x95?\x7fa\xd4\xae`_\xa0\xbf"
+p21
+tp22
+bsS'num_best_deltas'
+p23
+I16
+sS'expl_noise'
+p24
+F0.01
+sS'action_dim'
+p25
+I8
+sb.

mock/simulator/mini_ros/policies/mini_ars_CCW_policy

@@ -0,0 +1,67 @@
+(iars_lib.ars
+Policy
+p0
+(dp1
+S'state_dim'
+p2
+I8
+sS'learning_rate'
+p3
+F0.02
+sS'num_deltas'
+p4
+I16
+sS'seed'
+p5
+I1
+sS'episode_steps'
+p6
+I500
+sS'theta'
+p7
+cnumpy.core.multiarray
+_reconstruct
+p8
+(cnumpy
+ndarray
+p9
+(I0
+tp10
+S'b'
+p11
+tp12
+Rp13
+(I1
+(I8
+I8
+tp14
+cnumpy
+dtype
+p15
+(S'f8'
+p16
+I0
+I1
+tp17
+Rp18
+(I3
+S'<'
+p19
+NNNI-1
+I-1
+I0
+tp20
+bI00
+S'\x08=\x01T\x8df\xb5\xbf\xb0\xb1\xcd\x0c\xb9\x82\xd4\xbf\xcca\x8e?\xb7\xc3\xb5\xbf\x1b\x0bj\xae\x90\xb0\xd4\xbfv\xdb\r\xd1\x17\x8a\xc8?Y\xc6\xf0G\xa0\xe7\xab\xbf\x8a?H\xa6x>\x80?0\xa5\x1d\xf7\xd1\x83\x81?L\x8fPn\xbdK\xbe?\xc448\x98[\xc2\xba\xbf\xc2P+X\x08\xf1\xc8?\xb0\x07\xd7t\xe4\xc2\xcc?T\xf1)y\x12z\xad\xbfo&\xecg\x8a.\x82\xbfBam\xc9\xbe\x15\xba?\x1d7\x05\x1b\x0f\xdf\xb9\xbf=\xb8\xecb\xc8d\xad?k\xf7\x13xp\x91\xce?\xfaxUU+\x9a\x86\xbf\xc8\xa5C\xd8k\xa6\xc7\xbf\xdf\xd3\x0b\x1e\xa2\xa9\xc0?\x1eq2\xc9.\x90\xa1\xbf\x89\xca\x9e\x19\xe7_\xbe\xbf\xc1\xd8\x16\xce\xf6\x18\xb6?05\xf4\xa7Fx\xb8?\xa6\x1e<\x1aw\x18\xd0\xbf"V-\xca\x1e\xe7\xd3\xbfn\xc9\xf2qS\\\xc7\xbfn\x89:\xb6\xf1p\x9c?\xdd[3\xa5h$\xc9\xbf\\\xf0\x1c]\'\xdf\xae?\x86"\xbd\xb7=\x1f\xbf?D\xd2\xf7c\xb4g\xdf?\xe1o\xe31\xf7\xb4\xd7\xbf\x90\xba\x9f\x88]\xe5\xc7?<qJ\xcc\xf6\xff\xc1\xbfd\x01\x7f\xdd\x9e&\xbc?\x07!RW;h\xcb\xbf0\x91\x96{\xcai\xda\xbf\tX\xe5[^\xf4\xc0?\xf4\xb3\t\xce\x98F\xc9\xbf4\xf4|\x8d\x04\\\xb6\xbf\xf5iFd\xfbe\xc1?Z\xe8\x04\x9c\x07m\xca?\x90\x89\xa0\x9c\xaeV\x94?F\xd6\xaega0\xb4?:\xd0\xeei\x1a\x82\xb7\xbf \x8dx\xb7D\xab\xc2?\xe9jT\xae\xdd\xca\xc4?\x8f\xb0Nfe#\xd5\xbf\xb4\n\x0f7O\x9d\xc9\xbf"\\\x8f28\xe3\xa1?\xeb0\xd4\xbc\xffK\xb7\xbf\xfco\xfb\x15\xe7\x00b\xbf\x84\x8d\\7AW\xa6\xbf\xc6>\xccI+\xd1\xba?\x80@w\x8a\x81\xdc\xae\xbfD\xd8\xd7\x94P?\xa1?\x13\xab\xd3\xb0\xe3\xe2\xd0\xbf\xf5\xdc\xe6\x99\x0bQ\xa5\xbf\x9f\xd5\x9d\x0f\xe9c\xc5?\t\x96C~\xe7\x1a\xe5\xbf\xc3\xe3|\x84[\x00\xda\xbf\xc1\xff\xe1B\xce\xd5\xd3?'
+p21
+tp22
+bsS'num_best_deltas'
+p23
+I16
+sS'expl_noise'
+p24
+F0.01
+sS'action_dim'
+p25
+I8
+sb.

mock/simulator/mini_ros/policies/mini_ars_CW_policy

@@ -0,0 +1,67 @@
+(iars_lib.ars
+Policy
+p0
+(dp1
+S'state_dim'
+p2
+I8
+sS'learning_rate'
+p3
+F0.02
+sS'num_deltas'
+p4
+I16
+sS'seed'
+p5
+I1
+sS'episode_steps'
+p6
+I500
+sS'theta'
+p7
+cnumpy.core.multiarray
+_reconstruct
+p8
+(cnumpy
+ndarray
+p9
+(I0
+tp10
+S'b'
+p11
+tp12
+Rp13
+(I1
+(I8
+I8
+tp14
+cnumpy
+dtype
+p15
+(S'f8'
+p16
+I0
+I1
+tp17
+Rp18
+(I3
+S'<'
+p19
+NNNI-1
+I-1
+I0
+tp20
+bI00
+S'\x08=\x01T\x8df\xb5\xbf\xb0\xb1\xcd\x0c\xb9\x82\xd4\xbf\xcca\x8e?\xb7\xc3\xb5\xbf\x1b\x0bj\xae\x90\xb0\xd4\xbfv\xdb\r\xd1\x17\x8a\xc8?Y\xc6\xf0G\xa0\xe7\xab\xbf\x8a?H\xa6x>\x80?0\xa5\x1d\xf7\xd1\x83\x81?L\x8fPn\xbdK\xbe?\xc448\x98[\xc2\xba\xbf\xc2P+X\x08\xf1\xc8?\xb0\x07\xd7t\xe4\xc2\xcc?T\xf1)y\x12z\xad\xbfo&\xecg\x8a.\x82\xbfBam\xc9\xbe\x15\xba?\x1d7\x05\x1b\x0f\xdf\xb9\xbf=\xb8\xecb\xc8d\xad?k\xf7\x13xp\x91\xce?\xfaxUU+\x9a\x86\xbf\xc8\xa5C\xd8k\xa6\xc7\xbf\xdf\xd3\x0b\x1e\xa2\xa9\xc0?\x1eq2\xc9.\x90\xa1\xbf\x89\xca\x9e\x19\xe7_\xbe\xbf\xc1\xd8\x16\xce\xf6\x18\xb6?05\xf4\xa7Fx\xb8?\xa6\x1e<\x1aw\x18\xd0\xbf"V-\xca\x1e\xe7\xd3\xbfn\xc9\xf2qS\\\xc7\xbfn\x89:\xb6\xf1p\x9c?\xdd[3\xa5h$\xc9\xbf\\\xf0\x1c]\'\xdf\xae?\x86"\xbd\xb7=\x1f\xbf?D\xd2\xf7c\xb4g\xdf?\xe1o\xe31\xf7\xb4\xd7\xbf\x90\xba\x9f\x88]\xe5\xc7?<qJ\xcc\xf6\xff\xc1\xbfd\x01\x7f\xdd\x9e&\xbc?\x07!RW;h\xcb\xbf0\x91\x96{\xcai\xda\xbf\tX\xe5[^\xf4\xc0?\xf4\xb3\t\xce\x98F\xc9\xbf4\xf4|\x8d\x04\\\xb6\xbf\xf5iFd\xfbe\xc1?Z\xe8\x04\x9c\x07m\xca?\x90\x89\xa0\x9c\xaeV\x94?F\xd6\xaega0\xb4?:\xd0\xeei\x1a\x82\xb7\xbf \x8dx\xb7D\xab\xc2?\xe9jT\xae\xdd\xca\xc4?\x8f\xb0Nfe#\xd5\xbf\xb4\n\x0f7O\x9d\xc9\xbf"\\\x8f28\xe3\xa1?\xeb0\xd4\xbc\xffK\xb7\xbf\xfco\xfb\x15\xe7\x00b\xbf\x84\x8d\\7AW\xa6\xbf\xc6>\xccI+\xd1\xba?\x80@w\x8a\x81\xdc\xae\xbfD\xd8\xd7\x94P?\xa1?\x13\xab\xd3\xb0\xe3\xe2\xd0\xbf\xf5\xdc\xe6\x99\x0bQ\xa5\xbf\x9f\xd5\x9d\x0f\xe9c\xc5?\t\x96C~\xe7\x1a\xe5\xbf\xc3\xe3|\x84[\x00\xda\xbf\xc1\xff\xe1B\xce\xd5\xd3?'
+p21
+tp22
+bsS'num_best_deltas'
+p23
+I16
+sS'expl_noise'
+p24
+F0.01
+sS'action_dim'
+p25
+I8
+sb.

mock/simulator/mini_ros/policies/mini_ars_Forward_policy

@@ -0,0 +1,67 @@
+(iars_lib.ars
+Policy
+p0
+(dp1
+S'state_dim'
+p2
+I8
+sS'learning_rate'
+p3
+F0.02
+sS'num_deltas'
+p4
+I16
+sS'seed'
+p5
+I0
+sS'episode_steps'
+p6
+I1000
+sS'theta'
+p7
+cnumpy.core.multiarray
+_reconstruct
+p8
+(cnumpy
+ndarray
+p9
+(I0
+tp10
+S'b'
+p11
+tp12
+Rp13
+(I1
+(I8
+I8
+tp14
+cnumpy
+dtype
+p15
+(S'f8'
+p16
+I0
+I1
+tp17
+Rp18
+(I3
+S'<'
+p19
+NNNI-1
+I-1
+I0
+tp20
+bI00
+S'\xb0\xff\xe8\xef\x04U\xd5?a\xf4\xbf\xf4{E\xc2?Te\xd7\x92*\x1c\xdc?\x85\xc7g\xab\xe6\xae\xce?\xd4\xecx\xb3\x0c\xd8Q\xbf\xe0.Y\xd5um\xda\xbf\xc93G\xca\x85\xe9\xd0?\xf8\xbc\n\xc4ry\xcd\xbf\xb5\xc9gUU\xbf\xdf?\xd8\xe5\x18\xbc\xa7\xcc\xb2?\x10\x8d\xfb&a\xb3\xd8?\xf8\x8b\xb6\x7f\xee\xa6\xda\xbf\xf30\x0cv\x8b\x93\xc8?y\x16\xf7B\xc8k\xd3?\n\x8e*\xa5\x98\xba\xb7\xbf\xc7l\x94\xb8\x04\xd9\xc1?\x85\xbbt\\\xb37\xd8\xbf\xc1Q\xcd\xef\xd6_\xcb?\xddL\x8b\xff=\xcc\xc7?\xe0@c\xd7\xf6\x90P\xbf\xb2\xc2\xce\x9d\xa9\x95\xb2?\xfby\x16\x16i\xf6\xda?\x91\x98\x7f\xa6\x93\x19\xd4\xbf\xdfs\xd4\x9f\x89\xe8\xa1?\x18D\x88D\xa7\x8c\xdb\xbf\x06\'<\x9f\\\x17\xe0?\xaf`[\xfd\xc2\xf2\xd6?\xed\x1fF\x0fv\x11\xd0?\xb97\xcf\xdcq)\xc6?\x8c4\x1e!\xc3\xa7\xbb\xbf"0k!\xc1\xc5\xc7\xbf\x90g\xe9\x04\x98\x87\xc8?\x9e\xcc\x1d\x1e+\xe6\xe5?({\xdf6\x8ag\xde\xbf\x04\x05\xea\xef\xe3U\xbf?\xe9\xc9\x00Rx\x9b\x93?\r:-\r-\xd9\xde\xbf\xb7G{s\x12@\xc8\xbf\xcb#\x9c\x122\xde\xc2?\xb0\xd6?P\xc1M\xdd\xbf\x1c\xe1\xdd\x893:\xca\xbf\x95\xc3\x83%m\xac\xb9\xbf\x1aftD\xb6\xc6\xb4\xbf|h\xa4\x82\x1a\x84\xac?\x14\x1d3\x8aY\xcb\xd1?\xf7\x0c\xd5"\xab\x0f\xc3?B\xe6\x18\xe8\xc7}\xca?.\xebCW\xba\xae\xa2?\x98\x03J\xe13\xe9\xb8\xbf\xf26\xc4\xa5m\xe8\xce?\x1c\xf8\x98\xa5\x1b8\x97\xbf\x90|r\x88vj\xc7?x\xdc[\xd9\x00\xec\xb9?0\x88Y\xd73\x91\xc2?\xe4B;\xc2Q\x0f\xd7\xbf\x98)\xce\xaaKO\xb2?\xcb>"\xd3iy\xc4\xbf\xe8\xf9\x9bOl\xa7\x00\xbf\xa2I\x8a\xbcV\xe7\x99\xbf\xce\xa2\xf66\xe1\xa1\xd4\xbf\x96g\x84\xb4f.\xd0\xbf\x80\xf3\xb9~\xddc\xb4\xbf\xf6\xe0\x8c\x8e\xa6!\xd5?n{!\xeb\xc2:\xd0\xbf'
+p21
+tp22
+bsS'num_best_deltas'
+p23
+I16
+sS'expl_noise'
+p24
+F0.01
+sS'action_dim'
+p25
+I8
+sb.

mock/simulator/mini_ros/policies/mini_ars_Forward_policy_old

@@ -0,0 +1,67 @@
+(iars_lib.ars
+Policy
+p0
+(dp1
+S'state_dim'
+p2
+I8
+sS'learning_rate'
+p3
+F0.02
+sS'num_deltas'
+p4
+I16
+sS'seed'
+p5
+I1
+sS'episode_steps'
+p6
+I500
+sS'theta'
+p7
+cnumpy.core.multiarray
+_reconstruct
+p8
+(cnumpy
+ndarray
+p9
+(I0
+tp10
+S'b'
+p11
+tp12
+Rp13
+(I1
+(I8
+I8
+tp14
+cnumpy
+dtype
+p15
+(S'f8'
+p16
+I0
+I1
+tp17
+Rp18
+(I3
+S'<'
+p19
+NNNI-1
+I-1
+I0
+tp20
+bI00
+S'o\xe8R\xad\x835\xd6?\x1c\\\xb9\xe75\xba\xa4\xbf?\x1d\xdb!\xa7a\xd5?n\x92p\x8e\xfb-\xb7?\xca\t@\xbe[\x7f\xc1?vq\x89\x1a\xb6\x00\xb4\xbf/\xef\xe5\xd7\n\xeb\xd1\xbf }\x1bp\xe0Y\xa5?\x88\x05\x08\xa4-\x93\xdb?sH\xae\x91"\x13\xd5\xbf0\xccz\x8f\x12F\xd3?\x83\xc8@\'\x89"\xce?\xa7\xe4\x97\xc2\x14\xa8\xc3\xbftN\x94\'hY\xd1?Qs\x1a\xe3\xddu\xc5\xbf\x8b\x18\xe9"\x96f\xbc?\xd7{$f\xf5/\xb1?\xeb\x06\x7f\xa0\xea;\xdb\xbf_8\x11B(B\xd0?\x02\xc8\xf7\xe64\x94\xda\xbf\xd2ch\xea\x08\xdd\xd4?t\x9c\x15\xa5V=\xc3?/\xc7\xda\xcd\x08\xcb\xe3?\x94\xd5\xb4N\xa8\xba\xd3\xbfl\xe56\xb5ck\x8d\xbfB\xe9\xca\x98\xae:\xca\xbf\xbc\xf2b\xd9\xaf\xf8\xbb\xbfS\xd7E#\xfe@\xb5?\xc0\xd8v\x91\x900\xb7\xbf\x0b\xfd\r\xec4\x9b\xe2\xbfzODz\xa3\xd8\xb7?\xf9D,\'\xe9\x1b\xbf?\xc4\xee)~\xb4\x80\xde?\xa3\xa5\x82\x03F\x85\xca?\x1fB\'\xed\x80|\xa6?n.U\xdd3\x8c\xd0\xbfn\xbe\xef\xc8\xe6B\x94?\xa0\x989\xf7\xe24\xae?\x995,\x07\xddS\xd5\xbf\xde[v\xab\xba\x95\xc3\xbf\xf0\x17\xe6]\x99\x18\xdf?R\x17\xfa{)s\xd2?\xc2r\xd3D\xfb\xd8\xd2?\xa2\xe6n\xdd\xc1\xee\xdc?\xe6\xd6<\x94*H\xaf?3\xa3_w\x98=\xad?n\xf8\xe2M\x00\xa6\xc5?\xc6\xc49\x05\xf0S\xa3?\xd6\xa8\xefy\xa4)\xd3\xbf\xca=\'\xcd\xb0\x03\xe8\xbfdJ\xb7F\xd54\xc7?\xadVdj|2\xc0\xbf#U\x9b\x08\xd4b\xdc\xbf\xb0\xd7\xc1\xb4\x11\x96\xd4?\x88\x1a\xe2\x88k2\xb7?\xf7\x14eO\x97\x1f\xb3?$1\x83\xb5}\x85\x98\xbf\xcb\xd0\xb3E\xed\x95\xb7?\xaa\xab\x86\xf9A{\xc0\xbf\xeb\xf3\xca\x85\xa2\xf0\xbc\xbf\xbe\x0f\x02\x84\xf5\xca\xdd?\xc05\x8e\xfe\xbdJ\xde\xbf\x11m\xf7\n6\xa3\xc4?\x8d\x0f\t\xe5>6\xb4\xbf'
+p21
+tp22
+bsS'num_best_deltas'
+p23
+I16
+sS'expl_noise'
+p24
+F0.01
+sS'action_dim'
+p25
+I8
+sb.

mock/simulator/mini_ros/policies/mini_ars_Left_policy

@@ -0,0 +1,67 @@
+(iars_lib.ars
+Policy
+p0
+(dp1
+S'state_dim'
+p2
+I8
+sS'learning_rate'
+p3
+F0.02
+sS'num_deltas'
+p4
+I16
+sS'seed'
+p5
+I1
+sS'episode_steps'
+p6
+I500
+sS'theta'
+p7
+cnumpy.core.multiarray
+_reconstruct
+p8
+(cnumpy
+ndarray
+p9
+(I0
+tp10
+S'b'
+p11
+tp12
+Rp13
+(I1
+(I8
+I8
+tp14
+cnumpy
+dtype
+p15
+(S'f8'
+p16
+I0
+I1
+tp17
+Rp18
+(I3
+S'<'
+p19
+NNNI-1
+I-1
+I0
+tp20
+bI00
+S'"9\xc5 \x10I\xc2\xbf6Q\xb1\x0c\x0f\xa5\xc1\xbf\xe2W\x8e\xa3\xcf\x19\xb0?\xd5U\xf5\xb2\xd4\xb5\xd4?N\xf9\xb9]\xb9\x06\xb6\xbf\xf5\xd8\xf1N\x80\xe7\x9d\xbf\'\x8cU\x9f\x94@\xac?\xbe\n\xfa\xf2/K\xa1\xbfmDvv\x99\xf0\xdb?\xf8l\xf3~\x18<\xa8\xbf%\xd1\x06\x88\xcc\x1d\xbf\xbf5\']\x02\xc2h\xa3?)\xa6\x07Q\xad\xf2\xb0?sw\xfc\x16\x06\xc5\xd1\xbfd\xde\xfe\xa5"R\xd1?\xab\x0f\xe4\x04\xe8h\xcb\xbf\x8c\x9f\xb9\xff\xc8\x05\xb6?\x1e\x8f\xd3\xb27\xd4\x90\xbf\x92\x83\xcf\x06\xa9\x13\xde\xbf<\x1a\xd8%\x01l\xa2?Jp\xfe\x13D\xbc\xe1\xbfB\xd3>\xb5j\x15\xa5?]\xdb\xeb\xdc\xb1\xe3\xc0?\x98B\xf6w \xda\xb9?z\xdd\xa8rx\x10\x87?\xba!\x14\xd8\xa5\xe8\xcf\xbfs6\xde2\x85*\xf1\xbfO\x8e\xa7\x84_d\xdf\xbf\xcbn\xc5[\x83/\xd6?\x0f\x0bid\xe3\x89\xb7?)\xbf\xcd,\xddS\xc0?*\xc8\xce\xe2\x95\xbf\xb9\xbf\xcd\x90\x9b\x1a\xff5\xbc\xbf{\xfe\x1a9\x94D\xcd\xbf"\xb4\xb1\x9b\xcb;\xe0?\xfd6\xf7+[\x82\xdc\xbf+B\x91\x85\xad\xfc\xd2?M\xed+\xb9]\x8a\xc7?X\x99,\x1b\x8f=\xb1\xbfl\xf2\x0fBu\xa2\xbf\xbf&\xefU\xf3\xf6\xe1\xe2?\x12z)\xbb\xde\xee\x9a\xbf\x1f\x9c{^\x11\x9e\x9c?s\t\xbd\xd0\x99^\xc1\xbf\xb9Mh{\x96R\xd3?=m\xa7?5\x03\xd6\xbf\xb6\x8f\xf7\x91\xb5\xfe\xd2?\x18=l]\xf3f\xb4\xbfk\xd9\x19\xf7,\xa4\xd1\xbf\xf7\xcb#\xef\x9d_\xc7\xbfi\x8e\x12v\x91Q\xe4?\xe1y\xe1\xb4\xcb>\xdf?=(w\xc1\xf7K\xa9\xbf~N"5\xa69\x85\xbf\r\xb7\x95\xee\x93Q\xc4\xbf\xd8\x95\xe8\x93\tZ\xb3\xbf\x80\x15kI\xd8\x06\x8c\xbf(\x96\xa1\xcd\x9c\x87\xc1\xbft\xc6\x95\xac\x0c\xaa\xde\xbfhX\xa1\x94\x7f\x1d\xd9?h\x03F\xd9u\x96\xe6\xbf\x0c\xe5\xc7\x85\x16\xd0\xbe?\xa6\x0f1\xbd\xdb\xb0\xb7?\x0e\x95\xe3e\xfa\xd3\xd6\xbf'
+p21
+tp22
+bsS'num_best_deltas'
+p23
+I16
+sS'expl_noise'
+p24
+F0.01
+sS'action_dim'
+p25
+I8
+sb.

mock/simulator/mini_ros/rviz/spot.rviz

@@ -0,0 +1,262 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /RobotModel1/Links1
+      Splitter Ratio: 0.6205882430076599
+    Tree Height: 555
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        back:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        back_bracket:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        back_left_foot:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        back_left_hip:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        back_left_lower_leg:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        back_left_upper_leg:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        back_right_foot:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        back_right_hip:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        back_right_lower_leg:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        back_right_upper_leg:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        base_inertia:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+        battery:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        chassis_left:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        chassis_right:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front_bracket:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front_left_foot:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front_left_hip:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front_left_lower_leg:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front_left_upper_leg:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front_right_foot:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front_right_hip:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front_right_lower_leg:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+        front_right_upper_leg:
+          Alpha: 1
+          Show Axes: true
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 0.8253771066665649
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.004122655838727951
+        Y: 0.0018941210582852364
+        Z: -0.10473191738128662
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.3302026689052582
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.8954905271530151
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1200
+  X: 2392
+  Y: 87

mock/simulator/mini_ros/setup.py

@@ -0,0 +1,11 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+## Copies from http://docs.ros.org/melodic/api/catkin/html/howto/format2/installing_python.html and edited for our package
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+    packages=['mini_bullet', 'ars_lib'],
+    package_dir={'../mini_bullet/src': ''})
+
+setup(**setup_args)

mock/simulator/mini_ros/src/mini_ros/spot.cpp

@@ -0,0 +1,118 @@
+#include "mini_ros/spot.hpp"
+
+namespace spot
+{
+
+	// Spot Constructor
+	Spot::Spot()
+	{
+		cmd.x_velocity = 0.0;
+		cmd.y_velocity = 0.0;
+		cmd.rate = 0.0;
+        cmd.roll = 0.0;
+        cmd.pitch = 0.0;
+        cmd.yaw = 0.0;
+        cmd.z = 0.0;
+		cmd.motion = Stop;
+		cmd.movement = Viewing;
+	}
+
+	void Spot::update_command(const double & vx, const double & vy, const double & z,
+							  const double & w, const double & wx, const double & wy)
+	{
+		// If Command is nearly zero, just give zero
+		if (almost_equal(vx, 0.0) and almost_equal(vy, 0.0) and almost_equal(z, 0.0) and almost_equal(w, 0.0))
+		{
+			cmd.motion = Stop;
+			cmd.x_velocity = 0.0;
+			cmd.y_velocity = 0.0;
+			cmd.rate = 0.0;
+			cmd.roll = 0.0;
+	        cmd.pitch = 0.0;
+	        cmd.yaw = 0.0;
+	        cmd.z = 0.0;
+	        cmd.faster = 0.0;
+	        cmd.slower = 0.0;
+		} else
+		{
+			cmd.motion = Go;
+			if (cmd.movement == Stepping)
+			{
+				// Stepping Mode, use commands as vx, vy, rate, Z
+				cmd.x_velocity = vx;
+				cmd.y_velocity = vy;
+				cmd.rate = w;
+				cmd.z = z;
+				cmd.roll = 0.0;
+		        cmd.pitch = 0.0;
+		        cmd.yaw = 0.0;
+		        // change clearance height from +- 0-2 * scaling
+		        cmd.faster = 1.0 - wx;
+		        cmd.slower = -(1.0 - wy);
+			} else
+			{
+				// Viewing Mode, use commands as RPY, Z
+				cmd.x_velocity = 0.0;
+				cmd.y_velocity = 0.0;
+				cmd.rate = 0.0;
+				cmd.roll = vy;
+		        cmd.pitch = vx;
+		        cmd.yaw = w;
+		        cmd.z = z;
+		        cmd.faster = 0.0;
+		        cmd.slower = 0.0;
+			}
+		}
+		
+	}
+
+	void Spot::switch_movement()
+	{
+		if (!almost_equal(cmd.x_velocity, 0.0) and !almost_equal(cmd.y_velocity, 0.0) and !almost_equal(cmd.rate, 0.0))
+		{
+			ROS_WARN("MAKE SURE BOTH LINEAR [%.2f, %.2f] AND ANGULAR VELOCITY [%.2f] ARE AT 0.0 BEFORE SWITCHING!", cmd.x_velocity, cmd.y_velocity, cmd.rate);
+
+			ROS_WARN("STOPPING ROBOT...");
+
+			cmd.motion = Stop;
+			cmd.x_velocity = 0.0;
+			cmd.y_velocity = 0.0;
+			cmd.rate = 0.0;
+			cmd.roll = 0.0;
+	        cmd.pitch = 0.0;
+	        cmd.yaw = 0.0;
+	        cmd.z = 0.0;
+	        cmd.faster = 0.0;
+	        cmd.slower = 0.0;
+		} else
+		{
+			cmd.x_velocity = 0.0;
+			cmd.y_velocity = 0.0;
+			cmd.rate = 0.0;
+			cmd.roll = 0.0;
+	        cmd.pitch = 0.0;
+	        cmd.yaw = 0.0;
+	        cmd.z = 0.0;
+	        cmd.faster = 0.0;
+	        cmd.slower = 0.0;
+			if (cmd.movement == Viewing)
+			{
+				ROS_INFO("SWITCHING TO STEPPING MOTION, COMMANDS NOW MAPPED TO VX|VY|W|Z.");
+
+				cmd.movement = Stepping;
+				cmd.motion = Stop;
+			} else
+			{
+				ROS_INFO("SWITCHING TO VIEWING MOTION, COMMANDS NOW MAPPED TO R|P|Y|Z.");
+
+				cmd.movement = Viewing;
+				cmd.motion = Stop;
+			}
+		}
+	}
+
+	SpotCommand Spot::return_command()
+	{
+		return cmd;
+	}
+}

mock/simulator/mini_ros/src/mini_ros/teleop.cpp

@@ -0,0 +1,102 @@
+#include "mini_ros/teleop.hpp"
+
+namespace tele
+{
+	Teleop::Teleop(const int & linear_x, const int & linear_y, const int & linear_z,
+	               const int & angular, const double & l_scale, const double & a_scale,
+	               const int & LB, const int & RB, const int & B_scale, const int & LT,
+	               const int & RT, const int & UD, const int & LR,
+	               const int & sw, const int & es)
+	{
+		// step length or pitch
+		linear_x_ = linear_x;
+		// lateral fraction or roll
+		linear_y_ = linear_y;
+		// height
+		linear_z_ = linear_z;
+		// yaw rate or yaw
+		angular_ = angular;
+		// scale linear axis (usually just 1)
+		l_scale_ = l_scale;
+		// scale angular axis (usually just 1)
+		a_scale_ = a_scale;
+		// for incrementing and decrementing step velocity
+		// Bottom Bumpers
+		RB_ = RB;
+		LB_ = LB;
+		// scale Bottom Trigger axis (usually just 1)
+		B_scale_ = B_scale;
+		// Top Bumpers
+		RT_ = RT;
+		LT_ = LT;
+		// Switch between Stepping and Viewing
+		sw_ = sw;
+		// E-STOP
+		es_ = es;
+
+		// Arrow PAd
+		UD_ = UD;
+		LR_ = LR;
+
+		switch_trigger = false;
+		ESTOP = false;
+		updown = 0;
+        leftright = 0;
+
+		left_bump = false;
+		right_bump = false;
+	}
+
+	void Teleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
+	{
+		twist.linear.x = l_scale_*joy->axes[linear_x_];
+		twist.linear.y = l_scale_*joy->axes[linear_y_];
+		// NOTE: used to control robot height
+		twist.linear.z = -l_scale_*joy->axes[linear_z_];
+		twist.angular.z = a_scale_*joy->axes[angular_];
+		// NOTE: bottom bumpers used for changing step velocity
+		twist.angular.x = B_scale_*joy->axes[RB_];
+		twist.angular.y = B_scale_*joy->axes[LB_];
+		
+
+		// Switch Trigger: Button A
+		switch_trigger = joy->buttons[sw_];
+
+		// ESTOP: Button B
+		ESTOP = joy->buttons[es_];
+
+		// Arrow Pad
+		updown = joy->axes[UD_];
+		leftright = -joy->axes[LR_];
+
+		// Top Bumpers
+		left_bump = joy->buttons[LT_];
+		right_bump = joy->buttons[RT_];
+	}
+
+	geometry_msgs::Twist Teleop::return_twist()
+	{
+		return twist;
+	}
+
+	bool Teleop::return_trigger()
+	{
+		return switch_trigger;
+	}
+
+	bool Teleop::return_estop()
+	{
+		return ESTOP;
+	}
+
+	mini_ros::JoyButtons Teleop::return_buttons()
+	{
+		mini_ros::JoyButtons jb;
+		jb.updown = updown;
+		jb.leftright = leftright;
+		jb.left_bump = left_bump;
+		jb.right_bump = right_bump;
+
+		return jb;
+	}	
+}

mock/simulator/mini_ros/src/sensor_interface

@@ -0,0 +1,132 @@
+#!/usr/bin/env python3
+"""
+DESCRIPTION:
+
+SUBSCRIBERS:
+"""
+
+from __future__ import division
+import rospy
+from mini_ros.msg import IMUdata, ContactData
+from std_msgs.msg import String
+# Used for str(Boolean) --> Boolean
+from distutils.util import strtobool
+
+import sys
+
+import rospkg
+rospack = rospkg.RosPack()
+
+sys.path.append(rospack.get_path('mini_ros') + '/../')
+
+sys.path.append('../../')
+
+from spot_real.Control.RPi.lib.Teensy_Interface import TeensyInterface
+from spot_real.Control.RPi.lib.imu import IMU
+
+
+class SensorInterface():
+    def __init__(self):
+
+        rospy.init_node('SensorInterface', anonymous=True)
+        self.TI = TeensyInterface()
+        self.imu_pub = rospy.Publisher('spot/imu', IMUdata, queue_size=1)
+        self.cnt_pub = rospy.Publisher('spot/contact',
+                                       ContactData,
+                                       queue_size=1)
+        self.str_pub = rospy.Publisher('spot/teensydebug',
+                                       String,
+                                       queue_size=1)
+        print("PUT SPOT ON THE GROUND, CALIBRATING IMU")
+        self.imu = IMU()
+
+    def read_sensors(self):
+        """ Reads IMU and Contact Sensor data from Teensy 4.0
+            and publishes to respective topics
+        """
+        while not rospy.is_shutdown():
+            data = self.TI.read_buffer()
+
+            rospy.logdebug(data)
+
+            imu_dat = IMUdata()
+            imu_read = False
+            cnt_dat = ContactData()
+            cnt_read = False
+
+            msg = data.decode().split(",")
+            # REMOVE NEWLINE
+            msg[-1] = msg[-1].rstrip('\r\n')
+
+            # IMU
+            if msg[0] == "IMUDATA":
+                try:
+                    imu_msg = [float(x) for x in msg[1:]]
+                    # rospy.loginfo(imu_msg)
+                    # IMU
+                    imu_dat.roll = imu_msg[0]
+                    imu_dat.pitch = imu_msg[1]
+                    imu_dat.acc_x = imu_msg[2]
+                    imu_dat.acc_y = imu_msg[3]
+                    imu_dat.acc_z = imu_msg[4]
+                    imu_dat.gyro_x = imu_msg[5]
+                    imu_dat.gyro_y = imu_msg[6]
+                    imu_dat.gyro_z = imu_msg[7]
+                    imu_read = True
+                except:
+                    rospy.logdebug("bad imu read")
+
+            # CONTACT
+            elif msg[0] == "CONTACT":
+                try:
+                    cnt_msg = [strtobool(x) for x in msg[1:]]
+                    # rospy.loginfo(cnt_msg)
+                    # Contact Sensor
+                    cnt_dat.FL = cnt_msg[0]
+                    cnt_dat.FR = cnt_msg[1]
+                    cnt_dat.BL = cnt_msg[2]
+                    cnt_dat.BR = cnt_msg[3]
+                    cnt_read = True
+                except:
+                    rospy.logdebug("bad contact read")
+
+            # READ IMU
+            self.imu.filter_rpy()
+            imu_dat.roll = self.imu.true_roll
+            imu_dat.pitch = self.imu.true_pitch
+            imu_dat.acc_x = self.imu.imu_data[3]
+            imu_dat.acc_y = self.imu.imu_data[4]
+            imu_dat.acc_z = self.imu.imu_data[5]
+            imu_dat.gyro_x = self.imu.imu_data[0]
+            imu_dat.gyro_y = self.imu.imu_data[1]
+            imu_dat.gyro_z = self.imu.imu_data[2]
+            imu_read = True
+
+            if imu_read:
+                self.imu_pub.publish(imu_dat)
+                # rospy.logdebug("IMU")
+            if cnt_read:
+                self.cnt_pub.publish(cnt_dat)
+                # rospy.logdebug("CONTACT")
+
+            string_msg = String()
+            string_msg.data = data
+            self.str_pub.publish(string_msg)
+
+
+def main():
+    """ The main() function. """
+    si = SensorInterface()
+    rospy.loginfo("Ready To Log Data!")
+    # rate = rospy.Rate(1.0 / 60.0)
+    while not rospy.is_shutdown():
+        si.read_sensors()
+        # rate.sleep()
+        rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

mock/simulator/mini_ros/src/servo_calibration

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+"""
+DESCRIPTION:
+
+SUBSCRIBERS:
+"""
+
+from __future__ import division
+import rospy
+from mini_ros.srv import CalibServo, CalibServoResponse
+from mini_ros.msg import JointPulse
+import numpy as np
+
+import sys
+
+import rospkg
+rospack = rospkg.RosPack()
+
+sys.path.append(rospack.get_path('mini_ros') + '/../')
+
+sys.path.append('../../')
+
+
+class ServoCalibrator():
+    def __init__(self):
+
+        rospy.init_node('ServoCalibrator', anonymous=True)
+
+        self.serv = rospy.Service('servo_calibrator', CalibServo,
+                                  self.calib_service_cb)
+        self.jp_pub = rospy.Publisher('spot/pulse', JointPulse, queue_size=1)
+
+    def calib_service_cb(self, req):
+        """ Requests a servo to be moved to a certain position
+
+            Args: req
+            Returns: response
+        """
+        try:
+            jp_msg = JointPulse()
+
+            jp_msg.servo_num = req.servo_num
+            jp_msg.servo_pulse = req.servo_pulse
+
+            self.jp_pub.publish(jp_msg)
+            response = "Servo Command Sent."
+        except rospy.ROSInterruptException:
+            response = "FAILED to send Servo Command"
+        return CalibServoResponse(response)
+
+
+def main():
+    """ The main() function. """
+    srv_calib = ServoCalibrator()
+    rospy.loginfo(
+        "Use The servo_calibrator service (Pulse Width Unit is us (nominal ~500-2500))."
+    )
+    while not rospy.is_shutdown():
+        rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

mock/simulator/mini_ros/src/spot_pybullet_interface

@@ -0,0 +1,256 @@
+#!/usr/bin/env python
+"""
+DESCRIPTION:
+
+SUBSCRIBERS:
+"""
+
+from __future__ import division
+import os
+import rospy
+import numpy as np
+from mini_ros.msg import MiniCmd, JoyButtons
+import copy
+
+import sys
+
+import rospkg 
+rospack = rospkg.RosPack()
+
+
+sys.path.append(rospack.get_path('mini_ros') + '/../')
+
+sys.path.append('../../')
+
+from spotmicro.GymEnvs.spot_bezier_env import spotBezierEnv
+from spotmicro.Kinematics.SpotKinematics import SpotModel
+from spotmicro.GaitGenerator.Bezier import BezierGait
+
+# Controller Params
+STEPLENGTH_SCALE = 0.06
+Z_SCALE_CTRL = 0.12
+RPY_SCALE = 0.6
+SV_SCALE = 0.1
+CHPD_SCALE = 0.0005
+YAW_SCALE = 1.5
+
+# AGENT PARAMS
+CD_SCALE = 0.05
+SLV_SCALE = 0.05
+RESIDUALS_SCALE = 0.03
+Z_SCALE = 0.05
+# Filter actions
+alpha = 0.7
+# Added this to avoid filtering residuals
+# -1 for all
+
+
+class SpotCommander():
+    def __init__(self):
+
+        rospy.init_node('Policies', anonymous=True)
+        self.agents = {}
+        # self.movetypes = [
+        #     "Forward", "Backward", "Left", "Right", "CW", "CCW", "Stop"
+        # ]
+        self.movetypes = ["Stop"]
+        self.mini_cmd = MiniCmd()
+        self.jb = JoyButtons()
+        self.mini_cmd.x_velocity = 0.0
+        self.mini_cmd.y_velocity = 0.0
+        self.mini_cmd.rate = 0.0
+        self.mini_cmd.roll = 0.0
+        self.mini_cmd.pitch = 0.0
+        self.mini_cmd.yaw = 0.0
+        self.mini_cmd.z = 0.0
+        self.mini_cmd.motion = "Stop"
+        self.mini_cmd.movement = "Stepping"
+        # FIXED
+        self.BaseStepVelocity = 0.1
+        self.StepVelocity = self.BaseStepVelocity
+        # Stock, use Bumpers to change
+        self.BaseSwingPeriod = 0.2
+        self.SwingPeriod = self.BaseSwingPeriod
+        # Stock, use arrow pads to change
+        self.BaseClearanceHeight = 0.04
+        self.BasePenetrationDepth = 0.005
+        self.ClearanceHeight = self.BaseClearanceHeight
+        self.PenetrationDepth = self.BasePenetrationDepth
+
+        self.load_spot()
+        # mini_cmd_cb from mini_cmd topic
+        self.sub_cmd = rospy.Subscriber('mini_cmd', MiniCmd, self.mini_cmd_cb)
+        self.sub_jb = rospy.Subscriber('joybuttons', JoyButtons, self.jb_cb)
+        self.time = rospy.get_time()
+        print("READY TO GO!")
+
+    def load_spot(self):
+
+        self.env = spotBezierEnv(render=True,
+                                 on_rack=False,
+                                 height_field=False,
+                                 draw_foot_path=False)
+
+        self.env.reset()
+
+        seed = 0
+        # Set seeds
+        self.env.seed(seed)
+        np.random.seed(seed)
+
+        state_dim = self.env.observation_space.shape[0]
+        print("STATE DIM: {}".format(state_dim))
+        action_dim = self.env.action_space.shape[0]
+        print("ACTION DIM: {}".format(action_dim))
+        max_action = float(self.env.action_space.high[0])
+        print("RECORDED MAX ACTION: {}".format(max_action))
+
+        self.state = self.env.reset()
+
+        # Load Spot Model
+        self.spot = SpotModel()
+
+        self.dt = self.env._time_step
+
+        self.T_bf0 = self.spot.WorldToFoot
+        self.T_bf = copy.deepcopy(self.T_bf0)
+
+        self.bzg = BezierGait(dt=self.env._time_step)
+
+    def mini_cmd_cb(self, mini_cmd):
+        """ Reads the desired Minitaur command and passes it for execution
+
+            Args: mini_cmd
+        """
+        try:
+            # Update mini_cmd
+            self.mini_cmd = mini_cmd
+            # log input data as debug-level message
+            rospy.logdebug(mini_cmd)
+        except rospy.ROSInterruptException:
+            pass
+
+    def jb_cb(self, jb):
+        """ Reads the desired additional joystick buttons
+
+            Args: jb
+        """
+        try:
+            # Update jb
+            self.jb = jb
+            # log input data as debug-level message
+            rospy.logdebug(jb)
+        except rospy.ROSInterruptException:
+            pass
+
+    def move(self):
+        """ Turn joystick inputs into commands
+        """
+
+        if self.mini_cmd.motion != "Stop":
+            self.StepVelocity = self.BaseStepVelocity
+            self.SwingPeriod = np.clip(
+                self.BaseSwingPeriod +
+                (-self.mini_cmd.faster + -self.mini_cmd.slower) * SV_SCALE,
+                0.1, 0.3)
+            if self.mini_cmd.movement == "Stepping":
+                StepLength = self.mini_cmd.x_velocity + abs(
+                    self.mini_cmd.y_velocity * 0.66)
+                StepLength = np.clip(StepLength, -1.0, 1.0)
+                StepLength *= STEPLENGTH_SCALE
+                LateralFraction = self.mini_cmd.y_velocity * np.pi / 2
+                YawRate = self.mini_cmd.rate * YAW_SCALE
+                # x offset
+                pos = np.array(
+                    [0.0, 0.0, self.mini_cmd.z * Z_SCALE_CTRL])
+                orn = np.array([0.0, 0.0, 0.0])
+            else:
+                StepLength = 0.0
+                LateralFraction = 0.0
+                YawRate = 0.0
+                # RESET
+                self.ClearanceHeight = self.BaseClearanceHeight
+                self.PenetrationDepth = self.BasePenetrationDepth
+                self.StepVelocity = self.BaseStepVelocity
+                # x offset
+                pos = np.array(
+                    [0.0, 0.0, self.mini_cmd.z * Z_SCALE_CTRL])
+                orn = np.array([
+                    self.mini_cmd.roll * RPY_SCALE,
+                    self.mini_cmd.pitch * RPY_SCALE,
+                    self.mini_cmd.yaw * RPY_SCALE
+                ])
+        else:
+            StepLength = 0.0
+            LateralFraction = 0.0
+            YawRate = 0.0
+            # RESET
+            self.ClearanceHeight = self.BaseClearanceHeight
+            self.PenetrationDepth = self.BasePenetrationDepth
+            self.StepVelocity = self.BaseStepVelocity
+            self.SwingPeriod = self.BaseSwingPeriod
+            pos = np.array([0.0, 0.0, 0.0])
+            orn = np.array([0.0, 0.0, 0.0])
+
+        # TODO: integrate into controller
+        self.ClearanceHeight += self.jb.updown * CHPD_SCALE
+        self.PenetrationDepth += self.jb.leftright * CHPD_SCALE
+
+        # Manual Reset
+        if self.jb.left_bump or self.jb.right_bump:
+            self.ClearanceHeight = self.BaseClearanceHeight
+            self.PenetrationDepth = self.BasePenetrationDepth
+            self.StepVelocity = self.BaseStepVelocity
+            self.SwingPeriod = self.BaseSwingPeriod
+            self.env.reset()
+
+
+        # print("SL: {} \tSV: {} \nLAT: {} \tYAW: {}".format(
+        #     StepLength, self.StepVelocity, LateralFraction, YawRate))
+        # print("BASE VEL: {}".format(self.BaseStepVelocity))
+        # print("---------------------------------------")
+
+        contacts = self.state[-4:]
+
+        # Time
+        dt = rospy.get_time() - self.time
+        # print("dt: {}".format(dt))
+        self.time = rospy.get_time()
+
+        # Update Step Period
+        self.bzg.Tswing = self.SwingPeriod
+
+        self.T_bf = self.bzg.GenerateTrajectory(StepLength, LateralFraction,
+                                                YawRate, self.StepVelocity,
+                                                self.T_bf0, self.T_bf,
+                                                self.ClearanceHeight,
+                                                self.PenetrationDepth,
+                                                contacts, dt)
+
+        joint_angles = self.spot.IK(orn, pos, self.T_bf)
+        self.env.pass_joint_angles(joint_angles.reshape(-1))
+        # Get External Observations
+        # TODO
+        # self.env.spot.GetExternalObservations(bzg, bz_step)
+        # Step
+        action = self.env.action_space.sample()
+        action[:] = 0.0
+        self.state, reward, done, _ = self.env.step(action)
+
+
+def main():
+    """ The main() function. """
+    mini_commander = SpotCommander()
+    rate = rospy.Rate(600.0)
+    while not rospy.is_shutdown():
+        # This is called continuously. Has timeout functionality too
+        mini_commander.move()
+        rate.sleep()
+        # rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

mock/simulator/mini_ros/src/spot_real_interface

@@ -0,0 +1,388 @@
+#!/usr/bin/env python
+"""
+DESCRIPTION:
+
+SUBSCRIBERS:
+"""
+
+from __future__ import division
+import rospy
+import numpy as np
+from mini_ros.msg import MiniCmd, JoyButtons, IMUdata, ContactData, AgentData, JointAngles
+import copy
+import sys
+import os
+import rospkg
+rospack = rospkg.RosPack()
+
+sys.path.append(rospack.get_path('mini_ros') + '/../')
+
+sys.path.append('../../')
+
+from spotmicro.Kinematics.SpotKinematics import SpotModel
+from spotmicro.GaitGenerator.Bezier import BezierGait
+from spot_bullet.src.ars_lib.ars import ARSAgent, Normalizer, Policy
+from spotmicro.GymEnvs.spot_bezier_env import spotBezierEnv
+
+# Initialize Node
+rospy.init_node('Policies', anonymous=True)
+
+# Controller Params
+STEPLENGTH_SCALE = rospy.get_param("STEPLENGTH_SCALE")
+Z_SCALE_CTRL = rospy.get_param("Z_SCALE_CTRL")
+RPY_SCALE = rospy.get_param("RPY_SCALE")
+SV_SCALE = rospy.get_param("SV_SCALE")
+CHPD_SCALE = rospy.get_param("CHPD_SCALE")
+YAW_SCALE = rospy.get_param("YAW_SCALE")
+
+# AGENT PARAMS
+CD_SCALE = rospy.get_param("CD_SCALE")
+SLV_SCALE = rospy.get_param("SLV_SCALE")
+RESIDUALS_SCALE = rospy.get_param("RESIDUALS_SCALE")
+Z_SCALE = rospy.get_param("Z_SCALE")
+# Filter actions
+alpha = rospy.get_param("alpha")
+# Added this to avoid filtering residuals
+# -1 for all
+actions_to_filter = rospy.get_param("actions_to_filter")
+
+
+class SpotCommander():
+    def __init__(self, Agent=True, contacts=False):
+
+        self.Agent = Agent
+        self.agent_num = rospy.get_param("agent_num")
+        self.movetypes = ["Stop"]
+        self.mini_cmd = MiniCmd()
+        self.jb = JoyButtons()
+        self.mini_cmd.x_velocity = 0.0
+        self.mini_cmd.y_velocity = 0.0
+        self.mini_cmd.rate = 0.0
+        self.mini_cmd.roll = 0.0
+        self.mini_cmd.pitch = 0.0
+        self.mini_cmd.yaw = 0.0
+        self.mini_cmd.z = 0.0
+        self.mini_cmd.motion = "Stop"
+        self.mini_cmd.movement = "Stepping"
+        # FIXED
+        self.BaseStepVelocity = rospy.get_param("BaseStepVelocity")
+        self.StepVelocity = copy.deepcopy(self.BaseStepVelocity)
+        # Stock, use Bumpers to change
+        self.BaseSwingPeriod = rospy.get_param("Tswing")
+        self.SwingPeriod = copy.deepcopy(self.BaseSwingPeriod)
+        self.SwingPeriod_LIMITS = rospy.get_param("SwingPeriod_LIMITS")
+        # Stock, use arrow pads to change
+        self.BaseClearanceHeight = rospy.get_param("BaseClearanceHeight")
+        self.BasePenetrationDepth = rospy.get_param("BasePenetrationDepth")
+        self.ClearanceHeight = copy.deepcopy(self.BaseClearanceHeight)
+        self.PenetrationDepth = copy.deepcopy(self.BasePenetrationDepth)
+        self.ClearanceHeight_LIMITS = rospy.get_param("ClearanceHeight_LIMITS")
+        self.PenetrationDepth_LIMITS = rospy.get_param(
+            "PenetrationDepth_LIMITS")
+
+        # Time
+        self.time = rospy.get_time()
+
+        self.enable_contact = contacts
+
+        # Contacts: FL, FR, BL, BR
+        self.contacts = [0, 0, 0, 0]
+
+        # IMU: R, P, Ax, Ay, Az, Gx, Gy, Gz
+        self.imu = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
+        # See spot_params.yaml in config
+        self.spot = SpotModel(
+            shoulder_length=rospy.get_param("shoulder_length"),
+            elbow_length=rospy.get_param("elbow_length"),
+            wrist_length=rospy.get_param("wrist_length"),
+            hip_x=rospy.get_param("hip_x"),
+            hip_y=rospy.get_param("hip_y"),
+            foot_x=rospy.get_param("foot_x"),
+            foot_y=rospy.get_param("foot_y"),
+            height=rospy.get_param("height"),
+            com_offset=rospy.get_param("com_offset"))
+
+        self.T_bf0 = self.spot.WorldToFoot
+        self.T_bf = copy.deepcopy(self.T_bf0)
+
+        # See spot_params.yaml in config
+        self.bzg = BezierGait(dt=rospy.get_param("dt"),
+                              Tswing=rospy.get_param("Tswing"))
+
+        if self.Agent:
+            self.load_spot(contacts, agent_num=self.agent_num)
+        # cmd_cb from mini_cmd topic
+        self.sub_cmd = rospy.Subscriber('mini_cmd',
+                                        MiniCmd,
+                                        self.cmd_cb,
+                                        queue_size=1)
+        self.sub_jb = rospy.Subscriber('joybuttons',
+                                       JoyButtons,
+                                       self.jb_cb,
+                                       queue_size=1)
+        self.sub_imu = rospy.Subscriber('spot/imu',
+                                        IMUdata,
+                                        self.imu_cb,
+                                        queue_size=1)
+        self.sub_cnt = rospy.Subscriber('spot/contact',
+                                        ContactData,
+                                        self.cnt_cb,
+                                        queue_size=1)
+        self.ag_pub = rospy.Publisher('spot/agent', AgentData, queue_size=1)
+        self.ja_pub = rospy.Publisher('spot/joints', JointAngles, queue_size=1)
+        print("READY TO GO!")
+
+    def load_spot(self, contacts, state_dim=12, action_dim=14, agent_num=0):
+        self.policy = Policy(state_dim=state_dim, action_dim=action_dim)
+        self.normalizer = Normalizer(state_dim=state_dim)
+        env = spotBezierEnv(render=False,
+                            on_rack=False,
+                            height_field=False,
+                            draw_foot_path=False)
+        agent = ARSAgent(self.normalizer, self.policy, env)
+        my_path = os.path.abspath(os.path.dirname(__file__))
+        if contacts:
+            models_path = os.path.join(my_path,
+                                       "../../spot_bullet/models/contact")
+        else:
+            models_path = os.path.join(my_path,
+                                       "../../spot_bullet/models/no_contact")
+
+        print("MODEL PATH: {}".format(my_path))
+        file_name = "spot_ars_"
+        if os.path.exists(models_path + "/" + file_name + str(agent_num) +
+                          "_policy"):
+            print("Loading Existing agent: {}".format(agent_num))
+            agent.load(models_path + "/" + file_name + str(agent_num))
+            agent.policy.episode_steps = np.inf
+            self.policy = agent.policy
+
+        self.action = np.zeros(action_dim)
+        self.old_act = self.action[:actions_to_filter]
+
+    def imu_cb(self, imu):
+        """ Reads the IMU
+
+            Args: imu
+        """
+        try:
+            # Update imu
+            self.imu = [
+                imu.roll, imu.pitch,
+                np.radians(imu.gyro_x),
+                np.radians(imu.gyro_y),
+                np.radians(imu.gyro_z), imu.acc_x, imu.acc_y, imu.acc_z - 9.81
+            ]
+            # log input data as debug-level message
+            rospy.logdebug(imu)
+        except rospy.ROSInterruptException:
+            pass
+
+    def cnt_cb(self, cnt):
+        """ Reads the Contact Sensors
+
+            Args: cnt
+        """
+        try:
+            # Update contacts
+            self.contacts = [cnt.FL, cnt.FR, cnt.BL, cnt.BR]
+            # log input data as debug-level message
+            rospy.logdebug(cnt)
+        except rospy.ROSInterruptException:
+            pass
+
+    def cmd_cb(self, mini_cmd):
+        """ Reads the desired Minitaur command and passes it for execution
+
+            Args: mini_cmd
+        """
+        try:
+            # Update mini_cmd
+            self.mini_cmd = mini_cmd
+            # log input data as debug-level message
+            rospy.logdebug(mini_cmd)
+        except rospy.ROSInterruptException:
+            pass
+
+    def jb_cb(self, jb):
+        """ Reads the desired additional joystick buttons
+
+            Args: jb
+        """
+        try:
+            # Update jb
+            self.jb = jb
+            # log input data as debug-level message
+            rospy.logdebug(jb)
+        except rospy.ROSInterruptException:
+            pass
+
+    def move(self):
+        """ Turn joystick inputs into commands
+        """
+
+        # Move Type
+        if self.mini_cmd.movement == "Stepping":
+            step_or_view = False
+        else:
+            step_or_view = True
+
+        if self.mini_cmd.motion != "Stop":
+            self.StepVelocity = copy.deepcopy(self.BaseStepVelocity)
+            self.SwingPeriod = np.clip(
+                copy.deepcopy(self.BaseSwingPeriod) +
+                (-self.mini_cmd.faster + -self.mini_cmd.slower) * SV_SCALE,
+                self.SwingPeriod_LIMITS[0], self.SwingPeriod_LIMITS[1])
+            if self.mini_cmd.movement == "Stepping":
+                # Only take 2/3 of lateral step length or it will be too great.
+                StepLength = self.mini_cmd.x_velocity + abs(
+                    self.mini_cmd.y_velocity * 0.66)
+                StepLength = np.clip(StepLength, -1.0, 1.0)
+                StepLength *= STEPLENGTH_SCALE
+                # Convert lateral joystick action into poolar coordinates
+                # for lateral movement
+                LateralFraction = self.mini_cmd.y_velocity * np.pi / 2
+                YawRate = self.mini_cmd.rate * YAW_SCALE
+                # NOTE: NO HEIGHT MOD DURING WALK
+                pos = np.array([0.0, 0.0, 0.0])
+                orn = np.array([0.0, 0.0, 0.0])
+            else:
+                StepLength = 0.0
+                LateralFraction = 0.0
+                YawRate = 0.0
+                # RESET
+                self.ClearanceHeight = copy.deepcopy(self.BaseClearanceHeight)
+                self.PenetrationDepth = copy.deepcopy(
+                    self.BasePenetrationDepth)
+                self.StepVelocity = copy.deepcopy(self.BaseStepVelocity)
+                # x offset
+                pos = np.array([0.0, 0.0, self.mini_cmd.z * Z_SCALE_CTRL])
+                orn = np.array([
+                    self.mini_cmd.roll * RPY_SCALE,
+                    self.mini_cmd.pitch * RPY_SCALE,
+                    self.mini_cmd.yaw * RPY_SCALE
+                ])
+        else:
+            StepLength = 0.0
+            LateralFraction = 0.0
+            YawRate = 0.0
+            # RESET
+            self.ClearanceHeight = self.BaseClearanceHeight
+            self.PenetrationDepth = self.BasePenetrationDepth
+            self.StepVelocity = self.BaseStepVelocity
+            self.SwingPeriod = self.BaseSwingPeriod
+            pos = np.array([0.0, 0.0, 0.0])
+            orn = np.array([0.0, 0.0, 0.0])
+
+        # TODO: integrate into controller
+        self.ClearanceHeight += self.jb.updown * CHPD_SCALE
+        self.PenetrationDepth += self.jb.leftright * CHPD_SCALE
+
+        # Manual Reset
+        if self.jb.left_bump or self.jb.right_bump:
+            self.ClearanceHeight = copy.deepcopy(self.BaseClearanceHeight)
+            self.PenetrationDepth = copy.deepcopy(self.BasePenetrationDepth)
+            self.StepVelocity = copy.deepcopy(self.BaseStepVelocity)
+            self.SwingPeriod = copy.deepcopy(self.BaseSwingPeriod)
+
+        # OPTIONAL: Agent
+        if self.Agent and self.mini_cmd.motion != "Stop":
+            phases = copy.deepcopy(self.bzg.Phases)
+            # Total 12
+            state = []
+            # r, p, gz, gy, gz, ax, ay, az (8)
+            state.extend(self.imu)
+            # FL, FR, BL, BR (4)
+            state.extend(phases)
+            # FL, FR, BL, BR (4)
+            if self.enable_contact:
+                state.extend(self.contacts)
+            self.normalizer.observe(state)
+            # Don't normalize contacts
+            state[:-4] = self.normalizer.normalize(state)[:-4]
+            self.action = self.policy.evaluate(state, None, None)
+            self.action = np.tanh(self.action)
+            # EXP FILTER
+            self.action[:actions_to_filter] = alpha * self.old_act + (
+                1.0 - alpha) * self.action[:actions_to_filter]
+            self.old_act = self.action[:actions_to_filter]
+
+            self.ClearanceHeight += self.action[0] * CD_SCALE
+
+        # Time
+        dt = rospy.get_time() - self.time
+        # print("dt: {}".format(dt))
+        self.time = rospy.get_time()
+
+        # Update Step Period
+        self.bzg.Tswing = self.SwingPeriod
+
+        # CLIP
+        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])
+
+        self.T_bf = self.bzg.GenerateTrajectory(StepLength, LateralFraction,
+                                                YawRate, self.StepVelocity,
+                                                self.T_bf0, self.T_bf,
+                                                self.ClearanceHeight,
+                                                self.PenetrationDepth,
+                                                self.contacts, dt)
+
+        T_bf_copy = copy.deepcopy(self.T_bf)
+        # OPTIONAL: Agent
+        if self.Agent and self.mini_cmd.motion != "Stop":
+            self.action[2:] *= RESIDUALS_SCALE
+            # Add DELTA to XYZ Foot Poses
+            T_bf_copy["FL"][:3, 3] += self.action[2:5]
+            T_bf_copy["FR"][:3, 3] += self.action[5:8]
+            T_bf_copy["BL"][:3, 3] += self.action[8:11]
+            T_bf_copy["BR"][:3, 3] += self.action[11:14]
+            pos[2] += abs(self.action[1]) * Z_SCALE
+
+        joint_angles = self.spot.IK(orn, pos, T_bf_copy)
+
+        ja_msg = JointAngles()
+
+        ja_msg.fls = np.degrees(joint_angles[0][0])
+        ja_msg.fle = np.degrees(joint_angles[0][1])
+        ja_msg.flw = np.degrees(joint_angles[0][2])
+
+        ja_msg.frs = np.degrees(joint_angles[1][0])
+        ja_msg.fre = np.degrees(joint_angles[1][1])
+        ja_msg.frw = np.degrees(joint_angles[1][2])
+
+        ja_msg.bls = np.degrees(joint_angles[2][0])
+        ja_msg.ble = np.degrees(joint_angles[2][1])
+        ja_msg.blw = np.degrees(joint_angles[2][2])
+
+        ja_msg.brs = np.degrees(joint_angles[3][0])
+        ja_msg.bre = np.degrees(joint_angles[3][1])
+        ja_msg.brw = np.degrees(joint_angles[3][2])
+
+        # Move Type
+        ja_msg.step_or_view = step_or_view
+
+        self.ja_pub.publish(ja_msg)
+
+
+def main():
+    """ The main() function. """
+    mini_commander = SpotCommander()
+    rate = rospy.Rate(600.0)
+    while not rospy.is_shutdown():
+        # This is called continuously. Has timeout functionality too
+        mini_commander.move()
+        rate.sleep()
+        # rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

mock/simulator/mini_ros/src/spot_sm.cpp

@@ -0,0 +1,184 @@
+/// \file
+/// \brief Draws Each Obstacle in RViz using MarkerArrays
+///
+/// PARAMETERS:
+/// PUBLISHES:
+/// SUBSCRIBES:
+/// FUNCTIONS:
+
+#include <ros/ros.h>
+
+#include <math.h>
+#include <string>
+#include <vector>
+
+#include <mini_ros/spot.hpp>
+#include <mini_ros/teleop.hpp>
+#include "mini_ros/MiniCmd.h"
+#include "std_srvs/Empty.h"
+#include "std_msgs/Bool.h"
+
+// Global Vars
+spot::Spot spot_mini = spot::Spot();
+bool teleop_flag = false;
+bool motion_flag = false;
+bool ESTOP = false;
+// Init Time
+ros::Time current_time;
+ros::Time last_time;
+
+
+void teleop_callback(const geometry_msgs::Twist &tw)
+{ 
+  /// \brief cmd_vel subscriber callback. Records commanded twist
+  ///
+  /// \param tw (geometry_msgs::Twist): the commanded linear and angular velocity
+  /** 
+  * This function runs every time we get a geometry_msgs::Twist message on the "cmd_vel" topic.
+  * We generally use the const <message>ConstPtr &msg syntax to prevent our node from accidentally
+  * changing the message, in the case that another node is also listening to it.
+  */
+  
+  spot_mini.update_command(tw.linear.x, tw.linear.y, tw.linear.z, tw.angular.z, tw.angular.x, tw.angular.y);
+}
+
+void estop_callback(const std_msgs::Bool &estop)
+{ 
+  if (estop.data)
+  {
+    spot_mini.update_command(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    motion_flag = true;
+    if (!ESTOP)
+    {
+      ROS_ERROR("ENGAGING MANUAL E-STOP!");
+      ESTOP = true;
+    } else
+    {
+      ROS_WARN("DIS-ENGAGING MANUAL E-STOP!");
+      ESTOP = false;
+    }
+  }
+
+  last_time = ros::Time::now();
+}
+
+
+bool swm_callback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+/// Switches the Movement mode from FB (Forward/Backward) to LR (Left/Right)
+/// and vice versa
+{
+    spot_mini.switch_movement();
+    motion_flag = true;
+    return true;
+}
+
+
+int main(int argc, char** argv)
+/// The Main Function ///
+{
+    ROS_INFO("STARTING NODE: spot_mini State Machine");
+
+    // Vars
+    double frequency = 5;
+    // Seconds for timeout
+    double timeout = 1.0;
+
+    ros::init(argc, argv, "mini_sm_node"); // register the node on ROS
+    ros::NodeHandle nh; // get a handle to ROS
+    ros::NodeHandle nh_("~"); // get a handle to ROS
+    // Parameters
+    nh_.getParam("frequency", frequency);
+
+    // Init Subscriber
+    ros::Subscriber teleop_sub = nh.subscribe("teleop", 1, teleop_callback);
+    ros::Subscriber estop_sub = nh.subscribe("estop", 1, estop_callback);
+    // Init Command Publisher
+    ros::Publisher mini_pub = nh.advertise<mini_ros::MiniCmd>("mini_cmd", 1);
+
+    // Init Switch Movement Service Server
+    ros::ServiceServer switch_movement_server = nh.advertiseService("switch_movement", swm_callback);
+
+    // Init MiniCmd
+    mini_ros::MiniCmd mini_cmd;
+    // Placeholder
+    mini_cmd.x_velocity = 0.0;
+    mini_cmd.y_velocity = 0.0;
+    mini_cmd.rate = 0.0;
+    mini_cmd.roll = 0.0;
+    mini_cmd.pitch = 0.0;
+    mini_cmd.yaw = 0.0;
+    mini_cmd.z = 0.0;
+    mini_cmd.faster = 0.0;
+    mini_cmd.slower = 0.0;
+    mini_cmd.motion = "Stop";
+    mini_cmd.movement = "Stepping";
+
+    ros::Rate rate(frequency);
+    current_time = ros::Time::now();
+    last_time = ros::Time::now();
+    // Main While
+    while (ros::ok())
+    {
+        ros::spinOnce();
+        current_time = ros::Time::now();
+
+        spot::SpotCommand cmd = spot_mini.return_command();
+
+        // Condition for sending non-stop command
+        if (!motion_flag and !(current_time.toSec() - last_time.toSec() > timeout) and !ESTOP)
+        {
+          mini_cmd.x_velocity = cmd.x_velocity;
+          mini_cmd.y_velocity = cmd.y_velocity;
+          mini_cmd.rate = cmd.rate;
+          mini_cmd.roll = cmd.roll;
+          mini_cmd.pitch = cmd.pitch;
+          mini_cmd.yaw = cmd.yaw;
+          mini_cmd.z = cmd.z;
+          mini_cmd.faster = cmd.faster;
+          mini_cmd.slower = cmd.slower;
+          // Now convert enum to string
+          // Motion
+          if (cmd.motion == spot::Go)
+          {
+            mini_cmd.motion = "Go";
+          } else
+          {
+            mini_cmd.motion = "Stop";
+          }
+          // Movement
+          if (cmd.movement == spot::Stepping)
+          {
+            mini_cmd.movement = "Stepping";
+          } else
+          {
+            mini_cmd.movement = "Viewing";
+          }
+
+        } else
+        {
+          mini_cmd.x_velocity = 0.0;
+          mini_cmd.y_velocity = 0.0;
+          mini_cmd.rate = 0.0;
+          mini_cmd.roll = 0.0;
+          mini_cmd.pitch = 0.0;
+          mini_cmd.yaw = 0.0;
+          mini_cmd.z = 0.0;
+          mini_cmd.faster = 0.0;
+          mini_cmd.slower = 0.0;
+          mini_cmd.motion = "Stop";
+        }
+
+        if (current_time.toSec() - last_time.toSec() > timeout)
+        {
+          ROS_ERROR("TIMEOUT...ENGAGING E-STOP!");
+        }
+
+        // Now publish
+        mini_pub.publish(mini_cmd);
+        motion_flag = false;
+        
+        rate.sleep();
+    }
+
+    return 0;
+}

mock/simulator/mini_ros/src/teleop_node.cpp

@@ -0,0 +1,134 @@
+/// \file
+/// \brief Draws Each Obstacle in RViz using MarkerArrays
+///
+/// PARAMETERS:
+/// PUBLISHES:
+/// SUBSCRIBES:
+/// FUNCTIONS:
+
+#include <ros/ros.h>
+
+#include <math.h>
+#include <string>
+#include <vector>
+
+#include <mini_ros/spot.hpp>
+#include <mini_ros/teleop.hpp>
+#include "mini_ros/MiniCmd.h"
+#include "std_msgs/Bool.h"
+#include "std_srvs/Empty.h"
+#include "mini_ros/JoyButtons.h"
+
+#include <functional>  // To use std::bind
+
+
+int main(int argc, char** argv)
+/// The Main Function ///
+{
+    ROS_INFO("STARTING NODE: Teleoperation");
+
+    // Vars
+    double frequency = 60;
+    int linear_x = 4;
+    int linear_y = 3;
+    int linear_z = 1;
+    int angular = 0;
+    int sw = 0;
+    int es = 1;
+    int RB = 5;
+    int LB = 2;
+    int RT = 5;
+    int LT = 4;
+    int UD = 7;
+    int LR = 6;
+    double l_scale = 1.0;
+    double a_scale = 1.0;
+    double B_scale = 1.0;
+    double debounce_thresh = 0.15; // sec
+
+    ros::init(argc, argv, "teleop_node"); // register the node on ROS
+    ros::NodeHandle nh; // get a handle to ROS
+    ros::NodeHandle nh_("~"); // get a handle to ROS
+    // Parameters
+    nh_.getParam("frequency", frequency);
+    nh_.getParam("axis_linear_x", linear_x);
+    nh_.getParam("axis_linear_y", linear_y);
+    nh_.getParam("axis_linear_z", linear_z);
+    nh_.getParam("axis_angular", angular);
+    nh_.getParam("scale_linear", l_scale);
+    nh_.getParam("scale_angular", a_scale);
+    nh_.getParam("scale_bumper", B_scale);
+    nh_.getParam("button_switch", sw);
+    nh_.getParam("button_estop", es);
+    nh_.getParam("rb", RB);
+    nh_.getParam("lb", LB);
+    nh_.getParam("rt", RT);
+    nh_.getParam("lt", LT);
+    nh_.getParam("updown", UD);
+    nh_.getParam("leftright", LR);
+    nh_.getParam("debounce_thresh", debounce_thresh);
+
+    tele::Teleop teleop = tele::Teleop(linear_x, linear_y, linear_z, angular,
+                                       l_scale, a_scale, LB, RB, B_scale, LT,
+                                       RT, UD, LR, sw, es);
+
+    // Init Switch Movement Server
+    ros::ServiceClient switch_movement_client = nh.serviceClient<std_srvs::Empty>("switch_movement");
+    ros::service::waitForService("switch_movement", -1);
+
+    // Init ESTOP Publisher
+    ros::Publisher estop_pub = nh.advertise<std_msgs::Bool>("estop", 1);
+    // Init Command Publisher
+    ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("teleop", 1);
+    // Init Joy Button Publisher
+    ros::Publisher jb_pub = nh.advertise<mini_ros::JoyButtons>("joybuttons", 1);
+
+    // Init Subscriber (also handles pub)
+    // TODO: Figure out how to use std::bind properly
+    // ros::Subscriber joy_sub = nh.subscribe<sensor_msgs::Joy>("joy", 1, std::bind(&tele::Teleop::joyCallback, std::placeholders::_1, vel_pub), &teleop);
+    ros::Subscriber joy_sub = nh.subscribe<sensor_msgs::Joy>("joy", 1, &tele::Teleop::joyCallback, &teleop);
+
+    ros::Rate rate(frequency);
+
+    // Record time for debouncing buttons
+    ros::Time current_time = ros::Time::now();
+    ros::Time last_time = ros::Time::now();
+
+
+    // Main While
+    while (ros::ok())
+    {
+        ros::spinOnce();
+        current_time = ros::Time::now();
+
+        std_msgs::Bool estop;
+        estop.data = teleop.return_estop();
+
+        if (estop.data and current_time.toSec() - last_time.toSec() >= debounce_thresh)
+        {
+            ROS_INFO("SENDING E-STOP COMMAND!");
+            last_time = ros::Time::now();
+        } else if (!teleop.return_trigger())
+        {
+          // Send Twist
+          vel_pub.publish(teleop.return_twist());
+          estop.data = 0;
+        } else if (current_time.toSec() - last_time.toSec() >= debounce_thresh)
+        {
+          // Call Switch Service
+          std_srvs::Empty e;
+          switch_movement_client.call(e);
+          estop.data = 0;
+          last_time = ros::Time::now();
+        }
+        // pub buttons
+        jb_pub.publish(teleop.return_buttons());
+
+        estop_pub.publish(estop);
+        
+        
+        rate.sleep();
+    }
+
+    return 0;
+}

mock/simulator/mini_ros/srv/CalibServo.srv

@@ -0,0 +1,6 @@
+# Request
+int8 servo_num
+int32 servo_pulse
+---
+# Response
+string Response

mock/simulator/mini_ros/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/
+

mock/simulator/mini_ros/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)