def freezeBase(self, flag):
#toggle gravity
req_reset_gravity = SetPhysicsPropertiesRequest()
#ode config
req_reset_gravity.time_step = 0.001
req_reset_gravity.max_update_rate = 1000
req_reset_gravity.ode_config.sor_pgs_iters = 50
req_reset_gravity.ode_config.sor_pgs_w = 1.3
req_reset_gravity.ode_config.contact_surface_layer = 0.001
req_reset_gravity.ode_config.contact_max_correcting_vel = 100
req_reset_gravity.ode_config.erp = 0.2
req_reset_gravity.ode_config.max_contacts = 20
if (flag):
req_reset_gravity.gravity.z = 0.0
else:
req_reset_gravity.gravity.z = -9.81
self.reset_gravity(req_reset_gravity)
# create the message
req_reset_world = SetModelStateRequest()
#create model state
model_state = ModelState()
model_state.model_name = "hyq"
model_state.pose.position.x = 0.0
model_state.pose.position.y = 0.0
model_state.pose.position.z = 0.8
model_state.pose.orientation.w = 1.0
model_state.pose.orientation.x = 0.0
model_state.pose.orientation.y = 0.0
model_state.pose.orientation.z = 0.0
model_state.twist.linear.x = 0.0
model_state.twist.linear.y = 0.0
model_state.twist.linear.z = 0.0
model_state.twist.angular.x = 0.0
model_state.twist.angular.y = 0.0
model_state.twist.angular.z = 0.0
req_reset_world.model_state = model_state
# send request and get response (in this case none)
self.reset_world(req_reset_world)
def update_gravity_call(self):
rospy.wait_for_service('/gazebo/pause_physics')
try:
self.pause()
except (rospy.ServiceException, e):
print("/gazebo/pause_physics service call failed")
set_physics_request = SetPhysicsPropertiesRequest()
set_physics_request.time_step = self._time_step.data
set_physics_request.max_update_rate = self._max_update_rate.data
set_physics_request.gravity = self._gravity
set_physics_request.ode_config = self._ode_config
print(str(set_physics_request.gravity))
result = self.set_physics(set_physics_request)
rospy.loginfo("Gravity Update Result==" + str(result.success) +
",message==" + str(result.status_message))
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException, e):
print("/gazebo/unpause_physics service call failed")
def _update_properties(self, update_rate):
current_properties = self.get_physics(GetPhysicsPropertiesRequest())
new_properties = SetPhysicsPropertiesRequest()
new_properties.gravity = current_properties.gravity
new_properties.ode_config = current_properties.ode_config
new_properties.time_step = current_properties.time_step
new_properties.max_update_rate = update_rate
self.set_physics(new_properties)
def update_gravity_call(self):
self.pauseSim()
set_physics_request = SetPhysicsPropertiesRequest()
set_physics_request.time_step = self._time_step.data
set_physics_request.max_update_rate = self._max_update_rate.data
set_physics_request.gravity = self._gravity
set_physics_request.ode_config = self._ode_config
rospy.logdebug(str(set_physics_request.gravity))
result = self.set_physics(set_physics_request)
rospy.logdebug("Gravity Update Result==" + str(result.success) +
",message==" + str(result.status_message))
self.unpauseSim()
def update_gravity_call(self):
# to update the phsics parameters, we have to pause the Simulation first
self.pauseSim()
# Fill up the Service Request
set_physics_request = SetPhysicsPropertiesRequest()
set_physics_request.time_step = self._time_step.data
set_physics_request.max_update_rate = self._max_update_rate.data
set_physics_request.gravity = self._gravity
set_physics_request.ode_config = self._ode_config
rospy.logdebug(str(set_physics_request.gravity))
# We call the Set Physics Service Request
result = self.set_physics(set_physics_request)
rospy.logdebug("Gravity Update Result==" + str(result.success) +
",message==" + str(result.status_message))
# Once update the physics parameters, we unpause the simulation
self.unpauseSim()
def update_gravity_call(self):
"Update the gravity parameters of the environment"
self.pause_sim()
set_physics_request = SetPhysicsPropertiesRequest()
set_physics_request.time_step = self._time_step.data
set_physics_request.max_update_rate = self._max_update_rate.data
set_physics_request.gravity = self._gravity
set_physics_request.ode_config = self._ode_config
rospy.loginfo("tiger_application" + str(set_physics_request.gravity))
result = self.set_physics(set_physics_request)
rospy.loginfo("tiger_application" + "Gravity Update Result==" +
str(result.success) + ",message==" +
str(result.status_message))
self.unpause_sim()
def initialize_physics_params(self,
time_step=Float64(0.001),
max_update_rate=Float64(1000.0),
gravity=Vector3(0.0, 0.0, -9.80655),
ode_physics=ODE_PHYSICS_DEFAULT):
"""
Updates physics parameters at startup.
"""
set_physics_request = SetPhysicsPropertiesRequest()
set_physics_request.time_step = time_step.data
set_physics_request.max_update_rate = max_update_rate.data
set_physics_request.gravity = gravity
set_physics_request.ode_config = ode_physics
try:
self.services['set_physics'](set_physics_request)
except rospy.ServiceException as exc:
rospy.logerr('Failed to call set_physics service with following '
'error: {}.'.format(exc))
def setup_env(self,
verbose: int,
real_time_rate: float,
max_step_size: float,
play: bool = True):
# set up physics
get_physics_msg = GetPhysicsPropertiesRequest()
current_physics = self.get_physics.call(get_physics_msg)
set_physics_msg = SetPhysicsPropertiesRequest()
set_physics_msg.gravity = current_physics.gravity
set_physics_msg.ode_config = current_physics.ode_config
set_physics_msg.max_update_rate = real_time_rate / max_step_size
if max_step_size is None:
max_step_size = current_physics.time_step
self.max_step_size = max_step_size
set_physics_msg.time_step = max_step_size
self.set_physics.call(set_physics_msg)
if play:
self.play()
def update(self):
"""Adjust Gazebos update rate to meet desired output frequency of the target topic.
"""
frequency = self.rater.get_hz(self.param.topics.target)
frequency = frequency[0] if frequency else None
# Check if there's anything to do:
if not frequency or (frequency > self.param.frequency.min
and frequency < self.param.frequency.max):
return
current_properties = self.get_physics(GetPhysicsPropertiesRequest())
new_properties = SetPhysicsPropertiesRequest()
new_properties.gravity = current_properties.gravity
new_properties.ode_config = current_properties.ode_config
new_properties.time_step = current_properties.time_step
# Calculate new update rate
if frequency < self.param.frequency.min:
new_update_rate = (current_properties.max_update_rate -
(current_properties.max_update_rate -
self.param.update_rate.min) *
self.param.update_rate.control.down *
self.param.frequency.desired / frequency)
new_update_rate = max(new_update_rate, self.param.update_rate.min)
elif frequency > self.param.frequency.max:
new_update_rate = (current_properties.max_update_rate +
(self.param.update_rate.max -
current_properties.max_update_rate) *
self.param.update_rate.control.up *
self.param.frequency.desired / frequency)
new_update_rate = min(new_update_rate, self.param.update_rate.max)
new_properties.max_update_rate = new_update_rate
rospy.logdebug(f"NEW GAZEBO PHYSICS PROPERTIES: {new_properties}")
rospy.logdebug(f"CURRENT CAMERA UPDATE RATE: {frequency} ")
self.set_physics(new_properties)
def setPhysicsParameters(self, update_rate=1000.0):
'''
This method is used to set the physics parameters of Gazebo simulations. Mainly, this method is used
only to change in a fast way the simulation speed, so we can simulate faster than real time.
If update_rate = 1000.0 we are running at real time, but if update_rate = -1 we are running the simulation
as fast as possible.
'''
set_physics_msg = SetPhysicsPropertiesRequest()
set_physics_msg.time_step = self._time_step.data
set_physics_msg.max_update_rate = Float64(update_rate).data
set_physics_msg.gravity = self._gravity
set_physics_msg.ode_config = self._ode_config
# Block the code until the service is available
rospy.wait_for_service('/gazebo/set_physics_properties')
changed_physics_done = False
counter = 0
# We try to change the simulation physics until we reach the maximum tries or until the service call have succeded
while not changed_physics_done and not rospy.is_shutdown():
if counter < self._max_retry:
try:
self.set_physics(set_physics_msg)
changed_physics_done = True
except rospy.ServiceException as e:
counter += 1
rospy.logerr(
"/gazebo/set_physics_properties service call failed")
else:
error_message = "Maximum retries done" + str(
self._max_retry
) + ", please check Gazebo set_physics_properties service"
rospy.logerr(error_message)
assert False, error_message
try:
get_srv = rospy.ServiceProxy (get_srv_name, GetPhysicsProperties)
get_resp = get_srv ()
except rospy.ServiceException, e:
print ('Service initialization failed: %s' % e)
return False
if not get_resp.success:
print (get_resp.status_message)
return False
# Copy current physics properties, except gravity, so that we don't change
# anything else
set_req = SetPhysicsPropertiesRequest ()
set_req.time_step = get_resp.time_step
set_req.max_update_rate = get_resp.max_update_rate
set_req.ode_config = get_resp.ode_config
if on:
set_req.gravity.z = -9.8
else:
set_req.gravity.z = 0
# Set physics properties, everything except gravity is same as current state
set_srv_name = '/gazebo/set_physics_properties'
print 'Waiting for ROS service %s...' % set_srv_name
rospy.wait_for_service (set_srv_name)
class GravityControl(object):
def __init__(self):
print "Starting Gravity Service "
self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
#Bad for Sim#self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation', Empty)
service_name = '/gazebo/set_physics_properties'
rospy.loginfo("Waiting for service "+str(service_name))
rospy.wait_for_service(service_name)
rospy.loginfo("Service Found "+str(service_name))
self.set_physics = rospy.ServiceProxy(service_name, SetPhysicsProperties)
self.init_values()
print "Init Grav Sucessful"
def init_values(self):
print "Starting init Grav"
self._time_step = Float64(0.001)
self._max_update_rate = Float64(1000.0)
self._gravity = Vector3()
self._gravity.x = 0.0
self._gravity.y = 0.0
self._gravity.z = 0.0
self._ode_config = ODEPhysics()
self._ode_config.auto_disable_bodies = False
self._ode_config.sor_pgs_precon_iters = 0
self._ode_config.sor_pgs_iters = 50
self._ode_config.sor_pgs_w = 1.3
self._ode_config.sor_pgs_rms_error_tol = 0.0
self._ode_config.contact_surface_layer = 0.001
self._ode_config.contact_max_correcting_vel = 0.0
self._ode_config.cfm = 0.0
self._ode_config.erp = 0.2
self._ode_config.max_contacts = 20
self.update_gravity_call()
def update_gravity_call(self):
rospy.wait_for_service('/gazebo/pause_physics')
try:
self.pause()
print "Pause"
except rospy.ServiceException, e:
print ("/gazebo/pause_physics service call failed")
set_physics_request = SetPhysicsPropertiesRequest()
set_physics_request.time_step = self._time_step.data
set_physics_request.max_update_rate = self._max_update_rate.data
set_physics_request.gravity = self._gravity
set_physics_request.ode_config = self._ode_config
print str(set_physics_request.gravity)
result = self.set_physics(set_physics_request)
rospy.loginfo("Gravity Update Result=="+str(result.success)+",message=="+str(result.status_message))
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
print "Unpause"
except rospy.ServiceException, e:
print ("/gazebo/unpause_physics service call failed")
def __init__(self):
# Create the observation space
self.observation_space = spaces.Box(low=0,
high=255,
shape=(TRAINING_IMAGE_SIZE[1],
TRAINING_IMAGE_SIZE[0], 3),
dtype=np.uint8)
# Create the action space
self.action_space = spaces.Box(low=np.array([-1, 0]),
high=np.array([+1, +1]),
dtype=np.float32)
if node_type == SIMULATION_WORKER:
# ROS initialization
rospy.init_node('rl_coach', anonymous=True)
# wait for required services
rospy.wait_for_service(
'/deepracer_simulation_environment/get_waypoints')
rospy.wait_for_service(
'/deepracer_simulation_environment/reset_car')
rospy.wait_for_service('/gazebo/get_model_state')
rospy.wait_for_service('/gazebo/get_link_state')
rospy.wait_for_service('/gazebo/clear_joint_forces')
rospy.wait_for_service('/gazebo/get_physics_properties')
rospy.wait_for_service('/gazebo/set_physics_properties')
self.get_model_state = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state',
GetLinkState)
self.clear_forces_client = rospy.ServiceProxy(
'/gazebo/clear_joint_forces', JointRequest)
self.reset_car_client = rospy.ServiceProxy(
'/deepracer_simulation_environment/reset_car', ResetCarSrv)
get_waypoints_client = rospy.ServiceProxy(
'/deepracer_simulation_environment/get_waypoints',
GetWaypointSrv)
get_physics_properties = rospy.ServiceProxy(
'/gazebo/get_physics_properties', GetPhysicsProperties)
set_physics_properties = rospy.ServiceProxy(
'/gazebo/set_physics_properties', SetPhysicsProperties)
# Create the publishers for sending speed and steering info to the car
self.velocity_pub_dict = OrderedDict()
self.steering_pub_dict = OrderedDict()
for topic in VELOCITY_TOPICS:
self.velocity_pub_dict[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
for topic in STEERING_TOPICS:
self.steering_pub_dict[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
# Read in parameters
self.world_name = rospy.get_param('WORLD_NAME')
self.job_type = rospy.get_param('JOB_TYPE')
self.aws_region = rospy.get_param('AWS_REGION')
self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET')
self.metrics_s3_object_key = rospy.get_param(
'METRICS_S3_OBJECT_KEY')
self.aws_endpoint_url = rospy.get_param('AWS_ENDPOINT_URL')
self.metrics = []
self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \
rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \
':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID')
self.max_update_rate = rospy.get_param('SIM_UPDATE_RATE')
# Set the simulation rate
try:
resp = utils.gazebo_service_call(get_physics_properties)
# float64 time_step
# bool pause
# float64 max_update_rate
# geometry_msgs/Vector3 gravity
# float64 x
# float64 y
# float64 z
# gazebo_msgs/ODEPhysics ode_config
# bool auto_disable_bodies
# uint32 sor_pgs_precon_iters
# uint32 sor_pgs_iters
# float64 sor_pgs_w
# float64 sor_pgs_rms_error_tol
# float64 contact_surface_layer
# float64 contact_max_correcting_vel
# float64 cfm
# float64 erp
# uint32 max_contacts
# bool success
# string status_message
update_msg = SetPhysicsPropertiesRequest()
update_msg.time_step = resp.time_step
update_msg.max_update_rate = self.max_update_rate
update_msg.gravity = resp.gravity
update_msg.ode_config = resp.ode_config
# float64 time_step
# float64 max_update_rate
# geometry_msgs/Vector3 gravity
# float64 x
# float64 y
# float64 z
# gazebo_msgs/ODEPhysics ode_config
# bool auto_disable_bodies
# uint32 sor_pgs_precon_iters
# uint32 sor_pgs_iters
# float64 sor_pgs_w
# float64 sor_pgs_rms_error_tol
# float64 contact_surface_layer
# float64 contact_max_correcting_vel
# float64 cfm
# float64 erp
# uint32 max_contacts
# ---
# bool success
resp = utils.gazebo_service_call(set_physics_properties,
update_msg)
except Exception as ex:
utils.json_format_logger(
"Unable to set physics update rate: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
# Setup SIM_TRACE_LOG data upload to s3
self.setup_simtrace_data_upload_to_s3()
if self.job_type == TRAINING_JOB:
from custom_files.customer_reward_function import reward_function
self.reward_function = reward_function
self.target_number_of_episodes = rospy.get_param(
'NUMBER_OF_EPISODES')
self.target_reward_score = rospy.get_param(
'TARGET_REWARD_SCORE')
else:
from markov.defaults import reward_function
self.reward_function = reward_function
self.number_of_trials = 0
self.target_number_of_trials = rospy.get_param(
'NUMBER_OF_TRIALS')
# Request the waypoints
waypoints = None
try:
resp = utils.gazebo_service_call(get_waypoints_client)
waypoints = np.array(resp.waypoints).reshape(
resp.row, resp.col)
except Exception as ex:
utils.json_format_logger(
"Unable to retrieve waypoints: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
is_loop = np.all(waypoints[0, :] == waypoints[-1, :])
if is_loop:
self.center_line = LinearRing(waypoints[:, 0:2])
self.inner_border = LinearRing(waypoints[:, 2:4])
self.outer_border = LinearRing(waypoints[:, 4:6])
self.road_poly = Polygon(self.outer_border,
[self.inner_border])
else:
self.center_line = LineString(waypoints[:, 0:2])
self.inner_border = LineString(waypoints[:, 2:4])
self.outer_border = LineString(waypoints[:, 4:6])
self.road_poly = Polygon(
np.vstack(
(self.outer_border, np.flipud(self.inner_border))))
self.center_dists = [
self.center_line.project(Point(p), normalized=True)
for p in self.center_line.coords[:-1]
] + [1.0]
self.track_length = self.center_line.length
# Queue used to maintain image consumption synchronicity
self.image_queue = queue.Queue(IMG_QUEUE_BUF_SIZE)
rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image,
self.callback_image)
# Initialize state data
self.episodes = 0
self.start_ndist = rospy.get_param('START_POSITION', 0.0)
self.reverse_dir = False
self.change_start = rospy.get_param(
'CHANGE_START_POSITION', (self.job_type == TRAINING_JOB))
self.alternate_dir = rospy.get_param('ALTERNATE_DRIVING_DIRECTION',
False)
self.is_simulation_done = False
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.prev_progress = 0
self.prev_point = Point(0, 0)
self.prev_point_2 = Point(0, 0)
self.next_state = None
self.reward = None
self.reward_in_episode = 0
self.done = False
self.steps = 0
self.simulation_start_time = 0
self.allow_servo_step_signals = False
self._ode_config.contact_max_correcting_vel = 0.0
self._ode_config.cfm = 0.0
self._ode_config.erp = 0.2
self._ode_config.max_contacts = 20
self.update_gravity_call()
def update_gravity_call(self):
rospy.wait_for_service('/gazebo/pause_physics')
try:
self.pause()
except rospy.ServiceException, e:
print ("/gazebo/pause_physics service call failed")
set_physics_request = SetPhysicsPropertiesRequest()
set_physics_request.time_step = self._time_step.data
set_physics_request.max_update_rate = self._max_update_rate.data
set_physics_request.gravity = self._gravity
set_physics_request.ode_config = self._ode_config
print str(set_physics_request.gravity)
result = self.set_physics(set_physics_request)
rospy.loginfo("Gravity Update Result==" + str(result.success) + ",message==" + str(result.status_message))
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except rospy.ServiceException, e:
print ("/gazebo/unpause_physics service call failed")
