def __init__(self,
world_file=None,
world_string=None,
world_config=None,
port=None,
quiet=False):
"""
Args:
world_file (str|None): world file path
world_string (str|None): world xml string content,
world_config (list[str]): list of str config `key=value`
see `_xpath_modify_xml` for details
port (int): Gazebo port
quiet (bool) Set quiet output
"""
if port is None:
port = 0
self._port = port
# to avoid different parallel simulation has the same randomness
random.seed(port)
self._rendering_process = None
self._rendering_camera = None
# the default camera pose for rendering rgb_array, could be override
self._rendering_cam_pose = "10 -10 6 0 0.4 2.4"
gazebo.initialize(port=port, quiet=quiet)
if world_file:
world_file_abs_path = os.path.join(social_bot.get_world_dir(),
world_file)
world_string = gazebo.world_sdf(world_file_abs_path)
if world_config:
world_string = _xpath_modify_xml(world_string, world_config)
self._world = gazebo.new_world_from_string(world_string)
def __init__(self, port=None, quiet=False):
if port is None:
port = 0
self._port = port
# to avoid different parallel simulation has the same randomness
random.seed(port)
self._rendering_process = None
self._world = None
self._rendering_camera = None
# the default camera pose for rendering rgb_array, could be override
self._rendering_cam_pose = "10 -10 6 0 0.4 2.4"
gazebo.initialize(port=port, quiet=quiet)
def __init__(self, with_language=True, port=None):
if port is None:
port = 0
gazebo.initialize(port=port)
self._world = gazebo.new_world_from_file(
os.path.join(social_bot.get_world_dir(),
"pioneer2dx_camera.world"))
self._agent = self._world.get_agent()
assert self._agent is not None
logger.info("joint names: %s" % self._agent.get_joint_names())
self._joint_names = self._agent.get_joint_names()
self._teacher = teacher.Teacher(False)
task_group = teacher.TaskGroup()
task_group.add_task(GoalTask())
self._teacher.add_task_group(task_group)
self._with_language = with_language
# get observation dimension
image = self._agent.get_camera_observation("camera")
image = np.array(image, copy=False)
if with_language:
self._observation_space = gym.spaces.Dict(
image=gym.spaces.Box(low=0,
high=1,
shape=image.shape,
dtype=np.uint8),
sentence=DiscreteSequence(256, 20))
self._action_space = gym.spaces.Dict(
control=gym.spaces.Box(low=-0.2,
high=0.2,
shape=[len(self._joint_names)],
dtype=np.float32),
sentence=DiscreteSequence(256, 20))
else:
self._observation_space = gym.spaces.Box(low=0,
high=1,
shape=image.shape,
dtype=np.uint8)
self._action_space = gym.spaces.Box(low=-0.2,
high=0.2,
shape=[len(self._joint_names)],
dtype=np.float32)
def __init__(self, x_threshold=5, theta_threshold=0.314, noise=0.01, port=None):
if port is None:
port = 0
gazebo.initialize(port=port)
self._world = gazebo.new_world_from_file(
os.path.join(social_bot.get_world_dir(), "cartpole.world"))
self._agent = self._world.get_agent()
logger.info("joint names: %s" % self._agent.get_joint_names())
self._x_threshold = x_threshold
self._theta_threshold = theta_threshold
high = np.array([
self._x_threshold * 2,
np.finfo(np.float32).max, self._theta_threshold * 2,
np.finfo(np.float32).max
])
self.noise = noise
self.action_space = spaces.Box(-1, 1, shape=(1, ), dtype='float32')
self.observation_space = spaces.Box(-high, high, dtype=np.float32)
self._world.info()
def __init__(self,
world_file=None,
world_string=None,
world_config=None,
sim_time_precision=0.001,
port=None,
quiet=False):
"""
Args:
world_file (str|None): world file path
world_string (str|None): world xml string content,
world_config (list[str]): list of str config `key=value`
see `_modify_world_xml` for details
sim_time_precision (float): the time precision of the simulator
port (int): Gazebo port
quiet (bool) Set quiet output
"""
os.environ["GAZEBO_MODEL_DATABASE_URI"] = ""
if port is None:
port = 0
self._port = port
# This avoids different parallel simulations having the same randomness.
# When calling from alf, alf.environments.utils.create_environment calls
# env.seed() afterwards, which is the real seed being used. Not this one.
random.seed(port)
self._rendering_process = None
self._rendering_camera = None
# the default camera pose for rendering rgb_array, could be override
self._rendering_cam_pose = "10 -10 6 0 0.4 2.4"
gazebo.initialize(port=port, quiet=quiet)
if world_file:
world_file_abs_path = os.path.join(social_bot.get_world_dir(),
world_file)
world_string = gazebo.world_sdf(world_file_abs_path)
if world_config:
world_string = _modify_world_xml(world_string, world_config)
self._world = gazebo.new_world_from_string(world_string)
def __init__(self,
goal_name='beer',
max_steps=100,
reward_shaping=True,
motion_loss=0.0000,
use_internal_states_only=True,
port=None):
"""
Args:
goal_name (string): name of the object to lift off ground
max_steps (int): episode will end when the agent exceeds the number of steps.
reward_shaping (boolean): whether it adds distance based reward shaping.
motion_loss (float): if not zero, it will add -motion_loss * || V_{all_joints} ||^2 in
the reward when episode ends.
use_internal_states_only (boolean): whether to only use internal states (joint positions
and velocities) and goal positions, and not use camera sensors.
port: Gazebo port, need to specify when run multiple environment in parallel
"""
if port is None:
port = 0
gazebo.initialize(port=port)
self._world = gazebo.new_world_from_file(
os.path.join(social_bot.get_world_dir(), "pr2.world"))
self._agent = self._world.get_agent()
#logger.info("joint names: %s" % self._agent.get_joint_names())
self._all_joints = self._agent.get_joint_names()
# to avoid different parallel simulation has the same randomness
random.seed(port)
#self._world.info()
# passive joints are joints that could move but have no actuators, are only indirectly
# controled by the motion of active joints.
# Though in the simulation, we could "control" through API, we chose to be more realistic.
# from https://github.com/PR2/pr2_mechanism/blob/melodic-devel/pr2_mechanism_model/pr2.urdf
passive_joints = set(["r_gripper_l_finger_joint",
"r_gripper_r_finger_joint",
"r_gripper_r_finger_tip_joint",
"r_gripper_l_finger_tip_joint",
"r_gripper_r_parallel_root_joint",
"r_gripper_l_parallel_root_joint"])
# PR2 right arm has 7 DOF, gripper has 1 DoF, as specified on p18/p26 on PR2 manual
# https://www.clearpathrobotics.com/wp-content/uploads/2014/08/pr2_manual_r321.pdf
# we exclude rest of the joints
unused_joints = set([
"r_gripper_motor_slider_joint",
"r_gripper_motor_screw_joint",
"r_gripper_r_screw_screw_joint",
"r_gripper_l_screw_screw_joint",
"r_gripper_r_parallel_tip_joint",
"r_gripper_l_parallel_tip_joint"])
unused_joints = passive_joints.union(unused_joints)
self._r_arm_joints = list(
filter(lambda s: s.find('pr2::r_') != -1
and s.split("::")[-1] not in unused_joints, self._all_joints))
logger.info("joints in the right arm to control: %s" % self._r_arm_joints)
joint_states = list(map(lambda s: self._agent.get_joint_state(s), self._r_arm_joints))
self._r_arm_joints_limits = list(map(lambda s: s.get_effort_limits()[0],
joint_states));
logger.info('\n'.join(map(lambda s: str(s[0]) + ":" + str(s[1]), zip(
self._r_arm_joints, self._r_arm_joints_limits))))
self._goal_name = goal_name
self._goal = self._world.get_agent(self._goal_name)
self._max_steps = max_steps
self._steps_in_this_episode = 0
self._reward_shaping = reward_shaping
self._resized_image_size = (128, 128) #(84, 84)
self._use_internal_states_only = use_internal_states_only
self._cum_reward = 0.0
self._motion_loss = motion_loss
# a hack to work around gripper not open initially, might not need now.
self._gripper_reward_dir = 1
self._gripper_upper_limit = 0.07
self._gripper_lower_limit = 0.01
# whether to move head cameras and gripper when episode starts.
self._adjust_position_at_start = False
obs = self._get_observation()
self._prev_dist = self._get_finger_tip_distance()
self._prev_gripper_pos = self._get_gripper_pos()
if self._use_internal_states_only:
self.observation_space = gym.spaces.Box(
low=-np.inf, high=np.inf, shape=obs.shape, dtype=np.float32)
else:
self.observation_space = gym.spaces.Box(
low=0, high=256.0, shape=obs.shape, dtype=np.float32)
self.action_space = gym.spaces.Box(
low=-1, high=1, shape=[len(self._r_arm_joints)], dtype=np.float32)
self._world.reset()
joint_state = gazebo.JointState(1)
joint_state.set_positions([self.noise * random.random()])
joint_state.set_velocities([0.0])
self._agent.set_joint_state("cartpole::cartpole::cart_to_pole",
joint_state)
return self._get_state()
def main():
env = CartPole()
for _ in range(100):
print("reset")
env.reset()
env._world.info()
total_rewards = 0
while True:
obs, reward, done, info = env.step((random.random() - 0.5) * 0.5)
total_rewards += reward
if done:
print("total reward " + str(total_rewards))
break
if __name__ == "__main__":
logging.basicConfig(level=logging.DEBUG)
gazebo.initialize()
main()
# Copyright (c) 2019 Horizon Robotics. All Rights Reserved.
import random
import social_bot.pygazebo as bot
import numpy as np
import matplotlib.pyplot as plt
import time
import os
enable_camera = True
show_image = enable_camera & False
bot.initialize()
if enable_camera:
world = bot.new_world_from_file(
"../python/social_bot/worlds/turtlebot_camera.world")
else:
world = bot.new_world_from_file(
"../python/social_bot/worlds/turtlebot.world")
world.info()
agent = world.get_agent()
print(agent.get_joint_names())
steps = 0
t0 = time.time()
interval = 100
fig = None
for i in range(10000000):
steps += 1
def __init__(self,
with_language=False,
use_image_obs=False,
agent_type='pioneer2dx_noplugin',
goal_name='table',
max_steps=160,
port=None):
"""
Args:
with_language (bool): the observation will be a dict with an extra sentence
use_image_obs (bool): use image, or use internal states as observation
poses in internal states observation are in world coordinate
agent_type (string): select the agent robot, supporting pr2_differential,
pioneer2dx_noplugin, turtlebot, and irobot create for now
port: Gazebo port, need to specify when run multiple environment in parallel
"""
if port is None:
port = 0
gazebo.initialize(port=port)
self._world = gazebo.new_world_from_file(
os.path.join(social_bot.get_world_dir(), "grocery_ground.world"))
self._object_types = [
'coke_can', 'cube_20k', 'car_wheel', 'plastic_cup', 'beer',
'hammer'
]
self._pos_list = list(itertools.product(range(-5, 5), range(-5, 5)))
self._pos_list.remove((0, 0))
self._world.info()
self._world.insertModelFile('model://' + agent_type)
self._world.step(20)
time.sleep(0.1) # Avoid 'px!=0' error
self._random_insert_objects()
self._world.model_list_info()
self._world.info()
# Specify joints and sensors for the robots
control_joints = {
'pr2_differential': [
'pr2_differential::fl_caster_r_wheel_joint',
'pr2_differential::fr_caster_l_wheel_joint'
],
'pioneer2dx_noplugin': [
'pioneer2dx_noplugin::left_wheel_hinge',
'pioneer2dx_noplugin::right_wheel_hinge'
],
'turtlebot': [
'turtlebot::create::left_wheel',
'turtlebot::create::right_wheel'
],
'create': ['create::left_wheel', 'create::right_wheel'],
}
control_force = {
'pr2_differential': 20,
'pioneer2dx_noplugin': 0.5,
'turtlebot': 0.5,
'create': 0.5,
}
camera_sensor = {
'pr2_differential':
'default::pr2_differential::head_tilt_link::head_mount_prosilica_link_sensor',
'pioneer2dx_noplugin': ' ',
'turtlebot': 'default::turtlebot::kinect::link::camera',
'create': ' ',
}
self._agent = self._world.get_agent(agent_type)
self._agent_joints = control_joints[agent_type]
self._agent_control_force = control_force[agent_type]
self._agent_camera = camera_sensor[agent_type]
self._goal_name = goal_name
self._goal = self._world.get_model(goal_name)
logger.info("joints to control: %s" % self._agent_joints)
self._teacher = teacher.Teacher(False)
task_group = teacher.TaskGroup()
self._teacher_task = GroceryGroundGoalTask(max_steps=max_steps,
goal_name=self._goal_name,
success_distance_thresh=0.5,
fail_distance_thresh=3.0,
reward_shaping=True,
random_range=10.0)
task_group.add_task(self._teacher_task)
self._teacher.add_task_group(task_group)
self._with_language = with_language
self._use_image_obs = use_image_obs
obs = self.reset()
if self._use_image_obs:
obs_data_space = gym.spaces.Box(low=0,
high=255,
shape=obs.shape,
dtype=np.uint8)
else:
obs_data_space = gym.spaces.Box(low=-50,
high=50,
shape=obs.shape,
dtype=np.float32)
control_space = gym.spaces.Box(low=-self._agent_control_force,
high=self._agent_control_force,
shape=[len(self._agent_joints)],
dtype=np.float32)
if self._with_language:
self.observation_space = gym.spaces.Dict(data=obs_data_space,
sentence=DiscreteSequence(
128, 24))
self._action_space = gym.spaces.Dict(control=control_space,
sentence=DiscreteSequence(
128, 24))
else:
self.observation_space = obs_data_space
self.action_space = control_space