class Simulator(Observable):
def __init__(self,
vrep_host='127.0.0.1',
vrep_port=19997,
scene=None,
start=False):
self.io = VrepIO(vrep_host, vrep_port, scene, start)
# vrep.simxFinish(-1) # just in case, close all opened connections
# self._clientID = vrep.simxStart('127.0.0.1',19997, True, True, 5000, 5) # Connect to V-REP
self.robots = []
self.vrep_mode = vrep_mode
self.n_robots = 0
self.suffix_to_epuck = {}
self.seconds_to_run = 0
self._condition = threading.Condition()
self.routine_manager = RoutineManager()
self.eatable_objects = []
self.logger = Logger()
Observable.__init__(self)
def wait(self, seconds):
start = self.io.get_simulation_current_time()
while self.io.get_simulation_current_time() - start < seconds:
sleep(0.005)
def close(self):
self.detach_all_routines()
sleep(0.3)
self.io.stop_simulation()
self.io.close()
def get_object_position(self, object_name):
return array(self.io.get_object_position(object_name))
def set_object_position(self, object_name, position):
return self.io.set_object_position(object_name, position)
def add_sphere(self,
position,
sizes=[0.1, 0.1, 0.1],
mass=0.5,
eatable=True):
name = "Sphere_" + str(len(self.eatable_objects) + 1)
self.io.add_sphere(name, position, sizes, mass)
if eatable:
self.eatable_objects.append(name)
self.io._inject_lua_code("simSetObjectSpecialProperty({}, {})".format(
self.io.get_object_handle(name),
vrep.sim_objectspecialproperty_detectable_all))
self.n_spheres = len(self.eatable_objects)
for robot in self.robots:
robot.register_object(name)
def start_sphere_apparition(self,
period=5.,
min_pos=[-1., -1., .1],
max_pos=[1., 1., 1.]):
self.attach_routine(sphere_apparition,
freq=1. / period,
min_pos=min_pos,
max_pos=max_pos)
self.attach_routine(eating, freq=4.)
self.start_routine(sphere_apparition)
self.start_routine(eating)
def stop_sphere_apparition(self):
self.stop_routine(sphere_apparition)
self.stop_routine(eating)
def add_log(self, topic, data):
self.logger.add(topic, data)
def get_log(self, topic):
return self.logger.get_log(topic)
def _vrep_epuck_suffix(self, num):
if num == 0:
return ""
else:
return "#" + str(num - 1)
def epuck_from_object_name(self, name):
if not name.startswith("ePuck"):
return None
elif "#" in name:
suffix = "#" + name.split("#")[1]
else:
suffix = ""
return self.suffix_to_epuck[suffix]
def get_epuck(self, use_proximeters=range(8), verbose=False):
suffix = self._vrep_epuck_suffix(self.n_robots)
epuck = Epuck(pypot_io=VrepIO(self.io.vrep_host,
self.io.vrep_port + self.n_robots + 1),
simulator=self,
robot_id=self.n_robots,
use_proximeters=use_proximeters,
suffix=suffix)
self.suffix_to_epuck[suffix] = epuck
self.robots.append(epuck)
self.n_robots += 1
if verbose: print self.robots[-1]
return self.robots[-1]
def get_epuck_list(self, n_epucks, verbose=False):
return [self.get_epuck(verbose=verbose) for _ in range(n_epucks)]
def remove_object(self, name):
self.io.call_remote_api("simxRemoveObject",
self.io.get_object_handle(name),
sending=True)
def attach_routine(self, callback, freq, **kwargs):
routine = Routine(self, callback, self._condition, freq, **kwargs)
self.routine_manager.attach(routine)
def detach_routine(self, callback):
self.routine_manager.detach(callback)
print "Routine " + callback.__name__ + " detached"
def detach_all_routines(self):
self.routine_manager.detach_all()
def start_routine(self, callback):
if self.routine_manager.start(callback):
print "Routine " + callback.__name__ + " started"
def start_all_routines(self):
self.routine_manager.start_all()
def stop_routine(self, callback):
if self.routine_manager.stop(callback):
print "Routine " + callback.__name__ + " stopped"
def stop_all_routines(self):
self.routine_manager.stop_all()
def check_routines(self):
self.routine_manager.check()
class RobotMonitorThread(Thread):
"""
Class for monitoring objects in VREP simulator.
This class extends the Thread class and will run as in parallel to the main simulation.
Only one monitoring thread is enough for monitoring a simulation.
"""
def __init__(self, portnum, objname, height_threshold, force_threshold=10):
"""
Initializes the RobotMonitorThread thread
:param portnum: Port number on which the VREP remote API is listening on the server
(different than the one used to run the main robot simulation)
:param objname: Object name which is to be monitored
:height_threshold: If the object's height is lower than this value, the robot is considered to have falen
"""
# Call init of super class
Thread.__init__(self)
# Setup the log
home_dir = os.path.expanduser('~')
log('[MON] Monitor started')
# Create a VrepIO object using a different port number than the one used to run the main robot simulation
# Make sure that the VREP remote api on the server is listening on this port number
# Additional ports for listening can be set up on the server side by editing the file remoteApiConnections.txt
self.vrepio_obj = VrepIO(vrep_port=portnum)
# The object to be monitored
self.objname = objname
# Threshold for falling
self.height_threshold = height_threshold
# Threshold for force sensor
self.force_threshold = force_threshold
# The position of the object
self.objpos = None
# Initialize the foot sensor forces
self.l1 = 0.0
self.l2 = 0.0
self.l3 = 0.0
self.l4 = 0.0
self.l_heel_current = 0.0
self.l_heel_previous = 0.0
self.r1 = 0.0
self.r2 = 0.0
self.r3 = 0.0
self.r4 = 0.0
self.r_heel_current = 0.0
self.r_heel_previous = 0.0
# Lists to hold the foot positions as ('foot type',x,y) tuples
self.foot_position = list()
# Variable to store average foot step
self.avg_footstep = 0.0
# Flag for phase reset
self.phase_reset = False
# The x,y,z coordinates of the position
self.x = None
self.y = None
self.z = None
# The starting time
self.start_time = time.time()
self.up_time = 0.0
# The average height of the robot
self.avg_z = 0.0
# A list to store the height at each second
self.z_list = list()
# A flag which can stop the thread
self.stop_flag = False
# Lists to store torso orientations
self.torso_orientation_alpha = list()
self.torso_orientation_beta = list()
self.torso_orientation_gamma = list()
# Variables to store the torso orientation variance
self.var_torso_orientation_alpha = 0.0
self.var_torso_orientation_beta = 0.0
self.var_torso_orientation_gamma = 0.0
# A flag to indicate if the robot has fallen
self.fallen = False
# Set up the ROS listener
# rospy.init_node('nico_feet_force_listener', anonymous=True)
# rospy.Subscriber("/nico_feet_forces", String, force_sensor_callback)
def reset_timer(self):
self.start_time = time.time()
def run(self):
"""
The monitoring logic is implemented here. The self.objpos is updated at a preset frequency with the latest
object positions.
:return: None
"""
while not self.stop_flag:
# Update the current position
self.objpos = self.vrepio_obj.get_object_position(self.objname)
self.x = self.objpos[0]
self.y = self.objpos[1]
self.z = self.objpos[2]
# Update the time
self.up_time = time.time() - self.start_time
# Append the current height to the height list
self.z_list.append(self.z)
if self.z < self.height_threshold:
# Set the flag which indicates that the robot has fallen
self.fallen = True
# Calculate the average height
self.avg_z = sum(self.z_list) / float(len(self.z_list))
# Update force sensor readings (first index signifies force vector and second index signifies z axis force)
self.r1 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('right_sensor_1'), streaming=True)[1][2]
self.r2 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('right_sensor_2'), streaming=True)[1][2]
self.r3 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('right_sensor_3'), streaming=True)[1][2]
self.r4 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('right_sensor_4'), streaming=True)[1][2]
# Average the forces on the right heel
self.r_heel_current = (self.r3 + self.r4)/2.0
self.l1 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('left_sensor_1'), streaming=True)[1][2]
self.l2 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('left_sensor_2'), streaming=True)[1][2]
self.l3 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('left_sensor_3'), streaming=True)[1][2]
self.l4 = self.vrepio_obj.call_remote_api('simxReadForceSensor', self.vrepio_obj.get_object_handle('left_sensor_4'), streaming=True)[1][2]
# Average the forces on the right heel
self.l_heel_current = (self.l3 + self.l4) / 2.0
# Store the foot position if left foot strikes ground
if self.l_heel_previous < self.force_threshold <= self.l_heel_current:
# Get the position of the left foot
left_foot_position = self.vrepio_obj.get_object_position('left_foot_11_respondable')
left_foot_position_x = left_foot_position[0]
left_foot_position_y = left_foot_position[1]
self.foot_position.append(('l', left_foot_position_x, left_foot_position_y))
print('Left foot prev_z_force: {0} curr_z_force: {1}'.format(self.l_heel_previous, self.l_heel_current))
# Store the foot position and do phase reset (if needed) if the right foot strikes the ground
if self.r_heel_previous < self.force_threshold <= self.r_heel_current:
# Get the position of the left foot
right_foot_position = self.vrepio_obj.get_object_position('right_foot_11_respondable')
right_foot_position_x = right_foot_position[0]
right_foot_position_y = right_foot_position[1]
self.foot_position.append(('r', right_foot_position_x, right_foot_position_y))
# Perform phase reset
self.phase_reset = True
print('**** Resetting phase ****')
print('prev_z_force: {0} curr_z_force: {1}'.format(self.r_heel_previous, self.r_heel_current))
else:
self.phase_reset = False
self.l_heel_previous = self.l_heel_current
self.r_heel_previous = self.r_heel_current
# Store the current torso orientation
self.torso_orientation_alpha.append(self.vrepio_obj.get_object_orientation(self.objname)[0])
self.torso_orientation_beta.append(self.vrepio_obj.get_object_orientation(self.objname)[1])
self.torso_orientation_gamma.append(self.vrepio_obj.get_object_orientation(self.objname)[2])
# Sleep
time.sleep(0.1)
def calc_var_orientation(self):
"""
Function to compute the variation in the torso orientations
:return: None
"""
self.var_torso_orientation_alpha = np.var(self.torso_orientation_alpha)
self.var_torso_orientation_beta = np.var(self.torso_orientation_beta)
self.var_torso_orientation_gamma = np.var(self.torso_orientation_gamma)
def calc_avg_footstep(self):
# Inspect the foot position list and filter out only alternating types of positions (l,r,l,r,...)
# If there are multiple positions of same type (l,l,l,r,l,r,r,r,l,r,l,l,...) consider only the last position of
# each type (l1, l2, l3, r1, r2, l4, r3, l5, l6, r3, r4 -> l3, r2, l4, r3, l6, r4)
# A list to store the indices of alternating foot positions
alternating_indices = list()
alternating_indices.append(0)
# Find the indices of alternating foot positions
for idx in range(len(self.foot_position)):
foot_pos = self.foot_position[idx]
last_type = self.foot_position[alternating_indices[-1]][0]
curr_type = foot_pos[0]
if last_type == curr_type:
continue
else:
alternating_indices.append(idx)
# This list contains foot positions of alternating feet
alternating_foot_positions = [self.foot_position[i] for i in alternating_indices]
# Calculate the difference between the alternating foot positions (x-axis)
# Each tuple in the list is of the form ('l/r', x-pos, y-pos)
foot_step_sizes = [t[1] - s[1] for s, t in zip(alternating_foot_positions, alternating_foot_positions[1:])]
# Calculate the average foot step length in the x-direction
self.avg_footstep = np.mean([foot for foot in foot_step_sizes])
def stop(self):
"""
Sets the flag which will stop the thread
:return: None
"""
# Flag to stop the monitoring thread
self.stop_flag = True
# Close the monitoring vrep connection
self.vrepio_obj.close()
# Calculate the average foot step size
self.calc_avg_footstep()
# Calculate the variance of the torso orientation
self.calc_var_orientation()
# Store the results
GaitEvalResult.fallen = self.fallen
GaitEvalResult.up_time = self.up_time
GaitEvalResult.x = self.x
GaitEvalResult.y = abs(self.y)
GaitEvalResult.avg_footstep = self.avg_footstep
GaitEvalResult.var_torso_orientation_alpha = self.var_torso_orientation_alpha
GaitEvalResult.var_torso_orientation_beta = self.var_torso_orientation_beta
GaitEvalResult.var_torso_orientation_gamma = self.var_torso_orientation_gamma
print 'Fall:{0}, ' \
'Up_time:{1}, ' \
'X-distance:{2}, ' \
'Deviation (mag):{3}, ' \
'Avg. step length:{4}, ' \
'Torso orientation variance Alpha-Beta-Gamma:{5}|{6}|{7}'.format(self.fallen,
self.up_time,
self.x,
abs(self.y),
self.avg_footstep,
self.var_torso_orientation_alpha,
self.var_torso_orientation_beta,
self.var_torso_orientation_gamma)
log('[MON] Monitor finished')
class AcrobotVrepEnv(gym.Env):
metadata = {'render.modes': ['human']}
def __init__(self):
self._seed = None
self.max_speed = 10.0
self.max_torque = 0.5
self.dt = .2
self.step_counter = 0
self.time_counter = time.time()
# Coordinates of the outer link tip in local (homogeneous) coordinates
self.outer_tip_local = np.array([[0.0], [0.0], [-0.1], [1.0]])
# The tip of outer_link should go above this height (z coordinate in the global frame)
self.threshold = 0.75
pypot.vrep.close_all_connections()
self.vrepio = VrepIO(vrep_host=host,
vrep_port=port,
scene=scene,
start=False)
#self.vrepio.call_remote_api('simxSynchronous',True)
self.vrepio.start_simulation()
print('(AcrobotVrepEnv) initialized')
obs = np.array([np.inf] * 4)
self.action_space = spaces.Box(low=-self.max_torque,
high=self.max_torque,
shape=(1, ))
self.observation_space = spaces.Box(-obs, obs)
def _self_observe(self):
self.active_joint_pos = self.vrepio.call_remote_api(
'simxGetJointPosition',
self.vrepio.get_object_handle('active_joint'),
streaming=True)
# VREP Rempte API does not provide a direct function for retrieving joint velocities
# Refer to http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=2393 for details
self.active_joint_velocity = self.vrepio.call_remote_api(
'simxGetObjectFloatParameter',
self.vrepio.get_object_handle('active_joint'),
2012,
streaming=True)
self.passive_joint_pos = self.vrepio.call_remote_api(
'simxGetJointPosition',
self.vrepio.get_object_handle('passive_joint'),
streaming=True)
# VREP Rempte API does not provide a direct function for retrieving joint velocities
# Refer to http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=2393 for details
self.passive_joint_velocity = self.vrepio.call_remote_api(
'simxGetObjectFloatParameter',
self.vrepio.get_object_handle('passive_joint'),
2012,
streaming=True)
self.observation = np.array([
self.active_joint_pos, self.active_joint_velocity,
self.passive_joint_pos, self.passive_joint_velocity
]).astype('float32')
def _render(self, mode='human', close=False):
return
def _step(self, actions):
self.step_counter += 1
# Advance simulation by 1 step
#self.vrepio.call_remote_api('simxSynchronousTrigger')
actions = np.clip(actions, -self.max_torque, self.max_torque)[0]
apply_torque = actions
max_vel = 0.0
# If the torque to be applied is negative, change the sign of the max velocity and then apply a positive torque
# with the same magnitude
if apply_torque < 0.0:
max_vel = -self.max_speed
apply_torque = -apply_torque
else:
max_vel = self.max_speed
# step
# Set the target velocity
self.vrepio.call_remote_api(
'simxSetJointTargetVelocity',
self.vrepio.get_object_handle('active_joint'),
max_vel,
sending=True)
# Set the torque of active_joint
self.vrepio.call_remote_api(
'simxSetJointForce',
self.vrepio.get_object_handle('active_joint'),
apply_torque,
sending=True)
# observe again
self._self_observe()
# cost
# If the tip of the outer link is below threshold then reward is -1 else 0
# First get the position of the outer link frame
x, y, z = self.vrepio.call_remote_api(
'simxGetObjectPosition',
self.vrepio.get_object_handle('outer_link'),
-1,
streaming=True)
# Then calculate the orientation of the outer link frame in Euler angles
a, b, g = self.vrepio.call_remote_api(
'simxGetObjectOrientation',
self.vrepio.get_object_handle('outer_link'),
-1,
streaming=True)
# Compute the homogeneous rotation matrix
rot_mat = euler_matrix(a, b, g)
# Insert the position
rot_mat[0][3] = x
rot_mat[1][3] = y
rot_mat[2][3] = z
# The outer tip of outer_link is at coordinate (0,0,-0.1) in the local frame
# Convert this into a position in the global frame
tip_global = np.dot(rot_mat, self.outer_tip_local)
# Consider the height (z-coordinate) of the tip in the global frame
tip_height_global = tip_global[2][0]
# If this height is above the threshold then reward is 0 else -1
# If height is above threshold, terminate episode
terminal = False
if tip_height_global >= self.threshold:
terminal = True
print '=================================== Reached threshold ==================================='
reward = 0
else:
reward = -1
time.sleep(self.dt)
return self.observation, reward, terminal, {}
def _reset(self):
self.vrepio.stop_simulation()
print 'Reset called ######################'
print 'Duration of episode: {}'.format(time.time() - self.time_counter)
self.time_counter = time.time()
print 'Number of steps in episode: {}'.format(self.step_counter)
self.step_counter = 0
# Try self.vrepio.call_remote_api('simxStopSimulation',remote_api.simx_opmode_blocking)
pypot.vrep.close_all_connections()
self.vrepio = VrepIO(vrep_host=host,
vrep_port=port,
scene=scene,
start=False)
# self.vrepio.call_remote_api('simxSynchronous',True)
self.vrepio.start_simulation()
self._self_observe()
return self.observation
def _destroy(self):
self.vrepio.stop_simulation()
class Simulator(Observable):
def __init__(self, vrep_host='127.0.0.1', vrep_port=19997, scene=None, start=False):
self.io = VrepIO(vrep_host, vrep_port, scene, start)
# vrep.simxFinish(-1) # just in case, close all opened connections
# self._clientID = vrep.simxStart('127.0.0.1',19997, True, True, 5000, 5) # Connect to V-REP
self.robots = []
self.vrep_mode = vrep_mode
self.n_robots = 0
self.suffix_to_epuck = {}
self.seconds_to_run = 0
self._condition = threading.Condition()
self.routine_manager = RoutineManager()
self.eatable_objects = []
self.logger = Logger()
Observable.__init__(self)
def wait(self, seconds):
start = self.io.get_simulation_current_time()
while self.io.get_simulation_current_time() - start < seconds:
sleep(0.005)
def close(self):
self.detach_all_routines()
sleep(0.3)
self.io.stop_simulation()
self.io.close()
def get_object_position(self, object_name):
return array(self.io.get_object_position(object_name))
def set_object_position(self, object_name, position):
return self.io.set_object_position(object_name, position)
def add_sphere(self, position, sizes=[0.1, 0.1, 0.1], mass=0.5, eatable=True):
name = "Sphere_" + str(len(self.eatable_objects) + 1)
self.io.add_sphere(name, position, sizes, mass)
if eatable:
self.eatable_objects.append(name)
self.io._inject_lua_code("simSetObjectSpecialProperty({}, {})".format(self.io.get_object_handle(name), vrep.sim_objectspecialproperty_detectable_all))
self.n_spheres = len(self.eatable_objects)
for robot in self.robots:
robot.register_object(name)
def start_sphere_apparition(self, period=5., min_pos=[-1., -1., .1], max_pos=[1., 1., 1.]):
self.attach_routine(sphere_apparition, freq=1./period, min_pos=min_pos, max_pos=max_pos)
self.attach_routine(eating, freq=4.)
self.start_routine(sphere_apparition)
self.start_routine(eating)
def stop_sphere_apparition(self):
self.stop_routine(sphere_apparition)
self.stop_routine(eating)
def add_log(self, topic, data):
self.logger.add(topic, data)
def get_log(self, topic):
return self.logger.get_log(topic)
def _vrep_epuck_suffix(self, num):
if num == 0:
return ""
else:
return "#" + str(num - 1)
def epuck_from_object_name(self, name):
if not name.startswith("ePuck"):
return None
elif "#" in name:
suffix = "#" + name.split("#")[1]
else:
suffix = ""
return self.suffix_to_epuck[suffix]
def get_epuck(self, use_proximeters=range(8), verbose=False):
suffix = self._vrep_epuck_suffix(self.n_robots)
epuck = Epuck(pypot_io=VrepIO(self.io.vrep_host, self.io.vrep_port + self.n_robots + 1), simulator=self, robot_id=self.n_robots, use_proximeters=use_proximeters, suffix=suffix)
self.suffix_to_epuck[suffix] = epuck
self.robots.append(epuck)
self.n_robots += 1
if verbose: print self.robots[-1]
return self.robots[-1]
def get_epuck_list(self, n_epucks, verbose=False):
return [self.get_epuck(verbose=verbose) for _ in range(n_epucks)]
def remove_object(self, name):
self.io.call_remote_api("simxRemoveObject", self.io.get_object_handle(name), sending=True)
def attach_routine(self,callback, freq, **kwargs):
routine = Routine(self, callback, self._condition, freq, **kwargs)
self.routine_manager.attach(routine)
def detach_routine(self, callback):
self.routine_manager.detach(callback)
print "Routine " + callback.__name__ + " detached"
def detach_all_routines(self):
self.routine_manager.detach_all()
def start_routine(self, callback):
if self.routine_manager.start(callback):
print "Routine " + callback.__name__ + " started"
def start_all_routines(self):
self.routine_manager.start_all()
def stop_routine(self, callback):
if self.routine_manager.stop(callback):
print "Routine " + callback.__name__ + " stopped"
def stop_all_routines(self):
self.routine_manager.stop_all()
def check_routines(self):
self.routine_manager.check()
class RobotMonitorThread(Thread):
"""
Class for monitoring objects in VREP simulator.
This class extends the Thread class and will run as in parallel to the main simulation.
Only one monitoring thread is enough for monitoring a simulation.
"""
def __init__(self, portnum, objname, height_threshold):
"""
Initializes the RobotMonitorThread thread
:param portnum: Port number on which the VREP remote API is listening on the server
(different than the one used to run the main robot simulation)
:param objname: Object name which is to be monitored
:height_threshold: If the object's height is lower than this value, the robot is considered to have falen
"""
# Call init of super class
Thread.__init__(self)
# Create a VrepIO object using a different port number than the one used to run the main robot simulation
# Make sure that the VREP remote api on the server is listening on this port number
# Additional ports for listening can be set up on the server side by editing the file remoteApiConnections.txt
self.vrepio_obj = VrepIO(vrep_port=portnum)
# The object to be monitored
self.objname = objname
# Threshold for falling
self.height_threshold = height_threshold
# The position of the object
self.objpos = None
# The x,y,z coordinates of the position
self.x = None
self.y = None
self.z = None
# The orientation of the body
self.torso_euler_angles = None
# The starting time
self.start_time = time.time()
self.up_time = 0.0
# The average height of the robot
self.avg_z = 0.0
# A list to store the height at each second
self.z_list = list()
# A flag which can stop the thread
self.stop_flag = False
# A flag to indicate if the robot has fallen
self.fallen = False
# Set up the ROS listener
# rospy.init_node('nico_feet_force_listener', anonymous=True)
# rospy.Subscriber("/nico_feet_forces", String, force_sensor_callback)
def reset_timer(self):
self.start_time = time.time()
def run(self):
"""
The monitoring logic is implemented here. The self.objpos is updated at a preset frequency with the latest
object positions.
:return: None
"""
while not self.stop_flag:
# Update the current position
self.objpos = self.vrepio_obj.get_object_position(self.objname)
self.x = self.objpos[0]
self.y = self.objpos[1]
self.z = self.objpos[2]
self.torso_euler_angles = self.vrepio_obj.call_remote_api(
'simxGetObjectOrientation',
self.vrepio_obj.get_object_handle('torso_11_respondable'),
-1,
# Orientation needed with respect to world frame
streaming=True)
# Update the time
self.up_time = time.time() - self.start_time
# Append the current height to the height list
self.z_list.append(self.z)
if self.z < self.height_threshold:
# Set the flag which indicates that the robot has fallen
self.fallen = True
# Calculate the average height
self.avg_z = sum(self.z_list) / float(len(self.z_list))
# Sleep
time.sleep(0.1)
def stop(self):
"""
Sets the flag which will stop the thread
:return: None
"""
# Flag to stop the monitoring thread
self.stop_flag = True
# Close the monitoring vrep connection
self.vrepio_obj.close()
class RobotMonitorThread(Thread):
"""
Class for monitoring objects in VREP simulator.
This class extends the Thread class and will run as in parallel to the main simulation.
Only one monitoring thread is enough for monitoring a simulation.
"""
def __init__(self, portnum, objname, height_threshold, force_threshold=10):
"""
Initializes the RobotMonitorThread thread
:param portnum: Port number on which the VREP remote API is listening on the server
(different than the one used to run the main robot simulation)
:param objname: Object name which is to be monitored
:height_threshold: If the object's height is lower than this value, the robot is considered to have falen
"""
# Call init of super class
Thread.__init__(self)
# Create a VrepIO object using a different port number than the one used to run the main robot simulation
# Make sure that the VREP remote api on the server is listening on this port number
# Additional ports for listening can be set up on the server side by editing the file remoteApiConnections.txt
self.vrepio_obj = VrepIO(vrep_port=portnum)
# The object to be monitored
self.objname = objname
# Threshold for falling
self.height_threshold = height_threshold
# Threshold for force sensor
self.force_threshold = force_threshold
# The position of the object
self.objpos = None
# Initialize the forces
self.r1 = 0.0
self.r2 = 0.0
self.r3 = 0.0
self.r4 = 0.0
self.r_heel_current = 0.0
self.r_heel_previous = 0.0
# Flag for phase reset
self.phase_reset = False
# The x,y,z coordinates of the position
self.x = None
self.y = None
self.z = None
# The starting time
self.start_time = time.time()
self.up_time = 0.0
# The average height of the robot
self.avg_z = 0.0
# A list to store the height at each second
self.z_list = list()
# A flag which can stop the thread
self.stop_flag = False
# A flag to indicate if the robot has fallen
self.fallen = False
# Set up the ROS listener
# rospy.init_node('nico_feet_force_listener', anonymous=True)
# rospy.Subscriber("/nico_feet_forces", String, force_sensor_callback)
def reset_timer(self):
self.start_time = time.time()
def run(self):
"""
The monitoring logic is implemented here. The self.objpos is updated at a preset frequency with the latest
object positions.
:return: None
"""
while not self.stop_flag:
# Update the current position
self.objpos = self.vrepio_obj.get_object_position(self.objname)
self.x = self.objpos[0]
self.y = self.objpos[1]
self.z = self.objpos[2]
# Update the time
self.up_time = time.time() - self.start_time
# Append the current height to the height list
self.z_list.append(self.z)
if self.z < self.height_threshold:
# Set the flag which indicates that the robot has fallen
self.fallen = True
# Calculate the average height
self.avg_z = sum(self.z_list) / float(len(self.z_list))
# Update force sensor readings (first index signifies force vector and second index signifies z axis force)
self.r1 = self.vrepio_obj.call_remote_api(
'simxReadForceSensor',
self.vrepio_obj.get_object_handle('right_sensor_1'),
streaming=True)[1][2]
self.r2 = self.vrepio_obj.call_remote_api(
'simxReadForceSensor',
self.vrepio_obj.get_object_handle('right_sensor_2'),
streaming=True)[1][2]
self.r3 = self.vrepio_obj.call_remote_api(
'simxReadForceSensor',
self.vrepio_obj.get_object_handle('right_sensor_3'),
streaming=True)[1][2]
self.r4 = self.vrepio_obj.call_remote_api(
'simxReadForceSensor',
self.vrepio_obj.get_object_handle('right_sensor_4'),
streaming=True)[1][2]
# Average the forces on the right heel
self.r_heel_current = (self.r3 + self.r4) / 2.0
if self.r_heel_previous < self.force_threshold <= self.r_heel_current:
self.phase_reset = True
print('**** Resetting phase ****')
print('prev_z_force: {0} curr_z_force: {1}'.format(
self.r_heel_previous, self.r_heel_current))
else:
self.phase_reset = False
self.r_heel_previous = self.r_heel_current
# Sleep
time.sleep(0.1)
def stop(self):
"""
Sets the flag which will stop the thread
:return: None
"""
# Flag to stop the monitoring thread
self.stop_flag = True
# Close the monitoring vrep connection
self.vrepio_obj.close()