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