def MyPythonCallback():
# Get status of robot
status = TommIsim.GetStatus()
# Check if robot has left the fiducial operational range.
# Below we turn on the pre-programmed set of actions that
# end with the robot rolling over. Here, we check when this
# happens by the "roll" exceeding 55 degrees in either
# direction. For an actual AI training script, one should
# probably reset if any of yaw, pitch, or roll go out of
# range.
if fabs(status['roll']) > 55.0:
TommIsim.Reset()
def checkStatus(motor):
global keys
status = TommIsim.GetStatus()
margin = 0.2
for i in range(len(keys)):
key = keys[i]
if status[key] > motor[key] + margin or status[key] < motor[key] - margin:
return False, i
return True, -1
def softReset():
global motor
global count
status = TommIsim.GetStatus()
motor['BL_hip'] = status['BL_hip']
motor['BR_hip'] = status['BR_hip']
motor['BL_foot'] = status['BL_foot']
motor['BR_foot'] = status['BR_foot']
motor['FL_hip'] = status['FL_hip']
motor['FR_hip'] = status['FR_hip']
motor['FL_foot'] = status['FL_foot']
motor['FR_foot'] = status['FR_foot']
def MyPythonCallback():
global motor
global count
global metaCount
# global status
global keys
global prev_status
if metaCount > 50:
done, where = checkStatus(motor)
# flag = False
# for key in prev_status:
# if status[key] - prev_status[key] > 0.001:
# flag = True
# prev_status = status
# # print(key)
# break
# if not flag:
if done:
# time.sleep(5)
status = TommIsim.GetStatus()
for key in keys:
if "foot" in key: # or "FR_" in key:
status[key] = (status[key] - 130) / 10
else:
status[key] = (status[key] - 120) / 10
for key in ['x', 'y', 'z']:
status[key] = status[key] / 10
for key in ['yaw', 'pitch', 'roll']:
status[key] = status[key] / 180
# prev_status = status
# print(status, prev_status)
# for key in keys:
# status[key] = (status[key] - 110)/10
# for key in ['x', 'y', 'z']:
# status[key] = status[key]/10
# for key in ['yaw', 'pitch', 'roll']:
# status[key] = status[key]/180
content = json.dumps(status)
sock.send_string(content)
message = sock.recv().decode("utf-8")
if "Reset" in message:
TommIsim.Reset()
metaCount = 0
motor = {
'BL_foot': 120,
'BL_hip': 120,
'BR_foot': 120,
'BR_hip': 120,
'FL_foot': 120,
'FL_hip': 120,
'FR_foot': 120,
'FR_hip': 120
}
else:
motor = json.loads(message)
for key in keys:
if "foot" in key: # or "FR_" in key:
motor[key] = 130 + motor[key] * 10
else:
motor[key] = 120 + motor[key] * 10
TommIsim.SetMotors(motor)
else:
# print(status, prev_status)
count += 1
if count > 1000:
status = TommIsim.GetStatus()
print("################## DEAD END #####################",
keys[where], status[keys[where]], motor[keys[where]])
softReset()
count = 0
TommIsim.SetMotors(motor)
# status = TommIsim.GetStatus()
# print("Where: ", where)
# printComparision(status, motor)
else:
metaCount += 1
TommIsim.SetMotors(motor)
TommIsim.SetMotors(motor)
else:
# print(status, prev_status)
count += 1
if count > 1000:
status = TommIsim.GetStatus()
print("################## DEAD END #####################",
keys[where], status[keys[where]], motor[keys[where]])
softReset()
count = 0
TommIsim.SetMotors(motor)
# status = TommIsim.GetStatus()
# print("Where: ", where)
# printComparision(status, motor)
else:
metaCount += 1
TommIsim.SetMotors(motor)
#======================================================================
TommIsim.SetRunRealTime(True)
# TommIsim.SetUsePreprogrammedActions( True ) # comment this line to turn off the pre-programmed sequence used for the initial video
TommIsim.Setup()
TommIsim.RegisterCallback(MyPythonCallback)
# n.b. this will block until the simulation is complete
TommIsim.Run()
print("Done.")
def MyPythonCallback():
# Get the current status of the robot in the form of a dictionary.
# Entries like "BL_hip" indicate the angle of the Back Left leg
# hip joint.
status = TommIsim.GetStatus()
# are based on the physical limitations of the robot. Setting to values
# outside of these ranges will cause the joint to go only to the
# appropriate limit. These are:
#
# hip: 45 - 140 degrees
# foot: 50 - 180 degrees
#
# Finally, the simulation will remember the last setting for each joint
# and will keep trying to set it there if a new value is not entered
# in this dictionary. (i.e. you don't have to set every motor with
# every call to this).
#
# motors = {}
# motors['BL_foot'] = 47.2;
# motors['BL_hip' ] = 55.7
# TommIsim.SetMotors(motors)
#======================================================================
TommIsim.SetRunRealTime( False )
TommIsim.SetUsePreprogrammedActions( True ) # comment this line to turn off the pre-programmed sequence used for the initial video
TommIsim.Setup()
TommIsim.RegisterCallback(MyPythonCallback)
# n.b. this will block until the simulation is complete
TommIsim.Run()
print("Done.")