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()