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 __init__(self, connection_mode=pybullet.DIRECT, options=""): """Create a simulation and connect to it.""" self._client = pybullet.connect(pybullet.SHARED_MEMORY) if (self._client < 0): print("options=", options) self._client = pybullet.connect(connection_mode, options=options) self._shapes = {}
def __init__(self, urdfRoot=pybullet_data.getDataPath(), actionRepeat=50, isEnableSelfCollision=True, renders=True): print("init") self._timeStep = 0.01 self._urdfRoot = urdfRoot self._actionRepeat = actionRepeat self._isEnableSelfCollision = isEnableSelfCollision self._observation = [] self._envStepCounter = 0 self._renders = renders self._p = p if self._renders: p.connect(p.GUI) else: p.connect(p.DIRECT) 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(9) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None
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): # initialize random seed np.random.seed(int(time.time())) cid = p.connect(p.SHARED_MEMORY) # only show graphics if the browser is already running.... self.visualize = (cid >= 0) if cid < 0: cid = p.connect(p.DIRECT) # If no shared memory browser is active, we switch to headless mode (DIRECT is much faster)
def test(args): p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) fileName = os.path.join("mjcf", args.mjcf) print("fileName") print(fileName) p.loadMJCF(fileName) while (1): p.stepSimulation() p.getCameraImage(320,240) time.sleep(0.01)
def main(unused_args): timeStep = 0.01 c = p.connect(p.SHARED_MEMORY) if (c<0): c = p.connect(p.GUI) params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539] evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase' energy_weight = 0.01 finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep) print(finalReturn)
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 testJacobian(self): import pybullet as p clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.DIRECT) time_step = 0.001 gravity_constant = -9.81 urdfs = [ "TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf", "kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf" ] for urdf in urdfs: p.resetSimulation() p.setTimeStep(time_step) p.setGravity(0.0, 0.0, gravity_constant) robotId = p.loadURDF(urdf, useFixedBase=True) p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1]) numJoints = p.getNumJoints(robotId) endEffectorIndex = numJoints - 1 # Set a joint target for the position control and step the sim. self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)]) p.stepSimulation() # Get the joint and link state directly from Bullet. mpos, mvel, mtorq = self.getMotorJointStates(robotId) result = p.getLinkState(robotId, endEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1) link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result # Get the Jacobians for the CoM of the end-effector link. # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn. # The localPosition is always defined in terms of the link frame coordinates. zero_vec = [0.0] * len(mpos) jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec, zero_vec) assert (allclose(dot(jac_t, mvel), link_vt)) assert (allclose(dot(jac_r, mvel), link_vr)) p.disconnect()
def test_rolling_friction(self): import pybullet as p p.connect(p.DIRECT) p.loadURDF("plane.urdf") sphere = p.loadURDF("sphere2.urdf", [0, 0, 1]) p.resetBaseVelocity(sphere, linearVelocity=[1, 0, 0]) p.changeDynamics(sphere, -1, linearDamping=0, angularDamping=0) #p.changeDynamics(sphere,-1,rollingFriction=0) p.setGravity(0, 0, -10) for i in range(1000): p.stepSimulation() vel = p.getBaseVelocity(sphere) self.assertLess(vel[0][0], 1e-10) self.assertLess(vel[0][1], 1e-10) self.assertLess(vel[0][2], 1e-10) self.assertLess(vel[1][0], 1e-10) self.assertLess(vel[1][1], 1e-10) self.assertLess(vel[1][2], 1e-10) p.disconnect()
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 __init__(self, connection_mode=None): """Creates a Bullet client and connects to a simulation. Args: connection_mode: `None` connects to an existing simulation or, if fails, creates a new headless simulation, `pybullet.GUI` creates a new simulation with a GUI, `pybullet.DIRECT` creates a headless simulation, `pybullet.SHARED_MEMORY` connects to an existing simulation. """ self._shapes = {} if connection_mode is None: self._client = pybullet.connect(pybullet.SHARED_MEMORY) if self._client >= 0: return else: connection_mode = pybullet.DIRECT self._client = pybullet.connect(connection_mode)
def __init__(self): # start the bullet physics server p.connect(p.GUI) # p.connect(p.DIRECT) observation_high = np.array([ np.finfo(np.float32).max, np.finfo(np.float32).max, np.finfo(np.float32).max, np.finfo(np.float32).max]) action_high = np.array([0.1]) self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(-observation_high, observation_high) self.theta_threshold_radians = 1 self.x_threshold = 2.4 self._seed() # self.reset() self.viewer = None self._configure()
def main(unused_args): timeStep = 0.01 c = p.connect(p.SHARED_MEMORY) if (c<0): c = p.connect(p.GUI) p.resetSimulation() p.setTimeStep(timeStep) p.loadURDF("plane.urdf") p.setGravity(0,0,-10) minitaur = Minitaur() amplitude = 0.24795664427 speed = 0.2860877729434 for i in range(1000): a1 = math.sin(i*speed)*amplitude+1.57 a2 = math.sin(i*speed+3.14)*amplitude+1.57 joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57] minitaur.applyAction(joint_values) p.stepSimulation() # print(minitaur.getBasePosition()) time.sleep(timeStep) final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition())) print(final_distance)
def __init__(self, renders=True): # start the bullet physics server self._renders = renders if (renders): p.connect(p.GUI) else: p.connect(p.DIRECT) self.theta_threshold_radians = 12 * 2 * math.pi / 360 self.x_threshold = 0.4 #2.4 high = np.array([ self.x_threshold * 2, np.finfo(np.float32).max, self.theta_threshold_radians * 2, np.finfo(np.float32).max]) self.force_mag = 10 self.action_space = spaces.Discrete(2) self.observation_space = spaces.Box(-high, high, dtype=np.float32) self._seed() # self.reset() self.viewer = None self._configure()
def __init__(self, renders=True): # start the bullet physics server self._renders = renders if (renders): p.connect(p.GUI) else: p.connect(p.DIRECT) observation_high = np.array([ np.finfo(np.float32).max, np.finfo(np.float32).max, np.finfo(np.float32).max, np.finfo(np.float32).max]) action_high = np.array([0.1]) self.action_space = spaces.Discrete(9) self.observation_space = spaces.Box(-observation_high, observation_high) self.theta_threshold_radians = 1 self.x_threshold = 2.4 self._seed() # self.reset() self.viewer = None self._configure()
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 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, 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
import pybullet as p import time from pybullet_utils import urdfEditor ########################################## org2 = p.connect(p.DIRECT) org = p.connect(p.SHARED_MEMORY) if (org<0): org = p.connect(p.DIRECT) gui = p.connect(p.GUI) p.resetSimulation(physicsClientId=org) #door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf mb = p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER, physicsClientId=org) for i in range(p.getNumJoints(mb,physicsClientId=org)): p.setJointMotorControl2(mb,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org) #print("numJoints:",p.getNumJoints(mb,physicsClientId=org)) #print("base name:",p.getBodyInfo(mb,physicsClientId=org)) #for i in range(p.getNumJoints(mb,physicsClientId=org)):
import pybullet as p import numpy as np import time from AgentReacher import Reacher np.set_printoptions(suppress=True) np.set_printoptions(formatter={'float': '{: 0.2f}'.format}) timeStep = 0.016 c = p.connect(p.GUI) p.resetSimulation() p.setTimeStep(timeStep) p.setGravity(0, 0, -9.8) p.setRealTimeSimulation(1) reacher = Reacher() while True: for i in range(50): action = np.random.rand(2) * 2 - 1 obs, reward, done, info = reacher.step(action) print(obs) time.sleep(timeStep) reacher.reset()
def resetRobot(RoboBoi): p.resetBasePositionAndOrientation(RoboBoi, originalPos, originalOrientation) for joint in range(p.getNumJoints(RoboBoi)): p.resetJointState(RoboBoi, joint, 0) p.setJointMotorControl2(RoboBoi, joint, p.VELOCITY_CONTROL, 0) #p.setJointMotorControl2(RoboBoi,joint,p.TORQUE_CONTROL,0) p.setJointMotorControl2(RoboBoi, 9, p.POSITION_CONTROL, -0.4) p.setJointMotorControl2(RoboBoi, 6, p.POSITION_CONTROL, -0.4) return ## Loading physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf") RobotStartPosition = [0, 0, 0.75] RobotStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) RoboBoi = p.loadURDF("Robot.urdf", RobotStartPosition, RobotStartOrientation) # Create Parent-Child Map parent_child_map = numpy.array([[0, 1], [1, 2], [0, 3], [3, 4], [4, 5], \ [3, 6], [6, 7], [7, 8], [0, 9], [9, 10], [10, 11]]) ## Print all the links for i in range(p.getNumJoints(RoboBoi)): link = p.getJointInfo(RoboBoi, i) print(link)
def __init__(self, animate=False, max_steps=1000): self.animate = animate self.max_steps = max_steps # Initialize simulation if (animate): self.client_ID = p.connect(p.GUI) else: self.client_ID = p.connect(p.DIRECT) assert self.client_ID != -1, "Physics client failed to connect" # Set simulation world params p.setGravity(0, 0, -9.8) p.setTimeStep(1. / 120.) # Input and output dimensions defined in the environment self.obs_dim = 12 self.act_dim = 2 # sum rewards self.lastrew = 0 # parametrs for the track self.ctr = 0 self.iteration = 100 self.first = True self.X = [] self.Y = [] self.index = 0 self.amount = 0 self.reset_index = 0 env_width = 100 env_height = 100 # generating track heightfieldData = [0] * (env_height * env_width) # if we would like to generate new track every time ''' datasetx, datasety, x1,y1,cx,cy = getdataset() datasetx = np.int64(datasetx) datasety = np.int64(datasety) x1 = np.int64(x1) y1 = np.int64(y1)''' # for training we used pregenerated tracks if self.animate: self.tracking = 5 else: self.tracking = 1 exx = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'outx0.npy') exy = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'outy0.npy') inx = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'inx0.npy') iny = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'iny0.npy') checkx = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'checkpointsx0.npy') checky = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'checkpointsy0.npy') datasetx = np.load(exx) datasety = np.load(exy) x1 = np.load(inx) y1 = np.load(iny) cx = np.load(checkx) cy = np.load(checky) first = True for i in range(len(datasetx)): x = (datasetx[i]) y = (datasety[i]) xnew = x1[i] ynew = y1[i] if not first and xold == x and yold == y: continue else: heightfieldData.pop(env_height * x + y) heightfieldData.insert(env_height * x + y, 1) heightfieldData.pop(env_height * xnew + ynew) heightfieldData.insert(env_height * xnew + ynew, 1) xold = x yold = y first = False self.X = cy self.Y = cx self.amount = len(cx) terrainShape = p.createCollisionShape( shapeType=p.GEOM_HEIGHTFIELD, meshScale=[1, 1, 1], heightfieldTextureScaling=(env_width - 1) / 2, heightfieldData=heightfieldData, numHeightfieldRows=env_height, numHeightfieldColumns=env_width, physicsClientId=self.client_ID) mass = 0 terrain = p.createMultiBody(mass, terrainShape) p.resetBasePositionAndOrientation(terrain, [49.5, 49.5, 0], [0, 0, 0, 1]) # generating car in the right angle if (self.X[self.reset_index + 5] - self.X[self.reset_index]) < 0 and ( (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 2 and (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > -2): angle = 100 elif (self.X[self.reset_index + 5] - self.X[self.reset_index]) > 0 and ( (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 2 and (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > -2): angle = 0 elif (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 0 and ( (self.X[self.reset_index + 5] - self.X[self.reset_index]) < 2 and (self.X[self.reset_index + 5] - self.X[self.reset_index]) > -2): angle = -1 elif (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > 0 and ( (self.X[self.reset_index + 5] - self.X[self.reset_index]) < 2 and (self.X[self.reset_index + 5] - self.X[self.reset_index]) > -2): angle = 1 elif (self.X[self.reset_index + 5] - self.X[self.reset_index]) > 0 and (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > 0: angle = 0.5 elif (self.X[self.reset_index + 5] - self.X[self.reset_index]) > 0 and (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 0: angle = -0.5 elif (self.X[self.reset_index + 5] - self.X[self.reset_index]) < 0 and (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 0: angle = -2 elif (self.X[self.reset_index + 5] - self.X[self.reset_index]) < 0 and (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > 0: angle = 2 else: angle = 1 self.car = p.loadURDF("f10_racecar/racecar_differential.urdf", [self.X[0], self.Y[0], .3], [0, 0, angle, 1]) p.resetDebugVisualizerCamera( cameraDistance=10, cameraYaw=0, cameraPitch=270, cameraTargetPosition=[self.X[0], self.Y[0], 0]) # Input and output dimensions defined in the environment for wheel in range(p.getNumJoints(self.car)): p.setJointMotorControl2(self.car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0) p.changeDynamics(self.car, wheel, mass=1, lateralFriction=1.0) self.wheels = [8, 15] # spojuje klouby přes ramena dohromady a tím vytváří celek auta self.c = p.createConstraint(self.car, 9, self.car, 11, jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(self.c, gearRatio=1, maxForce=10000) self.c = p.createConstraint(self.car, 10, self.car, 13, jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(self.c, gearRatio=-1, maxForce=10000) self.c = p.createConstraint(self.car, 9, self.car, 13, jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(self.c, gearRatio=-1, maxForce=10000) self.c = p.createConstraint(self.car, 16, self.car, 18, jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(self.c, gearRatio=1, maxForce=10000) self.c = p.createConstraint(self.car, 16, self.car, 19, jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(self.c, gearRatio=-1, maxForce=10000) self.c = p.createConstraint(self.car, 17, self.car, 19, jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(self.c, gearRatio=-1, maxForce=10000) self.c = p.createConstraint(self.car, 1, self.car, 18, jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(self.c, gearRatio=-1, gearAuxLink=15, maxForce=10000) self.c = p.createConstraint(self.car, 3, self.car, 19, jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(self.c, gearRatio=-1, gearAuxLink=15, maxForce=10000) self.steering = [0, 2] # Limits of our joints. When using the * (multiply) operation on a list, it repeats the list that many times self.joints_rads_low = -1.57 self.joints_rads_high = 4.71 self.joints_rads_diff = self.joints_rads_high - self.joints_rads_low # self.Velocity = 1 self.stepCtr = 0 self.hokuyo_joint = 4 # Lidar rays initialisation self.replaceLines = True self.numRays = 10 self.rayFrom = [] self.rayTo = [] self.rayIds = [] self.rayHitColor = [1, 0, 0] self.rayMissColor = [0, 1, 0] self.rayLen = 7 self.rayStartLen = 0.25 for i in range(self.numRays): self.rayFrom.append([ self.rayStartLen * math.sin(-0.5 * 0.25 * 2. * math.pi + 0.75 * 2. * math.pi * float(i) / self.numRays), self.rayStartLen * math.cos(-0.5 * 0.25 * 2. * math.pi + 0.75 * 2. * math.pi * float(i) / self.numRays), 0 ]) self.rayTo.append([ self.rayLen * math.sin(-0.5 * 0.25 * 2. * math.pi + 0.75 * 2. * math.pi * float(i) / self.numRays), self.rayLen * math.cos(-0.5 * 0.25 * 2. * math.pi + 0.75 * 2. * math.pi * float(i) / self.numRays), 0 ]) if (self.replaceLines): self.rayIds.append( p.addUserDebugLine(self.rayFrom[i], self.rayTo[i], self.rayMissColor, parentObjectUniqueId=self.car, parentLinkIndex=self.hokuyo_joint)) else: self.rayIds.append(-1)
def __init__(self, connection="GUI", prev_epoch=0, seed=None): np.random.seed(seed) if connection == "GUI": self.pybullet = p.connect(p.GUI) else: self.pybullet = p.connect(p.DIRECT) p.setTimeStep(1.0 / SIMULATIONFREQUENCY) p.setGravity(0, 0, -9.81) self.ground = p.loadURDF( "/home/shawn/Documents/ANYmal/assets/plane.urdf", [0, 0, 0]) self.anymal = p.loadURDF( "/home/shawn/Documents/DRL/JueyingProURDF/urdf/jueying.urdf", [0.0, 0.0, 1.0]) self.action_space = spaces.Box(low=-2 * np.pi * np.ones(12), high=2 * np.pi * np.ones(12)) self.actionTime = 3.0 self.t = 0.0 self.epoch = prev_epoch # The number of DRL epochs that have been done self.lastTime = 0.0 # Used to add noise self.torqueNoise = 0.0 self.positionNoise = 0.0 maxPosition = np.pi * np.ones(12) maxVelocity = 2 * np.ones(12) maxPositionHistory = np.pi * np.ones(24) maxVelocityHistory = 2 * np.ones(24) maxBasePositionAndOrientation = np.ones(7) maxBasePosition = np.ones(3) maxBaseOrientationVector = np.ones(3) maxBaseVelocity = 1 * np.ones(3) maxBaseAngularVelocity = 2 * np.pi * np.ones(3) maxAcceleration = np.inf * np.ones(12) observationUpperBound = np.concatenate([ maxPosition, maxVelocity, maxBasePositionAndOrientation, maxBaseOrientationVector, maxBaseVelocity, maxBaseAngularVelocity, maxAcceleration ]) stateUpperBound = np.concatenate([ maxBaseOrientationVector, maxBaseVelocity, maxBaseAngularVelocity, maxPosition, maxVelocity, maxPositionHistory, maxVelocityHistory, maxPosition ]) # self.observation_space = spaces.Box( # low=-observationUpperBound, # high=observationUpperBound # ) self.observation_space = spaces.Box(low=-stateUpperBound, high=stateUpperBound) self.history_buffer = dict( ) # used to storage the joint state history and the last action self.history_buffer['last_action'] = np.nan * np.ones(12) self.history_buffer['joint_state'] = { 'time': [], 'position': [], 'velocity': [], 'position_error': [] } self.buffer_time_length = 0.03 p.setJointMotorControlArray(self.anymal, [joint.value for joint in Joints], p.POSITION_CONTROL, targetPositions=np.zeros(12), forces=np.zeros(12))
j, p.POSITION_CONTROL, targetPosition, targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=[0, 0, 0]) if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): p.setJointMotorControl2(body_id, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0) physicsClient = p.connect(p.GUI) body_id = p.loadURDF("../data/man_on_qolo/man_x_partitioned_on_qolo.urdf", flags=p.URDF_MAINTAIN_LINK_ORDER) prepare_man_on_qolo(body_id) disable_body_motors(body_id) colBoxId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[5, 5, 5]) box_ID = p.createMultiBody(0, colBoxId, -1, [0, 0, -5], [0, 0, 0, 1]) #time.sleep(1) for i in range(5000): p.stepSimulation() time.sleep(1 / 240.0)
def start(): p.connect(p.GUI) p.setAdditionalSearchPath(pd.getDataPath()) plane = p.loadURDF("plane.urdf") p.setGravity(0, 0, -9.8) p.setTimeStep(1. / 500) # p.setDefaultContactERP(0) # urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS urdfFlags = p.URDF_USE_SELF_COLLISION quadruped = p.loadURDF("laikago/laikago_toes.urdf", [0, 0, .5], [0, 0.5, 0.5, 0], flags=urdfFlags, useFixedBase=False) # enable collision between lower legs for j in range(p.getNumJoints(quadruped)): print(p.getJointInfo(quadruped, j)) # 2,5,8 and 11 are the lower legs lower_legs = [2, 5, 8, 11] for l0 in lower_legs: for l1 in lower_legs: if (l1 > l0): enableCollision = 1 print("collision for pair", l0, l1, p.getJointInfo(quadruped, l0)[12], p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision) p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision) jointIds = [] paramIds = [] jointOffsets = [] jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1] jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] for i in range(4): jointOffsets.append(0) jointOffsets.append(-0.7) jointOffsets.append(0.7) maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20) for j in range(p.getNumJoints(quadruped)): p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0) info = p.getJointInfo(quadruped, j) # print(info) jointName = info[1] jointType = info[2] if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): jointIds.append(j) p.getCameraImage(480, 320) p.setRealTimeSimulation(0) joints = [] with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream: for line in filestream: print("line=", line) maxForce = p.readUserDebugParameter(maxForceId) currentline = line.split(",") # print (currentline) # print("-----") frame = currentline[0] t = currentline[1] # print("frame[",frame,"]") joints = currentline[2:14] # print("joints=",joints) for j in range(12): targetPos = float(joints[j]) p.setJointMotorControl2(quadruped, jointIds[j], p.POSITION_CONTROL, jointDirections[j] * targetPos + jointOffsets[j], force=maxForce) p.stepSimulation() for lower_leg in lower_legs: # print("points for ", quadruped, " link: ", lower_leg) pts = p.getContactPoints(quadruped, -1, lower_leg) # print("num points=",len(pts)) # for pt in pts: # print(pt[9]) time.sleep(1. / 500.) index = 0 for j in range(p.getNumJoints(quadruped)): p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0) info = p.getJointInfo(quadruped, j) js = p.getJointState(quadruped, j) # print(info) jointName = info[1] jointType = info[2] if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): paramIds.append( p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, (js[0] - jointOffsets[index]) / jointDirections[index])) index = index + 1 p.setRealTimeSimulation(1) while (1): for i in range(len(paramIds)): c = paramIds[i] targetPos = p.readUserDebugParameter(c) maxForce = p.readUserDebugParameter(maxForceId) p.setJointMotorControl2(quadruped, jointIds[i], p.POSITION_CONTROL, jointDirections[i] * targetPos + jointOffsets[i], force=maxForce)
def start_client(self): self.client = p.connect(p.GUI) if (self._gui is True) else p.connect(p.DIRECT) p.setGravity(0, 0, -1 * self.G) p.setTimeStep(self.physics_sample_time) p.setRealTimeSimulation(self._real_time)
#get robot models #kdl ok, snake_tree = kdlurdf.treeFromFile(path_to_urdf) snake_chain = snake_tree.getChain(root, tip) #rbdl snake_rbdl = rbdl.loadModel(path_to_urdf) #u2c snake = u2c.URDFparser() snake.from_file(path_to_urdf) #pybullet sim = pb.connect(pb.DIRECT) snake_pb = pb.loadURDF(path_to_urdf, useFixedBase=True, flags=pb.URDF_USE_INERTIA_FROM_FILE) #joint info jointlist, names, q_max, q_min = snake.get_joint_info(root, tip) n_joints = snake.get_n_joints(root, tip) #declarations #kdl q_kdl = kdl.JntArray(n_joints) qdot_kdl = kdl.JntArray(n_joints) gravity_kdl = kdl.Vector() gravity_kdl[2] = -9.81
contours = cv2.findContours(mask, cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)[0] for c in contours: if(cv2.contourArea(c)<500): (x,y),r = cv2.minEnclosingCircle(c) r = int(r) if r!=0: center = (int(x),int(y)) cv2.circle(image,center,r,(0,255,0),2) cv2.imshow("Image", image) cv2.waitKey(0) cv2.destroyAllWindows() return center # setting physics client PhysicsClent=p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # setting gravity p.setGravity(0,0,-10) # setting plane plane=p.loadURDF("plane.urdf") # setting table table_pos=[0,0,0] table_ori=p.getQuaternionFromEuler([0,0,0]) table=p.loadURDF("table(1).urdf",table_pos,table_ori) # setting puck puck_pos=[-0.6,0.0,0.3]
import numpy as np import pybullet as p import pybullet_data import sys, os, time script_path = os.path.dirname(os.path.realpath(__file__)) os.sys.path.append(os.path.realpath(script_path + '/../')) from src.buff_digit import Buff_digit if __name__ == '__main__': client = p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.setRealTimeSimulation(1) p.setGravity(0, 0, -9.81) p.setPhysicsEngineParameter(enableConeFriction=0) p.setAdditionalSearchPath('../models') floor = p.loadURDF('floor/floor.urdf', useFixedBase=True) robot = Buff_digit(client) time.sleep(5) #SE2 poses (x, y, theta) pose1 = [0.2, -0.25, 0] pose2 = [-1.0, -1.0, -1.57] pose3 = [-4.0, 0.5, -3.1415] pose4 = [-1.0, 1.0, 1.57] base_limits = ((-2.5, -2.5), (2.5, 2.5)) for i in range(10): robot.plan_and_drive_to_pose(pose1, base_limits, obstacles=[]) time.sleep(1) robot.plan_and_drive_to_pose(pose2, base_limits, obstacles=[])
def initPyBullet(self): p.connect(p.DIRECT) # GUI or DIRECT p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf")
def __init__(self, config, use_visualizer=False, name=None): """Initialize. Args: config (dict): YAML config dict containing specifications. use_visualizer (bool): Whether or not to use visualizer. Note:: Pybullet only allows for one simulation to be connected to GUI. It is up to the user to manage this. name (str): Name of the world. (for multiple worlds) Returns: None Example: BulletWorld('cfg/my_config.yaml', True, 'MyBulletWorld') Notes: The world_factory is the recommended way for creating an appropriate instance of world according to the config parameters. Upon initialization the world automatically creates instances of BulletRobotInterface, BulletSensorInterface and BulletObjectInterface according to the config dictionary. """ # Get configuration parameters self.config = config # Connect to appropriate pybullet channel based on use_visualizer flag self.use_visualizer = use_visualizer if self.use_visualizer: print( "==================USING_VISUALIZER!!!!!=====================") self._physics_id = pybullet.connect(pybullet.GUI) else: print("==============NOT USING VISUALIZER!!!!=================") self._physics_id = pybullet.connect(pybullet.DIRECT) logging.info("New PhysicsID: " + str(self._physics_id)) self._time_step = self.config['sim_params']['time_step'] self.set_pb_physics() # check if config has camera. self.has_camera = False if 'sensor' in self.config: if 'camera' in self.config['sensor']: self.has_camera = True self.has_object = False # check if config has objects. if 'object' in self.config: self.has_object = True # Create an arena to load robot and objects self.arena = BulletArena(self.config, self._physics_id, has_camera=self.has_camera) self.robot_interface = BulletRobotInterface.create( config=self.config, physics_id=self._physics_id, arm_id=self.arena.arm_id, controlType=self.config['world']['controlType']) self.control_freq = self.config['control_freq'] if self.has_camera: self.camera_interface = BulletCameraInterface( physics_id=self._physics_id, image_height=self.config['sensor']['camera']['image'] ['height'], image_width=self.config['sensor']['camera']['image']['width'], near=self.config['sensor']['camera']['intrinsics'] ['near_plane'], far=self.config['sensor']['camera']['intrinsics']['far_plane'], fov=self.config['sensor']['camera']['intrinsics']['fov'], cameraEyePosition=self.config['sensor']['camera']['extrinsics'] ['eye_position'], cameraTargetPosition=self.config['sensor']['camera'] ['extrinsics']['target_position'], cameraUpVector=self.config['sensor']['camera']['extrinsics'] ['up_vector']) if self.has_object: self._load_object_interfaces() self.name = name # self.ctrl_steps_per_action = int((self.config['control_freq'] / float(self.config['policy_freq'] * self.config['sim_params']['time_step']))) self.ctrl_steps_per_action = int( (self.config['control_freq'] / float(self.config['policy_freq']))) self.is_sim = True self.step_counter = 0 self.ee_list = []
def main(): parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter, ) parser.add_argument("model", help="model file in a log dir") parser.add_argument("--gpu", type=int, default=0, help="gpu id") parser.add_argument("--save", action="store_true", help="save") args = parser.parse_args() args_file = path.Path(args.model).parent / "args" with open(args_file) as f: args_data = json.load(f) pprint.pprint(args_data) if args.gpu >= 0: chainer.cuda.get_device_from_id(args.gpu).use() model = contrib.models.Model( n_fg_class=len(args_data["class_names"][1:]), pretrained_resnet18=args_data["pretrained_resnet18"], centerize_pcd=args_data["centerize_pcd"], # with_occupancy=args_data['with_occupancy'], # loss=args_data['loss'], # loss_scale=args_data['loss_scale'], ) if args.gpu >= 0: model.to_gpu() print(f"==> Loading trained model: {args.model}") chainer.serializers.load_npz(args.model, model) print("==> Done model loading") split = "val" dataset = morefusion.datasets.YCBVideoRGBDPoseEstimationDataset( split=split) dataset_reindexed = morefusion.datasets.YCBVideoRGBDPoseEstimationDatasetReIndexed( # NOQA split=split, class_ids=args_data["class_ids"], ) # transform = Transform( # train=False, # with_occupancy=args_data['with_occupancy'], # ) pprint.pprint(args.__dict__) # ------------------------------------------------------------------------- depth2rgb = imgviz.Depth2RGB() for index in range(len(dataset)): frame = dataset.get_frame(index) image_id = dataset._ids[index] indices = dataset_reindexed.get_indices_from_image_id(image_id) examples = dataset_reindexed[indices] examples = [transform(example) for example in examples] if not examples: continue inputs = chainer.dataset.concat_examples(examples, device=args.gpu) with chainer.no_backprop_mode() and chainer.using_config( "train", False): quaternion_pred, translation_pred, confidence_pred = model.predict( class_id=inputs["class_id"], rgb=inputs["rgb"], pcd=inputs["pcd"], # pitch=inputs.get('pitch'), # origin=inputs.get('origin'), # grid_nontarget_empty=inputs.get('grid_nontarget_empty'), ) indices = model.xp.argmax(confidence_pred.array, axis=1) quaternion_pred = quaternion_pred[ model.xp.arange(quaternion_pred.shape[0]), indices] translation_pred = translation_pred[ model.xp.arange(translation_pred.shape[0]), indices] reporter = chainer.Reporter() reporter.add_observer("main", model) observation = {} with reporter.scope(observation): model.evaluate( class_id=inputs["class_id"], quaternion_true=inputs["quaternion_true"], translation_true=inputs["translation_true"], quaternion_pred=quaternion_pred, translation_pred=translation_pred, ) # TODO(wkentaro) observation_new = {} for k, v in observation.items(): if re.match(r"main/add_or_add_s/[0-9]+/.+", k): k_new = "/".join(k.split("/")[:-1]) observation_new[k_new] = v observation = observation_new print(f"[{index:08d}] {observation}") # --------------------------------------------------------------------- K = frame["intrinsic_matrix"] height, width = frame["rgb"].shape[:2] fovy = trimesh.scene.Camera(resolution=(width, height), focal=(K[0, 0], K[1, 1])).fov[1] batch_size = len(inputs["class_id"]) class_ids = cuda.to_cpu(inputs["class_id"]) quaternion_pred = cuda.to_cpu(quaternion_pred.array) translation_pred = cuda.to_cpu(translation_pred.array) quaternion_true = cuda.to_cpu(inputs["quaternion_true"]) translation_true = cuda.to_cpu(inputs["translation_true"]) Ts_pred = [] Ts_true = [] for i in range(batch_size): # T_cad2cam T_pred = tf.quaternion_matrix(quaternion_pred[i]) T_pred[:3, 3] = translation_pred[i] T_true = tf.quaternion_matrix(quaternion_true[i]) T_true[:3, 3] = translation_true[i] Ts_pred.append(T_pred) Ts_true.append(T_true) Ts = dict(true=Ts_true, pred=Ts_pred) vizs = [] depth_viz = depth2rgb(frame["depth"]) for which in ["true", "pred"]: pybullet.connect(pybullet.DIRECT) for i, T in enumerate(Ts[which]): cad_file = morefusion.datasets.YCBVideoModels().get_cad_file( class_id=class_ids[i]) morefusion.extra.pybullet.add_model( cad_file, position=tf.translation_from_matrix(T), orientation=tf.quaternion_from_matrix(T)[[1, 2, 3, 0]], ) ( rgb_rend, depth_rend, segm_rend, ) = morefusion.extra.pybullet.render_camera( np.eye(4), fovy, height, width) pybullet.disconnect() segm_rend = imgviz.label2rgb(segm_rend + 1, img=frame["rgb"], alpha=0.7) depth_rend = depth2rgb(depth_rend) rgb_input = imgviz.tile(cuda.to_cpu(inputs["rgb"]), border=(255, 255, 255)) viz = imgviz.tile( [ frame["rgb"], depth_viz, rgb_input, segm_rend, rgb_rend, depth_rend, ], (1, 6), border=(255, 255, 255), ) viz = imgviz.resize(viz, width=1800) if which == "pred": text = [] for class_id in np.unique(class_ids): add = observation[f"main/add_or_add_s/{class_id:04d}"] text.append(f"[{which}] [{class_id:04d}]: " f"add/add_s={add * 100:.1f}cm") text = "\n".join(text) else: text = f"[{which}]" viz = imgviz.draw.text_in_rectangle( viz, loc="lt", text=text, size=20, background=(0, 255, 0), color=(0, 0, 0), ) if which == "true": viz = imgviz.draw.text_in_rectangle( viz, loc="rt", text="singleview_pcd", size=20, background=(255, 0, 0), color=(0, 0, 0), ) vizs.append(viz) viz = imgviz.tile(vizs, (2, 1), border=(255, 255, 255)) if args.save: out_file = path.Path(args.model).parent / f"video/{index:08d}.jpg" out_file.parent.makedirs_p() imgviz.io.imsave(out_file, viz) yield viz
def euler_to_quaternion(r): (roll, pitch, yaw) = (r[0], r[1], r[2]) qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos( roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2) qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin( roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin( roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin( roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2) return [qw, qx, qy, qz] p.connect(p.DIRECT) file_path = cwd_path + "/gym_raisim_towr/envs/dll_rsc/rsc/anymal/anymal.urdf" p.setGravity(0, 0, -10) geo_anymal = p.loadURDF(file_path) ''' * ik function with an error of +- 0.14 radians * since pybullet has a recursice IK solver we r running stepSimulation 20 times to get the angles in the final target ee location. (values 20 was consistent to reach the target location any where inside the kinematic box of towr) ''' def pybullet_ik(base_pos, base_quat, ee1, ee2, ee3, ee4): base_pos = list(base_pos)
# print (', '.join(row)) # cnt += 1 # if first: # first = False # continue # print(row, cnt) # trajectory_point = {"timestep": row[0], "x": float(row[5]), "y": float(row[6]), "z": float(row[7])} # if row[4] == "hand": # print ("found hand") # trajectory.append(trajectory_point) # return trajectory show_gt = args.gt # using this parameter, the robot shows the behaviour with ground-truth input data this can be seen as current best case scenario clid = p.connect(p.SHARED_MEMORY) if (clid<0): p.connect(p.GUI) video_filename = os.path.basename(args.csvfile)[:-4]+".mp4" # print("Log video to: ") p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) # p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, video_filename) p.loadURDF(config["environment"]["model"],[0,0,0], globalScaling=0.01) jd = config["jd"] ll = config["ll"] ul = config["ul"] jr = config["jr"]
#!/usr/bin/env python import time import math import pybullet as p import pybullet_data CLIENT = p.connect(p.GUI, options='--width=1024 --height=786') def setup_world(): """ function to init the world """ p.resetSimulation() p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) p.setRealTimeSimulation(0, CLIENT) def main(): """ main function """ setup_world() p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -10) # urdf_fn = '/Users/krishneelchaudhary/Downloads/bullet3/data/kuka_lwr/kuka.urdf' urdf_fn = '/Users/krishneelchaudhary/Downloads/bullet3/examples/pybullet/gym/pybullet_data/kuka_iiwa/model_for_sdf.urdf'
def initPhysicsClient(): physicsClient = p.connect(p.GUI) p.setGravity(0, 0, -9.81) p.setPhysicsEngineParameter(fixedTimeStep=dt, numSubSteps=1) return physicsClient
"Expected torque vector of " "length {}, got {}".format(num_joints, len(torque))) def multiplyJacobian(robot, jacobian, vector): result = [0.0, 0.0, 0.0] i = 0 for c in range(len(vector)): if p.getJointInfo(robot, c)[3] > -1: for r in range(3): result[r] += jacobian[r][i] * vector[c] i += 1 return result clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.DIRECT) p.setAdditionalSearchPath(pybullet_data.getDataPath()) time_step = 0.001 gravity_constant = -9.81 p.resetSimulation() p.setTimeStep(time_step) p.setGravity(0.0, 0.0, gravity_constant) p.loadURDF("plane.urdf", [0, 0, -0.3]) kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True) #kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
def __init__(self): # initial ros node rospy.init_node('collision_check', anonymous = True) # get joint state data self.jointStateAddress = "/j2n6s300_driver/out/joint_state" self.joint_pos = np.array([0.0] * 6) self.kinova_joint_pos = np.array([0.0] * 6) rospy.Subscriber(self.jointStateAddress, JointState, self.jointFeedback) # get fridge location relative to the kinova base link # try: self._listener = tf.TransformListener() self._tfBuffer = tf2_ros.Buffer() self._listener2 = tf2_ros.TransformListener(self._tfBuffer) startTime = time.time() while True: if time.time() - startTime > 10: fridge_pos = [0.1568307262, 0.626700624618, 0.308028843394] print "Can not find tfBuffer. Use pre-defined value" break try: T_base2tag = self._tfBuffer.lookup_transform('j2n6s300_link_base', 'marker_frame', rospy.Time()) fridge_pos = np.array([ T_base2tag.transform.translation.x, T_base2tag.transform.translation.y, T_base2tag.transform.translation.z ]) print "Get correct tf from base to marker" break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as err: # print err pass # except: # tag_pos = [0.1568307262, 0.626700624618, 0.308028843394] rospy.sleep(1) self.physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally p.setGravity(0,0,-10) kinovaStartPos = [0,0,0.675] kinovaStartOri = p.getQuaternionFromEuler([0,0,0]) # two objects self.planeId = p.loadURDF("plane.urdf") # environment the file is in bullet default folder self.kinova = p.loadURDF("data/model2.urdf", kinovaStartPos, kinovaStartOri, useFixedBase=True) self.box = p.loadURDF("data/table.urdf", [0,1.4,0.38], kinovaStartOri, useFixedBase=True) # self.box = p.loadURDF("data/cube.urdf", [0,1.3,0],cubeStartOrientation) # self.box = p.loadSDF("data/ketchen.model") # self.box = p.loadURDF("data/kitchen_object.urdf") # self.fridge = p.loadURDF("kitchen/fridge.urdf", [0,1.7,1],cubeStartOrientation) # self.joint_pos = np.array([0., pi/2, pi/2, pi/2, pi/2, 0]) self.fridge = p.loadURDF("kitchen/fridge.urdf", [fridge_pos[0],fridge_pos[1]+0.075,fridge_pos[2]-0.18+0.675],kinovaStartOri, useFixedBase=True ) self.kinovaEndEffector = 7 self.jacobian = None self.closestPts = None p.setRealTimeSimulation(1)
import pybullet as p import time import pkgutil egl = pkgutil.get_loader('eglRenderer') p.connect(p.DIRECT) plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin") print("plugin=",plugin) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.setGravity(0,0,-10) p.loadURDF("plane.urdf",[0,0,-1]) p.loadURDF("r2d2.urdf") pixelWidth = 320 pixelHeight = 220 camTargetPos = [0,0,0] camDistance = 4 pitch = -10.0 roll=0 upAxisIndex = 2 while (p.isConnected()): for yaw in range (0,360,10) : start = time.time() p.stepSimulation()
#script to control a simulated robot hand using a VR glove #see https://twitter.com/erwincoumans/status/821953216271106048 #and https://www.youtube.com/watch?v=I6s37aBXbV8 #vr glove was custom build using Spectra Symbolflex sensors (4.5") #inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle #with BLE Link to receive serial (for wireless bluetooth serial) import serial import time import pybullet as p import pybullet_data #first try to connect to shared memory (VR), if it fails use local GUI c = p.connect(p.SHARED_MEMORY) if (c < 0): c = p.connect(p.GUI) #p.resetSimulation() p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -10) print(c) if (c < 0): p.connect(p.GUI) #load the MuJoCo MJCF hand objects = p.loadMJCF("MPL/mpl2.xml") hand = objects[0] ho = p.getQuaternionFromEuler([0, 3.14, 0]) hand_cid = p.createConstraint(hand, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.1, 0, 0], [0.500000, 0.300006, 0.700000], ho)
import pybullet as p from time import sleep import matplotlib.pyplot as plt import numpy as np physicsClient = p.connect(p.GUI) p.setGravity(0,0,0) bearStartPos1 = [-3.3,0,0] bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0]) bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1) bearStartPos2 = [0,0,0] bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0]) bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2) textureId = p.loadTexture("checker_grid.jpg") #p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId) #p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId) useRealTimeSimulation = 1 if (useRealTimeSimulation): p.setRealTimeSimulation(1) while 1: if (useRealTimeSimulation): camera = p.getDebugVisualizerCamera() viewMat = camera[2] projMat = camera[3] #An example of setting the view matrix for the projective texture. #viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
def __init_bullet(self): # Simulation world setup self._physics_client = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -9.81) id_plane = p.loadURDF("plane.urdf")
import pybullet as p p.connect(p.SHARED_MEMORY) objects = [ p.loadURDF("plane.urdf", 0.000000, 0.000000, -.300000, 0.000000, 0.000000, 0.000000, 1.000000) ] objects = [ p.loadURDF("quadruped/minitaur.urdf", [-0.000046, -0.000068, 0.200774], [-0.000701, 0.000387, -0.000252, 1.000000], useFixedBase=False) ] ob = objects[0] jointPositions = [ 0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000, -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193, 0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517 ] for ji in range(p.getNumJoints(ob)): p.resetJointState(ob, ji, jointPositions[ji]) p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0) cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000], [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000], [0.000000, 0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 0.000000, 1.000000]) p.changeConstraint(cid0, maxForce=500.000000) cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000], [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000], [0.000000, 0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 0.000000, 1.000000]) p.changeConstraint(cid1, maxForce=500.000000) cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
import pybullet as p usePort = True if (usePort): id = p.connect(p.GRPC,"localhost:12345") else: id = p.connect(p.GRPC,"localhost") print("id=",id) if (id<0): print("Cannot connect to GRPC server") exit(0) print ("Connected to GRPC") r2d2 = p.loadURDF("r2d2.urdf") print("numJoints = ", p.getNumJoints(r2d2))
def __init__(self, urdf_root=pybullet_data.getDataPath(), renders=False, is_discrete=True, name="mobile_robot", max_distance=1.6, shape_reward=False, record_data=False, srl_model="raw_pixels", random_target=False, force_down=True, state_dim=-1, learn_states=False, verbose=False, save_path='srl_zoo/data/', env_rank=0, srl_pipe=None, fpv=False, **_): super(MobileRobotGymEnv, self).__init__(srl_model=srl_model, relative_pos=RELATIVE_POS, env_rank=env_rank, srl_pipe=srl_pipe) self._timestep = 1. / 240. self._urdf_root = urdf_root self._observation = [] self._env_step_counter = 0 self._renders = renders self._width = RENDER_WIDTH self._height = RENDER_HEIGHT self._cam_dist = 4.4 self._cam_yaw = 90 self._cam_pitch = -90 self._cam_roll = 0 self._max_distance = max_distance self._shape_reward = shape_reward self._random_target = random_target self._force_down = force_down self.camera_target_pos = (2, 2, 0) self._is_discrete = is_discrete self.terminated = False self.renderer = p.ER_TINY_RENDERER self.debug = False self.n_contacts = 0 self.state_dim = state_dim self.relative_pos = RELATIVE_POS self.cuda = th.cuda.is_available() self.saver = None self.verbose = verbose self.max_steps = MAX_STEPS self.robot_pos = np.zeros(3) self.robot_uid = None self.target_pos = np.zeros(3) self.target_uid = None # Boundaries of the square env self._min_x, self._max_x = 0, 4 self._min_y, self._max_y = 0, 4 self.has_bumped = False # Used for handling collisions self.collision_margin = 0.1 self.walls = None self.fpv = fpv self.srl_model = srl_model if record_data: self.saver = EpisodeSaver(name, max_distance, state_dim, globals_=getGlobals(), relative_pos=RELATIVE_POS, learn_states=learn_states, path=save_path) if self._renders: client_id = p.connect(p.SHARED_MEMORY) if client_id < 0: p.connect(p.GUI) p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33]) 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: self.action_space = spaces.Box(low=-1, high=1, shape=(2,), dtype=np.float32) if self.srl_model == "ground_truth": self.state_dim = self.getGroundTruthDim() 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: self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(self.state_dim,), dtype=np.float32)
import pybullet as p import pybullet_data as pd import time import math usePhysX = True useMaximalCoordinates = True if usePhysX: p.connect(p.PhysX,options="--numCores=8 --solver=pgs") p.loadPlugin("eglRendererPlugin") else: p.connect(p.GUI) p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024) p.setPhysicsEngineParameter(contactBreakingThreshold=0.01) p.setAdditionalSearchPath(pd.getDataPath()) #Always make ground plane maximal coordinates, to avoid performance drop in PhysX #See https://github.com/NVIDIAGameWorks/PhysX/issues/71 p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json") jran = 50 iran = 100 num=64 radius=0.1 numDominoes=0
def __init__(self, useFixedBase=False, useStairs=True, resetFunc=None): # Simulation Configuration self.useMaximalCoordinates = False self.resetFunc = resetFunc self.useRealTime = True self.debugLidar = False self.rotateCamera = False self.debug = False self.fixedTimeStep = 1. / 550 self.numSolverIterations = 200 self.useFixedBase = useFixedBase self.useStairs = useStairs self.init_oritentation = p.getQuaternionFromEuler([0, 0, 90.0]) self.init_position = [0, 0, 0.3] self.reflection = False self.state = RobotState.OFF # Parameters for Servos - still wrong self.kp = 0.045 #0.012 self.kd = .4 #.2 self.maxForce = 25.0 self.angles = [0.0, 0.0, 0.0, \ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.physId = p.connect(p.SHARED_MEMORY) if (self.physId < 0): p.connect(p.GUI) self.angle = 90 self.oldTextId = 0 self.textId = 0 self.oldDebugInfo = [] self.rot = (0, 0, 0) self.pos = (0, 0, 0) self.t = 0 if self.reflection: p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_reflection, 1) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 1) self.IDkp = p.addUserDebugParameter("Kp", 0, 0.05, self.kp) # 0.05 self.IDkd = p.addUserDebugParameter("Kd", 0, 1, self.kd) # 0.5 self.IDmaxForce = p.addUserDebugParameter("MaxForce", 0, 50, 12.5) p.setRealTimeSimulation(self.useRealTime) self.quadruped = self.loadModels() self.createEnv() self.changeDynamics(self.quadruped) self.jointNameToId = self.getJointNames(self.quadruped) replaceLines = True self.numRays = 360 self.rayFrom = [] self.rayTo = [] self.rayIds = [] self.rayHitColor = [1, 0, 0] self.rayMissColor = [0, 1, 0] rayLen = 12 rayStartLen = 0.12 for i in range(self.numRays): #rayFrom.append([0,0,0]) h = 0.045 self.rayFrom.append([ rayStartLen * math.sin(math.pi * 2 * float(i) / self.numRays), rayStartLen * math.cos(math.pi * 2 * float(i) / self.numRays), h ]) self.rayTo.append([ rayLen * math.sin(math.pi * 2 * float(i) / self.numRays), rayLen * math.cos(math.pi * 2 * float(i) / self.numRays), h ]) if self.debugLidar: if (replaceLines): self.rayIds.append( p.addUserDebugLine( self.rayFrom[i], self.rayTo[i], self.rayMissColor, parentObjectUniqueId=self.quadruped, parentLinkIndex=self.jointNameToId["base_lidar"])) else: self.rayIds.append(-1) self.L = 140 self.W = 75 + 5 + 40 self.dirs = [[-1, 1, 1], [1, 1, 1], [-1, 1, 1], [1, 1, 1]] self.roll = 0 self.Lp = np.array([[120, -100, self.W / 2, 1], [120, -100, -self.W / 2, 1], [-50, -100, self.W / 2, 1], [-50, -100, -self.W / 2, 1]]) self.kin = Kinematic() p.setPhysicsEngineParameter( numSolverIterations=self.numSolverIterations) #p.setTimeStep(self.fixedTimeStep) p.setRealTimeSimulation(self.useRealTime) self.ref_time = time.time() p.getCameraImage(320, 200) #160,100) p.resetDebugVisualizerCamera(1, 85.6, 0, [-0.61, 0.12, 0.25]) # Camera Settings fov, aspect, nearplane, farplane = 90, 1.3, .0111, 100 self.projection_matrix = p.computeProjectionMatrixFOV( fov, aspect, nearplane, farplane) self.lastLidarTime = 0
import pybullet as p p.connect(p.GUI) cube = p.loadURDF("cube.urdf") frequency = 240 timeStep = 1. / frequency p.setGravity(0, 0, -9.8) p.changeDynamics(cube, -1, linearDamping=0, angularDamping=0) p.setPhysicsEngineParameter(fixedTimeStep=timeStep) for i in range(frequency): p.stepSimulation() pos, orn = p.getBasePositionAndOrientation(cube) print(pos)
def setup_simulation(self): """ Setup the simulation. """ ########## PYBULLET SETUP ########## if self.record_movie and self.gui == p.GUI: p.connect(self.gui, options=('--background_color_red={}' ' --background_color_green={}' ' --background_color_blue={}' ' --mp4={}' ' --mp4fps={}').format( self.vis_options_background_color_red, self.vis_options_background_color_green, self.vis_options_background_color_blue, self.movie_name, int(self.movie_speed / self.time_step))) p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1) elif self.gui == p.GUI: p.connect(self.gui, options=(' --background_color_red={}' ' --background_color_green={}' ' --background_color_blue={}').format( self.vis_options_background_color_red, self.vis_options_background_color_green, self.vis_options_background_color_blue)) else: p.connect(self.gui) p.resetSimulation() p.setAdditionalSearchPath(pybullet_data.getDataPath()) # Everything should fall down p.setGravity(*[g * self.units.gravity for g in self.gravity]) p.setPhysicsEngineParameter(fixedTimeStep=self.time_step, numSolverIterations=self.solver_iterations, numSubSteps=self.num_substep, solverResidualThreshold=1e-10, erp=0.0, contactERP=self.contactERP, frictionERP=0.0, globalCFM=self.globalCFM, reportSolverAnalytics=1) # Turn off rendering while loading the models self.rendering(0) if self.ground == 'floor': # Add floor self.plane = p.loadURDF('plane.urdf', [0, 0, -0.], globalScaling=0.01 * self.units.meters) # When plane is used the link id is -1 self.link_plane = -1 p.changeDynamics(self.plane, -1, lateralFriction=self.ground_friction_coef) self.sim_data.add_table('base_linear_velocity') self.sim_data.add_table('base_angular_velocity') self.sim_data.add_table('base_orientation') elif self.ground == 'ball': # Add floor and ball self.floor = p.loadURDF('plane.urdf', [0, 0, -0.], globalScaling=0.01 * self.units.meters) if self.ball_info: self.ball_radius, ball_pos = self.load_ball_info() else: ball_pos = None self.plane = self.add_ball(self.ball_radius, self.ball_density, self.ball_mass, self.ground_friction_coef, ball_pos) # When ball is used the plane id is 2 as the ball has 3 links self.link_plane = 2 self.sim_data.add_table('ball_rotations') self.sim_data.add_table('ball_velocity') # Add the animal model if '.sdf' in self.model and self.behavior is not None: self.animal, self.link_id, self.joint_id = load_sdf( self.model, force_concave=self.enable_concave_mesh) elif '.sdf' in self.model and self.behavior is None: self.animal = p.loadSDF(self.model)[0] # Generate joint_name to id dict self.link_id[p.getBodyInfo(self.animal)[0].decode('UTF-8')] = -1 for n in range(p.getNumJoints(self.animal)): info = p.getJointInfo(self.animal, n) _id = info[0] joint_name = info[1].decode('UTF-8') link_name = info[12].decode('UTF-8') _type = info[2] self.joint_id[joint_name] = _id self.joint_type[joint_name] = _type self.link_id[link_name] = _id elif '.urdf' in self.model: self.animal = p.loadURDF(self.model) p.resetBasePositionAndOrientation( self.animal, self.model_offset, p.getQuaternionFromEuler([0., 0., 0.])) self.num_joints = p.getNumJoints(self.animal) # Body colors color_wings = [91 / 100, 96 / 100, 97 / 100, 0.7] color_eyes = [67 / 100, 21 / 100, 12 / 100, 1] self.color_body = [140 / 255, 100 / 255, 30 / 255, 1] self.color_legs = [170 / 255, 130 / 255, 50 / 255, 1] self.color_collision = [0, 1, 0, 1] nospecular = [0.5, 0.5, 0.5] # Color the animal p.changeVisualShape(self.animal, -1, rgbaColor=self.color_body, specularColor=nospecular) for link_name, _id in self.joint_id.items(): if 'Wing' in link_name and 'Fake' not in link_name: p.changeVisualShape(self.animal, _id, rgbaColor=color_wings) elif 'Eye' in link_name and 'Fake' not in link_name: p.changeVisualShape(self.animal, _id, rgbaColor=color_eyes) # and 'Fake' not in link_name: elif ('Tarsus' in link_name or 'Tibia' in link_name or 'Femur' in link_name or 'Coxa' in link_name): p.changeVisualShape(self.animal, _id, rgbaColor=self.color_legs, specularColor=nospecular) elif 'Fake' not in link_name: p.changeVisualShape(self.animal, _id, rgbaColor=self.color_body, specularColor=nospecular) # print('Link name {} id {}'.format(link_name, _id)) # Configure contacts # Disable/Enable all self-collisions for link0 in self.link_id.keys(): for link1 in self.link_id.keys(): p.setCollisionFilterPair( bodyUniqueIdA=self.animal, bodyUniqueIdB=self.animal, linkIndexA=self.link_id[link0], linkIndexB=self.link_id[link1], enableCollision=0, ) # Disable/Enable tarsi-ground collisions for link in self.link_id.keys(): if 'Tarsus' in link: p.setCollisionFilterPair(bodyUniqueIdA=self.animal, bodyUniqueIdB=self.plane, linkIndexA=self.link_id[link], linkIndexB=self.link_plane, enableCollision=1) # Disable/Enable selected self-collisions for (link0, link1) in self.self_collisions: p.setCollisionFilterPair( bodyUniqueIdA=self.animal, bodyUniqueIdB=self.animal, linkIndexA=self.link_id[link0], linkIndexB=self.link_id[link1], enableCollision=1, ) # ADD container columns # ADD ground reaction forces and friction forces _ground_sensor_ids = [] for contact in self.ground_contacts: _ground_sensor_ids.append((self.animal, self.plane, self.link_id[contact], self.link_plane)) self.sim_data.contact_flag.add_parameter(f"{contact}_flag") for axis in ('x', 'y', 'z'): self.sim_data.contact_position.add_parameter(contact + '_' + axis) self.sim_data.contact_normal_force.add_parameter(contact + '_' + axis) self.sim_data.contact_lateral_force.add_parameter(contact + '_' + axis) # ADD self collision forces _collision_sensor_ids = [] for link0, link1 in self.self_collisions: _collision_sensor_ids.append( (self.animal, self.animal, self.link_id[link0], self.link_id[link1])) contacts = '-'.join((link0, link1)) for axis in ['x', 'y', 'z']: self.sim_data.contact_position.add_parameter(contacts + '_' + axis) self.sim_data.contact_normal_force.add_parameter(contacts + '_' + axis) self.sim_data.contact_lateral_force.add_parameter(contacts + '_' + axis) # Generate sensors self.joint_sensors = JointSensors(self.animal, self.sim_data, meters=self.units.meters, velocity=self.units.velocity, torques=self.units.torques) self.contact_sensors = ContactSensors( self.sim_data, tuple([*_ground_sensor_ids, *_collision_sensor_ids]), meters=self.units.meters, newtons=self.units.newtons) self.com_sensor = COMSensor(self.animal, self.sim_data, meters=self.units.meters, kilograms=self.units.kilograms) # ADD base position parameters for axis in ['x', 'y', 'z']: self.sim_data.base_position.add_parameter(f'{axis}') # self.sim_data.thorax_force.add_parameter(f'{axis}') if self.ground == 'ball': self.sim_data.ball_rotations.add_parameter(f'{axis}') self.sim_data.ball_velocity.add_parameter(f'{axis}') if self.ground == 'floor': self.sim_data.base_linear_velocity.add_parameter(f'{axis}') self.sim_data.base_angular_velocity.add_parameter(f'{axis}') self.sim_data.base_orientation.add_parameter(f'{axis}') # ADD joint parameters for name, _ in self.joint_id.items(): self.sim_data.joint_positions.add_parameter(name) self.sim_data.joint_velocities.add_parameter(name) self.sim_data.joint_torques.add_parameter(name) # ADD Center of mass parameters for axis in ('x', 'y', 'z'): self.sim_data.center_of_mass.add_parameter(f"{axis}") # ADD muscles if self.use_muscles: self.initialize_muscles() # ADD controller if self.controller_config: self.controller = NeuralSystem( config_path=self.controller_config, container=self.container, ) # Disable default bullet controllers p.setJointMotorControlArray(self.animal, np.arange(self.num_joints), p.VELOCITY_CONTROL, targetVelocities=np.zeros( (self.num_joints, 1)), forces=np.zeros((self.num_joints, 1))) p.setJointMotorControlArray(self.animal, np.arange(self.num_joints), p.POSITION_CONTROL, forces=np.zeros((self.num_joints, 1))) p.setJointMotorControlArray(self.animal, np.arange(self.num_joints), p.TORQUE_CONTROL, forces=np.zeros((self.num_joints, 1))) # Disable link linear and angular damping for njoint in range(self.num_joints): p.changeDynamics(self.animal, njoint, linearDamping=0.0) p.changeDynamics(self.animal, njoint, angularDamping=0.0) p.changeDynamics(self.animal, njoint, jointDamping=0.0) self.total_mass = 0.0 for j in np.arange(-1, p.getNumJoints(self.animal)): self.total_mass += p.getDynamicsInfo(self.animal, j)[0] / self.units.kilograms self.bodyweight = -1 * self.total_mass * self.gravity[2] print('Total mass = {}'.format(self.total_mass)) if self.gui == p.GUI: self.rendering(1)
#script to control a simulated robot hand using a VR glove #see https://twitter.com/erwincoumans/status/821953216271106048 #and https://www.youtube.com/watch?v=I6s37aBXbV8 #vr glove was custom build using Spectra Symbolflex sensors (4.5") #inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle #with BLE Link to receive serial (for wireless bluetooth serial) import serial import time import pybullet as p #first try to connect to shared memory (VR), if it fails use local GUI c = p.connect(p.SHARED_MEMORY) if (c<0): c = p.connect(p.GUI) p.setInternalSimFlags(0)#don't load default robot assets etc p.resetSimulation() #p.resetSimulation() p.setGravity(0,0,-10) objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)] objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
# R = np.sqrt(X**2 + Y**2) # Z = np.sin(R) # # Plot the surface. # surf = ax.plot_surface(X, Y, Z) # plt.show() # convert to an obj file #### for pybullet now ########## # set up the view physicsClient = p.connect(p.GUI) # turn on simulation #physicsClient = p.connect(p.DIRECT) # just print to command line # ability to get files from the intenet that are not downloaded locally :) p.setAdditionalSearchPath(pybullet_data.getDataPath()) planeId = p.loadURDF('plane.urdf') planeId = p.loadURDF('kuka_iiwa/model.urdf') # # make a visual shape id # visualShapeId = p.createVisualShape( # shapeType=p.GEOM_MESH, # fileName='plant.obj', # rgbaColor=None, # meshScale=[10e-4, 10e-4, 10e-4]) # # create a collision shape id
def save_traj_from_pt(args): if use_gpu: torch.backends.cudnn.deterministic = True print(colored("Using CUDA.", p_color)) torch.cuda.manual_seed(args.seed) torch.manual_seed(args.seed) np.random.seed(args.seed) random.seed(args.seed) """ Create environment and get environment's info. """ if args.env_id > 10 and args.env_id <= 20: import pybullet import pybullet_envs pybullet.connect(pybullet.DIRECT) env = gym.make(args.env_name) env.seed(args.seed) if args.render: env.render(mode="human") state_dim = env.observation_space.shape[0] is_disc_action = args.env_discrete action_dim = (0 if is_disc_action else env.action_space.shape[0]) if is_disc_action: a_bound = 1 action_num = env.action_space.n print("State dim: %d, action num: %d" % (state_dim, action_num)) else: args.a_bound_pre = np.asscalar(env.action_space.high[0]) """ always normalize env. """ from my_utils.my_gym_utils import NormalizeGymWrapper env = NormalizeGymWrapper(env) a_bound = np.asscalar(env.action_space.high[0]) a_low = np.asscalar(env.action_space.low[0]) assert a_bound == -a_low print("State dim: %d, action dim: %d, action bound %d" % (state_dim, action_dim, a_bound)) """ Set method and hyper parameter in file name""" method_name = args.rl_method.upper() hypers = rl_hypers_parser(args) exp_name = "%s-%s_s%d" % (method_name, hypers, args.seed) """ Set path for trajectory files """ traj_path = "../imitation_data/TRAJ_h5/%s/" % (args.env_name) pathlib.Path(traj_path).mkdir(parents=True, exist_ok=True) print("%s trajectory will be saved at %s" % (colored(method_name, p_color), colored(traj_path, p_color))) """define actor and critic""" if is_disc_action: if args.rl_method == "dqn": policy_updater = DQN(state_dim=state_dim, action_num=action_num, args=args) if args.rl_method == "qr_dqn": policy_updater = QR_DQN(state_dim=state_dim, action_num=action_num, args=args) if args.rl_method == "clipped_ddqn": policy_updater = Clipped_DDQN(state_dim=state_dim, action_num=action_num, args=args) if args.rl_method == "ppo": policy_updater = PPO(state_dim=state_dim, action_dim=action_dim, args=args, a_bound=action_num, is_discrete=True) else: if args.rl_method == "ac": policy_updater = AC(state_dim=state_dim, action_dim=action_dim, args=args, a_bound=a_bound) if args.rl_method == "sac": policy_updater = SAC(state_dim=state_dim, action_dim=action_dim, args=args, a_bound=a_bound) if args.rl_method == "td3": policy_updater = TD3(state_dim=state_dim, action_dim=action_dim, args=args, a_bound=a_bound) if args.rl_method == "trpo": policy_updater = TRPO(state_dim=state_dim, action_dim=action_dim, args=args, a_bound=a_bound) if args.rl_method == "ppo": policy_updater = PPO(state_dim=state_dim, action_dim=action_dim, args=args, a_bound=a_bound) """ Load policy """ load_step = args.demo_iter print(load_step) model_path = "../imitation_data/RL_models/%s/%s-%s" % ( args.env_name, args.env_name, exp_name) model_filename = model_path + ("_policy_T%d.pt" % (load_step)) policy_updater.load_model(model_filename) policy_updater.policy_to_device(device_cpu) print("RL model is loaded from %s" % model_filename) for noise_level in args.noise_level_list: """ Reset seed again to make sure all noises use the same transition's RNG """ if use_gpu: torch.cuda.manual_seed(args.seed) torch.backends.cudnn.deterministic = True torch.manual_seed(args.seed) np.random.seed(args.seed) random.seed(args.seed) env.seed(args.seed) """ Storages and counters """ expert_state_list = [] expert_action_list = [] expert_mask_list = [] expert_reward_list = [] total_step, avg_reward_episode = 0, 0 for i_episode in count(): state = env.reset() reward_episode = 0 if "TN" in args.noise_type: noise_model = TN(env.action_space, mu=0.0, theta=0.15, max_sigma=noise_level, min_sigma=0.01, decay_period=1000) for t in range(0, args.t_max): state_var = torch.FloatTensor(state) if args.traj_deterministic: action = policy_updater.greedy_action( torch.FloatTensor(state).unsqueeze(0)).to( device_cpu).detach().numpy() else: action = policy_updater.sample_action( torch.FloatTensor(state).unsqueeze(0)).to( device_cpu).detach().numpy() # add noise to action if args.noise_type == "normal" and noise_level > 0: action = action + np.random.normal(0, noise_level, action.shape) elif args.noise_type == "SDNTN" and noise_level > 0: action = action + noise_model.get_noise(t) * norm( action, ord=1) / action_dim # action = np.clip(action, a_min=a_low, a_max=a_bound) next_state, reward, done, _ = env.step( np.clip(action, a_min=a_low, a_max=a_bound)) if args.render: env.render("human") time.sleep(0.001) if t + 1 == args.t_max: done = 1 expert_state_list.append(state) expert_action_list.append(action) expert_mask_list.append(int(not done)) expert_reward_list.append(reward) state = next_state total_step += 1 reward_episode += reward if done: break avg_reward_episode += reward_episode if i_episode % 10 == 0: print('Episode {}\t reward: {:.2f}'.format( i_episode, reward_episode)) if total_step >= args.demo_file_size: break expert_states = np.array(expert_state_list) expert_actions = np.array(expert_action_list) expert_masks = np.array(expert_mask_list) expert_rewards = np.array(expert_reward_list) print("Total steps %d, total episode %d, AVG reward: %f" % (total_step, i_episode + 1, avg_reward_episode / (i_episode + 1))) print(expert_actions.min()) print(expert_actions.max()) """ save data """ traj_filename = traj_path + ( "/%s_TRAJ-N%d_%s%0.2f" % (args.env_name, args.demo_file_size, args.noise_type, noise_level)) if args.traj_deterministic: traj_filename += "_det" hf = h5py.File(traj_filename + ".h5", 'w') hf.create_dataset('expert_source_path', data=model_filename) # network model file. hf.create_dataset('expert_states', data=expert_states) hf.create_dataset('expert_actions', data=expert_actions) hf.create_dataset('expert_masks', data=expert_masks) hf.create_dataset('expert_rewards', data=expert_rewards) print("TRAJ result is saved at %s" % traj_filename)
pos.append((x, y, z)) x += 2.0 * rad if x > xymax: x = xymin + (x - xymax) y += 2.0 * rad if y > xymax: y = xymin + (y - xymax) z += 2.0 * rad for p in pos: ob = pb.loadURDF("sphere_1cm.urdf", basePosition=p, useMaximalCoordinates=True) if __name__ == '__main__': pb.connect(pb.GUI, options="--opengl2") #pb.connect(pb.DIRECT) pb.setAdditionalSearchPath(pybullet_data.getDataPath()) pb.setInternalSimFlags(0) pb.resetSimulation() #pb.configureDebugVisualizer(pb.COV_ENABLE_WIREFRAME ,1) pb.configureDebugVisualizer(pb.COV_ENABLE_SHADOWS, 0) pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 1) pb.configureDebugVisualizer(pb.COV_ENABLE_TINY_RENDERER, 0) pb.configureDebugVisualizer(pb.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) pb.configureDebugVisualizer(pb.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) pb.configureDebugVisualizer(pb.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
import matplotlib.pyplot as plt import pybullet import time import numpy as np #to reshape for matplotlib plt.ion() img = [[1,2,3]*50]*100#np.random.rand(200, 320) #img = [tandard_normal((50,100)) image = plt.imshow(img,interpolation='none',animated=True,label="blah") ax = plt.gca() pybullet.connect(pybullet.DIRECT) #pybullet.loadPlugin("eglRendererPlugin") pybullet.loadURDF("plane.urdf",[0,0,-1]) pybullet.loadURDF("r2d2.urdf") pybullet.setGravity(0,0,-10) camTargetPos = [0,0,0] cameraUp = [0,0,1] cameraPos = [1,1,1] pitch = -10.0 roll=0 upAxisIndex = 2 camDistance = 4
import pybullet as p import time p.connect(p.GUI) planeUidA = p.loadURDF("plane_transparent.urdf", [0, 0, 0]) planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, -1]) texUid = p.loadTexture("tex256.png") p.changeVisualShape(planeUidA, -1, rgbaColor=[1, 1, 1, 0.5]) p.changeVisualShape(planeUid, -1, rgbaColor=[1, 1, 1, 0.5]) p.changeVisualShape(planeUid, -1, textureUniqueId=texUid) width = 256 height = 256 pixels = [255] * width * height * 3 colorR = 0 colorG = 0 colorB = 0 #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) #p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) blue = 0 logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json") for i in range(100000): p.stepSimulation() for i in range(width): for j in range(height): pixels[(i + j * width) * 3 + 0] = i pixels[(i + j * width) * 3 + 1] = (j + blue) % 256
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 __init__(self): """ Initialise the environment """ self.random_goal = True self.goal_oriented = True self.obs_type = 1 self.reward_type = 1 self.action_type = 1 # Define action space self.action_space = spaces.Box( low=np.float32( np.array([-0.5, -0.25, -0.25, -0.25, -0.5, -0.005]) / 30), high=np.float32( np.array([0.5, 0.25, 0.25, 0.25, 0.5, 0.005]) / 30), dtype=np.float32) # Define observation space if self.obs_type == 1: self.obs_space_low = np.float32( np.concatenate((MIN_END_EFF_COORDS, JOINT_MIN), axis=0)) self.obs_space_high = np.float32( np.concatenate((MAX_END_EFF_COORDS, JOINT_MAX), axis=0)) elif self.obs_type == 2: self.obs_space_low = np.float32( np.concatenate((MIN_GOAL_COORDS, JOINT_MIN), axis=0)) self.obs_space_high = np.float32( np.concatenate((MAX_GOAL_COORDS, JOINT_MAX), axis=0)) elif self.obs_type == 3: self.obs_space_low = np.float32( np.concatenate(([-1.0] * 6, JOINT_MIN), axis=0)) self.obs_space_high = np.float32( np.concatenate(([1.0] * 6, JOINT_MAX), axis=0)) elif self.obs_type == 4: self.obs_space_low = np.float32( np.concatenate(([-1.0] * 3, JOINT_MIN), axis=0)) self.obs_space_high = np.float32( np.concatenate(([1.0] * 3, JOINT_MAX), axis=0)) elif self.obs_type == 5: self.obs_space_low = np.float32( np.concatenate(([-1.0] * 6, MIN_GOAL_COORDS, JOINT_MIN), axis=0)) self.obs_space_high = np.float32( np.concatenate(([1.0] * 6, MAX_GOAL_COORDS, JOINT_MAX), axis=0)) self.observation_space = spaces.Box(low=self.obs_space_low, high=self.obs_space_high, dtype=np.float32) if self.goal_oriented: self.observation_space = spaces.Dict( dict(desired_goal=spaces.Box(low=np.float32(MIN_GOAL_COORDS), high=np.float32(MAX_GOAL_COORDS), dtype=np.float32), achieved_goal=spaces.Box( low=np.float32(MIN_END_EFF_COORDS), high=np.float32(MAX_END_EFF_COORDS), dtype=np.float32), observation=self.observation_space)) # Initialise goal position if self.random_goal: self.goal = self.sample_random_goal() else: self.goal = FIXED_GOAL_COORDS # Connect to physics client. By default, do not render self.physics_client = p.connect(p.DIRECT) # Load URDFs self.create_world() # reset environment self.reset()
import pybullet as p draw = 1 printtext = 0 if (draw): p.connect(p.GUI) else: p.connect(p.DIRECT) r2d2 = p.loadURDF("r2d2.urdf") def drawAABB(aabb): f = [aabbMin[0], aabbMin[1], aabbMin[2]] t = [aabbMax[0], aabbMin[1], aabbMin[2]] p.addUserDebugLine(f, t, [1, 0, 0]) f = [aabbMin[0], aabbMin[1], aabbMin[2]] t = [aabbMin[0], aabbMax[1], aabbMin[2]] p.addUserDebugLine(f, t, [0, 1, 0]) f = [aabbMin[0], aabbMin[1], aabbMin[2]] t = [aabbMin[0], aabbMin[1], aabbMax[2]] p.addUserDebugLine(f, t, [0, 0, 1]) f = [aabbMin[0], aabbMin[1], aabbMax[2]] t = [aabbMin[0], aabbMax[1], aabbMax[2]] p.addUserDebugLine(f, t, [1, 1, 1]) f = [aabbMin[0], aabbMin[1], aabbMax[2]] t = [aabbMax[0], aabbMin[1], aabbMax[2]] p.addUserDebugLine(f, t, [1, 1, 1]) f = [aabbMax[0], aabbMin[1], aabbMin[2]]
def render(self, mode='human'): """ Render Pybullet simulation """ p.disconnect(self.physics_client) self.physics_client = p.connect(p.GUI) self.create_world()