def get_current_variable_values_for_arena(self): """ :return: (dict) returns all the exposed variables and their values in the environment's stage except for objects. """ if self._pybullet_client_w_o_goal_id is not None: client = self._pybullet_client_w_o_goal_id is not None else: client = self._pybullet_client_full_id variable_params = dict() variable_params["floor_color"] = \ pybullet.getVisualShapeData(WorldConstants.FLOOR_ID, physicsClientId=client)[0][7][:3] variable_params["floor_friction"] = \ pybullet.getDynamicsInfo(WorldConstants.FLOOR_ID, -1, physicsClientId=client)[1] variable_params["stage_color"] = \ pybullet.getVisualShapeData(WorldConstants.STAGE_ID, physicsClientId=client)[0][7][:3] variable_params["stage_friction"] = \ pybullet.getDynamicsInfo(WorldConstants.STAGE_ID, -1, physicsClientId=client)[1] variable_params["gravity"] = \ self._current_gravity return variable_params
def _get_obs(self, skip_fixed: bool = False, skip_rigid: bool = False): for oid in self.obj_ids['fixed']: visual_data = p.getVisualShapeData(objectUniqueId=oid) rgba = visual_data[0][-1] if skip_fixed: rgba = (rgba[0], rgba[1], rgba[2], 0) else: rgba = (rgba[0], rgba[1], rgba[2], 1) p.changeVisualShape(objectUniqueId=oid, linkIndex=-1, rgbaColor=rgba) for oid in self.obj_ids['rigid']: visual_data = p.getVisualShapeData(objectUniqueId=oid) rgba = visual_data[0][-1] if skip_rigid: rgba = (rgba[0], rgba[1], rgba[2], 0) else: rgba = (rgba[0], rgba[1], rgba[2], 1) p.changeVisualShape(objectUniqueId=oid, linkIndex=-1, rgbaColor=rgba) # Get RGB-D camera image observations. obs = {'color': (), 'depth': ()} for config in self.agent_cams: color, depth, _ = self.render_camera(config) obs['color'] += (color, ) obs['depth'] += (depth, ) return obs
def update_state(self): image = None if not self.stereo_images: image = self.grab_image( self.view_matrix, self.proj_matrix ) #[self.grab_image(self.view_matrix, self.proj_matrix)] else: image_l = self.grab_image(self.view_matrix_left, self.proj_matrix_left) image_r = self.grab_image(self.view_matrix_right, self.proj_matrix_right) image = [image_l, image_r] item = Item([], [], []) goal = Item([], [], []) item_pose = p.getBasePositionAndOrientation(self.itemId) item_dim = p.getVisualShapeData(self.itemId)[0][3] goal_pose = p.getBasePositionAndOrientation(self.goalId) goal_dim = p.getVisualShapeData(self.goalId)[0][3] item.set_pos_and_ori_from_pose(item_pose) item.set_dim(item_dim) goal.set_pos_and_ori_from_pose(goal_pose) goal.set_dim(goal_dim) self.state = State(image, item, goal)
def info(self): """Normally returns dict of: object id : (position, rotation, dimensions) However, I made a few changes. First, removing IDs in some tasks, so we shouldn't query their position. Second, adding object mesh for gt_state and soft bodies. The second `if` test is redundant but just in case. Here, we instead map to the mesh data instead of (position, rotation, dimension). This is: (nb_vertices, (positions)) where the latter is a tuple that has all the 3D positions of each vertex in the simulation mesh, e.g., it's 100 for cloth. Note on soft body IDs: cloth-cover: ID 5 is cloth (ID 4 is item to cover) cloth-flat(notarget): ID 5 is cloth (ID 4 is the zone, though in the no target case we remove it ... historical reasons). bag tasks: all have ID 5 as the bag, because they use ID 4 for the zone, (with bag-alone removing zone) and other items are added afterwards. To see how we use the special case for soft bodies, see: ravens/agents/gt_state.py and the extraact_x_y_theta method. We depend on assuming that len(info[id]) = 2 instead of 3. """ removed_IDs = [] if (isinstance(self.task, tasks.names['cloth-flat-notarget']) or isinstance(self.task, tasks.names['bag-alone-open'])): removed_IDs.append(self.task.zone_ID) # Daniel: special case for soft bodies (and gt_state). For now only cloth. softbody_id = -1 if self.is_cloth_env(): assert len(self.task.def_IDs) == 1, self.task.def_IDs softbody_id = self.task.def_IDs[0] # object id : (position, rotation, dimensions) info = {} for object_id in (self.fixed_objects + self.objects): if object_id in removed_IDs: continue # Daniel: special cases for soft bodies (and gt_state), and then bags. if (object_id == softbody_id) and self.is_cloth_env(): info[object_id] = p.getMeshData(object_id, -1, flags=p.MESH_DATA_SIMULATION_MESH) elif isinstance(self.task, tasks.names['bag-color-goal']): # Note: we don't do this for sorting? :X position, rotation = p.getBasePositionAndOrientation(object_id) dimensions = p.getVisualShapeData(object_id)[0][3] rgba_color = p.getVisualShapeData(object_id)[0][7] # see pybullet docs assert rgba_color[3] == 1, rgba_color rgb_color = rgba_color[0:3] # we can ignore the last component it is always 1 info[object_id] = (position, rotation, dimensions, rgb_color) else: # The usual case. position, rotation = p.getBasePositionAndOrientation(object_id) dimensions = p.getVisualShapeData(object_id)[0][3] info[object_id] = (position, rotation, dimensions) return info
def test_color(self): black_color = (0, 0, 0, 1) white_color = (1, 1, 1, 1) RobotSphereClassTest.robot.setColor(black_color) shape_data =\ p.getVisualShapeData(RobotSphereClassTest.robot.getRobotModel(), physicsClientId=RobotSphereClassTest.client) self.assertTrue(black_color == shape_data[0][-1]) RobotSphereClassTest.robot.setColor(white_color) shape_data =\ p.getVisualShapeData(RobotSphereClassTest.robot.getRobotModel(), physicsClientId=RobotSphereClassTest.client) self.assertTrue(white_color == shape_data[0][-1])
def reset(self): p.resetSimulation() self.step_counter = 0 #载入yumi self.yumiUid = p.loadURDF( os.path.join(os.path.dirname(os.path.realpath(__file__)), "assets/yumi_with_hands.urdf"), useFixedBase=True, flags=p.URDF_USE_SELF_COLLISION + p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) #载入地面 self.planeUid = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), basePosition=[0, 0, -0.65]) #载入桌子 self.tableUid = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table/table.urdf"), basePosition=[0.5, 0, -0.65]) p.setGravity(0, 0, -10) p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(1. / 240.) self.joint2Index = {} # jointIndex map to jointName for i in range(p.getNumJoints(self.yumiUid)): self.joint2Index[p.getJointInfo(self.yumiUid, i)[1].decode('utf-8')] = i self.jointColor = {} # jointName map to jointColor for data in p.getVisualShapeData(self.yumiUid): self.jointColor[p.getJointInfo( self.yumiUid, data[1])[1].decode('utf-8')] = data[7] # recover color for joint, index in self.joint2Index.items(): if joint in self.jointColor and joint != 'world_joint': p.changeVisualShape(self.yumiUid, index, rgbaColor=self.jointColor[joint])
def reset(self, env): self.total_rewards = 0 # Add goal post. line_urdf = 'assets/line/line.urdf' self.zone_size = (0.5, 1, 0.2) self.zone_pose = ((0.5, -0.86, 0), p.getQuaternionFromEuler((0, 0, 0))) env.add_object(line_urdf, self.zone_pose, fixed=True) # Add box. box_size = self.random_size(0.05, 0.25, 0.05, 0.15, 0.05, 0.05) box_template = 'assets/box/box-template.urdf' box_urdf = self.fill_template(box_template, {'DIM': box_size}) px = self.bounds[0, 0] + 0.1 + np.random.rand() * 0.3 position = (px, 0.3, box_size[2] / 2) theta = np.random.rand() * np.pi / 4 - np.pi / 8 rotation = p.getQuaternionFromEuler((0, 0, theta)) box_pose = (position, rotation) box_id = env.add_object(box_urdf, box_pose) os.remove(box_urdf) self.color_random_brown(box_id) self.object_points = {box_id: self.get_object_points(box_id)} # Move end effector to start position next to box. box_dim = p.getVisualShapeData(box_id)[0][3] start_position = np.array([0, box_dim[1] / 2 + 0.01, 0]) start_position = self.apply(box_pose, start_position) rotation = tuple(env.home_pose[3:]) joints = env.solve_IK((tuple(start_position) + rotation)) for i in range(len(env.joints)): p.resetJointState(env.ur5, env.joints[i], joints[i])
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId): dyn = p.getDynamicsInfo(bodyUid, linkIndex, physicsClientId=physicsClientId) urdfLink.urdf_inertial.mass = dyn[0] urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2] # todo urdfLink.urdf_inertial.origin_xyz = dyn[3] urdfLink.urdf_inertial.origin_rpy = p.getEulerFromQuaternion(dyn[4]) visualShapes = p.getVisualShapeData(bodyUid, physicsClientId=physicsClientId) matIndex = 0 for v in visualShapes: if (v[1] == linkIndex): urdfVisual = UrdfVisual() urdfVisual.geom_type = v[2] if (v[2] == p.GEOM_BOX): urdfVisual.geom_extents = v[3] if (v[2] == p.GEOM_SPHERE): urdfVisual.geom_radius = v[3][0] if (v[2] == p.GEOM_MESH): urdfVisual.geom_meshfilename = v[4].decode("utf-8") if urdfVisual.geom_meshfilename == UNKNOWN_FILE: continue urdfVisual.geom_meshscale = v[3] if (v[2] == p.GEOM_CYLINDER): urdfVisual.geom_length = v[3][0] urdfVisual.geom_radius = v[3][1] if (v[2] == p.GEOM_CAPSULE): urdfVisual.geom_length = v[3][0] urdfVisual.geom_radius = v[3][1] urdfVisual.origin_xyz = v[5] urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6]) urdfVisual.material_rgba = v[7] name = 'mat_{}_{}'.format(linkIndex, matIndex) urdfVisual.material_name = name urdfLink.urdf_visual_shapes.append(urdfVisual) matIndex = matIndex + 1 collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex, physicsClientId=physicsClientId) for v in collisionShapes: urdfCollision = UrdfCollision() urdfCollision.geom_type = v[2] if (v[2] == p.GEOM_BOX): urdfCollision.geom_extents = v[3] if (v[2] == p.GEOM_SPHERE): urdfCollision.geom_radius = v[3][0] if (v[2] == p.GEOM_MESH): urdfCollision.geom_meshfilename = v[4].decode("utf-8") if urdfCollision.geom_meshfilename == UNKNOWN_FILE: continue urdfCollision.geom_meshscale = v[3] if (v[2] == p.GEOM_CYLINDER): urdfCollision.geom_length = v[3][0] urdfCollision.geom_radius = v[3][1] if (v[2] == p.GEOM_CAPSULE): urdfCollision.geom_length = v[3][0] urdfCollision.geom_radius = v[3][1] pos, orn = p.multiplyTransforms(dyn[3], dyn[4], v[5], v[6]) urdfCollision.origin_xyz = pos urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn) urdfLink.urdf_collision_shapes.append(urdfCollision)
def get_current_variable_values(self): """ :return: (dict) returns all the exposed variables in the environment along with their corresponding values. """ # TODO: not a complete list yet of what we want to expose variable_params = dict() variable_params['joint_positions'] = self._latest_full_state[ 'positions'] variable_params['control_index'] = self._control_index variable_params['joint_velocities'] = self._latest_full_state[ 'velocities'] if self._pybullet_client_w_o_goal_id is not None: client = self._pybullet_client_w_o_goal_id else: client = self._pybullet_client_full_id position, _ = pybullet. \ getBasePositionAndOrientation(WorldConstants.ROBOT_ID, physicsClientId= client) variable_params[ 'robot_height'] = position[-1] + WorldConstants.ROBOT_HEIGHT for robot_finger_link in WorldConstants.LINK_IDS: variable_params[robot_finger_link] = dict() variable_params[robot_finger_link]['color'] = \ pybullet.getVisualShapeData(WorldConstants.ROBOT_ID, physicsClientId=client)\ [WorldConstants.VISUAL_SHAPE_IDS[robot_finger_link]][7][:3] variable_params[robot_finger_link]['mass'] = \ pybullet.getDynamicsInfo(WorldConstants.ROBOT_ID, WorldConstants.LINK_IDS[robot_finger_link], physicsClientId=client)[0] return variable_params
def test_texture(self): p.setAdditionalSearchPath( os.path.dirname(os.path.realpath(__file__)), physicsClientId=RobotSphereClassTest.client) texture_path_name = "This is not a texture" try: RobotSphereClassTest.robot.setTexture(texture_path_name) except Exception as error: print("Normal error", error) self.assertTrue( RobotSphereClassTest.robot.getTextureId() is None) self.assertTrue( RobotSphereClassTest.robot.getTexturePathName() !=\ texture_path_name) texture_path_name = "data/texture.png" RobotSphereClassTest.robot.setTexture(texture_path_name) time.sleep(1) shape_data =\ p.getVisualShapeData(RobotSphereClassTest.robot.getRobotModel(), flags=p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS, physicsClientId=RobotSphereClassTest.client) # Texture id is not updating in pybullet code, to check # self.assertTrue(shape_data[0][-1] != -1) self.assertTrue( RobotSphereClassTest.robot.getTexturePathName() ==\ texture_path_name)
def import_scene(self, scene, texture_scale=1.0, load_texture=True, class_id=None): """ Import a scene into the simulator. A scene could be a synthetic one or a realistic Gibson Environment. :param scene: Scene object :param texture_scale: Option to scale down the texture for rendering :param load_texture: If you don't need rgb output, texture loading could be skipped to make rendering faster :param class_id: Class id for rendering semantic segmentation """ if class_id is None: class_id = self.next_class_id self.next_class_id += 1 new_objects = scene.load() for item in new_objects: self.objects.append(item) for new_object in new_objects: for shape in p.getVisualShapeData(new_object): id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[: 8] visual_object = None if type == p.GEOM_MESH: filename = filename.decode('utf-8') if filename not in self.visual_objects.keys(): self.renderer.load_object(filename, texture_scale=texture_scale, load_texture=load_texture) self.visual_objects[filename] = len( self.renderer.visual_objects) - 1 visual_object = self.visual_objects[filename] elif type == p.GEOM_PLANE: pass # By default, we add an additional floor surface to "smooth out" that of the original mesh. # Normally you don't need to render this additionally added floor surface. # However, if you do want to render it for some reason, you can uncomment the block below. # filename = os.path.join(gibson2.assets_path, 'models/mjcf_primitives/cube.obj') # self.renderer.load_object(filename, # transform_orn=rel_orn, # transform_pos=rel_pos, # input_kd=color[:3], # scale=[100, 100, 0.01]) # visual_object = len(self.renderer.visual_objects) - 1 if visual_object is not None: self.renderer.add_instance(visual_object, pybullet_uuid=new_object, class_id=class_id) if scene.is_interactive: for obj in scene.scene_objects: self.import_articulated_object(obj) self.scene = scene return new_objects
def _reset(self): self.terminated = False 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._kuka = kukakr6(self._urdfRoot, timeStep=self._timeStep, basePosition=[0, 0, 0.75], useInverseKinematics=self._useIK, action_space=self.action_dim, includeVelObs=self.includeVelObs, init_joints=self.init_joints) # 桌子 tableId = p.loadURDF(os.path.join(self._urdfRoot, "kuka_kr6_support/table.urdf"), useFixedBase=True) # tableId = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "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._kuka.workspace_lim[2][0] = self._h_table p.setGravity(0, 0, -9.8)
def _get_data(self, index): if self._data is None: body_data = pb.getVisualShapeData(self._body_id, physicsClientId=self.client_id) link_data = next( (i for i in body_data if i[1] == self._link_index)) self._data = link_data return self._data[index]
def bullet2pyqtgraph(Id): shape_data = p.getVisualShapeData(Id) mesh_items = [] for i in range(len(shape_data)): # process item i mesh = visualGeometryType2mesh(shape_data[i]) mesh_items.append(mesh) return mesh_items
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId): dyn = p.getDynamicsInfo(bodyUid,linkIndex,physicsClientId=physicsClientId) urdfLink.urdf_inertial.mass = dyn[0] urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2] #todo urdfLink.urdf_inertial.origin_xyz = dyn[3] rpy = p.getEulerFromQuaternion(dyn[4]) urdfLink.urdf_inertial.origin_rpy = rpy visualShapes = p.getVisualShapeData(bodyUid,physicsClientId=physicsClientId) matIndex = 0 for v in visualShapes: if (v[1]==linkIndex): urdfVisual = UrdfVisual() urdfVisual.geom_type = v[2] if (v[2]==p.GEOM_BOX): urdfVisual.geom_extents = v[3] if (v[2]==p.GEOM_SPHERE): urdfVisual.geom_radius = v[3][0] if (v[2]==p.GEOM_MESH): urdfVisual.geom_meshfilename = v[4].decode("utf-8") urdfVisual.geom_meshscale = v[3] if (v[2]==p.GEOM_CYLINDER): urdfVisual.geom_length=v[3][0] urdfVisual.geom_radius=v[3][1] if (v[2]==p.GEOM_CAPSULE): urdfVisual.geom_length=v[3][0] urdfVisual.geom_radius=v[3][1] urdfVisual.origin_xyz = v[5] urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6]) urdfVisual.material_rgba = v[7] name = 'mat_{}_{}'.format(linkIndex,matIndex) urdfVisual.material_name = name urdfLink.urdf_visual_shapes.append(urdfVisual) matIndex=matIndex+1 collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId) for v in collisionShapes: urdfCollision = UrdfCollision() urdfCollision.geom_type = v[2] if (v[2]==p.GEOM_BOX): urdfCollision.geom_extents = v[3] if (v[2]==p.GEOM_SPHERE): urdfCollision.geom_radius = v[3][0] if (v[2]==p.GEOM_MESH): urdfCollision.geom_meshfilename = v[4].decode("utf-8") urdfCollision.geom_meshscale = v[3] if (v[2]==p.GEOM_CYLINDER): urdfCollision.geom_length=v[3][0] urdfCollision.geom_radius=v[3][1] if (v[2]==p.GEOM_CAPSULE): urdfCollision.geom_length=v[3][0] urdfCollision.geom_radius=v[3][1] pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\ v[5], v[6]) urdfCollision.origin_xyz = pos urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn) urdfLink.urdf_collision_shapes.append(urdfCollision)
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink): dyn = p.getDynamicsInfo(bodyUid, linkIndex) urdfLink.urdf_inertial.mass = dyn[0] urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2] #todo urdfLink.urdf_inertial.origin_xyz = dyn[3] rpy = p.getEulerFromQuaternion(dyn[4]) urdfLink.urdf_inertial.origin_rpy = rpy visualShapes = p.getVisualShapeData(bodyUid) matIndex = 0 for v in visualShapes: if (v[1] == linkIndex): print("visualShape base:", v) urdfVisual = UrdfVisual() urdfVisual.geom_type = v[2] if (v[2] == p.GEOM_BOX): urdfVisual.geom_extents = v[3] if (v[2] == p.GEOM_SPHERE): urdfVisual.geom_radius = v[3][0] if (v[2] == p.GEOM_MESH): urdfVisual.geom_meshfile = v[4].decode("utf-8") if (v[2] == p.GEOM_CYLINDER): urdfVisual.geom_radius = v[3][1] urdfVisual.geom_length = v[3][0] urdfVisual.origin_xyz = v[5] urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6]) urdfVisual.material_rgba = v[7] name = 'mat_{}_{}'.format(linkIndex, matIndex) urdfVisual.material_name = name urdfLink.urdf_visual_shapes.append(urdfVisual) matIndex = matIndex + 1 collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex) for v in collisionShapes: print("collisionShape base:", v) urdfCollision = UrdfCollision() print("geom type=", v[0]) urdfCollision.geom_type = v[2] if (v[2] == p.GEOM_BOX): urdfCollision.geom_extents = v[3] if (v[2] == p.GEOM_SPHERE): urdfCollision.geom_radius = v[3][0] if (v[2] == p.GEOM_MESH): urdfCollision.geom_meshfile = v[4].decode("utf-8") #localInertiaFrame*childTrans if (v[2] == p.GEOM_CYLINDER): urdfCollision.geom_radius = v[3][1] urdfCollision.geom_length = v[3][0] pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\ v[5], v[6]) urdfCollision.origin_xyz = pos urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn) urdfLink.urdf_collision_shapes.append(urdfCollision)
def get_object_points(self, obj): obj_shape = p.getVisualShapeData(obj) obj_dim = obj_shape[0][3] xv, yv, zv = np.meshgrid( np.arange(-obj_dim[0] / 2, obj_dim[0] / 2, 0.02), np.arange(-obj_dim[1] / 2, obj_dim[1] / 2, 0.02), np.arange(-obj_dim[2] / 2, obj_dim[2] / 2, 0.02), sparse=False, indexing='xy') return np.vstack((xv.reshape(1, -1), yv.reshape(1, -1), zv.reshape(1, -1)))
def _setup_gripper(self): # add kuka arm # self.armId = p.loadSDF("kuka_iiwa/kuka_with_gripper2.sdf")[0] # self.armId = p.loadURDF("franka_panda/panda.urdf", useFixedBase=True) self.armId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True) p.resetBasePositionAndOrientation(self.armId, [0, 0, 0], [0, 0, 0, 1]) # TODO modify dynamics to induce traps # for j in range(p.getNumJoints(self.armId)): # p.changeDynamics(self.armId, j, linearDamping=0, angularDamping=0) # orientation of the end effector self.endEffectorOrientation = p.getQuaternionFromEuler( [0, math.pi / 2, 0]) self.endEffectorIndex = kukaEndEffectorIndex self.numJoints = p.getNumJoints(self.armId) # get the joint ids # TODO try out arm with attached grippers # self.armInds = [i for i in range(pandaNumDofs)] self.armInds = [i for i in range(self.numJoints)] # create a constraint to keep the fingers centered # c = p.createConstraint(self.armId, # 9, # self.armId, # 10, # jointType=p.JOINT_GEAR, # jointAxis=[1, 0, 0], # parentFramePosition=[0, 0, 0], # childFramePosition=[0, 0, 0]) # p.changeConstraint(c, gearRatio=-1, erp=0.1, maxForce=50) # joint damping coefficents # self.jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # self.jd = [ # 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, # 0.00001, 0.00001, 0.00001, 0.00001 # ] p.enableJointForceTorqueSensor(self.armId, self.endEffectorIndex) self._calculate_init_joints() for i in self.armInds: p.resetJointState(self.armId, i, self.initJoints[i]) # self._open_gripper() # self._close_gripper() # make arm translucent visual_data = p.getVisualShapeData(self.armId) for link in visual_data: link_id = link[1] if link_id == -1: continue rgba = list(link[7]) rgba[3] = 0.4 p.changeVisualShape(self.armId, link_id, rgbaColor=rgba)
def import_scene(self, scene, texture_scale=1.0, load_texture=True, class_id=0): """ Import a scene. A scene could be a synthetic one or a realistic Gibson Environment. :param scene: Scene object :param texture_scale: Option to scale down the texture for rendering :param load_texture: If you don't need rgb output, texture loading could be skipped to make rendering faster :param class_id: The class_id for background for rendering semantics. """ new_objects = scene.load() for item in new_objects: self.objects.append(item) for new_object in new_objects: for shape in p.getVisualShapeData(new_object): id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[: 8] if type == p.GEOM_MESH: filename = filename.decode('utf-8') if not filename in self.visual_objects.keys(): self.renderer.load_object(filename, texture_scale=texture_scale, load_texture=load_texture) self.visual_objects[filename] = len( self.renderer.visual_objects) - 1 self.renderer.add_instance( len(self.renderer.visual_objects) - 1, pybullet_uuid=new_object, class_id=class_id) else: self.renderer.add_instance( self.visual_objects[filename], pybullet_uuid=new_object, class_id=class_id) elif type == p.GEOM_PLANE: pass #don't load plane, it will cause z fighting #filename = os.path.join(gibson2.assets_path, 'models/mjcf_primitives/cube.obj') # self.renderer.load_object(filename, # transform_orn=rel_orn, # transform_pos=rel_pos, # input_kd=color[:3], # scale=[100, 100, 0.01]) # self.renderer.add_instance(len(self.renderer.visual_objects) - 1, # pybullet_uuid=new_object, # class_id=class_id, # dynamic=True) self.scene = scene return new_objects
def reset(self): self.terminated = False 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 self.obj_pose, self.target_pose = self._sample_pose() if (self.fixedPositionObj): #we use a fixed starting position for the cube self._objID = p.loadURDF(os.path.join( self._urdfRoot, "franka_description/cube_small.urdf"), basePosition=[0.7, 0.0, 0.64]) else: 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"), self.target_pose) self._debugGUI() p.setGravity(0, 0, -9.8) # Let the world run for a bit for _ in range(10): p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation)
def get_shape(self, body, flags=0): """ Get body's shape :param body: int body's ID :param flags: int Texture unique id :return: tuple Body shape information """ return p.getVisualShapeData(body, flags, physicsClientId=self.client_id)
def print_log(object_id, force): shape_data = p.getVisualShapeData(object_id)[0] dynamics_data = p.getDynamicsInfo(object_id, -1) print('***********************************') print('Object: %s' % shape_data[4]) print('Dimensions: (%f, %f, %f)' % shape_data[3]) print('Mass: %f' % dynamics_data[0]) print('Lateral Friction: %f' % dynamics_data[1]) print('Inertia Diagonal: (%f, %f, %f)' % dynamics_data[2]) print('Force: (%f, %f, %f)' % tuple(force)) print('***********************************')
def get_entity(self, id): """ Fetch an entity in the simulator and perform a lazzy update of the corresponding scene node """ if id not in self.nodes_map: raise ValueError("Invalid id provided : '{}'".format(id)) scene_node = self.nodes_map[id] sim_id = self.entity_id_map[id] visual_shapes = p.getVisualShapeData(sim_id) for idx, shape in enumerate(visual_shapes): primitive_shape = scene_node.shapes[idx] link_id = shape[1] position = shape[5] orientation = shape[6] if link_id != -1: link_state = p.getLinkState(sim_id, link_id) t_link = link_state[0] q_link = link_state[1] t_inertial = link_state[2] q_inertial = link_state[3] tf_world_link = np.dot(translation_matrix(t_link), quaternion_matrix(q_link)) tf_inertial_link = np.dot(translation_matrix(t_inertial), quaternion_matrix(q_inertial)) world_transform = np.dot(tf_world_link, np.linalg.inv(tf_inertial_link)) else: t_link, q_link = p.getBasePositionAndOrientation(sim_id) world_transform = np.dot(translation_matrix(t_link), quaternion_matrix(q_link)) if link_id != -1: shape_transform = np.dot(translation_matrix(position), quaternion_matrix(orientation)) shape_transform = np.dot(world_transform, shape_transform) shape_transform = np.linalg.inv(np.dot(np.linalg.inv(shape_transform), scene_node.pose.transform())) position = translation_from_matrix(shape_transform) orientation = quaternion_from_matrix(shape_transform) primitive_shape.pose.pos.x = position[0] primitive_shape.pose.pos.y = position[1] primitive_shape.pose.pos.z = position[2] primitive_shape.pose.from_quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) else: shape_transform = np.dot(translation_matrix(position), quaternion_matrix(orientation)) shape_transform = np.dot(world_transform, shape_transform) position = translation_from_matrix(shape_transform) orientation = quaternion_from_matrix(shape_transform) scene_node.pose.pos.x = position[0] scene_node.pose.pos.y = position[1] scene_node.pose.pos.z = position[2] scene_node.pose.from_quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) return self.nodes_map[id]
def _get_object_state(self, id): pos, orn = pb.getBasePositionAndOrientation(id) size = pb.getVisualShapeData(id)[0][3] orn = pb.getEulerFromQuaternion(orn) lv, av = pb.getBaseVelocity(id) return { "position": np.array(pos), "size": np.array(size), "orientation": np.array(orn), "lin_vel": np.array(lv), "ang_vel": np.array(av) }
def test_scale_dimension(self): robot_pose_init = RobotSphereClassTest.robot.getPosition() init_dimension = RobotSphereClassTest.robot.getDimension() RobotSphereClassTest.robot.setScale(2) self.assertTrue(RobotSphereClassTest.robot.getScale() == 2) robot_pose_actual = RobotSphereClassTest.robot.getPosition() self.assertTrue(robot_pose_actual == robot_pose_init) shape_data =\ p.getVisualShapeData(RobotSphereClassTest.robot.getRobotModel(), physicsClientId=RobotSphereClassTest.client) actual_dimensions = shape_data[0][3] self.assertTrue( init_dimension*RobotSphereClassTest.robot.getScale() ==\ actual_dimensions[0])
def reset(self): self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) self._envStepCounter = 0 p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), [0, 0, 0]) # Load robot self._icub = iCubEnv(urdfRootPath=self._urdfRoot, timeStep=self._timeStep, useInverseKinematics=self._useIK, arm=self._control_arm, useOrientation=self._useOrientation) ## Load table and object for simulation self._tableId = p.loadURDF( os.path.join(pybullet_data.getDataPath(), "table/table.urdf"), [0.85, 0.0, 0.0]) #limit iCub workspace to table plane table_info = p.getVisualShapeData(self._tableId, -1)[0] self._h_table = table_info[5][2] + table_info[3][2] self._icub.workspace_lim[2][0] = self._h_table # Randomize start position of object obj_pose, self._tg_pose = self._sample_pose() self._objID = p.loadURDF( os.path.join(pybullet_data.getDataPath(), "lego/lego.urdf"), obj_pose) self._debugGUI() p.setGravity(0, 0, -9.8) # Let the world run for a bit for _ in range(10): p.stepSimulation() self._observation = self.getExtendedObservation() self._init_dist_hand_obj = goal_distance( np.array(self._observation[0:3]), np.array(obj_pose)) self._max_dist_obj_tg = goal_distance(np.array(obj_pose), np.array(self._tg_pose)) return np.array(self._observation)
def _compute_workspace(self, table_id): table_shape = p.getVisualShapeData(table_id, -1) table_top_shape = table_shape[0] table_height = table_top_shape[5][2] + table_top_shape[3][2] / 2 table_leg_pos = table_shape[1][5] # Rotation of the vector is needed # getVisualShapeData returns position of local visual frame, relative to link/joint frame table_leg_pos = p.rotateVector(self._table_orn, table_leg_pos) max_workspace_x = abs(table_leg_pos[0]) max_workspace_y = abs(table_leg_pos[1]) return [ [-max_workspace_x + 0.3, max_workspace_x - 0.05], # X [-max_workspace_y + 0.3, max_workspace_y - 0.3], # Y [table_height, 1] # Z ]
def storeVisualShapes(self, multiBodyId, pos, orn): visualShapes = p.getVisualShapeData(multiBodyId) for visualShape in visualShapes: linkIndex = visualShape[1] visualType = visualShape[2] meshPath = visualShape[4] pos = TranslationMatrix(visualShape[5]) orn = getOrnMatrixFromQuaternion(visualShape[6]) if (visualType == p.GEOM_MESH): meshPath = meshPath.decode("utf-8") wavefrontData = pywavefront.Wavefront(meshPath) wavefrontVisualizer = WavefrontVisualiser( wavefrontData, pos, orn) self.visualShapes[multiBodyId][linkIndex] = VisualShape( linkIndex, wavefrontVisualizer, pos, orn)
def info(self): """Environment info variable with object poses, dimensions, and colors.""" # Some tasks create and remove zones, so ignore those IDs. # removed_ids = [] # if (isinstance(self.task, tasks.names['cloth-flat-notarget']) or # isinstance(self.task, tasks.names['bag-alone-open'])): # removed_ids.append(self.task.zone_id) info = {} # object id : (position, rotation, dimensions) for obj_ids in self.obj_ids.values(): for obj_id in obj_ids: pos, rot = p.getBasePositionAndOrientation(obj_id) dim = p.getVisualShapeData(obj_id)[0][3] info[obj_id] = (pos, rot, dim) return info
def info(self): """Environment info dictionary.""" # Some tasks create and remove zones, so ignore those IDs. removed_ids = [] if (isinstance(self.task, tasks.names['cloth-flat-notarget']) or isinstance(self.task, tasks.names['bag-alone-open'])): removed_ids.append(self.task.zone_id) info = {} # object id : (position, rotation, dimensions) for object_id in self.fixed_objects + self.objects: if object_id in removed_ids: continue position, rotation = p.getBasePositionAndOrientation(object_id) dimensions = p.getVisualShapeData(object_id)[0][3] info[object_id] = (position, rotation, dimensions) return info
def get_world_visual_pose(body_unique_id: int, link_index: int): """Pose of visual frame with respect to world frame expressed in world frame. Args: body_unique_id (int): The body unique id, as returned by loadURDF, etc. link_index (int): Link index or -1 for the base. Returns: pos_orn (tuple): See description. """ world_link_pose = get_world_link_pose(body_unique_id, link_index) visual_shape_data = p.getVisualShapeData(body_unique_id, link_index) local_visual_pose = (visual_shape_data[link_index + 1][5], visual_shape_data[link_index + 1][6]) return p.multiplyTransforms(world_link_pose[0], world_link_pose[1], local_visual_pose[0], local_visual_pose[1])
import pybullet as p p.connect(p.GUI) plane = p.loadURDF("plane.urdf") visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS) print(visualData) curTexUid = visualData[0][8] print(curTexUid) texUid = p.loadTexture("tex256.png") print("texUid=", texUid) p.changeVisualShape(plane, -1, textureUniqueId=texUid) for i in range(100): p.getCameraImage(320, 200) p.changeVisualShape(plane, -1, textureUniqueId=curTexUid) for i in range(100): p.getCameraImage(320, 200)