def reset(self, bullet_client):
self._p = bullet_client
#print("Created bullet_client with id=", self._p._client)
if (self.doneLoading == 0):
self.ordered_joints = []
self.doneLoading = 1
if self.self_collision:
self.objects = self._p.loadMJCF(
os.path.join(pybullet_data_local.getDataPath(), "mjcf",
self.model_xml),
flags=pybullet.URDF_USE_SELF_COLLISION
| pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
self._p, self.objects)
else:
self.objects = self._p.loadMJCF(
os.path.join(pybullet_data_local.getDataPath(), "mjcf",
self.model_xml))
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
self._p, self.objects)
self.robot_specific_reset(self._p)
s = self.calc_state(
) # optimization: calc_state() can calculate something in self.* for calc_potential() to use
return s
def reset(self, bullet_client):
self._p = bullet_client
self.ordered_joints = []
print(os.path.join(os.path.dirname(__file__), "data", self.model_urdf))
if self.self_collision:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
self._p,
self._p.loadURDF(os.path.join(
pybullet_data_local.getDataPath(), self.model_urdf),
basePosition=self.basePosition,
baseOrientation=self.baseOrientation,
useFixedBase=self.fixed_base,
flags=pybullet.URDF_USE_SELF_COLLISION))
else:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
self._p,
self._p.loadURDF(os.path.join(
pybullet_data_local.getDataPath(), self.model_urdf),
basePosition=self.basePosition,
baseOrientation=self.baseOrientation,
useFixedBase=self.fixed_base))
self.robot_specific_reset(self._p)
s = self.calc_state(
) # optimization: calc_state() can calculate something in self.* for calc_potential() to use
self.potential = self.calc_potential()
return s
def __init__(self,
urdf_root=pybullet_data_local.getDataPath(),
action_repeat=1,
observation_noise_stdev=minitaur_gym_env.SENSOR_NOISE_STDDEV,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False,
render=False):
"""Initialize the minitaur standing up gym environment.
Args:
urdf_root: The path to the urdf data folder.
action_repeat: The number of simulation steps before actions are applied.
observation_noise_stdev: The standard deviation of observation noise.
self_collision_enabled: Whether to enable self collision in the sim.
motor_velocity_limit: The velocity limit of each motor.
pd_control_enabled: Whether to use PD controller for each motor.
render: Whether to render the simulation.
"""
super(MinitaurStandGymEnv, self).__init__(urdf_root=urdf_root,
action_repeat=action_repeat,
observation_noise_stdev=observation_noise_stdev,
self_collision_enabled=self_collision_enabled,
motor_velocity_limit=motor_velocity_limit,
pd_control_enabled=pd_control_enabled,
accurate_motor_model_enabled=True,
motor_overheat_protection=True,
render=render)
# Set the action dimension to 1, and reset the action space.
action_dim = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
def __init__(self,
urdf_root=pybullet_data_local.getDataPath(),
self_collision_enabled=True,
pd_control_enabled=False,
leg_model_enabled=True,
on_rack=False,
render=False):
"""Initialize the minitaur and ball gym environment.
Args:
urdf_root: The path to the urdf data folder.
self_collision_enabled: Whether to enable self collision in the sim.
pd_control_enabled: Whether to use PD controller for each motor.
leg_model_enabled: Whether to use a leg motor to reparameterize the action
space.
on_rack: Whether to place the minitaur on rack. This is only used to debug
the walking gait. In this mode, the minitaur's base is hanged midair so
that its walking gait is clearer to visualize.
render: Whether to render the simulation.
"""
super(MinitaurBallGymEnv, self).__init__(urdf_root=urdf_root,
self_collision_enabled=self_collision_enabled,
pd_control_enabled=pd_control_enabled,
leg_model_enabled=leg_model_enabled,
on_rack=on_rack,
render=render)
self._cam_dist = 2.0
self._cam_yaw = -70
self._cam_pitch = -30
self.action_space = spaces.Box(np.array([-1]), np.array([1]))
self.observation_space = spaces.Box(np.array([-math.pi, 0]), np.array([math.pi, 100]))
def get_cube(_p, x, y, z):
body = _p.loadURDF(os.path.join(pybullet_data_local.getDataPath(), "cube_small.urdf"), [x, y, z])
_p.changeDynamics(body, -1, mass=1.2) #match Roboschool
part_name, _ = _p.getBodyInfo(body)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(_p, part_name, bodies, 0, -1)
def build_world(args, enable_draw):
arg_parser = build_arg_parser(args)
print("enable_draw=", enable_draw)
env = PyBulletDeepMimicEnv(arg_parser, enable_draw)
world = RLWorld(env, arg_parser)
#world.env.set_playback_speed(playback_speed)
motion_file = arg_parser.parse_string("motion_file")
print("motion_file=", motion_file)
bodies = arg_parser.parse_ints("fall_contact_bodies")
print("bodies=", bodies)
int_output_path = arg_parser.parse_string("int_output_path")
print("int_output_path=", int_output_path)
agent_files = pybullet_data_local.getDataPath(
) + "/" + arg_parser.parse_string("agent_files")
AGENT_TYPE_KEY = "AgentType"
print("agent_file=", agent_files)
with open(agent_files) as data_file:
json_data = json.load(data_file)
print("json_data=", json_data)
assert AGENT_TYPE_KEY in json_data
agent_type = json_data[AGENT_TYPE_KEY]
print("agent_type=", agent_type)
agent = PPOAgent(world, id, json_data)
agent.set_enable_training(False)
world.reset()
return world
def __init__(self,
urdfRootPath=pybullet_data_local.getDataPath(),
timeStep=0.01):
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self.maxVelocity = .35
self.maxForce = 200.
self.fingerAForce = 2
self.fingerBForce = 2.5
self.fingerTipForce = 2
self.useInverseKinematics = 1 # default to yes
self.useSimulation = 1
self.useNullSpace = 21
self.useOrientation = 1
self.kukaEndEffectorIndex = 6
self.kukaGripperIndex = 7
#lower limits for null space - the space where the robot can move without moving the end effector
self.ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
self.ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space
self.jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
self.rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
self.jd = [
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001
]
self.reset()
def reset(self):
# print("-----------reset simulation---------------")
if self._physics_client_id < 0:
if self._renders:
self._p = bc.BulletClient(connection_mode=p2.GUI)
else:
self._p = bc.BulletClient()
self._physics_client_id = self._p._client
p = self._p
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data_local.getDataPath(), "cartpole.urdf"),
[0, 0, 0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0, 0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
p = self._p
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
def episode_restart(self, bullet_client):
self._p = bullet_client
Scene.episode_restart(
self, bullet_client) # contains cpp_world.clean_everything()
if (self.stadiumLoaded == 0):
self.stadiumLoaded = 1
# stadium_pose = cpp_household.Pose()
# if self.zero_at_running_strip_start_line:
# stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants
filename = os.path.join(pybullet_data_local.getDataPath(),
"plane_stadium.sdf")
self.ground_plane_mjcf = self._p.loadSDF(filename)
#filename = os.path.join(pybullet_data_local.getDataPath(),"stadium_no_collision.sdf")
#self.ground_plane_mjcf = self._p.loadSDF(filename)
#
for i in self.ground_plane_mjcf:
self._p.changeDynamics(i,
-1,
lateralFriction=0.8,
restitution=0.5)
self._p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.8])
self._p.configureDebugVisualizer(
pybullet.COV_ENABLE_PLANAR_REFLECTION, i)
def test(args):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data_local.getDataPath())
fileName = os.path.join("mjcf", args.mjcf)
print("fileName")
print(fileName)
p.loadMJCF(fileName)
while (1):
p.stepSimulation()
p.getCameraImage(320, 240)
time.sleep(0.01)
def build_arg_parser(args):
arg_parser = ArgParser()
arg_parser.load_args(args)
arg_file = arg_parser.parse_string('arg_file', '')
if (arg_file != ''):
path = pybullet_data_local.getDataPath() + "/args/" + arg_file
succ = arg_parser.load_file(path)
Logger.print2(arg_file)
assert succ, Logger.print2('Failed to load args from: ' + arg_file)
return arg_parser
def build_agent(world, id, file):
agent = None
with open(pybullet_data_local.getDataPath() + "/" + file) as data_file:
json_data = json.load(data_file)
assert AGENT_TYPE_KEY in json_data
agent_type = json_data[AGENT_TYPE_KEY]
if (agent_type == PPOAgent.NAME):
agent = PPOAgent(world, id, json_data)
else:
assert False, 'Unsupported agent type: ' + agent_type
return agent
def __init__(self,
urdfRoot=pybullet_data_local.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=False,
isDiscrete=False,
maxSteps=1000):
#print("KukaGymEnv __init__")
self._isDiscrete = isDiscrete
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
self.seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([largeValObservation] * observationDim)
if (self._isDiscrete):
self.action_space = spaces.Discrete(7)
else:
action_dim = 3
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
def __init__(self, pybullet_client, motion_data, baseShift):
"""Constructs a humanoid and reset it to the initial states.
Args:
pybullet_client: The instance of BulletClient to manage different
simulations.
"""
self._baseShift = baseShift
self._pybullet_client = pybullet_client
self.kin_client = BulletClient(
pybullet_client.DIRECT
) # use SHARED_MEMORY for visual debugging, start a GUI physics server first
self.kin_client.resetSimulation()
self.kin_client.setAdditionalSearchPath(pybullet_data_local.getDataPath())
self.kin_client.configureDebugVisualizer(self.kin_client.COV_ENABLE_Y_AXIS_UP, 1)
self.kin_client.setGravity(0, -9.8, 0)
self._motion_data = motion_data
print("LOADING humanoid!")
self._humanoid = self._pybullet_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0],
globalScaling=0.25,
useFixedBase=False)
self._kinematicHumanoid = self.kin_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0],
globalScaling=0.25,
useFixedBase=False)
#print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid))
pose = HumanoidPose()
for i in range(self._motion_data.NumFrames() - 1):
frameData = self._motion_data._motion_data['Frames'][i]
pose.PostProcessMotionData(frameData)
self._pybullet_client.resetBasePositionAndOrientation(self._humanoid, self._baseShift,
[0, 0, 0, 1])
self._pybullet_client.changeDynamics(self._humanoid, -1, linearDamping=0, angularDamping=0)
for j in range(self._pybullet_client.getNumJoints(self._humanoid)):
ji = self._pybullet_client.getJointInfo(self._humanoid, j)
self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0)
self._pybullet_client.changeVisualShape(self._humanoid, j, rgbaColor=[1, 1, 1, 1])
#print("joint[",j,"].type=",jointTypes[ji[2]])
#print("joint[",j,"].name=",ji[1])
self._initial_state = self._pybullet_client.saveState()
self._allowed_body_parts = [11, 14]
self.Reset()
def __init__(self,
urdfRoot=pybullet_data_local.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=False,
isDiscrete=False):
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._width = 341
self._height = 256
self._isDiscrete = isDiscrete
self.terminated = 0
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
self.seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
if (self._isDiscrete):
self.action_space = spaces.Discrete(7)
else:
action_dim = 3
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self._height, self._width, 4),
dtype=np.uint8)
self.viewer = None
def __init__(self,
pybullet_client,
urdf_root=pybullet_data_local.getDataPath(),
time_step=0.01):
"""Constructs an example simulation and reset it to the initial states.
Args:
pybullet_client: The instance of BulletClient to manage different
simulations.
urdf_root: The path to the urdf folder.
time_step: The time step of the simulation.
"""
self._pybullet_client = pybullet_client
self._urdf_root = urdf_root
self.m_actions_taken_since_reset = 0
self.time_step = time_step
self.stateId = -1
self.Reset(reload_urdf=True)
def __init__(self,
urdfRoot=pybullet_data_local.getDataPath(),
actionRepeat=10,
isEnableSelfCollision=True,
isDiscrete=False,
renders=True):
print("init")
self._timeStep = 0.01
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._ballUniqueId = -1
self._envStepCounter = 0
self._renders = renders
self._width = 100
self._height = 10
self._isDiscrete = isDiscrete
if self._renders:
self._p = bc.BulletClient(connection_mode=pybullet.GUI)
else:
self._p = bc.BulletClient()
self.seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
if (isDiscrete):
self.action_space = spaces.Discrete(9)
else:
action_dim = 2
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self._height, self._width, 4),
dtype=np.uint8)
self.viewer = None
def build_agents(self):
num_agents = self.env.get_num_agents()
print("num_agents=", num_agents)
self.agents = []
Logger.print2('')
Logger.print2('Num Agents: {:d}'.format(num_agents))
agent_files = self.arg_parser.parse_strings('agent_files')
print("len(agent_files)=", len(agent_files))
assert (len(agent_files) == num_agents or len(agent_files) == 0)
model_files = self.arg_parser.parse_strings('model_files')
assert (len(model_files) == num_agents or len(model_files) == 0)
output_path = self.arg_parser.parse_string('output_path')
int_output_path = self.arg_parser.parse_string('int_output_path')
for i in range(num_agents):
curr_file = agent_files[i]
curr_agent = self._build_agent(i, curr_file)
if curr_agent is not None:
curr_agent.output_dir = output_path
curr_agent.int_output_dir = int_output_path
Logger.print2(str(curr_agent))
if (len(model_files) > 0):
curr_model_file = model_files[i]
if curr_model_file != 'none':
curr_agent.load_model(pybullet_data_local.getDataPath() + "/" + curr_model_file)
self.agents.append(curr_agent)
Logger.print2('')
self.set_enable_training(self.enable_training)
return
def __init__(
self,
urdf_root=pybullet_data_local.getDataPath(),
action_repeat=1,
distance_weight=1.0,
energy_weight=0.005,
shake_weight=0.0,
drift_weight=0.0,
distance_limit=float("inf"),
observation_noise_stdev=0.0,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False, #not needed to be true if accurate motor model is enabled (has its own better PD)
leg_model_enabled=True,
accurate_motor_model_enabled=True,
motor_kp=1.0,
motor_kd=0.02,
torque_control_enabled=False,
motor_overheat_protection=True,
hard_reset=True,
on_rack=False,
render=False,
kd_for_pd_controllers=0.3,
env_randomizer=minitaur_env_randomizer.MinitaurEnvRandomizer()):
"""Initialize the minitaur gym environment.
Args:
urdf_root: The path to the urdf data folder.
action_repeat: The number of simulation steps before actions are applied.
distance_weight: The weight of the distance term in the reward.
energy_weight: The weight of the energy term in the reward.
shake_weight: The weight of the vertical shakiness term in the reward.
drift_weight: The weight of the sideways drift term in the reward.
distance_limit: The maximum distance to terminate the episode.
observation_noise_stdev: The standard deviation of observation noise.
self_collision_enabled: Whether to enable self collision in the sim.
motor_velocity_limit: The velocity limit of each motor.
pd_control_enabled: Whether to use PD controller for each motor.
leg_model_enabled: Whether to use a leg motor to reparameterize the action
space.
accurate_motor_model_enabled: Whether to use the accurate DC motor model.
motor_kp: proportional gain for the accurate motor model.
motor_kd: derivative gain for the accurate motor model.
torque_control_enabled: Whether to use the torque control, if set to
False, pose control will be used.
motor_overheat_protection: Whether to shutdown the motor that has exerted
large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
(OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more
details.
hard_reset: Whether to wipe the simulation and load everything when reset
is called. If set to false, reset just place the minitaur back to start
position and set its pose to initial configuration.
on_rack: Whether to place the minitaur on rack. This is only used to debug
the walking gait. In this mode, the minitaur's base is hanged midair so
that its walking gait is clearer to visualize.
render: Whether to render the simulation.
kd_for_pd_controllers: kd value for the pd controllers of the motors
env_randomizer: An EnvRandomizer to randomize the physical properties
during reset().
"""
self._time_step = 0.01
self._action_repeat = action_repeat
self._num_bullet_solver_iterations = 300
self._urdf_root = urdf_root
self._self_collision_enabled = self_collision_enabled
self._motor_velocity_limit = motor_velocity_limit
self._observation = []
self._env_step_counter = 0
self._is_render = render
self._last_base_position = [0, 0, 0]
self._distance_weight = distance_weight
self._energy_weight = energy_weight
self._drift_weight = drift_weight
self._shake_weight = shake_weight
self._distance_limit = distance_limit
self._observation_noise_stdev = observation_noise_stdev
self._action_bound = 1
self._pd_control_enabled = pd_control_enabled
self._leg_model_enabled = leg_model_enabled
self._accurate_motor_model_enabled = accurate_motor_model_enabled
self._motor_kp = motor_kp
self._motor_kd = motor_kd
self._torque_control_enabled = torque_control_enabled
self._motor_overheat_protection = motor_overheat_protection
self._on_rack = on_rack
self._cam_dist = 1.0
self._cam_yaw = 0
self._cam_pitch = -30
self._hard_reset = True
self._kd_for_pd_controllers = kd_for_pd_controllers
self._last_frame_time = 0.0
print("urdf_root=" + self._urdf_root)
self._env_randomizer = env_randomizer
# PD control needs smaller time step for stability.
if pd_control_enabled or accurate_motor_model_enabled:
self._time_step /= NUM_SUBSTEPS
self._num_bullet_solver_iterations /= NUM_SUBSTEPS
self._action_repeat *= NUM_SUBSTEPS
if self._is_render:
self._pybullet_client = bc.BulletClient(connection_mode=pybullet.GUI)
else:
self._pybullet_client = bc.BulletClient()
self.seed()
self.reset()
observation_high = (self.minitaur.GetObservationUpperBound() + OBSERVATION_EPS)
observation_low = (self.minitaur.GetObservationLowerBound() - OBSERVATION_EPS)
action_dim = 8
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = spaces.Box(observation_low, observation_high, dtype=np.float32)
self.viewer = None
self._hard_reset = hard_reset # This assignment need to be after reset()
def __init__(self,
pybullet_sim_factory=boxstack_pybullet_sim,
render=True,
render_sleep=False,
debug_visualization=True,
hard_reset=False,
render_width=240,
render_height=240,
action_repeat=1,
time_step=1. / 240.,
num_bullet_solver_iterations=50,
urdf_root=pybullet_data_local.getDataPath()):
"""Initialize the gym environment.
Args:
urdf_root: The path to the urdf data folder.
"""
self._pybullet_sim_factory = pybullet_sim_factory
self._time_step = time_step
self._urdf_root = urdf_root
self._observation = []
self._action_repeat = action_repeat
self._num_bullet_solver_iterations = num_bullet_solver_iterations
self._env_step_counter = 0
self._is_render = render
self._debug_visualization = debug_visualization
self._render_sleep = render_sleep
self._render_width = render_width
self._render_height = render_height
self._cam_dist = .3
self._cam_yaw = 50
self._cam_pitch = -35
self._hard_reset = True
self._last_frame_time = 0.0
optionstring = '--width={} --height={}'.format(render_width,
render_height)
print("urdf_root=" + self._urdf_root)
if self._is_render:
self._pybullet_client = bc.BulletClient(
connection_mode=pybullet.GUI, options=optionstring)
else:
self._pybullet_client = bc.BulletClient()
if (debug_visualization == False):
self._pybullet_client.configureDebugVisualizer(
flag=self._pybullet_client.COV_ENABLE_GUI, enable=0)
self._pybullet_client.configureDebugVisualizer(
flag=self._pybullet_client.COV_ENABLE_RGB_BUFFER_PREVIEW,
enable=0)
self._pybullet_client.configureDebugVisualizer(
flag=self._pybullet_client.COV_ENABLE_DEPTH_BUFFER_PREVIEW,
enable=0)
self._pybullet_client.configureDebugVisualizer(
flag=self._pybullet_client.
COV_ENABLE_SEGMENTATION_MARK_PREVIEW,
enable=0)
self._pybullet_client.setAdditionalSearchPath(urdf_root)
self.seed()
self.reset()
observation_high = (self._example_sim.GetObservationUpperBound())
observation_low = (self._example_sim.GetObservationLowerBound())
action_dim = self._example_sim.GetActionDimension()
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(observation_low, observation_high)
self.viewer = None
self._hard_reset = hard_reset # This assignment need to be after reset()
def ExploreWorker(rank, num_processes, childPipe, args):
print("hi:", rank, " out of ", num_processes)
import pybullet as op1
import pybullet_data_local as pd
logName = ""
p1 = 0
n = 0
space = 2
simulations = []
sims_per_worker = 10
offsetY = rank * space
while True:
n += 1
try:
# Only block for short times to have keyboard exceptions be raised.
if not childPipe.poll(0.0001):
continue
message, payload = childPipe.recv()
except (EOFError, KeyboardInterrupt):
break
if message == _RESET:
if (useGUI):
p1 = bullet_client.BulletClient(op1.GUI)
else:
p1 = bullet_client.BulletClient(op1.DIRECT)
p1.setTimeStep(timeStep)
p1.setPhysicsEngineParameter(numSolverIterations=8)
p1.setPhysicsEngineParameter(minimumSolverIslandSize=100)
p1.configureDebugVisualizer(p1.COV_ENABLE_Y_AXIS_UP, 1)
p1.configureDebugVisualizer(p1.COV_ENABLE_RENDERING, 0)
p1.setAdditionalSearchPath(pd.getDataPath())
p1.setGravity(0, -9.8, 0)
logName = str("batchsim") + str(rank)
for j in range(3):
offsetX = 0 #-sims_per_worker/2.0*space
for i in range(sims_per_worker):
offset = [offsetX, 0, offsetY]
sim = panda_sim.PandaSimAuto(p1, offset)
simulations.append(sim)
offsetX += space
offsetY += space
childPipe.send(["reset ok"])
p1.configureDebugVisualizer(p1.COV_ENABLE_RENDERING, 1)
for i in range(100):
p1.stepSimulation()
logId = p1.startStateLogging(op1.STATE_LOGGING_PROFILE_TIMINGS,
logName)
continue
if message == _EXPLORE:
sum_rewards = rank
if useGUI:
numSteps = int(20000)
else:
numSteps = int(5)
for i in range(numSteps):
for s in simulations:
s.step()
p1.stepSimulation()
#print("logId=",logId)
#print("numSteps=",numSteps)
childPipe.send([sum_rewards])
continue
if message == _CLOSE:
p1.stopStateLogging(logId)
childPipe.send(["close ok"])
break
childPipe.close()
from pybullet_utils_local import bullet_client
import time
import math
import motion_capture_data
from pybullet_envs_local.deep_mimic.env import humanoid_stable_pd
import pybullet_data_local
import pybullet as p1
import humanoid_pose_interpolator
import numpy as np
pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
pybullet_client.setAdditionalSearchPath(pybullet_data_local.getDataPath())
z2y = pybullet_client.getQuaternionFromEuler([-math.pi * 0.5, 0, 0])
#planeId = pybullet_client.loadURDF("plane.urdf",[0,0,0],z2y)
planeId = pybullet_client.loadURDF("plane_implicit.urdf", [0, 0, 0],
z2y,
useMaximalCoordinates=True)
pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9)
#print("planeId=",planeId)
pybullet_client.configureDebugVisualizer(pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
pybullet_client.setGravity(0, -9.8, 0)
pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)
mocapData = motion_capture_data.MotionCaptureData()
#motionPath = pybullet_data_local.getDataPath()+"/data/motions/humanoid3d_walk.txt"
motionPath = pybullet_data_local.getDataPath() + "/data/motions/humanoid3d_backflip.txt"
mocapData.Load(motionPath)
timeStep = 1. / 600
#video requires ffmpeg available in path
createVideo=False
fps=240.
timeStep = 1./fps
if createVideo:
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=0 --mp4=\"pybullet_grasp.mp4\" --mp4fps="+str(fps) )
else:
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP,1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000)
p.resetDebugVisualizerCamera(cameraDistance=1.3, cameraYaw=38, cameraPitch=-22, cameraTargetPosition=[0.35,-0.13,0])
p.setAdditionalSearchPath(pd.getDataPath())
p.setTimeStep(timeStep)
p.setGravity(0,-9.8,0)
panda = panda_sim.PandaSimAuto(p,[0,0,0])
panda.control_dt = timeStep
logId = panda.bullet_client.startStateLogging(panda.bullet_client.STATE_LOGGING_PROFILE_TIMINGS, "log.json")
panda.bullet_client.submitProfileTiming("start")
for i in range (100000):
panda.bullet_client.submitProfileTiming("full_step")
panda.step()
p.stepSimulation()
if createVideo:
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
def __init__(self,
urdf_root=pybullet_data_local.getDataPath(),
urdf_version=None,
distance_weight=1.0,
energy_weight=0.005,
shake_weight=0.0,
drift_weight=0.0,
distance_limit=float("inf"),
observation_noise_stdev=SENSOR_NOISE_STDDEV,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False,
leg_model_enabled=True,
accurate_motor_model_enabled=False,
remove_default_joint_damping=False,
motor_kp=1.0,
motor_kd=0.02,
control_latency=0.0,
pd_latency=0.0,
torque_control_enabled=False,
motor_overheat_protection=False,
hard_reset=True,
on_rack=False,
render=False,
num_steps_to_log=1000,
action_repeat=1,
control_time_step=None,
env_randomizer=None,
forward_reward_cap=float("inf"),
reflection=True,
log_path=None):
"""Initialize the minitaur gym environment.
Args:
urdf_root: The path to the urdf data folder.
urdf_version: [DEFAULT_URDF_VERSION, DERPY_V0_URDF_VERSION,
RAINBOW_DASH_V0_URDF_VERSION] are allowable
versions. If None, DEFAULT_URDF_VERSION is used. DERPY_V0_URDF_VERSION
is the result of first pass system identification for derpy.
We will have a different URDF and related Minitaur class each time we
perform system identification. While the majority of the code of the
class remains the same, some code changes (e.g. the constraint location
might change). __init__() will choose the right Minitaur class from
different minitaur modules based on
urdf_version.
distance_weight: The weight of the distance term in the reward.
energy_weight: The weight of the energy term in the reward.
shake_weight: The weight of the vertical shakiness term in the reward.
drift_weight: The weight of the sideways drift term in the reward.
distance_limit: The maximum distance to terminate the episode.
observation_noise_stdev: The standard deviation of observation noise.
self_collision_enabled: Whether to enable self collision in the sim.
motor_velocity_limit: The velocity limit of each motor.
pd_control_enabled: Whether to use PD controller for each motor.
leg_model_enabled: Whether to use a leg motor to reparameterize the action
space.
accurate_motor_model_enabled: Whether to use the accurate DC motor model.
remove_default_joint_damping: Whether to remove the default joint damping.
motor_kp: proportional gain for the accurate motor model.
motor_kd: derivative gain for the accurate motor model.
control_latency: It is the delay in the controller between when an
observation is made at some point, and when that reading is reported
back to the Neural Network.
pd_latency: latency of the PD controller loop. PD calculates PWM based on
the motor angle and velocity. The latency measures the time between when
the motor angle and velocity are observed on the microcontroller and
when the true state happens on the motor. It is typically (0.001-
0.002s).
torque_control_enabled: Whether to use the torque control, if set to
False, pose control will be used.
motor_overheat_protection: Whether to shutdown the motor that has exerted
large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
(OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more
details.
hard_reset: Whether to wipe the simulation and load everything when reset
is called. If set to false, reset just place the minitaur back to start
position and set its pose to initial configuration.
on_rack: Whether to place the minitaur on rack. This is only used to debug
the walking gait. In this mode, the minitaur's base is hanged midair so
that its walking gait is clearer to visualize.
render: Whether to render the simulation.
num_steps_to_log: The max number of control steps in one episode that will
be logged. If the number of steps is more than num_steps_to_log, the
environment will still be running, but only first num_steps_to_log will
be recorded in logging.
action_repeat: The number of simulation steps before actions are applied.
control_time_step: The time step between two successive control signals.
env_randomizer: An instance (or a list) of EnvRandomizer(s). An
EnvRandomizer may randomize the physical property of minitaur, change
the terrrain during reset(), or add perturbation forces during step().
forward_reward_cap: The maximum value that forward reward is capped at.
Disabled (Inf) by default.
log_path: The path to write out logs. For the details of logging, refer to
minitaur_logging.proto.
Raises:
ValueError: If the urdf_version is not supported.
"""
# Set up logging.
self._log_path = log_path
self.logging = minitaur_logging.MinitaurLogging(log_path)
# PD control needs smaller time step for stability.
if control_time_step is not None:
self.control_time_step = control_time_step
self._action_repeat = action_repeat
self._time_step = control_time_step / action_repeat
else:
# Default values for time step and action repeat
if accurate_motor_model_enabled or pd_control_enabled:
self._time_step = 0.002
self._action_repeat = 5
else:
self._time_step = 0.01
self._action_repeat = 1
self.control_time_step = self._time_step * self._action_repeat
# TODO(b/73829334): Fix the value of self._num_bullet_solver_iterations.
self._num_bullet_solver_iterations = int(
NUM_SIMULATION_ITERATION_STEPS / self._action_repeat)
self._urdf_root = urdf_root
self._self_collision_enabled = self_collision_enabled
self._motor_velocity_limit = motor_velocity_limit
self._observation = []
self._true_observation = []
self._objectives = []
self._objective_weights = [
distance_weight, energy_weight, drift_weight, shake_weight
]
self._env_step_counter = 0
self._num_steps_to_log = num_steps_to_log
self._is_render = render
self._last_base_position = [0, 0, 0]
self._distance_weight = distance_weight
self._energy_weight = energy_weight
self._drift_weight = drift_weight
self._shake_weight = shake_weight
self._distance_limit = distance_limit
self._observation_noise_stdev = observation_noise_stdev
self._action_bound = 1
self._pd_control_enabled = pd_control_enabled
self._leg_model_enabled = leg_model_enabled
self._accurate_motor_model_enabled = accurate_motor_model_enabled
self._remove_default_joint_damping = remove_default_joint_damping
self._motor_kp = motor_kp
self._motor_kd = motor_kd
self._torque_control_enabled = torque_control_enabled
self._motor_overheat_protection = motor_overheat_protection
self._on_rack = on_rack
self._cam_dist = 1.0
self._cam_yaw = 0
self._cam_pitch = -30
self._forward_reward_cap = forward_reward_cap
self._hard_reset = True
self._last_frame_time = 0.0
self._control_latency = control_latency
self._pd_latency = pd_latency
self._urdf_version = urdf_version
self._ground_id = None
self._reflection = reflection
self._env_randomizers = convert_to_list(
env_randomizer) if env_randomizer else []
#self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
if self._is_render:
self._pybullet_client = bc.BulletClient(
connection_mode=pybullet.GUI)
else:
self._pybullet_client = bc.BulletClient()
if self._urdf_version is None:
self._urdf_version = DEFAULT_URDF_VERSION
self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
self._pybullet_client.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,
0)
self.seed()
self.reset()
observation_high = (self._get_observation_upper_bound() +
OBSERVATION_EPS)
observation_low = (self._get_observation_lower_bound() -
OBSERVATION_EPS)
action_dim = NUM_MOTORS
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(observation_low, observation_high)
self.viewer = None
self._hard_reset = hard_reset # This assignment need to be after reset()
r"""Running a pre-trained ppo agent on minitaur_trotting_env."""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import os
import time
import tensorflow.compat.v1 as tf
from pybullet_envs_local.minitaur.agents.scripts import utility
import pybullet_data_local
from pybullet_envs_local.minitaur.envs import simple_ppo_agent
flags = tf.app.flags
FLAGS = tf.app.flags.FLAGS
LOG_DIR = os.path.join(pybullet_data_local.getDataPath(),
"policies/ppo/minitaur_trotting_env")
CHECKPOINT = "model.ckpt-14000000"
def main(argv):
del argv # Unused.
config = utility.load_config(LOG_DIR)
policy_layers = config.policy_layers
value_layers = config.value_layers
env = config.env(render=True)
network = config.network
with tf.Session() as sess:
agent = simple_ppo_agent.SimplePPOPolicy(sess,
env,
def get_sphere(_p, x, y, z):
body = _p.loadURDF(os.path.join(pybullet_data_local.getDataPath(), "sphere2red_nocol.urdf"), [x, y, z])
part_name, _ = _p.getBodyInfo(body)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(_p, part_name, bodies, 0, -1)
def reset(self):
if not self._isInitialized:
if self.enable_draw:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=p1.GUI)
#disable 'GUI' since it slows down a lot on Mac OSX and some other platforms
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_GUI, 0)
else:
self._pybullet_client = bullet_client.BulletClient()
self._pybullet_client.setAdditionalSearchPath(
pybullet_data_local.getDataPath())
z2y = self._pybullet_client.getQuaternionFromEuler(
[-math.pi * 0.5, 0, 0])
self._planeId = self._pybullet_client.loadURDF(
"plane_implicit.urdf", [0, 0, 0],
z2y,
useMaximalCoordinates=True)
#print("planeId=",self._planeId)
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
self._pybullet_client.setGravity(0, -9.8, 0)
self._pybullet_client.setPhysicsEngineParameter(
numSolverIterations=10)
self._pybullet_client.changeDynamics(self._planeId,
linkIndex=-1,
lateralFriction=0.9)
self._mocapData = motion_capture_data.MotionCaptureData()
motion_file = self._arg_parser.parse_strings('motion_file')
print("motion_file=", motion_file[0])
motionPath = pybullet_data_local.getDataPath(
) + "/" + motion_file[0]
#motionPath = pybullet_data_local.getDataPath()+"/motions/humanoid3d_backflip.txt"
self._mocapData.Load(motionPath)
timeStep = 1. / 240.
useFixedBase = False
self._humanoid = humanoid_stable_pd.HumanoidStablePD(
self._pybullet_client, self._mocapData, timeStep, useFixedBase,
self._arg_parser)
self._isInitialized = True
self._pybullet_client.setTimeStep(timeStep)
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1)
selfCheck = False
if (selfCheck):
curTime = 0
while self._pybullet_client.isConnected():
self._humanoid.setSimTime(curTime)
state = self._humanoid.getState()
#print("state=",state)
pose = self._humanoid.computePose(
self._humanoid._frameFraction)
for i in range(10):
curTime += timeStep
#taus = self._humanoid.computePDForces(pose)
#self._humanoid.applyPDForces(taus)
#self._pybullet_client.stepSimulation()
time.sleep(timeStep)
#print("numframes = ", self._humanoid._mocap_data.NumFrames())
#startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
rnrange = 1000
rn = random.randint(0, rnrange)
startTime = float(rn) / rnrange * self._humanoid.getCycleTime()
self.t = startTime
self._humanoid.setSimTime(startTime)
self._humanoid.resetPose()
#this clears the contact points. Todo: add API to explicitly clear all contact points?
#self._pybullet_client.stepSimulation()
self._humanoid.resetPose()
self.needs_update_time = self.t - 1 #force update
def __init__(self,
urdfRoot=pybullet_data_local.getDataPath(),
actionRepeat=80,
isEnableSelfCollision=True,
renders=False,
isDiscrete=False,
maxSteps=8,
dv=0.06,
removeHeightHack=False,
blockRandom=0.3,
cameraRandom=0,
width=48,
height=48,
numObjects=5,
isTest=False):
"""Initializes the KukaDiverseObjectEnv.
Args:
urdfRoot: The diretory from which to load environment URDF's.
actionRepeat: The number of simulation steps to apply for each action.
isEnableSelfCollision: If true, enable self-collision.
renders: If true, render the bullet GUI.
isDiscrete: If true, the action space is discrete. If False, the
action space is continuous.
maxSteps: The maximum number of actions per episode.
dv: The velocity along each dimension for each action.
removeHeightHack: If false, there is a "height hack" where the gripper
automatically moves down for each action. If true, the environment is
harder and the policy chooses the height displacement.
blockRandom: A float between 0 and 1 indicated block randomness. 0 is
deterministic.
cameraRandom: A float between 0 and 1 indicating camera placement
randomness. 0 is deterministic.
width: The image width.
height: The observation image height.
numObjects: The number of objects in the bin.
isTest: If true, use the test set of objects. If false, use the train
set of objects.
"""
self._isDiscrete = isDiscrete
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._dv = dv
self._p = p
self._removeHeightHack = removeHeightHack
self._blockRandom = blockRandom
self._cameraRandom = cameraRandom
self._width = width
self._height = height
self._numObjects = numObjects
self._isTest = isTest
if self._renders:
self.cid = p.connect(p.SHARED_MEMORY)
if (self.cid < 0):
self.cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
else:
self.cid = p.connect(p.DIRECT)
self.seed()
if (self._isDiscrete):
if self._removeHeightHack:
self.action_space = spaces.Discrete(9)
else:
self.action_space = spaces.Discrete(7)
else:
self.action_space = spaces.Box(low=-1, high=1,
shape=(3, )) # dx, dy, da
if self._removeHeightHack:
self.action_space = spaces.Box(low=-1, high=1,
shape=(4, )) # dx, dy, dz, da
self.viewer = None
import pybullet as p
import time
import pybullet_data_local
#cid = p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data_local.getDataPath())
p.resetSimulation()
objects = [
p.loadURDF("plane.urdf", 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
0.000000, 1.000000)
]
objects = [
p.loadURDF("samurai.urdf", 0.000000, 0.000000, 0.000000, 0.000000,
0.000000, 0.000000, 1.000000)
]
objects = [
p.loadURDF("pr2_gripper.urdf", 0.500000, 0.300006, 0.700000, -0.000000,
-0.000000, -0.000031, 1.000000)
]
pr2_gripper = objects[0]
print("pr2_gripper=")
print(pr2_gripper)
jointPositions = [0.550569, 0.000000, 0.549657, 0.000000]
for jointIndex in range(p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper, jointIndex, jointPositions[jointIndex])
def __init__(self, renders=False, arg_file=''):
self._arg_parser = ArgParser()
Logger.print2(
"===========================================================")
succ = False
if (arg_file != ''):
path = pybullet_data_local.getDataPath() + "/args/" + arg_file
succ = self._arg_parser.load_file(path)
Logger.print2(arg_file)
assert succ, Logger.print2('Failed to load args from: ' + arg_file)
self._p = None
self._time_step = 1. / 240.
self._internal_env = None
self._renders = renders
self._discrete_actions = False
self._arg_file = arg_file
self._render_height = 200
self._render_width = 320
self.theta_threshold_radians = 12 * 2 * math.pi / 360
self.x_threshold = 0.4 #2.4
high = np.array([
self.x_threshold * 2,
np.finfo(np.float32).max, self.theta_threshold_radians * 2,
np.finfo(np.float32).max
])
ctrl_size = 43 #numDof
root_size = 7 # root
action_dim = ctrl_size - root_size
action_bound_min = np.array([
-4.79999999999, -1.00000000000, -1.00000000000, -1.00000000000,
-4.00000000000, -1.00000000000, -1.00000000000, -1.00000000000,
-7.77999999999, -1.00000000000, -1.000000000, -1.000000000,
-7.850000000, -6.280000000, -1.000000000, -1.000000000,
-1.000000000, -12.56000000, -1.000000000, -1.000000000,
-1.000000000, -4.710000000, -7.779999999, -1.000000000,
-1.000000000, -1.000000000, -7.850000000, -6.280000000,
-1.000000000, -1.000000000, -1.000000000, -8.460000000,
-1.000000000, -1.000000000, -1.000000000, -4.710000000
])
#print("len(action_bound_min)=",len(action_bound_min))
action_bound_max = np.array([
4.799999999, 1.000000000, 1.000000000, 1.000000000, 4.000000000,
1.000000000, 1.000000000, 1.000000000, 8.779999999, 1.000000000,
1.0000000, 1.0000000, 4.7100000, 6.2800000, 1.0000000, 1.0000000,
1.0000000, 12.560000, 1.0000000, 1.0000000, 1.0000000, 7.8500000,
8.7799999, 1.0000000, 1.0000000, 1.0000000, 4.7100000, 6.2800000,
1.0000000, 1.0000000, 1.0000000, 10.100000, 1.0000000, 1.0000000,
1.0000000, 7.8500000
])
#print("len(action_bound_max)=",len(action_bound_max))
self.action_space = spaces.Box(action_bound_min, action_bound_max)
observation_min = np.array([0.0] + [-100.0] + [-4.0] * 105 +
[-500.0] * 90)
observation_max = np.array([1.0] + [100.0] + [4.0] * 105 +
[500.0] * 90)
state_size = 197
self.observation_space = spaces.Box(observation_min,
observation_min,
dtype=np.float32)
self.seed()
self.viewer = None
self._configure()
