Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def _reset(self):
assert self.robot is not None, "Pleases introduce robot to environment before resetting."
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_KEYBOARD_SHORTCUTS, 0)
p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 1)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
self.frame = 0
self.done = 0
self.reward = 0
dump = 0
state = self.robot.reset()
self.scene.episode_restart()
return state
def set_renderer(enable):
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, int(enable), physicsClientId=CLIENT)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale)
#collisionShapeId = -1
uiCube = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,1,0], linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=linkIndices,linkJointTypes=linkJointTypes,linkJointAxis=linkJointAxis, useMaximalCoordinates=False)
p.changeVisualShape(uiCube,0,rgbaColor=[0,0,0,1])
textOrn = p.getQuaternionFromEuler([0,0,-1.5707963])
numLines = 1
lines = [-1]*numLines
p.stepSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 0)
p.configureDebugVisualizer(p.COV_ENABLE_KEYBOARD_SHORTCUTS, 0)
#p.addUserDebugLine([0,0,0],[1,0,0],[1,0,0],lineWidth=2)
#p.addUserDebugLine([0,0,0],[0,1,0],[0,1,0],lineWidth=2)
#p.addUserDebugLine([0,0,0],[0,0,1],[0,0,1],lineWidth=2)
#objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
#kitchenObj = p.loadSDF("kitchens/1.sdf")
botlabobjects = p.loadSDF("botlab/botlab.sdf", globalScaling=2, useMaximalCoordinates=useMaximalCoordinatesEnvObjects)
#botlabobjects = p.loadSDF("botlab/newsdf.sdf", globalScaling=2)
print("num botlabobjects = ", botlabobjects )
for o in botlabobjects:
pos,orn = p.getBasePositionAndOrientation(o)
zero=[0,0,0]#[0,1,2.2]
y2x = p.getQuaternionFromEuler([3.14/2.,0,-0])
## Properties already instantiated from SensorEnv/CameraEnv
# @self.robot
self.gui = config["mode"] == "gui"
self.model_id = config["model_id"]
self.timestep = config["speed"]["timestep"]
self.frame_skip = config["speed"]["frameskip"]
self.resolution = config["resolution"]
self.tracking_camera = tracking_camera
self.robot = None
target_orn, target_pos = config["target_orn"], self.config["target_pos"]
initial_orn, initial_pos = config["initial_orn"], self.config["initial_pos"]
if config["display_ui"]:
#self.physicsClientId = p.connect(p.DIRECT)
self.physicsClientId = p.connect(p.GUI, "--opengl2")
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
elif (self.gui):
self.physicsClientId = p.connect(p.GUI, "--opengl2")
else:
self.physicsClientId = p.connect(p.DIRECT)
self.camera = Camera()
self._seed()
self._cam_dist = 3
self._cam_yaw = 0
self._cam_pitch = -30
self.scene_type = scene_type
self.scene = None
import pybullet as p
import time
import math
from datetime import datetime
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-.98])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
sawyerId = p.loadURDF("sawyer_robot/sawyer_description/urdf/sawyer.urdf",[0,0,0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.resetBasePositionAndOrientation(sawyerId,[0,0,0],[0,0,0,1])
#bad, get it from name! sawyerEndEffectorIndex = 18
sawyerEndEffectorIndex = 16
numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents
jd=[0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001]
p.setGravity(0,0,0)
t=0.
prevPose=[0,0,0]
prevPose1=[0,0,0]
hasPrevPose = 0
useRealTimeSimulation = 0
p.setRealTimeSimulation(useRealTimeSimulation)
import math
import copy
import minitaur_demo
import kuka_demo
import pendulum_demo
vr_shift=[-1,-0.6,-1]
useMaximalCoordinatesEnvObjects=False #there is some issue with maximal coordinate bodies
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
sphereRadius = 0.02
meshScale = [.1,.1,.01]
shift = [0,0,0]
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
link_Masses=[1]
linkCollisionShapeIndices=[colBoxId]
linkVisualShapeIndices=[-1]
linkPositions=[[0,0,0.02]]
linkOrientations=[[0,0,0,1]]
linkInertialFramePositions=[[0,0,0]]
linkInertialFrameOrientations=[[0,0,0,1]]
linkIndices=[0]
linkJointTypes=[p.JOINT_FIXED]
linkJointAxis=[[0,0,1]]
import pybullet as p
import time,math
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
atlas = p.loadURDF("atlas/atlas_v4_with_multisense.urdf", [-2,3,-0.5])
for i in range (p.getNumJoints(atlas)):
p.setJointMotorControl2(atlas,i,p.POSITION_CONTROL,0)
print(p.getJointInfo(atlas,i))
if 1:
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:
time.sleep(0.1)
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
if (controllerId == uiControllerId):
controllerId = -1
print("Using controllerId="+str(controllerId))
once = 0
p.configureDebugVisualizer(p.COV_ENABLE_VR_RENDER_CONTROLLERS, 0)
p.configureDebugVisualizer(p.COV_ENABLE_VR_PICKING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_VR_TELEPORTING, 0)
if (once):
logId = -1#p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "userDebugItems1.json")
print("logId userdebug")
print(logId)
for i in range (numLines):
spacing = 0.01
textPos = [.1-(i+1)*spacing,.1,0.011]
text = "ABCDEFGH"*10
lines[i] = p.addUserDebugText(text,textColorRGB=[0,0,0], textSize = 0.01, textPosition = textPos,textOrientation = textOrn,parentObjectUniqueId=uiCube, parentLinkIndex = -1)
if (once):
once = 0
import pybullet as p
import time
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
p.changeVisualShape(ground,-1,rgbaColor=[1,1,1,0.8])
#p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
print("hasNumpy = ",p.isNumpyEnabled())
anymal = p.loadURDF("ANYmal/robot.urdf",[3,3,3], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES, useMaximalCoordinates=False)
p.resetSimulation()
ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
#todo, tweak this value to trade solver quality versus performance
p.setPhysicsEngineParameter(solverResidualThreshold=1e-2)