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)
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")
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)
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.)
import pybullet as p
import time
import math
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
def 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],
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)
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]
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)
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)
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)
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.)
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 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
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])
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)
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()
#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):
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)
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)):
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),
)
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
])
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)
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
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()
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()
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)
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)
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)
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))]
)
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
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')
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)
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)
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:
import pybullet as p
import pybullet_data as pd
import time
import math
usePhysX = True
useMaximalCoordinates = True
if usePhysX:
p.connect(p.PhysX,options="--numCores=8 --solver=pgs")
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
p.setAdditionalSearchPath(pd.getDataPath())
#Always make ground plane maximal coordinates, to avoid performance drop in PhysX
#See https://github.com/NVIDIAGameWorks/PhysX/issues/71
p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json")
jran = 50
iran = 100
num=64
radius=0.1
numDominoes=0
def 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)
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)
import pybullet as p
p.connect(p.GUI)
cube = p.loadURDF("cube.urdf")
frequency = 240
timeStep = 1. / frequency
p.setGravity(0, 0, -9.8)
p.changeDynamics(cube, -1, linearDamping=0, angularDamping=0)
p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
for i in range(frequency):
p.stepSimulation()
pos, orn = p.getBasePositionAndOrientation(cube)
print(pos)
import pybullet as p
import time
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)