Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
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])
self.renderer = p.ER_BULLET_HARDWARE_OPENGL
self.debug = True
# Debug sliders for moving the camera
self.x_slider = p.addUserDebugParameter("x_slider", -10, 10, self.camera_target_pos[0])
self.y_slider = p.addUserDebugParameter("y_slider", -10, 10, self.camera_target_pos[1])
self.z_slider = p.addUserDebugParameter("z_slider", -10, 10, self.camera_target_pos[2])
self.dist_slider = p.addUserDebugParameter("cam_dist", 0, 10, self._cam_dist)
self.yaw_slider = p.addUserDebugParameter("cam_yaw", -180, 180, self._cam_yaw)
self.pitch_slider = p.addUserDebugParameter("cam_pitch", -180, 180, self._cam_pitch)
else:
p.connect(p.DIRECT)
global CONNECTED_TO_SIMULATOR
CONNECTED_TO_SIMULATOR = True
if self._is_discrete:
self.action_space = spaces.Discrete(N_DISCRETE_ACTIONS)
else:
if self.action_joints:
# 7 angles for the arm rotation, from -1 to 1
action_dim = 7
self._action_bound = 1
else:
# 3 direction for the arm movement, from -1 to 1
def _update_debug_panels(self):
if not (self.r_mode == "human_eye" or self.r_mode == "human_play"):
return
if not self.debug_sliders:
cameraDistSlider = p.addUserDebugParameter("Distance",0,15,4)
cameraYawSlider = p.addUserDebugParameter("Camera Yaw",-180,180,-45)
cameraPitchSlider = p.addUserDebugParameter("Camera Pitch",-90,90,-30)
self.debug_sliders = {
'dist' :cameraDistSlider,
'yaw' : cameraYawSlider,
'pitch': cameraPitchSlider
}
self.viewMatrix = p.computeViewMatrixFromYawPitchRoll([0, 0, 0], 10, 0, 90, 0, 2)
self.projMatrix = p.computeProjectionMatrix(-0.01, 0.01, -0.01, 0.01, 0.01, 128)
p.getCameraImage(256, 256, viewMatrix = self.viewMatrix, projectionMatrix = self.projMatrix)
else:
cameraDist = p.readUserDebugParameter(self.debug_sliders['dist'])
cameraYaw = p.readUserDebugParameter(self.debug_sliders['yaw'])
cameraPitch = p.readUserDebugParameter(self.debug_sliders['pitch'])
pos_xyz, quat_wxyz = self.robot.getViewPosAndOrientation()
p.getCameraImage(256, 256, viewMatrix = self.viewMatrix, projectionMatrix = self.projMatrix)
speed2=170
speed3=300
speed1=322
speed2=237
speed3=436
spurWidth=robot.W/2+20
stepLength=0
stepHeight=72
iXf=120
iXb=-132
IDspurWidth = p.addUserDebugParameter("spur width", 0, robot.W, spurWidth)
IDstepLength = p.addUserDebugParameter("step length", -70, 115, stepLength)
IDstepHeight = p.addUserDebugParameter("step height", 0, 150, stepHeight)
IDspeed1 = p.addUserDebugParameter("speed 1", 100, 1000, speed1)
IDspeed2 = p.addUserDebugParameter("speed 2", 100, 1000, speed2)
IDspeed3 = p.addUserDebugParameter("speed 3", 100, 1000, speed3)
IDixf = p.addUserDebugParameter("iXf", 0, 400, iXf)
IDixb = p.addUserDebugParameter("iXb", -200, 200, iXb)
walk=False
# Gamepad Initialisation
# Dictionary of game controller buttons we want to include.
gamepadInputs = {'ABS_X': 128, 'ABS_RZ': 127, 'ABS_Y': 126, 'ABS_Z': 125,
'BTN_TIGGER': 124, 'BTN_THUMB': 123, 'BTN_THUMB2': 122, 'BTN_TOP': 121, # right side of gamepad
'ABS_HAT0X': 120, 'ABS_HAT0Y': 119, # left
'BTN_TOP2': 118, 'BTN_BASE': 117, # left top
'BTN_PINKIE': 116, 'BTN_BASE2': 115 # right top
}
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.])
targetPosXId = p.addUserDebugParameter("targetPosX",-1,1,0.2)
targetPosYId = p.addUserDebugParameter("targetPosY",-1,1,0)
targetPosZId = p.addUserDebugParameter("targetPosZ",-1,1,-0.1)
nullSpaceId = p.addUserDebugParameter("nullSpace",0,1,1)
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
sleep(1.)
def _startDebugRoomMap(self):
cameraDistSlider = p.addUserDebugParameter("Distance",0,15,4)
cameraYawSlider = p.addUserDebugParameter("Camera Yaw",-180,180,-45)
cameraPitchSlider = p.addUserDebugParameter("Camera Pitch",-90,90,-30)
self.debug_sliders = {
'dist' :cameraDistSlider,
'yaw' : cameraYawSlider,
'pitch': cameraPitchSlider
}
# TODO: Needs refactoring
speed1=240
speed2=170
speed3=300
speed1=322
speed2=237
speed3=436
spurWidth=robot.W/2+20
stepLength=0
stepHeight=72
iXf=120
iXb=-132
IDspurWidth = p.addUserDebugParameter("spur width", 0, robot.W, spurWidth)
IDstepLength = p.addUserDebugParameter("step length", -70, 115, stepLength)
IDstepHeight = p.addUserDebugParameter("step height", 0, 150, stepHeight)
IDspeed1 = p.addUserDebugParameter("speed 1", 100, 1000, speed1)
IDspeed2 = p.addUserDebugParameter("speed 2", 100, 1000, speed2)
IDspeed3 = p.addUserDebugParameter("speed 3", 100, 1000, speed3)
IDixf = p.addUserDebugParameter("iXf", 0, 400, iXf)
IDixb = p.addUserDebugParameter("iXb", -200, 200, iXb)
walk=False
# Gamepad Initialisation
# Dictionary of game controller buttons we want to include.
gamepadInputs = {'ABS_X': 128, 'ABS_RZ': 127, 'ABS_Y': 126, 'ABS_Z': 125,
'BTN_TIGGER': 124, 'BTN_THUMB': 123, 'BTN_THUMB2': 122, 'BTN_TOP': 121, # right side of gamepad
'ABS_HAT0X': 120, 'ABS_HAT0Y': 119, # left
'BTN_TOP2': 118, 'BTN_BASE': 117, # left top
'BTN_PINKIE': 116, 'BTN_BASE2': 115 # right top
global joy_x, joy_z, joy_y, joy_rz
commandInput, commandValue = gamepad.read()
# Gamepad button command filter
if commandInput == 'ABS_X':
joy_x = commandValue
if commandInput == 'ABS_Y':
joy_y = commandValue
if commandInput == 'ABS_Z':
joy_z = commandValue
if commandInput == 'ABS_RZ':
joy_rz = commandValue
if commandInput == 'BTN_TOP2':
resetPose()
robot=spotmicroai.Robot()
IDheight = p.addUserDebugParameter("height", -40, 90, 0)
IDroll = p.addUserDebugParameter("roll", -20, 20, 0)
#robot.feetPosition(Lp)
resetPose()
# Initialise the gamepad object using the gamepad inputs Python package
gamepad = ThreadedInputs()
for gamepadInput in gamepadInputs:
gamepad.append_command(gamepadInput, gamepadInputs[gamepadInput])
gamepad.start()
while True:
handleGamepad()
height = p.readUserDebugParameter(IDheight)
roll = p.readUserDebugParameter(IDroll)
speed1=322
speed2=237
speed3=436
spurWidth=robot.W/2+20
stepLength=0
stepHeight=72
iXf=120
iXb=-132
IDspurWidth = p.addUserDebugParameter("spur width", 0, robot.W, spurWidth)
IDstepLength = p.addUserDebugParameter("step length", -70, 115, stepLength)
IDstepHeight = p.addUserDebugParameter("step height", 0, 150, stepHeight)
IDspeed1 = p.addUserDebugParameter("speed 1", 100, 1000, speed1)
IDspeed2 = p.addUserDebugParameter("speed 2", 100, 1000, speed2)
IDspeed3 = p.addUserDebugParameter("speed 3", 100, 1000, speed3)
IDixf = p.addUserDebugParameter("iXf", 0, 400, iXf)
IDixb = p.addUserDebugParameter("iXb", -200, 200, iXb)
walk=False
# Gamepad Initialisation
# Dictionary of game controller buttons we want to include.
gamepadInputs = {'ABS_X': 128, 'ABS_RZ': 127, 'ABS_Y': 126, 'ABS_Z': 125,
'BTN_TIGGER': 124, 'BTN_THUMB': 123, 'BTN_THUMB2': 122, 'BTN_TOP': 121, # right side of gamepad
'ABS_HAT0X': 120, 'ABS_HAT0Y': 119, # left
'BTN_TOP2': 118, 'BTN_BASE': 117, # left top
'BTN_PINKIE': 116, 'BTN_BASE2': 115 # right top
}
def resetPose():
# TODO: globals are bad
global joy_x, joy_z, joy_y, joy_rz,joy_z
if self.use_srl:
env_object = self if USE_GROUND_TRUTH or USE_JOINTS else None
self.srl_model = loadSRLModel(SRL_MODEL_PATH, self.cuda, STATE_DIM, env_object)
self.state_dim = self.srl_model.state_dim
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])
self.renderer = p.ER_BULLET_HARDWARE_OPENGL
self.debug = True
# Debug sliders for moving the camera
self.x_slider = p.addUserDebugParameter("x_slider", -10, 10, self.camera_target_pos[0])
self.y_slider = p.addUserDebugParameter("y_slider", -10, 10, self.camera_target_pos[1])
self.z_slider = p.addUserDebugParameter("z_slider", -10, 10, self.camera_target_pos[2])
self.dist_slider = p.addUserDebugParameter("cam_dist", 0, 10, self._cam_dist)
self.yaw_slider = p.addUserDebugParameter("cam_yaw", -180, 180, self._cam_yaw)
self.pitch_slider = p.addUserDebugParameter("cam_pitch", -180, 180, self._cam_pitch)
else:
p.connect(p.DIRECT)
global CONNECTED_TO_SIMULATOR
CONNECTED_TO_SIMULATOR = True
if self._is_discrete:
self.action_space = spaces.Discrete(N_DISCRETE_ACTIONS)
else:
if self.action_joints:
# 7 angles for the arm rotation, from -1 to 1
import pybullet as p
if __name__ == '__main__':
s = Simulator(mode='gui')
scene = BuildingScene('Ohoopee')
s.import_scene(scene)
door = InteractiveObj(os.path.join(gibson2.assets_path, 'models', 'scene_components',
'realdoor.urdf'),
scale=0.3)
s.import_interactive_object(door)
x = p.addUserDebugParameter('x', -10, 10, 0)
y = p.addUserDebugParameter('y', -10, 10, 0)
z = p.addUserDebugParameter('z', -5, 5, 0)
rotate = p.addUserDebugParameter('rotate', -np.pi, np.pi, 0)
while True:
x_pos = p.readUserDebugParameter(x)
y_pos = p.readUserDebugParameter(y)
z_pos = p.readUserDebugParameter(z)
rotate_pos = p.readUserDebugParameter(rotate)
door.set_position_rotation([x_pos, y_pos, z_pos],
p.getQuaternionFromEuler([0, 0, rotate_pos]))
s.step()