def __init__(self, args):
self.name = "Homeostasis_ROS"
# create motor message
self.motorMessage = Float32MultiArray()
# create predError message
self.predErrorMessage = Float32()
# create phase increment message
self.phaseIncrementMessage = Float32()
# initialize ros node
rospy.init_node(self.name)
# initialize publishers and subscribers
self.pub = rospy.Publisher("/homeostasis_motor",
Float32MultiArray,
queue_size=5)
self.predErrorPub = rospy.Publisher("/predictionError",
Float32,
queue_size=5)
self.phaseIncrementPub = rospy.Publisher("/phaseIncrement",
Float32,
queue_size=5)
self.sub = rospy.Subscriber("/tinyImu", tinyIMU,
self.receiveSensorValues)
# initialize homeokinesis
self.homeostasis = Homeostasis(dim_m=1, dim_s=120)
self.homeostasis.setLearningRates(0.3, 0.1)
if args.verbose:
print("initialization finished")
self.minX = np.ones(6) * 100000
self.maxX = np.ones(6) * -100000
self.avgX = np.zeros(6)
self.phase1 = 0
self.phase2 = 0
self.motor1 = 0
self.motor2 = 0
self.phaseIncrement = 0
class Homeostasis_ROS(object):
def __init__(self, args):
self.name = "Homeostasis_ROS"
# create motor message
self.motorMessage = Float32MultiArray()
# initialize ros node
rospy.init_node(self.name)
# initialize publishers and subscribers
self.pub = rospy.Publisher("/homeostasis_motor",
Float32MultiArray,
queue_size=5)
self.sub = rospy.Subscriber("/homeostasis_sensor", Float32MultiArray,
self.receiveSensorValues)
# initialize homeokinesis
self.homeostasis = Homeostasis(dim_m=1, dim_s=2)
if args.verbose:
printWithTime("initialization finished")
def receiveSensorValues(self, msg):
sensorData = np.array(msg.data).reshape((2, 1))
if args.verbose:
printWithTime("SensorData received")
print(sensorData)
self.homeostasis.x = sensorData
self.homeostasis.learningStep()
self.motorMessage.data = [self.homeostasis.y[0, 0]]
self.pub.publish(self.motorMessage)
if args.verbose:
printWithTime("MotorData sent")
print(self.motorMessage.data)
def __init__(self, args):
self.name = "Homeostasis_ROS"
# create motor message
self.motorMessage = Float32MultiArray()
# initialize ros node
rospy.init_node(self.name)
# initialize publishers and subscribers
self.pub = rospy.Publisher("/homeostasis_motor",
Float32MultiArray,
queue_size=5)
self.sub = rospy.Subscriber("/homeostasis_sensor", Float32MultiArray,
self.receiveSensorValues)
# initialize homeokinesis
self.homeostasis = Homeostasis(dim_m=1, dim_s=2)
if args.verbose:
printWithTime("initialization finished")
class Homeostasis_ROS(object):
def __init__(self, args):
self.name = "Homeostasis_ROS"
# create motor message
self.motorMessage = Float32MultiArray()
# create predError message
self.predErrorMessage = Float32()
# create phase increment message
self.phaseIncrementMessage = Float32()
# initialize ros node
rospy.init_node(self.name)
# initialize publishers and subscribers
self.pub = rospy.Publisher("/homeostasis_motor",
Float32MultiArray,
queue_size=5)
self.predErrorPub = rospy.Publisher("/predictionError",
Float32,
queue_size=5)
self.phaseIncrementPub = rospy.Publisher("/phaseIncrement",
Float32,
queue_size=5)
self.sub = rospy.Subscriber("/tinyImu", tinyIMU,
self.receiveSensorValues)
# initialize homeokinesis
self.homeostasis = Homeostasis(dim_m=1, dim_s=120)
self.homeostasis.setLearningRates(0.3, 0.1)
if args.verbose:
print("initialization finished")
self.minX = np.ones(6) * 100000
self.maxX = np.ones(6) * -100000
self.avgX = np.zeros(6)
self.phase1 = 0
self.phase2 = 0
self.motor1 = 0
self.motor2 = 0
self.phaseIncrement = 0
def receiveSensorValues(self, msg):
#sensorData = np.array(msg.data).reshape((2,1))
if args.verbose:
print("SensorData received: " + time.strftime('%X'))
print(msg)
self.homeostasis.x = np.roll(self.homeostasis.x, 6)
self.homeostasis.x[0] = (msg.accel.x + 32768.0) / (2.0 * 32768.0)
self.homeostasis.x[1] = (msg.accel.y + 32768.0) / (2.0 * 32768.0)
self.homeostasis.x[2] = (msg.accel.z + 32768.0) / (2.0 * 32768.0)
self.homeostasis.x[3] = (msg.gyro.x + 32768.0) / (2.0 * 32768.0)
self.homeostasis.x[4] = (msg.gyro.y + 32768.0) / (2.0 * 32768.0)
self.homeostasis.x[5] = (msg.gyro.z + 32768.0) / (2.0 * 32768.0)
print("x: " + str(self.homeostasis.x))
# recurrent
#self.homeostasis.x[6] = self.motor1
#self.homeostasis.x[7] = self.motor2
#self.homeostasis.x[8] = self.phase1
#self.homeostasis.x[9] = self.phase2
#self.homeostasis.x[10] = self.phaseIncrement
#self.homeostasis.x[11] = self.phaseOffset
#for i in range(0,6):
# if(self.homeostasis.x[i] > self.maxX[i]):
# self.maxX[i] = self.homeostasis.x[i]
# if(self.homeostasis.x[i] < self.minX[i]):
# self.minX[i] = self.homeostasis.x[i]
#
# self.avgX[i] = self.avgX[i] * 0.99 + self.homeostasis.x[i] * 0.01
#
# print("x " + str(i) + " min: " + str(self.minX[i]) + " max " + str(self.maxX[i]) + " avg " + str(self.avgX[i]))
self.homeostasis.learningStep()
print("C: " + str(self.homeostasis.controller.C))
print("A: %s" % self.homeostasis.model.A)
# min 0.5 Hz, max 3 Hz:
# min 1 Pi Rad/s bei 100Hz pi/20 Increments per step
# max 6 Pi Rad/s bei 100Hz 4pi/20 Increments per step
# phase Increment is between -1 and 1
# +1 /2 [0; 1]
# * 5 pi [0; 5pi]
# + 1 pi [1pi; 6pi]
# /100 [1pi/100; 4pi/100]
# ()((incr + 1)/ 2) * 5 pi + 1 pi) / 100)
self.phaseIncrement = self.homeostasis.y[0, 0]
#print("increment: " + str(self.phaseIncrement))
self.phaseIncrement = ((
(self.phaseIncrement + 1) / 2) * 3 * np.pi + 1 * np.pi) / 20
print("self.phaseIncrement")
#self.phaseOffset1 += self.homeostasis.y[1,0] * np.pi * 2.0
#print("offset1: " + str(self.phaseOffset1))
#self.phaseOffset2 += self.homeostasis.y[2,0] * np.pi * 2.0
#print("offset2: " + str(self.phaseOffset2))
#self.phaseOffset3 += self.homeostasis.y[3,0] * np.pi * 2.0
#print("offset3: " + str(self.phaseOffset3))
print("prediction: %s" % self.homeostasis.xPred)
print("PredictionError: " +
str(self.homeostasis.calculatePredictionError()[0, 0]))
#self.predErrorArray.append(self.homeostasis.calculatePredictionError()[0,0])
#plt.scatter(self.homeostasis.y.flatten().tolist())
self.phaseOffset1 = 0
self.phaseOffset2 = 0
self.phaseOffset3 = 0
self.phase1 += self.phaseIncrement
self.motor1 = np.sin(self.phase1)
self.motor2 = np.sin(self.phase1 + self.phaseOffset1)
self.motor3 = np.sin(self.phase1 + self.phaseOffset2)
self.motor4 = np.sin(self.phase1 + self.phaseOffset3)
motor = [self.motor1, self.motor2, self.motor3, self.motor4]
for m in range(len(motor)):
motor[m] *= 32768.0 # same coding as imu
self.motorMessage.data = motor
self.pub.publish(self.motorMessage)
# publish prediction error
self.predErrorMessage.data = self.homeostasis.calculatePredictionError(
)[0, 0]
self.predErrorPub.publish(self.predErrorMessage)
# publish phaseIncrement
print("phaseIncrement: %s" % self.phaseIncrement)
self.phaseIncrementPub.data = self.phaseIncrement
self.phaseIncrementPub.publish(self.phaseIncrementMessage)
if args.verbose:
print("MotorData sent: " + time.strftime('%X'))
print(self.motorMessage.data)
from classes.pendulum_c import Pendulum
from classes.homeostasis_c import Homeostasis
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
# initialize variables
dim_m = 1
dim_s = 2
# global variable
pendulum = Pendulum()
homeostasis = Homeostasis(dim_m, dim_s)
controller = homeostasis.getController()
model = homeostasis.getModel()
line = 0
def animate(i):
global pendulum
global homeostasis
homeostasis.x = pendulum.getMeasurement().reshape(2, 1)
homeostasis.learningStep()
line_x = np.zeros(2)
line_y = np.zeros(2)
line_x[1] = homeostasis.x[0]
line_y[1] = homeostasis.x[1]
line.set_data(line_x, line_y)
if (i > 200 and i < 400):
pendulum.calculateStep(np.ones((1, 1)))