def __init__(self, customURDFPath):
# ==========
# Objects config
# ==========
self.plane_pos = (0, 0, 0)
self.table_pos = (0.7, 0, 0)
self.object0_pos = (0.7, 0.0, 0.53)
self.object0_orn = (0, 0, math.pi) # Euler: roll, pitch, yaw
self.object1_pos = (0.58, 0.0, 0.53)
self.object1_orn = (0, 0, math.pi / 2) # Euler: roll, pitch, yaw
# ==========
# Load URDF
# ==========
pybulletDataPath = pybullet_data.getDataPath()
self.planeUid = p.loadURDF(os.path.join(pybulletDataPath,
"plane.urdf"),
basePosition=self.plane_pos)
self.tableUid = p.loadURDF(os.path.join(customURDFPath,
"cabinet.urdf"),
basePosition=self.table_pos,
useFixedBase=True)
self.object0Uid = p.loadURDF(
os.path.join(ycb_objects.getDataPath(), 'YcbHammer', "model.urdf"),
basePosition=self.object0_pos,
baseOrientation=p.getQuaternionFromEuler(self.object0_orn))
self.object1Uid = p.loadURDF(
os.path.join(ycb_objects.getDataPath(), 'YcbBanana', "model.urdf"),
basePosition=self.object1_pos,
baseOrientation=p.getQuaternionFromEuler(self.object1_orn))
# ==========
# RGB-D camera config
# Reference: https://towardsdatascience.com/simulate-images-for-ml-in-pybullet-the-quick-easy-way-859035b2c9dd
# ==========
self.camera_target_pos = [0.7, 0, 0.6]
self.camera_euler_orn = [0, 0, -90] # roll pitch yaw
self.view_matrix = p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=[0.7, 0, 0.6],
distance=.35,
roll=0,
pitch=0,
yaw=-90,
upAxisIndex=2)
self.proj_matrix = p.computeProjectionMatrixFOV(fov=60,
aspect=float(480) /
float(360),
nearVal=0.1,
farVal=0.4)
def load_object(self, obj_name):
# Load object. Randomize its start position if requested
self._obj_name = obj_name
self._obj_init_pose = self._sample_pose()
self.obj_id = p.loadURDF(os.path.join(ycb_objects.getDataPath(), obj_name, "model.urdf"),
basePosition=self._obj_init_pose[:3], baseOrientation=self._obj_init_pose[3:7],
physicsClientId=self._physics_client_id)
def LoadObjectURDF(ObjectName):
print(ycb_objects.getDataPath())
urdfRootPath = pybullet_data.getDataPath()
state_object = BasePosAndOriDict["general"][0]
stateOrientation = p.getQuaternionFromEuler(
BasePosAndOriDict["general"][1])
if not ObjectName.isalpha():
objectUid = p.loadURDF(os.path.join(
urdfRootPath,
"random_urdfs/" + ObjectName + "/" + ObjectName + ".urdf"),
basePosition=state_object,
baseOrientation=stateOrientation)
else:
if ObjectName in BasePosAndOriDict:
state_object = BasePosAndOriDict[ObjectName][0]
stateOrientation = p.getQuaternionFromEuler(
BasePosAndOriDict[ObjectName][1])
objectUid = p.loadURDF(os.path.join(ycb_objects.getDataPath(),
ObjectName, "model.urdf"),
basePosition=state_object,
baseOrientation=stateOrientation)
if ObjectName == 'YcbBanana' or ObjectName == 'YcbTennisBall':
p.changeDynamics(objectUid,
-1,
lateralFriction=0.8,
spinningFriction=0.,
rollingFriction=0.)
if ObjectName == 'YcbTomatoSoupCan':
p.changeDynamics(objectUid,
-1,
lateralFriction=1.0,
spinningFriction=0.8,
rollingFriction=1.0)
if ObjectName == 'YcbPottedMeatCan':
p.changeDynamics(objectUid,
-1,
lateralFriction=0.8,
spinningFriction=0,
rollingFriction=0)
if ObjectName == "YcbSugarBox" or ObjectName == 'YcbGelatinBox' or ObjectName == 'YcbChipsCan':
p.changeDynamics(objectUid,
-1,
lateralFriction=1.0,
spinningFriction=0.,
rollingFriction=0.)
return objectUid
def main():
# ------------------------ #
# --- Setup simulation --- #
# ------------------------ #
# Create pybullet GUI
physics_client_id = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.8, 120, -50, [0.0, -0.0, -0.0])
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
sim_timestep = 1.0 / 240
p.setTimeStep(sim_timestep)
# 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)
# ------------------- #
# --- Setup robot --- #
# ------------------- #
robot = iCubHandsEnv(physics_client_id, use_IK=1, control_arm='r')
p.stepSimulation()
# -------------------------- #
# --- Load other objects --- #
# -------------------------- #
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table/table.urdf"),
[1, 0.0, 0.0])
p.loadURDF(
os.path.join(ycb_objects.getDataPath(), 'YcbFoamBrick', "model.urdf"),
[0.5, -0.03, 0.7])
# Run the world for a bit
for _ in range(100):
p.stepSimulation()
# ------------------ #
# --- Start Demo --- #
# ------------------ #
robot.pre_grasp()
for _ in range(10):
p.stepSimulation()
time.sleep(sim_timestep)
# 1: go above the object
pos_1 = [0.49, 0.0, 0.8]
quat_1 = p.getQuaternionFromEuler([0, 0, m.pi / 2])
robot.apply_action(pos_1 + list(quat_1), max_vel=5)
robot.pre_grasp()
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 2: turn hand above the object
pos_2 = [0.485, 0.0, 0.72]
quat_2 = p.getQuaternionFromEuler([m.pi / 2, 1 / 3 * m.pi, -m.pi])
robot.apply_action(pos_2 + list(quat_2), max_vel=5)
robot.pre_grasp()
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 3: close fingers
pos_cl = [
0, 0.6, 0.8, 1.0, 0, 0.6, 0.8, 1.0, 0, 0.6, 0.8, 1.0, 0, 0.6, 0.8, 1.0,
1.57, 0.8, 0.5, 0.8
]
robot.grasp(pos_cl)
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 4: go up
pos_4 = [0.45, 0, 0.9]
quat_4 = p.getQuaternionFromEuler([m.pi / 2, 1 / 3 * m.pi, -m.pi])
robot.apply_action(pos_4 + list(quat_4), max_vel=5)
robot.grasp(pos_cl)
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 5: go right
pos_5 = [0.3, -0.2, 0.9]
quat_5 = p.getQuaternionFromEuler([0.0, 0.0, m.pi / 2])
robot.apply_action(pos_5 + list(quat_5), max_vel=5)
robot.grasp(pos_cl)
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 6: open hand
robot.pre_grasp()
for _ in range(50):
p.stepSimulation()
time.sleep(sim_timestep)
# ------------------------ #
# --- Play with joints --- #
# ------------------------ #
param_ids = []
joint_ids = []
num_joints = p.getNumJoints(robot.robot_id)
joint_states = p.getJointStates(robot.robot_id, range(0, num_joints))
joint_poses = [x[0] for x in joint_states]
idx = 0
for i in range(num_joints):
joint_info = p.getJointInfo(robot.robot_id, i)
joint_name = joint_info[1]
joint_type = joint_info[2]
if joint_type is p.JOINT_REVOLUTE or joint_type is p.JOINT_PRISMATIC:
param_ids.append(
p.addUserDebugParameter(joint_name.decode("utf-8"),
joint_info[8], joint_info[9],
joint_poses[i]))
joint_ids.append(i)
idx += 1
while True:
new_pos = []
for i in param_ids:
new_pos.append(p.readUserDebugParameter(i))
p.setJointMotorControlArray(robot.robot_id,
joint_ids,
p.POSITION_CONTROL,
targetPositions=new_pos)
p.stepSimulation()
time.sleep(sim_timestep)
import os import random from pybullet_object_models import primitive_objects from pybullet_object_models import random_objects from pybullet_object_models import ycb_objects from pybullet_object_models import superquadric_objects from pybullet_object_models import google_scanned_objects # Get the path to the objects inside each package # data_path = primitive_objects.getDataPath() # data_path = random_objects.getDataPath() data_path = ycb_objects.getDataPath() # data_path = superquadric_objects.getDataPath() # data_path = google_scanned_objects.getDataPath() # With this path you can access to the object files, e.g. its URDF model obj_name = 'banana' # name of the object folder # or a full list of the available objects can be found with # model_list = primitive_objects.getModelList() # model_list = random_objects.getModelList() model_list = ycb_objects.getModelList() # model_list = superquadric_objects.getModelList() # model_list = google_scanned_objects.getModelList() # from which an object can be selected rand_object_name = random.choice(model_list) # the full path is then path_to_urdf = os.path.join(data_path, rand_object_name, "model.urdf")
ax = fig.add_subplot(projection='3d')
ax.scatter([p[0] for p in pcd.points], [p[1] for p in pcd.points], [p[2] for p in pcd.points], s=0.5)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_xlim([-0.5, 0.5])
ax.set_ylim([-0.5, 0.5])
ax.set_zlim([-0.5, 0.5])
plt.show()
if __name__=="__main__":
p.connect(p.GUI)
pybullet_data.getDataPath()
# Load URDF (articulated -> more than 1 joint)
objId = p.loadURDF(os.path.join(ycb_objects.getDataPath(), 'YcbMustardBottle', "model.urdf"),
basePosition=[0, 0, 0],
baseOrientation=[0, 0, 0, 1],
useFixedBase=1)
# Get instance segmented pointcloud
pcd = generatePointCloud(objId)
# pcd = removeHiddenPoints(pcd)
# Plot pointcloud
visualizePointCloud(pcd)
def get_ycb_objects_list():
obj_list = [dir for dir in os.listdir(ycb_objects.getDataPath()) if not dir.startswith('__') and not dir.endswith('.py')]
return obj_list
