def create(self):
p.connect(p.GUI)
p.createMultiBody(0, 0)
p.setGravity(0, 0, -9.8)
p.setAdditionalSearchPath(data.getDataPath())
p.loadSDF("stadium.sdf")
#p.setRealTimeSimulation(True)
#bulid a plate
self.plateId = p.createCollisionShape(
p.GEOM_BOX,
halfExtents=[self.plateLong, self.plateWidth, self.plateHigh])
#bulid a motor
self.motorId = p.createCollisionShape(p.GEOM_CYLINDER,
radius=self.motorRadius,
height=self.motorHeight)
#bulid a ball
self.ballId = p.createCollisionShape(p.GEOM_SPHERE,
radius=self.ballRadius,
height=self.motorHeight)
self.ballUid = p.createMultiBody(self.ballMass, self.ballId,
self.visualShapeId, self.ballStartPos,
self.ballStartOrn)
#bulit a multi-body
self.linkCollisionShapeIndices = [self.motorId]
self.controlerUid = p.createMultiBody(
self.mass,
self.plateId,
self.visualShapeId,
self.basePosition,
self.baseOrientation,
linkMasses=self.link_Masses,
linkCollisionShapeIndices=self.linkCollisionShapeIndices,
linkVisualShapeIndices=self.linkVisualShapeIndices,
linkPositions=self.linkPositions,
linkOrientations=self.linkOrientations,
linkInertialFramePositions=self.linkInertialFramePositions,
linkInertialFrameOrientations=self.linkInertialFrameOrientations,
linkParentIndices=self.indices,
linkJointTypes=self.jointTypes,
linkJointAxis=self.axis)
#change some physical parameters
p.changeDynamics(self.controlerUid,
-1,
spinningFriction=0.01,
rollingFriction=0.01,
linearDamping=0.0)
#constraint objects
#p.createConstraint(self.motorId,-1,-1,-1,p.JOINT_POINT2POINT,[0,0,0],[0,0,0],[0,0,1.5])
p.createConstraint(self.controlerUid, -1, -1, -1, p.JOINT_POINT2POINT,
[0, 0, 0], [0, 0, 0], [0, 0, 1.5])
p.setRealTimeSimulation(self.useRealTimeSim)
def main():
p = argparse.ArgumentParser()
p.add_argument('sdf')
args = p.parse_args()
sdffile = str(path.Path(args.sdf).resolve())
print(sdffile)
pb.connect(pb.GUI) # type: ignore
pb.loadSDF(sdffile) # type: ignore
input()
def __init__(self, init_pos, robot_id, dt):
self.id = robot_id
self.dt = dt
self.pybullet_id = p.loadSDF("../models/robot.sdf")[0]
self.joint_ids = list(range(p.getNumJoints(self.pybullet_id)))
self.initial_position = init_pos
self.reset()
self.passedtime = 0
self.converttime = 10
self.is_formation = 0
self.formation = []
self.x = None
self.y = None
# No friction between body and surface.
p.changeDynamics(self.pybullet_id,
-1,
lateralFriction=5.,
rollingFriction=0.)
# Friction between joint links and surface.
for i in range(p.getNumJoints(self.pybullet_id)):
p.changeDynamics(self.pybullet_id,
i,
lateralFriction=5.,
rollingFriction=0.)
self.messages_received = []
self.messages_to_send = []
self.neighbors = []
def reset(self):
self._kuka_hand = p.loadSDF("model.sdf")
self.kuka_handId = self._kuka_hand[0]
print("self._kuka_hand", self._kuka_hand)
print("self.kuka_handId", self.kuka_handId)
#self.cameraSetup()
# reset joints to the middle valu
p.resetBasePositionAndOrientation(
self.kuka_handId, [-0.100000, 0.000000, 0.070000],
[0.000000, 0.000000, 0.000000, 1.000000])
self.jointInfo.get_infoForAll_joints(self._kuka_hand)
self.numJoints = p.getNumJoints(self.kuka_handId)
self.num_Active_joint = self.jointInfo.getNumberOfActiveJoints()
self.indexOf_activeJoints = self.jointInfo.getIndexOfActiveJoints()
self.motorNames = []
self.motorIndices = []
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.kuka_handId, i)
qIndex = jointInfo[3]
if qIndex > -1:
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def main():
# Open GUI and set pybullet_data in the path
p.connect(p.GUI)
#p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Load plane contained in pybullet_data
p.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf"))
# Set gravity for simulation
p.setGravity(0,0,-9.8)
# load robot model
robotIds = p.loadSDF(os.path.join(robot_data.getDataPath(), "iCub/icub_fixed_model.sdf"))
icubId = robotIds[0]
# set constraint between base_link and world
p.createConstraint(icubId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],
[p.getBasePositionAndOrientation(icubId)[0][0],
p.getBasePositionAndOrientation(icubId)[0][1],
p.getBasePositionAndOrientation(icubId)[0][2]*1.2],
p.getBasePositionAndOrientation(icubId)[1])
##init_pos for standing
# without FT_sensors
init_pos = [0]*15 + [-29.4, 28.8, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 44.59, 0, 0, 0]
# with FT_sensors
#init_pos = [0]*19 + [-29.4, 28.8, 0, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 0, 44.59, 0, 0, 0]
# all set to zero
#init_pos = [0]*p.getNumJoints(icubId)
# Load other objects
p.loadURDF(os.path.join(pybullet_data.getDataPath(),"table/table.urdf"), [1.1, 0.0, 0.0])
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "lego/lego.urdf"), [0.5,0.0,0.8])
# add debug slider
jointIds=[]
paramIds=[]
joints_num = p.getNumJoints(icubId)
print("len init_pos ",len(init_pos))
print("len num joints", joints_num)
for j in range (joints_num):
info = p.getJointInfo(icubId,j)
jointName = info[1]
#jointType = info[2]
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), info[8], info[9], init_pos[j]/180*m.pi))
while True:
for i in range(joints_num):
p.setJointMotorControl2(icubId, i, p.POSITION_CONTROL,
targetPosition=p.readUserDebugParameter(i),
targetVelocity=0.0, positionGain=0.25, velocityGain=0.75, force=50)
p.stepSimulation()
time.sleep(0.01)
def parseWorld(p, filepath):
# load element tree from file
tree = ET.parse(filepath)
root = tree.getroot()
if 'version' in root.attrib:
print("version=",root.attrib['version'])
models = {}
log("Parsing models...", 'INFO')
for model_xml in root.iter('model'):
log(" Adding link {}.".format(model_xml.attrib['name']), 'DEBUG')
newmodel = parseModel(model_xml, "")
models[model_xml.attrib['name']] = newmodel
if 'uri' in newmodel.keys() and newmodel['uri'] is not None:
bodies = p.loadSDF(newmodel['uri'])
#transform all bodies using the 'pose'
for b in bodies:
old_pos,old_orn = p.getBasePositionAndOrientation(b)
pose_xyz = newmodel['pose_xyz']
pose_rpy = newmodel['pose_rpy']
pose_orn = p.getQuaternionFromEuler(pose_rpy)
new_pos, new_orn = p.multiplyTransforms(pose_xyz, pose_orn, old_pos,old_orn)
p.resetBasePositionAndOrientation(b, new_pos, new_orn)
p.changeDynamics(b, -1, mass=0)
print("bodies=",bodies)
return models
def reset(self):
objects = p.loadSDF(
os.path.join(self.urdfRootPath, "mz07_with_gripper.sdf"))
self.robotUid = objects[0]
p.resetBasePositionAndOrientation(
self.robotUid, [-0.100000, 0.000000, 0.070000],
[0.000000, 0.000000, 0.000000, 1.000000])
self.jointPositions = [0, 0, 0, 0, 0, 0]
self.numJoints = p.getNumJoints(self.robotUid)
for jointIndex in range(self.numJoints):
p.resetJointState(self.robotUid, jointIndex,
self.jointPositions[jointIndex])
p.setJointMotorControl2(
self.robotUid,
jointIndex,
p.POSITION_CONTROL,
targetPosition=self.jointPositions[jointIndex],
force=self.maxForce)
self.motorNames = []
self.motorIndices = []
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.robotUid, i)
qIndex = jointInfo[3]
if qIndex > -1:
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def placeRandBlock(self):
# randomly generate a 3d pose of the block to grasp
# limit is at xpos = 0.215, center is xpos = 0.195
#xpos = 0.198 + 0.035 * (random.random()-0.5)
#ypos = 0 + 0.056 * (random.random()-0.5)
#ang = 3.14 * 0.5 + 3.1415925438 * random.random()
"""
xpos = 0.195
ypos = -0.015
ang = 3.14 * 0.5
orn = p.getQuaternionFromEuler([0, 1.57079632679, ang])
self.blockUid = p.loadURDF("data/block.urdf",
xpos, ypos, -0.0149,
orn[0], orn[1], orn[2], orn[3])
"""
xpos = 0.195 + 0.04
#xpos = 0.195 + 0.065 + 0.01
#xpos = 0.195 + 0.065 + 0.025 * (random.random()-0.5)
ypos = 0.000
#ypos = 0+ 0.055 * (random.random()-0.5)
q = R.from_euler('xyz', [90, 0, 0], degrees=True).as_quat()
#self.mugUid = p.loadURDF("data/mug.urdf", xpos, ypos, -0.18,
# q[0], q[1], q[2], q[3])
self.mugUid = p.loadSDF("data/mug.sdf",
useMaximalCoordinates = False)[0]
# set the position of the base to be on the table
p.resetBasePositionAndOrientation(self.mugUid, [xpos, ypos, -0.15], q)
def reset(self):
robot_path = resource_filename(__name__,
"/kuka_handlit_model/model.sdf")
self._kuka_hand = p.loadSDF(robot_path)
self.kuka_handId = self._kuka_hand[0]
if self.kuka_loc != None:
p.resetBasePositionAndOrientation(self.kuka_handId,
self.kuka_loc[0:3],
self.kuka_loc[3:])
self.jointInfo.get_infoForAll_joints(self._kuka_hand)
self.numJoints = p.getNumJoints(self.kuka_handId)
self.num_Active_joint = self.jointInfo.getNumberOfActiveJoints()
self.indexOf_activeJoints = self.jointInfo.getIndexOfActiveJoints()
resetJoints = p.calculateInverseKinematics(self.kuka_handId,
self.kukaEndEffectorIndex,
self.ee_xyz)
self.motorNames = []
self.motorIndices = []
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.kuka_handId, i)
qIndex = jointInfo[3]
if qIndex > -1:
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
for i in range(self.num_Active_joint):
p.resetJointState(self.kuka_handId, self.indexOf_activeJoints[i],
resetJoints[i])
def _reset(self):
#print("KukaGymEnv _reset")
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
texUid = p.loadTexture("./cube_new/aaa.png")
cube_objects = p.loadSDF("./cube_new/model.sdf")
self.cubeId = cube_objects[0]
p.changeVisualShape(cube_objects[0], -1,rgbaColor =[1,1,1,1])
p.changeVisualShape(cube_objects[0], -1, textureUniqueId = texUid)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
self.tray = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.table = p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-0.820000,0.000000,0.000000,0.0,1.0)
#print("table:::",self.table)
#print("tray:::",self.tray)
#This is where the object spawn at random i should adjust this so it spawn at random position with specific area bound if reward increased
xpos = 0.58
ypos = 0.04
ang = 3.14*0.5
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3])
#This where robot is reset i should modefy this to adjust height of end-effector
p.setGravity(0,0,-10)
self._kuka_hand = Kuka_Handlit(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array([self._observation])
def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
#p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,0,0)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
import random
xpos = 0.55 +0.12*random.random()
ypos = 0 +0.2*random.random()
ang = 3.14*0.5+3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
orn = np.array((-0.0, 1, 0.0, 0.0))
pos = (0.5321085918022986, -0.0011177175302174629, 0.2522914797947443),
self.constraint = p.createConstraint(self.kukaUid,7,-1,-1,p.JOINT_FIXED,pos,orn,[0.500000,0.300006,0.700000])
def _reset(self):
p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
#p.loadURDF("%splane.urdf" % self._urdfRoot)
stadiumobjects = p.loadSDF("%sstadium.sdf" % self._urdfRoot)
#move the stadium objects slightly above 0
for i in stadiumobjects:
pos, orn = p.getBasePositionAndOrientation(i)
newpos = [pos[0], pos[1], pos[2] + 0.1]
p.resetBasePositionAndOrientation(i, newpos, orn)
dist = 5 + 2. * random.random()
ang = 2. * 3.1415925438 * random.random()
ballx = dist * math.sin(ang)
bally = dist * math.cos(ang)
ballz = 1
self._ballUniqueId = p.loadURDF("sphere2red.urdf",
[ballx, bally, ballz])
p.setGravity(0, 0, -10)
self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot,
timeStep=self._timeStep)
self._envStepCounter = 0
for i in range(100):
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def episode_restart(self):
Scene.episode_restart(self) # contains cpp_world.clean_everything()
# stadium_pose = cpp_household.Pose()
# if self.zero_at_running_strip_start_line:
# stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants
self.stadium = p.loadSDF("stadium.sdf")
self.ground_plane_mjcf = p.loadMJCF("mjcf/ground_plane.xml")
def load_pybullet_from_urdf_or_sdf(inp_path, position = [0, 0, 0], quaternion = [0, 0, 0, 1], fixed = True, packageMap = None):
full_path = os.path.expandvars(inp_path)
ext = full_path.split(".")[-1]
if ext == "urdf":
print "Loading ", full_path, " as URDF in pos ", position
if packageMap is not None:
# load text of URDF, resolve package URLS to absolute paths,
# and save it out to a temp directory
with open(full_path, 'r') as urdf_file:
full_text = urdf_file.read()
with open("/tmp/resolved_urdf.urdf", 'w') as out_file:
# Find package references
matches = re.finditer(r'package:\/\/[^\/]*\/', full_text)
curr_ind = 0
# Replace them in output file with the global path
for match in matches:
out_file.write(full_text[curr_ind:match.start()])
out_file.write(packageMap.resolveFilename(match.group(0)))
curr_ind = match.end()
out_file.write(full_text[curr_ind:-1])
full_path = "/tmp/resolved_urdf.urdf"
return pybullet.loadURDF(full_path, basePosition=position, baseOrientation=quaternion, useFixedBase=fixed)
elif ext == "sdf":
print "Loading ", full_path, " as SDF in pos ", position
return pybullet.loadSDF(full_path)
else:
print "Unknown extension in path ", full_path, ": ", ext
exit(-1)
def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
#for i in range (p.getNumJoints(self.kukaUid)):
# print(p.getJointInfo(self.kukaUid,i))
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range (self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid,i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def load_model(self, path, pose=None, fixed_base=True, scaling=1.0):
"""
Load the objects in the scene
:param path: str
Path to the object
:param pose: Pose
Position and Quaternion
:param fixed_base: str
Fixed in the scene
:param scaling: float
Scale object
:return: int
ID of the loaded object
"""
if path.endswith('.urdf'):
body = p.loadURDF(path, useFixedBase=fixed_base, flags=0, globalScaling=scaling,
physicsClientId=self.client_id)
elif path.endswith('.sdf'):
body = p.loadSDF(path, physicsClientId=self.client_id)
elif path.endswith('.xml'):
body = p.loadMJCF(path, physicsClientId=self.client_id)
elif path.endswith('.bullet'):
body = p.loadBullet(path, physicsClientId=self.client_id)
else:
raise ValueError(path)
if pose is not None:
self.set_pose(body, pose)
return body
def reset(self): """Resets the robot back to a deterministic starting position.""" objects = p.loadSDF(os.path.join(self.urdf_rootpath, "kuka_iiwa/kuka_with_gripper2.sdf")) self.kuka_uid = objects[0] p.resetBasePositionAndOrientation(self.kuka_uid, [-0.100000, 0.000000, 0.070000], [0.000000, 0.000000, 0.000000, 1.000000]) self.joint_positions = [ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ] self.num_joints = p.getNumJoints(self.kuka_uid) for joint_index in range(self.num_joints): p.resetJointState(self.kuka_uid, joint_index, self.joint_positions[joint_index]) p.setJointMotorControl2(self.kuka_uid, joint_index, p.POSITION_CONTROL, targetPosition=self.joint_positions[joint_index], force=self.max_force) self.tray_uid = p.loadURDF(os.path.join(self.urdf_rootpath, "tray/tray.urdf"), 0.640000, 0.075000, -0.190000, 0.000000, 0.000000, 1.000000, 0.000000) self.end_effector_pos = [0.537, 0.0, 0.5] self.end_effector_angle = 0 self.motor_names = [] self.motor_indices = [] for i in range(self.num_joints): joint_info = p.getJointInfo(self.kuka_uid, i) q_idx = joint_info[3] if q_idx > -1: self.motor_names.append(str(joint_info[1])) self.motor_indices.append(i)
def __init__(self, init_pos, robot_id, dt):
self.id = robot_id
self.dt = dt
self.pybullet_id = p.loadSDF(os.path.join(os.path.dirname(__file__),"../models/robot.sdf"))[0]
self.joint_ids = list(range(p.getNumJoints(self.pybullet_id)))
self.initial_position = init_pos
self.reset()
self.time_elapsed = 0
self.switch_time = 10
self.is_goal = 0
self.goal = []
self.gx = None
self.gy = None
# No friction between bbody and surface.
p.changeDynamics(self.pybullet_id, -1, lateralFriction=5., rollingFriction=0.)
# Friction between joint links and surface.
for i in range(p.getNumJoints(self.pybullet_id)):
p.changeDynamics(self.pybullet_id, i, lateralFriction=5., rollingFriction=0.)
self.messages_received = []
self.messages_to_send = []
self.neighbors = []
def __init__(self, init_pos, robot_id, dt):
self.id = robot_id
self.dt = dt
self.pybullet_id = p.loadSDF("../models/robot.sdf")[0]
self.joint_ids = list(range(p.getNumJoints(self.pybullet_id)))
self.initial_position = init_pos
self.reset()
# No friction between body and surface.
p.changeDynamics(self.pybullet_id,
-1,
lateralFriction=5.,
rollingFriction=0.)
# Friction between joint links and surface.
for i in range(p.getNumJoints(self.pybullet_id)):
p.changeDynamics(self.pybullet_id,
i,
lateralFriction=5.,
rollingFriction=0.)
self.messages_received = []
self.messages_to_send = []
self.neighbors = []
self.state = np.zeros((6, 3))
self.p = [[0.5, -1], [0.5, 1], [1.5, -1], [1.5, 1], [2.5, -1],
[2.5, 1]] # square
def __init__(self,
render=False,
torque_limits=[100] * 6,
init_noise=.005,
):
self.render = render
self.torque_limits = np.array(torque_limits)
self.init_noise = init_noise
low = -np.ones(6,dtype=np.float32)
self.action_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32)
low = -np.ones(17, dtype=np.float32)*np.inf
self.observation_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32)
if render:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self.plane_id = p.loadSDF(pybullet_data.getDataPath() + "/plane_stadium.sdf")[0]
self.walker_id = p.loadMJCF(pybullet_data.getDataPath() + "/mjcf/walker2d.xml")[0]
p.setGravity(0, 0, -9.8)
self.dt = p.getPhysicsEngineParameters()['fixedTimeStep']
self.reset()
def __init__(self, mech, k=[2000.0, 20.0], d=[0.45, 0.45]):
"""
This class defines the actions a gripper can take such as grasping a handle
and executing PD control
:param mech: the gen.generator_busybox.Mechanism being actuated
:param k: a vector of length 2 where the first entry is the linear position
(stiffness) gain and the second entry is the angular position gain
:param d: a vector of length 2 where the first entry is the linear derivative
(damping) gain and the second entry is the angular derivative gain
"""
self.use_gripper = False
if self.use_gripper:
self.id = p.loadSDF("models/gripper/gripper_high_fric.sdf")[0]
self._left_finger_tip_id = 2
self._right_finger_tip_id = 5
self._left_finger_base_joint_id = 0
self._right_finger_base_joint_id = 3
self._finger_force = 20
self.pose_tip_world_reset = util.Pose([0.0, 0.0, 0.2], \
[0.50019904, 0.50019904, -0.49980088, 0.49980088])
# get mass of gripper
mass = 0
for link in range(p.getNumJoints(self.id)):
mass += p.getDynamicsInfo(self.id, link)[0]
self._mass = mass
self.errors = []
self.forces = []
# control parameters
self.k = k
self.d = d
def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
#for i in range (p.getNumJoints(self.kukaUid)):
# print(p.getJointInfo(self.kukaUid,i))
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range (self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid,i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def scene_2D_object(p):
plane = [p.loadSDF(os.path.join(urdfRoot, "plane_stadium.sdf"))]
colcubeId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[0.05, 0.05, 0.05])
cube = [p.createMultiBody(2, colcubeId, 2, [0.3, 0.3, 0.0])]
colwallId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[0.05, 1.5, 0.5])
wall = [
p.createMultiBody(0, colwallId, 2, [1.4, 0, 0.0],
p.getQuaternionFromEuler([0, 0, 0]))
]
wall = [
p.createMultiBody(0, colwallId, 2, [-1.4, 0, 0.0],
p.getQuaternionFromEuler([0, 0, 0]))
]
wall = [
p.createMultiBody(0, colwallId, 2, [0, 1.4, 0],
p.getQuaternionFromEuler([0, 0, math.pi / 2]))
]
wall = [
p.createMultiBody(0, colwallId, 2, [0, -1.4, 0],
p.getQuaternionFromEuler([0, 0, math.pi / 2]))
]
return [cube]
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
# see https://github.com/bulletphysics/bullet3/issues/1934 to load multiple colors
if self.render:
visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.1,
rgbaColor=[1, 0, 0, 1],
specularColor=[0.4, .4, 0])
self.desired_pos_sphere = p.createMultiBody(
baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseVisualShapeIndex=visualShapeId)
rand_ori = self.np_random.uniform(
low=-1, high=1, size=(4, )) + np.asarray([0, 0, 0, 2])
rand_ori = rand_ori / np.linalg.norm(rand_ori)
rand_pos = self.np_random.uniform(low=-0.5, high=0.5, size=(3, ))
#TODO Start with random vel
self.quad = p.loadURDF(os.path.join(currentdir, "quad.urdf"),
[0, 0, 0.5] + rand_pos,
rand_ori,
flags=p.URDF_USE_INERTIA_FROM_FILE)
filename = os.path.join(pybullet_data.getDataPath(),
"plane_stadium.sdf")
self.ground_plane_mjcf = p.loadSDF(filename)
# filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf")
# self.ground_plane_mjcf = self._p.loadSDF(filename)
#
for i in self.ground_plane_mjcf:
p.changeDynamics(i, -1, lateralFriction=0.8, restitution=0.5)
p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.8])
self.timeStep = 0.01
self.gravity = -9.8
p.setGravity(0, 0, self.gravity)
# Default 0.04 for linear and angular damping.
# p.changeDynamics(self.quad, -1, linearDamping=1)
# p.changeDynamics(self.quad, -1, angularDamping=1)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
info = p.getDynamicsInfo(self.quad, -1)
self.mass = info[0]
self.zero_thrust = -self.mass * self.gravity
initialCartPos = self.np_random.uniform(low=-2, high=2, size=(1, ))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1, ))
# p.resetJointState(self.quad, 1, initialAngle)
# p.resetJointState(self.quad, 0, initialCartPos)
world_pos, world_ori = p.getBasePositionAndOrientation(self.quad)
world_vel, world_rot_vel = p.getBaseVelocity(self.quad)
self.state = world_pos + world_ori + world_vel + world_rot_vel
return np.array(self.state)
def reset(self):
objects = p.loadSDF(
os.path.join(self.urdfRootPath,
"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
p.resetBasePositionAndOrientation(
self.kukaUid,
[-0.100000, 0.000000, 0.070000],
[0.000000, 0.000000, 0.000000, 1.000000],
)
self.jointPositions = [
0.006418,
0.413184,
-0.011401,
-1.589317,
0.005379,
1.137684,
-0.006539,
0.000048,
-0.299912,
0.000000,
-0.000043,
0.299960,
0.000000,
-0.000200,
]
if self.jp_override:
for j, v in self.jp_override.items():
j_ix = int(j) - 1
if j_ix >= 0 and j_ix <= 13:
self.jointPositions[j_ix] = v
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range(self.numJoints):
p.resetJointState(self.kukaUid, jointIndex,
self.jointPositions[jointIndex])
p.setJointMotorControl2(
self.kukaUid,
jointIndex,
p.POSITION_CONTROL,
targetPosition=self.jointPositions[jointIndex],
force=self.maxForce,
)
self.trayUid = p.loadURDF(
os.path.join(self.urdfRootPath, "tray/tray.urdf"),
0.640000,
0.075000,
-0.190000,
0.000000,
0.000000,
1.000000,
0.000000,
)
self.endEffectorPos = [0.537, 0.0, 0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
def __init__(self):
sdf = 'salamandra_robotica.sdf'
self.identity = pybullet.loadSDF(sdf)[0]
pybullet.resetBasePositionAndOrientation(
bodyUniqueId=self.identity,
posObj=[0, 0, 0.1],
ornObj=[0, 0, 0, 1],
)
def _load_model_file_into_sim(self, filepath):
sim_id = None
if filepath[-5:] == '.urdf':
sim_id = p.loadURDF(filepath, flags=p.URDF_MERGE_FIXED_LINKS)
elif filepath[-4:] == '.sdf':
sim_id = p.loadSDF(filepath)
if isinstance(sim_id, tuple): sim_id = sim_id[0]
return sim_id
def _load_scene(self):
if self.hoop is None:
self.hoop = pb.loadSDF(os.path.join(
os.path.dirname(__file__),
'assets/bbbot_gazebo/models/hoop/model.sdf'),
physicsClientId=self.client)[0]
pb.resetBasePositionAndOrientation(self.hoop,
self.hoopStartPos,
self.hoopStartOrientation,
physicsClientId=self.client)
if self.cyl is None:
self.cyl = pb.loadSDF(os.path.join(
os.path.dirname(__file__),
'assets/bbbot_gazebo/models/cylinder/model.sdf'),
physicsClientId=self.client)[0]
pb.resetBasePositionAndOrientation(self.cyl,
self.cylStartPos,
self.cylStartOrientation,
physicsClientId=self.client)
if self.backboard is None:
self.backboard = pb.loadSDF(os.path.join(
os.path.dirname(__file__),
'assets/bbbot_gazebo/models/backboard/model.sdf'),
physicsClientId=self.client)[0]
pb.changeDynamics(self.backboard, -1, restitution=1.0)
pb.resetBasePositionAndOrientation(self.backboard,
self.backboardStartPos,
self.backboardStartOrientation,
physicsClientId=self.client)
if self.ball is None:
# TODO: why is this not respecting SDL restitution?
self.ball = pb.loadSDF(os.path.join(
os.path.dirname(__file__),
'assets/bbbot_gazebo/models/basketball/model.sdf'),
physicsClientId=self.client)[0]
# print (pb.getDynamicsInfo(self.ball, -1))
pb.changeDynamics(self.ball, -1, restitution=0.853)
# print (pb.getDynamicsInfo(self.ball, -1))
pb.resetBasePositionAndOrientation(self.ball,
self.ballStartPos,
self.ballStartOrientation,
physicsClientId=self.client)
def __init__(self):
self.gravity = -9.8
self.state = None
self.stateWOCrop = None
self.believedState = None
self.pixelWidth = 1000
self.pixelHeight = 1000
self.camDistance = -3
self.camTargetPos = [0, 0, 0]
self.near = 1
self.far = 1000
self.fov = 50
self.boxcount = 0
self.Y = 0
self.X = 0
self.ldcWidth = 0.68
self.ldcHeight = 0.68
self.ldcLength = 1.2
self.slabWidth = 0.02
self.resp = 0
self.resw = 0
# This will be given value in the render function
self.action_space = None
physicsClient = p.connect(p.DIRECT)
p.setGravity(0, 0, self.gravity) # this sets the gravity
p.setPhysicsEngineParameter(fixedTimeStep=1 / 60,
numSolverIterations=10)
p.loadSDF('ldc-model-big_center.sdf')
# p.setRealTimeSimulation(1)
###### Adding floor ########k
planeId = p.createCollisionShape(p.GEOM_PLANE)
planeVisualId = p.createVisualShape(
p.GEOM_PLANE, rgbaColor=[0.7686, 0.847, 0.933, 1])
p.createMultiBody(0, planeId, planeVisualId, [0, 0, 0], [0, 0, 0, 1])
self.resPix()
self.resWorld()
def reset(self):
objects = p.loadSDF(
os.path.join(self.urdfRootPath,
"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
p.resetBasePositionAndOrientation(
self.kukaUid, [-0.100000, 0.000000, 0.070000],
[0.000000, 0.000000, 0.000000, 1.000000])
# self.jointPositions = [0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048,
# -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200]
self.init_joints = [
0.0052822753, 0.63178448, -0.009966143, -1.7201607, 0.008282072,
0.7897110, -0.00860167, 0.000048, -0.299912, 0.000000, -0.000043,
0.299960, 0.000000, -0.000200
]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range(self.numJoints):
p.resetJointState(self.kukaUid, jointIndex,
self.init_joints[jointIndex])
p.setJointMotorControl2(
self.kukaUid,
jointIndex,
p.POSITION_CONTROL,
targetPosition=self.init_joints[jointIndex],
force=self.maxForce)
goal_pose = spaces.Box(
low=np.array([0.54, -0.155, 0.31630]) - np.array([0.2, 0.2, 0.1]),
high=np.array([0.54, -0.155, 0.31630]) + np.array([0.2, 0.2, 0.1]))
# 爪子初始位置 随机化
for _ in range(200):
self.applyAction(goal_pose.sample(), 0, fingerAngle=0.25)
p.stepSimulation()
# 框子
self.trayUid = p.loadURDF(
os.path.join(self.urdfRootPath, "tray/tray.urdf"), 0.540000,
-0.255000, -0.190000, 0.000000, 0.000000, 1.000000, 0.000000)
self.endEffectorAngle = 0
self.current_endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid, i)
qIndex = jointInfo[3]
if qIndex > -1:
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def __init__(
self,
render=False,
torque_limits=[100] * 6,
init_noise=.005,
physics_params=None,
dynamics_params=None,
):
self.args = locals()
self.torque_limits = np.array(torque_limits)
self.init_noise = init_noise
self.cur_step = 0
low = -np.ones(6)
self.action_space = gym.spaces.Box(low=low,
high=-low,
dtype=np.float32)
low = -np.ones(19) * np.inf
self.observation_space = gym.spaces.Box(low=low,
high=-low,
dtype=np.float32)
if render:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self.plane_id = p.loadSDF(pybullet_data.getDataPath() +
"/plane_stadium.sdf")[0]
self.walker_id = p.loadMJCF(pybullet_data.getDataPath() +
"/mjcf/walker2d.xml")[0]
#flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)[0] # TODO not sure the self collision needs to be here..
p.setGravity(0, 0, -9.8)
if physics_params is None:
physics_params = {}
if dynamics_params is None:
dynamics_params = {}
p.changeDynamics(self.plane_id, -1, **dynamics_params)
for i in range(p.getNumJoints(self.walker_id)):
p.changeDynamics(self.walker_id, i, **dynamics_params)
p.changeDynamics(self.walker_id, -1, **dynamics_params)
p.setPhysicsEngineParameter(**physics_params)
self.dt = p.getPhysicsEngineParameters()['fixedTimeStep']
self.reset()
def reset(self):
self.ordered_joints = []
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( # TODO: Not sure if this works, try it with kuka
p.loadSDF(os.path.join("models_robot", self.model_sdf)))
self.robot_specific_reset()
s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use
self.potential = self.calc_potential()
return s
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") self.stadium = p.loadSDF(filename) planeName = os.path.join(pybullet_data.getDataPath(),"mjcf/ground_plane.xml") self.ground_plane_mjcf = p.loadMJCF(planeName) for i in self.ground_plane_mjcf: p.changeVisualShape(i,-1,rgbaColor=[0,0,0,0])
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") self.ground_plane_mjcf = p.loadSDF(filename) for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) for j in range(p.getNumJoints(i)): p.changeDynamics(i,j,lateralFriction=0)
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"plane_stadium.sdf") self.ground_plane_mjcf=p.loadSDF(filename) #filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") #self.ground_plane_mjcf = p.loadSDF(filename) # for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8]) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
import pybullet as p
import pybullet_data
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadSDF("stadium.sdf")
p.setGravity(0, 0, -10)
objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0]
p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1])
p.changeDynamics(sphere, -1, linearDamping=0.9)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
# split by alignment word
chunks = wholeFile.split(b'\xaa\xbb')
log = list()
for chunk in chunks:
if len(chunk) == sz:
values = struct.unpack(fmt, chunk)
record = list()
for i in range(ncols):
record.append(values[i])
log.append(record)
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
p.loadURDF("tray/tray.urdf",[0,0,0])
p.loadURDF("block.urdf",[0,0,2])
log = readLogFile("data/block_grasp_log.bin")
recordNum = len(log)
itemNum = len(log[0])
objectNum = p.getNumBodies()
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)
def Step(stepIndex):
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)
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.)
import pybullet as p
import time
p.connect(p.GUI)
p.resetSimulation()
timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
print("load plane.urdf")
p.loadURDF("plane.urdf")
print("load r2d2.urdf")
p.loadURDF("r2d2.urdf",0,0,1)
print("load kitchen/1.sdf")
p.loadSDF("kitchens/1.sdf")
print("load 100 times plate.urdf")
for i in range (100):
p.loadURDF("dinnerware/plate.urdf",0,i,1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.stopStateLogging(timinglog)
print("stopped state logging")
p.getCameraImage(320,200)
while (1):
p.stepSimulation()
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,-10)
useRealTimeSim = 1
#for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this
#p.loadURDF("plane.urdf")
p.loadSDF(os.path.join(pybullet_data.getDataPath(),"stadium.sdf"))
car = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"racecar/racecar.urdf"))
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
inactive_wheels = [3,5,7]
wheels = [2]
for wheel in inactive_wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
steering = [4,6]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-10,10,0)
maxForceSlider = p.addUserDebugParameter("maxForce",0,10,10)
import pybullet as p
p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin")
p.loadSDF("newsdf.sdf")
while (1):
p.getCameraImage(320, 240, flags=p.ER_NO_SEGMENTATION_MASK)
p.stepSimulation()
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0]
print ("kuka gripper=")
print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970])
jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ]
for jointIndex in range (p.getNumJoints(kuka_gripper)):
p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0])
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
uiCube = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 1, 0],
useMaximalCoordinates=True)
textOrn = p.getQuaternionFromEuler([0, 0, -1.5707963])
numLines = 1
lines = [-1] * numLines
p.stepSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
#objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
kitchenObj = p.loadSDF("kitchens/1.sdf")
#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [
p.loadURDF("pr2_gripper.urdf", 0.500000, 0.300006, 0.700000, -0.000000, -0.000000, -0.000031,
1.000000)
]
pr2_gripper = objects[0]
print("pr2_gripper=")
print(pr2_gripper)
jointPositions = [0.550569, 0.000000, 0.549657, 0.000000]
for jointIndex in range(p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper, jointIndex, jointPositions[jointIndex])
p.setJointMotorControl2(pr2_gripper, jointIndex, p.POSITION_CONTROL, targetPosition=0, force=0)
pr2_cid = p.createConstraint(pr2_gripper, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.2, 0, 0],
