def _reset(self): if (self.physicsClientId<0): conInfo = p.getConnectionInfo() if (conInfo['isConnected']): self.ownsPhysicsClient = False self.physicsClientId = 0 else: self.ownsPhysicsClient = True self.physicsClientId = p.connect(p.SHARED_MEMORY) if (self.physicsClientId<0): if (self.isRender): self.physicsClientId = p.connect(p.GUI) else: self.physicsClientId = p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) if self.scene is None: self.scene = self.create_single_player_scene() if not self.scene.multiplayer and self.ownsPhysicsClient: self.scene.episode_restart() self.robot.scene = self.scene self.frame = 0 self.done = 0 self.reward = 0 dump = 0 s = self.robot.reset() self.potential = self.robot.calc_potential() return s
def episode_restart(self): Scene.episode_restart(self) # 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.getDataPath(),"plane_stadium.sdf") self.ground_plane_mjcf=p.loadSDF(filename) #filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") #self.ground_plane_mjcf = p.loadSDF(filename) # for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8]) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
def _reset(self): if (self.stateId>=0): #print("restoreState self.stateId:",self.stateId) p.restoreState(self.stateId) r = MJCFBaseBulletEnv._reset(self) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene( self.stadium_scene.ground_plane_mjcf) self.ground_ids = set([(self.parts[f].bodies[self.parts[f].bodyIndex], self.parts[f].bodyPartIndex) for f in self.foot_ground_object_names]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) if (self.stateId<0): self.stateId=p.saveState() #print("saving state self.stateId:",self.stateId) return r
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)
def __enter__(self): print("connecting") optionstring='--width={} --height={}'.format(pixelWidth,pixelHeight) optionstring += ' --window_backend=2 --render_device=0' print(self.connection_mode, optionstring,*self.argv) cid = pybullet.connect(self.connection_mode, options=optionstring,*self.argv) if cid < 0: raise ValueError print("connected") pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0) pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0) pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0) pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0) pybullet.resetSimulation() pybullet.loadURDF("plane.urdf",[0,0,-1]) pybullet.loadURDF("r2d2.urdf") pybullet.loadURDF("duck_vhacd.urdf") pybullet.setGravity(0,0,-10)
import pybullet as p import pybullet_data as pd import math import time import numpy as np import panda_sim_grasp as panda_sim #video requires ffmpeg available in path createVideo=False if createVideo: p.connect(p.GUI, options="--mp4=\"pybullet_grasp.mp4\", --mp4fps=240") else: p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP,1) p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.resetDebugVisualizerCamera(cameraDistance=1.3, cameraYaw=38, cameraPitch=-22, cameraTargetPosition=[0.35,-0.13,0]) p.setAdditionalSearchPath(pd.getDataPath()) timeStep=1./120.#240. p.setTimeStep(timeStep) p.setGravity(0,-9.8,0) panda = panda_sim.PandaSimAuto(p,[0,0,0]) 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()
import pybullet as p import time p.connect(p.GUI) useCollisionShapeQuery = True p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1) geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2]) baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0]) #[0,0.5,0.5,0] basePositionB = [1.5, 0, 1] obA = -1 obB = -1 obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1]) obB = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geomBox, basePosition=basePositionB, baseOrientation=baseOrientationB) lineWidth = 3 colorRGB = [1, 0, 0] lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0], lineToXYZ=[0, 0, 0], lineColorRGB=colorRGB, lineWidth=lineWidth, lifeTime=0) pitch = 0 yaw = 0 while (p.isConnected()): pitch += 0.01 if (pitch >= 3.1415 * 2.):
p.connect(p.GUI) amplitudeId= p.addUserDebugParameter("amplitude",0,3.14,0.5) timeScaleId = p.addUserDebugParameter("timeScale",0,10,1) jointTypeNames={} jointTypeNames[p.JOINT_REVOLUTE]="JOINT_REVOLUTE" jointTypeNames[p.JOINT_FIXED]="JOINT_FIXED" p.setPhysicsEngineParameter(numSolverIterations=100) p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True) #disable rendering during creation. #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1) jointNamesToIndex={} p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) for j in range(p.getNumJoints(vision)): jointInfo = p.getJointInfo(vision,j) jointInfoName = jointInfo[1].decode("utf-8") print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]]) jointNamesToIndex[jointInfoName ]=j
def build_scene( floor_textures=FLOOR_TEXTURES, wall_textures=WALL_TEXTURES, fix_light_position=False, gui=False): """ Builds the scene """ if gui: physics_client = pybullet.connect(pybullet.GUI) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0, physicsClientId=physics_client) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0, physicsClientId=physics_client) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0, physicsClientId=physics_client) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_SHADOWS, 1, physicsClientId=physics_client) else: physics_client = pybullet.connect(pybullet.DIRECT) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_SHADOWS, 1, physicsClientId=physics_client) # Add current folder path to the bullet research path pybullet.setAdditionalSearchPath( os.path.dirname(os.path.realpath(__file__))) # load the floor floorId = pybullet.loadURDF("urdf/floor.urdf", globalScaling=1, basePosition=[0., 0., 0.], baseOrientation=pybullet.getQuaternionFromEuler([0, 0, 0]), useFixedBase=1) floor_texture = pybullet.loadTexture(np.random.choice(floor_textures)) pybullet.changeVisualShape(floorId, -1, textureUniqueId=floor_texture) # load the walls wallsId = pybullet.loadURDF("urdf/walls.urdf", globalScaling=1, basePosition=[0., 0., 0.], baseOrientation=pybullet.getQuaternionFromEuler([0, 0, 0]), useFixedBase=1) wall_texture = pybullet.loadTexture(np.random.choice(wall_textures)) pybullet.changeVisualShape(wallsId, -1, textureUniqueId=wall_texture) # Light position if fix_light_position: translation = [1, -1, 1.6] else: xz = np.random.uniform(-1, 1, size=2) translation = np.array([xz[0], 1, xz[1]]) pybullet.configureDebugVisualizer( lightPosition=translation, physicsClientId=physics_client)
import pybullet as p import pybullet_data_local as pd import math import time import numpy as np from pybullet_envs_local.examples import panda_sim p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1) p.setAdditionalSearchPath(pd.getDataPath()) timeStep = 1. / 60. p.setTimeStep(timeStep) p.setGravity(0, -9.8, 0) panda = panda_sim.PandaSim(p, [0, 0, 0]) while (1): panda.step() p.stepSimulation() time.sleep(timeStep)
rcs_rear = 9 physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally p.setTimeStep(1 / timestep) p.setRealTimeSimulation(realTime) p.setGravity(0, 0, -9.81) planeId = p.loadURDF("plane.urdf") rocketStartPos = [r.randrange(-20, 20, 1), 0, 20] rocketStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) rocket = p.loadURDF("rocketwithrcs.urdf", rocketStartPos, rocketStartOrientation) p.setRealTimeSimulation(0) p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, syntheticCamera) p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, syntheticCamera) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, syntheticCamera) heightPID = PID.PID(0.5, 0, 3, 0, 1) pitchPID = PID.PID(1, 0.02, 2, -0.2, 0.2) XPID = PID.PID(0.01, 0, 0.02, -1, 1) #Debug drymass = p.getDynamicsInfo(rocket, -1)[0] + p.getDynamicsInfo( rocket, 1)[0] + p.getDynamicsInfo(rocket, 2)[0] * 4 throttleInput = p.addUserDebugParameter("Throttle Position", 0.4, 1, 0.4) nozzleInput = p.addUserDebugParameter("Nozzle Position", -0.2, 0.2, 0)
import pybullet_data p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) useMaximalCoordinates = False p.setGravity(0, 0, -10) plane = p.loadURDF("plane.urdf", [0, 0, -1], useMaximalCoordinates=useMaximalCoordinates) p.setRealTimeSimulation(0) velocity = 1 num = 40 p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) #disable this to make it faster p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0) p.setPhysicsEngineParameter(enableConeFriction=1) for i in range(num): print("progress:", i, num) x = velocity * math.sin(2. * 3.1415 * float(i) / num) y = velocity * math.cos(2. * 3.1415 * float(i) / num) print("velocity=", x, y) sphere = p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates) p.changeDynamics(sphere, -1, lateralFriction=0.02)
import pybullet as p import time import math useGui = True if (useGui): p.connect(p.GUI) else: p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) #p.loadURDF("samurai.urdf") p.loadURDF("r2d2.urdf", [3, 3, 1]) rayFrom = [] rayTo = [] rayIds = [] numRays = 1024 rayLen = 13 rayHitColor = [1, 0, 0] rayMissColor = [0, 1, 0] replaceLines = True for i in range(numRays):
import pybullet as p import pybullet_data from time import sleep, time from mathutils import Vector #https://github.com/majimboo/py-mathutils from math import sqrt, pi from numpy import cross, diagonal import numpy as np from scipy.spatial import ConvexHull, distance from pyquaternion import Quaternion #physicsClient = p.connect(p.DIRECT) #p.DIRECT for non-graphical version physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version # This is to change the visualizer window settings p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, enable=0) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, enable=0) p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, enable=0) # change init camera distance/location to view scene p.resetDebugVisualizerCamera(cameraDistance=.5, cameraYaw=45, cameraPitch=-20, cameraTargetPosition=[0.0, 0.0, 0.0]) rID = p.loadURDF("/Users/carlyndougherty/PycharmProjects/cr_grasper/RobotURDFs/barrett_hand_description/urdf/bh.urdf", basePosition=[0,0,-.1], globalScaling=1, useFixedBase=True) oID = p.loadURDF("/Users/carlyndougherty/PycharmProjects/cr_grasper/ObjectURDFs/cube/cube_small.urdf", globalScaling=1, useFixedBase=True) p.addUserDebugLine([0, 0, 0], [.2, 0, 0], [1, 0, 0], parentObjectUniqueId=rID, parentLinkIndex=-1, lineWidth=500) p.addUserDebugLine([0, 0, 0], [0, .2, 0], [0, 1, 0], parentObjectUniqueId=rID, parentLinkIndex=-1, lineWidth=500) p.addUserDebugLine([0, 0, 0], [0, 0, .2], [0, 0, 1], parentObjectUniqueId=rID, parentLinkIndex=-1, lineWidth=500) active_grasp_joints = [3,6,9] max_grasp_force = 100 target_grasp_velocity = .7
def __init__(self, robot_desc_file, ee_joint_name, ee_link_name, control_mode, base_pos, rest_arm_qpos=None, left_ee_joint_name=None, left_ee_link_name=None, left_fing_link_prefix=None, left_joint_suffix=None, left_rest_arm_qpos=None, output_noise_var=0.0, dt=1.0/240.0, kp=1.0, kd=0.1, min_z=0.0, visualize=False, cam_dist=1.5, cam_yaw=25, cam_pitch=-35, cam_target=(0.5, 0, 0), debug_level=0): assert(control_mode in ('ee_position', 'position', 'velocity', 'torque')) self.control_mode = control_mode # Variance of normal noise in the output action of the manipulator # Noise is proportional to max allowed value for each action self.output_noise_var = output_noise_var self.dt = dt; self.kp = kp; self.kd = kd; self.min_z = min_z self.debug_level = debug_level self.visualize = visualize; self.cam_dist = cam_dist self.cam_yaw = cam_yaw; self.cam_pitch = cam_pitch self.cam_target = list(cam_target) # Create and connect bullet simulation client. if visualize: self.sim = bclient.BulletClient(connection_mode=pybullet.GUI) # disable aux menus in the gui pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0) # don't render during init pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING,0) else: self.sim = bclient.BulletClient(connection_mode=pybullet.DIRECT) self._aux_sim = bclient.BulletClient( connection_mode=pybullet.DIRECT) # Load ground. self.sim.setAdditionalSearchPath(pybullet_data.getDataPath()) self.plane_id = self.sim.loadURDF("plane.urdf",[0,0,0]) self._aux_sim.setAdditionalSearchPath(pybullet_data.getDataPath()) self._aux_sim.loadURDF("plane.urdf", [0, 0, 0]) # Load robot from URDF. if not os.path.isabs(robot_desc_file): robot_description_folder = os.path.split(__file__)[0] data_path = os.path.join(robot_description_folder, 'data') robot_desc_file = os.path.join(data_path, robot_desc_file) self.info = self.load_robot( robot_desc_file, ee_joint_name, ee_link_name, left_ee_joint_name, left_ee_link_name, left_fing_link_prefix, left_joint_suffix, base_pos=base_pos, base_quat=[0,0,0,1]) # Set simulation parameters. # time step: https://github.com/bulletphysics/bullet3/issues/1460 self.sim.setRealTimeSimulation(0) self.sim.setGravity(0, 0, BulletManipulator.GRAVITY) self.sim.setPhysicsEngineParameter( fixedTimeStep=self.dt, numSolverIterations=5, numSubSteps=2) # Init aux sim. self._aux_sim.setRealTimeSimulation(0) self._aux_sim.setGravity(0, 0, BulletManipulator.GRAVITY) self._aux_sim.setPhysicsEngineParameter( fixedTimeStep=self.dt, numSolverIterations=5, numSubSteps=2) # Turn on FT sensors (for model-free gravity compensation compute). #BulletManipulatorSim.toggle_ft_sensors(self.sim, self.robots, on=True) # Reset to initial position and visualize. self.rest_qpos = (self.info.joint_maxpos+self.info.joint_minpos)/2 if rest_arm_qpos is not None: assert(len(self.info.arm_jids_lst)==len(rest_arm_qpos)) self.rest_qpos[self.info.arm_jids_lst] = rest_arm_qpos[:] if left_rest_arm_qpos is not None: assert(len(self.info.left_arm_jids_lst)==len(left_rest_arm_qpos)) self.rest_qpos[self.info.left_arm_jids_lst] = left_rest_arm_qpos[:] self.reset() if self.visualize: pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING,1) self.refresh_viz()
def remove_created_bodies(self, list_ids, n_bodies): for i in range(n_bodies): p.removeBody(list_ids[i]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
def main(): pixelWidth, pixelHeight = 1920 // 2, 1080 // 2 window_name = 'frame' scn = Scene(pixelHeight, pixelWidth, window_name) # VTK STUFF sphere = vtk.vtkSphereSource() sphere.SetRadius(0.01 / 2) mapper = vtk.vtkPolyDataMapper() if vtk.VTK_MAJOR_VERSION <= 5: mapper.SetInput(sphere.GetOutput()) else: mapper.SetInputConnection(sphere.GetOutputPort()) actors = {} poses = {} dataDir = '/home/biomed/Art/bullet3/data' #p.loadPlugin("eglRendererPlugin") p.loadURDF(os.path.join(dataDir, "plane.urdf"), [0, 0, .4]) # p.loadURDF(os.path.join(dataDir, "r2d2.urdf")) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) ob = {} count = 0 for i in range(5): for j in range(5): for k in range(5): ob[count] = p.loadURDF(os.path.join( dataDir, "sphere_1cm.urdf"), [ 0.02 * i + np.random.random_sample(1) * 0.02, 0.02 * j + np.random.random_sample(1) * 0.01, 0.02 * k ], useMaximalCoordinates=True) actors[count] = vtk.vtkActor() actors[count].SetMapper(mapper) scn.ren.AddActor(actors[count]) count = count + 1 main_start = time.time() cap = cv2.VideoCapture(0) res = set_res(cap, 1280 // 2, 960 // 2) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) count = 0 scn.start() while True: ret, frame = cap.read() for k in actors.keys(): pos, rot = p.getBasePositionAndOrientation(ob[k]) actors[k].SetPosition(pos) cv2.imshow(window_name, scn.render(count % 2)) count = count + 1 if cv2.waitKey(1) & 0xFF == ord('q'): play = False break stop = time.time() scn.stop() cap.release() cv2.destroyAllWindows() p.resetSimulation()
import pybullet as p import math import time dt = 1. / 240. p.connect(p.SHARED_MEMORY_GUI) p.loadURDF("r2d2.urdf", [0, 0, 1]) p.loadURDF("plane.urdf") p.setGravity(0, 0, -10) radius = 5 t = 0 p.configureDebugVisualizer(shadowMapWorldSize=5) p.configureDebugVisualizer(shadowMapResolution=8192) while (1): t += dt p.configureDebugVisualizer( lightPosition=[radius * math.sin(t), radius * math.cos(t), 3]) p.stepSimulation() time.sleep(dt)
def reset(self): self.step_counter = 0 p.resetSimulation() p.configureDebugVisualizer( p.COV_ENABLE_RENDERING, 0) # we will enable rendering after we loaded everything p.resetDebugVisualizerCamera(cameraDistance=3, cameraYaw=30, cameraPitch=-32, cameraTargetPosition=[0, 0, 0]) urdfRootPath = pybullet_data.getDataPath() p.setGravity(0, 0, -10) planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"), basePosition=[0, 0, 0]) rest_poses = [0, -0.2, 0, -1.2, 0, 0, 0] radius = 0.8 self.arm1Uid = p.loadURDF(os.path.join(urdfRootPath, "kuka_iiwa/model.urdf"), useFixedBase=True, basePosition=[radius, 0, 0], baseOrientation=[0, 0, 1, 0]) self.arm2Uid = p.loadURDF( os.path.join(urdfRootPath, "kuka_iiwa/model.urdf"), useFixedBase=True, basePosition=[-radius * 0.5, radius * 0.866, 0], baseOrientation=[0, 0, 0.5, -0.8660254]) self.arm3Uid = p.loadURDF( os.path.join(urdfRootPath, "kuka_iiwa/model.urdf"), useFixedBase=True, basePosition=[-radius * 0.5, -radius * 0.866, 0], baseOrientation=[0, 0, -0.5, -0.8660254]) self.nJointsPerArm = p.getNumJoints(self.arm1Uid) for i in range(7): p.resetJointState(self.arm1Uid, i, rest_poses[i]) p.resetJointState(self.arm2Uid, i, rest_poses[i]) p.resetJointState(self.arm3Uid, i, rest_poses[i]) #we need to first set force limit to zero to use torque control!! #see: https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_dynamics.py p.setJointMotorControlArray(self.arm1Uid, range(self.nJointsPerArm), p.VELOCITY_CONTROL, forces=np.zeros(self.nJointsPerArm)) p.setJointMotorControlArray(self.arm2Uid, range(self.nJointsPerArm), p.VELOCITY_CONTROL, forces=np.zeros(self.nJointsPerArm)) p.setJointMotorControlArray(self.arm3Uid, range(self.nJointsPerArm), p.VELOCITY_CONTROL, forces=np.zeros(self.nJointsPerArm)) #create a base baseUid = p.loadURDF(os.path.join(urdfRootPath, "table_square/table_square.urdf"), useFixedBase=True) #create a box state_object = [ np.random.uniform(-0.1, 0.1), np.random.uniform(-0.1, 0.1), 1.0 ] half_ext = 0.35 cuid = p.createCollisionShape(p.GEOM_BOX, halfExtents=[half_ext] * 3) vuid = p.createVisualShape(p.GEOM_BOX, halfExtents=[half_ext] * 3, rgbaColor=[0, 0, 1, 0.8]) mass_box = 0.5 self.objectUid = p.createMultiBody(mass_box, cuid, vuid, basePosition=state_object) p.changeDynamics(self.objectUid, -1, lateralFriction=2.0, spinningFriction=0.8, rollingFriction=0.8) #get observations, only joint positions are considered arm1_joint_state = getMotorJointStates(self.arm1Uid) arm2_joint_state = getMotorJointStates(self.arm2Uid) arm3_joint_state = getMotorJointStates(self.arm3Uid) arm1_joint_pos = arm1_joint_state[0] arm2_joint_pos = arm2_joint_state[0] arm3_joint_pos = arm3_joint_state[0] self.observation = np.concatenate( (arm1_joint_pos, arm2_joint_pos, arm3_joint_pos)) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) return np.array(self.observation).astype(np.float32)
def reset(self): super(FeedingEnv, self).reset() self.build_assistive_env('wheelchair') self.furniture.set_on_ground() 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]) if self.general_model: # Randomize the human body shape gender = self.np_random.choice(['male', 'female']) # body_shape = self.np_random.randn(1, self.human.num_body_shape) body_shape = self.np_random.uniform(-2, 5, (1, self.human.num_body_shape)) # human_height = self.np_random.uniform(1.59, 1.91) if gender == 'male' else self.np_random.uniform(1.47, 1.78) human_height = self.np_random.uniform(1.5, 1.9) else: gender = self.gender body_shape = self.body_shape_filename human_height = self.human_height # Randomize human pose joint_angles = [(self.human.j_left_hip_x, -90), (self.human.j_right_hip_x, -90), (self.human.j_left_knee_x, 70), (self.human.j_right_knee_x, 70), (self.human.j_left_shoulder_z, -45), (self.human.j_right_shoulder_z, 45), (self.human.j_left_elbow_y, -90), (self.human.j_right_elbow_y, 90)] # u = self.np_random.uniform # joint_angles += [(self.human.j_waist_x, u(-30, 45)), (self.human.j_waist_y, u(-45, 45)), (self.human.j_waist_z, u(-30, 30)), (self.human.j_lower_neck_x, u(-30, 30)), (self.human.j_lower_neck_y, u(-30, 30)), (self.human.j_lower_neck_z, u(-10, 10)), (self.human.j_upper_neck_x, u(-45, 45)), (self.human.j_upper_neck_y, u(-30, 30)), (self.human.j_upper_neck_z, u(-30, 30))] # joint_angles += [(self.human.j_waist_x, u(-20, 30)), (self.human.j_waist_y, u(-45, 0)), (self.human.j_waist_z, u(0, 30)), (self.human.j_lower_neck_x, u(-30, 30)), (self.human.j_lower_neck_y, u(-30, 30)), (self.human.j_lower_neck_z, u(-10, 10)), (self.human.j_upper_neck_x, u(-30, 30)), (self.human.j_upper_neck_y, u(-30, 30)), (self.human.j_upper_neck_z, u(-30, 30))] joint_angles += [ (j, self.np_random.uniform(-10, 10)) for j in (self.human.j_waist_x, self.human.j_waist_y, self.human.j_waist_z, self.human.j_lower_neck_x, self.human.j_lower_neck_y, self.human.j_lower_neck_z, self.human.j_upper_neck_x, self.human.j_upper_neck_y, self.human.j_upper_neck_z) ] self.human.init(self.directory, self.id, self.np_random, gender=gender, height=human_height, body_shape=body_shape, joint_angles=joint_angles, position=[0, 0, 0], orientation=[0, 0, 0]) # Place human in chair chair_seat_position = np.array([0, 0.05, 0.6]) self.human.set_base_pos_orient( self.furniture.get_base_pos_orient()[0] + chair_seat_position - self.human.get_vertex_positions(self.human.bottom_index), [0, 0, 0, 1]) # Create a table self.table = Furniture() self.table.init('table', self.directory, self.id, self.np_random) self.generate_target() p.resetDebugVisualizerCamera(cameraDistance=1.10, cameraYaw=40, cameraPitch=-45, cameraTargetPosition=[-0.2, 0, 0.75], physicsClientId=self.id) # 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) # 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.08] * 3) target_ee_orient = np.array( p.getQuaternionFromEuler(np.array( self.robot.toc_ee_orient_rpy[self.task]), physicsClientId=self.id)) while True: # Continually resample initial end effector poses until we find one where the robot isn't colliding with the person # target_ee_pos = np.array([-0.1, -0.6, 1.2]) + np.array([self.np_random.uniform(-0.1, 0.1), self.np_random.uniform(-0.2, 0.3), self.np_random.uniform(-0.1, 0.1)]) mouth_pos = self.human.get_pos_orient(self.human.mouth)[0] target_ee_pos = mouth_pos + np.array([ self.np_random.uniform(-0.3, 0), self.np_random.uniform(-0.6, -0.3), self.np_random.uniform(-0.3, 0.1) ]) if self.robot.mobile: # Randomize robot base pose pos = np.array(self.robot.toc_base_pos_offset[self.task]) pos[:2] += self.np_random.uniform(-0.1, 0.1, size=2) orient = np.array(self.robot.toc_ee_orient_rpy[self.task]) orient[2] += self.np_random.uniform(-np.deg2rad(30), np.deg2rad(30)) self.robot.set_base_pos_orient(pos, orient) # Randomize starting joint angles self.robot.set_joint_angles( [3], [0.75 + self.np_random.uniform(-0.1, 0.1)]) # Randomly set friction of the ground self.plane.set_frictions( self.plane.base, lateral_friction=self.np_random.uniform(0.025, 0.5), spinning_friction=0, rolling_friction=0) elif self.robot.wheelchair_mounted: # Use IK to find starting joint angles for mounted robots self.robot.ik_random_restarts(right=True, target_pos=target_ee_pos, target_orient=target_ee_orient, max_iterations=1000, max_ik_random_restarts=40, success_threshold=0.03, step_sim=False, check_env_collisions=False) else: # Use TOC with JLWKI to find an optimal base position for the robot near the person self.robot.position_robot_toc( self.task, 'right', [(target_ee_pos, target_ee_orient)], [(self.target_pos, None)], self.human, step_sim=False, check_env_collisions=False, attempts=50) # Check if the robot is colliding with the person or table self.tool.reset_pos_orient() _, _, _, _, dists_human = self.robot.get_closest_points(self.human, distance=0) _, _, _, _, dists_table = self.robot.get_closest_points(self.table, distance=0) _, _, _, _, dists_tool = self.tool.get_closest_points(self.human, distance=0) if not dists_human and not dists_table and not dists_tool: break # 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) # 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.08]*3) # Place a bowl on a table self.bowl = Furniture() self.bowl.init('bowl', self.directory, self.id, self.np_random) 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 food spoon_pos, spoon_orient = self.tool.get_base_pos_orient() food_radius = 0.005 food_mass = 0.001 batch_positions = [] for i in range(2): for j in range(2): for k in range(2): batch_positions.append( np.array([ i * 2 * food_radius - 0.005, j * 2 * food_radius, k * 2 * food_radius + 0.01 ]) + spoon_pos) self.foods = self.create_spheres(radius=food_radius, mass=food_mass, batch_positions=batch_positions, visual=False, collision=True) colors = [[60. / 256., 186. / 256., 84. / 256., 1], [244. / 256., 194. / 256., 13. / 256., 1], [219. / 256., 50. / 256., 54. / 256., 1], [72. / 256., 133. / 256., 237. / 256., 1]] for i, f in enumerate(self.foods): p.changeVisualShape(f.body, -1, rgbaColor=colors[i % len(colors)], physicsClientId=self.id) self.total_food_count = len(self.foods) self.foods_active = [f for f in self.foods] # Enable rendering p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id) # Drop food in the spoon for _ in range(25): p.stepSimulation(physicsClientId=self.id) self.init_env_variables() return self._get_obs()
def birrt_smoothing(): ################################################################ # TODO your code to implement the birrt algorithm with smoothing ################################################################ pass 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',
def reset(self): self.step_counter = 0 p.resetSimulation() p.configureDebugVisualizer( p.COV_ENABLE_RENDERING, 0) # we will enable rendering after we loaded everything p.resetDebugVisualizerCamera(cameraDistance=3, cameraYaw=30, cameraPitch=-32, cameraTargetPosition=[0, 0, 0]) urdfRootPath = pybullet_data.getDataPath() p.setGravity(0, 0, -10) planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"), basePosition=[0, 0, 0]) rest_poses = [0, 0, 0, 0, 0, 0, 0] radius = 0.5 self.agent1Uid = p.loadURDF(os.path.join(urdfRootPath, "sphere_small.urdf"), useFixedBase=False, basePosition=[radius, 0, 0.8]) self.agent2Uid = p.loadURDF( os.path.join(urdfRootPath, "sphere_small.urdf"), useFixedBase=False, basePosition=[-radius * 0.5, radius * 0.866, 0.8]) self.agent3Uid = p.loadURDF( os.path.join(urdfRootPath, "sphere_small.urdf"), useFixedBase=False, basePosition=[-radius * 0.5, -radius * 0.866, 0.8]) p.changeDynamics(self.agent1Uid, -1, lateralFriction=2.0, spinningFriction=0.8, rollingFriction=0.8) p.changeDynamics(self.agent2Uid, -1, lateralFriction=2.0, spinningFriction=0.8, rollingFriction=0.8) p.changeDynamics(self.agent3Uid, -1, lateralFriction=2.0, spinningFriction=0.8, rollingFriction=0.8) #create a base baseUid = p.loadURDF(os.path.join(urdfRootPath, "table_square/table_square.urdf"), useFixedBase=True) #create a box state_object = [ np.random.uniform(-0.1, 0.1), np.random.uniform(-0.1, 0.1), 1.0 ] half_ext = 0.35 cuid = p.createCollisionShape(p.GEOM_BOX, halfExtents=[half_ext] * 3) vuid = p.createVisualShape(p.GEOM_BOX, halfExtents=[half_ext] * 3, rgbaColor=[0, 0, 1, 0.8]) mass_box = 0.5 self.objectUid = p.createMultiBody(mass_box, cuid, vuid, basePosition=state_object) p.changeDynamics(self.objectUid, -1, lateralFriction=2.0, spinningFriction=0.8, rollingFriction=0.8) agent1_pose = np.concatenate( p.getBasePositionAndOrientation(self.agent1Uid)) agent2_pose = np.concatenate( p.getBasePositionAndOrientation(self.agent2Uid)) agent3_pose = np.concatenate( p.getBasePositionAndOrientation(self.agent3Uid)) self.observation = np.concatenate( [agent1_pose, agent2_pose, agent3_pose]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) return np.array(self.observation).astype(np.float32)
def __init__( self, use_GUI=False, # False gravity=-9.81, # -9.81 tracking_cam=True, # True timestep=1. / 60, # 1./240 decision_interval=.25, # 1.0, center_link_mass=None, # None outer_link_mass=None, # None wheel_mass=None, # None rolling_friction=None, # None lateral_fricition=None, # None spinning_fricition=None, # None friction_anchor=None, # 1 wheel_restitution=None, # 0 a_upper=7 * np.pi / 18, # np.pi/2 # limits for robot 7*np.pi/18 a_lower=-7 * np.pi / 18): # -np.pi/2 # -7*np.pi/18 self.tracking_cam = tracking_cam self.timestep = timestep self.decision_interval = decision_interval self.a_upper = a_upper self.a_lower = a_lower # initalize pybullet simulation enviroment, if use_GUI: p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) else: p.connect(p.DIRECT) p.resetSimulation() p.setAdditionalSearchPath(pybullet_data.getDataPath()) # Load "floor", otherwise unfixed based will "fall through plane" p.loadURDF("plane.urdf") p.createCollisionShape(p.GEOM_PLANE) # Define starting position and orientation for robot StartingOrientation = p.getQuaternionFromEuler([0, 0, 0]) StartingPosition = [1, 1, .03] # x,y,z # Load robot URDF file created in solidworks self.robot = p.loadURDF( r'C:\Users\Jesse\Desktop\DeepRobots\Snakebot_PyBullet\Snakebot_urdf.SLDASM\Snakebot_urdf.SLDASM\urdf\Snakebot_urdf.SLDASM.urdf', StartingPosition, StartingOrientation, useFixedBase=0) pf.color_code_joints(self.robot) # Modify default robot model parameters if center_link_mass is not None: p.changeDynamics(self.robot, linkIndex=-1, mass=center_link_mass) if outer_link_mass is not None: p.changeDynamics(self.robot, linkIndex=0, mass=outer_link_mass) p.changeDynamics(self.robot, linkIndex=3, mass=outer_link_mass) if wheel_mass is not None: pf.add_wheels_mass(self.robot, wheel_mass=wheel_mass) if rolling_friction is not None: pf.add_rolling_friction(self.robot, friction=rolling_friction) if lateral_fricition is not None: pf.add_lateral_friction(self.robot, friction=lateral_fricition) if spinning_fricition is not None: pf.add_spinning_friction(self.robot, friction=spinning_fricition) if friction_anchor is not None: pf.add_friction_anchor(self.robot, friction_anchor=friction_anchor) if wheel_restitution is not None: pf.add_wheel_restitution(self.robot, wheel_restitution=wheel_restitution) # Initalize Physics Engine parameters (always after modifiying robot parameters) p.setGravity(0, 0, gravity) p.setTimeStep(timestep) p.setRealTimeSimulation(1) # 1 = True, 0 = False p.setPhysicsEngineParameter(fixedTimeStep=timestep, numSolverIterations=1, useSplitImpulse=1, numSubSteps=1, enableConeFriction=1, collisionFilterMode=0) # Get current state of robot self.update_system_params() self._set_init_system_params() self.theta_displacement = 0 # for theta reward functions self.a1dot = 0 self.a2dot = 0 self.state = (self.theta, self.a1, self.a2)
import math cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000") p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024) p.setTimeStep(1. / 120.) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json") #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) p.loadURDF("plane100.urdf", useMaximalCoordinates=True) #disable rendering during creation. p.setPhysicsEngineParameter(contactBreakingThreshold=0.04) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) #disable tinyrenderer, software (CPU) renderer, we don't use it here p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0) shift = [0, -0.02, 0] meshScale = [0.1, 0.1, 0.1] vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000], [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
def __init__(self, gui): self.gui = gui self.paused = False self.gravity = True # config values self.ball_start_position = [0.5, 0, 0.05] self.ball_start_orientation = p.getQuaternionFromEuler((0, 0.25, 0)) self.start_position = [0, 0, 0.43] self.start_orientation = p.getQuaternionFromEuler((0, 0.25, 0)) self.initial_joints_positions = { "LAnklePitch": -30, "LAnkleRoll": 0, "LHipPitch": 30, "LHipRoll": 0, "LHipYaw": 0, "LKnee": 60, "RAnklePitch": 30, "RAnkleRoll": 0, "RHipPitch": -30, "RHipRoll": 0, "RHipYaw": 0, "RKnee": -60, "LShoulderPitch": 0, "LShoulderRoll": 0, "LElbow": 45, "RShoulderPitch": 0, "RShoulderRoll": 0, "RElbow": -45, "HeadPan": 0, "HeadTilt": 0 } # Instantiating Bullet if self.gui: self.client_id = p.connect(p.GUI) else: self.client_id = p.connect(p.DIRECT) p.setGravity(0, 0, -9.81) self.time = 0 # disable debug interface, only show robot p.configureDebugVisualizer(p.COV_ENABLE_GUI, False) # Loading floor p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally self.plane_index = p.loadURDF('plane.urdf') p.changeDynamics(self.plane_index, -1, lateralFriction=1, spinningFriction=-1, rollingFriction=-1, restitution=0.9) # Loading ball rospack = rospkg.RosPack() self.ball_index = p.loadURDF( rospack.get_path("bitbots_throw") + '/urdf/ball.urdf', self.ball_start_position, self.ball_start_orientation) # Loading robot path = rospack.get_path("wolfgang_description") flags = p.URDF_USE_INERTIA_FROM_FILE + p.URDF_USE_SELF_COLLISION + p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS self.robot_index = p.loadURDF(path + "/urdf/robot.urdf", self.start_position, self.start_orientation, flags=flags) # Engine parameters # time step should be at 240Hz (due to pyBullet documentation) self.timestep = 1 / 240 # standard parameters seem to be best. leave them like they are # p.setPhysicsEngineParameter(fixedTimeStep=self.timestep, numSubSteps=1) # no real time, as we will publish own clock self.real_time = False p.setRealTimeSimulation(0) # Retrieving joints and foot pressure sensors self.joints = {} self.pressure_sensors = {} self.links = {} # Collecting the available joints link_index = 0 for i in range(p.getNumJoints(self.robot_index)): joint_info = p.getJointInfo(self.robot_index, i) name = joint_info[1].decode('utf-8') # we can get the links by seeing where the joint is attached self.links[joint_info[12].decode('utf-8')] = link_index link_index += 1 if name in self.initial_joints_positions.keys(): # remember joint self.joints[name] = Joint(i, self.robot_index) elif name in [ "LLB", "LLF", "LRF", "LRB", "RLB", "RLF", "RRF", "RRB" ]: p.enableJointForceTorqueSensor(self.robot_index, i) self.pressure_sensors[name] = PressureSensor( name, i, self.robot_index, 10, 5) # set friction for feet self.set_foot_dynamics(0.0, 0.0, 0.0) # reset robot to initial position self.reset()
def __init__(self, gui): self.gui = gui self.paused = False self.follow_camera = False self.gravity = True # config values self.start_position = [0, 0, 0.43] self.start_orientation = p.getQuaternionFromEuler((0, 0.13, 0)) self.initial_joints_positions = {"LAnklePitch": -26, "LAnkleRoll": 0, "LHipPitch": 26, "LHipRoll": 0, "LHipYaw": 0, "LKnee": -60, "RAnklePitch": 26, "RAnkleRoll": 0, "RHipPitch": -26, "RHipRoll": 0, "RHipYaw": 0, "RKnee": 60, "LShoulderPitch": 0, "LShoulderRoll": 0, "LElbow": 45, "RShoulderPitch": 0, "RShoulderRoll": 0, "RElbow": -45, "HeadPan": 0, "HeadTilt": 0} # Instantiating Bullet if self.gui: self.client_id = p.connect(p.GUI) else: self.client_id = p.connect(p.SHARED_MEMORY_SERVER) p.setGravity(0, 0, -9.81) self.time = 0 # disable DEBUG interface, only show robot p.configureDebugVisualizer(p.COV_ENABLE_GUI, False) # set camera angle p.resetDebugVisualizerCamera(cameraDistance=0.7, cameraYaw=0, cameraPitch=-10, cameraTargetPosition=self.start_position) # Loading floor p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally self.plane_index = p.loadURDF('plane.urdf') p.changeDynamics(self.plane_index, -1, lateralFriction=100000000000000, spinningFriction=100000000000, rollingFriction=0.1, restitution=0.9) # Loading robot flags = p.URDF_USE_INERTIA_FROM_FILE self.robot_index = p.loadURDF("/urdf/robot.urdf", self.start_position, self.start_orientation, flags=flags) # Engine parameters # time step should be at 240Hz (due to pyBullet documentation) self.time_step = 1 / 240 # standard parameters seem to be best. leave them like they are # p.setPhysicsEngineParameter(fixedTimeStep=self.time_step, numSubSteps=1) # no real time, as we will publish own clock p.setRealTimeSimulation(0) # Retrieving joints and foot pressure sensors self.joints = {} self.pressure_sensors = {} self.links = {} # Collecting the available joints for i in range(p.getNumJoints(self.robot_index)): joint_info = p.getJointInfo(self.robot_index, i) name = joint_info[1].decode('utf-8') # we can get the links by seeing where the joint is attached self.links[joint_info[12].decode('utf-8')] = joint_info[16] if name in self.initial_joints_positions.keys(): # remember joint self.joints[name] = Joint(i, self.robot_index) elif name in ["LLB", "LLF", "LRF", "LRB", "RLB", "RLF", "RRF", "RRB"]: self.pressure_sensors[name] = PressureSensor(name, i, self.robot_index) # set friction for feet for link_name in self.links.keys(): if link_name in ["l_foot", "r_foot", "llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb"]: # print(self.parts[part].body_name) p.changeDynamics(self.robot_index, self.links[link_name], lateralFriction=10000000000000000, spinningFriction=1000000000000000, rollingFriction=0.1) # reset robot to initial position self.reset()
time.sleep(2) rviz = subprocess.Popen('rviz') time.sleep(2) print("Initializing node... ") rospy.init_node('bullet_point_cloud') 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=1000, physicsClientId=pcid) time_step = 1./200. p.setTimeStep(time_step, physicsClientId=pcid) p.loadURDF(model_dir + '/models/table/table.urdf', [0.5,0.0,-0.33], useFixedBase=True, physicsClientId=pcid) object_id = p.loadURDF(model_dir + '/models/object_models/urdf/bowl.urdf', useFixedBase=False, physicsClientId=pcid)
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) stoneId = p.createCollisionShape(p.GEOM_SPHERE) boxHalfLength = 0.5 boxHalfWidth = 2.5 boxHalfHeight = 0.1 segmentLength = 5 colBoxId = p.createCollisionShape( p.GEOM_BOX, halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight]) mass = 1 visualShapeId = -1 segmentStart = 0
def __init__( self, display=True, seed=None, pos_noise=0.05, rpy_noise=0.1, num_rings=5, ring_separation=5., ): # Create random number generator self.rng = np.random.default_rng(seed) # Choose the time step self.dt = 0.01 # Create empty list of drones self.drones = [] self.max_num_drones = 40 # Connect to and configure pybullet self.display = display if self.display: pybullet.connect(pybullet.GUI) pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0) else: pybullet.connect(pybullet.DIRECT) pybullet.setGravity(0, 0, -9.81) pybullet.setPhysicsEngineParameter(fixedTimeStep=self.dt, numSubSteps=4, restitutionVelocityThreshold=0.05, enableFileCaching=0) # Load plane self.plane_id = pybullet.loadURDF( os.path.join('.', 'urdf', 'plane.urdf'), basePosition=np.array([0., 0., 0.]), baseOrientation=pybullet.getQuaternionFromEuler([0., 0., 0.]), useFixedBase=1) # Load rings self.num_rings = num_rings self.ring_separation = ring_separation self.rings = [] self.add_ring([0., 0., 0.25], [0., -np.pi / 2, 0.], 2.5, 0.5, 'big-ring.urdf') for i in range(num_rings): x = (i + 1) * ring_separation y = self.rng.uniform(low=-5., high=5.) z = self.rng.uniform(low=1., high=3.) self.add_ring([x, y, z], [0., 0., 0.], 1., 0.25, 'ring.urdf') self.add_ring([(num_rings + 1) * ring_separation, 0., 0.25], [0., np.pi / 2, 0.], 2.5, 0.5, 'big-ring.urdf') # # Eliminate linear and angular damping (a poor model of drag) # pybullet.changeDynamics(self.robot_id, -1, linearDamping=0., angularDamping=0.) # Set contact parameters object_ids = [self.plane_id] for ring in self.rings: object_ids.append(ring['id']) for object_id in object_ids: pybullet.changeDynamics(object_id, -1, lateralFriction=1.0, spinningFriction=0.0, rollingFriction=0.0, restitution=0.5, contactDamping=-1, contactStiffness=-1) self.pos_noise = pos_noise self.rpy_noise = rpy_noise self.camera_drone_name = None self.camera_drone_yaw = None self.camera_viewfromstart = True self.camera() self.update_display() self.l = 0.175 self.kF = 7e-6 self.kM = 1e-7 self.min_spin_rate = 100 # <-- rad/s self.max_spin_rate = 900 # <-- rad/s self.s_min = self.min_spin_rate**2 self.s_max = self.max_spin_rate**2 # motor[0]: front (+z spin) # motor[1]: rear (+z spin) # motor[2]: left (-z spin) # motor[3]: right (-z spin) self.M = linalg.inv( np.array([[0., 0., self.kF * self.l, -self.kF * self.l], [-self.kF * self.l, self.kF * self.l, 0., 0.], [-self.kM, -self.kM, self.kM, self.kM], [self.kF, self.kF, self.kF, self.kF]]))
def wrapped_load_func(*args, **kwargs): p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, False) res = load_func(*args, **kwargs) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True) return res
def reset(self): super(ArmManipulationEnv, self).reset() self.build_assistive_env('bed', fixed_human_base=False, human_impairment='no_tremor') # Update robot and human motor gains self.robot.motor_forces = 20.0 self.human.motor_forces = 2.0 self.furniture.set_friction(self.furniture.base, friction=5) # Setup human in the air and let them settle into a resting pose on the bed joints_positions = [(self.human.j_right_shoulder_x, 30)] self.human.setup_joints(joints_positions, use_static_joints=False, reactive_force=None) self.human.set_base_pos_orient([-0.25, 0.2, 0.95], [-np.pi / 2.0, 0, 0]) p.setGravity(0, 0, -1, physicsClientId=self.id) if not self.robot.mobile: self.robot.set_gravity(0, 0, 0) # Add small variation in human joint positions motor_indices, motor_positions, motor_velocities, motor_torques = self.human.get_motor_joint_states( ) self.human.set_joint_angles( motor_indices, self.np_random.uniform(-0.1, 0.1, size=len(motor_indices))) # Let the person settle on the bed for _ in range(100): p.stepSimulation(physicsClientId=self.id) self.furniture.set_friction(self.furniture.base, friction=0.3) # Lock human joints and set velocities to 0 joints_positions = [(self.human.j_right_shoulder_x, 60), (self.human.j_right_shoulder_y, -60), (self.human.j_right_elbow, 0)] self.human.setup_joints(joints_positions, use_static_joints=True, reactive_force=0.01) self.human.set_mass(self.human.base, mass=0) self.human.set_base_velocity(linear_velocity=[0, 0, 0], angular_velocity=[0, 0, 0]) # Let the right arm fall to the ground for _ in range(100): p.stepSimulation(physicsClientId=self.id) elbow_pos = self.human.get_pos_orient(self.human.right_elbow)[0] wrist_pos = self.human.get_pos_orient(self.human.right_wrist)[0] stomach_pos = self.human.get_pos_orient(self.human.stomach)[0] waist_pos = self.human.get_pos_orient(self.human.waist)[0] # Initialize the tool in the robot's gripper self.tool_right.init(self.robot, self.task, self.directory, self.id, self.np_random, right=True, mesh_scale=[0.001] * 3) if not self.robot.has_single_arm: self.tool_left.init(self.robot, self.task, self.directory, self.id, self.np_random, right=False, mesh_scale=[0.001] * 3) target_ee_right_pos = np.array([ -1, 0.4 if self.robot.has_single_arm else -0.3, 0.8 ]) + self.np_random.uniform(-0.05, 0.05, size=3) target_ee_left_pos = np.array([-1, 0.7, 0.8]) + self.np_random.uniform( -0.05, 0.05, size=3) target_ee_orient = self.get_quaternion( self.robot.toc_ee_orient_rpy[self.task]) if self.robot.has_single_arm: base_position = self.init_robot_pose( target_ee_right_pos, target_ee_orient, [(target_ee_right_pos, target_ee_orient)], [(wrist_pos, None), (waist_pos, None), (elbow_pos, None), (stomach_pos, None)], arm='right', tools=[self.tool_right], collision_objects=[self.human, self.furniture], wheelchair_enabled=False) else: base_position = self.init_robot_pose( target_ee_right_pos, target_ee_orient, [[(target_ee_right_pos, target_ee_orient)], [(target_ee_left_pos, target_ee_orient)]], [[(wrist_pos, None), (waist_pos, None)], [(elbow_pos, None), (stomach_pos, None)]], arm=['right', 'left'], tools=[self.tool_right, self.tool_left], collision_objects=[self.human, self.furniture], wheelchair_enabled=False) if self.robot.wheelchair_mounted: # Load a nightstand in the environment for mounted arms self.nightstand = Furniture() self.nightstand.init('nightstand', self.directory, self.id, self.np_random) self.nightstand.set_base_pos_orient( np.array([-1.2, 0.7, 0]) + base_position, [0, 0, 0, 1]) # Open gripper to hold the tools self.robot.set_gripper_open_position(self.robot.right_gripper_indices, self.robot.gripper_pos[self.task], set_instantly=True) if not self.robot.has_single_arm: self.robot.set_gripper_open_position( self.robot.left_gripper_indices, self.robot.gripper_pos[self.task], set_instantly=True) p.setGravity(0, 0, -9.81, physicsClientId=self.id) self.tool_right.set_gravity(0, 0, 0) if not self.robot.has_single_arm: self.tool_left.set_gravity(0, 0, 0) # Enable rendering p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id) self.init_env_variables() return self._get_obs()
def stop_recording(self): p.stopStateLogging(self.rec_id) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
import pybullet as p import time import math p.connect(p.GUI) useMaximalCoordinates = False p.setGravity(0,0,-10) plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinates) p.setRealTimeSimulation(0) velocity = 1 num = 40 p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.setPhysicsEngineParameter(enableConeFriction=1) for i in range (num): print("progress:",i,num) x = velocity*math.sin(2.*3.1415*float(i)/num) y = velocity*math.cos(2.*3.1415*float(i)/num) print("velocity=",x,y) sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates) p.changeDynamics(sphere,-1,lateralFriction=0.02) #p.changeDynamics(sphere,-1,rollingFriction=10) p.changeDynamics(sphere,-1,linearDamping=0) p.changeDynamics(sphere,-1,angularDamping=0)
ls = p.getLinkState(jacoId, jacoEndEffectorIndex) p.setGravity(0, 0, -10) t = 0. prevPose = [0, 0, 0] prevPose1 = [0, 0, 0] hasPrevPose = 0 useNullSpace = 0 useOrientation = 1 useSimulation = 1 useRealTimeSimulation = 1 ikSolver = 0 p.setRealTimeSimulation(useRealTimeSimulation) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) trailDuration = 5 pg.init() os.environ['SDL_VIDEO_WINDOW_POS'] = "%d, %d" % (0, 0) pg.display.set_mode((500, 500)) pg.display.set_caption("Control Interface") runUI = UI(logData) # positions and orientations of end-effector pos = list(ls[4]) orn = list(ls[5]) # grab the initial positions of the end-effector for autonomous grasp # grab the initial euler orientations of the end-effector for autonomous grasp X_0 = pos[0]
# Panda config panda_end_effector_index = 11 #8 panda_num_dofs = 7 base_orientation = [-0.707107, 0.0, 0.0, 0.707107] base_offset = [0, 0, 0] # Base position offset [0,0,0] (meters) # General PyBullet config gui = True use_simulation = False # Startup PyBullet if gui: p.connect(p.GUI) # p.connect(p.GUI, options="--opengl2") p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1) p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 0) p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1) else: p.connect(p.DIRECT) p.setAdditionalSearchPath(pd.getDataPath()) flags = p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES # Set simulation time step timeStep = 1. / 60. p.setTimeStep(timeStep) # Set gravity p.setGravity(0, -9.81, 0)
import pybullet as p import json import time useGUI = True if useGUI: p.connect(p.GUI) 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())
import pybullet as p import numpy as np sim = 4 physicsClient = p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) # p.setPhysicsEngineParameter(numSolverIterations=10) # p.setPhysicsEngineParameter(contactBreakingThreshold=0.001) time_step = 0.005 p.setTimeStep(time_step) p.setGravity(0, 0, -9.81) # p.setGravity(0, 0, 0) plane = p.loadURDF('plane.urdf') pos1 = np.array([-0.5, 0, 0.5]) pos2 = np.array([0.5, 0, 0.5]) pos3 = np.array([0.5, 0, 1.5]) pos4 = np.array([-0.5, 0, 1.5]) gripper_visual = p.createVisualShape(p.GEOM_SPHERE, radius=0.02, rgbaColor=[1, 0, 0, 1]) gripper_collision = p.createCollisionShape(p.GEOM_SPHERE, radius=0.0001) # gripper_collision = -1 mass = 1.0 if sim in [3, 4] else 0.0 cloth_attachment1 = p.createMultiBody( baseMass=mass,
import pybullet as p import time import math cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000") p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024) p.setTimeStep(1. / 120.) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json") #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) p.loadURDF("plane100.urdf", useMaximalCoordinates=True) #disable rendering during creation. p.setPhysicsEngineParameter(contactBreakingThreshold=0.04) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) #disable tinyrenderer, software (CPU) renderer, we don't use it here p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0) shift = [0, -0.02, 0] meshScale = [0.1, 0.1, 0.1] vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000], [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000], [1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
def __init__(self, time_step: SimulationTime, gui: bool = False): self._gui = gui if self._INSTANCE is not None and p.isConnected(self._INSTANCE._physics_client_id): # Instance could already exist when running test, just to be sure resetting it here. p.resetSimulation(self._INSTANCE._physics_client_id) self._physics_client_id = -1 if gui: self._physics_client_id = p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_KEYBOARD_SHORTCUTS, 0) p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) else: self._physics_client_id = p.connect(p.DIRECT) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setTimeStep(time_step.seconds) p.setGravity(0, 0, -GRAVITY)
import pybullet as p import time #p.connect(p.UDP,"192.168.86.100") cid = p.connect(p.SHARED_MEMORY) if (cid<0): p.connect(p.GUI) p.resetSimulation() #disable rendering during loading makes it much faster p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) 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]) pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000]) print ("pr2_cid") print (pr2_cid) objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)] kuka = objects[0] jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ] for jointIndex in range (p.getNumJoints(kuka)): p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
def reset(self): if self._physics_client_id < 0: if self._renders: self._p = bc.BulletClient(connection_mode=pb.GUI) self._p.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) else: self._p = bc.BulletClient() # self._p.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) self._physics_client_id = self._p._client p = self._p p.resetSimulation() # load ramp # load soccerbot # p.loadURDF("/home/shahryar/catkin_ws/src/soccer_ws/soccer_description/models/soccerbot_stl.urdf", # p.loadURDF("/home/shahryar/PycharmProjects/DeepRL/gym-soccerbot/gym_soccerbot/soccer_description/models/soccerbot_stl.urdf", # "/home/shahryar/PycharmProjects/DeepRL/gym-soccerbot/gym_soccerbot/soccer_description/models/soccerbot_stl.urdf", urdfBotPath = gym_soccerbot.getDataPath() self.soccerbotUid = p.loadURDF(os.path.join(urdfBotPath, "soccer_description/models/soccerbot_stl.urdf"), useFixedBase=False, useMaximalCoordinates=False, basePosition=[0, 0, self.STANDING_HEIGHT], baseOrientation=[0., 0., 0., 1.], flags=pb.URDF_USE_INERTIA_FROM_FILE | pb.URDF_USE_MATERIAL_COLORS_FROM_MTL | pb.URDF_USE_SELF_COLLISION | pb.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS ) # |p.URDF_USE_SELF_COLLISION|p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) urdfRootPath = pybullet_data.getDataPath() self.planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane_implicit.urdf"), useMaximalCoordinates=True, basePosition=[0, 0, 0]) # TODO change dynamics ... # for i in range(p.getNumJoints(bodyUniqueId=self.soccerbotUid)): # print(p.getJointInfo(bodyUniqueId=self.soccerbotUid, jointIndex=i)[1]) p.changeDynamics(self.planeUid, linkIndex=-1, lateralFriction=0.9, spinningFriction=0.9, rollingFriction=0) # p.changeDynamics(self.soccerbotUid, linkIndex=Links.IMU, mass=0.01, localInertiaDiagonal=[7e-7, 7e-7, 7e-7]) # p.changeDynamics(self.soccerbotUid, linkIndex=Links.IMU, mass=0., localInertiaDiagonal=[0., 0., 0.]) ''' p.changeDynamics(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6, frictionAnchor=1, lateralFriction=1, rollingFriction=1, spinningFriction=1) p.changeDynamics(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6, frictionAnchor=1, lateralFriction=1, rollingFriction=1, spinningFriction=1) ''' # TODO change timestep ... self.timeStep = 1./240 p.setTimeStep(self.timeStep) p.setGravity(0, 0, -9.81) self.gravity = [0, 0, -9.81] p.setRealTimeSimulation(0) # To manually step simulation later p = self._p # TODO reset joint state p.resetBasePositionAndOrientation(self.soccerbotUid, [0, 0, self.STANDING_HEIGHT], [0., 0., 0., 1.]) p.resetBaseVelocity(self.soccerbotUid, [0, 0, 0], [0, 0, 0]) self.prev_lin_vel = np.array([0, 0, 0]) #p.resetJointStates(self.soccerbotUid, list(range(0, 18, 1)), 0) #pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) #standing_poses = [0] * (self.JOINT_DIM + 2) standing_poses = self._standing_poses() for i in range(self.JOINT_DIM + 2): p.resetJointState(self.soccerbotUid, i, standing_poses[i]) # WARM UP SIMULATION for _ in range(self.WARM_UP_STEPS): p.stepSimulation() # TODO set state??? # TODO get observation feet = self._feet() imu = self._imu() joint_states = p.getJointStates(self.soccerbotUid, list(range(0, self.JOINT_DIM, 1))) joints_pos = np.array([state[0] for state in joint_states], dtype=np.float32) start_pos = np.array([0, 0, self.STANDING_HEIGHT], dtype=np.float32) observation = np.concatenate((joints_pos, imu, start_pos, feet)) #pb.resetSimulation() """ From core.py in gym: Returns: observation (object): the initial observation. """ if self._renders: pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) return observation