Encoding positions of the end-effector using the SNS-Toolbox

There is an example of how to map joint angles into positions of the leg. To get a basic understanding of how to design neural networks using SNS, please go to the:

https://sns-toolbox.readthedocs.io/en/latest/#

This example is based on the conference paper: Zadokha, B., Szczecinski, N. S.: Encoding 3D Leg Kinematics using Spatially-Distributed, Population Coded Network Model. (2024)

Trossen Robotics Hexapod IV

The leg configuration

Initial Setup

The neural network was designed and simulated through PyCharm 2022.

import scipy
import numpy as np
from sns_toolbox.connections import NonSpikingSynapse
import sns_toolbox.networks
import matplotlib.pyplot as plt
from sns_toolbox.neurons import NonSpikingNeuron
import time
import seaborn as sns

For simulation, SNS requires a time vector:

dt = 1 # step
tMax = 2e3 # time vector length (2000 ms)
t = np.arange(0, tMax, dt)
numSteps = len(t)

Training Data

Forward Kinematics

For forward kinematics calculations, the Product of Exponentials formula was used:

$$T = e^{S_{n-1}\theta_{n-1}}(e^{S_n\theta_n}M)$$

def FrwrdKnmtcsFunc(w, thetaValue, p):
    expoOne = np.eye(4, dtype=float)
    if hasattr(thetaValue, "__len__"):
        for index in range(len(thetaValue)):
            expo = RodriguesFunc(w[:, index], thetaValue[index], p[:, index])
            expoOne = np.matmul(expoOne, expo)
    else:
        expo = RodriguesFunc(w[:, 0], thetaValue, p[:, 0])
        expoOne = np.matmul(expoOne, expo)
    positionM = np.atleast_2d(p[:, -1]).T
    # print(positionM)
    M = np.concatenate((np.concatenate((np.eye(3, dtype=float), positionM), axis=1),
                        np.array([[0, 0, 0, 1]])), axis=0)
    TranslMat = np.matmul(expoOne, M)
    return TranslMat

To calculate all exponents, Rodrigues' rotation formula was used:

$$e^{S\theta} = I+\sin\theta\hat{[S]}+(1-\cos\theta)\hat{[S]}^2$$

where S with hat is a skew-symmetric representation of a screw axis matrix S

def RodriguesFunc(w, theta, p):
    Scross = np.cross(-w, p)
    Smatrix = [[0.0, -w[2], w[1]], [w[2], 0.0, -w[0]], [-w[1], w[0], 0.0]]  # Skew-symmetric
    Rmatrix = np.eye(3, dtype=float)+np.dot(Smatrix, np.sin(theta))+np.dot(np.matmul(Smatrix, Smatrix), (1-np.cos(theta)))
    c = np.matmul(np.matmul(np.eye(3, dtype=float)-Rmatrix, Smatrix), Scross)
    d = np.matmul(w, w.reshape((3, 1)))
    e = np.dot(theta, np.dot(d.item(), Scross))
    TranslVec = c+e
    f = np.concatenate((Rmatrix, TranslVec.reshape((3, 1))), axis=1)
    expoFinal = np.concatenate((f, np.array([[0, 0, 0, 1]])), axis=0)
    return expoFinal

Zero position of the leg and axis matrix:

# R1 leg
L1 = 52.52 # mm
L2 = 60 # mm
L3 = 95 # mm
# distance from the CoM (0,0,0)
x0 = 125 # mm
y02 = 100 # mm
y01 = 65 # mm
y03 = 65 # mm

w1 = np.array([[0], [0], [1]])
# R1 leg
w2R1 = np.array([[1/np.sqrt(2)], [1/np.sqrt(2)], [0]])
wR1 = np.concatenate((np.concatenate((w1, w2R1), axis=1), w2R1), axis=1)

# zero position of the R1 leg
p1R1 = np.array([[x0], [-y01], [0]])
p2R1 = np.array([[x0+L1*np.cos(45*np.pi/180)],
                 [-y02-L1*np.cos(45*np.pi/180)],
                 [0]])  # mm
p3R1 = np.array([[x0+(L1+L2)*np.cos(45*np.pi/180)],
                 [-y02-(L1+L2)*np.cos(45*np.pi/180)],
                 [-15]])  # mm
p4R1 = np.array([[x0+(L1+L2+L3)*np.cos(45*np.pi/180)],
                 [-y02-(L1+L2+L3)*np.cos(45*np.pi/180)],
                 [-15-90]])  # mm
pR1 = np.concatenate((np.concatenate((np.concatenate((p1R1, p2R1), axis=1), p3R1), axis=1), p4R1), axis=1)

The network use only servos 2 and 3 as inputs. To generate data x, y, and z positions were calculated using joint angle vector from -1.6 to 1.6 rads:

numJoints = 2 # number of joints
numSensPerJoint = 11 # number of interneuron compartments

mag = 1 # activation function magnitude
delayInit = 2 # membrane capacitance of the neuron
widthbellInit = 20 # width parameter of the bell curve

thetaMin = -1.6 # min joint angle
thetaMax = 1.6 # max joint angle
# joint angles vector for mapping
jointTheta = np.linspace(thetaMin, thetaMax, numSensPerJoint)

# output: xyz2, xyz3, xyzEnd
ActualX = np.zeros([len(jointTheta), len(jointTheta)])
ActualY = np.zeros([len(jointTheta), len(jointTheta)])
ActualZ = np.zeros([len(jointTheta), len(jointTheta)])

for i in range(len(jointTheta)):
    for j in range(len(jointTheta)):
        p1TrnslMat = FrwrdKnmtcsFunc(wR1, np.array([0, jointTheta[i], jointTheta[j]]), pR1)
        ActualX[i, j] = p1TrnslMat[0, 3]  # x coord
        ActualY[i, j] = p1TrnslMat[1, 3]  # y coord
        ActualZ[i, j] = p1TrnslMat[2, 3]  # z coord

Inputs

Generating two patterns of joint angles over time:

T2 = 400
T3 = 500
theta2vec = (thetaMin + thetaMax) / 2 + (thetaMax - thetaMin) / 2 * np.sin(2 * np.pi * t / T2)
theta3vec = (thetaMin + thetaMax) / 2 + (thetaMax - thetaMin) / 2 * np.sin(2 * np.pi * t / T3)

Bell curve activation function

For every joint, sensory neurons to get an encoded implementation of an input are defined by using the Gaussian bell curve equation:

def bell_curve(magnitude, width, theta, shift):
    return magnitude*np.exp(-width*pow((theta-shift), 2))

Neural Network Design

widthBell is the width of the sensory-encoding bell curves. Also note that smaller value of widthBell makes the curves broader, delay is a membrane capacitance of the neuron

def frwrdKnmtcsSNS3D(delay, numJoints, numSensPerJoint, numSteps,
                   widthBell, ActualMapX, ActualMapY, ActualMapZ,
                   servo2vec, servo3vec, jointTheta):

Create a network object:

    net = sns_toolbox.networks.Network(name='endPointPrediction')

Create a basic nonspiking neuron:

    bellNeur = NonSpikingNeuron(membrane_capacitance=delay, membrane_conductance=1)

Create the "bell curve" neurons, which are the sensory (input) neurons. Each sensory neuron has a bell-curve receptive field with a unique "preferred angle", that is, the angle at which the peak response is evoked:

# servo 2
for index in range(numSensPerJoint):
    nameStr2 = 'Bell2' + str(index)
    net.add_neuron(neuron_type=bellNeur, name=nameStr2)
    net.add_input(nameStr2)
    net.add_output(nameStr2)
# servo 3
for index in range(numSensPerJoint):
    nameStr3 = 'Bell3' + str(index)
    net.add_neuron(neuron_type=bellNeur, name=nameStr3)
    net.add_input(nameStr3)
    net.add_output(nameStr3)

Define key neuron and synapse parameters. 'mag' is like R in the functional subnetwork publications: it is the maximum expected neural activation. delE is the synaptic reversal potential relative to the rest potential of the neurons:

mag = 1
delE = 20

Initialize empty arrays to store input current for each set of sensory neurons (theta2 = input2, theta3 = input3), the input current for the whole network (inputNet), and the validation data X(t) (ActualT):

input2 = np.zeros([numSteps, numSensPerJoint])  # bell curve responses
input3 = np.zeros([numSteps, numSensPerJoint])  # bell curve responses
inputNet = np.zeros([numSteps, numSensPerJoint * numJoints])
# Actual X, Y, Z values as a function of time
ActualT = np.zeros([numSteps, 3])

Each sensory neuron synapses onto a number of "combo" neurons. Each combo neuron receives synaptic input from one sensory neuron from each joint. All these synapses may be identical, because they are simply generating all the possible combinations of the joint angles/independent variables. All the combo neurons may be identical, because they are simply integrating the joint angles:

kIdent = 1
gIdent = kIdent * mag / (delE - kIdent * mag)  # From Szczecinski, Hunt, and Quinn 2017
identitySyn = NonSpikingSynapse(max_conductance=gIdent, reversal_potential=delE, e_lo=0, e_hi=mag)
comboNeur = NonSpikingNeuron(membrane_capacitance=delay, membrane_conductance=1, bias=0)
net.add_neuron(neuron_type=bellNeur, name='OutputEndX')
net.add_neuron(neuron_type=bellNeur, name='OutputEndY')
net.add_neuron(neuron_type=bellNeur, name='OutputEndZ')

For the network to perform the desired calculation, the synapses from each combo neuron to the output neuron should have effective gains (i.e., Vout/Vcombo) that are proportional to the value that the output neuron encodes. Here, we take all the "training data", that is, the actual X coordinate of the leg, normalize it to values between 0 and 1, and use those to set the unique properties of the synapse from each combo neuron to the output neuron:

mapVectorX = np.matrix(ActualMapX).getA1()
mapVectorY = np.matrix(ActualMapY).getA1()
mapVectorZ = np.matrix(ActualMapZ).getA1()

mapNormX = (mapVectorX - min(mapVectorX)) / (max(mapVectorX - min(mapVectorX)))
mapNormY = (mapVectorY - min(mapVectorY)) / (max(mapVectorY - min(mapVectorY)))
mapNormZ = (mapVectorZ - min(mapVectorZ)) / (max(mapVectorZ - min(mapVectorZ)))

Now we create the each combo neuron, its input connections, and its output connections. Here, we have 2 nested for loops, one for each joint/independent variable. A higher-dimensional network would require more for loops or an approach in which we NDgrid the joint angles/sensory neurons and then use a single for loop:

k = 0
for i in range(numSensPerJoint):
    for j in range(numSensPerJoint):
        nameStr = 'combo' + str(i) + str(j)
        net.add_neuron(neuron_type=comboNeur, name=nameStr)
        net.add_connection(identitySyn, source='Bell2' + str(i), destination=nameStr)
        net.add_connection(identitySyn, source='Bell3' + str(j), destination=nameStr)
        net.add_output(nameStr)

SNS-Toolbox does not enable synapses with max_cond = 0, so if kSyn = 0, we must pass it machine epsilon instead:

        kSynX = max(mapNormX[k], np.finfo(float).eps)
        kSynY = max(mapNormY[k], np.finfo(float).eps)
        kSynZ = max(mapNormZ[k], np.finfo(float).eps)
        gX = kSynX * mag / (delE - kSynX * mag)
        gY = kSynY * mag / (delE - kSynX * mag)
        gZ = kSynZ * mag / (delE - kSynX * mag)

Synapses from the combo neurons to the output neuron(s) each have unique conductance value that corresponds to the desired output in that scenario. Note that the e_lo for the synapses = mag and e_hi = 2*mag, because multiple combo neurons will be active at any time, but we only care about the most active, so setting e_lo this way serves as a threshold mechanism:

        synOutputX = NonSpikingSynapse(max_conductance=gX, reversal_potential=delE, e_lo=mag, e_hi=2 * mag)
        synOutputY = NonSpikingSynapse(max_conductance=gY, reversal_potential=delE, e_lo=mag, e_hi=2 * mag)
        synOutputZ = NonSpikingSynapse(max_conductance=gZ, reversal_potential=delE, e_lo=mag, e_hi=2 * mag)
        net.add_connection(synOutputX, source=nameStr, destination='OutputEndX')
        net.add_connection(synOutputY, source=nameStr, destination='OutputEndY')
        net.add_connection(synOutputZ, source=nameStr, destination='OutputEndZ')
        # Increment k, which is a linear counting variable through all the for loops.
        k += 1
net.add_output('OutputEndX')
net.add_output('OutputEndY')
net.add_output('OutputEndZ')

The input current for the theta2 and theta3 sensory neurons using the bellCurve function:

for i in range(numSensPerJoint):
    input2[:, i] = bell_curve(mag, widthBell, theta=servo2vec, shift=jointTheta[i])
    input3[:, i] = bell_curve(mag, widthBell, theta=servo3vec, shift=jointTheta[i])

Concatenate the inputs into one network input array. Calculate the "Actual X, Y, Z values as a function of time", ActualT:

for i in range(len(t)):
    inputNet[i, :] = np.concatenate((input2[i, :], input3[i, :]), axis=None)
    p1TrnslMat = FrwrdKnmtcsFunc(wR1, np.array([0, theta2vec[i], theta3vec[i]]), pR1)
    ActualT[i, 0] = p1TrnslMat[0, 3]  # x coord
    ActualT[i, 1] = p1TrnslMat[1, 3]  # y coord
    ActualT[i, 2] = p1TrnslMat[2, 3]  # z coord

Create an output matrix for the network:

numOut = net.get_num_outputs()
PredictedOut = np.zeros([numSteps, numOut])

Compile and render the network model:

model = net.compile(backend='numpy', dt=dt)
render(net, view=True, save=True, filename='MapNet', img_format='png')

Simulate the network response:

for i in range(len(t)):
    PredictedOut[i, :] = model(inputNet[i, :])
return PredictedOut, ActualT

PredictedOutOne, ActualTOne = frwrdKnmtcsSNS3D(delay=delayInit, numJoints=numJoints, numSensPerJoint=numSensPerJointInit,
                                       numSteps=numSteps, widthBell=widthbellInit,
                                       ActualMapX=ActualX, ActualMapY=ActualY, ActualMapZ=ActualZ,
                                       servo2vec=theta2vec, servo3vec=theta3vec, jointTheta=jointTheta)

Note: to get an actual prediction for x, y, and z positions those output vectors must be denormalized:

predOutXOne = min(ActualTOne[:, 0]) + (max(ActualTOne[:, 0]) - min(ActualTOne[:, 0])) * PredictedOutOne[:, -3] / mag
predOutYOne = min(ActualTOne[:, 1]) + (max(ActualTOne[:, 1]) - min(ActualTOne[:, 1])) * PredictedOutOne[:, -2] / mag
predOutZOne = min(ActualTOne[:, 2]) + (max(ActualTOne[:, 2]) - min(ActualTOne[:, 2])) * PredictedOutOne[:, -1] / mag