def read_u():
global beta_color
global beta
global U
#Subscribers
rospy.Subscriber('/103/blue', Float32, callback_blue)
rospy.Subscriber('/105/red', Float32, callback_red)
rospy.Subscriber('/vicon/HandLeft/HandLeft', TransformStamped, callback_U)
u_value = (U_location.linear.x / 7) * 2.75
pub_u_value.publish(u_value)
U = [u_value, u_value]
#Control floats
k_r = 0.00045
k_b = 0.00073
beta_init = [color_103 * k_b - 0.37, 0.20 - color_105 * k_r]
#Beta correction block
r_off = np.sqrt((robot_locations[1].linear.x - 3.53) *
(robot_locations[1].linear.x - 3.53) +
(robot_locations[1].linear.y - 3.04) *
(robot_locations[1].linear.y - 3.04))
#print("from beta:" + str(r_off))
red_beta_control = -(0.6 - 2.9 * math.pi * hypsecant.pdf(r_off + 0.15))
blue_beta_control = (0.6 - 2.9 * math.pi * hypsecant.pdf(r_off + 0.15))
#beta = [blue_beta_control*beta_init[0], red_beta_control/beta_init[1]]
#Multiply gains if not dark
if beta_init[0] < 1:
beta[0] = beta_init[0]
else:
beta[0] = 4 * blue_beta_control * beta_init[0]
if beta_init[1] > -0.6:
beta[1] = beta_init[1]
else:
beta[1] = 4 * red_beta_control / beta_init[1]
if (float(beta[0]) + float(beta[1])) > 1:
beta_color = 6
elif (float(beta[0]) + float(beta[1])) < (-1):
beta_color = 1
else:
beta_color = 0
print("Beta =" + str(beta))
pass
def f_sech(x):
"""Deuxième condition initiale, solution analytique des solitons."""
u_0 = 0.5
u_inf = 0.1
x_0 = 0
Delta = delta/sqrt((u_0-u_inf)/12)
return u_inf + (u_0-u_inf)*(pi*hypsecant.pdf((x-x_0)/Delta))**2
def sech(x):
return hypsecant.pdf(x) * np.pi
def d_tanh(x):
return (hypsecant.pdf(x) * np.pi)**2
def funcHypsec(x, mu, sigma):
return hypsecant.pdf(x, mu, sigma)
from scipy.stats import hypsecant
import matplotlib.pyplot as plt
fig, ax = plt.subplots(1, 1)
# Calculate a few first moments:
mean, var, skew, kurt = hypsecant.stats(moments='mvsk')
# Display the probability density function (``pdf``):
x = np.linspace(hypsecant.ppf(0.01),
hypsecant.ppf(0.99), 100)
ax.plot(x, hypsecant.pdf(x),
'r-', lw=5, alpha=0.6, label='hypsecant pdf')
# Alternatively, the distribution object can be called (as a function)
# to fix the shape, location and scale parameters. This returns a "frozen"
# RV object holding the given parameters fixed.
# Freeze the distribution and display the frozen ``pdf``:
rv = hypsecant()
ax.plot(x, rv.pdf(x), 'k-', lw=2, label='frozen pdf')
# Check accuracy of ``cdf`` and ``ppf``:
vals = hypsecant.ppf([0.001, 0.5, 0.999])
np.allclose([0.001, 0.5, 0.999], hypsecant.cdf(vals))
# True
# Generate random numbers:
def read_u():
global beta_color
global beta
global U
#Subscribers
rospy.Subscriber('/103/blue', Float32, callback_blue)
rospy.Subscriber('/105/red', Float32, callback_red)
rospy.Subscriber('/vicon/HandLeft/HandLeft', TransformStamped, callback_U)
u_value = (U_location.linear.x/7)*2.75
pub_u_value.publish(u_value)
U = [u_value, u_value]
#Control floats
k_r = 0.00045
k_b = 0.00073
beta_init = [color_103*k_b-0.37, 0.20-color_105*k_r]
#Beta correction block
r_off = np.sqrt((robot_locations[1].linear.x-3.53)*(robot_locations[1].linear.x-3.53) + (robot_locations[1].linear.y-3.04)*(robot_locations[1].linear.y-3.04))
#print("from beta:" + str(r_off))
red_beta_control = -(0.6-2.9*math.pi*hypsecant.pdf(r_off+0.15))
blue_beta_control = (0.6-2.9*math.pi*hypsecant.pdf(r_off+0.15))
beta = [blue_beta_control*beta_init[0], red_beta_control/beta_init[1]]
'''
Beta = 0.28 at Maximum Red
Beta = 0.83 at 100 Red
Beta = -1.89 at 0 Red
'''
#Set Beta values for Red
if (beta[1]>0.2) and (beta[1]<0.4):
beta[1] = -0.5
elif (beta[1] > 0.4):
beta[1] = 0
else: beta[1] = 0.5
'''
Beta = 1.14 for max blue
Beta = 0.41 for 100 blue
Beta = -0.08 for zero blue
'''
#Set Beta Values
if beta[0]>0.4:
beta[0] = 0.5
elif beta[0]>0.1:
beta[0] = 0
else: beta[0] = -0.5
if (float(beta[0])+ float(beta[1])) > 0.5:
beta_color = 6
elif (float(beta[0])+ float(beta[1])) < (-0.5):
beta_color = 1
else: beta_color = 0
#beta = [1, 1]
print("Beta =" + str(beta))
#Publish Betas
pub_beta[0].publish(beta[1])
pub_beta[1].publish(beta[0])
pass
def sol_(x):
return u1 * (pi * hypsecant.pdf(
(x - x1) / Delta1))**2 + u2 * (pi * hypsecant.pdf(
(x - x2) / Delta2))**2
def soliton2(x, c, a):
return 0.5 * c * (pi * hypsecant.pdf((x - a) * sqrt(c) / 2))**2