def reset(self): self.ordered_joints = [] print(os.path.join(os.path.dirname(__file__), "data", self.model_urdf)) if self.self_collision: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base, flags=p.URDF_USE_SELF_COLLISION)) else: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base)) self.robot_specific_reset() s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use self.potential = self.calc_potential() return s
def setupWorld(): p.resetSimulation() p.loadURDF("planeMesh.urdf") kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10]) for i in range (p.getNumJoints(kukaId)): p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0) for i in range (numObjects): cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2]) #p.changeDynamics(cube,-1,mass=100) p.stepSimulation() p.setGravity(0,0,-10)
def reset(self): if self.isEnableSelfCollision: self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2], flags=p.URDF_USE_SELF_COLLISION) else: self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2]) self.kp = 1 self.kd = 1 self.maxForce = 3.5 self.nMotors = 8 self.motorIdList = [] self.motorDir = [-1, -1, -1, -1, 1, 1, 1, 1] self.buildJointNameToIdDict() self.buildMotorIdList()
def _randomly_place_objects(self, urdfList): """Randomly places the objects in the bin. Args: urdfList: The list of urdf files to place in the bin. Returns: The list of object unique ID's. """ # Randomize positions of each object urdf. objectUids = [] for urdf_name in urdfList: xpos = 0.4 +self._blockRandom*random.random() ypos = self._blockRandom*(random.random()-.5) angle = np.pi/2 + self._blockRandom * np.pi * random.random() orn = p.getQuaternionFromEuler([0, 0, angle]) urdf_path = os.path.join(self._urdfRoot, urdf_name) uid = p.loadURDF(urdf_path, [xpos, ypos, .15], [orn[0], orn[1], orn[2], orn[3]]) objectUids.append(uid) # Let each object fall to the tray individual, to prevent object # intersection. for _ in range(500): p.stepSimulation() return objectUids
def reset(self): objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf")) self.kukaUid = objects[0] #for i in range (p.getNumJoints(self.kukaUid)): # print(p.getJointInfo(self.kukaUid,i)) p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000]) self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ] self.numJoints = p.getNumJoints(self.kukaUid) for jointIndex in range (self.numJoints): p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex]) p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce) self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000) self.endEffectorPos = [0.537,0.0,0.5] self.endEffectorAngle = 0 self.motorNames = [] self.motorIndices = [] for i in range (self.numJoints): jointInfo = p.getJointInfo(self.kukaUid,i) qIndex = jointInfo[3] if qIndex > -1: #print("motorname") #print(jointInfo[1]) self.motorNames.append(str(jointInfo[1])) self.motorIndices.append(i)
def get_cube(x, y, z): body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), x, y, z) p.changeDynamics(body,-1, mass=1.2)#match Roboschool part_name, _ = p.getBodyInfo(body, 0) part_name = part_name.decode("utf8") bodies = [body] return BodyPart(part_name, bodies, 0, -1)
def setupWorld(self): numObjects = 50 maximalCoordinates = False p.resetSimulation() p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates) kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10], useMaximalCoordinates=maximalCoordinates) for i in range(p.getNumJoints(kukaId)): p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0) for i in range(numObjects): cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2]) #p.changeDynamics(cube,-1,mass=100) p.stepSimulation() p.setGravity(0, 0, -10)
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0): print('start evaluation') beforeTime = time.time() p.resetSimulation() p.setTimeStep(timeStep) p.loadURDF("%s/plane.urdf" % urdfRoot) p.setGravity(0, 0, -10) global minitaur minitaur = Minitaur(urdfRoot) start_position = current_position() last_position = None # for tracing line total_energy = 0 for i in range(maxNumSteps): torques = minitaur.getMotorTorques() velocities = minitaur.getMotorVelocities() total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep joint_values = evaluate_func_map[evaluateFunc](i, params) minitaur.applyAction(joint_values) p.stepSimulation() if (is_fallen()): break if i % 100 == 0: sys.stdout.write('.') sys.stdout.flush() time.sleep(sleepTime) print(' ') alpha = objectiveParams[0] final_distance = np.linalg.norm(start_position - current_position()) finalReturn = final_distance - alpha * total_energy elapsedTime = time.time() - beforeTime print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy, "finalReturn", finalReturn, "elapsed_time", elapsedTime) return finalReturn
def _reset(self): p.resetSimulation() #p.setPhysicsEngineParameter(numSolverIterations=300) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf")) dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 p.setGravity(0,0,-10) self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation)
def reset(self): self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath,0,0,.2) self.kp = 1 self.kd = 0.1 self.maxForce = 3.5 self.nMotors = 8 self.motorIdList = [] self.motorDir = [1, 1, 1, 1, 1, 1, 1, 1] self.buildJointNameToIdDict() self.buildMotorIdList()
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 _reset(self): self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) xpos = 0.5 +0.2*random.random() ypos = 0 +0.25*random.random() ang = 3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation)
def _reset(self): """Environment reset called at the beginning of an episode. """ # Set the camera settings. look = [0.23, 0.2, 0.54] distance = 1. pitch = -56 + self._cameraRandom*np.random.uniform(-3, 3) yaw = 245 + self._cameraRandom*np.random.uniform(-3, 3) roll = 0 self._view_matrix = p.computeViewMatrixFromYawPitchRoll( look, distance, yaw, pitch, roll, 2) fov = 20. + self._cameraRandom*np.random.uniform(-2, 2) aspect = self._width / self._height near = 0.01 far = 10 self._proj_matrix = p.computeProjectionMatrixFOV( fov, aspect, near, far) self._attempted_grasp = False self._env_step = 0 self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() # Choose the objects in the bin. urdfList = self._get_random_object( self._numObjects, self._isTest) self._objectUids = self._randomly_place_objects(urdfList) self._observation = self._get_observation() return np.array(self._observation)
def reset(self): self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) self.kp = 1 self.kd = 0.1 self.maxForce = 100 nJoints = p.getNumJoints(self.quadruped) self.jointNameToId = {} for i in range(nJoints): jointInfo = p.getJointInfo(self.quadruped, i) self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] self.resetPose() for i in range(100): p.stepSimulation()
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 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 _reset(self): # print("-----------reset simulation---------------") p.resetSimulation() self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0]) self.timeStep = 0.01 p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) p.setGravity(0,0, -10) p.setTimeStep(self.timeStep) p.setRealTimeSimulation(0) initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) p.resetJointState(self.cartpole, 1, initialAngle) p.resetJointState(self.cartpole, 0, initialCartPos) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] return np.array(self.state)
def _reset(self): # print("-----------reset simulation---------------") p.resetSimulation() self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0]) p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0) p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0) p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0) self.timeStep = 0.02 p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0) p.setGravity(0,0, -9.8) p.setTimeStep(self.timeStep) p.setRealTimeSimulation(0) randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) p.resetJointState(self.cartpole, 1, randstate[0], randstate[1]) p.resetJointState(self.cartpole, 0, randstate[2], randstate[3]) #print("randstate=",randstate) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] #print("self.state=", self.state) return np.array(self.state)
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 from time import sleep physicsClient = p.connect(p.GUI) p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf", [0,0,-2]) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5) p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) p.setRealTimeSimulation(1) while p.isConnected(): p.setGravity(0,0,-10) sleep(1./240.)
import pybullet as p import time import math from datetime import datetime from time import sleep p.connect(p.GUI) p.loadURDF("plane.urdf", [0, 0, -0.3]) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1]) kukaEndEffectorIndex = 6 numJoints = p.getNumJoints(kukaId) #Joint damping coefficents. Using large values for the joints that we don't want to move. jd = [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 0.5] #jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5] p.setGravity(0, 0, 0) while 1: p.stepSimulation() for i in range(1): pos = [0, 0, 1.26] orn = p.getQuaternionFromEuler([0, 0, 3.14]) jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, jointDamping=jd)
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.resetSimulation() p.setGravity(0,0,0) print(c) if (c<0): p.connect(p.GUI) #load the MuJoCo MJCF hand minitaur = p.loadURDF("quadruped/minitaur.urdf") robot_cid = -1 POSITION=1 ORIENTATION=2 BUTTONS=6 p.setRealTimeSimulation(1) controllerId = -1 while True: events = p.getVREvents() for e in (events): #print (e[BUTTONS]) if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED ):
) args = parser.parse_args() sim = simulation.Simulation("rrr/robot.urdf", fixed=True, panels=True) sliders = {} target = None joints = sim.getJoints() bx = 0.07 bz = 0.25 ################################################################################################### ################################################################################################### """ Definitions control sliders """ if args.mode == "direct": target = p.loadURDF("target2/robot.urdf") for joint in joints: sliders[joint] = p.addUserDebugParameter(joint, -math.pi, math.pi, 0.0) elif args.mode == "inverse": target = p.loadURDF("target2/robot.urdf") sliders["target_x"] = p.addUserDebugParameter("target_x", -1, 1, 0.4) sliders["target_y"] = p.addUserDebugParameter("target_y", -1, 1, 0) sliders["target_z"] = p.addUserDebugParameter("target_z", -1, 1, 0.25) elif args.mode == "triangle" or args.mode == "triangle-points": sliders["triangle_x"] = p.addUserDebugParameter("triangle_x", 0.01, 0.8, 0.4) sliders["triangle_z"] = p.addUserDebugParameter("triangle_z", -0.2, 0.3, 0) sliders["triangle_h"] = p.addUserDebugParameter("triangle_h", 0.01, 0.3, 0.1) sliders["triangle_w"] = p.addUserDebugParameter("triangle_w", 0.01, 0.3, 0.2)
import pybullet as p import pybullet_data import math from statistics import mean import time import decimal from time import sleep physicsClient = p.connect(p.GUI) #p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -10) planeId = p.loadURDF("plane2.urdf") #planeId = p.loadSDF("stadium.sdf") cubeStartPos = [0, 0, 1.5] cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) boxId = p.loadURDF( "1dHopper_withoutBeam.urdf", cubeStartPos, cubeStartOrientation, useFixedBase=0, globalScaling=1.0 ) cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) currentSpringLength = float(0.5) useRealTimeSimulation = 0 stateArray = ["LOADING", "COMPRESSION", "THRUST", "UNLOADING", "FLIGHT"] currentState = "NULL" hasThrusted = False toeOffTheGround = False isRecording = False # Marker for stance phase time recording startTime = time.time() touchDownTime = time.time()
import pybullet_data import numpy as np import matplotlib.pyplot as plt def plot_from_dict(steps, data_dict): for name, data in data_dict.items(): plt.plot(steps, data, label='%s' % name) plt.legend() #plt.show() 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") #creat a cylinder test_visual1 = p.createVisualShape(p.GEOM_CYLINDER, radius=0.5, length=0.5) test_collision1 = p.createCollisionShape(p.GEOM_CYLINDER, radius=0.5, height=0.5) #creat a capsule test_visual2 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.1, length=1, visualFramePosition=[0, 0, 0.5], visualFrameOrientation=[0, 0, 0, 1]) test_collision2 = p.createCollisionShape( p.GEOM_CAPSULE,
def load_urdf(self, urdf): return pb.loadURDF(urdf)
log = list() for chunk in chunks: if len(chunk) == sz: values = struct.unpack(fmt, chunk) record = list() for i in range(ncols): record.append(values[i]) log.append(record) return log #clid = p.connect(p.SHARED_MEMORY) p.connect(p.GUI) p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf") p.loadURDF("tray/tray.urdf", [0, 0, 0]) p.loadURDF("block.urdf", [0, 0, 2]) log = readLogFile("data/block_grasp_log.bin") recordNum = len(log) itemNum = len(log[0]) objectNum = p.getNumBodies() print('record num:'), print(recordNum) print('item num:'), print(itemNum) def Step(stepIndex):
def init_grasp(self): try: p.removeBody(self.box_id) except: pass pos_traj = np.load(os.path.join(self.env_root, 'init', 'pos.npy')) orn_traj = np.load(os.path.join(self.env_root, 'init', 'orn.npy')) self.fix_orn = np.load(os.path.join(self.env_root, 'init', 'orn.npy')) for j in range(7): self.p.resetJointState(self.robotId, j, self.data_q[0][j], self.data_dq[0][j]) self.robot.gripperControl(0) for init_t in range(100): box = p.getAABB(self.obj_id, -1) center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])] center[0] -= 0.05 center[1] -= 0.05 center[2] += 0.03 # center = (box[0]+box[1])*0.5 points = np.array([pos_traj[0], center]) start_id = 0 init_traj = point2traj(points) start_id = self.move(init_traj, orn_traj, start_id) # grasping grasp_stage_num = 10 for grasp_t in range(grasp_stage_num): gripperPos = grasp_t / float(grasp_stage_num) * 180.0 self.robot.gripperControl(gripperPos) start_id += 1 pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([pos, [pos[0], pos[1] + 0.05, pos[2] + 0.3]]) start_id = self.move(up_traj, orn_traj, start_id) if self.opt.rand_start == 'rand': # move in z-axis direction pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([ pos, [pos[0], pos[1], pos[2] + (random.random() - 0.5) * 0.1] ]) start_id = self.move(up_traj, orn_traj, start_id) # move in y-axis direction pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([ pos, [pos[0], pos[1] + (random.random() - 0.5) * 0.2 + 0.2, pos[2]] ]) start_id = self.move(up_traj, orn_traj, start_id) # move in x-axis direction pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([ pos, [pos[0] + (random.random() - 0.5) * 0.2, pos[1], pos[2]] ]) start_id = self.move(up_traj, orn_traj, start_id) elif self.opt.rand_start == 'two': prob = random.random() if prob < 0.5: pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([pos, [pos[0], pos[1] + 0.2, pos[2]]]) start_id = self.move(up_traj, orn_traj, start_id) if self.opt.rand_box == 'rand': self.box_file = os.path.join(self.env_root, "urdf/openbox/openbox.urdf") self.box_position = [ 0.42 + (random.random() - 0.5) * 0.2, 0.00 + (random.random() - 0.5) * 0.4, 0.34 ] self.box_scaling = 0.00037 self.box_orientation = p.getQuaternionFromEuler( [0, 0, math.pi / 2]) self.box_id = p.loadURDF(fileName=self.box_file, basePosition=self.box_position, baseOrientation=self.box_orientation, globalScaling=self.box_scaling ) #, physicsClientId=self.physical_id) else: self.box_file = os.path.join(self.env_root, "urdf/openbox/openbox.urdf") self.box_position = [0.42, 0.00, 0.34] self.box_scaling = 0.00037 self.box_orientation = p.getQuaternionFromEuler( [0, 0, math.pi / 2]) self.box_id = p.loadURDF(fileName=self.box_file, basePosition=self.box_position, baseOrientation=self.box_orientation, globalScaling=self.box_scaling ) #, physicsClientId=self.physical_id) texture_path = os.path.join(self.env_root, 'texture/sun_textures') texture_file = os.path.join( texture_path, random.sample(os.listdir(texture_path), 1)[0]) textid = p.loadTexture(texture_file) # p.changeVisualShape (self.box_id, -1, rgbaColor=[1, 1, 1, 0.9]) # p.changeVisualShape (self.box_id, -1, textureUniqueId=textid) p.changeVisualShape(self.box_id, -1, rgbaColor=[1, 0, 0, 1]) self.start_pos = p.getLinkState(self.robotId, 7)[0] box = p.getAABB(self.box_id, -1) box_center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])] obj = p.getAABB(self.obj_id, -1) obj_center = [(x + y) * 0.5 for x, y in zip(obj[0], obj[1])] self.last_aabb_dist = sum([ (x - y)**2 for x, y in zip(box_center, obj_center) ])**0.5
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)] objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)] #load the MuJoCo MJCF hand objects = p.loadMJCF("MPL/mpl2.xml")
def load_object(): boxId = p.loadURDF("object.urdf") return boxId
jump_amp = 0.5 maxForce = 3.5 kneeFrictionForce = 0 kp = 1 kd = .5 maxKneeForce = 1000 # physId = p.connect(p.SHARED_MEMORY_GUI) if (1): p.connect(p.GUI) p.resetSimulation() p.setAdditionalSearchPath(pybullet_data.getDataPath()) angle = 0 # pick in range 0..0.2 radians orn = p.getQuaternionFromEuler([0, angle, 0]) p.loadURDF("plane.urdf", [0, 0, 0], orn) p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) # p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, # "genericlogdata.bin", # maxLogDof=16, # logFlags=p.STATE_LOG_JOINT_TORQUES) p.setTimeOut(4000000) p.setGravity(0, 0, 0) p.setTimeStep(fixedTimeStep) orn = p.getQuaternionFromEuler([0, 0, 0.4]) p.setRealTimeSimulation(0) quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3], orn, useFixedBase=False,
# 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"] rest_pose = config["rest_pose"] robot_name = config["robot"]["name"] robot_origin_pos = config["robot"]["origin_pos"] robot_origin_orientation = config["robot"]["origin_orientation"] robotId = p.loadURDF(config["robot"]["model"], robot_origin_pos, robot_origin_orientation) robotEndeffectorIndex = config["robot"]["endeffector_index"] p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
window.addItem(grid) global_axis = gl.GLAxisItem() global_axis.updateGLOptions({'glLineWidth': (4, )}) window.addItem(global_axis) print("WINDOW not updated") window.update() # configure pybullet and load plane.urdf and quadcopter.urdf physicsClient = p.connect( p.DIRECT) # pybullet only for computations no visualisation p.setGravity(0, 0, -gravity) p.setTimeStep(Tsample_physics) # disable real-time simulation, we want to step through the # physics ourselves with p.stepSimulation() p.setRealTimeSimulation(0) planeId = p.loadURDF("plane.urdf", [0, 0, 0], p.getQuaternionFromEuler([0, 0, 0])) quadcopterId = p.loadURDF("quadrotor.urdf", [0, 0, 1], p.getQuaternionFromEuler([0, 0, 0])) # do a few steps to start simulation and let the quadcopter land safely for i in range(int(2 / Tsample_physics)): p.stepSimulation() # create a pyqtgraph mesh from the quadcopter to visualize # the quadcopter in the 3D pyqtgraph window quadcopterMesh = bullet2pyqtgraph(quadcopterId)[0] window.addItem(quadcopterMesh) window.update() # Initialize PID controller gains: Kp = np.zeros(6)
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] return rayFrom,rayTo cid = p.connect(p.SHARED_MEMORY) if (cid<0): p.connect(p.GUI) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1./120.) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) p.loadURDF("plane100.urdf", useMaximalCoordinates=True) #disable rendering during creation. 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],
chunks = wholeFile.split(b'\xaa\xbb') log = list() for chunk in chunks: if len(chunk) == sz: values = struct.unpack(fmt, chunk) record = list() for i in range(ncols): record.append(values[i]) log.append(record) return log #clid = p.connect(p.SHARED_MEMORY) p.connect(p.GUI) p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf") p.loadURDF("tray/tray.urdf",[0,0,0]) p.loadURDF("block.urdf",[0,0,2]) log = readLogFile("data/block_grasp_log.bin") recordNum = len(log) itemNum = len(log[0]) objectNum = p.getNumBodies() print('record num:'), print(recordNum) print('item num:'), print(itemNum) def Step(stepIndex): for objectId in range(objectNum):
import pybullet as p import pybullet_data import time physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF p.setGravity(0,0,-10) planeId = p.loadURDF("road_plain.urdf") cubeStartPos = [0,0,1] cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) p.stepSimulation() cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) print(cubePos,cubeOrn) time.sleep(5) p.disconnect()
controls = {} robotPath = "phantomx_description/urdf/phantomx.urdf" sim = Simulation(robotPath, gui=True, panels=True, useUrdfInertia=False) # sim.setFloorFrictions(lateral=0, spinning=0, rolling=0) pos, rpy = sim.getRobotPose() sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1]) leg_center_pos = [0.1248, -0.06164, 0.001116 + 0.5] leg_angle = -math.pi / 4 params = Parameters() if args.mode == "frozen-direct": crosses = [] for i in range(4*6): crosses.append(p.loadURDF("target2/robot.urdf")) for name in sim.getJoints(): print(name) if "c1" in name or "thigh" in name or "tibia" in name: controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi, 0) elif args.mode == "direct": for name in sim.getJoints(): print(name) if "c1" in name or "thigh" in name or "tibia" in name: controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi, 0) elif args.mode == "inverse": cross = p.loadURDF("target2/robot.urdf") # Use your own DK function alphas = kinematics.computeDK(0, 0, 0, use_rads=True) controls["target_x"] = p.addUserDebugParameter("target_x", -0.4, 0.4, alphas[0]) controls["target_y"] = p.addUserDebugParameter("target_y", -0.4, 0.4, alphas[1])
CONTROLLER_ID = 0 POSITION=1 ORIENTATION=2 NUM_MOVE_EVENTS=5 BUTTONS=6 ANALOG_AXIS=8 #assume that the VR physics server is already started before c = p.connect(p.SHARED_MEMORY) print(c) if (c<0): p.connect(p.GUI) p.setInternalSimFlags(0)#don't load default robot assets etc p.resetSimulation() p.loadURDF("plane.urdf") prevPosition=[[0,0,0]]*p.VR_MAX_CONTROLLERS colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS widths = [3]*p.VR_MAX_CONTROLLERS #use a few default colors colors[0] = [0,0,0] colors[1] = [0.5,0,0] colors[2] = [0,0.5,0] colors[3] = [0,0,0.5] colors[4] = [0.5,0.5,0.] colors[5] = [.5,.5,.5] controllerId = -1 pt=[0,0,0]
p.setJointMotorControl2(bodyIndex=robot, jointIndex=knee_motor_id, controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=standstilltorque) p.setJointMotorControl2(bodyIndex=robot, jointIndex=spring_motor_id, controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=standstilltorque) 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") robotStartPos = [0, 0, 0.23] robotStartOrientation = p.getQuaternionFromEuler([np.pi / 2, 0, 0]) robot = p.loadURDF("/home/pramod/Single_leg_test/Urdf/Test_1/urdf/Test_1.urdf", robotStartPos, robotStartOrientation, useFixedBase=1) # ResetLeg() a = 0.06 b = 0.03 x_path = [] z_path = [] y_path = [] for j in range(0, 361, 5): # Ellipse x = -0.06 + a * m.cos(m.radians(j))
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 pixelWidth = 320 pixelHeight = 200 nearPlane = 0.01
import pybullet as p import time import math useGui = True if (useGui): p.connect(p.GUI) else: p.connect(p.DIRECT) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.loadURDF("samurai.urdf") p.loadURDF("r2d2.urdf",[3,3,1]) rayFrom=[] rayTo=[] numRays = 1024 rayLen = 13 for i in range (numRays): rayFrom.append([0,0,1]) rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1]) if (not useGui): timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json")
BUTTONS = 6 import pybullet_data cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation() p.setGravity(0, 0, -10) useRealTimeSim = 1 #for video recording (works best on Mac and Linux, not well on Windows) #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") p.setRealTimeSimulation(useRealTimeSim) # either this p.loadURDF("plane.urdf") #p.loadSDF("stadium.sdf") car = p.loadURDF( "racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) for i in range(p.getNumJoints(car)): print(p.getJointInfo(car, i)) for wheel in range(p.getNumJoints(car)): p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0) p.getJointInfo(car, wheel) wheels = [8, 15]
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.setAdditionalSearchPath(pybullet_data.getDataPath()) 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 = [
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)
import pybullet as p import time cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) p.loadURDF("plane.urdf") kuka = p.loadURDF("kuka_iiwa/model.urdf") p.addUserDebugText("tip", [0, 0, 0.1], textColorRGB=[1, 0, 0], textSize=1.5, parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0], parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0], parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1], parentObjectUniqueId=kuka, parentLinkIndex=6) p.setRealTimeSimulation(0) angle = 0 while (True): time.sleep(0.01) p.resetJointState(kuka, 2, angle) p.resetJointState(kuka, 3, angle) angle += 0.01
from mykinematics import * from config_parse import * import time import numpy as np direct = p.connect(p.GUI) delta = 0.001 c1 = 0.23 c2 = -0.3084 p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.setGravity(0, 0, -9.81) # p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setAdditionalSearchPath('models') p.loadURDF('floor/floor.urdf') arm = p.loadURDF('dof.urdf') # for i in range(p.getNumJoints(arm)): # print(p.getJointInfo(arm, i)) # print('') # print([x[0] for x in p.getJointStates(arm, [0,1,2,3,4])]) def rot_to_quat(phi): R = get_rot_matrix_from_euler(0, phi, 0) tr = R[0, 0] + R[1, 1] + R[2, 2] if tr > 0.0: S = np.sqrt(tr + 1.0) * 2 qw = 0.25 * S qx = (R[2, 1] - R[1, 2]) / S
def f(params): dic_params = { pname: param for pname, param in zip(param_names, params) } offset = 0.01 if object_name == "yball" else 0.0 init_poses = { "rake": { "push": np.array([0.0, 0.0, 0.0, -0.04 - offset, 0.0]), "draw": np.array([0.0, 0.0, 0.0, 0.055 + offset, 0.025]), "tap_from_right": np.array([0.0, 0.0, 0.0, 0.03, -0.145 - offset]), "tap_from_left": np.array([0.0, 0.0, 0.0, 0.02, 0.14 + offset]) }, "stick": { "push": np.array([0.0, 0.0, 0.0, -0.035 - offset, 0.0]), "draw": np.array([0.0, 0.0, 0.0, 0.035 + offset, 0.03]), "tap_from_right": np.array([0.0, 0.0, 0.0, 0.06, -0.065 - offset]), "tap_from_left": np.array([0.0, 0.0, 0.0, 0.06, 0.05 + offset]) }, "hook": { "push": np.array([0.0, 0.0, 0.0, -0.04 - offset, 0.0]), "draw": np.array([0.0, 0.0, 0.0, 0.15 + offset, 0.05]), "tap_from_right": np.array([0.0, 0.0, 0.0, 0.06, -0.075 - offset]), "tap_from_left": np.array([0.0, 0.0, 0.0, 0.09, 0.10 + offset]) } } costs = list() sim_eff_history = np.zeros((N_EXPERIMENTS, 2)) for tool_name in tools: for action_name in actions: target_pos, target_var, gnd_weight, mdist, real_eff_history = load_experiment( "affordance-datasets/visual-affordances-of-objects-and-tools/{}/{}/{}/effData.txt" .format(tool_name, object_name, action_name), get_eff_data=True) for iter in range(N_EXPERIMENTS): success = False while not success: try: if WATCHDOG: signal.alarm(10) with stdout_redirected(): robotID = load_robot(tool_name) toolID = robotID[0] nonlocal objID if (toolID == objID): raise ValueError mu = init_poses[tool_name][action_name] yaw, pitch, roll, x, y = np.random.normal( mu, np.array([1.0, 1.0, 1.0, 0.0, 0.0])) initial_xy = np.array([x, y]) p.resetBasePositionAndOrientation( objID, posObj=[x, y, 0.05], ornObj=[yaw, pitch, roll, 1]) dic_params2 = dict(dic_params) dic_params2['linearDamping'] = 0.0 dic_params2['angularDamping'] = 0.0 #dic_params2['mass']=0.001*models[object_name][1] #dic_params2['rollingFriction']=0.00001 #dic_params['lateralFriction']=0.0001 #dic_params['spinningFriction']=0.0001 #dic_params['rollingFriction']=0.0001 #del dic_params2['xnoise'] #del dic_params2['ynoise'] p.changeDynamics(objID, -1, **dic_params2) get_obj_xy(objID) speed = 0.04 if action_name == "push": base_speed = [-speed, 0, 0] base_pos_limit = lambda js: js[0] <= -0.12 elif action_name == "draw": base_speed = [speed, 0, 0] base_pos_limit = lambda js: js[0] >= 0.12 elif action_name == "tap_from_left": base_speed = [0, speed, 0] base_pos_limit = lambda js: js[1] >= 0.12 elif action_name == "tap_from_right": base_speed = [0, -speed, 0] base_pos_limit = lambda js: js[1] <= -0.12 else: raise ValueError # Let object fall to the ground and stop it pxyz, pori = p.getBasePositionAndOrientation(objID) position_after_fall = 100 * np.ones_like(pxyz) orientation_after_fall = 100 * np.ones_like(pori) while not np.allclose(position_after_fall[-1:], pxyz[-1:], atol=1e-6): p.stepSimulation() pxyz = position_after_fall position_after_fall, orientation_after_fall = p.getBasePositionAndOrientation( objID) p.resetBasePositionAndOrientation( objID, posObj=[ initial_xy[0], initial_xy[1], position_after_fall[2] ], ornObj=orientation_after_fall) p.resetBaseVelocity(objID, [0.0, 0.0, 0.0]) ppos = get_obj_xy(objID) npos = 100 * np.ones_like(ppos) iters = 0 # Move tool p.resetBaseVelocity(toolID, base_speed) action_finnished = False while not np.allclose(npos, ppos, atol=1e-6) or iters < 100: js, jor = p.getBasePositionAndOrientation( toolID) if (base_pos_limit(js)): p.resetBaseVelocity(toolID, [0, 0, 0]) action_finnished = True elif not action_finnished: p.resetBaseVelocity(toolID, base_speed) p.stepSimulation() ppos = npos npos = get_obj_xy(objID) if action_finnished: iters += 1 # time.sleep(1./1000.) pos = npos delete_robot(toolID) sim_eff_history[iter] = pos - initial_xy success = True except ValueError: p.resetSimulation() p.setAdditionalSearchPath( pybullet_data.getDataPath()) # optionally p.setGravity(0, 0, -9.8) p.setPhysicsEngineParameter(enableFileCaching=0) planeId = p.loadURDF("plane.urdf") p.changeDynamics(planeId, -1, restitution=.97, linearDamping=0.0, angularDamping=0.0) objID = load_object() kld = KLD(real_eff_history, sim_eff_history) if PLOTTING: plt.scatter(real_eff_history[:, 1], -real_eff_history[:, 0], s=40, c="red", edgecolors='none', label="real") plt.scatter(sim_eff_history[:, 1], -sim_eff_history[:, 0], s=40, c="blue", edgecolors='none', label="sim") plt.legend(loc=2) plt.ylim(-0.3, 0.3) plt.xlim(-0.3, 0.3) plt.title('action: {}, tool: {}'.format( action_name, tool_name)) plt.xlabel('[m]') plt.ylabel('[m]') plt.savefig("{}.png".format(kld)) plt.savefig("{}.pdf".format(kld)) plt.show() costs.append(kld) sim_eff_history = np.zeros((N_EXPERIMENTS, 2)) logging.info("cost:{}".format(kld)) if WATCHDOG: signal.alarm(0) out = sum(costs) / len(costs) print('\033[93m' + str(dic_params) + '\033[0m') pbar.set_description('cost: %0.2f' % (out)) pbar.update(1) return out
def init_decor(client): """ Creation of the different elements in the environment """ global x1, x2, x3 pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) pybullet.loadURDF("/decor/table/table.urdf", basePosition=[3, 0, 0.30], globalScaling=10.0, physicsClientId=client) pybullet.loadURDF("/decor/ball/totemball.urdf", basePosition=[2.7, x1, 0.65], physicsClientId=client) pybullet.loadURDF("/decor/teddy/totemteddybear.urdf", basePosition=[2.7, x2, 0.65], physicsClientId=client) pybullet.loadURDF("/decor/bird/totembird.urdf", basePosition=[2.7, x3, 0.65], physicsClientId=client) pybullet.loadURDF("/decor/bed/bed.urdf", basePosition=[0.3, -2.9, 1], physicsClientId=client) pybullet.loadURDF("/decor/box/box.urdf", basePosition=[0.5, 0.5, 0.19], physicsClientId=client) pybullet.loadURDF("/decor/chair/chair.urdf", basePosition=[3, -2, 0.3], physicsClientId=client) pybullet.loadURDF("/decor/night/night.urdf", basePosition=[0, -2, 0.2], physicsClientId=client) pybullet.loadURDF("/decor/floor/floor.urdf", basePosition=[3, 0, 0.01], globalScaling=10.0, physicsClientId=client)
import pybullet as p import pybullet_data as pd import time 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.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=[]
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))
import pybullet as p from time import sleep physicsClient = p.connect(p.GUI) p.setGravity(0,0,-10) planeId = p.loadURDF("plane.urdf") cubeStartPos = [0,0,1] cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) useRealTimeSimulation = 0 if (useRealTimeSimulation): p.setRealTimeSimulation(1) while 1: if (useRealTimeSimulation): p.setGravity(0,0,-10) sleep(0.01) # Time in seconds. else: p.stepSimulation()
def __init__(self, files, actuation_spec, observation_spec, gamma, horizon, timestep=1 / 240, n_intermediate_steps=1, debug_gui=False, **viewer_params): """ Constructor. Args: files (list): Paths to the URDF files to load; actuation_spec (list): A list of tuples specifying the names of the joints which should be controllable by the agent and tehir control mode. Can be left empty when all actuators should be used in position control; observation_spec (list): A list containing the names of data that should be made available to the agent as an observation and their type (ObservationType). An entry in the list is given by: (name, type); gamma (float): The discounting factor of the environment; horizon (int): The maximum horizon for the environment; timestep (float, 0.00416666666): The timestep used by the PyBullet simulator; n_intermediate_steps (int): The number of steps between every action taken by the agent. Allows the user to modify, control and access intermediate states; **viewer_params: other parameters to be passed to the viewer. See PyBulletViewer documentation for the available options. """ # Store simulation parameters self._timestep = timestep self._n_intermediate_steps = n_intermediate_steps # Create the simulation and viewer if debug_gui: pybullet.connect(pybullet.GUI) else: pybullet.connect(pybullet.DIRECT) pybullet.setTimeStep(self._timestep) pybullet.setGravity(0, 0, -9.81) pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) self._viewer = PyBulletViewer( self._timestep * self._n_intermediate_steps, **viewer_params) self._state = None # Load model and create access maps self._model_map = dict() for file_name, kwargs in files.items(): model_id = pybullet.loadURDF(file_name, **kwargs) model_name = pybullet.getBodyInfo(model_id)[1].decode('UTF-8') self._model_map[model_name] = model_id self._model_map.update(self._custom_load_models()) self._joint_map = dict() self._link_map = dict() for model_id in self._model_map.values(): for joint_id in range(pybullet.getNumJoints(model_id)): joint_data = pybullet.getJointInfo(model_id, joint_id) if joint_data[2] != pybullet.JOINT_FIXED: joint_name = joint_data[1].decode('UTF-8') self._joint_map[joint_name] = (model_id, joint_id) link_name = joint_data[12].decode('UTF-8') self._link_map[link_name] = (model_id, joint_id) # Read the actuation spec and build the mapping between actions and ids # as well as their limits assert (len(actuation_spec) > 0) self._action_data = list() for name, mode in actuation_spec: if name in self._joint_map: data = self._joint_map[name] + (mode, ) self._action_data.append(data) low, high = self._compute_action_limits() action_space = Box(np.array(low), np.array(high)) # Read the observation spec to build a mapping at every step. It is # ensured that the values appear in the order they are specified. if len(observation_spec) == 0: raise AttributeError( "No Environment observations were specified. " "Add at least one observation to the observation_spec.") else: self._observation_map = observation_spec # We can only specify limits for the joint positions, all other # information can be potentially unbounded low, high = self._compute_observation_limits() observation_space = Box(low, high) # Finally, we create the MDP information and call the constructor of # the parent class mdp_info = MDPInfo(observation_space, action_space, gamma, horizon) super().__init__(mdp_info) # Save initial state of the MDP self._initial_state = pybullet.saveState()
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()
float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2] ] return rayFrom, rayTo cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1. / 120.) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) p.loadURDF("./pybullet_data/plane100.urdf", useMaximalCoordinates=True) #disable rendering during creation. 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] #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH, fileName="duck.obj", rgbaColor=[1, 1, 1, 1], specularColor=[0.4, .4, 0], visualFramePosition=shift, meshScale=meshScale)
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])
import pybullet as p import math import numpy as np import pybullet_data p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) plane = p.loadURDF("plane100.urdf") cube = p.loadURDF("cube.urdf", [0, 0, 1]) def getRayFromTo(mouseX, mouseY): width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera( ) camPos = [ camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1], camTarget[2] - dist * camForward[2] ] farPlane = 10000 rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])] lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] + rayForward[2] * rayForward[2]) invLen = farPlane * 1. / lenFwd rayForward = [ invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2] ] rayFrom = camPos oneOverWidth = float(1) / float(width)
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])
PERIOD = np.array([2.0, 2.0, 2.0]) # sec PHASE = np.array([-np.pi / 4, 0, 0]) else: if not INVERSE: PERIOD = np.array([2.0, 2.0, 2.0]) # sec PHASE = np.array([0, 2 * np.pi / 3, -2 * np.pi / 3]) else: PERIOD = np.array([2.0, 2.0, 2.0]) # sec PHASE = np.array([0, -2 * np.pi / 3, 2 * np.pi / 3]) timestep = 0 # set env. physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf") snakeStartPos = [0, 0, 0.5] snakeStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) snakeId = p.loadURDF("./urdf/trident_snake.urdf", snakeStartPos, snakeStartOrientation) p.changeDynamics(bodyUniqueId=snakeId, linkIndex=2, lateralFriction=1) p.changeDynamics(bodyUniqueId=snakeId, linkIndex=5, lateralFriction=1) p.changeDynamics(bodyUniqueId=snakeId, linkIndex=7, lateralFriction=1) # enable joint F/T sensor num_joints = p.getNumJoints(snakeId) for i in range(num_joints): p.enableJointForceTorqueSensor(snakeId, i) def get_joint_data_array(body_id, joint_id):
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]]
import pybullet as p import time import pybullet_data import pybullet_envs p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # to load plane.urdf p.loadURDF("plane.urdf") # p.loadURDF("urdf/plane.urdf",basePosition=[0,0,-1.5]) # humanoid = p.loadURDF("cassie/urdf/cassie_collide.urdf",[0,0,0.8], useFixedBase=False) # humanoid = p.loadURDF("urdf/simbicon_urdf/biped2d.urdf",[0, 0, 1.2]) # humanoid = p.loadURDF("urdf/simbicon_urdf/humanoid_nohead.urdf",[0, 0, 0.31]) # humanoid = p.loadURDF("urdf/simbicon_urdf/flame.urdf",[0, 0, 1.0]) humanoid = p.loadURDF("urdf/simbicon_urdf/flame3.urdf", [0, 0, 0.9]) # humanoid = p.loadURDF("urdf/simbicon_urdf/demo.urdf") # gravId = p.addUserDebugParameter("gravity",-10,10,-10) gravId = p.addUserDebugParameter("gravity", -10, 10, -10) jointIds = [] paramIds = [] test_visual = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.2, 1, 0.1], rgbaColor=[1, 0, 0, 1]) test_collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 1, 0.1]) test_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=test_collision, \ baseVisualShapeIndex=test_visual, basePosition = [-0.15, 0, 0]) # shift = [0, -0.02, 0] # meshScale = [0.1, 0.1, 0.1] # visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,