class Darwin_Set_Angles():
def __init__(self):
rospy.init_node('darwin_set_angles', anonymous=False)
self.omega = 1.0
self.shoulder_limits = 1.74533
self.darwin = Darwin()
self.rotate_sinusoid()
def rotate_sinusoid(self):
# Set the shoulder roll angle and elbow so that arms are at the side
new_angles = {}
new_angles['j_high_arm_l'] = 1.50
new_angles['j_high_arm_r'] = 1.50
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
self.darwin.set_angles_slow(new_angles)
t=0
while(t<120):
new_angles['j_shoulder_l'] = self.shoulder_limits*(math.sin(self.omega*t))
# According to right hand rule, positive angle for l shoulder=negative angle for right
new_angles['j_shoulder_r'] = new_angles['j_shoulder_l']
self.darwin.set_angles_slow(new_angles)
t=t+1
def __init__(self):
rospy.init_node('darwin_set_angles', anonymous=False)
self.omega = 1.0
self.shoulder_limits = 1.74533
self.darwin = Darwin()
self.rotate_sinusoid()
def __init__(self):
rospy.init_node('darwin_set_angles', anonymous=False)
self.omega = 1.0
self.shoulder_limit_max = 1.745
self.shoulder_limit_min = -1.745
# Declare lists to be used for plots
self.y_list = []
self.t_list = []
# Iteration constants
self.step = 0.2
self.count = 120
# Origin of oscillation
self.origin_j_shoulder_l = 0.0
self.darwin = Darwin()
self.rotate_sinusoid()
class DarwinWalkOptimization():
def __init__(self):
################################## Constant Definitions ##################################
# Matsuoka Oscillator related initialization
self.WEIGHT_BOUNDS = [0.5, 7.5] # Weights of the neural oscillator connections
self.Tr1_BOUNDS = [0.25, 1.5] # Rise time constant
self.Tr2_BOUNDS = [0.25, 0.75] # Rise time constant
self.Ta1_BOUNDS = [0.25, 1.5] # Adaptation time constant
self.Ta2_BOUNDS = [0.25, 0.75] # Adaptation time constant
self.b_BOUNDS = [1.0, 8.0] # Constant of the Matsuoka Oscillator
self.s_BOUNDS = [1.0, 3.0] # Constant of the Matsuoka Oscillator
self.TRIAL_DURATION = 30 # How many seconds should each trial last
self.STEP_DURATION = 0.2 # The step increment in seconds
# PSO specific initialization
self.POPULATION_SIZE = 30
self.MAX_ITERS = 100
self.VEL_BOUND_PCT = 0.2 # Velocity is at most 20% of the position range
self.C1 = 2.0 # Acceleration constant
self.C2 = 2.0 # Acceleration constant
# Inertial weight, linearly scales from 0.9 to 0.4 over the entire run
self.INERTIAL_WEIGHT_BOUNDS = [0.4, 0.9]
# Darwin specific initialization
# Hashmap of joint origins
self.joint_origins = {}
self.joint_origins['j_thigh1_l'] = -0.524
self.joint_origins['j_thigh2_l'] = 0.611
self.joint_origins['j_tibia_l'] = -1.134
self.joint_origins['j_ankle1_l'] = 0.0
self.joint_origins['j_ankle2_l'] = 0.262
self.joint_origins['j_thigh1_r'] = 0.524
self.joint_origins['j_thigh2_r'] = -0.611
self.joint_origins['j_tibia_r'] = 1.134
self.joint_origins['j_ankle1_r'] = 0.0
self.joint_origins['j_ankle2_r'] = 0.262
# Hashmap of joint upper limits
self.joint_upper_limits = {}
self.joint_upper_limits['j_thigh1_l'] = 0.0
self.joint_upper_limits['j_thigh2_l'] = 1.745
self.joint_upper_limits['j_tibia_l'] = 0.0
self.joint_upper_limits['j_ankle1_l'] = 1.047
self.joint_upper_limits['j_ankle2_l'] = 1.047
self.joint_upper_limits['j_thigh1_r'] = 0.0
self.joint_upper_limits['j_thigh2_r'] = 0.524
self.joint_upper_limits['j_tibia_r'] = 2.269
self.joint_upper_limits['j_ankle1_r'] = 1.047
self.joint_upper_limits['j_ankle2_r'] = 1.047
# Hashmap of joint lower limits
self.joint_lower_limits = {}
self.joint_lower_limits['j_thigh1_l'] = -1.047
self.joint_lower_limits['j_thigh2_l'] = -0.524
self.joint_lower_limits['j_tibia_l'] = -2.269
self.joint_lower_limits['j_ankle1_l'] = -1.047
self.joint_lower_limits['j_ankle2_l'] = -0.524
self.joint_lower_limits['j_thigh1_r'] = 0.0
self.joint_lower_limits['j_thigh2_r'] = -1.745
self.joint_lower_limits['j_tibia_r'] = 0.0
self.joint_lower_limits['j_ankle1_r'] = -1.047
self.joint_lower_limits['j_ankle2_r'] = -0.524
# Variable to track distance walked by each individual
self.distance_walked = 0.0
# Variables to track the position of the robot
self.current_model_x = 0.0
self.current_model_y = 0.0
self.current_model_z = 0.0
# Variable (list) to track the height of the robot
self.model_polled_z = []
# Variable to tracj=k if the robot has fallen
self.has_fallen = False
# Constant fall limit - If the z coordinate of the robot is below this, then the robot has fallen
self.DARWIN_COM_FALL_LIMIT = 0.2
# ROS specific initialization
self.model_name = 'darwin'
self.relative_entity_name = 'world'
# Initialize the ROS node
rospy.init_node('darwin_walk_demo', anonymous=False)
# Subscribe to the /gazebo/model_states topic
rospy.Subscriber("/gazebo/model_states", ModelStates, self.subscriber_callback_modelstate)
# Register the client for service gazebo/get_model_state
rospy.wait_for_service('/gazebo/get_model_state')
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
# Register the client for service gazebo/set_model_state
rospy.wait_for_service('/gazebo/set_model_state')
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# Register the client for service /darwin/controller_manager/list_controllers
rospy.wait_for_service('/darwin/controller_manager/list_controllers')
self.get_joint_state = rospy.ServiceProxy('/darwin/controller_manager/list_controllers', ListControllers)
# Create the Darwin model
self.darwin = Darwin()
# Initialize log file
# Current time
curr_time = time.strftime('%Y%m%d%H%M%S')
# Create a log file
self.log_file = open('/home/sayantan/Knowledge/Independant Studies/Robot Walking/code/robot_walking/data/log_'
+ str(curr_time) + '_matsuoka_walk_optimization.log', 'w')
# To control if log contents are dumped on screen as well
self.verbose = True
self.log_file.write('Created log file at ' + str(curr_time) + '\n')
# Write the parameters to the log file
self.log_file.write('\n')
self.log_file.write("Constants and Parameters: " + "\n")
self.log_file.write("###########################################" + "\n")
self.log_file.write("WEIGHT_BOUNDS = " + str(self.WEIGHT_BOUNDS) + "\n")
self.log_file.write("Tr1_BOUNDS = " + str(self.Tr1_BOUNDS) + "\n")
self.log_file.write("Tr2_BOUNDS = " + str(self.Tr2_BOUNDS) + "\n")
self.log_file.write("Ta1_BOUNDS = " + str(self.Ta1_BOUNDS) + "\n")
self.log_file.write("Ta2_BOUNDS = " + str(self.Ta2_BOUNDS) + "\n")
self.log_file.write("b_BOUNDS = " + str(self.b_BOUNDS) + "\n")
self.log_file.write("s_BOUNDS = " + str(self.s_BOUNDS) + "\n")
self.log_file.write("TRIAL_DURATION = " + str(self.TRIAL_DURATION) + "\n")
self.log_file.write("STEP_DURATION = " + str(self.STEP_DURATION) + "\n")
self.log_file.write("POPULATION_SIZE = " + str(self.POPULATION_SIZE) + "\n")
self.log_file.write("MAX_ITERS = " + str(self.MAX_ITERS) + "\n")
self.log_file.write("VEL_BOUND_PCT = " + str(self.VEL_BOUND_PCT) + "\n")
self.log_file.write("C1 = " + str(self.C1) + "\n")
self.log_file.write("C2 = " + str(self.C2) + "\n")
self.log_file.write("INERTIAL_WEIGHT_BOUNDS = " + str(self.INERTIAL_WEIGHT_BOUNDS) + "\n")
self.log_file.write("###########################################" + "\n" + "\n")
# Initiate the walk optimization
self.pso()
# Logging function
def log_write(self, logtext):
curr_time = time.strftime('%Y%m%d%H%M%S')
logtext = "[" + str(curr_time) +"] " + logtext
self.log_file.write(logtext + "\n")
if self.verbose:
print logtext
# Function to implement the differential equations of the Matsuoka oscillator
def matsuoka(self, state, individual):
# Unpack the individual
a1 = individual[0]
a2 = individual[1]
a3 = individual[2]
a4 = individual[3]
a5 = individual[4]
a6 = individual[5]
a7 = individual[6]
a8 = individual[7]
a9 = individual[8]
Tr1 = individual[9]
Tr2 = individual[10]
Ta1 = individual[11]
Ta2 = individual[12]
b = individual[13]
s = individual[14]
# Unpack the state vector
x1 = state[0]
x2 = state[1]
x3 = state[2]
x4 = state[3]
x5 = state[4]
x6 = state[5]
x7 = state[6]
x8 = state[7]
x9 = state[8]
x10 = state[9]
x11 = state[10]
x12 = state[11]
x13 = state[12]
x14 = state[13]
x15 = state[14]
x16 = state[15]
x17 = state[16]
x18 = state[17]
x19 = state[18]
x20 = state[19]
f1 = state[20]
f2 = state[21]
f3 = state[22]
f4 = state[23]
f5 = state[24]
f6 = state[25]
f7 = state[26]
f8 = state[27]
f9 = state[28]
f10 = state[29]
f11 = state[30]
f12 = state[31]
f13 = state[32]
f14 = state[33]
f15 = state[34]
f16 = state[35]
f17 = state[36]
f18 = state[37]
f19 = state[38]
f20 = state[39]
y1 = state[40]
y2 = state[41]
y3 = state[42]
y4 = state[43]
y5 = state[44]
y6 = state[45]
y7 = state[46]
y8 = state[47]
y9 = state[48]
y10 = state[49]
y11 = state[50]
y12 = state[51]
y13 = state[52]
y14 = state[53]
y15 = state[54]
y16 = state[55]
y17 = state[56]
y18 = state[57]
y19 = state[58]
y20 = state[59]
# Hip neurons
x1_d = ((-1.0 * a2 * y2) - (a1 * y3) + (s) - (b * f1) - (x1)) / float(Tr1)
x2_d = ((-1.0 * a2 * y1) - (a1 * y4) + (s) - (b * f2) - (x2)) / float(Tr1)
x3_d = ((-1.0 * a1 * y1) - (a2 * y4) + (s) - (b * f3) - (x3)) / float(Tr1)
x4_d = ((-1.0 * a1 * y2) - (a2 * y3) + (s) - (b * f4) - (x4)) / float(Tr1)
# Knee neurons
x5_d = ((-1.0 * a3 * y6) - (a5 * y1) + (s) - (b * f5) - (x5)) / float(Tr2)
x6_d = ((-1.0 * a3 * y5) + (s) - (b * f6) - (x6)) / float(Tr2)
x7_d = ((-1.0 * a3 * y8) - (a5 * y2) + (s) - (b * f7) - (x7)) / float(Tr2)
x8_d = ((-1.0 * a3 * y7) + (s) - (b * f8) - (x8)) / float(Tr2)
# Ankle neurons
x9_d = ((-1.0 * a4 * y10) + (s) - (b * f9) - (x9)) / float(Tr2)
x10_d = ((-1.0 * a4 * y9) - (a6 * y1) + (s) - (b * f10) - (x10)) / float(Tr2)
x11_d = ((-1.0 * a4 * y12) + (s) - (b * f11) - (x11)) / float(Tr2)
x12_d = ((-1.0 * a4 * y11) - (a6 * y2) + (s) - (b * f12) - (x12)) / float(Tr2)
# Lateral hip neurons
x13_d = ((-1.0 * a8 * y14) - (a7 * y1) + (s) - (b * f13) - (x13)) / float(Tr1)
x14_d = ((-1.0 * a8 * y13) + (s) - (b * f14) - (x14)) / float(Tr1)
x15_d = ((-1.0 * a8 * y16) - (a7 * y2) + (s) - (b * f15) - (x15)) / float(Tr1)
x16_d = ((-1.0 * a8 * y15) + (s) - (b * f16) - (x15)) / float(Tr1)
# Lateral ankle neurons
x17_d = ((-1.0 * a8 * y18) - (a9 * y13) + (s) - (b * f17) - (x17)) / float(Tr1)
x18_d = ((-1.0 * a8 * y17) - (a9 * y14) + (s) - (b * f18) - (x18)) / float(Tr1)
x19_d = ((-1.0 * a8 * y20) - (a9 * y15) + (s) - (b * f19) - (x19)) / float(Tr1)
x20_d = ((-1.0 * a8 * y19) - (a9 * y16) + (s) - (b * f20) - (x20)) / float(Tr1)
# Calculate the elements of the next state
x1 += self.STEP_DURATION * x1_d
x2 += self.STEP_DURATION * x2_d
x3 += self.STEP_DURATION * x3_d
x4 += self.STEP_DURATION * x4_d
x5 += self.STEP_DURATION * x5_d
x6 += self.STEP_DURATION * x6_d
x7 += self.STEP_DURATION * x7_d
x8 += self.STEP_DURATION * x8_d
x9 += self.STEP_DURATION * x9_d
x10 += self.STEP_DURATION * x10_d
x11 += self.STEP_DURATION * x11_d
x12 += self.STEP_DURATION * x12_d
x13 += self.STEP_DURATION * x13_d
x14 += self.STEP_DURATION * x14_d
x15 += self.STEP_DURATION * x15_d
x16 += self.STEP_DURATION * x16_d
x17 += self.STEP_DURATION * x17_d
x18 += self.STEP_DURATION * x18_d
x19 += self.STEP_DURATION * x19_d
x20 += self.STEP_DURATION * x20_d
y1 = max(0.0, x1)
y2 = max(0.0, x2)
y3 = max(0.0, x3)
y4 = max(0.0, x4)
y5 = max(0.0, x5)
y6 = max(0.0, x6)
y7 = max(0.0, x7)
y8 = max(0.0, x8)
y9 = max(0.0, x9)
y10 = max(0.0, x10)
y11 = max(0.0, x11)
y12 = max(0.0, x12)
y13 = max(0.0, x13)
y14 = max(0.0, x14)
y15 = max(0.0, x15)
y16 = max(0.0, x16)
y17 = max(0.0, x17)
y18 = max(0.0, x18)
y19 = max(0.0, x19)
y20 = max(0.0, x20)
f1_d = (y1 - f1) / float(Ta1)
f2_d = (y2 - f2) / float(Ta1)
f3_d = (y3 - f3) / float(Ta1)
f4_d = (y4 - f4) / float(Ta1)
f5_d = (y5 - f5) / float(Ta2)
f6_d = (y6 - f6) / float(Ta2)
f7_d = (y7 - f7) / float(Ta2)
f8_d = (y8 - f8) / float(Ta2)
f9_d = (y9 - f9) / float(Ta2)
f10_d = (y10 - f10) / float(Ta2)
f11_d = (y11 - f11) / float(Ta2)
f12_d = (y12 - f12) / float(Ta2)
f13_d = (y13 - f13) / float(Ta1)
f14_d = (y14 - f14) / float(Ta1)
f15_d = (y15 - f15) / float(Ta1)
f16_d = (y16 - f16) / float(Ta1)
f17_d = (y17 - f17) / float(Ta1)
f18_d = (y18 - f18) / float(Ta1)
f19_d = (y19 - f19) / float(Ta1)
f20_d = (y20 - f20) / float(Ta1)
f1 += self.STEP_DURATION * f1_d
f2 += self.STEP_DURATION * f2_d
f3 += self.STEP_DURATION * f3_d
f4 += self.STEP_DURATION * f4_d
f5 += self.STEP_DURATION * f5_d
f6 += self.STEP_DURATION * f6_d
f7 += self.STEP_DURATION * f7_d
f8 += self.STEP_DURATION * f8_d
f9 += self.STEP_DURATION * f9_d
f10 += self.STEP_DURATION * f10_d
f11 += self.STEP_DURATION * f11_d
f12 += self.STEP_DURATION * f12_d
f13 += self.STEP_DURATION * f13_d
f14 += self.STEP_DURATION * f14_d
f15 += self.STEP_DURATION * f15_d
f16 += self.STEP_DURATION * f16_d
f17 += self.STEP_DURATION * f17_d
f18 += self.STEP_DURATION * f18_d
f19 += self.STEP_DURATION * f19_d
f20 += self.STEP_DURATION * f20_d
# Create the return state
ret_state = [x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14, x15, x16, x17, x18, x19, x20,
f1, f2, f3, f4, f5, f6, f7, f8, f9, f10, f11, f12, f13, f14, f15, f16, f17, f18, f19, f20,
y1, y2, y3, y4, y5, y6, y7, y8, y9, y10, y11, y12, y13, y14, y15, y16, y17, y18, y19, y20]
return ret_state
# Function for Walking using Matsuoka Oscillator
def matsuoka_walking_fitness(self, individual):
# Reset the simulation before each walk
self.reset_simulation()
# Set initial state - [x1-20, f1-20, y1-20]
state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # x1-20
0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, # f1-20
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ] # y1-20
# Set the shoulder roll angle and elbow so that arms are at the side
new_angles = {}
new_angles['j_high_arm_l'] = 1.50
new_angles['j_high_arm_r'] = 1.50
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
new_angles['j_tilt'] = -0.262 # Tilt the head forward a little
self.darwin.set_angles_slow(new_angles)
# Calculate the number of iterations
num_iterations = self.TRIAL_DURATION / self.STEP_DURATION
i = 0
while (i < num_iterations):
state = self.matsuoka(state, individual)
# Unpack the state vector
y1 = state[40]
y2 = state[41]
y3 = state[42]
y4 = state[43]
y5 = state[44]
y6 = state[45]
y7 = state[46]
y8 = state[47]
y9 = state[48]
y10 = state[49]
y11 = state[50]
y12 = state[51]
y13 = state[52]
y14 = state[53]
y15 = state[54]
y16 = state[55]
y17 = state[56]
y18 = state[57]
y19 = state[58]
y20 = state[59]
# Calculate the joint angles
hip_left_lateral = (y15 - y16)
hip_right_lateral = (y13 - y14)
hip_left = -(y2 - y4) # (-) sign according to right hand rotation rule
hip_right = (y1 - y3)
knee_left = -(y7 - y8) # (-) sign according to right hand rotation rule
knee_right = (y5 - y6)
ankle_left = -(y12 - y11) # (-) sign according to right hand rotation rule
ankle_right = (y10 - y9)
ankle_left_lateral = hip_left_lateral
ankle_right_lateral = hip_right_lateral
# Validate the angles so that they do not exceed the max and min joint limits
# Check the lower limits
if hip_left_lateral < self.joint_lower_limits['j_thigh1_l']: hip_left_lateral = self.joint_lower_limits['j_thigh1_l']
if hip_right_lateral < self.joint_lower_limits['j_thigh1_r']: hip_right_lateral = self.joint_lower_limits['j_thigh1_r']
if hip_left<self.joint_lower_limits['j_thigh2_l']: hip_left = self.joint_lower_limits['j_thigh2_l']
if hip_right<self.joint_lower_limits['j_thigh2_r']: hip_right = self.joint_lower_limits['j_thigh2_r']
if knee_left<self.joint_lower_limits['j_tibia_l']: knee_left = self.joint_lower_limits['j_tibia_l']
if knee_right<self.joint_lower_limits['j_tibia_r']: knee_right = self.joint_lower_limits['j_tibia_r']
if ankle_left<self.joint_lower_limits['j_ankle1_l']: ankle_left = self.joint_lower_limits['j_ankle1_l']
if ankle_right<self.joint_lower_limits['j_ankle1_r']: ankle_right = self.joint_lower_limits['j_ankle1_r']
if ankle_left_lateral < self.joint_lower_limits['j_ankle2_l']: ankle_left_lateral = self.joint_lower_limits['j_ankle2_l']
if ankle_right_lateral < self.joint_lower_limits['j_ankle2_r']: ankle_right_lateral = self.joint_lower_limits['j_ankle2_r']
# Check the upper limits
if hip_left_lateral>self.joint_upper_limits['j_thigh1_l']: hip_left_lateral = self.joint_upper_limits['j_thigh1_l']
if hip_right_lateral>self.joint_upper_limits['j_thigh1_r']: hip_right_lateral = self.joint_upper_limits['j_thigh1_r']
if hip_left>self.joint_upper_limits['j_thigh2_l']: hip_left = self.joint_upper_limits['j_thigh2_l']
if hip_right>self.joint_upper_limits['j_thigh2_r']: hip_right = self.joint_upper_limits['j_thigh2_r']
if knee_left>self.joint_upper_limits['j_tibia_l']: knee_left = self.joint_upper_limits['j_tibia_l']
if knee_right>self.joint_upper_limits['j_tibia_r']: knee_right = self.joint_upper_limits['j_tibia_r']
if ankle_left>self.joint_upper_limits['j_ankle1_l']: ankle_left = self.joint_upper_limits['j_ankle1_l']
if ankle_right>self.joint_upper_limits['j_ankle1_r']: ankle_right = self.joint_upper_limits['j_ankle1_r']
if ankle_left_lateral>self.joint_upper_limits['j_ankle2_l']: ankle_left_lateral = self.joint_upper_limits['j_ankle2_l']
if ankle_right_lateral>self.joint_upper_limits['j_ankle2_r']: ankle_right_lateral = self.joint_upper_limits['j_ankle2_r']
# Wait for 10 seconds (till the oscillations stabilize) before assigning angles to joints
if (i * self.STEP_DURATION > 10):
# Hip
new_angles['j_thigh1_l'] = hip_left_lateral
new_angles['j_thigh1_r'] = -hip_right_lateral
new_angles['j_thigh2_l'] = hip_left
new_angles['j_thigh2_r'] = hip_right
# Knee
new_angles['j_tibia_l'] = knee_left
new_angles['j_tibia_r'] = knee_right
# Ankle
new_angles['j_ankle1_l'] = ankle_left
new_angles['j_ankle1_r'] = ankle_right
new_angles['j_ankle2_l'] = ankle_left_lateral
new_angles['j_ankle2_r'] = -ankle_right_lateral
# Set the robot joint angles
self.darwin.set_angles_slow(new_angles, self.STEP_DURATION)
# Calculate the current distance travelled by the robot
# Start position is always (x=0, y=0)
# self.distance_walked = np.sign(self.current_model_x) * math.sqrt((self.current_model_x)**2 + (self.current_model_y)**2)
# Consider only the distance in x direction as fitness
if (self.current_model_x < 0.1):
self.distance_walked = -1.0 # Penalize individuals which move less than 0.1m
else:
self.distance_walked = self.current_model_x
# If the robot has fallen, return the distance travelled till now as the fitness score
if (self.has_fallen):
self.log_write("Robot fall detected")
self.log_write("Individual: " + str(individual))
self.log_write("Location: x = " + str(self.current_model_x) + " y = " + str(self.current_model_y) + " z = " + str(self.current_model_z))
self.log_write("Current fitness = " + str(self.distance_walked))
return self.distance_walked
i += 1
# If the trial duration has ended return the distance travelled as the fitness score
# If the control has reached here, then the robot has not fallen. Hence it gets a bonus score
self.log_write("Robot did not fall during the trial")
self.log_write("Individual: " + str(individual))
self.log_write("Location: x = " + str(self.current_model_x) + " y = " + str(self.current_model_y) + " z = " + str(self.current_model_z))
self.log_write("Current fitness = " + str(self.distance_walked))
return self.distance_walked + 0.2*self.distance_walked
# Function to reset the robot in the simulation (before start of run and after a fall)
def reset_simulation(self):
# Reset the joints
new_angles = {}
new_angles['j_ankle1_l'] = 0.0
new_angles['j_ankle1_r'] = 0.0
new_angles['j_ankle2_l'] = 0.0
new_angles['j_ankle2_r'] = 0.0
new_angles['j_gripper_l'] = 0.0
new_angles['j_gripper_r'] = 0.0
new_angles['j_high_arm_l'] = 1.50 # Arm by the side of the body
new_angles['j_high_arm_r'] = 1.50 # Arm by the side of the body
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
new_angles['j_pan'] = 0.0
new_angles['j_pelvis_l'] = 0.0
new_angles['j_pelvis_r'] = 0.0
new_angles['j_shoulder_l'] = 0.0
new_angles['j_shoulder_r'] = 0.0
new_angles['j_thigh1_l'] = 0.0
new_angles['j_thigh1_r'] = 0.0
new_angles['j_thigh2_l'] = 0.0
new_angles['j_thigh2_r'] = 0.0
new_angles['j_tibia_l'] = 0.0
new_angles['j_tibia_r'] = 0.0
new_angles['j_tilt'] = -0.262 # Tilt the head forward a little
new_angles['j_wrist_l'] = 0.0
new_angles['j_wrist_r'] = 0.0
# Default delay of setting angles is 2 seconds
self.darwin.set_angles_slow(new_angles)
rospy.sleep(5)
# Reset the robot position
# Send model to x=0,y=0
pose = Pose()
pose.position.z = 0.31
twist = Twist()
md_state = ModelState()
md_state.model_name = self.model_name
md_state.pose = pose
md_state.twist = twist
md_state.reference_frame = self.relative_entity_name
# Service call to reset model
try:
response = self.set_model_state(md_state)
except rospy.ServiceException, e:
self.log_write("Darwin model state initialization service call failed: " + str(e))
# Reset the distance variable
self.distance_walked = 0.0
# Reset the position variables
self.current_model_x = 0.0
self.current_model_y = 0.0
self.current_model_z = 0.0
# Reset the height list
self.model_polled_z = []
# Reset the robot fall detection flag
self.has_fallen = False
if angle_elbow_to_hand_l > 1.75:
darwin.set_angles({"j_low_arm_l": 1.5})
elif angle_elbow_to_hand_l < 0.25:
darwin.set_angles({"j_low_arm_l": 0})
else:
darwin.set_angles({"j_low_arm_l": (angle_elbow_to_hand_l - 0.25)})
if angle_elbow_to_hand_r > 1.75:
darwin.set_angles({"j_low_arm_r": -1.5})
elif angle_elbow_to_hand_r < 0.25:
darwin.set_angles({"j_low_arm_r": 0})
else:
darwin.set_angles({"j_low_arm_r": -(angle_elbow_to_hand_r - 0.25)})
if __name__ == '__main__':
rospy.init_node("walker_demo", anonymous=True)
print('Hello')
darwin = Darwin()
rospy.loginfo("Darwin initialization finished")
print("Hello")
rospy.Subscriber('tf', tf2_msgs.msg.TFMessage, callback, darwin)
rospy.spin()
#!/usr/bin/env python
import time
import rospy
import math
from darwin_gazebo.darwin import Darwin
# from ros_cqi.ros_cqi.darwin_interface import DarwinInterface
if __name__ == "__main__":
rospy.init_node("walker_demo")
rospy.loginfo("Instantiating Darwin Client")
darwin = Darwin()
rospy.sleep(1)
rospy.loginfo("Darwin Walker Demo Starting")
darwin.apply_fallback_force()
while True:
head = darwin.get_head_position()
angles = darwin.get_body_rotation()
if (head.twist.linear.x > 0.01):
darwin.move_arms_straight()
darwin.move_low_arm(angles[1])
darwin.move_tibia(angles[1])
rospy.loginfo("Darwin Walker Demo Finished")
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
import tf2_msgs.msg
import roslib
import math
import geometry_msgs.msg as msgs
import turtlesim.srv
from darwin_gazebo.darwin import Darwin
import vectormath as v
if __name__ == '__main__':
rospy.init_node("walker_demo", anonymous=True)
print('Hello')
darwin = Darwin()
rospy.loginfo("Darwin initialization finished")
print("Hello")
while True:
x = input()
y = input()
z = input()
if x == 555:
break
darwin.set_angles({"j_shoulder_l": x})
darwin.set_angles({"j_high_arm_l": y})
darwin.set_angles({"j_low_arm_l": z})
def initiate_walk(self, walk_seconds,walk_velocity):
darwin = Darwin()
darwin.set_walk_velocity(walk_velocity[0],walk_velocity[1],walk_velocity[2])
rospy.sleep(walk_seconds)
darwin.set_walk_velocity(0,0,0)
#!/usr/bin/env python
import rospy
from darwin_gazebo.darwin import Darwin
if __name__ == "__main__":
rospy.init_node("walker_demo")
rospy.loginfo("Instantiating Darwin Client")
darwin = Darwin()
rospy.sleep(1)
rospy.loginfo("Darwin Walker Demo Starting")
darwin.set_walk_velocity(0.2, 0, 0)
rospy.sleep(3)
darwin.set_walk_velocity(1, 0, 0)
rospy.sleep(3)
darwin.set_walk_velocity(0, 1, 0)
rospy.sleep(3)
darwin.set_walk_velocity(0, -1, 0)
rospy.sleep(3)
darwin.set_walk_velocity(-1, 0, 0)
rospy.sleep(3)
darwin.set_walk_velocity(1, 1, 0)
rospy.sleep(5)
darwin.set_walk_velocity(0, 0, 0)
rospy.loginfo("Darwin Walker Demo Finished")
class Darwin_Set_Angles():
def __init__(self):
rospy.init_node('darwin_set_angles', anonymous=False)
self.omega = 1.0
self.shoulder_limit_max = 1.745
self.shoulder_limit_min = -1.745
# Declare lists to be used for plots
self.y_list = []
self.t_list = []
# Iteration constants
self.step = 0.2
self.count = 120
# Origin of oscillation
self.origin_j_shoulder_l = 0.0
self.darwin = Darwin()
self.rotate_sinusoid()
# Function to calculate next states
def matsuoka(self, state):
# Tunable parameters, static for now
a = 2.5
s = 1.0
b = 2.0
Tr = 0.75
Ta = 0.75
x1 = state[0]
x2 = state[1]
f1 = state[2]
f2 = state[3]
y1 = state[4]
y2 = state[5]
x1_d = ((-1.0*a*y2) + (s) - (b*f1) - (x1))/float(Tr)
x2_d = ((-1.0*a*y1) + (s) - (b*f2) - (x2))/float(Tr)
x1 = x1 + self.step*x1_d
x2 = x2 + self.step*x2_d
y1 = max(0.0, x1)
y2 = max(0.0, x2)
f1_d = (y1 - f1)/float(Ta)
f2_d = (y2 - f2)/float(Ta)
f1 = f1 + self.step*f1_d
f2 = f2 + self.step*f2_d
return [x1, x2, f1, f2, y1, y2]
def rotate_sinusoid(self):
# Set initial state
self.state = [0.0, 1.0, 0.0, 1.0, 0.0 ,0.0]
# Set the shoulder roll angle and elbow so that arms are at the side
new_angles = {}
new_angles['j_high_arm_l'] = 1.50
new_angles['j_high_arm_r'] = 1.50
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
self.darwin.set_angles_slow(new_angles)
i=0
while(i<self.count):
self.state = self.matsuoka(self.state)
y_oscillator = self.state[4]-self.state[5]
y_joint = y_oscillator + self.origin_j_shoulder_l
self.t_list.append(i*self.step)
# Check angle limits
if (y_joint > self.shoulder_limit_max):
y_joint = self.shoulder_limit_max
if (y_joint < self.shoulder_limit_min):
y_joint = self.shoulder_limit_min
self.y_list.append(y_joint)
new_angles['j_shoulder_l'] = y_joint
# According to right hand rule, positive angle for l shoulder=negative angle for right
new_angles['j_shoulder_r'] = new_angles['j_shoulder_l']
# The last parameter is the delay, this should match the step size
self.darwin.set_angles_slow(new_angles,self.step)
print i
i=i+1
# Plot
plt.figure()
plt.plot(self.t_list,self.y_list)
plt.show()
ans = [x for x in frange(home, end + delta, delta, direction)]
#print ans
#print (len(ans))
# found few mismatch with an extra element contribiting to error in some test cases
# clearing them off
final_ans = [ans[x] for x in range(0, int(interval) + 1)]
#print final_ans
#print (len(final_ans))
return final_ans
if __name__ == "__main__":
rospy.init_node("walker_demo")
rospy.loginfo("Instantiating Darwin Client")
darwin = Darwin()
rospy.sleep(1)
darwin.set_angles({"j_pan": 1})
steps = 5.0
# enter all the desired joint angles here
j_high_arm_r = 1.343676989
j_high_arm_l = 1.378893255
j_shoulder_r = -0.831647219
j_shoulder_l = 0.711766896
j_low_arm_r = 1.499488737
j_low_arm_l = -1.508986304
#!/usr/bin/env python
import rospy
from darwin_gazebo.darwin import Darwin
if __name__=="__main__":
rospy.init_node("walker_demo")
rospy.loginfo("Instantiating Darwin Client")
darwin=Darwin()
rospy.sleep(1)
rospy.loginfo("Darwin Walker Demo Starting")
darwin.set_walk_velocity(0.2,0,0)
rospy.sleep(3)
darwin.set_walk_velocity(1,0,0)
rospy.sleep(3)
darwin.set_walk_velocity(0,1,0)
rospy.sleep(3)
darwin.set_walk_velocity(0,-1,0)
rospy.sleep(3)
darwin.set_walk_velocity(-1,0,0)
rospy.sleep(3)
darwin.set_walk_velocity(1,1,0)
rospy.sleep(5)
darwin.set_walk_velocity(0,0,0)
rospy.loginfo("Darwin Walker Demo Finished")
class Darwin_Walk_Optimization():
def __init__(self):
################################## Constant Definitions ##################################
# Matsuoka Oscillator related initialization
self.WEIGHT_BOUNDS = [1.0,3.0] # Weights of the neural oscillator connections
self.Tr_BOUNDS = [0.1,3.75] # Rise time constant
self.Ta_BOUNDS = [0.01,3.75] # Adaptation time constant
self.b_BOUNDS = [1.0,6.0] # Constant of the Matsuoka Oscillator
self.s_BOUNDS = [0.0,10.0] # Constant of the Matsuoka Oscillator
self.NUM_WEIGHTS = 20 # Number of weights in the network
self.VEL_BOUND_PCT = 0.2 # Velocity is at most 20% of the position range
self.TRIAL_DURATION = 30 # How many seconds should each trial last
self.STEP_DURATION = 0.2 # The step increment in seconds
# PSO specific initialization
self.POPULATION_SIZE = 100
# Each individual consists of 24 positions (weights, and oscillator constants),
# 24 velocities, 24 PBest positions and 1 PBest score
self.INDIVIDUAL_VECTOR_SIZE = 73
self.GBEST_VECTOR_SIZE = 25
self.MAX_ITERS = 200
self.C1 = 2.0 # Acceleration constant
self.C2 = 2.0 # Acceleration constant
# Inertial weight, linearly scales from 0.9 to 0.4 over the entire run
self.INERTIAL_WEIGHT_BOUNDS = [0.4,0.9]
# Darwin specific initialization
# Hashmap of joint origins
self.joint_origins = {}
self.joint_origins['j_thigh2_l'] = 0.611
self.joint_origins['j_tibia_l'] = -1.134
self.joint_origins['j_ankle1_l'] = 0.0
self.joint_origins['j_thigh2_r'] = -0.611
self.joint_origins['j_tibia_r'] = 1.134
self.joint_origins['j_ankle1_r'] = 0.0
# Hashmap of joint upper limits
self.joint_upper_limits = {}
self.joint_upper_limits['j_thigh2_l'] = 1.745
self.joint_upper_limits['j_tibia_l'] = 0.0
self.joint_upper_limits['j_ankle1_l'] = 1.047
self.joint_upper_limits['j_thigh2_r'] = 0.524
self.joint_upper_limits['j_tibia_r'] = 2.269
self.joint_upper_limits['j_ankle1_r'] = 1.047
# Hashmap of joint lower limits
self.joint_lower_limits = {}
self.joint_lower_limits['j_thigh2_l'] = -0.524
self.joint_lower_limits['j_tibia_l'] = -2.269
self.joint_lower_limits['j_ankle1_l'] = -1.047
self.joint_lower_limits['j_thigh2_r'] = -1.745
self.joint_lower_limits['j_tibia_r'] = 0.0
self.joint_lower_limits['j_ankle1_r'] = -1.047
# Variable to track distance walked by each individual
self.distance_walked = 0.0
# Variables to track the position of the robot
self.current_model_x = 0.0
self.current_model_y = 0.0
self.current_model_z = 0.0
# Variable (list) to track the height of the robot
self.model_polled_z = []
# Variable to tracj=k if the robot has fallen
self.has_fallen = False
# Constant fall limit - If the z coordinate of the robot is below this, then the robot has fallen
self.DARWIN_COM_FALL_LIMIT = 0.2
# ROS specific initialization
self.model_name = 'darwin'
self.relative_entity_name = 'world'
# Initialize the ROS node
rospy.init_node('darwin_walk_demo', anonymous=False)
# Subscribe to the /gazebo/model_states topic
rospy.Subscriber("/gazebo/model_states", ModelStates, self.subscriber_callback_modelstate)
# Register the client for service gazebo/get_model_state
rospy.wait_for_service('/gazebo/get_model_state')
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
# Register the client for service gazebo/set_model_state
rospy.wait_for_service('/gazebo/set_model_state')
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# Register the client for service /darwin/controller_manager/list_controllers
rospy.wait_for_service('/darwin/controller_manager/list_controllers')
self.get_joint_state = rospy.ServiceProxy('/darwin/controller_manager/list_controllers', ListControllers)
# Create the Darwin model
self.darwin = Darwin()
# Initialize log file
# Current time
curr_time = time.strftime('%Y%m%d%H%M%S')
# Create a log file
self.log_file = open('/home/sayantan/Knowledge/Independant Studies/Robot Walking/code/robot_walking/data/log_'+str(curr_time)+'_darwin_walk_optimization.log', 'w')
self.log_file.write('Created log file at ' + str(curr_time) + '\n')
# Initiate the walk optimization
self.pso()
# Function to calculate the next state of the Matsuoka neural oscillators
def matsuoka(self, individual, state):
print "State: " + str(state)
print "Individual: " + str(individual)
# Extract the weights
w1 = individual[0]
w2 = individual[1]
w3 = individual[2]
w4 = individual[3]
w5 = individual[4]
w6 = individual[5]
w7 = individual[6]
w8 = individual[7]
w9 = individual[8]
w10 = individual[9]
w11 = individual[10]
w12 = individual[11]
w13 = individual[12]
w14 = individual[13]
w15 = individual[14]
w16 = individual[15]
w17 = individual[16]
w18 = individual[17]
w19 = individual[18]
w20 = individual[19]
# Extract the network constants
Tr = individual[20]
Ta = individual[21]
b = individual[22]
s = individual[23]
# Extract the current states
# The numbering scheme: For f21 <variable=f><oscillator#=2><1 for extensor, 2 for flexor>
# Each oscillator has the state [x1,x2,f1,f2,y1,y2]
# There are 6 such oscillators
x11 = state[0]
x12 = state[1]
f11 = state[2]
f12 = state[3]
y11 = state[4]
y12 = state[5]
x21 = state[6]
x22 = state[7]
f21 = state[8]
f22 = state[9]
y21 = state[10]
y22 = state[11]
x31 = state[12]
x32 = state[13]
f31 = state[14]
f32 = state[15]
y31 = state[16]
y32 = state[17]
x41 = state[18]
x42 = state[19]
f41 = state[20]
f42 = state[21]
y41 = state[22]
y42 = state[23]
x51 = state[24]
x52 = state[25]
f51 = state[26]
f52 = state[27]
y51 = state[28]
y52 = state[29]
x61 = state[30]
x62 = state[31]
f61 = state[32]
f62 = state[33]
y61 = state[34]
y62 = state[35]
# Calculate the derivatives of the membrane potentials
# This is based on the connections between the individual neurons
x11_d = ((-1.0*w1*y12) - (w6*y21) + (s) - (b*f11) - (x11))/float(Tr)
x12_d = ((-1.0*w2*y11) - (w8*y22) + (s) - (b*f12) - (x12))/float(Tr)
x21_d = ((-1.0*w3*y22) - (w5*y11) + (s) - (b*f21) - (x21))/float(Tr)
x22_d = ((-1.0*w4*y21) - (w7*y12) + (s) - (b*f22) - (x22))/float(Tr)
x31_d = ((-1.0*w13*y32) - (w9*y11) + (s) - (b*f31) - (x31))/float(Tr)
x32_d = ((-1.0*w14*y31) + (s) - (b*f32) - (x32))/float(Tr)
x41_d = ((-1.0*w15*y42) - (w10*y21) + (s) - (b*f41) - (x41))/float(Tr)
x42_d = ((-1.0*w16*y41) + (s) - (b*f42) - (x42))/float(Tr)
x51_d = ((-1.0*w17*y52) + (s) - (b*f51) - (x51))/float(Tr)
x52_d = ((-1.0*w18*y51) - (w11*y11) + (s) - (b*f52) - (x52))/float(Tr)
x61_d = ((-1.0*w19*y62) + (s) - (b*f61) - (x61))/float(Tr)
x62_d = ((-1.0*w20*y61) - (w12*y21) + (s) - (b*f62) - (x62))/float(Tr)
# Calculate the elements of the next state
x11 = x11 + self.STEP_DURATION*x11_d
x12 = x12 + self.STEP_DURATION*x12_d
x21 = x21 + self.STEP_DURATION*x21_d
x22 = x22 + self.STEP_DURATION*x22_d
x31 = x31 + self.STEP_DURATION*x31_d
x32 = x32 + self.STEP_DURATION*x32_d
x41 = x41 + self.STEP_DURATION*x41_d
x42 = x42 + self.STEP_DURATION*x42_d
x51 = x51 + self.STEP_DURATION*x51_d
x52 = x52 + self.STEP_DURATION*x52_d
x61 = x61 + self.STEP_DURATION*x61_d
x62 = x62 + self.STEP_DURATION*x62_d
y11 = max(0.0, x11)
y12 = max(0.0, x12)
y21 = max(0.0, x21)
y22 = max(0.0, x22)
y31 = max(0.0, x31)
y32 = max(0.0, x32)
y41 = max(0.0, x41)
y42 = max(0.0, x42)
y51 = max(0.0, x51)
y52 = max(0.0, x52)
y61 = max(0.0, x61)
y62 = max(0.0, x62)
f11_d = (y11 - f11)/float(Ta)
f12_d = (y12 - f12)/float(Ta)
f21_d = (y21 - f21)/float(Ta)
f22_d = (y22 - f22)/float(Ta)
f31_d = (y31 - f31)/float(Ta)
f32_d = (y32 - f32)/float(Ta)
f41_d = (y41 - f41)/float(Ta)
f42_d = (y42 - f42)/float(Ta)
f51_d = (y51 - f51)/float(Ta)
f52_d = (y52 - f52)/float(Ta)
f61_d = (y61 - f61)/float(Ta)
f62_d = (y62 - f62)/float(Ta)
f11 = f11 + self.STEP_DURATION*f11_d
f12 = f12 + self.STEP_DURATION*f12_d
f21 = f21 + self.STEP_DURATION*f21_d
f22 = f22 + self.STEP_DURATION*f22_d
f31 = f31 + self.STEP_DURATION*f31_d
f32 = f32 + self.STEP_DURATION*f32_d
f41 = f41 + self.STEP_DURATION*f41_d
f42 = f42 + self.STEP_DURATION*f42_d
f51 = f51 + self.STEP_DURATION*f51_d
f52 = f52 + self.STEP_DURATION*f52_d
f61 = f61 + self.STEP_DURATION*f61_d
f62 = f62 + self.STEP_DURATION*f62_d
# Create the return state
ret_state = [x11,x12,f11,f12,y11,y12,
x21,x22,f21,f22,y21,y22,
x31,x32,f31,f32,y31,y32,
x41,x42,f41,f42,y41,y42,
x51,x52,f51,f52,y51,y52,
x61,x62,f61,f62,y61,y62
]
return ret_state
# Function for Walking using Matsuoka Oscillator
def matsuoka_walking_fitness(self, individual):
joint_plot_actual = []
t1 = []
t2 = []
joint_plot_rectified = []
print 'Received Individual:' + str(individual)
# Reset the simulation before each walk
self.reset_simulation()
# Set initial state - [x1,x2,f1,f2,y1,y2] for each oscillator
# There are 6 oscillators, one each for the joints j_thigh2_l,j_tibia_l,j_ankle1_l,j_thigh2_r,j_tibia_r,j_ankle1_r
oscillator_state = []
count = 0
while(count < 6):
oscillator_state.extend([0.0, 1.0, 0.0, 1.0, 0.0, 0.0])
count += 1
# Set the shoulder roll angle and elbow so that arms are at the side
new_angles = {}
new_angles['j_high_arm_l'] = 1.50
new_angles['j_high_arm_r'] = 1.50
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
self.darwin.set_angles_slow(new_angles)
# Calculate the number of iterations
num_iterations = self.TRIAL_DURATION/self.STEP_DURATION
i=0
while(i<num_iterations):
# Compute the next state from the Matsuoka oscillator
# Pass the individual (with weights and constants)
oscillator_state = self.matsuoka(individual,oscillator_state)
# Shift the positions to the joint origins
oscillator_joint_1 = (oscillator_state[0*6 + 4] - oscillator_state[0*6 + 5]) # + self.joint_origins['j_thigh2_l']
oscillator_joint_2 = (oscillator_state[1*6 + 4] - oscillator_state[1*6 + 5]) #+ self.joint_origins['j_tibia_l']
oscillator_joint_3 = (oscillator_state[2*6 + 4] - oscillator_state[2*6 + 5]) # + self.joint_origins['j_ankle1_l']
oscillator_joint_4 = (oscillator_state[3*6 + 4] - oscillator_state[3*6 + 5]) # + self.joint_origins['j_thigh2_r']
oscillator_joint_5 = (oscillator_state[4*6 + 4] - oscillator_state[4*6 + 5]) #+ self.joint_origins['j_tibia_r']
oscillator_joint_6 = (oscillator_state[5*6 + 4] - oscillator_state[5*6 + 5]) # + self.joint_origins['j_ankle1_r']
joint_plot_actual.append(oscillator_state[5*6 + 4] - oscillator_state[5*6 + 5])
t1.append(i*self.STEP_DURATION)
# If joint angles exceed the joint limits, set the joint limit as the joint angle
oscillator_joint_1 = self.joint_upper_limits['j_thigh2_l'] if (oscillator_joint_1>self.joint_upper_limits['j_thigh2_l']) else oscillator_joint_1
oscillator_joint_1 = self.joint_lower_limits['j_thigh2_l'] if (oscillator_joint_1<self.joint_lower_limits['j_thigh2_l']) else oscillator_joint_1
oscillator_joint_2 = self.joint_upper_limits['j_tibia_l'] if (oscillator_joint_2>self.joint_upper_limits['j_tibia_l']) else oscillator_joint_2
oscillator_joint_2 = self.joint_lower_limits['j_tibia_l'] if (oscillator_joint_2<self.joint_lower_limits['j_tibia_l']) else oscillator_joint_2
oscillator_joint_3 = self.joint_upper_limits['j_ankle1_l'] if (oscillator_joint_3>self.joint_upper_limits['j_ankle1_l']) else oscillator_joint_3
oscillator_joint_3 = self.joint_lower_limits['j_ankle1_l'] if (oscillator_joint_3<self.joint_lower_limits['j_ankle1_l']) else oscillator_joint_3
oscillator_joint_4 = self.joint_upper_limits['j_thigh2_r'] if (oscillator_joint_4>self.joint_upper_limits['j_thigh2_r']) else oscillator_joint_4
oscillator_joint_4 = self.joint_lower_limits['j_thigh2_r'] if (oscillator_joint_4<self.joint_lower_limits['j_thigh2_r']) else oscillator_joint_4
oscillator_joint_5 = self.joint_upper_limits['j_tibia_r'] if (oscillator_joint_5>self.joint_upper_limits['j_tibia_r']) else oscillator_joint_5
oscillator_joint_5 = self.joint_lower_limits['j_tibia_r'] if (oscillator_joint_5<self.joint_lower_limits['j_tibia_r']) else oscillator_joint_5
oscillator_joint_6 = self.joint_upper_limits['j_ankle1_r'] if (oscillator_joint_6>self.joint_upper_limits['j_ankle1_r']) else oscillator_joint_6
oscillator_joint_6 = self.joint_lower_limits['j_ankle1_r'] if (oscillator_joint_6<self.joint_lower_limits['j_ankle1_r']) else oscillator_joint_6
joint_plot_rectified.append(oscillator_joint_6)
t2.append(i*self.STEP_DURATION)
print "Joint angles:"
print "[[" + str(oscillator_joint_1) + ", " + str(oscillator_joint_2) + ", " + str(oscillator_joint_3) + ", " + str(oscillator_joint_4) + ", " + str(oscillator_joint_5) + ", " + str(oscillator_joint_6) + "]]"
if (i*self.STEP_DURATION > 5.0):
# Set the new angles
new_angles['j_thigh2_l'] = oscillator_joint_1
new_angles['j_tibia_l'] = oscillator_joint_2
new_angles['j_ankle1_l'] = oscillator_joint_3
new_angles['j_thigh2_r'] = oscillator_joint_4
new_angles['j_tibia_r'] = oscillator_joint_5
new_angles['j_ankle1_r'] = oscillator_joint_6
# The last parameter is the delay, this should match the step size
self.darwin.set_angles_slow(new_angles,self.STEP_DURATION)
# Calculate the current distance travelled by the robot
self.distance_walked = np.sign(self.current_model_x)*math.sqrt((self.current_model_x)*(self.current_model_x) + (self.current_model_y)*(self.current_model_y))
# If the robot has fallen, return the distance travelled till now as the fitness score
if(self.has_fallen):
#plt.figure()
#plt.plot(t1, joint_plot_actual)
#plt.plot(t2, joint_plot_rectified)
#plt.show()
return self.distance_walked
i = i + 1
# If the trial duration has ended return the distance travelled as the fitness score
return self.distance_walked
def reset_simulation(self):
# Reset the joints
new_angles = {}
new_angles['j_ankle1_l'] = 0.0
new_angles['j_ankle1_r'] = 0.0
new_angles['j_ankle2_l'] = 0.0
new_angles['j_ankle2_r'] = 0.0
new_angles['j_gripper_l'] = 0.0
new_angles['j_gripper_r'] = 0.0
new_angles['j_high_arm_l'] = 1.50 # Arm by the side of the body
new_angles['j_high_arm_r'] = 1.50 # Arm by the side of the body
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
new_angles['j_pan'] = 0.0
new_angles['j_pelvis_l'] = 0.0
new_angles['j_pelvis_r'] = 0.0
new_angles['j_shoulder_l'] = 0.0
new_angles['j_shoulder_r'] = 0.0
new_angles['j_thigh1_l'] = 0.0
new_angles['j_thigh1_r'] = 0.0
new_angles['j_thigh2_l'] = 0.0
new_angles['j_thigh2_r'] = 0.0
new_angles['j_tibia_l'] = 0.0
new_angles['j_tibia_r'] = 0.0
new_angles['j_tilt'] = 0.0
new_angles['j_wrist_l'] = 0.0
new_angles['j_wrist_r'] = 0.0
# Default delay of setting angles is 2 seconds
self.darwin.set_angles_slow(new_angles)
# Reset the robot position
# Send model to x=0,y=0
pose = Pose()
pose.position.z = 0.31
twist = Twist()
md_state = ModelState()
md_state.model_name = self.model_name
md_state.pose = pose
md_state.twist = twist
md_state.reference_frame = self.relative_entity_name
# Service call to reset model
try:
response = self.set_model_state(md_state)
except rospy.ServiceException, e:
print "Darwin model state initialization service call failed: %s"%e
rospy.loginfo("Result of model reset: " + str(response))
# Reset the distance variable
self.distance_walked = 0.0
# Reset the position variables
self.current_model_x = 0.0
self.current_model_y = 0.0
self.current_model_z = 0.0
# Reset the height list
self.model_polled_z = []
# Reset the robot fall detection flag
self.has_fallen = False
def __init__(self):
################################## Constant Definitions ##################################
# Matsuoka Oscillator related initialization
self.WEIGHT_BOUNDS = [1.0, 3.5] # Weights of the neural oscillator connections
self.Tr_BOUNDS = [0.1, 2.0] # Rise time constant
self.Ta_BOUNDS = [0.1, 2.0] # Adaptation time constant
self.b_BOUNDS = [0.1, 2.0] # Constant of the Matsuoka Oscillator
self.s_BOUNDS = [1.0, 2.0] # Constant of the Matsuoka Oscillator
self.NUM_WEIGHTS = 7 # Number of distinct weights in the network
self.VEL_BOUND_PCT = 0.2 # Velocity is at most 20% of the position range
self.TRIAL_DURATION = 30 # How many seconds should each trial last
self.STEP_DURATION = 0.2 # The step increment in seconds
# PSO specific initialization
self.POPULATION_SIZE = 20
# Each individual consists of 11 positions (weights, and oscillator constants),
# 11 velocities, 11 PBest positions and 1 PBest score
self.INDIVIDUAL_VECTOR_SIZE = 34
self.GBEST_VECTOR_SIZE = 12
self.MAX_ITERS = 10
self.C1 = 2.0 # Acceleration constant
self.C2 = 2.0 # Acceleration constant
# Inertial weight, linearly scales from 0.9 to 0.4 over the entire run
self.INERTIAL_WEIGHT_BOUNDS = [0.4, 0.9]
# Darwin specific initialization
# Hashmap of joint origins
self.joint_origins = {}
self.joint_origins['j_thigh2_l'] = 0.611
self.joint_origins['j_tibia_l'] = -1.134
self.joint_origins['j_ankle1_l'] = 0.0
self.joint_origins['j_thigh2_r'] = -0.611
self.joint_origins['j_tibia_r'] = 1.134
self.joint_origins['j_ankle1_r'] = 0.0
# Hashmap of joint upper limits
self.joint_upper_limits = {}
self.joint_upper_limits['j_thigh2_l'] = 1.745
self.joint_upper_limits['j_tibia_l'] = 0.0
self.joint_upper_limits['j_ankle1_l'] = 1.047
self.joint_upper_limits['j_thigh2_r'] = 0.524
self.joint_upper_limits['j_tibia_r'] = 2.269
self.joint_upper_limits['j_ankle1_r'] = 1.047
# Hashmap of joint lower limits
self.joint_lower_limits = {}
self.joint_lower_limits['j_thigh2_l'] = -0.524
self.joint_lower_limits['j_tibia_l'] = -2.269
self.joint_lower_limits['j_ankle1_l'] = -1.047
self.joint_lower_limits['j_thigh2_r'] = -1.745
self.joint_lower_limits['j_tibia_r'] = 0.0
self.joint_lower_limits['j_ankle1_r'] = -1.047
# Variable to track distance walked by each individual
self.distance_walked = 0.0
# Variables to track the position of the robot
self.current_model_x = 0.0
self.current_model_y = 0.0
self.current_model_z = 0.0
# Variable (list) to track the height of the robot
self.model_polled_z = []
# Variable to tracj=k if the robot has fallen
self.has_fallen = False
# Constant fall limit - If the z coordinate of the robot is below this, then the robot has fallen
self.DARWIN_COM_FALL_LIMIT = 0.2
# ROS specific initialization
self.model_name = 'darwin'
self.relative_entity_name = 'world'
# Initialize the ROS node
rospy.init_node('darwin_walk_demo', anonymous=False)
# Subscribe to the /gazebo/model_states topic
rospy.Subscriber("/gazebo/model_states", ModelStates, self.subscriber_callback_modelstate)
# Register the client for service gazebo/get_model_state
rospy.wait_for_service('/gazebo/get_model_state')
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
# Register the client for service gazebo/set_model_state
rospy.wait_for_service('/gazebo/set_model_state')
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# Register the client for service /darwin/controller_manager/list_controllers
rospy.wait_for_service('/darwin/controller_manager/list_controllers')
self.get_joint_state = rospy.ServiceProxy('/darwin/controller_manager/list_controllers', ListControllers)
# Create the Darwin model
self.darwin = Darwin()
# Initialize log file
# Current time
curr_time = time.strftime('%Y%m%d%H%M%S')
# Create a log file
self.log_file = open('/home/sayantan/Knowledge/Independant Studies/Robot Walking/code/robot_walking/data/log_'
+ str(curr_time) + '_darwin_walk_optimization_symmetric.log', 'w')
self.log_file.write('Created log file at ' + str(curr_time) + '\n')
# Initiate the walk optimization
self.pso()
class DarwinWalkOptimization():
def __init__(self):
################################## Constant Definitions ##################################
# Matsuoka Oscillator related initialization
self.WEIGHT_BOUNDS = [1.0, 3.5] # Weights of the neural oscillator connections
self.Tr_BOUNDS = [0.1, 2.0] # Rise time constant
self.Ta_BOUNDS = [0.1, 2.0] # Adaptation time constant
self.b_BOUNDS = [0.1, 2.0] # Constant of the Matsuoka Oscillator
self.s_BOUNDS = [1.0, 2.0] # Constant of the Matsuoka Oscillator
self.NUM_WEIGHTS = 7 # Number of distinct weights in the network
self.VEL_BOUND_PCT = 0.2 # Velocity is at most 20% of the position range
self.TRIAL_DURATION = 30 # How many seconds should each trial last
self.STEP_DURATION = 0.2 # The step increment in seconds
# PSO specific initialization
self.POPULATION_SIZE = 20
# Each individual consists of 11 positions (weights, and oscillator constants),
# 11 velocities, 11 PBest positions and 1 PBest score
self.INDIVIDUAL_VECTOR_SIZE = 34
self.GBEST_VECTOR_SIZE = 12
self.MAX_ITERS = 10
self.C1 = 2.0 # Acceleration constant
self.C2 = 2.0 # Acceleration constant
# Inertial weight, linearly scales from 0.9 to 0.4 over the entire run
self.INERTIAL_WEIGHT_BOUNDS = [0.4, 0.9]
# Darwin specific initialization
# Hashmap of joint origins
self.joint_origins = {}
self.joint_origins['j_thigh2_l'] = 0.611
self.joint_origins['j_tibia_l'] = -1.134
self.joint_origins['j_ankle1_l'] = 0.0
self.joint_origins['j_thigh2_r'] = -0.611
self.joint_origins['j_tibia_r'] = 1.134
self.joint_origins['j_ankle1_r'] = 0.0
# Hashmap of joint upper limits
self.joint_upper_limits = {}
self.joint_upper_limits['j_thigh2_l'] = 1.745
self.joint_upper_limits['j_tibia_l'] = 0.0
self.joint_upper_limits['j_ankle1_l'] = 1.047
self.joint_upper_limits['j_thigh2_r'] = 0.524
self.joint_upper_limits['j_tibia_r'] = 2.269
self.joint_upper_limits['j_ankle1_r'] = 1.047
# Hashmap of joint lower limits
self.joint_lower_limits = {}
self.joint_lower_limits['j_thigh2_l'] = -0.524
self.joint_lower_limits['j_tibia_l'] = -2.269
self.joint_lower_limits['j_ankle1_l'] = -1.047
self.joint_lower_limits['j_thigh2_r'] = -1.745
self.joint_lower_limits['j_tibia_r'] = 0.0
self.joint_lower_limits['j_ankle1_r'] = -1.047
# Variable to track distance walked by each individual
self.distance_walked = 0.0
# Variables to track the position of the robot
self.current_model_x = 0.0
self.current_model_y = 0.0
self.current_model_z = 0.0
# Variable (list) to track the height of the robot
self.model_polled_z = []
# Variable to tracj=k if the robot has fallen
self.has_fallen = False
# Constant fall limit - If the z coordinate of the robot is below this, then the robot has fallen
self.DARWIN_COM_FALL_LIMIT = 0.2
# ROS specific initialization
self.model_name = 'darwin'
self.relative_entity_name = 'world'
# Initialize the ROS node
rospy.init_node('darwin_walk_demo', anonymous=False)
# Subscribe to the /gazebo/model_states topic
rospy.Subscriber("/gazebo/model_states", ModelStates, self.subscriber_callback_modelstate)
# Register the client for service gazebo/get_model_state
rospy.wait_for_service('/gazebo/get_model_state')
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
# Register the client for service gazebo/set_model_state
rospy.wait_for_service('/gazebo/set_model_state')
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# Register the client for service /darwin/controller_manager/list_controllers
rospy.wait_for_service('/darwin/controller_manager/list_controllers')
self.get_joint_state = rospy.ServiceProxy('/darwin/controller_manager/list_controllers', ListControllers)
# Create the Darwin model
self.darwin = Darwin()
# Initialize log file
# Current time
curr_time = time.strftime('%Y%m%d%H%M%S')
# Create a log file
self.log_file = open('/home/sayantan/Knowledge/Independant Studies/Robot Walking/code/robot_walking/data/log_'
+ str(curr_time) + '_darwin_walk_optimization_symmetric.log', 'w')
self.log_file.write('Created log file at ' + str(curr_time) + '\n')
# Initiate the walk optimization
self.pso()
# Function to generate individuals (network weights and parameters)
def generate_individual_position(self, Tr_bounds, Ta_bounds, b_bounds, s_bounds, weight_bounds):
Tr = random.uniform(Tr_bounds[0], Tr_bounds[1])
Ta = random.uniform(Ta_bounds[0], Ta_bounds[1])
b = random.uniform(b_bounds[0], b_bounds[1])
s = random.uniform(s_bounds[0], s_bounds[1])
# Setting the couplings of the 2 hip joints
# Generating weights used in network of 4 oscillators
# This is the weight in each connection of the 4 neuron network
# The lower limit is the greatest of the numbers the weight should be greater than
# The upper limit is the smallest of the numbers the weight should be smaller than
a = random.uniform(max(b, (1 + (Tr / Ta)), weight_bounds[0]), min(((1 + b) / 2.0), weight_bounds[1]))
# All weights in for the hip neurons are symmetric
a1_2 = a1_3 = a3_1 = a3_4 = a4_3 = a4_2 = a2_4 = a2_1 = a
# Generating the coupling of the knee joints
# The upper limit should be satisfied and the product of the weights should be above a limit
a5_6 = random.uniform(weight_bounds[0], min((1 + b), weight_bounds[1]))
a6_5 = random.uniform(weight_bounds[0], min((1 + b), weight_bounds[1]))
'''while ((a5_6 * a6_5) < (1 + (Tr/Ta))**2):
print "stuck"
a5_6 = random.uniform(weight_bounds[0], min((1 + b), weight_bounds[1]))
a6_5 = random.uniform(weight_bounds[0], min((1 + b), weight_bounds[1]))
'''
# The knee neuron couplings in the other leg are symmetric
a7_8 = a5_6
a8_7 = a6_5
# Generating the coupling of the ankle joints
# The upper limit should be satisfied and the product of the weights should be above a limit
a9_10 = random.uniform(weight_bounds[0], min((1 + b), weight_bounds[1]))
a10_9 = random.uniform(weight_bounds[0], min((1 + b), weight_bounds[1]))
'''while ((a9_10 * a10_9) < (1 + (Tr/Ta))**2):
a9_10 = random.uniform(weight_bounds[0], min((1 + b), weight_bounds[1]))
a10_9 = random.uniform(weight_bounds[0], min((1 + b), weight_bounds[1]))
'''
# The knee neuron couplings in the other leg are symmetric
a11_12 = a9_10
a12_11 = a10_9
# Generating the coupling weight between hip and knee
a1_5 = random.uniform(weight_bounds[0], weight_bounds[1])
a2_7 = a1_5
# Generating the coupling weight between hip and ankle
a1_10 = random.uniform(weight_bounds[0], weight_bounds[1])
a2_12 = a1_10
return [a, a5_6, a6_5, a9_10, a10_9, a1_5, a1_10, Tr, Ta, b, s]
# Function to calculate the next state of the Matsuoka neural oscillators
def matsuoka(self, individual, state):
print "State: " + str(state)
print "Individual: " + str(individual)
# Extract the network weights and parameters
a1_3 = a3_1 = a3_4 = a4_3 = a4_2 = a2_4 = a2_1 = a1_2 = individual[0]
a5_6 = a7_8 = individual[1]
a6_5 = a8_7 = individual[2]
a9_10 = a11_12 = individual[3]
a10_9 = a12_11 = individual[4]
a1_5 = a2_7 = individual[5]
a1_10 = a2_12 = individual[6]
Tr = individual[7]
Ta = individual[8]
b = individual[9]
s = individual[10]
# Extract the current states
# The numbering scheme: For f21 <variable=f><oscillator#=2><1 for extensor, 2 for flexor>
# Each oscillator has the state [x1,x2,f1,f2,y1,y2]
# There are 6 such oscillators
x1 = state[0]
x2 = state[1]
x3 = state[2]
x4 = state[3]
x5 = state[4]
x6 = state[5]
x7 = state[6]
x8 = state[7]
x9 = state[8]
x10 = state[9]
x11 = state[10]
x12 = state[11]
f1 = state[12]
f2 = state[13]
f3 = state[14]
f4 = state[15]
f5 = state[16]
f6 = state[17]
f7 = state[18]
f8 = state[19]
f9 = state[20]
f10 = state[21]
f11 = state[22]
f12 = state[23]
y1 = state[24]
y2 = state[25]
y3 = state[26]
y4 = state[27]
y5 = state[28]
y6 = state[29]
y7 = state[30]
y8 = state[31]
y9 = state[32]
y10 = state[33]
y11 = state[34]
y12 = state[35]
# Calculate the derivatives of the membrane potentials
# This is based on the connections between the individual neurons
# Hip neurons
x1_d = ((-1.0 * a3_1 * y3) - (a2_1 * y2) + (s) - (b * f1) - (x1)) / float(Tr)
x3_d = ((-1.0 * a1_3 * y1) - (a4_3 * y4) + (s) - (b * f3) - (x3)) / float(Tr)
x4_d = ((-1.0 * a3_4 * y3) - (a2_4 * y2) + (s) - (b * f4) - (x4)) / float(Tr)
x2_d = ((-1.0 * a4_2 * y4) - (a1_2 * y1) + (s) - (b * f2) - (x2)) / float(Tr)
# Knee neurons
x5_d = ((-1.0 * a6_5 * y6) - (a1_5 * y1) + (s) - (b * f5) - (x5)) / float(Tr)
x6_d = ((-1.0 * a5_6 * y5) + (s) - (b * f6) - (x6)) / float(Tr)
x7_d = ((-1.0 * a8_7 * y8) - (a2_7 * y2) + (s) - (b * f7) - (x7)) / float(Tr)
x8_d = ((-1.0 * a7_8 * y7) + (s) - (b * f8) - (x8)) / float(Tr)
# Ankle neurons
x9_d = ((-1.0 * a10_9 * y10) + (s) - (b * f9) - (x9)) / float(Tr)
x10_d = ((-1.0 * a9_10 * y9) - (a1_10 * y1) + (s) - (b * f10) - (x10)) / float(Tr)
x11_d = ((-1.0 * a12_11 * y12) + (s) - (b * f11) - (x11)) / float(Tr)
x12_d = ((-1.0 * a11_12 * y11) - (a2_12 * y2) + (s) - (b * f12) - (x12)) / float(Tr)
# Calculate the elements of the next state
x1 += self.STEP_DURATION * x1_d
x2 += self.STEP_DURATION * x2_d
x3 += self.STEP_DURATION * x3_d
x4 += self.STEP_DURATION * x4_d
x5 += self.STEP_DURATION * x5_d
x6 += self.STEP_DURATION * x6_d
x7 += self.STEP_DURATION * x7_d
x8 += self.STEP_DURATION * x8_d
x9 += self.STEP_DURATION * x9_d
x10 += self.STEP_DURATION * x10_d
x11 += self.STEP_DURATION * x11_d
x12 += self.STEP_DURATION * x12_d
y1 = max(0.0, x1)
y2 = max(0.0, x2)
y3 = max(0.0, x3)
y4 = max(0.0, x4)
y5 = max(0.0, x5)
y6 = max(0.0, x6)
y7 = max(0.0, x7)
y8 = max(0.0, x8)
y9 = max(0.0, x9)
y10 = max(0.0, x10)
y11 = max(0.0, x11)
y12 = max(0.0, x12)
f1_d = (y1 - f1) / float(Ta)
f2_d = (y2 - f2) / float(Ta)
f3_d = (y3 - f3) / float(Ta)
f4_d = (y4 - f4) / float(Ta)
f5_d = (y5 - f5) / float(Ta)
f6_d = (y6 - f6) / float(Ta)
f7_d = (y7 - f7) / float(Ta)
f8_d = (y8 - f8) / float(Ta)
f9_d = (y9 - f9) / float(Ta)
f10_d = (y10 - f10) / float(Ta)
f11_d = (y11 - f11) / float(Ta)
f12_d = (y12 - f12) / float(Ta)
f1 += self.STEP_DURATION * f1_d
f2 += self.STEP_DURATION * f2_d
f3 += self.STEP_DURATION * f3_d
f4 += self.STEP_DURATION * f4_d
f5 += self.STEP_DURATION * f5_d
f6 += self.STEP_DURATION * f6_d
f7 += self.STEP_DURATION * f7_d
f8 += self.STEP_DURATION * f8_d
f9 += self.STEP_DURATION * f9_d
f10 += self.STEP_DURATION * f10_d
f11 += self.STEP_DURATION * f11_d
f12 += self.STEP_DURATION * f12_d
# Create the return state
ret_state = [x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12,
f1, f2, f3, f4, f5, f6, f7, f8, f9, f10, f11, f12,
y1, y2, y3, y4, y5, y6, y7, y8, y9, y10, y11, y12]
return ret_state
# Function for Walking using Matsuoka Oscillator
def matsuoka_walking_fitness(self, individual):
joint_plot_1 = []
t1 = []
t2 = []
joint_plot_2 = []
print 'Received Individual:' + str(individual)
# Reset the simulation before each walk
self.reset_simulation()
# Set initial state - [x1-12, f1-12, y1-12]
oscillator_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # x1-12
0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, # f1-12
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # y1-12
# Set the shoulder roll angle and elbow so that arms are at the side
new_angles = {}
new_angles['j_high_arm_l'] = 1.50
new_angles['j_high_arm_r'] = 1.50
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
self.darwin.set_angles_slow(new_angles)
# Calculate the number of iterations
num_iterations = self.TRIAL_DURATION / self.STEP_DURATION
i = 0
while (i < num_iterations):
# Compute the next state from the Matsuoka oscillator
# Pass the individual (with weights and constants)
oscillator_state = self.matsuoka(individual, oscillator_state)
# Unpack the state vector
y1 = oscillator_state[24]
y2 = oscillator_state[25]
y3 = oscillator_state[26]
y4 = oscillator_state[27]
y5 = oscillator_state[28]
y6 = oscillator_state[29]
y7 = oscillator_state[30]
y8 = oscillator_state[31]
y9 = oscillator_state[32]
y10 = oscillator_state[33]
y11 = oscillator_state[34]
y12 = oscillator_state[35]
# Right Leg
# neuron 1+3 => j_thigh2_r (hip)
# neuron 5+6 => j_tibia_r (knee)
# neuron 9+10 => j_ankle1_r (ankle)
# Left Leg
# neuron 2+4 => j_thigh2_l (hip)
# neuron 7+8 => j_tibia_l (knee)
# neuron 11+12 => j_ankle1_l (ankle
# Shift the positions to the joint origins
joint_j_thigh2_r = y1 - y3
joint_j_tibia_r = y5 - y6
joint_j_ankle1_r = y9 - y10
joint_j_thigh2_l = y1 - y3
joint_j_tibia_l = (y7 - y8)
joint_j_ankle1_l = (y11 - y12)
# If joint angles exceed the joint limits, set the joint limit as the joint angle
joint_j_thigh2_l = self.joint_upper_limits['j_thigh2_l'] if (joint_j_thigh2_l > self.joint_upper_limits['j_thigh2_l']) else joint_j_thigh2_l
joint_j_thigh2_l = self.joint_lower_limits['j_thigh2_l'] if (joint_j_thigh2_l < self.joint_lower_limits['j_thigh2_l']) else joint_j_thigh2_l
joint_j_tibia_l = self.joint_upper_limits['j_tibia_l'] if (joint_j_tibia_l > self.joint_upper_limits['j_tibia_l']) else joint_j_tibia_l
joint_j_tibia_l = self.joint_lower_limits['j_tibia_l'] if (joint_j_tibia_l < self.joint_lower_limits['j_tibia_l']) else joint_j_tibia_l
joint_j_ankle1_l = self.joint_upper_limits['j_ankle1_l'] if (joint_j_ankle1_l > self.joint_upper_limits['j_ankle1_l']) else joint_j_ankle1_l
joint_j_ankle1_l = self.joint_lower_limits['j_ankle1_l'] if (joint_j_ankle1_l < self.joint_lower_limits['j_ankle1_l']) else joint_j_ankle1_l
joint_j_thigh2_r = self.joint_upper_limits['j_thigh2_r'] if (joint_j_thigh2_r > self.joint_upper_limits['j_thigh2_r']) else joint_j_thigh2_r
joint_j_thigh2_r = self.joint_lower_limits['j_thigh2_r'] if (joint_j_thigh2_r < self.joint_lower_limits['j_thigh2_r']) else joint_j_thigh2_r
joint_j_tibia_r = self.joint_upper_limits['j_tibia_r'] if (joint_j_tibia_r > self.joint_upper_limits['j_tibia_r']) else joint_j_tibia_r
joint_j_tibia_r = self.joint_lower_limits['j_tibia_r'] if (joint_j_tibia_r < self.joint_lower_limits['j_tibia_r']) else joint_j_tibia_r
joint_j_ankle1_r = self.joint_upper_limits['j_ankle1_r'] if (joint_j_ankle1_r > self.joint_upper_limits['j_ankle1_r']) else joint_j_ankle1_r
joint_j_ankle1_r = self.joint_lower_limits['j_ankle1_r'] if (joint_j_ankle1_r < self.joint_lower_limits['j_ankle1_r']) else joint_j_ankle1_r
print "Joint angles:"
print "[[" + str(joint_j_thigh2_l) + ", " + str(joint_j_tibia_l) + ", " + str(
joint_j_ankle1_l) + ", " + str(joint_j_thigh2_r) + ", " + str(joint_j_tibia_r) + ", " + str(
joint_j_ankle1_r) + "]]"
if (i * self.STEP_DURATION > 5.0):
# Set the new angles
new_angles['j_thigh2_l'] = joint_j_thigh2_l
new_angles['j_tibia_l'] = joint_j_tibia_l
new_angles['j_ankle1_l'] = joint_j_ankle1_l
new_angles['j_thigh2_r'] = joint_j_thigh2_r
new_angles['j_tibia_r'] = joint_j_tibia_r
new_angles['j_ankle1_r'] = joint_j_ankle1_r
joint_plot_1.append(joint_j_thigh2_r)
joint_plot_2.append(joint_j_thigh2_r)
t1.append(i*self.STEP_DURATION)
# The last parameter is the delay, this should match the step size
self.darwin.set_angles_slow(new_angles, self.STEP_DURATION)
# Calculate the current distance travelled by the robot #np.sign(self.current_model_x) *
self.distance_walked = math.sqrt((self.current_model_x)**2 + (self.current_model_y)**2)
# If the robot has fallen, return the distance travelled till now as the fitness score
if (self.has_fallen):
#plt.figure()
#plt.plot(t1, joint_plot_1)
#plt.plot(t1, joint_plot_2)
#plt.show()
return self.distance_walked
i = i + 1
# If the trial duration has ended return the distance travelled as the fitness score
return self.distance_walked
def reset_simulation(self):
# Reset the joints
new_angles = {}
new_angles['j_ankle1_l'] = 0.0
new_angles['j_ankle1_r'] = 0.0
new_angles['j_ankle2_l'] = 0.0
new_angles['j_ankle2_r'] = 0.0
new_angles['j_gripper_l'] = 0.0
new_angles['j_gripper_r'] = 0.0
new_angles['j_high_arm_l'] = 1.50 # Arm by the side of the body
new_angles['j_high_arm_r'] = 1.50 # Arm by the side of the body
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
new_angles['j_pan'] = 0.0
new_angles['j_pelvis_l'] = 0.0
new_angles['j_pelvis_r'] = 0.0
new_angles['j_shoulder_l'] = 0.0
new_angles['j_shoulder_r'] = 0.0
new_angles['j_thigh1_l'] = 0.0
new_angles['j_thigh1_r'] = 0.0
new_angles['j_thigh2_l'] = 0.0
new_angles['j_thigh2_r'] = 0.0
new_angles['j_tibia_l'] = 0.0
new_angles['j_tibia_r'] = 0.0
new_angles['j_tilt'] = 0.0
new_angles['j_wrist_l'] = 0.0
new_angles['j_wrist_r'] = 0.0
# Default delay of setting angles is 2 seconds
self.darwin.set_angles_slow(new_angles)
# Reset the robot position
# Send model to x=0,y=0
pose = Pose()
pose.position.z = 0.31
twist = Twist()
md_state = ModelState()
md_state.model_name = self.model_name
md_state.pose = pose
md_state.twist = twist
md_state.reference_frame = self.relative_entity_name
# Service call to reset model
try:
response = self.set_model_state(md_state)
except rospy.ServiceException, e:
print "Darwin model state initialization service call failed: %s" % e
rospy.loginfo("Result of model reset: " + str(response))
# Reset the distance variable
self.distance_walked = 0.0
# Reset the position variables
self.current_model_x = 0.0
self.current_model_y = 0.0
self.current_model_z = 0.0
# Reset the height list
self.model_polled_z = []
# Reset the robot fall detection flag
self.has_fallen = False
def __init__(self):
################################## Constant Definitions ##################################
# Matsuoka Oscillator related initialization
self.WEIGHT_BOUNDS = [0.5, 7.5] # Weights of the neural oscillator connections
self.Tr1_BOUNDS = [0.25, 1.5] # Rise time constant
self.Tr2_BOUNDS = [0.25, 0.75] # Rise time constant
self.Ta1_BOUNDS = [0.25, 1.5] # Adaptation time constant
self.Ta2_BOUNDS = [0.25, 0.75] # Adaptation time constant
self.b_BOUNDS = [1.0, 8.0] # Constant of the Matsuoka Oscillator
self.s_BOUNDS = [1.0, 3.0] # Constant of the Matsuoka Oscillator
self.TRIAL_DURATION = 30 # How many seconds should each trial last
self.STEP_DURATION = 0.2 # The step increment in seconds
# PSO specific initialization
self.POPULATION_SIZE = 30
self.MAX_ITERS = 100
self.VEL_BOUND_PCT = 0.2 # Velocity is at most 20% of the position range
self.C1 = 2.0 # Acceleration constant
self.C2 = 2.0 # Acceleration constant
# Inertial weight, linearly scales from 0.9 to 0.4 over the entire run
self.INERTIAL_WEIGHT_BOUNDS = [0.4, 0.9]
# Darwin specific initialization
# Hashmap of joint origins
self.joint_origins = {}
self.joint_origins['j_thigh1_l'] = -0.524
self.joint_origins['j_thigh2_l'] = 0.611
self.joint_origins['j_tibia_l'] = -1.134
self.joint_origins['j_ankle1_l'] = 0.0
self.joint_origins['j_ankle2_l'] = 0.262
self.joint_origins['j_thigh1_r'] = 0.524
self.joint_origins['j_thigh2_r'] = -0.611
self.joint_origins['j_tibia_r'] = 1.134
self.joint_origins['j_ankle1_r'] = 0.0
self.joint_origins['j_ankle2_r'] = 0.262
# Hashmap of joint upper limits
self.joint_upper_limits = {}
self.joint_upper_limits['j_thigh1_l'] = 0.0
self.joint_upper_limits['j_thigh2_l'] = 1.745
self.joint_upper_limits['j_tibia_l'] = 0.0
self.joint_upper_limits['j_ankle1_l'] = 1.047
self.joint_upper_limits['j_ankle2_l'] = 1.047
self.joint_upper_limits['j_thigh1_r'] = 0.0
self.joint_upper_limits['j_thigh2_r'] = 0.524
self.joint_upper_limits['j_tibia_r'] = 2.269
self.joint_upper_limits['j_ankle1_r'] = 1.047
self.joint_upper_limits['j_ankle2_r'] = 1.047
# Hashmap of joint lower limits
self.joint_lower_limits = {}
self.joint_lower_limits['j_thigh1_l'] = -1.047
self.joint_lower_limits['j_thigh2_l'] = -0.524
self.joint_lower_limits['j_tibia_l'] = -2.269
self.joint_lower_limits['j_ankle1_l'] = -1.047
self.joint_lower_limits['j_ankle2_l'] = -0.524
self.joint_lower_limits['j_thigh1_r'] = 0.0
self.joint_lower_limits['j_thigh2_r'] = -1.745
self.joint_lower_limits['j_tibia_r'] = 0.0
self.joint_lower_limits['j_ankle1_r'] = -1.047
self.joint_lower_limits['j_ankle2_r'] = -0.524
# Variable to track distance walked by each individual
self.distance_walked = 0.0
# Variables to track the position of the robot
self.current_model_x = 0.0
self.current_model_y = 0.0
self.current_model_z = 0.0
# Variable (list) to track the height of the robot
self.model_polled_z = []
# Variable to tracj=k if the robot has fallen
self.has_fallen = False
# Constant fall limit - If the z coordinate of the robot is below this, then the robot has fallen
self.DARWIN_COM_FALL_LIMIT = 0.2
# ROS specific initialization
self.model_name = 'darwin'
self.relative_entity_name = 'world'
# Initialize the ROS node
rospy.init_node('darwin_walk_demo', anonymous=False)
# Subscribe to the /gazebo/model_states topic
rospy.Subscriber("/gazebo/model_states", ModelStates, self.subscriber_callback_modelstate)
# Register the client for service gazebo/get_model_state
rospy.wait_for_service('/gazebo/get_model_state')
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
# Register the client for service gazebo/set_model_state
rospy.wait_for_service('/gazebo/set_model_state')
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# Register the client for service /darwin/controller_manager/list_controllers
rospy.wait_for_service('/darwin/controller_manager/list_controllers')
self.get_joint_state = rospy.ServiceProxy('/darwin/controller_manager/list_controllers', ListControllers)
# Create the Darwin model
self.darwin = Darwin()
# Initialize log file
# Current time
curr_time = time.strftime('%Y%m%d%H%M%S')
# Create a log file
self.log_file = open('/home/sayantan/Knowledge/Independant Studies/Robot Walking/code/robot_walking/data/log_'
+ str(curr_time) + '_matsuoka_walk_optimization.log', 'w')
# To control if log contents are dumped on screen as well
self.verbose = True
self.log_file.write('Created log file at ' + str(curr_time) + '\n')
# Write the parameters to the log file
self.log_file.write('\n')
self.log_file.write("Constants and Parameters: " + "\n")
self.log_file.write("###########################################" + "\n")
self.log_file.write("WEIGHT_BOUNDS = " + str(self.WEIGHT_BOUNDS) + "\n")
self.log_file.write("Tr1_BOUNDS = " + str(self.Tr1_BOUNDS) + "\n")
self.log_file.write("Tr2_BOUNDS = " + str(self.Tr2_BOUNDS) + "\n")
self.log_file.write("Ta1_BOUNDS = " + str(self.Ta1_BOUNDS) + "\n")
self.log_file.write("Ta2_BOUNDS = " + str(self.Ta2_BOUNDS) + "\n")
self.log_file.write("b_BOUNDS = " + str(self.b_BOUNDS) + "\n")
self.log_file.write("s_BOUNDS = " + str(self.s_BOUNDS) + "\n")
self.log_file.write("TRIAL_DURATION = " + str(self.TRIAL_DURATION) + "\n")
self.log_file.write("STEP_DURATION = " + str(self.STEP_DURATION) + "\n")
self.log_file.write("POPULATION_SIZE = " + str(self.POPULATION_SIZE) + "\n")
self.log_file.write("MAX_ITERS = " + str(self.MAX_ITERS) + "\n")
self.log_file.write("VEL_BOUND_PCT = " + str(self.VEL_BOUND_PCT) + "\n")
self.log_file.write("C1 = " + str(self.C1) + "\n")
self.log_file.write("C2 = " + str(self.C2) + "\n")
self.log_file.write("INERTIAL_WEIGHT_BOUNDS = " + str(self.INERTIAL_WEIGHT_BOUNDS) + "\n")
self.log_file.write("###########################################" + "\n" + "\n")
# Initiate the walk optimization
self.pso()
# Subscribe to the /gazebo/model_states topic
rospy.Subscriber("/gazebo/model_states", ModelStates, subscriber_callback_modelstate)
# Register the client for service gazebo/get_model_state
rospy.wait_for_service('/gazebo/get_model_state')
get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
# Register the client for service gazebo/set_model_state
rospy.wait_for_service('/gazebo/set_model_state')
set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# Register the client for service /darwin/controller_manager/list_controllers
rospy.wait_for_service('/darwin/controller_manager/list_controllers')
get_joint_state = rospy.ServiceProxy('/darwin/controller_manager/list_controllers', ListControllers)
darwin = Darwin()
# Set the shoulder roll angle and elbow so that arms are at the side
new_angles = {}
new_angles['j_high_arm_l'] = 1.50
new_angles['j_high_arm_r'] = 1.50
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
darwin.set_angles_slow(new_angles)
# Declare lists to be used for plots
hip_left = []
knee_left = []
ankle_left = []
hip_right = []
knee_right = []
ankle_right = []
#!/usr/bin/env python
import rospy
from darwin_gazebo.darwin import Darwin
if __name__ == "__main__":
rospy.init_node("walker_demo")
rospy.loginfo("Instantiating Darwin Client")
darwin = Darwin()
rospy.sleep(1)
rospy.loginfo("Darwin Walker Demo Starting")
darwin.set_angles({"j_high_arm_l": 0.5})
darwin.set_angles({"j_high_arm_r": 0.5})
rospy.sleep(2)
darwin.set_angles({"j_high_arm_l": 0.8})
darwin.set_angles({"j_high_arm_r": 0.8})
rospy.sleep(2)
darwin.set_walk_velocity(0.2, 0, 0)
rospy.sleep(3)
darwin.set_walk_velocity(1, 0, 0)
rospy.sleep(3)
darwin.set_walk_velocity(0, 1, 0)
rospy.sleep(3)
darwin.set_walk_velocity(0, -1, 0)
rospy.sleep(3)
darwin.set_walk_velocity(-1, 0, 0)
rospy.sleep(3)