Ejemplo n.º 1
0
def setupWorld():
	p.resetSimulation()
	p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
	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)
Ejemplo n.º 2
0
 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)
import pybullet as p
import time
import pybullet_data

p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.)
p.setPhysicsEngineParameter(numSubSteps=1)

p.loadURDF("plane.urdf")

objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob, [0.789351, 0.962124, 0.113124],
                                  [0.710965, 0.218117, 0.519402, -0.420923])
jointPositions = [
    -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000,
    -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000,
    0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000,
    -0.007374, 0.000000
]
for jointIndex in range(p.getNumJoints(ob)):
  p.resetJointState(ob, jointIndex, jointPositions[jointIndex])

#first let the humanoid fall
#p.setRealTimeSimulation(1)
#time.sleep(5)
p.setRealTimeSimulation(0)
#p.saveWorld("lyiing.py")
Ejemplo n.º 5
0
import pybullet as p
import time

p.connect(p.GUI)
p.loadURDF("plane.urdf",useMaximalCoordinates=True)
p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)

gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
for i in range (10):
    for j in range (10):
        for k in range (5):
            ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
while True:
    gravX = p.readUserDebugParameter(gravXid)
    gravY = p.readUserDebugParameter(gravYid)
    gravZ = p.readUserDebugParameter(gravZid)
    p.setGravity(gravX,gravY,gravZ)
    time.sleep(0.01)

Ejemplo n.º 6
0
import pybullet as p
import time

p.connect(p.GUI)
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(enableSAT=1)
p.loadURDF("cube_concave.urdf", [0, 0, -25],
           globalScaling=50,
           useFixedBase=True,
           flags=p.URDF_INITIALIZE_SAT_FEATURES)
p.loadURDF("cube.urdf", [0, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES)
p.loadURDF("duck_vhacd.urdf", [1, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES)

while (p.isConnected()):
  p.stepSimulation()
  pts = p.getContactPoints()
  #print("num contacts = ", len(pts))
  time.sleep(1. / 240.)
Ejemplo n.º 7
0
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],
Ejemplo n.º 8
0

def birrt_smoothing():
    ################################################################
    # TODO your code to implement the birrt algorithm with smoothing
    ################################################################
    pass


if __name__ == "__main__":
    args = get_args()

    # set up simulator
    physicsClient = p.connect(p.GUI)
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setPhysicsEngineParameter(enableFileCaching=0)
    p.setGravity(0, 0, -9.8)
    p.configureDebugVisualizer(p.COV_ENABLE_GUI, False)
    p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, True)
    p.resetDebugVisualizerCamera(cameraDistance=1.400,
                                 cameraYaw=58.000,
                                 cameraPitch=-42.200,
                                 cameraTargetPosition=(0.0, 0.0, 0.0))

    # load objects
    plane = p.loadURDF("plane.urdf")
    ur5 = p.loadURDF('assets/ur5/ur5.urdf',
                     basePosition=[0, 0, 0.02],
                     useFixedBase=True)
    obstacle1 = p.loadURDF('assets/block.urdf',
                           basePosition=[1 / 4, 0, 1 / 2],
Ejemplo n.º 9
0
    def setup_simulation(self, gui=False, easy_bookcases=False, clientId=None):
        '''
        Initializes the simulation by setting up the environment and spawning
        all objects used later.

        Params:
            gui (bool): Specifies if a GUI should be spawned.
        '''
        # Setup simulation parameters
        if clientId is None:
            mode = pb.GUI if gui else pb.DIRECT
            self.clientId = pb.connect(mode)
        else:
            self.clientId = clientId
        pb.setGravity(0.0, 0.0, 0.0, self.clientId)
        pb.setPhysicsEngineParameter(fixedTimeStep=self.p["world"]["tau"],
                                     physicsClientId=self.clientId)
        pb.setAdditionalSearchPath(pybullet_data.getDataPath())

        pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0)
        pb.configureDebugVisualizer(pb.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
        pb.configureDebugVisualizer(pb.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
        pb.configureDebugVisualizer(pb.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
        pb.setPhysicsEngineParameter(enableFileCaching=0)

        # Setup humans
        self.humans = [
            Human(self.clientId, self.tau)
            for _ in range(self.p['world']['n_humans'])
        ]

        # Spawn robot
        self.robotId = self.spawn_robot()

        # Spawn setpoint
        self.spId = self.spawn_setpoint()

        # Spawn all objects in the environment
        self.additionalIds = self.spawn_additional_objects()

        # Enable collision of base and all objects
        # for id in self.additionalIds:
        #     pb.setCollisionFilterPair(self.robotId, id, -1, -1, True,
        #                               self.clientId)

        # Spawn bookcases
        self.spawn_kallax()

        # Figure out joint mapping: self.joint_mapping maps as in
        # desired_mapping list.
        self.joint_mapping = np.zeros(7, dtype=int)
        self.link_mapping = np.zeros(self.n_links, dtype=int)
        self.joint_limits = np.zeros((7, 2), dtype=float)
        self.eeLinkId = None
        self.baseLinkId = None
        self.lidarLinkId1 = None
        self.lidarLinkId2 = None

        joint_names = ["panda_joint{}".format(x) for x in range(1, 8)]
        link_names = self.p["joints"]["link_names"]

        for j in range(
                pb.getNumJoints(self.robotId, physicsClientId=self.clientId)):
            info = pb.getJointInfo(self.robotId,
                                   j,
                                   physicsClientId=self.clientId)
            j_name, l_name = info[1].decode("utf-8"), info[12].decode("utf-8")
            idx = info[0]
            if j_name in joint_names:
                map_idx = joint_names.index(j_name)
                self.joint_mapping[map_idx] = idx
                self.joint_limits[map_idx, :] = info[8:10]
            if l_name in link_names:
                self.link_mapping[link_names.index(l_name)] = idx
            if l_name == self.p["joints"]["ee_link_name"]:
                self.eeLinkId = idx
            if l_name == self.p["joints"]["base_link_name"]:
                self.baseLinkId = idx
            if l_name == self.p["sensors"]["lidar"]["link_id1"]:
                self.lidarLinkId1 = idx
            if l_name == self.p["sensors"]["lidar"]["link_id2"]:
                self.lidarLinkId2 = idx

        for j in range(
                pb.getNumJoints(self.spId, physicsClientId=self.clientId)):
            info = pb.getJointInfo(self.spId, j, physicsClientId=self.clientId)
            link_name = info[12].decode("utf-8")
            idx = info[0]
            if link_name == "grasp_loc":
                self.spGraspLinkId = idx

        self.actuator_selection = np.zeros(7, bool)
        for i, name in enumerate(joint_names):
            if name in self.p["joints"]["joint_names"]:
                self.actuator_selection[i] = 1

        # Prepare lidar
        n_scans = self.p["sensors"]["lidar"]["n_scans"]
        mag_ang = self.p["sensors"]["lidar"]["ang_mag"]
        scan_range = self.p["sensors"]["lidar"]["range"]
        angs = ((np.array(range(n_scans)) - (n_scans - 1) / 2.0) * 2.0 /
                n_scans * mag_ang)
        r_uv = np.vstack((np.cos(angs), np.sin(angs), np.zeros(angs.shape[0])))
        r_from = r_uv * 0.1
        r_to = r_uv * scan_range

        self.rays = (r_from, r_to)

        for human in self.humans:
            self.configure_ext_collisions(human.leg_l, self.robotId,
                                          self.collision_links)
            self.configure_ext_collisions(human.leg_r, self.robotId,
                                          self.collision_links)
Ejemplo n.º 10
0
p.connect(p.GUI)
useMaximalCoordinates = False

p.setGravity(0,0,-10)
plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinates)

p.setRealTimeSimulation(0)


velocity = 1
num = 40
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.setPhysicsEngineParameter(enableConeFriction=1)

for i in range (num):
	print("progress:",i,num)
	
	x = velocity*math.sin(2.*3.1415*float(i)/num)
	y = velocity*math.cos(2.*3.1415*float(i)/num)
	print("velocity=",x,y)
	sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates)
	p.changeDynamics(sphere,-1,lateralFriction=0.02)
	#p.changeDynamics(sphere,-1,rollingFriction=10)
	p.changeDynamics(sphere,-1,linearDamping=0)
	p.changeDynamics(sphere,-1,angularDamping=0)
	p.resetBaseVelocity(sphere,linearVelocity=[x,y,0])

	prevPos = [0,0,0]
Ejemplo n.º 11
0
import pybullet as p
import time
p.connect(p.DIRECT)
p.setGravity(0,0,-10)
p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.)
p.setPhysicsEngineParameter(numSubSteps=1)

objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000])
ob = objects[1]
p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923])
jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
	p.resetJointState(ob,jointIndex,jointPositions[jointIndex])


#first let the humanoid fall
#p.setRealTimeSimulation(1)
#time.sleep(5)
p.setRealTimeSimulation(0)
#p.saveWorld("lyiing.py")

#now do a benchmark
print("Starting benchmark")
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"pybullet_humanoid_timings.json")
for i in range(1000):
	p.stepSimulation()
p.stopStateLogging(logId)
Ejemplo n.º 12
0
import pybullet as p
import time

p.connect(p.GUI)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM = 0.000001)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)


p.loadURDF("plane.urdf")
radius = 0.025
distance = 1.86
yaw=135
pitch=-11
targetPos=[0,0,0]

p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200)
p.resetDebugVisualizerCamera(distance, yaw,pitch, targetPos)
objectId = -1

for i in range (10):
	objectId = p.loadURDF("cube_small.urdf",[1,1,radius+i*2*radius])

p.changeDynamics(objectId,-1,100)

timeStep = 1./240.
p.setGravity(0,0,-10)
while (p.isConnected()):
	p.stepSimulation()
	time.sleep(timeStep)
Ejemplo n.º 13
0
restitutionId = p.addUserDebugParameter("restitution", 0, 1, 1)
restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold", 0, 3, 0.2)

lateralFrictionId = p.addUserDebugParameter("lateral friction", 0, 1, 0.5)
spinningFrictionId = p.addUserDebugParameter("spinning friction", 0, 1, 0.03)
rollingFrictionId = p.addUserDebugParameter("rolling friction", 0, 1, 0.03)

plane = p.loadURDF("plane_with_restitution.urdf")
sphere = p.loadURDF("sphere_with_restitution.urdf", [0, 0, 2])

p.setRealTimeSimulation(1)
p.setGravity(0, 0, -10)
while (1):
  restitution = p.readUserDebugParameter(restitutionId)
  restitutionVelocityThreshold = p.readUserDebugParameter(restitutionVelocityThresholdId)
  p.setPhysicsEngineParameter(restitutionVelocityThreshold=restitutionVelocityThreshold)

  lateralFriction = p.readUserDebugParameter(lateralFrictionId)
  spinningFriction = p.readUserDebugParameter(spinningFrictionId)
  rollingFriction = p.readUserDebugParameter(rollingFrictionId)
  p.changeDynamics(plane, -1, lateralFriction=1)
  p.changeDynamics(sphere, -1, lateralFriction=lateralFriction)
  p.changeDynamics(sphere, -1, spinningFriction=spinningFriction)
  p.changeDynamics(sphere, -1, rollingFriction=rollingFriction)

  p.changeDynamics(plane, -1, restitution=restitution)
  p.changeDynamics(sphere, -1, restitution=restitution)
  pos, orn = p.getBasePositionAndOrientation(sphere)
  #print("pos=")
  #print(pos)
  time.sleep(0.01)
Ejemplo n.º 14
0
import pybullet as p
import time

p.connect(p.GUI)
fileIO = p.loadPlugin("fileIOPlugin")
if (fileIO>=0):
	p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO])
	objs= p.loadSDF("pickup/model.sdf")
	dobot =objs[0]
	p.changeVisualShape(dobot,-1,rgbaColor=[1,1,1,1])
		
else:
	print("fileIOPlugin is disabled.")


p.setPhysicsEngineParameter(enableFileCaching=False)

while (1):
	p.stepSimulation()
	time.sleep(1./240.)
Ejemplo n.º 15
0
	def clean_everything(self):
		#p.resetSimulation()
		p.setGravity(0, 0, -self.gravity)
		p.setDefaultContactERP(0.9)
		p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=5, numSubSteps=self.frame_skip)
Ejemplo n.º 16
0
def run_simulation(
    hand_verts,
    hand_faces,
    obj_verts,
    obj_faces,
    simulation_step=1 / 240,
    num_iterations=35,
    object_friction=3,
    hand_friction=3,
    hand_restitution=0,
    object_restitution=0.5,
    object_mass=1,
    verbose=False,
    vhacd_resolution=1000,
    vhacd_exe='/home/ecorona/GHANds/v-hacd/bin/linux/testVHACD',
    wait_time=0,
    save_video=False,
    save_video_path=None,
    save_hand_path=None,
    save_obj_path=None,
    save_simul_folder=None,
    use_gui=False,
):
    if use_gui:
        conn_id = p.connect(p.GUI)
    else:
        conn_id = p.connect(p.DIRECT)

    hand_indicies = hand_faces.flatten().tolist()
    p.resetSimulation(physicsClientId=conn_id)
    p.setPhysicsEngineParameter(enableFileCaching=0, physicsClientId=conn_id)
    p.setPhysicsEngineParameter(numSolverIterations=150,
                                physicsClientId=conn_id)
    p.setPhysicsEngineParameter(fixedTimeStep=simulation_step,
                                physicsClientId=conn_id)
    p.setGravity(0, 9.8, 0, physicsClientId=conn_id)

    # add hand
    base_tmp_dir = "tmp/objs"
    os.makedirs(base_tmp_dir, exist_ok=True)
    hand_tmp_fname = tempfile.mktemp(suffix=".obj", dir=base_tmp_dir)
    save_obj(hand_tmp_fname, hand_verts, hand_faces)

    if save_hand_path is not None:
        shutil.copy(hand_tmp_fname, save_hand_path)

    hand_collision_id = p.createCollisionShape(
        p.GEOM_MESH,
        fileName=hand_tmp_fname,
        flags=p.GEOM_FORCE_CONCAVE_TRIMESH,
        indices=hand_indicies,
        physicsClientId=conn_id,
    )
    hand_visual_id = p.createVisualShape(
        p.GEOM_MESH,
        fileName=hand_tmp_fname,
        rgbaColor=[0, 0, 1, 1],
        specularColor=[0, 0, 1],
        physicsClientId=conn_id,
    )

    hand_body_id = p.createMultiBody(
        baseMass=0,
        baseCollisionShapeIndex=hand_collision_id,
        baseVisualShapeIndex=hand_visual_id,
        physicsClientId=conn_id,
    )

    p.changeDynamics(
        hand_body_id,
        -1,
        lateralFriction=hand_friction,
        restitution=hand_restitution,
        physicsClientId=conn_id,
    )

    obj_tmp_fname = tempfile.mktemp(suffix=".obj", dir=base_tmp_dir)
    os.makedirs(base_tmp_dir, exist_ok=True)
    # Save object obj
    if save_obj_path is not None:
        final_obj_tmp_fname = tempfile.mktemp(suffix=".obj", dir=base_tmp_dir)
        save_obj(final_obj_tmp_fname, obj_verts, obj_faces)
        shutil.copy(final_obj_tmp_fname, save_obj_path)
    # Get obj center of mass
    obj_center_mass = np.mean(obj_verts, axis=0)
    obj_verts -= obj_center_mass
    # add object
    use_vhacd = True

    if use_vhacd:
        if verbose:
            print("Computing vhacd decomposition")
            time1 = time.time()
        # convex hull decomposition
        save_obj(obj_tmp_fname, obj_verts, obj_faces)

        if not vhacd(obj_tmp_fname, vhacd_exe, resolution=vhacd_resolution):
            raise RuntimeError("Cannot compute convex hull "
                               "decomposition for {}".format(obj_tmp_fname))
        else:
            print("Succeeded vhacd decomp")

        obj_collision_id = p.createCollisionShape(p.GEOM_MESH,
                                                  fileName=obj_tmp_fname,
                                                  physicsClientId=conn_id)
        if verbose:
            time2 = time.time()
            print("Computed v-hacd decomposition at res {} {:.6f} s".format(
                vhacd_resolution, (time2 - time1)))
    else:
        obj_collision_id = p.createCollisionShape(p.GEOM_MESH,
                                                  vertices=obj_verts,
                                                  physicsClientId=conn_id)

    obj_visual_id = p.createVisualShape(
        p.GEOM_MESH,
        fileName=obj_tmp_fname,
        rgbaColor=[1, 0, 0, 1],
        specularColor=[1, 0, 0],
        physicsClientId=conn_id,
    )
    obj_body_id = p.createMultiBody(
        baseMass=object_mass,
        basePosition=obj_center_mass,
        baseCollisionShapeIndex=obj_collision_id,
        baseVisualShapeIndex=obj_visual_id,
        physicsClientId=conn_id,
    )

    p.changeDynamics(
        obj_body_id,
        -1,
        lateralFriction=object_friction,
        restitution=object_restitution,
        physicsClientId=conn_id,
    )

    # simulate for several steps
    if save_video:
        images = []
        if use_gui:
            renderer = p.ER_BULLET_HARDWARE_OPENGL
        else:
            renderer = p.ER_TINY_RENDERER

    for step_idx in range(num_iterations):
        p.stepSimulation(physicsClientId=conn_id)
        if save_video:
            img = take_picture(renderer, conn_id=conn_id)
            images.append(img)
        if save_simul_folder:
            hand_step_path = os.path.join(save_simul_folder,
                                          "{:08d}_hand.obj".format(step_idx))
            shutil.copy(hand_tmp_fname, hand_step_path)
            obj_step_path = os.path.join(save_simul_folder,
                                         "{:08d}_obj.obj".format(step_idx))
            pos, orn = p.getBasePositionAndOrientation(obj_body_id,
                                                       physicsClientId=conn_id)
            mat = np.reshape(p.getMatrixFromQuaternion(orn), (3, 3))
            obj_verts_t = pos + np.dot(mat, obj_verts.T).T
            save_obj(obj_step_path, obj_verts_t, obj_faces)
        time.sleep(wait_time)

    if save_video:
        write_video(images, save_video_path)
        print("Saved gif to {}".format(save_video_path))
    pos_end = p.getBasePositionAndOrientation(obj_body_id,
                                              physicsClientId=conn_id)[0]

    #if use_vhacd:
    #    os.remove(obj_tmp_fname)
    if save_obj_path is not None:
        os.remove(final_obj_tmp_fname)
    os.remove(hand_tmp_fname)
    distance = np.linalg.norm(pos_end - obj_center_mass)
    p.disconnect(physicsClientId=conn_id)
    return distance
Ejemplo n.º 17
0
    p.resetJointState(robot, joint_id["r_leg_kny"], np.pi / 2, 0.)
    # ankle
    p.resetJointState(robot, joint_id["l_leg_aky"], -np.pi / 4, 0.)
    p.resetJointState(robot, joint_id["r_leg_aky"], -np.pi / 4, 0.)


if __name__ == "__main__":

    # Environment Setup
    p.connect(p.GUI)
    p.resetDebugVisualizerCamera(cameraDistance=1.5,
                                 cameraYaw=120,
                                 cameraPitch=-30,
                                 cameraTargetPosition=[1, 0.5, 1.5])
    p.setGravity(0, 0, -9.8)
    p.setPhysicsEngineParameter(fixedTimeStep=SimConfig.CONTROLLER_DT,
                                numSubSteps=SimConfig.N_SUBSTEP)
    if SimConfig.VIDEO_RECORD:
        if not os.path.exists('video'):
            os.makedirs('video')
        for f in os.listdir('video'):
            os.remove('video/' + f)
        p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "video/atlas.mp4")

    # Create Robot, Ground
    p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
    robot = p.loadURDF(
        cwd + "/robot_model/atlas/atlas.urdf",
        SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT,
        SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT)

    p.loadURDF(cwd + "/robot_model/ground/plane.urdf", [0, 0, 0])
Ejemplo n.º 18
0
from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerStable import PDControllerStableMultiDof

explicitPD = PDControllerExplicitMultiDof(p)
stablePD = PDControllerStableMultiDof(p)

p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16])

import pybullet_data
p.setTimeOut(10000)
useMotionCapture=False
useMotionCaptureReset=False#not useMotionCapture
useExplicitPD = True

p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(numSolverIterations=30)
#p.setPhysicsEngineParameter(solverResidualThreshold=1e-30)

#explicit PD control requires small timestep
timeStep = 1./600.
#timeStep = 1./240.

p.setPhysicsEngineParameter(fixedTimeStep=timeStep)

path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
#path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt"
#path = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"


#p.loadURDF("plane.urdf",[0,0,-1.03])
print("path = ", path)
Ejemplo n.º 19
0
    def reset(self):
        look = [1.9, 0.5, 1]
        roll = -10
        pitch = -35
        yaw = 110

        look_2 = [-0.3, 0.5, 1.3]
        pitch_2 = -56
        yaw_2 = 245
        roll_2 = 0
        distance = 1.

        self._view_matrix_2 = p.computeViewMatrixFromYawPitchRoll(
            look_2, distance, yaw_2, pitch_2, roll_2, 2)

        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)

        # counter and flag
        self.goal_rotation_angle = 0
        self._env_step = 0
        self.terminated = 0
        self.finger_angle = 0.3
        self._success = False

        self.out_of_range = False
        self._collision_box = False
        self.drop_down = False
        self._rank_before = 0
        self.inverse_rank = 0

        p.resetSimulation()
        p.setPhysicsEngineParameter(numSolverIterations=300)
        p.setTimeStep(self._timeStep)

        self._planeUid = p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"),
                                    [0, 0, -1])
        self._tableUid = p.loadURDF(
            os.path.join(self._urdfRoot, "table/table.urdf"),
            [0.5000000, 0.00000, -.820000],
            p.getQuaternionFromEuler(np.radians([0, 0, 90.])))

        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, test=self._isTest)
        self._objectUids = self._randomly_place_objects(urdfList)
        self._init_obj_high = self._obj_high()

        self._rank_before = self._rank_1()

        self._domain_random()
        return self._get_observation()
Ejemplo n.º 20
0
#add parent dir to find package. Only needed for source code build, pip install doesn't need it.
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)

import pybullet_data
import pybullet as p
import time

p.connect(p.GUI_SERVER)
p.setAdditionalSearchPath(pybullet_data.getDataPath())

while(1):
	#this is a no-op command, to allow GUI updates on Mac OSX (main thread)
	p.setPhysicsEngineParameter()
	time.sleep(0.01)
	
import pybullet as p
import time

p.connect(p.GUI)
p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)


terrain_mass=0
terrain_visual_shape_id=-1
terrain_position=[0,0,0]
terrain_orientation=[0,0,0,1]
terrain_collision_shape_id = p.createCollisionShape(
	shapeType=p.GEOM_MESH,
  fileName="terrain.obj",
  flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE,
  meshScale=[0.5, 0.5, 0.5])
p.createMultiBody(
	terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
	terrain_position, terrain_orientation)
            
            
useMaximalCoordinates = True
sphereRadius = 0.005
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])

mass = 1
visualShapeId = -1


for i in range (5):
Ejemplo n.º 22
0
    def reset(self):
        self.terminated = False
        p.resetSimulation()
        p.setPhysicsEngineParameter(numSolverIterations=150)
        p.setTimeStep(self._timestep)
        p.loadURDF(os.path.join(self._urdf_root, "plane.urdf"), [0, 0, 0])
        p.setGravity(0, 0, -10)

        # Init the robot randomly
        x_start = self._max_x / 2 + self.np_random.uniform(
            -self._max_x / 3, self._max_x / 3)
        y_start = self._max_y / 2 + self.np_random.uniform(
            -self._max_y / 3, self._max_y / 3)
        self.robot_pos = np.array([x_start, y_start, 0])

        # Initialize target position
        x_pos = 0.9 * self._max_x
        y_pos = self._max_y * 3 / 4
        if self._random_target:
            margin = 0.1 * self._max_x
            x_pos = self.np_random.uniform(self._min_x + margin,
                                           self._max_x - margin)
            y_pos = self.np_random.uniform(self._min_y + margin,
                                           self._max_y - margin)

        self.target_uid = p.loadURDF("/urdf/cylinder.urdf", [x_pos, y_pos, 0],
                                     useFixedBase=True)
        self.target_pos = np.array([x_pos, y_pos, 0])

        # Add walls
        # Path to the urdf file
        wall_urdf = "/urdf/wall.urdf"
        # Rgba colors
        red, green, blue = [0.8, 0, 0, 1], [0, 0.8, 0, 1], [0, 0, 0.8, 1]

        wall_left = p.loadURDF(wall_urdf, [self._max_x / 2, 0, 0],
                               useFixedBase=True)
        # Change color
        p.changeVisualShape(wall_left, -1, rgbaColor=red)

        # getQuaternionFromEuler -> define orientation
        wall_bottom = p.loadURDF(wall_urdf, [self._max_x, self._max_y / 2, 0],
                                 p.getQuaternionFromEuler([0, 0, np.pi / 2]),
                                 useFixedBase=True)

        wall_right = p.loadURDF(wall_urdf, [self._max_x / 2, self._max_y, 0],
                                useFixedBase=True)
        p.changeVisualShape(wall_right, -1, rgbaColor=green)

        wall_top = p.loadURDF(wall_urdf, [self._min_x, self._max_y / 2, 0],
                              p.getQuaternionFromEuler([0, 0, np.pi / 2]),
                              useFixedBase=True)
        p.changeVisualShape(wall_top, -1, rgbaColor=blue)

        self.walls = [wall_left, wall_bottom, wall_right, wall_top]

        # Add mobile robot
        self.robot_uid = p.loadURDF(os.path.join(self._urdf_root,
                                                 "racecar/racecar.urdf"),
                                    self.robot_pos,
                                    useFixedBase=True)

        self._env_step_counter = 0
        for _ in range(50):
            p.stepSimulation()

        self._observation = self.getObservation()

        if self.saver is not None:
            self.saver.reset(self._observation, self.getTargetPos(),
                             self.getRobotPos())

        if self.srl_model != "raw_pixels":
            return self.getSRLState(self._observation)

        return np.array(self._observation)
Ejemplo n.º 23
0
maxMotorForce = 5000
maxGearForce = 10000
jointFriction = 0.1


p.connect(p.GUI)


amplitudeId= p.addUserDebugParameter("amplitude",0,3.14,0.5)
timeScaleId = p.addUserDebugParameter("timeScale",0,10,1)


jointTypeNames={}
jointTypeNames[p.JOINT_REVOLUTE]="JOINT_REVOLUTE"
jointTypeNames[p.JOINT_FIXED]="JOINT_FIXED"
p.setPhysicsEngineParameter(numSolverIterations=100)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)


jointNamesToIndex={}


p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)


for j in range(p.getNumJoints(vision)):
Ejemplo n.º 24
0
    def generate_dataset(self):

        for num_brick in tqdm(
                range(self.min_num_brick, self.max_num_brick + 1),
                "Number of Brick Progress",
        ):
            # separate dataset for GN-based physics engine (GN Physics)
            dataset_name = self.dataset_prefix + "_" + str(num_brick)

            nodes_train, nodes_eval = [], []
            edges_train, edges_eval = [], []
            node_feats_train, node_feats_eval = [], []
            edge_feats_train, edge_feats_eval = [], []

            split = int(self.num_episode * self.train_eval_split)
            for episode in tqdm(range(self.num_episode), "Episode Progress"):
                if episode < split:
                    nodes_dummy = nodes_train
                    edges_dummy = edges_train
                    node_feats_dummy = node_feats_train
                    edge_feats_dummy = edge_feats_train
                    episode_dummy = copy.deepcopy(episode)
                else:
                    nodes_dummy = nodes_eval
                    edges_dummy = edges_eval
                    node_feats_dummy = node_feats_eval
                    edge_feats_dummy = edge_feats_eval
                    episode_dummy = copy.deepcopy(episode) - split

                # use test_simulator.py for debugging and visualization
                p.connect(p.DIRECT)

                p.setAdditionalSearchPath(pybullet_data.getDataPath())
                p.setPhysicsEngineParameter(numSolverIterations=10)
                p.setTimeStep(1.0 / 60.0)

                # generate new design
                self.designer.clear()
                self.designer.generate_design(num_brick=num_brick)

                for b in self.designer.built:
                    pos = b.anchor
                    rot = p.getQuaternionFromEuler(
                        np.array(b.rotation) * np.pi / 2)
                    p.loadURDF("urdf/brick.urdf", pos, rot)
                p.loadURDF("plane.urdf", useMaximalCoordinates=True)

                # start simulation
                p.setGravity(0, 0, -10)
                for frame in range(self.num_frame):
                    p.stepSimulation()
                    time.sleep(1.0 / 240.0)

                    # remember the ground!
                    for objectID in range(num_brick + 1):
                        pos, ort = p.getBasePositionAndOrientation(objectID)
                        rot = p.getEulerFromQuaternion(ort)
                        vel_linear, vel_angular = p.getBaseVelocity(objectID)
                        if objectID < num_brick:
                            mass = 1.0
                        else:
                            mass = 9999999.0

                        nodes_dummy.append([episode_dummy, frame, objectID])
                        node_feats_dummy.append(pos + rot + vel_linear +
                                                vel_angular + (mass, ))

                    contact_list = p.getContactPoints()
                    for contact in contact_list:
                        edges_dummy.append(
                            [episode_dummy, frame, contact[1], contact[2]])
                        edge_feats_dummy.append(contact[5]  # positionA
                                                + contact[6]  # positionB
                                                + (
                                                    contact[9],  # normalForce
                                                    contact[10],  # friction1
                                                    contact[12],  # friction2
                                                ))

                p.disconnect()

            np.savez(
                self.save_dir / str(dataset_name + "_TRAIN"),
                nodes=np.array(nodes_train),
                node_feats=np.array(node_feats_train),
                edges=np.array(edges_train),
                edge_feats=np.array(edge_feats_train),
            )
            np.savez(
                self.save_dir / str(dataset_name + "_EVAL"),
                nodes=np.array(nodes_eval),
                node_feats=np.array(node_feats_eval),
                edges=np.array(edges_eval),
                edge_feats=np.array(edge_feats_eval),
            )
Ejemplo n.º 25
0
import sys
import time
import numpy as np
import pybullet as p
from scipy.spatial.transform import Rotation as R
from env.robot import Manipulator
from env.work import Work

if __name__ == '__main__':

    p.connect(p.GUI)
    p.setPhysicsEngineParameter(enableFileCaching=0)
    p.setRealTimeSimulation(False)
    p.setGravity(0, 0, -9.8)
    p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
    p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)

    # Plane
    plane_pos = [0, 0, -0.1]
    p.loadURDF("urdf/plane/plane.urdf", plane_pos)

    robot_base_pose = [0, 0, 0, 0, 0, 0]
    #robot_tool_pose = [0, 0, 0, -0.3, 0.4, 0.2]
    robot_tool_pose = [0, 0, -0.1, 0.0, 0.0, 0.0]

    robot = Manipulator(tool_pose=robot_tool_pose, base_pose=robot_base_pose)

    # Reset joint position
    basic_joint = np.array([
        0.0, -0.5 * np.pi, 1.0 * np.pi, 0.0 * np.pi, 0.5 * np.pi, 0.0 * np.pi
    ])
Ejemplo n.º 26
0
    def __init__(self,
                 assets_root,
                 task=None,
                 disp=False,
                 shared_memory=False,
                 hz=240):
        """Creates OpenAI Gym-style environment with PyBullet.

    Args:
      assets_root: root directory of assets.
      task: the task to use. If None, the user must call set_task for the
        environment to work properly.
      disp: show environment with PyBullet's built-in display viewer.
      shared_memory: run with shared memory.
      hz: PyBullet physics simulation step speed. Set to 480 for deformables.

    Raises:
      RuntimeError: if pybullet cannot load fileIOPlugin.
    """
        self.pix_size = 0.003125
        self.obj_ids = {'fixed': [], 'rigid': [], 'deformable': []}
        self.homej = np.array([-1, -0.5, 0.5, -0.5, -0.5, 0]) * np.pi
        self.agent_cams = cameras.RealSenseD415.CONFIG

        self.assets_root = assets_root

        color_tuple = [
            gym.spaces.Box(0,
                           255,
                           config['image_size'] + (3, ),
                           dtype=np.uint8) for config in self.agent_cams
        ]
        depth_tuple = [
            gym.spaces.Box(0.0, 20.0, config['image_size'], dtype=np.float32)
            for config in self.agent_cams
        ]
        self.observation_space = gym.spaces.Dict({
            'color':
            gym.spaces.Tuple(color_tuple),
            'depth':
            gym.spaces.Tuple(depth_tuple),
        })
        # TODO(ayzaan): Delete below and uncomment vector box bounds.
        position_bounds = gym.spaces.Box(low=-1.0,
                                         high=1.0,
                                         shape=(3, ),
                                         dtype=np.float32)
        # position_bounds = gym.spaces.Box(
        #     low=np.array([0.25, -0.5, 0.], dtype=np.float32),
        #     high=np.array([0.75, 0.5, 0.28], dtype=np.float32),
        #     shape=(3,),
        #     dtype=np.float32)
        self.action_space = gym.spaces.Dict({
            'pose0':
            gym.spaces.Tuple((position_bounds,
                              gym.spaces.Box(-1.0,
                                             1.0,
                                             shape=(4, ),
                                             dtype=np.float32))),
            'pose1':
            gym.spaces.Tuple((position_bounds,
                              gym.spaces.Box(-1.0,
                                             1.0,
                                             shape=(4, ),
                                             dtype=np.float32)))
        })

        # Start PyBullet.
        disp_option = p.DIRECT
        if disp:
            disp_option = p.GUI
            if shared_memory:
                disp_option = p.SHARED_MEMORY
        client = p.connect(disp_option)
        file_io = p.loadPlugin('fileIOPlugin', physicsClientId=client)
        if file_io < 0:
            raise RuntimeError('pybullet: cannot load FileIO!')
        if file_io >= 0:
            p.executePluginCommand(file_io,
                                   textArgument=assets_root,
                                   intArgs=[p.AddFileIOAction, p.CNSFileIO],
                                   physicsClientId=client)

        p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
        p.setPhysicsEngineParameter(enableFileCaching=0)
        p.setAdditionalSearchPath(assets_root)
        p.setAdditionalSearchPath(tempfile.gettempdir())
        p.setTimeStep(1. / hz)

        # If using --disp, move default camera closer to the scene.
        if disp:
            target = p.getDebugVisualizerCamera()[11]
            p.resetDebugVisualizerCamera(cameraDistance=1.1,
                                         cameraYaw=90,
                                         cameraPitch=-25,
                                         cameraTargetPosition=target)

        if task:
            self.set_task(task)
Ejemplo n.º 27
0
    def init_simulation(self,
                        floor_height=0.3,
                        visualize=True,
                        sim_timestep=0.001,
                        cont_timestep_mult=16,
                        lateral_friction=1.0,
                        joint_damping=0.0,
                        contact_stiffness=10000.0,
                        contact_damping=200.0,
                        contact_damping_multiplier=None):

        self.visualize = visualize
        if self.visualize:
            physicsClient = p.connect(p.GUI)
        else:
            physicsClient = p.connect(p.DIRECT)

        dir_path = os.path.dirname(os.path.realpath(__file__))
        cubeStartPos = [0, 0, 0]
        cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
        planeId = p.loadURDF(dir_path + '/urdf/plane_with_restitution.urdf')
        self.robotId = p.loadURDF(dir_path + '/urdf/teststand.urdf',
                                  cubeStartPos,
                                  cubeStartOrientation,
                                  flags=p.URDF_USE_INERTIA_FROM_FILE)
        cubePos, cubeOrn = p.getBasePositionAndOrientation(self.robotId)

        if isinstance(lateral_friction, list):
            self.random_lateral_friction = True
            self.lateral_friction_range = lateral_friction
            self.lateral_friction = np.random.uniform(
                self.lateral_friction_range[0], self.lateral_friction_range[1])
        else:
            self.random_lateral_friction = False
            self.lateral_friction = lateral_friction

        if isinstance(contact_stiffness, list):
            self.random_contact_stiffness = True
            self.contact_stiffness_range = contact_stiffness
            self.contact_stiffness = np.random.uniform(
                self.contact_stiffness_range[0],
                self.contact_stiffness_range[1])
        else:
            self.random_contact_stiffness = False
            self.contact_stiffness = contact_stiffness

        useRealTimeSimulation = False

        # Query all the joints.
        num_joints = p.getNumJoints(self.robotId)
        print("Number of joints={}".format(num_joints))

        for ji in range(num_joints):
            p.changeDynamics(self.robotId,
                             ji,
                             linearDamping=.04,
                             angularDamping=0.04,
                             restitution=0.0,
                             lateralFriction=self.lateral_friction,
                             maxJointVelocity=1000)
            p.changeDynamics(self.robotId, ji, jointDamping=joint_damping)

        p.changeDynamics(planeId, -1, lateralFriction=self.lateral_friction)

        self.contact_damping = contact_damping
        self.contact_damping_multiplier = contact_damping_multiplier

        if self.random_contact_stiffness:
            self.contact_stiffness = np.random.uniform(
                self.contact_stiffness_range[0],
                self.contact_stiffness_range[1])

        if self.contact_damping_multiplier is not None:
            contact_damping = self.contact_damping_multiplier * 2.0 * np.sqrt(
                self.contact_stiffness)
        else:
            if self.contact_damping is None:
                contact_damping = 2.0 * np.sqrt(self.contact_stiffness)
            else:
                contact_damping = self.contact_damping
        p.changeDynamics(planeId,
                         -1,
                         contactStiffness=self.contact_stiffness,
                         contactDamping=contact_damping)

        p.setGravity(0.0, 0.0, -9.81)
        #p.setPhysicsEngineParameter(1e-3, numSubSteps=1)

        self.sim_timestep = sim_timestep
        self.cont_timestep_mult = cont_timestep_mult
        self.dt = self.cont_timestep_mult * self.sim_timestep
        p.setPhysicsEngineParameter(fixedTimeStep=self.sim_timestep)

        print(p.getPhysicsEngineParameters())

        return p, self.robotId, planeId, [1, 2, 3], [2, 3]
conid = p.connect(p.SHARED_MEMORY)
if (conid<0):
	p.connect(p.GUI)
	
p.setInternalSimFlags(0)
p.resetSimulation()
	
plane = p.loadURDF(os.path.join(dataDir, "plane.urdf"),useMaximalCoordinates=useMaximalCoordinates)

sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)

gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,-5)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)

centers = 0

mass = 1
visualShapeId = -1

n_links = 5

link_Masses=[1] * n_links
linkCollisionShapeIndices=[colSphereId] * n_links
linkVisualShapeIndices=[-1] * n_links
linkPositions=[[0,0,0.11 * (j+1)] for j in range(n_links)]
linkOrientations=[[0,0,0,1]] * n_links
Ejemplo n.º 29
0
    def __init__(self):
        # create the physics simulator
        self.physicsClient = p.connect(p.GUI)
        p.setGravity(0, 0, -9.81)

        self.max_communication_distance = 2.0

        # We will integrate every 4ms (250Hz update)
        self.dt = 1. / 250.
        p.setPhysicsEngineParameter(self.dt, numSubSteps=1)

        # Create the plane.
        self.planeId = p.loadURDF("../models/plane.urdf")
        p.changeDynamics(self.planeId,
                         -1,
                         lateralFriction=5.,
                         rollingFriction=0)

        self.goalId = p.loadURDF("../models/goal.urdf")
        self.goalId = p.loadURDF("../models/goal2.urdf")

        # the balls
        self.ball1 = p.loadURDF("../models/ball1.urdf")
        p.resetBasePositionAndOrientation(self.ball1, [2., 4., 0.5],
                                          (0., 0., 0.5, 0.5))
        self.ball2 = p.loadURDF("../models/ball2.urdf")
        p.resetBasePositionAndOrientation(self.ball2, [4., 2., 0.5],
                                          (0., 0., 0.5, 0.5))

        p.resetDebugVisualizerCamera(7.0, 90.0, -43.0, (1., 1., 0.0))

        # Add objects
        wallId = p.loadSDF("../models/walls.sdf")[0]
        p.resetBasePositionAndOrientation(wallId, [0., -1., 0],
                                          (0., 0., 0.5, 0.5))
        wallId = p.loadSDF("../models/walls.sdf")[0]
        p.resetBasePositionAndOrientation(wallId, [0., 1., 0],
                                          (0., 0., 0.5, 0.5))
        wallId = p.loadSDF("../models/walls.sdf")[0]
        p.resetBasePositionAndOrientation(wallId, [3., -1., 0],
                                          (0., 0., 0.5, 0.5))
        wallId = p.loadSDF("../models/walls.sdf")[0]
        p.resetBasePositionAndOrientation(wallId, [3., 1., 0],
                                          (0., 0., 0.5, 0.5))
        wallId = p.loadSDF("../models/walls.sdf")[0]
        p.resetBasePositionAndOrientation(wallId, [1., 2., 0],
                                          (0., 0., 0., 1.))
        wallId = p.loadSDF("../models/walls.sdf")[0]
        p.resetBasePositionAndOrientation(wallId, [2., -2., 0],
                                          (0., 0., 0., 1.))

        # tube
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-1., 5., 0], (0., 0., 0., 1.))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-1., 6., 0], (0., 0., 0., 1.))

        # #arena
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-2, 4., 0], (0., 0., 0.5, 0.5))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-2., 7., 0], (0., 0., 0.5, 0.5))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-2., 9., 0], (0., 0., 0.5, 0.5))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-2., 11., 0], (0., 0., 0.5, 0.5))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-2., 13., 0], (0., 0., 0.5, 0.5))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-3., 3., 0], (0., 0., 0., 1.))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-5., 3., 0], (0., 0., 0., 1.))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-7., 3., 0], (0., 0., 0., 1.))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-8, 4., 0], (0., 0., 0.5, 0.5))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-8., 6., 0], (0., 0., 0.5, 0.5))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-8., 8., 0], (0., 0., 0.5, 0.5))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-8., 10., 0], (0., 0., 0.5, 0.5))
        # wallId = p.loadSDF("../models/walls.sdf")[0]
        # p.resetBasePositionAndOrientation(wallId, [-8., 12., 0], (0., 0., 0.5, 0.5))

        # create 6 robots
        self.robots = []
        for (i, j) in itertools.product(range(3), range(2)):
            self.robots.append(
                Robot([1. * i + 0.5, 1. * j - 0.5, 0.3], 2 * i + j, self.dt))
            p.stepSimulation()

        self.time = 0.0

        self.stepSimulation()
        self.stepSimulation()
Ejemplo n.º 30
0
16 iiwa_gripper_finger2_inner_knuckle_joint
17 iiwa_gripper_finger2_finger_tip_joint
"""

p.setGravity(0, 0, 0)
t = 0.
prevPose = [0, 0, 0]
prevPose1 = [0, 0, 0]
hasPrevPose = 0
useNullSpace = 1
useOrientation = 1
ikSolver = 0
p.setRealTimeSimulation(0)
# one action/simulation_step takes 0.002*20=0.04 seconds in real time
p.setPhysicsEngineParameter(fixedTimeStep=0.002 * 20,
                            numSolverIterations=5,
                            numSubSteps=20)
# trailDuration is duration (in seconds) after debug lines will be removed automatically
# use 0 for no-removal
trailDuration = 15

i = 0
mp = 1
g = 0.5
start_time = time.process_time()
# 25 simulation_steps = 1 seconds
while i < (25 * 50):
    i += 1
    t = t + 0.04
    p.stepSimulation()
Ejemplo n.º 31
0
	def clean_everything(self):
		p.resetSimulation()
		p.setGravity(0, 0, -self.gravity)
		p.setDefaultContactERP(0.9)
		p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=5, numSubSteps=self.frame_skip)
    def reset(self):

        if (self.test_phase):
            global test_steps, test_done
            if (test_steps != 0):
                self.save_data_test()
                test_steps = 0

        p.resetSimulation()
        p.setPhysicsEngineParameter(numSolverIterations=150)
        p.setTimeStep(self._timeStep)
        self._envStepCounter = 0

        p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"),
                   useFixedBase=True)
        # Load robot
        self._panda = pandaEnv(self._urdfRoot,
                               timeStep=self._timeStep,
                               basePosition=[0, 0, 0.625],
                               useInverseKinematics=self._useIK,
                               action_space=self.action_dim,
                               includeVelObs=self.includeVelObs)

        # Load table and object for simulation
        tableId = p.loadURDF(os.path.join(self._urdfRoot,
                                          "franka_description/table.urdf"),
                             useFixedBase=True)

        table_info = p.getVisualShapeData(tableId, -1)[0]
        self._h_table = table_info[5][2] + table_info[3][2]

        #limit panda workspace to table plane
        self._panda.workspace_lim[2][0] = self._h_table
        # Randomize start position of object and target.

        #we take the target point

        if (self.fixedPositionObj):
            if (self.object_position == 0):
                #we have completely fixed position
                self.obj_pose = [0.6, 0.1, 0.64]
                self.target_pose = [0.4, 0.45, 0.64]
                self._objID = p.loadURDF(os.path.join(
                    self._urdfRoot, "franka_description/cube_small.urdf"),
                                         basePosition=self.obj_pose)
                self._targetID = p.loadURDF(os.path.join(
                    self._urdfRoot, "franka_description/domino/domino.urdf"),
                                            basePosition=self.target_pose)

            elif (self.object_position == 1):
                #we have completely fixed position
                self.obj_pose = [
                    np.random.uniform(0.5, 0.6),
                    np.random.uniform(0, 0.1), 0.64
                ]
                self.target_pose = [0.4, 0.45, 0.64]
                # self.target_pose = [np.random.uniform(0.4,0.5),np.random.uniform(0.45,0.55),0.64]
                self._objID = p.loadURDF(os.path.join(
                    self._urdfRoot, "franka_description/cube_small.urdf"),
                                         basePosition=self.obj_pose)
                self._targetID = p.loadURDF(os.path.join(
                    self._urdfRoot, "franka_description/domino/domino.urdf"),
                                            basePosition=self.target_pose)

            elif (self.object_position == 2):
                #we have completely fixed position
                self.obj_pose = [
                    np.random.uniform(0.4, 0.6),
                    np.random.uniform(0, 0.2), 0.64
                ]
                self.target_pose = [
                    np.random.uniform(0.3, 0.5),
                    np.random.uniform(0.35, 0.55), 0.64
                ]
                self._objID = p.loadURDF(os.path.join(
                    self._urdfRoot, "franka_description/cube_small.urdf"),
                                         basePosition=self.obj_pose)
                self._targetID = p.loadURDF(os.path.join(
                    self._urdfRoot, "franka_description/domino/domino.urdf"),
                                            basePosition=self.target_pose)

            elif (self.object_position == 3):
                print("")
        else:
            self.obj_pose, self.target_pose = self._sample_pose()
            self._objID = p.loadURDF(os.path.join(
                self._urdfRoot, "franka_description/cube_small.urdf"),
                                     basePosition=self.obj_pose)
            #useful to see where is the taget point
            self._targetID = p.loadURDF(os.path.join(
                self._urdfRoot, "franka_description/domino/domino.urdf"),
                                        basePosition=self.target_pose)

        if self.type_physics == 1:
            # Randomizing the physics of the object...
            self.currentMass = np.random.uniform(0.1, 0.8)
            self.currentFriction = np.random.uniform(0.1, 0.7)
            self.currentDamping = np.random.uniform(0.01, 0.2)
            p.changeDynamics(self._objID,
                             linkIndex=-1,
                             mass=self.currentMass,
                             lateralFriction=self.currentFriction,
                             linearDamping=self.currentDamping)

            # Randomizing the physics of the robot... (only joints damping and controller gains)
            for i in range(7):
                p.changeDynamics(self._panda.pandaId,
                                 linkIndex=i,
                                 linearDamping=np.random.uniform(0.25, 20))

        elif self.type_physics == 2:
            # Randomizing the physics of the object...
            self.currentMass = 0.8
            self.currentFriction = 0.2
            self.currentDamping = 0.2
            p.changeDynamics(self._objID,
                             linkIndex=-1,
                             mass=self.currentMass,
                             lateralFriction=self.currentFriction,
                             linearDamping=self.currentDamping)

            # Randomizing the physics of the robot... (only joints damping and controller gains)
            for i in range(7):
                p.changeDynamics(self._panda.pandaId,
                                 linkIndex=i,
                                 linearDamping=0.25)

        self._debugGUI()
        p.setGravity(0, 0, -9.8)
        # Let the world run for a bit
        for _ in range(10):
            p.stepSimulation()

        #we take the dimension of the observation

        return self.getExtendedObservation()
  def reset(self):
    """Environment reset called at the beginning of an episode.
    """
    # Set the camera settings.

    # look = [0.1, 0., 0.44]
    # distance = 0.8
    # pitch = -90
    # yaw = -90
    # roll = 180

    #[0.4317558029454219, 0.1470448842904527, 0.2876218894185256]#[0.23, 0.2, 0.54] # from where the input is
    # set 1
    # look = [0.1, -0.3, 0.54] 
    # distance = 1.0
    # pitch = -45 + self._cameraRandom * np.random.uniform(-3, 3)
    # yaw = -45 + self._cameraRandom * np.random.uniform(-3, 3)
    # roll = 145
    # pos_range = [0.3, 0.7, -0.2, 0.3]

    # set 2
    # look = [-0.3, -0.8, 0.54] 
    # distance = 0.7
    # pitch = -28 + self._cameraRandom * np.random.uniform(-3, 3)
    # yaw = -45 + self._cameraRandom * np.random.uniform(-3, 3)
    # roll = 180
    # pos_range = [0.1, 0.8, -0.45, 0.15]

    # set 3
    look = [0.4, 0.1, 0.54] 
    distance = 2.0
    pitch = -90
    yaw = -90
    roll = 180
    pos_range = [0.3, 0.7, -0.2, 0.3]

    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])

    self.tableUid = p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.640000,
                               0.000000, 0.000000, 0.0, 1.0)

    p.setGravity(0, 0, -10)
    # self._tm700 = tm700(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)

    self._envStepCounter = 0
    p.stepSimulation()

    # num of objs to be placed
    while len(self.model_paths) != 0:
      urdfList = []
      num_obj = random.randint(1, 3)
      for i in range(num_obj):
        urdfList.append(self.model_paths.pop())
      print('img {img_cnt}, {n_obj} objects'.format(img_cnt=self.img_save_cnt+1, n_obj=len(urdfList)))
      self._objectUids = self._randomly_place_objects(urdfList, pos_range)
      self._observation = self._get_observation()
      self.img_save_cnt += 1
      for uid in self._objectUids:
        p.removeBody(uid)
      if self.img_save_cnt == 2: 
        raise Exception('stop')

    return np.array(self._observation)
Ejemplo n.º 34
0
import pybullet as p
import time

p.connect(p.GUI)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.setPhysicsEngineParameter(
    constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM=0.000001)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)

p.loadURDF("plane.urdf")
radius = 0.025
distance = 1.86
yaw = 135
pitch = -11
targetPos = [0, 0, 0]

p.setPhysicsEngineParameter(solverResidualThreshold=0.001,
                            numSolverIterations=200)
p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos)
objectId = -1

for i in range(10):
    objectId = p.loadURDF("cube_small.urdf", [1, 1, radius + i * 2 * radius])

p.changeDynamics(objectId, -1, 100)

timeStep = 1. / 240.
p.setGravity(0, 0, -10)
while (p.isConnected()):
    p.stepSimulation()
    time.sleep(timeStep)
Ejemplo n.º 35
0
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)

ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)

p.changeVisualShape(ground,-1,rgbaColor=[1,1,1,0.8])
#p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)

print("hasNumpy = ",p.isNumpyEnabled())


anymal = p.loadURDF("quadruped/ANYmal/robot.urdf",[3,3,3], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES, useMaximalCoordinates=False)
p.resetSimulation()
ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)

#todo, tweak this value to trade solver quality versus performance
p.setPhysicsEngineParameter(solverResidualThreshold=1e-2)

index = 0
numX = 3 
numY = 3

for i in range (numX):
	for j in range (numY):
		print("loading animal ", index)
		index+=1
		
		#anymal = p.loadURDF("atlas/atlas_v4_with_multisense.urdf",[i*3,j*3,1], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
		anymal = p.loadURDF("quadruped/ANYmal/robot.urdf",[(i-numX/2)*2,(j-numY/2)*2,0.6], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES, useMaximalCoordinates=False)
		
		for j in range(p.getNumJoints(anymal)):
			p.setJointMotorControl2(anymal,j,p.POSITION_CONTROL,targetPosition=0, force=500)
Ejemplo n.º 36
0
        p.disconnect()
    except Exception:
        pass
    p.connect(p.GUI)
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.resetSimulation()
    p.resetDebugVisualizerCamera(cameraDistance=0.6,
                                 cameraYaw=20,
                                 cameraPitch=-40,
                                 cameraTargetPosition=[0, 0, 0.1])
    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)
    p.setRealTimeSimulation(0)
    p.setPhysicsEngineParameter(numSubSteps=4)
    p.setPhysicsEngineParameter(numSolverIterations=10)

    p.setGravity(0, 0, GRAVITY)
    StartPos = [0, 0, 0.3]
    StartOrientation = p.getQuaternionFromEuler([0, 0, 0])
    planeId = p.loadURDF("plane.urdf")
    botId = p.loadURDF("tiptap.urdf", StartPos, StartOrientation)

    # Disable the motors for torque control:
    p.setJointMotorControlArray(
        botId,
        [j for j in range(p.getNumJoints(botId))],
        p.VELOCITY_CONTROL,
        forces=[0.0 for j in range(p.getNumJoints(botId))]
    )
Ejemplo n.º 37
0
    def reset(self):
        self.terminated = False
        self.n_contacts = 0
        self.n_steps_outside = 0
        p.resetSimulation()
        p.setPhysicsEngineParameter(numSolverIterations=150)
        p.setTimeStep(self._timestep)
        p.loadURDF(os.path.join(self._urdf_root, "plane.urdf"), [0, 0, -1])

        self.table_uid = p.loadURDF(os.path.join(self._urdf_root, "table/table.urdf"), 0.5000000, 0.00000, -.820000,
                                    0.000000, 0.000000, 0.0, 1.0)

        # Initialize button position
        x_pos = 0.5
        y_pos = 0

        self.button_uid = p.loadURDF("../data/simple_button.urdf", [x_pos, y_pos, Z_TABLE], useFixedBase=True)
        self.button_pos = np.array([x_pos, y_pos, Z_TABLE])
        p.changeDynamics(self.button_uid, 1, contactStiffness=1000000.0, contactDamping=1.0)

        self.box_uid = p.loadURDF("../data/target_box_new.urdf", [x_pos-0.1, y_pos-0.1, Z_TABLE+0.065], useFixedBase=True)
        self.box_pos = np.array([x_pos, y_pos, Z_TABLE])
        p.changeDynamics(self.box_uid, 0, contactStiffness=1000000.0, contactDamping=1.0)

        p.setGravity(0, 0, -10)
        # load my kuka
        self._kuka = kuka.Kuka(urdf_root_path="../data", timestep=self._timestep,
                               use_inverse_kinematics=(not self.action_joints),
                               small_constraints=(not self._random_target))
        self._env_step_counter = 0
        # Close the gripper and wait for the arm to be in rest position
        for _ in range(500):
            if self.action_joints:
                self._kuka.applyAction(list(np.array(self._kuka.joint_positions)[:7]) + [0, 0])
            else:
                self._kuka.applyAction([0, 0, 0, 0, 0])
            p.stepSimulation()

        # Randomize init arm pos: take 5 random actions
        for _ in range(N_RANDOM_ACTIONS_AT_INIT):
            if self._is_discrete:
                action = [0, 0, 0, 0, 0]
                sign = 1 if self.np_random.rand() > 0.5 else -1
                action_idx = self.np_random.randint(3)  # dx, dy or dz
                action[action_idx] += sign * DELTA_V
            else:
                if self.action_joints:
                    joints = np.array(self._kuka.joint_positions)[:7]
                    joints += DELTA_THETA * self.np_random.normal(joints.shape)
                    action = list(joints) + [0, 0]
                else:
                    action = np.zeros(5)
                    rand_direction = self.np_random.normal((3,))
                    # L2 normalize, so that the random direction is not too high or too low
                    rand_direction /= np.linalg.norm(rand_direction, 2)
                    action[:3] += DELTA_V_CONTINUOUS * rand_direction

            self._kuka.applyAction(list(action))
            p.stepSimulation()

        self._observation = self.getExtendedObservation()

        self.button_pos = np.array(p.getLinkState(self.button_uid, BUTTON_LINK_IDX)[0])
        # self.button_pos[2] += BUTTON_DISTANCE_HEIGHT  # Set the target position on the top of the button
        if self.saver is not None:
            self.saver.reset(self._observation, self.getTargetPos(), self.getGroundTruth())

        if self.srl_model != "raw_pixels":
            return self.getSRLState(self._observation)

        return self._observation
Ejemplo n.º 38
0
    def __init__(self,
                 robotPath,
                 floor=True,
                 fixed=False,
                 transparent=False,
                 gui=True,
                 realTime=True,
                 panels=False,
                 useUrdfInertia=True,
                 dt=0.002):
        """Creates an instance of humanoid simulation

        Keyword Arguments:
            field {bool} -- enable the display of the field (default: {False})
            fixed {bool} -- makes the base of the robot floating/fixed (default: {False})
            transparent {bool} -- makes the robot transparent (default: {False})
            gui {bool} -- enables the gui visualizer, if False it will runs headless (default {True})
            realTime {bool} -- try to have simulation in real time (default {True})
            panels {bool} -- show/hide the user interaction pyBullet panels (default {False})
            useUrdfInertia {bool} -- use URDF from URDF file (default {True})
            dt {float} -- time step (default {0.002})
        """

        self.dir = os.path.dirname(os.path.abspath(__file__))
        self.gui = gui
        self.realTime = realTime
        self.t = 0
        self.start = time.time()
        self.dt = dt
        self.mass = None

        # Debug lines drawing
        self.lines = []
        self.currentLine = 0
        self.lastLinesDraw = 0
        self.lineColors = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0],
                           [1, 0, 1], [0, 1, 1]]

        # Instanciating bullet
        if gui:
            physicsClient = p.connect(p.GUI)
        else:
            physicsClient = p.connect(p.DIRECT)
        p.setGravity(0, 0, -9.81)

        # Light GUI
        if not panels:
            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)

        p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 1)

        # Loading floor and/or plane ground
        if floor:
            self.floor = p.loadURDF(self.dir + '/bullet/plane.urdf')
        else:
            self.floor = None

        # Loading robot
        startPos = [0, 0, 0]
        if not fixed:
            startPos[2] = 1
        startOrientation = p.getQuaternionFromEuler([0, 0, 0])
        flags = p.URDF_USE_SELF_COLLISION
        if useUrdfInertia:
            flags += p.URDF_USE_INERTIA_FROM_FILE
        self.robot = p.loadURDF(robotPath,
                                startPos,
                                startOrientation,
                                flags=flags,
                                useFixedBase=fixed)

        # Setting frictions parameters to default ones
        self.setFloorFrictions()

        # Engine parameters
        p.setPhysicsEngineParameter(fixedTimeStep=self.dt, maxNumCmdPer1ms=0)
        # p.setRealTimeSimulation(0)
        # p.setPhysicsEngineParameter(numSubSteps=1)

        # Retrieving joints and frames
        self.joints = {}
        self.jointsInfos = {}
        self.jointsIndexes = {}
        self.frames = {}
        self.maxTorques = {}

        # Collecting the available joints
        n = 0
        for k in range(p.getNumJoints(self.robot)):
            jointInfo = p.getJointInfo(self.robot, k)
            name = jointInfo[1].decode('utf-8')
            if not name.endswith('_fixing') and not name.endswith('_passive'):
                if '_frame' in name:
                    self.frames[name] = k
                else:
                    self.jointsIndexes[name] = n
                    n += 1
                    self.joints[name] = k
                    self.jointsInfos[name] = {'type': jointInfo[2]}
                    if jointInfo[8] < jointInfo[9]:
                        self.jointsInfos[name]['lowerLimit'] = jointInfo[8]
                        self.jointsInfos[name]['upperLimit'] = jointInfo[9]

        # Changing robot opacity if transparent set to true
        if transparent:
            for k in range(p.getNumJoints(self.robot)):
                p.changeVisualShape(self.robot,
                                    k,
                                    rgbaColor=[0.3, 0.3, 0.3, 0.3])

        print('* Found ' + str(len(self.joints)) + ' DOFs')
        print('* Found ' + str(len(self.frames)) + ' frames')
Ejemplo n.º 39
0
    def __init__(
        self,
        display=True,
        seed=None,
        pos_noise=0.05,
        rpy_noise=0.1,
        num_rings=5,
        ring_separation=5.,
    ):

        # Create random number generator
        self.rng = np.random.default_rng(seed)

        # Choose the time step
        self.dt = 0.01

        # Create empty list of drones
        self.drones = []
        self.max_num_drones = 40

        # Connect to and configure pybullet
        self.display = display
        if self.display:
            pybullet.connect(pybullet.GUI)
            pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
        else:
            pybullet.connect(pybullet.DIRECT)
        pybullet.setGravity(0, 0, -9.81)
        pybullet.setPhysicsEngineParameter(fixedTimeStep=self.dt,
                                           numSubSteps=4,
                                           restitutionVelocityThreshold=0.05,
                                           enableFileCaching=0)

        # Load plane
        self.plane_id = pybullet.loadURDF(
            os.path.join('.', 'urdf', 'plane.urdf'),
            basePosition=np.array([0., 0., 0.]),
            baseOrientation=pybullet.getQuaternionFromEuler([0., 0., 0.]),
            useFixedBase=1)

        # Load rings
        self.num_rings = num_rings
        self.ring_separation = ring_separation
        self.rings = []
        self.add_ring([0., 0., 0.25], [0., -np.pi / 2, 0.], 2.5, 0.5,
                      'big-ring.urdf')
        for i in range(num_rings):
            x = (i + 1) * ring_separation
            y = self.rng.uniform(low=-5., high=5.)
            z = self.rng.uniform(low=1., high=3.)
            self.add_ring([x, y, z], [0., 0., 0.], 1., 0.25, 'ring.urdf')
        self.add_ring([(num_rings + 1) * ring_separation, 0., 0.25],
                      [0., np.pi / 2, 0.], 2.5, 0.5, 'big-ring.urdf')

        # # Eliminate linear and angular damping (a poor model of drag)
        # pybullet.changeDynamics(self.robot_id, -1, linearDamping=0., angularDamping=0.)

        # Set contact parameters
        object_ids = [self.plane_id]
        for ring in self.rings:
            object_ids.append(ring['id'])
        for object_id in object_ids:
            pybullet.changeDynamics(object_id,
                                    -1,
                                    lateralFriction=1.0,
                                    spinningFriction=0.0,
                                    rollingFriction=0.0,
                                    restitution=0.5,
                                    contactDamping=-1,
                                    contactStiffness=-1)

        self.pos_noise = pos_noise
        self.rpy_noise = rpy_noise

        self.camera_drone_name = None
        self.camera_drone_yaw = None
        self.camera_viewfromstart = True

        self.camera()
        self.update_display()

        self.l = 0.175
        self.kF = 7e-6
        self.kM = 1e-7
        self.min_spin_rate = 100  # <-- rad/s
        self.max_spin_rate = 900  # <-- rad/s
        self.s_min = self.min_spin_rate**2
        self.s_max = self.max_spin_rate**2

        # motor[0]: front (+z spin)
        # motor[1]: rear (+z spin)
        # motor[2]: left (-z spin)
        # motor[3]: right (-z spin)

        self.M = linalg.inv(
            np.array([[0., 0., self.kF * self.l, -self.kF * self.l],
                      [-self.kF * self.l, self.kF * self.l, 0., 0.],
                      [-self.kM, -self.kM, self.kM, self.kM],
                      [self.kF, self.kF, self.kF, self.kF]]))
#add parent dir to find package. Only needed for source code build, pip install doesn't need it.
import os
import inspect
currentdir = os.path.dirname(
    os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)

import pybullet_data
import pybullet as p
import time

p.connect(p.GUI_SERVER)
p.setAdditionalSearchPath(pybullet_data.getDataPath())

while (1):
    #this is a no-op command, to allow GUI updates on Mac OSX (main thread)
    p.setPhysicsEngineParameter()
    time.sleep(0.01)
Ejemplo n.º 41
0
    def __init__(self, fixed_slider, init_sliders_pose, with_gui):
        # store the arguments
        self.fixed_slider = fixed_slider
        self.init_sliders_pose = init_sliders_pose
        self.with_gui = with_gui

        # connect to the server
        if self.with_gui:
            self.physicsClient = p.connect(p.GUI)
        else:
            self.physicsClient = p.connect(p.DIRECT)

        # create sliders
        if self.with_gui:
            self.slider_a = p.addUserDebugParameter("a", 0, 1,
                                                    init_sliders_pose[0])
            self.slider_b = p.addUserDebugParameter("b", 0, 1,
                                                    init_sliders_pose[1])
        else:
            self.slider_a = init_sliders_pose[0]
            self.slider_b = init_sliders_pose[0]

        # Load the plain.
        plain_urdf = os.path.join(
            rospkg.RosPack().get_path("robot_properties_teststand"),
            "urdf", "plane_with_restitution.urdf")
        self.planeId = p.loadURDF(plain_urdf)

        print("Loaded ground.")

        self.config = TeststandConfig()

        # Load the robot
        robotStartPos = [0., 0., 0.]
        robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])

        self.urdf_path = self.config.urdf_path
        self.robotId = p.loadURDF(self.urdf_path, robotStartPos,
                                  robotStartOrientation,
                                  flags=p.URDF_USE_INERTIA_FROM_FILE,
                                  useFixedBase=True)
        p.getBasePositionAndOrientation(self.robotId)

        # Create the robot wrapper in pinocchio.
        self.pin_robot = self.config.buildRobotWrapper()

        # Query all the joints.
        num_joints = p.getNumJoints(self.robotId)

        for ji in range(num_joints):
            p.changeDynamics(self.robotId, ji, linearDamping=.04,
                             angularDamping=0.04, restitution=0.0,
                             lateralFriction=0.5)

        p.setGravity(0, 0, -9.81)
        p.setPhysicsEngineParameter(1e-3, numSubSteps=1)

        self.joint_names = ['joint_z', 'HFE', 'KFE']

        self.wrapper = PinBulletWrapper(self.robotId, self.pin_robot,
                                        self.joint_names, ['END'],
                                        useFixedBase=True)

        # Constrain the slider to a fixed position.
        if fixed_slider:
            p.createConstraint(self.robotId, 0, self.robotId, -1,
                               p.JOINT_FIXED,
                               [0, 0, 0], [0, 0, 0], [0, 0, 0.45])

        # Initialize the device.
        self.device = Device('bullet_teststand')
        self.device.initialize(self.config.yaml_path)

        # Initialize signals that are not filled in sim2signals.
        self.device.contact_sensors.value = 1 * [0.]
        self.device.height_sensors.value = 1 * [0.]
        self.device.ati_force.value = 3 * [0.]
        self.device.ati_torque.value = 3 * [0.]

        # Sync the current robot state to the graph input signals.
        self._sim2signal()

        self.steps_ = 0

        super(TeststandBulletRobot, self).__init__('bullet_teststand',
                                                   self.device)
Ejemplo n.º 42
0
                   cubeStartOrientation)
cubePos, cubeOrn = p.getBasePositionAndOrientation(robot)
nJoints = p.getNumJoints(robot)

# Drawing inertia boxes
if False:
    drawInertiaBox(robot, -1, [0, 1, 0])
    for i in range(nJoints):
        drawInertiaBox(robot, i, [0, 1, 0])

# Map des joints
jointsMap = []
framesMap = []
t = 0
dt = 0.01
p.setPhysicsEngineParameter(fixedTimeStep=dt)

# Collecting the available joints
for k in range(nJoints):
    jointInfo = p.getJointInfo(robot, k)
    name = jointInfo[1].decode('utf-8')
    if '_fixing' not in name:
        if '_frame' in name:
            framesMap.append([name, k])
        else:
            jointsMap.append(k)

print('* Found ' + str(len(jointsMap)) + ' DOFs')
print('* Found ' + str(len(framesMap)) + ' frames')

while True:
Ejemplo n.º 43
0
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
Ejemplo n.º 44
0
 def setup(self):
     """Sets up the robot + tray + objects.
 """
     test = self._test
     if not self._urdf_list:  # Load from procedural random objects.
         if not self._object_filenames:
             self._object_filenames = self._get_random_objects(
                 num_objects=self._num_objects,
                 test=test,
                 replace=self._allow_duplicate_objects,
             )
         self._urdf_list = self._object_filenames
     logging.info('urdf_list %s', self._urdf_list)
     pybullet.resetSimulation(physicsClientId=self.cid)
     pybullet.setPhysicsEngineParameter(numSolverIterations=150,
                                        physicsClientId=self.cid)
     pybullet.setTimeStep(self._time_step, physicsClientId=self.cid)
     pybullet.setGravity(0, 0, -10, physicsClientId=self.cid)
     plane_path = os.path.join(self._urdf_root, 'plane.urdf')
     pybullet.loadURDF(plane_path, [0, 0, -1], physicsClientId=self.cid)
     table_path = os.path.join(self._urdf_root, 'table/table.urdf')
     pybullet.loadURDF(table_path, [0.5, 0.0, -.82], [0., 0., 0., 1.],
                       physicsClientId=self.cid)
     self._kuka = kuka.Kuka(urdfRootPath=self._urdf_root,
                            timeStep=self._time_step,
                            clientId=self.cid)
     self._block_uids = []
     self._urdf_list = [
         ('/home/jonathan/Desktop/Projects/objects/6a0c8c43b13693fa46eb89c2e933753d.obj',
          'obj', [4, 4, 4]),
         ('/home/jonathan/Desktop/Projects/objects/6aec84952a5ffcf33f60d03e1cb068dc.obj',
          'obj', [2, 2, 2]),
         ('/home/jonathan/Desktop/Projects/objects/bed29baf625ce9145b68309557f3a78c.obj',
          'obj', [2, 2, 2]),
         ('/home/jonathan/Desktop/Projects/objects/dac6ea4929f1e47f178d703a993fe24c.obj',
          'obj', [2, 2, 2]),
         ('/home/jonathan/Desktop/Projects/objects/f452c1053f88cd2fc21f7907838a35d1.obj',
          'obj', [2, 2, 2])
     ]
     for name in self._urdf_list:
         if (name[1] == 'urdf'):
             print(name[0], name[1])
             urdf_name = name[0]
             xpos = 0.4 + self._block_random * random.random()
             ypos = self._block_random * (random.random() - .5)
             angle = np.pi / 2 + self._block_random * np.pi * random.random(
             )
             ori = pybullet.getQuaternionFromEuler([0, 0, angle])
             uid = pybullet.loadURDF(urdf_name, [xpos, ypos, .15],
                                     [ori[0], ori[1], ori[2], ori[3]],
                                     physicsClientId=self.cid)
             self._block_uids.append(uid)
             for _ in range(500):
                 pybullet.stepSimulation(physicsClientId=self.cid)
         if (name[1] == 'obj'):
             obj_name = name[0]
             scale = name[2]
             xpos = 0.4 + self._block_random * random.random()
             ypos = self._block_random * (random.random() - .5)
             angle = np.pi / 2 + self._block_random * np.pi * random.random(
             )
             ori = pybullet.getQuaternionFromEuler([0, 0, angle])
             uid = pybullet.createMultiBody(
                 0.05,
                 pybullet.createCollisionShape(pybullet.GEOM_MESH,
                                               fileName=name[0],
                                               meshScale=scale),
                 pybullet.createVisualShape(pybullet.GEOM_MESH,
                                            fileName=name[0],
                                            meshScale=scale),
                 [xpos, ypos, .15], [ori[0], ori[1], ori[2], ori[3]],
                 physicsClientId=self.cid)
             self._block_uids.append(uid)
             for _ in range(500):
                 pybullet.stepSimulation(physicsClientId=self.cid)
import motorController
import walkGenerator

# motor parameters
motor_kp = 0.5
motor_kd = 0.5
motor_torque = 20
motor_max_velocity = 5.0

# physics parameters
fixedTimeStep = 1. / 2000
numSolverIterations = 200

physicsClient = p.connect(p.GUI)
p.setTimeStep(fixedTimeStep)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.setAdditionalSearchPath(pybullet_data.getDataPath())  # to load plane.urdf

p.setGravity(0, 0, 0)

p.resetDebugVisualizerCamera(cameraDistance=1,
                             cameraYaw=10,
                             cameraPitch=-0,
                             cameraTargetPosition=[0.4, 0, 0.1])

planeID = p.loadURDF("plane.urdf")
robotID = p.loadURDF(os.path.abspath(os.path.dirname(__file__)) +
                     '/humanoid_leg_12dof.8.urdf', [0, 0, 0.31],
                     p.getQuaternionFromEuler([0, 0, 0]),
                     useFixedBase=False)
Ejemplo n.º 46
0
    def generate_dataset(self):

        # combined dataset for graph classification (GC)
        combined_dataset_name = dataset_prefix
        combined_dataset_dir = self.save_dir / combined_dataset_name
        pathlib.Path(combined_dataset_dir).mkdir(parents=True, exist_ok=True)

        combined_node_id = 0
        combined_graph_id = 0
        combined_graphnode2id = {}

        combined_a = open(
            combined_dataset_dir / str(combined_dataset_name + "_A.txt"), "w")
        combined_graph_indicator = open(
            combined_dataset_dir /
            str(combined_dataset_name + "_graph_indicator.txt"),
            "w",
        )
        combined_graph_labels = open(
            combined_dataset_dir /
            str(combined_dataset_name + "_graph_labels.txt"), "w")
        combined_node_attributes = open(
            combined_dataset_dir /
            str(combined_dataset_name + "_node_attributes.txt"),
            "w",
        )

        for num_brick in tqdm(
                range(self.min_num_brick, self.max_num_brick + 1),
                "Number of Brick Progress",
        ):
            # separate dataset for graph classification (GC)
            dataset_name = self.dataset_prefix + "_" + str(num_brick)
            dataset_dir = self.save_dir / dataset_name
            pathlib.Path(dataset_dir).mkdir(parents=True, exist_ok=True)

            node_id = 0
            graphnode2id = {}

            a = open(dataset_dir / str(dataset_name + "_A.txt"), "w")
            graph_indicator = open(
                dataset_dir / str(dataset_name + "_graph_indicator.txt"), "w")
            graph_labels = open(
                dataset_dir / str(dataset_name + "_graph_labels.txt"), "w")
            node_attributes = open(
                dataset_dir / str(dataset_name + "_node_attributes.txt"), "w")

            for episode in tqdm(range(self.num_episode), "Episode Progress"):
                # use test_simulator.py for debugging and visualization
                p.connect(p.DIRECT)

                p.setAdditionalSearchPath(pybullet_data.getDataPath())
                p.setPhysicsEngineParameter(numSolverIterations=10)
                p.setTimeStep(1.0 / 60.0)

                # generate new design
                self.designer.clear()
                self.designer.generate_design(num_brick=num_brick)

                # update combined graph_id
                graph_id = episode + 1
                combined_graph_id += 1

                init_pos_list = []
                final_pos_list = []

                for i, b in enumerate(self.designer.built):
                    # write to separate GC dataset
                    node_id += 1
                    graphnode2id[str(graph_id) + "_" + str(i)] = node_id
                    graph_indicator.write(str(graph_id) + "\n")
                    node_attributes.write(",".join(
                        str(i) for i in b.bounding.flatten().tolist()) + "\n")

                    # write to combined GC dataset
                    combined_node_id += 1
                    combined_graphnode2id[str(combined_graph_id) + "_" +
                                          str(i)] = combined_node_id
                    combined_graph_indicator.write(
                        str(combined_graph_id) + "\n")
                    combined_node_attributes.write(",".join(
                        str(i) for i in b.bounding.flatten().tolist()) + "\n")

                    pos = b.anchor
                    rot = p.getQuaternionFromEuler(
                        np.array(b.rotation) * np.pi / 2)
                    brickID = p.loadURDF("urdf/brick.urdf", pos, rot)
                    init_pos_list.append(pos)

                p.loadURDF("plane.urdf", useMaximalCoordinates=True)

                p.setGravity(0, 0, -10)
                for frame in range(self.num_frame):
                    p.stepSimulation()
                    time.sleep(1.0 / 240.0)

                    if frame == self.num_frame - 1:
                        for brickID in len(num_brick):
                            pos, _ = p.getBasePositionAndOrientation(brickID)
                            final_pos_list.append(pos)

                for edge in self.designer.G.edges:
                    # write to separate GC dataset
                    row = graphnode2id[str(graph_id) + "_" + str(edge[0])]
                    col = graphnode2id[str(graph_id) + "_" + str(edge[1])]
                    a.write(str(row) + ", " + str(col) + "\n")
                    a.write(str(col) + ", " + str(row) + "\n")

                    # write to combined GC dataset
                    combined_row = combined_graphnode2id[str(combined_graph_id)
                                                         + "_" + str(edge[0])]
                    combined_col = combined_graphnode2id[str(combined_graph_id)
                                                         + "_" + str(edge[1])]
                    combined_a.write(
                        str(combined_row) + ", " + str(combined_col) + "\n")
                    combined_a.write(
                        str(combined_col) + ", " + str(combined_row) + "\n")

                label = int(
                    self.determine_stable(init_pos_list, final_pos_list))
                graph_labels.write(str(label) + "\n")
                combined_graph_labels.write(str(label) + "\n")

                p.disconnect()

            a.close()
            graph_indicator.close()
            graph_labels.close()
            node_attributes.close()
            shutil.make_archive(self.save_dir / dataset_name, "zip",
                                dataset_dir)
            shutil.rmtree(dataset_dir)

        combined_a.close()
        combined_graph_indicator.close()
        combined_graph_labels.close()
        combined_node_attributes.close()
        shutil.make_archive(self.save_dir / combined_dataset_name, "zip",
                            combined_dataset_dir)
        shutil.rmtree(combined_dataset_dir)
Ejemplo n.º 47
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)
Ejemplo n.º 48
0
import pybullet as p
import time

useMaximalCoordinates = 0

p.connect(p.GUI)
p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(fixedTimeStep=1./120.)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])

mass = 1
visualShapeId = -1


for i in range (5):
	for j in range (5):
		for k in range (5):
			if (k&2):
				sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
			else:
				sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
			p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
			


p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)