Months have passed. It is 29 December 2020 now, and Miranda and I are at Bitwäscherei, in Zurich, during the remoteC3 (Chaos Computer Club)
We have a few things to get back to:
- Locomotion
- Sim2Real
- Vision
- Object detection / segmentation
We plan to work on a chicken cam, but are waiting for the raspberry pi camera to arrive. So first, locomotion.
Miranda would like to add more leg segments, but process-wise, we need the software and hardware working together with basics, before adding more parts.
I’ve tested the GPIO, and it’s working, after adjusting the minimum pulse value for the MG996R servo.
So, where did we leave off, before KonS MFRU / ICAF ?
Locomotion:
There was a walking table in simulation, and the progress was saving so that we could reload the state, in Ray and Tune.
I remember the best walker had a ‘reward’ of 902, so I searched for 902
grep -R ‘episode_reward_mean\”\: 902’
And found these files:
409982 Aug 1 07:52 events.out.tfevents.1596233543.chrx
334 Jul 31 22:12 params.json
304 Jul 31 22:12 params.pkl
132621 Aug 1 07:52 progress.csv
1542332 Aug 1 07:52 result.json
and there are checkpoint directories, with binary files.
So what are these files? How do I extract actions?
Well it looks like this info keeps track of Ray/Tune progress. If we want logs, we seem to need to make them ourselves. The original minitaur code used google protobuf to log state. So I set the parameter to log to a directory.
log_path="/media/chrx/0FEC49A4317DA4DA/logs"
So now when I run it again, it makes a file in the format below:
message RobotableEpisode {
// The state-action pair at each step of the log.
repeated RobotableStateAction state_action = 1;
}
message RobotableMotorState {
// The current angle of the motor.
double angle = 1;
// The current velocity of the motor.
double velocity = 2;
// The current torque exerted at this motor.
double torque = 3;
// The action directed to this motor. The action is the desired motor angle.
double action = 4;
}
message RobotableStateAction {
// Whether the state/action information is valid. It is always true if the
// proto is from simulation. It might be false when communication error
// happens on robotable hardware.
bool info_valid = 6;
// The time stamp of this step. It is computed since the reset of the
// environment.
google.protobuf.Timestamp time = 1;
// The position of the base of the minitaur.
robotics.messages.Vector3d base_position = 2;
// The orientation of the base of the minitaur. It is represented as (roll,
// pitch, yaw).
robotics.messages.Vector3d base_orientation = 3;
// The angular velocity of the base of the minitaur. It is the time derivative
// of (roll, pitch, yaw).
robotics.messages.Vector3d base_angular_vel = 4;
// The motor states (angle, velocity, torque, action) of eight motors.
repeated RobotableMotorState motor_states = 5;
}
I’m pretty much only interested in that last line,
repeated RobotableMotorState motor_states = 5;
So that’s the task, to decode the protobuf objects.
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
import argparse
from gym_robotable.envs import logging
if __name__ == "__main__":
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--log_file', help='path to protobuf file', default='/media/chrx/0FEC49A4317DA4DA/logs/robotable_log_2020-12-29-191602')
args = parser.parse_args()
logging = logging.RobotableLogging()
episode = logging.restore_episode(args.log_file)
print(dir (episode))
print("episode=",episode)
fields = episode.ListFields()
for field in fields:
print(field)This prints out some json-like info. On the right path.
time {
seconds: 5
}
base_position {
x: 0.000978083148860855
y: 1.7430418253385236
z: -0.0007063670972221042
}
base_orientation {
x: -0.026604138524100478
y: 0.00973575985636693
z: -0.08143286338936992
}
base_angular_vel {
x: 0.172553297157456
y: -0.011541306494121786
z: -0.010542314686643973
}
motor_states {
angle: -0.12088901254000686
velocity: -0.868766524998517
torque: 3.3721667267908284
action: 3.6539504528045654
}
motor_states {
angle: 0.04232669165311699
velocity: 1.5488756496627718
torque: 3.4934419908437704
action: 1.4116498231887817
}
motor_states {
angle: 0.8409251448232009
velocity: -1.617737108768752
torque: -3.3539541961507124
action: -3.7024881839752197
}
motor_states {
angle: 0.13926660037454777
velocity: -0.9575437158301312
torque: 3.563701581854714
action: 1.104300618171692
}
info_valid: true
])
We will have to experiment to work out how to translate this data to something usable. The servos are controlled with throttle, from -1 to 1.
Technically I should probably rewrite the simulation to output this “throttle” value. But let’s work with what we have, for now.
My first attempt will be to extract and normalize the torque values to get a sequence of actions… nope.
Ok so plan B. Some visualisation of the values should help. I have it outputting the JSON now.
episode_proto = logging.restore_episode(args.log_file)
jsonObj = MessageToJson(episode_proto)
print(jsonObj)
I decided to use StreamLit, which is integrated with various plotting libraries. After looking at the different plotting options, Plotly seems the most advanced.
Ok so in JSON,
{
"time": "1970-01-01T00:00:05Z",
"basePosition": {
"x": 0.000978083148860855,
"y": 1.7430418253385236,
"z": -0.0007063670972221042
},
"baseOrientation": {
"x": -0.026604138524100478,
"y": 0.00973575985636693,
"z": -0.08143286338936992
},
"baseAngularVel": {
"x": 0.172553297157456,
"y": -0.011541306494121786,
"z": -0.010542314686643973
},
"motorStates": [
{
"angle": -0.12088901254000686,
"velocity": -0.868766524998517,
"torque": 3.3721667267908284,
"action": 3.6539504528045654
},
{
"angle": 0.04232669165311699,
"velocity": 1.5488756496627718,
"torque": 3.4934419908437704,
"action": 1.4116498231887817
},
{
"angle": 0.8409251448232009,
"velocity": -1.617737108768752,
"torque": -3.3539541961507124,
"action": -3.7024881839752197
},
{
"angle": 0.13926660037454777,
"velocity": -0.9575437158301312,
"torque": 3.563701581854714,
"action": 1.104300618171692
}
],
"infoValid": true
}Plotly uses Panda dataframes, which is tabular data. 2 dimensions. So I need to transform this to something usable.
Something like time on the x-axis
and angle / velocity / torque / action on the y axis.
Ok so how to do this…?
Well I’ve almost got it, but I mostly had to give up on StreamLit’s native line_chart for now. Plotly’s has line chart code that can handle multiple variables. So I’m getting sidetracked by this bug:
When I import plotly’s library,
import plotly.graph_objects as go
“No module named ‘plotly.graph_objects’; ‘plotly’ is not a package”
https://stackoverflow.com/questions/57105747/modulenotfounderror-no-module-named-plotly-graph-objects
import plotly.graph_objects as go ? no…
from plotly import graph_objs as go ? no…
from plotly import graph_objects as go ? no…
hmm.
pip3 install plotly==4.14.1 ? Requirement already satisfied: plotly==4.14.1 in /usr/local/lib/python3.6/dist-packages (4.14.1)
no… why are the docs wrong then?
Ah ha.
“This is the well known name shadowing trap.” – stackoverflow
I named my file plotly.py – that is the issue.
So, ok run it again… (streamlit run plot.py) and open localhost:8501…
Now,
Running as root without --no-sandbox is not supported. See https://crbug.com/638180
Ah ha. I went back to StreamLit notation and it worked.
#fig.show()
st.plotly_chart(fig)Ok excellent, so here is my first round of code:
import pandas as pd
import numpy as np
import streamlit as st
import time
from plotly import graph_objects as go
import os
import inspect
from google.protobuf.json_format import MessageToJson
import argparse
from gym_robotable.envs import logging
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
if __name__ == "__main__":
st.title('Analyticz')
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--log_file', help='path to protobuf file', default='/media/chrx/0FEC49A4317DA4DA/logs/robotable_log_2020-12-29-191602')
args = parser.parse_args()
logging = logging.RobotableLogging()
episode_proto = logging.restore_episode(args.log_file)
times = []
angles = [[]]*4 # < bugs!
velocities = [[]]*4
torques = [[]]*4
actions = [[]]*4
for step in range(len(episode_proto.state_action)):
step_log = episode_proto.state_action[step]
times.append(str(step_log.time.seconds) + '.' + str(step_log.time.nanos))
for i in range(4):
angles[i].append(step_log.motor_states[i].angle)
velocities[i].append(step_log.motor_states[i].velocity)
torques[i].append(step_log.motor_states[i].torque)
actions[i].append(step_log.motor_states[i].action)
print(angles)
print(times)
print(len(angles))
print(len(velocities))
print(len(torques))
print(len(actions))
print(len(times))
# Create traces
fig = go.Figure()
fig.add_trace(go.Scatter(x=times, y=angles[0],
mode='lines',
name='Angles'))
fig.add_trace(go.Scatter(x=times, y=velocities[0],
mode='lines+markers',
name='Velocities'))
fig.add_trace(go.Scatter(x=times, y=torques[0],
mode='markers',
name='Torques'))
fig.add_trace(go.Scatter(x=times, y=actions[0],
mode='markers',
name='Actions'))
st.plotly_chart(fig)
And it’s plotting data for one leg.
If this is just 5 seconds of simulation, then velocities looks like it might be the closest match. You can imagine it going up a bit, back a bit, then a big step forward.
So, one idea is to do symbolic regression, to approximate the trigonometry equations for quadrupedal walking, (or just google them), and generalise to a walking algorithm, to use for locomotion. I could use genetic programming, like at university (https://gplearn.readthedocs.io/en/stable/examples.html#symbolic-regressor). But that’s overkill and probably won’t work. Gotta smooth the graph incrementally. Normalize it.
Let’s see what happens next, visually, after 5 seconds of data, and then view the same, for other legs.
Ok there is 30 seconds of walking.
The tools I wrote for the walker, are run with ‘python3 play_tune.py –replay 1’. It looks for the best checkpoint and replays it from there.
But now I seem to be getting the same graph for different legs. What? We’re going to have to investigate.
Ok turns out [[]]*4 is the wrong way to initialise arrays in python. It makes all sublists the same. Here’s the correct way:
velocities = [[] for i in range(4)]
Now I have 4 different legs.
The graph is very spiky, so I’ve added a rolling window average, and normalised it between -1 and 1 since that’s what the servo throttle allows.
I am thinking that maybe because the range between min and max for the 4 legs are:
3.1648572819886085
1.7581604444983845
5.4736002843351805
1.986915632875287
The rear legs aren’t moving as much, so maybe it doesn’t make sense to normalize them all to [-1, 1] all on the same scale. Like maybe the back right leg that moves so much should be normalized to [-1, 1] and then all the other legs are scaled down proportionally. Anyway, let’s see. Good enough for now.
In the code, the motors order is:
front right, front left, back right, back left.
Ok so to save the outputs…
import pandas as pd
import numpy as np
import streamlit as st
import time
from plotly import graph_objects as go
import os
import inspect
from google.protobuf.json_format import MessageToJson
import argparse
from gym_robotable.envs import logging
import plotly.express as px
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
def normalize_negative_one(img):
normalized_input = (img - np.amin(img)) / (np.amax(img) - np.amin(img))
return 2*normalized_input - 1
if name == "main":
st.title('Analyticz')
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--log_file', help='path to protobuf file', default='/media/chrx/0FEC49A4317DA4DA/walkinglogs/robotable_log_2021-01-17-231240')
args = parser.parse_args()
logging = logging.RobotableLogging()
episode_proto = logging.restore_episode(args.log_file)
times = []
velocities = [[] for i in range(4)]
for step in range(len(episode_proto.state_action)):
step_log = episode_proto.state_action[step]
times.append(str(step_log.time.seconds) + '.' + str(step_log.time.nanos))
for i in range(4):
velocities[i].append(step_log.motor_states[i].velocity)
#truncate because a bunch of trailing zeros
velocities[0] = velocities[0][0:3000]
velocities[1] = velocities[1][0:3000]
velocities[2] = velocities[2][0:3000]
velocities[3] = velocities[3][0:3000]
times = times[0:3000]
#get moving averages
window_size_0=40
numbers_series_0 = pd.Series(velocities[0])
windows_0 = numbers_series_0.rolling(window_size_0)
moving_averages_0 = windows_0.mean()
moving_averages_list_0 = moving_averages_0.tolist()
without_nans_0 = moving_averages_list_0[window_size_0 - 1:]
window_size_1=40
numbers_series_1 = pd.Series(velocities[1])
windows_1 = numbers_series_1.rolling(window_size_1)
moving_averages_1 = windows_1.mean()
moving_averages_list_1 = moving_averages_1.tolist()
without_nans_1 = moving_averages_list_1[window_size_1 - 1:]
window_size_2=40
numbers_series_2 = pd.Series(velocities[2])
windows_2 = numbers_series_2.rolling(window_size_2)
moving_averages_2 = windows_2.mean()
moving_averages_list_2 = moving_averages_2.tolist()
without_nans_2 = moving_averages_list_2[window_size_2 - 1:]
window_size_3=40
numbers_series_3 = pd.Series(velocities[3])
windows_3 = numbers_series_3.rolling(window_size_3)
moving_averages_3 = windows_3.mean()
moving_averages_list_3 = moving_averages_3.tolist()
without_nans_3 = moving_averages_list_3[window_size_3 - 1:]
#normalize between -1 and 1
avg_0 = np.asarray(without_nans_0)
avg_1 = np.asarray(without_nans_1)
avg_2 = np.asarray(without_nans_2)
avg_3 = np.asarray(without_nans_3)
avg_0 = normalize_negative_one(avg_0)
avg_1 = normalize_negative_one(avg_1)
avg_2 = normalize_negative_one(avg_2)
avg_3 = normalize_negative_one(avg_3)
np.save('velocity_front_right', avg_0)
np.save('velocity_front_left', avg_1)
np.save('velocity_back_right', avg_2)
np.save('velocity_back_left', avg_3)
np.save('times', times)
# Create traces
fig0 = go.Figure()
fig0.add_trace(go.Scatter(x=times, y=velocities[0],
mode='lines',
name='Velocities 0'))
fig0.add_trace(go.Scatter(x=times, y=avg_0.tolist(),
mode='lines',
name='Norm Moving Average 0'))
st.plotly_chart(fig0)
fig1 = go.Figure()
fig1.add_trace(go.Scatter(x=times, y=velocities[1],
mode='lines',
name='Velocities 1'))
fig1.add_trace(go.Scatter(x=times, y=avg_1.tolist(),
mode='lines',
name='Norm Moving Average 1'))
st.plotly_chart(fig1)
fig2 = go.Figure()
fig2.add_trace(go.Scatter(x=times, y=velocities[2],
mode='lines',
name='Velocities 2'))
fig2.add_trace(go.Scatter(x=times, y=avg_2.tolist(),
mode='lines',
name='Norm Moving Average 2'))
st.plotly_chart(fig2)
fig3 = go.Figure()
fig3.add_trace(go.Scatter(x=times, y=velocities[3],
mode='lines',
name='Velocities 3'))
fig3.add_trace(go.Scatter(x=times, y=avg_3.tolist(),
mode='lines',
name='Norm Moving Average 3'))
st.plotly_chart(fig3)
(Excuse the formatting.) Then I’m loading those npy files and iterating them to the motors.
import time
import numpy as np
from board import SCL, SDA
import busio
from adafruit_pca9685 import PCA9685
from adafruit_motor import servo
i2c = busio.I2C(SCL, SDA)
pca = PCA9685(i2c, reference_clock_speed=25630710)
pca.frequency = 50
servo0 = servo.ContinuousServo(pca.channels[0], min_pulse=685, max_pulse=2280)
servo1 = servo.ContinuousServo(pca.channels[1], min_pulse=810, max_pulse=2095)
servo2 = servo.ContinuousServo(pca.channels[2], min_pulse=700, max_pulse=2140)
servo3 = servo.ContinuousServo(pca.channels[3], min_pulse=705, max_pulse=2105)
velocity_front_right = np.load('velocity_front_right.npy')
velocity_front_left = np.load('velocity_front_left.npy')
velocity_back_right = np.load('velocity_back_right.npy')
velocity_back_left = np.load('velocity_back_left.npy')
times = np.load('times.npy')
reverse left motors
velocity_front_left = -velocity_front_left
velocity_back_left = -velocity_back_left
print (velocity_front_right.size)
print (velocity_front_left.size)
print (velocity_back_right.size)
print (velocity_back_left.size)
print (times.size)
for time in times:
print(time)
for a,b,c,d in np.nditer([velocity_front_right, velocity_front_left, velocity_back_right, velocity_back_left]):
servo0.throttle = a/4
servo1.throttle = b/4
servo2.throttle = c/4
servo3.throttle = d/4
print (a, b, c, d)
servo0.throttle = 0
servo1.throttle = 0
servo2.throttle = 0
servo3.throttle = 0
pca.deinit()
Honestly it doesn’t look terrible, but these MG996R continuous rotation servos are officially garbage.
While we wait for new servos to arrive, I’m testing on SG90s. I’ve renormalized about 90 degrees
def normalize_0_180(img):
normalized_0_180 = (180*(img - np.min(img))/np.ptp(img)).astype(int)
return normalized_0_180
That was also a bit much variation, and since we’ve got 180 degree servos now, but it still looks a bit off, I halved the variance in the test.
velocity_front_right = ((velocity_front_right - 90)/2)+90 velocity_front_left = ((velocity_front_left - 90)/2)+90 velocity_back_right = ((velocity_back_right - 90)/2)+90 velocity_back_left = ((velocity_back_left - 90)/2)+90 # reverse left motors velocity_front_left = 180-velocity_front_left velocity_back_left = 180-velocity_back_left
lol. ok. Sim2Real.
So, it’s not terrible, but we’re not quite there either. Also i think it’s walking backwards.
I am not sure the math is correct.
I changed the smoothing code to use this code which smoothes based on the preceding plot.
def anchor(signal, weight):
buffer = []
last = signal[0]
for i in signal:
smoothed_val = last * weight + (1 - weight) * i
buffer.append(smoothed_val)
last = smoothed_val
return buffer
OK i realised I was wrong all along. Two things.
First, I just didn’t see that the angles values were on that original graph. They were so small. Of course we’re supposed to use the angles, rather than the velocities, for 180 degree servos.
Second problem was, I was normalizing from min to max of the graph. Of course it should be -PI/2 to PI/2, since the simulator works with radians, obviously. Well anyway, hindsight is 20/20. Now we have a fairly accurate sim2real. I use the anchor code above twice, to get a really smooth line.
Here’s the final code.
import pandas as pd
import numpy as np
import streamlit as st
import time
from plotly import graph_objects as go
import os
import inspect
from google.protobuf.json_format import MessageToJson
import argparse
from gym_robotable.envs import logging
import plotly.express as px
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
def anchor(signal, weight):
buffer = []
last = signal[0]
for i in signal:
smoothed_val = last * weight + (1 - weight) * i
buffer.append(smoothed_val)
last = smoothed_val
return buffer
assume radians
def normalize_0_180(img):
normalized_0_180 = np.array(img)*57.2958 + 90
return normalized_0_180
if name == "main":
st.title('Analyticz')
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--log_file', help='path to protobuf file', default='/media/chrx/0FEC49A4317DA4DA/walkinglogs/robotable_log_2021-01-17-231240')
args = parser.parse_args()
logging = logging.RobotableLogging()
episode_proto = logging.restore_episode(args.log_file)
times = []
angles = [[] for i in range(4)]
for step in range(len(episode_proto.state_action)):
step_log = episode_proto.state_action[step]
times.append(str(step_log.time.seconds) + '.' + str(step_log.time.nanos))
for i in range(4):
print (step)
print (step_log.motor_states[i].angle)
angles[i].append(step_log.motor_states[i].angle)
#truncate because a bunch of trailing zeros
angles[0] = angles[0][0:3000]
angles[1] = angles[1][0:3000]
angles[2] = angles[2][0:3000]
angles[3] = angles[3][0:3000]
avg_0 = normalize_0_180(angles[0])
avg_1 = normalize_0_180(angles[1])
avg_2 = normalize_0_180(angles[2])
avg_3 = normalize_0_180(angles[3])
avg_0 = anchor(avg_0, 0.8)
avg_1 = anchor(avg_1, 0.8)
avg_2 = anchor(avg_2, 0.8)
avg_3 = anchor(avg_3, 0.8)
avg_0 = anchor(avg_0, 0.8)
avg_1 = anchor(avg_1, 0.8)
avg_2 = anchor(avg_2, 0.8)
avg_3 = anchor(avg_3, 0.8)
avg_0 = anchor(avg_0, 0.8)
avg_1 = anchor(avg_1, 0.8)
avg_2 = anchor(avg_2, 0.8)
avg_3 = anchor(avg_3, 0.8)
np.save('angle_front_right_180', avg_0)
np.save('angle_front_left_180', avg_1)
np.save('angle_back_right_180', avg_2)
np.save('angle_back_left_180', avg_3)
# Create traces
fig0 = go.Figure()
fig0.add_trace(go.Scatter(x=times, y=angles[0],
mode='lines',
name='Angles 0'))
fig0.add_trace(go.Scatter(x=times, y=avg_0,
mode='lines',
name='Norm Moving Average 0'))
st.plotly_chart(fig0)
fig1 = go.Figure()
fig1.add_trace(go.Scatter(x=times, y=angles[1],
mode='lines',
name='Angles 1'))
fig1.add_trace(go.Scatter(x=times, y=avg_1,
mode='lines',
name='Norm Moving Average 1'))
st.plotly_chart(fig1)
fig2 = go.Figure()
fig2.add_trace(go.Scatter(x=times, y=angles[2],
mode='lines',
name='Angles 2'))
fig2.add_trace(go.Scatter(x=times, y=avg_2,
mode='lines',
name='Norm Moving Average 2'))
st.plotly_chart(fig2)
fig3 = go.Figure()
fig3.add_trace(go.Scatter(x=times, y=angles[3],
mode='lines',
name='Angles 3'))
fig3.add_trace(go.Scatter(x=times, y=avg_3,
mode='lines',
name='Norm Moving Average 3'))
st.plotly_chart(fig3)
OK.
So there’s a milestone that took way too long. We’ve got Sim 2 Real working, ostensibly.
After some fortuitous googling, I found the Spot Micro, or, Spot Mini Mini project. The Spot Micro guys still have a big focus on inverse kinematics, which I’m trying to avoid for as long as I can.
They’ve done a very similar locomotion project using pyBullet, and I was able to find a useful paper, in the inspiration section, alerting me to kMPs.
Kinematic Motion Primitives. It’s a similar idea to what I did above.
Instead, what these guys did was to take a single wave of their leg data, and repeat that, and compare that to a standardized phase. (More or less). Makes sense. Looks a bit complicated to work out the phase of the wave in my case.
I’ll make a new topic, and try to extract kMPs from the data, for the next round of locomotion sim2real. I will probably also train the robot for longer, to try evolve a gait that isn’t so silly.
