def __call__(self, env, policy, debug=False, visualize=False): episode_memory = queue() observation = None result = [] for episode in range(self.validate_episodes): # reset at the start of episode episode_memory.clear() observation = env.reset() episode_memory.append(observation) observation = episode_memory.getObservation(self.window_length, observation, self.pic) episode_steps = 0 episode_reward = 0. assert observation is not None # start episode done = False while not done and (episode_steps <= self.max_episode_length or not self.max_episode_length): action = policy(observation) observation, reward, done, info = env.step(action) episode_memory.append(observation) observation = episode_memory.getObservation(self.window_length, observation, self.pic) if visualize: if self.bullet: import pybullet pybullet.resetDebugVisualizerCamera(cameraDistance=10, cameraYaw=0, cameraPitch=-6.6,cameraTargetPosition=[10,0,0]) env.render() episode_reward += reward episode_steps += 1 result.append(episode_reward) if debug: prRed('[Evaluate] reward:{}'.format(result)) return result
def __init__(self, urdfRoot=pybullet_data.getDataPath(), actionRepeat=1, isEnableSelfCollision=True, renders=True): print("init") 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.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) self.action_space = spaces.Discrete(7) self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4)) self.viewer = None
def __init__(self, urdfRoot=pybullet_data.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, renderer='tiny', # ('tiny', 'egl', 'debug') ): self.action_space = spaces.Discrete(2) self.iter = cycle(range(0, 360, 10)) # how we want to show assert renderer in ('tiny', 'egl', 'debug', 'plugin') self._renderer = renderer self._render_width = 84 self._render_height = 84 # connecting if self._renderer == "tiny" or self._renderer == "plugin": optionstring = '--width={} --height={}'.format(self._render_width, self._render_height) p.connect(p.DIRECT, options=optionstring) if self._renderer == "plugin": plugin_fn = os.path.join( p.__file__.split("bullet3")[0], "bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so") plugin = p.loadPlugin(plugin_fn, "_tinyRendererPlugin") if plugin < 0: print("\nPlugin Failed to load! Try installing via `pip install -e .`\n") sys.exit() print("plugin =", plugin) elif self._renderer == "egl": optionstring = '--width={} --height={}'.format(self._render_width, self._render_height) optionstring += ' --window_backend=2 --render_device=0' p.connect(p.GUI, options=optionstring) elif self._renderer == "debug": #print("Connection: SHARED_MEMORY") #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]) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
# -*- coding: utf-8 -*- """ Created on 2019 @author: Richard Bloemenkamp """ import pybullet as p import time import numpy as np p.connect(p.GUI) p.createCollisionShape(p.GEOM_PLANE) plId=p.createMultiBody(0,0) p.resetDebugVisualizerCamera( cameraDistance=4, cameraYaw=10, cameraPitch=-20, cameraTargetPosition=[0.0, 0.0, 0.25]) sh_colFoot = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.01,0.01,0.1]) sh_colBody = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.1,0.1,0.1]) sh_colPx = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.01,0.1,0.1]) sh_colPy = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.1,0.01,0.1]) bodyId=p.createMultiBody(baseMass=0.8,baseCollisionShapeIndex = sh_colBody, basePosition = [0,0,1.5],baseOrientation=[0,0,0,1]) footId=p.createMultiBody(baseMass=.1,baseCollisionShapeIndex = sh_colFoot, basePosition = [0,0,0.5],baseOrientation=[0,0,0,1]) #4 inertia increasing plates cubeId3=p.createMultiBody(baseMass=.1,baseCollisionShapeIndex = sh_colPx, basePosition = [-0.5,0,1.5],baseOrientation=[0,0,0,1]) cubeId4=p.createMultiBody(baseMass=.1,baseCollisionShapeIndex = sh_colPx, basePosition = [0.5,0,1.5],baseOrientation=[0,0,0,1]) cubeId5=p.createMultiBody(baseMass=.1,baseCollisionShapeIndex = sh_colPy, basePosition = [0,-0.5,1.5],baseOrientation=[0,0,0,1])
try: result = sys.stdin.read(1) except IOError: pass finally: termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) return result p.connect(p.GUI) # create the main plane p.resetDebugVisualizerCamera(cameraDistance=5, cameraYaw=90, cameraPitch=-10, cameraTargetPosition=[0, 0, 3]) p.createCollisionShape(p.GEOM_PLANE) p.createMultiBody(0, 0) # create spheres and cubes sphereRadius = 0.5 colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius) colBoxId = p.createCollisionShape( p.GEOM_BOX, halfExtents=[sphereRadius, sphereRadius, sphereRadius]) sphereMass = 2 cubeMass = 1 visualShapeId = -1 # not needed since we're using the collision shape ID, can set to -1 basePositionSphere = [0, 1, 5 * sphereRadius + 1] basePositionCube = [0, -1, 5 * sphereRadius + 1]
return path if __name__ == "__main__": args = get_args() # set up simulator physicsClient = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(enableFileCaching=0) p.setGravity(0, 0, -9.8) p.configureDebugVisualizer(p.COV_ENABLE_GUI, False) p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, True) p.resetDebugVisualizerCamera( cameraDistance=1.400, cameraYaw=58.000, cameraPitch=-42.200, cameraTargetPosition=(0.0, 0.0, 0.0), ) # load objects plane = p.loadURDF("plane.urdf") ur5 = p.loadURDF("assets/ur5/ur5.urdf", basePosition=[0, 0, 0.02], useFixedBase=True) obstacle1 = p.loadURDF("assets/block.urdf", basePosition=[1 / 4, 0, 1 / 2], useFixedBase=True) obstacle2 = p.loadURDF("assets/block.urdf", basePosition=[2 / 4, 0, 2 / 3], useFixedBase=True) obstacles = [plane, obstacle1, obstacle2]
def __init__(self, urdfRoot=pybullet_data.getDataPath(), actionRepeat=80, isEnableSelfCollision=True, renders=False, isDiscrete=True, maxSteps=11, dv=0.06, removeHeightHack=False, blockRandom=0.1, cameraRandom=0, width=64, height=64, numObjects=5, isTest=False): ############## self._width = IM_WIDTH self._height = IM_HEIGHT self.img_save_cnt = 0 self.N_DATA_TO_GENERATE = N_DATA_TO_GENERATE self.N_UNSEEN_DATA_TO_GENERATE = N_UNSEEN_DATA_TO_GENERATE ############## self._isDiscrete = isDiscrete # self._timeStep = 1. / 240. self._timeStep = 1. / 60. 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._isTest = isTest self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 3), dtype=np.uint8) ############################# self.model_paths, self.unseen_model_paths = self.get_data_path() ############################# # disable GUI or not if USE_GUI: 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]) # cameraposition of rendering 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 physicsClient = p.connect(p.GUI) # or p.DIRECT for non graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally #p.setTimeStep(1.0) planeID = p.loadURDF("plane.urdf") p.resetDebugVisualizerCamera(cameraDistance=0.5, cameraYaw=75, cameraPitch=-20, cameraTargetPosition=[0.0, 0.0, 0.25]) baseStartPos = [0, 0, 0.5] baseStartOrientation = p.getQuaternionFromEuler([1, 0, 0]) quadrupedID = p.loadURDF("MQ_SIM_ASEM/urdf/MQ_SIM_ASEM.urdf", baseStartPos, baseStartOrientation) #p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) # Set Joints to home position for joint in range(p.getNumJoints(quadrupedID)): p.setJointMotorControl2(quadrupedID, joint, p.POSITION_CONTROL, targetPosition=0.0, force=10, maxVelocity=5) # Set Shoulder Joints joint = 0
def reset(self): super(DrinkingEnv, self).reset() self.build_assistive_env('wheelchair') if self.robot.wheelchair_mounted: wheelchair_pos, wheelchair_orient = self.furniture.get_base_pos_orient( ) self.robot.set_base_pos_orient( wheelchair_pos + np.array(self.robot.toc_base_pos_offset[self.task]), [0, 0, -np.pi / 2.0]) # Update robot and human motor gains self.robot.motor_gains = self.human.motor_gains = 0.005 joints_positions = [(self.human.j_right_elbow, -90), (self.human.j_left_elbow, -90), (self.human.j_right_hip_x, -90), (self.human.j_right_knee, 80), (self.human.j_left_hip_x, -90), (self.human.j_left_knee, 80)] joints_positions += [ (self.human.j_head_x, self.np_random.uniform(-30, 30)), (self.human.j_head_y, self.np_random.uniform(-30, 30)), (self.human.j_head_z, self.np_random.uniform(-30, 30)) ] self.human.setup_joints(joints_positions, use_static_joints=True, reactive_force=None) self.generate_target() p.resetDebugVisualizerCamera(cameraDistance=1.10, cameraYaw=55, cameraPitch=-45, cameraTargetPosition=[-0.2, 0, 0.75], physicsClientId=self.id) # Initialize the tool in the robot's gripper self.tool.init(self.robot, self.task, self.directory, self.id, self.np_random, right=True, mesh_scale=[0.045] * 3, alpha=0.75) self.cup_top_center_offset = np.array([0, 0, -0.055]) self.cup_bottom_center_offset = np.array([0, 0, 0.07]) target_ee_pos = np.array([-0.2, -0.5, 1]) + self.np_random.uniform( -0.05, 0.05, size=3) target_ee_orient = self.get_quaternion( self.robot.toc_ee_orient_rpy[self.task]) self.init_robot_pose(target_ee_pos, target_ee_orient, [(target_ee_pos, target_ee_orient), (self.target_pos, None)], [(self.target_pos, target_ee_orient)], arm='right', tools=[self.tool], collision_objects=[self.human, self.furniture]) # Open gripper to hold the tool self.robot.set_gripper_open_position(self.robot.right_gripper_indices, self.robot.gripper_pos[self.task], set_instantly=True) if not self.robot.mobile: self.robot.set_gravity(0, 0, 0) self.human.set_gravity(0, 0, 0) self.tool.set_gravity(0, 0, 0) p.setPhysicsEngineParameter(numSubSteps=4, numSolverIterations=10, physicsClientId=self.id) # Generate water cup_pos, cup_orient = self.tool.get_base_pos_orient() water_radius = 0.005 water_mass = 0.001 batch_positions = [] for i in range(4): for j in range(4): for k in range(4): batch_positions.append( np.array([ i * 2 * water_radius - 0.02, j * 2 * water_radius - 0.02, k * 2 * water_radius + 0.075 ]) + cup_pos) self.waters = self.create_spheres(radius=water_radius, mass=water_mass, batch_positions=batch_positions, visual=False, collision=True) for w in self.waters: p.changeVisualShape(w.body, -1, rgbaColor=[0.25, 0.5, 1, 1], physicsClientId=self.id) self.total_water_count = len(self.waters) self.waters_active = [w for w in self.waters] # Enable rendering p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id) # Drop water in the cup for _ in range(50): p.stepSimulation(physicsClientId=self.id) self.init_env_variables() return self._get_obs()
import pybullet_data import os import time # physics parameter setting fixedTimeStep = 1. / 1000 numSolverIterations = 200 physicsClient = p.connect(p.GUI) p.setTimeStep(fixedTimeStep) p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # to load plane.urdf p.setGravity(0, 0, -9.81) p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=10, cameraPitch=-5, cameraTargetPosition=[0.3, 0.5, 0.1]) StartPos = [0, 0, 0.3] StartOrientation = p.getQuaternionFromEuler([0, 0, 0]) # samurai.urdf plane.urdf planeId = p.loadURDF("plane.urdf") robot = p.loadURDF("test-1.urdf", StartPos, StartOrientation) p.setRealTimeSimulation(1) # real-time instead of step control while 1: # p.stepSimulation() # time.sleep(0.01)
block_config[block] = (pos, quat, colors[b]) block_id = {} for blk, (pos, quat, rgb) in block_config.items(): b = pb.loadURDF('cube.urdf', basePosition=pos, baseOrientation=quat, useFixedBase=False) pb.changeVisualShape(b, linkIndex=-1, rgbaColor=rgb + (1, )) block_id[blk] = b env.step() # let blocks settle # from check/camera.py pb.resetDebugVisualizerCamera( 1.2000000476837158, 56.799964904785156, -22.20000648498535, (-0.6051651835441589, 0.26229506731033325, -0.24448847770690918)) def get_tip_targets(p, q, d): m = pb.getMatrixFromQuaternion(q) t1 = p[0] - d * m[1], p[1] - d * m[4], p[2] - d * m[7] t2 = p[0] + d * m[1], p[1] + d * m[4], p[2] + d * m[7] return (t1, t2) action = [0.] * env.num_joints # env.set_position(action) env.goto_position(action, 1)
########################################## t_stance = 0.2 ### Initialization steps = int(t_stance / dt) t = 0.0 # Video logger settings log_video = False if log_video: video_name = "./video/small model/trial5.mp4" fps = int(1 / dt) if fps > 480: fps = 480 writer = imageio.get_writer(video_name, fps=fps) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.resetDebugVisualizerCamera(cameraDistance=0.1, cameraYaw=50, cameraPitch=-60, cameraTargetPosition=[0, 0, 0]) # Data logger settings log_data = True eCOM_history = np.array([0]) eq_history = np.array([0] * model.state.nqd) des_history = list() tau_history = list() state_history = list() # Starting position and legs abduction range theta_g0 = pi / 6 theta_gf = -pi / 2 # p.resetJointState(model.Id, model.control_indices[1][0],theta_g0) # p.resetJointState(model.Id, model.control_indices[2][0],-theta_gf) model.set_bent_position(theta_rg=theta_g0, theta_lg=-theta_gf,
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()
jointPoses : [float] * numDofs """ numJoints = p.getNumJoints(bodyId) for i in range(numJoints): jointInfo = p.getJointInfo(bodyId, i) qIndex = jointInfo[3] if qIndex > -1: p.setJointMotorControl2(bodyIndex=bodyId, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=jointPoses[qIndex-7]) if __name__ == "__main__": guiClient = p.connect(p.GUI) p.resetDebugVisualizerCamera(2., 180, 0., [0.52, 0.2, np.pi/4.]) baxterId, endEffectorId = setUpWorld() lowerLimits, upperLimits, jointRanges, restPoses = getJointRanges(baxterId, includeFixed=False) #targetPosition = [0.2, 0.8, -0.1] #targetPosition = [0.8, 0.2, -0.1] targetPosition = [0.2, 0.0, -0.1] p.addUserDebugText("TARGET", targetPosition, textColorRGB=[1,0,0], textSize=1.5) maxIters = 100000
import time p.connect(p.GUI) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM = 0.000001) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) p.loadURDF("plane.urdf") radius = 0.025 distance = 1.86 yaw=135 pitch=-11 targetPos=[0,0,0] p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200) p.resetDebugVisualizerCamera(distance, yaw,pitch, targetPos) objectId = -1 for i in range (10): objectId = p.loadURDF("cube_small.urdf",[1,1,radius+i*2*radius]) p.changeDynamics(objectId,-1,100) timeStep = 1./240. p.setGravity(0,0,-10) while (p.isConnected()): p.stepSimulation() time.sleep(timeStep)
def __init__(self, urdf_path=URDF_PATH, max_steps=250, env_rank=0, random_target=False, srl_pipe=None, is_discrete=True, action_repeat=1, srl_model="ground_truth", control_mode="position", seed=0, debug_mode=False, **_): super(JakaButtonObsGymEnv, self).__init__(srl_model=srl_model, relative_pos=True, env_rank=env_rank, srl_pipe=srl_pipe) self.seed(seed) self.urdf_path = urdf_path self._random_target = random_target self._observation = None self.debug_mode = debug_mode self._inmoov = None self._observation = None self.action_repeat = action_repeat self._jaka_id = -1 self._button_id = -1 self.max_steps = max_steps self._step_counter = 0 self._render = False self.position_control = (control_mode == "position") self.discrete_action = is_discrete self.srl_model = srl_model self.camera_target_pos = (0.0, 0.0, 1.0) self._width = RENDER_WIDTH self._height = RENDER_HEIGHT self.terminated = False self.n_contacts = 0 self.state_dim = self.getGroundTruthDim() self._first_reset_flag = False if debug_mode: client_id = p.connect(p.SHARED_MEMORY) if client_id < 0: p.connect(p.GUI) p.resetDebugVisualizerCamera(2., 180, -41, [0., -0.1, 0.1]) else: p.connect(p.DIRECT) self.action_space = spaces.Discrete(6) if self.srl_model == "raw_pixels": self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 3), dtype=np.uint8) else: # Todo: the only possible observation for srl_model is ground truth self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(3, ), dtype=np.float32) self.reset()
def __init__(self, urdfRoot=pybullet_data.getDataPath(), actionRepeat=1, # <---- was 8? isEnableSelfCollision=True, renders=False, isDiscrete=False, maxSteps=1000, # <---- was 8? dv=0.06, removeHeightHack=True, blockRandom=0.3, cameraRandom=0, width=48, height=48, numObjects=3, 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.observation_space = spaces.Box(-observation_high, observation_high) <--- is absent self.viewer = None
def __init__(self, urdfRoot=pybullet_data.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
# Get infos on end effector and gripper link # end effector link = 'lbr_iiwa_link_7', gripper link = 'base_link' kukaEndEffectorIndex = 6 kukaGripperIndex = 7 jointInfoEndEffector = p.getJointInfo(kukaUid, kukaEndEffectorIndex) linkNameEndEffector = str(jointInfoEndEffector[12].decode("utf-8")) jointInfoGripper = p.getJointInfo(kukaUid, kukaGripperIndex) linkNameGripper = str(jointInfoGripper[12].decode("utf-8")) print("-> link : end effector={0}, gripper={1}".format(linkNameEndEffector, linkNameGripper)) # reset the initial view of the environment (to be closer to robot) p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=0, cameraPitch=-40, cameraTargetPosition=[0.35, 0.0, 0.7]) # reset the position and orientation of the base (root) of the robot kuka p.resetBasePositionAndOrientation(kukaUid, [-0.100000, 0.000000, 0.070000], [0.000000, 0.000000, 0.000000, 1.000000]) #Add sliders for all joints (useful to move the joints) joint_name_to_ids = {} # dictionary to associate a joint name to index joint_name_to_slider = {} # dictionary to assciate a joint name to a slider # Put kuka robot in an 'initial position' for grasping initial_positions = [ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539 ]
p.resetJointState(robot, joint_id["leftAnklePitch"], -0.6, 0.) p.resetJointState(robot, joint_id["rightAnklePitch"], -0.6, 0.) p.resetJointState(robot, joint_id["rightShoulderPitch"], 0.2, 0.) p.resetJointState(robot, joint_id["leftShoulderPitch"], 0.2, 0.) p.resetJointState(robot, joint_id["leftShoulderRoll"], -1.1, 0.) p.resetJointState(robot, joint_id["rightShoulderRoll"], 1.1, 0.) p.resetJointState(robot, joint_id["rightElbowPitch"], 1.57, 0.) p.resetJointState(robot, joint_id["leftElbowPitch"], -1.57, 0.) if __name__ == "__main__": # Environment Setup p.connect(p.GUI) p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=120, cameraPitch=-30, cameraTargetPosition=[1, 0.5, 1.5]) p.setGravity(0, 0, -9.81) p.setPhysicsEngineParameter(fixedTimeStep=DT, numSubSteps=1) if VIDEO_RECORD: if not os.path.exists('video'): os.makedirs('video') for f in os.listdir('video'): if f == 'valkyrie_kin.mp4': os.remove('video/' + f) p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "video/valkyrie_kin.mp4") # Create Robot, Ground p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) robot = p.loadURDF(cwd + "/robot_model/valkyrie/valkyrie.urdf",
"/home/h3ct0r/catkin_ws_espeleo/src/espeleo_planner/espeleo_planner/test/maps/map_01_frontiers.stl", rgbaColor=(0.6, 0.3, 0.1, 0.7)) body_id = p.createMultiBody( baseCollisionShapeIndex=col_shape_id, baseVisualShapeIndex=viz_shape_id, basePosition=(0, 0, 0), baseOrientation=(0, 0, 0, 1), ) cam_dist = 2.60 cam_yaw = 97.60 cam_pitch = -6.20 cam_pos = [-0.03, 0.24, 0] p.resetDebugVisualizerCamera(cameraDistance=cam_dist, cameraYaw=cam_yaw, cameraPitch=cam_pitch, cameraTargetPosition=cam_pos) def estimate_position(startPos, startOrientation): start_time = datetime.datetime.now() startQuatOrientation = p.getQuaternionFromEuler(startOrientation) # boxId = p.loadURDF("/home/h3ct0r/catkin_ws_espeleo/src/espeleo_planner/espeleo_planner/urdf/espeleo2_low_poly.urdf",startPos, startQuatOrientation) # linkId = 0 boxId = p.loadURDF( "/home/h3ct0r/catkin_ws_espeleo/src/espeleo_planner/espeleo_planner/urdf/espeleo2_low_poly_prismatic.urdf", startPos, startQuatOrientation) linkId = 1
import pybullet as p from time import sleep import pybullet_data physicsClient = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) p.resetDebugVisualizerCamera(3, -420, -30, [0.3, 0.9, -2]) p.setGravity(0, 0, -10) tex = p.loadTexture("uvmap.png") planeId = p.loadURDF("plane.urdf", [0, 0, -2]) boxId = p.loadURDF("cube.urdf", [0, 3, 2], useMaximalCoordinates=True) bunnyId = p.loadSoftBody("torus/torus_textured.obj", simFileName="torus.vtk", mass=3, useNeoHookean=1, NeoHookeanMu=180, NeoHookeanLambda=600, NeoHookeanDamping=0.01, collisionMargin=0.006, useSelfCollision=1, frictionCoeff=0.5, repulsionStiffness=800) p.changeVisualShape(bunnyId, -1, rgbaColor=[1, 1, 1, 1],
import os import pybullet as p import time import pybullet_data import numpy as np from gym_ergojr import get_scene from gym_ergojr.models import MODEL_PATH from gym_ergojr.utils.pybullet import DistanceBetweenObjects from gym_ergojr.utils.urdf_helper import URDF physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally p.resetDebugVisualizerCamera(cameraDistance=0.4, cameraYaw=135, cameraPitch=-35, cameraTargetPosition=[0, 0.05, 0]) p.setGravity(0, 0, -10) # good enough frequency = 100 # Hz p.setTimeStep(1 / frequency) p.setRealTimeSimulation(0) planeId = p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path()) startPos = [0, 0, 0] # xyz startOrientation = p.getQuaternionFromEuler( [0, 0, 0]) # rotated around which axis? # np.deg2rad(90) xml_path = get_scene("ergojr-pusher") robot_file = URDF(xml_path, force_recompile=True).get_path()
p.setRealTimeSimulation(1) else: fastForward = True p.setRealTimeSimulation(0) elif nkey in keys and keys[nkey] & p.KEY_WAS_TRIGGERED: p.resetSimulation() p.setRealTimeSimulation(0) shape = util.nextGen(nbobj, rad, pos1, shape) p.setRealTimeSimulation(1) maxFit = 0 continue posbody = p.getBasePositionAndOrientation(shape[0].getId())[0] curCam = p.getDebugVisualizerCamera() p.resetDebugVisualizerCamera(cameraDistance=curCam[10], cameraYaw=curCam[8], cameraPitch=curCam[9], cameraTargetPosition=posbody) curx = posbody[0] cury = posbody[1] curz = posbody[2] fitness = (np.abs(curx - basex) + np.abs(cury - basey)) if maxFit < fitness: maxFit = fitness # print(maxFit) if fastForward: p.stepSimulation() # time.sleep(1/1000)
pcl_node = PointCloudPublisher() pcl_node.run_thread() br = tf.TransformBroadcaster() ########################################################## model_dir = os.getcwd() # Hack pcid = p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0, physicsClientId=pcid) p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0, physicsClientId=pcid) p.resetDebugVisualizerCamera(cameraDistance=1.2, cameraYaw=90, cameraPitch=-15, cameraTargetPosition=[0.5, -0.35, 0.2], physicsClientId=pcid) p.resetSimulation(physicsClientId=pcid) p.setGravity(0, 0, -9.81, physicsClientId=pcid) p.setPhysicsEngineParameter(numSolverIterations=900, physicsClientId=pcid) time_step = 1. / 200. p.setTimeStep(time_step, physicsClientId=pcid) p.loadURDF(model_dir + '/models/table/table.urdf', [0.5, -0.4, -0.075], useFixedBase=True, physicsClientId=pcid) robot = p.loadURDF(model_dir + '/models/kinova/kinova.urdf', useFixedBase=True,
while True: with torch.no_grad(): value, action, _, recurrent_hidden_states = actor_critic.act( obs, recurrent_hidden_states, masks, deterministic=args.det, player_act=player_act) if num_step >= 5000: env.reset() num_step = 0 # Obser reward and next obs obs, reward, done, infos = env.step(action) player_act = None if infos[0]: if 'player_move' in infos[0].keys(): player_act = infos[0]['player_move'] num_step += 1 masks.fill_(0.0 if done else 1.0) if args.env_name.find('Bullet') > -1: if torsoId > -1: distance = 5 yaw = 0 humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId) p.resetDebugVisualizerCamera(distance, yaw, -20, humanPos)
def precompute_environment_costs(numEnvs, K, L, params, husky, sphere, GUI, seed, environment, obsUid): # Parameters numRays = params['numRays'] senseRadius = params['senseRadius'] robotRadius = params['robotRadius'] robotHeight = params['robotHeight'] thetas_nominal = params['thetas_nominal'] T_horizon = params['T_horizon'] # Fix random seed for consistency of results np.random.seed(seed) # Initialize costs for the different environments and different controllers costs = np.zeros((numEnvs, L)) for env in range(0, numEnvs): # Print if (env % 10 == 0): print(env, "out of", numEnvs) # Sample environment # heightObs = 20*robotHeight # obsUid = generate_obstacles(pybullet, heightObs, robotRadius) for l in range(0, L): # Initialize position of robot state = [0.0, 1.0, 0.0] # [x, y, theta] quat = pybullet.getQuaternionFromEuler([ 0.0, 0.0, state[2] + np.pi / 2 ]) # pi/2 since Husky visualization is rotated by pi/2 pybullet.resetBasePositionAndOrientation(husky, [state[0], state[1], 0.0], quat) pybullet.resetBasePositionAndOrientation( sphere, [state[0], state[1], robotHeight], [0, 0, 0, 1]) # Cost for this particular controller (lth controller) in this environment cost_env_l = 0.0 all_angles = [] for t in range(0, T_horizon): # Get sensor measurement y = getDistances(pybullet, state, robotHeight, numRays, senseRadius, thetas_nominal) # Compute control input # u = compute_control(y, K[l]) angle = compute_fgm_angle(y, thetas_nominal, state[2]) all_angles.append(angle) # Update state # state = robot_update_state(state, u) state, cost_env_l, done, _ = environment.step_fgm(angle) # Update position of pybullet object # quat = pybullet.getQuaternionFromEuler([0.0, 0.0, state[2]+np.pi/2]) # pi/2 since Husky visualization is rotated by pi/2 # pybullet.resetBasePositionAndOrientation(husky, [state[0], state[1], 0.0], quat) # pybullet.resetBasePositionAndOrientation(sphere, [state[0], state[1], robotHeight], [0,0,0,1]) if (GUI): pybullet.resetDebugVisualizerCamera(cameraDistance=5.0, cameraYaw=0.0, cameraPitch=-45.0, cameraTargetPosition=[ state[0], state[1], 2 * robotHeight ]) time.sleep(0.025) # Check if the robot is in collision. If so, cost = 1.0. # Get closest points. Note: Last argument is distance threshold. Since it's set to 0, the function will only return points if the distance is less than zero. So, closestPoints is non-empty iff there is a collision. # closestPoints = pybullet.getClosestPoints(sphere, obsUid, 0.0) # See if the robot is in collision. If so, cost = 1.0. ''' if closestPoints: # Check if closestPoints is non-empty cost_env_l = 1.0 break # break out of simulation for this environment ''' if cost_env_l == 1.0: break # Check that cost is between 0 and 1 (for sanity) if (cost_env_l > 1.0): raise ValueError("Cost is greater than 1!") if (cost_env_l < 0.0): raise ValueError("Cost is less than 0!") # Record cost for this environment and this controller costs[env][l] = cost_env_l # Remove obstacles pybullet.removeBody(obsUid) print("mean angle = " + str(sum(all_angles) / len(all_angles))) return costs
controlMode=p.TORQUE_CONTROL, force=-torque) # print("Torque:",torque) if (useRealTimeSimulation): p.setRealTimeSimulation(1) while 1: if (useRealTimeSimulation): p.setGravity(0, 0, -10) sleep(0.01) # Time in seconds. else: # p.setJointMotorControl2(bodyUniqueId=boxId,jointIndex=0, controlMode=p.VELOCITY_CONTROL, targetVelocity = 1, force = 500) robotPos = p.getLinkState(boxId, 3)[0] p.resetDebugVisualizerCamera(5, 0, -20, robotPos) footLength = p.getJointState(boxId, 4)[0] # print("JointInfo:",footLength) #Get foot joint info springLength = abs(float(1) - abs(footLength)) legLength = springLength + 0.5 legLengthDifference = springLength - currentSpringLength currentSpringLength = springLength if abs(legLengthDifference) < 0.000001: legLengthDifference = 0 # print("legLengthDifference:",legLengthDifference) stiffness = float(1) / springLength springHardness = 5000.0 #print("stiffness:", stiffness) legTorque = stiffness * float(abs(1 - springLength)) * springHardness legAndBodyAngle = 1.57079632679 - p.getJointState(boxId, 3)[0] #print("legTorque:", legTorque) toeLinkHeight = p.getLinkState(boxId, 5)[0][2]
def init_simulator(): global boxId, reset, low_energy_mode, high_performance_mode, terrain robot_start_pos = [0, 0, 0.42] p.connect(p.GUI) # or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally reset = p.addUserDebugParameter("reset", 1, 0, 0) low_energy_mode = p.addUserDebugParameter("low_energy_mode", 1, 0, 0) high_performance_mode = p.addUserDebugParameter("high_performance_mode", 1, 0, 0) p.setGravity(0, 0, -9.8) p.setTimeStep(1.0/freq) p.resetDebugVisualizerCamera(0.2, 45, -30, [1, -1, 1]) # p.setPhysicsEngineParameter(numSolverIterations=30) # p.setPhysicsEngineParameter(enableConeFriction=0) # planeId = p.loadURDF("plane.urdf") # p.changeDynamics(planeId, -1, lateralFriction=1.0) # boxId = p.loadURDF("/home/wgx/Workspace_Ros/src/cloudrobot/src/quadruped_robot.urdf", robot_start_pos, # useFixedBase=FixedBase) heightPerturbationRange = 0.06 numHeightfieldRows = 256 numHeightfieldColumns = 256 if terrain == "plane": planeShape = p.createCollisionShape(shapeType=p.GEOM_PLANE) ground_id = p.createMultiBody(0, planeShape) p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1]) elif terrain == "random1": heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns for j in range(int(numHeightfieldColumns/2)): for i in range(int(numHeightfieldRows/2)): height = random.uniform(0, heightPerturbationRange) heightfieldData[2*i+2*j*numHeightfieldRows] = height heightfieldData[2*i+1+2*j*numHeightfieldRows] = height heightfieldData[2*i+(2*j+1)*numHeightfieldRows] = height heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows] = height terrainShape = p.createCollisionShape(shapeType=p.GEOM_HEIGHTFIELD, meshScale=[.05, .05, 1], heightfieldTextureScaling=( numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns) ground_id = p.createMultiBody(0, terrainShape) p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1]) elif terrain == "random2": terrain_shape = p.createCollisionShape( shapeType=p.GEOM_HEIGHTFIELD, meshScale=[.5, .5, .5], fileName="heightmaps/ground0.txt", heightfieldTextureScaling=128) ground_id = p.createMultiBody(0, terrain_shape) path = rospack.get_path('quadruped_ctrl') textureId = p.loadTexture(path+"/model/grass.png") p.changeVisualShape(ground_id, -1, textureUniqueId=textureId) p.resetBasePositionAndOrientation(ground_id, [1, 0, 0.2], [0, 0, 0, 1]) elif terrain == "stairs": planeShape = p.createCollisionShape(shapeType=p.GEOM_PLANE) ground_id = p.createMultiBody(0, planeShape) # p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0.0872, 0, 0.9962]) p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1]) # many box colSphereId = p.createCollisionShape( p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.01]) colSphereId1 = p.createCollisionShape( p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.02]) colSphereId2 = p.createCollisionShape( p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.03]) colSphereId3 = p.createCollisionShape( p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.04]) # colSphereId4 = p.createCollisionShape( # p.GEOM_BOX, halfExtents=[0.03, 0.03, 0.03]) p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0]) p.changeDynamics(colSphereId, -1, lateralFriction=lateralFriction) p.createMultiBody(100, colSphereId1, basePosition=[1.2, 1.0, 0.0]) p.changeDynamics(colSphereId1, -1, lateralFriction=lateralFriction) p.createMultiBody(100, colSphereId2, basePosition=[1.4, 1.0, 0.0]) p.changeDynamics(colSphereId2, -1, lateralFriction=lateralFriction) p.createMultiBody(100, colSphereId3, basePosition=[1.6, 1.0, 0.0]) p.changeDynamics(colSphereId3, -1, lateralFriction=lateralFriction) # p.createMultiBody(10, colSphereId4, basePosition=[2.7, 1.0, 0.0]) # p.changeDynamics(colSphereId4, -1, lateralFriction=0.5) p.changeDynamics(ground_id, -1, lateralFriction=lateralFriction) boxId = p.loadURDF("mini_cheetah/mini_cheetah.urdf", robot_start_pos, useFixedBase=False) p.changeDynamics(boxId, 3, spinningFriction=spinningFriction) p.changeDynamics(boxId, 7, spinningFriction=spinningFriction) p.changeDynamics(boxId, 11, spinningFriction=spinningFriction) p.changeDynamics(boxId, 15, spinningFriction=spinningFriction) jointIds = [] for j in range(p.getNumJoints(boxId)): p.getJointInfo(boxId, j) jointIds.append(j) # slope terrain # colSphereId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.5, 0.5, 0.001]) # colSphereId1 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.5, 0.06]) # BoxId = p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0]) # BoxId = p.createMultiBody(100, colSphereId1, basePosition=[1.6, 1.0, 0.0]) # stairs # colSphereId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.02]) # colSphereId1 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.04]) # colSphereId2 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.06]) # colSphereId3 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.08]) # colSphereId4 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[1.0, 0.4, 0.1]) # BoxId = p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0]) # BoxId = p.createMultiBody(100, colSphereId1, basePosition=[1.2, 1.0, 0.0]) # BoxId = p.createMultiBody(100, colSphereId2, basePosition=[1.4, 1.0, 0.0]) # BoxId = p.createMultiBody(100, colSphereId3, basePosition=[1.6, 1.0, 0.0]) # BoxId = p.createMultiBody(100, colSphereId4, basePosition=[2.7, 1.0, 0.0]) reset_robot()
import pybullet as p import time p.connect(p.GUI) p.loadURDF("plane.urdf", [0, 0, -0.25]) minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True) print(p.getNumJoints(minitaur)) p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=23.2, cameraPitch=-6.6, cameraTargetPosition=[-0.064, .621, -0.2]) motorJointId = 1 p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0) p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1) angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0) jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0) textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2]) p.setRealTimeSimulation(1) while (1): frictionForce = p.readUserDebugParameter(jointFrictionForceSlider) angularDamping = p.readUserDebugParameter(angularDampingSlider) p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=0, force=frictionForce) p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping) time.sleep(0.01)
def __init__(self, urdfRoot=robot_data.getDataPath(), actionRepeat=4, useIK=1, isDiscrete=0, control_arm='l', useOrientation=0, rnd_obj_pose=1, renders=False, maxSteps=1000): self._control_arm = control_arm self._isDiscrete = isDiscrete self._timeStep = 1. / 240. self._useIK = 1 if self._isDiscrete else useIK self._useOrientation = useOrientation self._urdfRoot = urdfRoot self._actionRepeat = actionRepeat 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._h_table = [] self._target_dist_min = 0.05 self._rnd_obj_pose = rnd_obj_pose # Initialize PyBullet simulator self._p = p if self._renders: self._cid = p.connect(p.SHARED_MEMORY) if (self._cid < 0): self._cid = p.connect(p.GUI) p.resetDebugVisualizerCamera(2.5, 90, -60, [0.0, -0.0, -0.0]) else: self._cid = p.connect(p.DIRECT) # initialize simulation environment self.seed() self.reset() observationDim = len(self._observation) observation_high = np.array([largeValObservation] * observationDim) self.observation_space = spaces.Box(-observation_high, observation_high, dtype='float32') if (self._isDiscrete): self.action_space = spaces.Discrete( 13) if self._icub.useOrientation else spaces.Discrete(7) else: action_dim = self._icub.getActionDimension() self._action_bound = 1 action_high = np.array([self._action_bound] * action_dim) self.action_space = spaces.Box(-action_high, action_high, dtype='float32') self.viewer = None
p.changeDynamics(sphere, -1, linearDamping=0.9) p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1]) forward = 0 turn = 0 forwardVec = [2, 0, 0] cameraDistance = 1 cameraYaw = 35 cameraPitch = -35 while (1): spherePos, orn = p.getBasePositionAndOrientation(sphere) cameraTargetPosition = spherePos p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition) camInfo = p.getDebugVisualizerCamera() camForward = camInfo[5] keys = p.getKeyboardEvents() for k, v in keys.items(): if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)): turn = -0.5 if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)): turn = 0 if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)): turn = 0.5 if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)): turn = 0
def evaluate(actor_critic, ob_rms, env_name, act_func, seed, k): num_stack = 1 log_interval = 1 load_dir = './trained_models/ppo/' #sd = np.random.randint(1000, size=1) sd = [1] env = make_env(env_name, int(sd[0]), 0, './tmp/test/' + str(seed) + '_' + act_func + '_') env = DummyVecEnv([env]) #actor_critic, ob_rms = \ # torch.load(os.path.join(load_dir, 'ppo_'+env_name+'_'+act_func+ '_'+str(seed)+".pt")) if len(env.observation_space.shape) == 1: env = VecNormalize(env, ret=False) env.ob_rms = ob_rms # An ugly hack to remove updates def _obfilt(self, obs): if self.ob_rms: obs = np.clip((obs - self.ob_rms.mean) / np.sqrt(self.ob_rms.var + self.epsilon), -self.clipob, self.clipob) return obs else: return obs env._obfilt = types.MethodType(_obfilt, env) render_func = env.venv.envs[0].render else: render_func = env.envs[0].render obs_shape = env.observation_space.shape obs_shape = (obs_shape[0] * num_stack, *obs_shape[1:]) current_obs = torch.zeros(1, *obs_shape) states = torch.zeros(1, actor_critic.state_size) masks = torch.zeros(1, 1) if torch.cuda.is_available(): current_obs = current_obs.cuda() states = states.cuda() masks = masks.cuda() def update_current_obs(obs): shape_dim0 = env.observation_space.shape[0] obs = torch.from_numpy(obs).float() if num_stack > 1: current_obs[:, :-shape_dim0] = current_obs[:, shape_dim0:] current_obs[:, -shape_dim0:] = obs #render_func('human') obs = env.reset() update_current_obs(obs) if env_name.find('Bullet') > -1: import pybullet as p torsoId = -1 for i in range(p.getNumBodies()): if (p.getBodyInfo(i)[0].decode() == "torso"): torsoId = i for i in range(5000): value, action, _, states = actor_critic.act(Variable(current_obs, volatile=True), Variable(states, volatile=True), Variable(masks, volatile=True), deterministic=True) #if i % 1000 == 0: # print("STEP: ", i) states = states.data cpu_actions = action.data.squeeze(1).cpu().numpy() # Obser reward and next obs obs, reward, done, _ = env.step(cpu_actions) masks.fill_(0.0 if done else 1.0) if current_obs.dim() == 4: current_obs *= masks.unsqueeze(2).unsqueeze(2) else: current_obs *= masks update_current_obs(obs) if env_name.find('Bullet') > -1: if torsoId > -1: distance = 5 yaw = 0 humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId) p.resetDebugVisualizerCamera(distance, yaw, -20, humanPos) #render_func('human') #print(dir(env.envs[0])) process_file(env_name, act_func, sd[0], seed, k)
else: p.connect(p.DIRECT) useZUp = False useYUp = not useZUp if useYUp: p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1) from pdControllerExplicit import PDControllerExplicitMultiDof from pdControllerStable import PDControllerStableMultiDof explicitPD = PDControllerExplicitMultiDof(p) stablePD = PDControllerStableMultiDof(p) p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) import pybullet_data p.setTimeOut(10000) useMotionCapture=False useMotionCaptureReset=False#not useMotionCapture useExplicitPD = True p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=30) #p.setPhysicsEngineParameter(solverResidualThreshold=1e-30) #explicit PD control requires small timestep timeStep = 1./600. #timeStep = 1./240.
def __init__(self, block_random=0.3, camera_random=0, simple_observations=False, continuous=False, remove_height_hack=False, urdf_list=None, render_mode='GUI', num_objects=5, dv=0.06, target=False, target_filenames=None, non_target_filenames=None, num_resets_per_setup=1, render_width=128, render_height=128, downsample_width=64, downsample_height=64, test=False, allow_duplicate_objects=True, max_num_training_models=900, max_num_test_models=100): """Creates a KukaGraspingEnv. Args: block_random: How much randomness to use in positioning blocks. camera_random: How much randomness to use in positioning camera. simple_observations: If True, observations are the position and orientation of end-effector and closest block, rather than images. continuous: If True, actions are continuous, else discrete. remove_height_hack: If True and continuous is True, add a dz component to action space. urdf_list: List of objects to populate the bin with. render_mode: GUI, DIRECT, or TCP. num_objects: The number of random objects to load. dv: Velocity magnitude of cartesian dx, dy, dz actions per time step. target: If True, then we receive reward only for grasping one "target" object. target_filenames: Objects that we want to grasp. non_target_filenames: Objects that we dont want to grasp. num_resets_per_setup: How many env resets before calling setup again. render_width: Width of camera image to render with. render_height: Height of camera image to render with. downsample_width: Width of image observation. downsample_height: Height of image observation. test: If True, uses test split of objects. allow_duplicate_objects: If True, samples URDFs with replacement. max_num_training_models: The number of distinct models to choose from when selecting the num_objects placed in the tray for training. max_num_test_models: The number of distinct models to choose from when selecting the num_objects placed in the tray for testing. """ self._time_step = 1. / 200. self._max_steps = 15 # Open-source search paths. self._urdf_root = OSS_DATA_ROOT self._models_dir = os.path.join(self._urdf_root, 'random_urdfs') self._action_repeat = 200 self._env_step = 0 self._renders = render_mode in ['GUI', 'TCP'] # Size we render at. self._width = render_width self._height = render_height # Size we downsample to. self._downsample_width = downsample_width self._downsample_height = downsample_height self._target = target self._num_objects = num_objects self._dv = dv self._urdf_list = urdf_list if target_filenames: target_filenames = [ self._get_urdf_path(f) for f in target_filenames ] if non_target_filenames: non_target_filenames = [ self._get_urdf_path(f) for f in non_target_filenames ] self._object_filenames = (target_filenames or []) + (non_target_filenames or []) self._target_filenames = target_filenames or [] self._block_random = block_random self._cam_random = camera_random self._simple_obs = simple_observations self._continuous = continuous self._remove_height_hack = remove_height_hack self._resets = 0 self._num_resets_per_setup = num_resets_per_setup self._test = test self._allow_duplicate_objects = allow_duplicate_objects self._max_num_training_models = max_num_training_models self._max_num_test_models = max_num_test_models if render_mode == 'GUI': self.cid = pybullet.connect(pybullet.GUI) pybullet.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33]) elif render_mode == 'DIRECT': self.cid = pybullet.connect(pybullet.DIRECT) elif render_mode == 'TCP': self.cid = pybullet.connect(pybullet.TCP, 'localhost', 6667) self.setup() if self._continuous: self.action_space = spaces.Box(low=-1, high=1, shape=(4, )) if self._remove_height_hack: self.action_space = spaces.Box( low=-1, high=1, shape=(5, )) # dx, dy, dz, da, close else: self.action_space = spaces.Discrete(8) if self._remove_height_hack: self.action_space = spaces.Discrete(10) if self._simple_obs: # (3 pos + 4 quat) x 2 self.observation_space = spaces.Box(low=-100, high=100, shape=(14, )) else: # image (self._height, self._width, 3) x position of the gripper (3,) img_space = spaces.Box(low=0, high=255, shape=(self._downsample_height, self._downsample_width, 3)) pos_space = spaces.Box(low=-5, high=5, shape=(3, )) self.observation_space = spaces.Tuple((img_space, pos_space)) self.viewer = None
def move_and_look_at(self,i,j,k,x,y,z): lookat = [x,y,z] distance = 10 yaw = 10 p.resetDebugVisualizerCamera(distance, yaw, -20, lookat)
targetVelS = 0 width = 512 #Setting parameters for the camera image height = 512 fov = 60 aspect = width / height near = 0.02 #Near plane far = 6 kp = 0.1 kd = 0.9 change = False # Whether Lanes can be changed or not j = 0 # Counter prevOrNext = 1 # To determine whether the searcher should go to the next lane or the previous lane while searching EPSILON = 0.004 # Maximum Possible Error while turning p.resetDebugVisualizerCamera(10, 0, -89, [10,10,10]) # Loading Model for Detection detector=CustomObjectDetection() detector.setModelTypeAsYOLOv3() detector.setModelPath('detection_model-ex-005--loss-0004.657.h5') detector.setJsonPath('detection_config.json') detector.loadModel() def mapp(): ''' To Initialize the depth map. ''' img = np.zeros((400,400,3),dtype = int) for i in range(400):
erp_rb_Id= p.addUserDebugParameter("erp_rb",0,1,defaultERP) relPosTarget_rb_Id= p.addUserDebugParameter("relPosTarget_rb",-limitVal,limitVal,0) motA_lb_Id= p.addUserDebugParameter("motA_lb",-limitVal,limitVal,-legpos) motB_lb_Id= p.addUserDebugParameter("motB_lb",-limitVal,limitVal,-legpos) motC_lb_Id= p.addUserDebugParameter("motC_lb",-limitVal,limitVal,legposSleft) erp_lb_Id= p.addUserDebugParameter("erp_lb",0,1,defaultERP) relPosTarget_lb_Id= p.addUserDebugParameter("relPosTarget_lb",-limitVal,limitVal,0) camTargetPos=[0.25,0.62,-0.15] camDist = 2 camYaw = -2 camPitch=-16 p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos) c_rf = p.createConstraint(vision,knee_front_rightR_joint,vision,motor_front_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,maxForce=maxGearForce) c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce) c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce) c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
objs = p.loadSDF("botlab/botlab.sdf", globalScaling=2.0) zero = [0, 0, 0] p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) print("converting y to z axis") for o in objs: pos, orn = p.getBasePositionAndOrientation(o) y2x = p.getQuaternionFromEuler([3.14 / 2., 0, 3.14 / 2]) newpos, neworn = p.multiplyTransforms(zero, y2x, pos, orn) p.resetBasePositionAndOrientation(o, newpos, neworn) else: p.loadURDF("plane.urdf", [0, 0, -3]) p.loadURDF("boston_box.urdf", [-2, 3, -2], useFixedBase=True) p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=148, cameraPitch=-9, cameraTargetPosition=[0.36, 5.3, -0.62]) p.loadURDF("boston_box.urdf", [0, 3, -2], useFixedBase=True) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.getCameraImage(320, 200) #, renderer=p.ER_BULLET_HARDWARE_OPENGL ) t = 0 p.setRealTimeSimulation(1) while (1): p.setGravity(0, 0, -10) time.sleep(0.01) t += 0.01 keys = p.getKeyboardEvents()
import pybullet as p import time import math p.connect(p.GUI) #don't create a ground plane, to allow for gaps etc p.resetSimulation() #p.createCollisionShape(p.GEOM_PLANE) #p.createMultiBody(0,0) #p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]); p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) sphereRadius = 0.05 colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius) #a few different ways to create a mesh: vertices = [[-0.246350, -0.246483, -0.000624], [-0.151407, -0.176325, 0.172867], [-0.246350, 0.249205, -0.000624], [-0.151407, 0.129477, 0.172867], [0.249338, -0.246483, -0.000624], [0.154395, -0.176325, 0.172867], [0.249338, 0.249205, -0.000624], [0.154395, 0.129477, 0.172867]] indices = [ 0, 3, 2, 3, 6, 2, 7, 4, 6, 5, 0, 4, 6, 0, 2, 3, 5, 7, 0, 1, 3, 3, 7, 6, 7, 5, 4, 5, 1, 0, 6, 4, 0, 3, 1, 5 ] #convex mesh from obj stoneId = p.createCollisionShape(p.GEOM_MESH, vertices=vertices, indices=indices) boxHalfLength = 0.5
def update_camera(robot): base_pos = np.array(pybullet.getBasePositionAndOrientation(robot)[0]) [yaw, pitch, dist] = pybullet.getDebugVisualizerCamera()[8:11] pybullet.resetDebugVisualizerCamera(dist, yaw, pitch, base_pos) return