def __init__(self, body_id, link_index, client_id): self._body_id = body_id self._link_index = link_index self.client_id = client_id self._data = pb.getCollisionShapeData(body_id, link_index, physicsClientId=client_id)
def reset(self): # pybullet data path -> /home_directory/.local/lib/python3.8/site-packages/pybullet_data pybullet_data_path = pybullet_data.getDataPath() #print("=> data path -> {0}".format(pybullet_data_path)) # Add a search data path p.setAdditionalSearchPath(pybullet_data_path) # pybullet data path -> /home_directory/.local/lib/python3.8/site-packages/pybullet_data p.loadURDF("plane.urdf", [0, 0, 0], physicsClientId=self._physics_client_id) # Load table and object self.table_id = p.loadURDF("table/table.urdf", basePosition=[0.85, 0.0, 0.0], useFixedBase=True, physicsClientId=self._physics_client_id) table_info = p.getCollisionShapeData( self.table_id, -1, physicsClientId=self._physics_client_id)[0] # p.getCollisionShapeData return a list of collision shape, in our case there is only one shape so [0] # table_info[5] (x,y,z) -> position of the collision shape (box) into table frame # table_info[3] (x,y,z) -> size of collision shape (box) # voir doc/table_collision.pdf self._h_table = table_info[5][2] + table_info[3][2] / 2 # set ws limit on z according to table height #print('world limit before table = ', self._ws_lim) self._ws_lim[2][:] = [self._h_table, self._h_table + 0.3] #print('world limit = ', self._ws_lim) self.load_object(self._obj_name)
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 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 generatePointCloud(uId) -> o3d.geometry.PointCloud: ''' Generate Open3D pointcloud from pyBullet URDF ID. The poincloud has its origin at the base of ''' # 1. Collect all shape data of parts along links in URDF # Link index description # -1: base # 0: link after first joint # # partsInfo = [ part1_info , part2_info , ... ] # Check pybulletQuickStartGuide for the return type of p.getCollisionShapeData() partsInfo = [] for linkId in range(-1, p.getNumJoints(uId)): linkInfo = p.getCollisionShapeData(uId, linkId) for part in linkInfo: partsInfo.append(part) # 2. Calcuated total volume of the URDF. The volume is later used to decide the number of points cloud. TotalVolume = 0 for i in partsInfo: mesh = o3d.io.read_triangle_mesh(i[4].decode('UTF-8')) # i[4] -> mesh file path. aabb = mesh.get_axis_aligned_bounding_box() TotalVolume += np.prod(aabb.max_bound - aabb.min_bound) # 4. Generating pointcloud pc_xyz = np.empty((0, 3), dtype=float) for i in partsInfo: # Read mesh file of URDF. mesh = o3d.io.read_triangle_mesh(i[4].decode('UTF-8')) # If mesh has more than 8 triangles if np.asarray(mesh.triangles).shape[0] >= 1: # 4-a. Calculate local volume (part volume) aabb = mesh.get_axis_aligned_bounding_box() LocalVolume = np.prod(aabb.max_bound - aabb.min_bound) # 4-b. Get point cloud from the part given number of points PointsNum = max(int(5000 * (LocalVolume / TotalVolume)), 1000) pcd_part = mesh.sample_points_poisson_disk(number_of_points=PointsNum, init_factor=2) # pcd = pcd.voxel_down_sample(voxel_size=0.025) # 4-c. Convert pointcloud to numpy array and concatenates all parts pc_new = np.asarray(pcd_part.points) pc_xyz = np.append(pc_xyz, pc_new, axis=0) # Convert numpy pointcloud back to o3d type pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(pc_xyz) return pcd
def get_obj_info(oID): #TODO: what about not mesh objects? obj_data = p.getCollisionShapeData(oID, -1)[0] geometry_type = obj_data[2] #print("geometry type: " + str(geometry_type)) dimensions = obj_data[3] #print("dimensions: "+ str(dimensions)) local_frame_pos = obj_data[5] #print("local frome position: " + str(local_frame_pos)) local_frame_orn = obj_data[6] #print("local frame oren: " + str(local_frame_orn)) diagonal = sqrt(dimensions[0]**2+dimensions[1]**2+dimensions[2]**2) #print("diagonal: ", diagonal) max_radius = diagonal/2 return local_frame_pos, max_radius
def reset(self): p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), [0, 0, 0], physicsClientId=self._physics_client_id) # Load table and object self.table_id = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table/table.urdf"), basePosition=[0.85, 0.0, 0.0], useFixedBase=True, physicsClientId=self._physics_client_id) table_info = p.getCollisionShapeData(self.table_id, -1, physicsClientId=self._physics_client_id)[0] self._h_table = table_info[5][2] + table_info[3][2]/2 # set ws limit on z according to table height self._ws_lim[2][:] = [self._h_table, self._h_table + 0.3] self.load_object(self._obj_name)
def get_obj_info(oID): #TODO: what about not mesh objects? """ get object data to figure out how far away the hand needs to be to make its approach """ obj_data = p.getCollisionShapeData(oID, -1)[0] geometry_type = obj_data[2] #print("geometry type: " + str(geometry_type)) dimensions = obj_data[3] #print("dimensions: "+ str(dimensions)) local_frame_pos = obj_data[5] #print("local frome position: " + str(local_frame_pos)) local_frame_orn = obj_data[6] #print("local frame oren: " + str(local_frame_orn)) diagonal = sqrt(dimensions[0]**2 + dimensions[1]**2 + dimensions[2]**2) #print("diagonal: ", diagonal) max_radius = diagonal / 2 return local_frame_pos, max_radius
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] 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 i in range(len(visualShapes)): v = visualShapes[i] if (v[1] == linkIndex and v[2] == p.GEOM_MESH): urdfVisual = UrdfVisual() urdfVisual.geom_type = v[2] urdfVisual.geom_meshfilename = v[4].decode("utf-8") urdfVisual.geom_meshscale = v[3] 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 i in range(len(collisionShapes)): v = collisionShapes[i] if (v[2] == p.GEOM_MESH): urdfCollision = UrdfCollision() urdfCollision.geom_type = v[2] urdfCollision.geom_meshfilename = urdfLink.urdf_visual_shapes[ i].geom_meshfilename urdfCollision.geom_meshscale = v[3] 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_world_collision_pose(body_unique_id: int, link_index: int): """Pose of collision 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 of ): See description. """ world_inertial_pose = get_world_inertial_pose(body_unique_id, link_index) collision_shape_data = p.getCollisionShapeData(body_unique_id, link_index) if len(collision_shape_data) > 1: raise NotImplementedError local_collision_pose = (collision_shape_data[0][5], collision_shape_data[0][6]) return p.multiplyTransforms(world_inertial_pose[0], world_inertial_pose[1], local_collision_pose[0], local_collision_pose[1])
def InitModel(self): # Map names to id self.numJoints = p.getNumJoints(self.linkage) logging.info("There are %d links in file\n", self.numJoints) vis_data = p.getVisualShapeData(self.linkage) for i in range(self.numJoints): # map joint names to id (same mapping for links) jointInfo = p.getJointInfo(self.linkage, i) self.jointDict[jointInfo[1].decode('UTF-8')] = i # record parent and child ids for each joint [child, parent] # self.linkJoints.append([[jointInfo[16]], [i]]) # ignore the dummy link # if(i > 0) : # self.linkJoints[jointInfo[16]][self.LINK_PARENT_IDX].append(jointInfo[16]) # measure force/torque at the joints p.enableJointForceTorqueSensor(self.linkage, i, enableSensor=True) # Store initial positions and orientations state = p.getLinkState(self.linkage, jointInfo[0]) self.initPosOrn.append([state[4], state[5]]) # debugging coll_data = p.getCollisionShapeData(self.linkage, i) logging.debug("%s %d", jointInfo[1].decode('UTF-8'), i) logging.debug("com frame (posn (global) & orn (global)) %s %s", str(state[0]), str(p.getEulerFromQuaternion(state[1]))) logging.debug( "inertial offset (posn (link frame), orn (link frame)) %s %s", str(state[2]), str(p.getEulerFromQuaternion(state[3]))) logging.debug("link frame (posn (global), orn (global)) %s %s", str(state[4]), str(p.getEulerFromQuaternion(state[5]))) logging.debug("type %s", str(jointInfo[2])) logging.debug("axis %s", str(jointInfo[13])) logging.debug("collision (posn (COM frame), orn(COM frame)) %s %s", str(coll_data[0][5]), str(p.getEulerFromQuaternion(coll_data[0][6]))) logging.debug( "visual (posn (link frame), orn(link frame)) %s %s\n\n", str(vis_data[i][5]), str(p.getEulerFromQuaternion(vis_data[i][6]))) # Create list of actuated joints self.actJointIds = [ self.jointDict[act_name] for act_name in self.actJointNames ] self.actJointNum = len(self.actJointNames) # Create list of spherical joints self.spherJointIds = [ self.jointDict[spher_name] for spher_name in self.spherJointNames ] self.spherJointNum = len(self.spherJointNames) # Create list of dumb joints self.dumbJointIds = [ self.jointDict[dumb_name] for dumb_name in self.dumbJointNames ] self.dumbJointNum = len(self.dumbJointNames) # Init struct self.linkVelo = [[0, 0, 0], [0, 0, 0]] * p.getNumJoints(self.linkage) # Enable collision by default self.ToggleCollision(1) # Go to home position self.HomePos() # Constraints for i in range(self.numConstraints): parent_id = self.jointDict[self.constraintLinks[i][ self.CONS_PARENT_IDX]] child_id = self.jointDict[self.constraintLinks[i][ self.CONS_CHILD_IDX]] self.constraintDict[self.constraintLinks[i][ self.CONS_NAME_IDX]] = p.createConstraint( self.linkage, parent_id, self.linkage, child_id, self.constraintType, self.constraintAxis, self.constraintParentPos[i], self.constraintChildPos) p.changeConstraint(self.constraintDict[self.constraintLinks[i][ self.CONS_NAME_IDX]], maxForce=self.CONSTRAINT_MAX_FORCE) # # Add to linkJoint list (make constraints -ve ids) # self.linkJoints[parent_id][self.LINK_PARENT_IDX].append(parent_id*-1) # self.linkJoints[child_id][self.LINK_CHILD_IDX].append(child_id*-1) # Set joint control self.SetActJointControlType(self.POSITION_CONTROL) self.ResetActJointControl() self.ControlDumbJoints() self.ControlSpherJoints() # Gravity p.setGravity(0, 0, self.GRAV_ACCELZ) # Simulation type self.SetSimType(self.useRealTime)
def get_object_shape_info(self): info = list(p.getCollisionShapeData(self.obj_id, -1, physicsClientId=self._physics_client_id)[0]) info[4] = p.getVisualShapeData(self.obj_id, -1, physicsClientId=self._physics_client_id)[0][4] return info
'/point_sample/pc_save.npy') if NPYisExist: continue with suppress_stdout(): try: ArticulatedObjId = pb.loadURDF(directory_path + data_num + '/mobility.urdf', [0, 0, 0], [0, 0, 0, 1], useFixedBase=1) except: error_file.write(str(data_num) + "\n") continue URDFInfo = [] for jointId in range(pb.getNumJoints(ArticulatedObjId)): jointInfo = pb.getCollisionShapeData(ArticulatedObjId, jointId) for part in jointInfo: URDFInfo.append([jointId, part]) with open(directory_path + data_num + '/result.json') as f: json_file = json.load(f) PartInfo = [] parse_json(json_file, PartInfo) segment_match = {} for j in PartInfo: for i in URDFInfo: for k in j['objs'][0]: result = i[1][4].decode('utf-8').find(k + '.') if result != -1:
urdf_flags = p.URDF_USE_INERTIA_FROM_FILE | p.URDF_MAINTAIN_LINK_ORDER linkage = p.loadURDF("assembly.urdf", startPos, startOrn, useFixedBase=1, flags=urdf_flags) # See startup info numJoints = p.getNumJoints(linkage) logging.info("There are %d links in file\n", numJoints) jointInfoList = {} for i in range(numJoints): jointInfo = p.getJointInfo(linkage, i) jointInfoList[jointInfo[1].decode('UTF-8')] = jointInfo[0] state = p.getLinkState(linkage, jointInfo[0]) coll_data = p.getCollisionShapeData(linkage, jointInfo[0]) logging.debug("%s %d", jointInfo[1].decode('UTF-8'), i) logging.debug("links: %s %d", jointInfo[12], jointInfo[16]) logging.debug("com frame (posn & orn) %s %s", str(state[0]), str(p.getEulerFromQuaternion(state[1]))) logging.debug("inertial offset (posn & orn) %s %s", str(state[2]), str(p.getEulerFromQuaternion(state[3]))) logging.debug("link frame (posn & orn) %s %s", str(state[4]), str(p.getEulerFromQuaternion(state[5]))) logging.debug("type %s", str(jointInfo[2])) logging.debug("collision (path, posn, orn) %s \n\n", str(coll_data)) # Set collision characteristics p.setCollisionFilterPair(linkage, linkage, jointInfoList['dogbone_joint_far_left'], jointInfoList['left_eye_joint'], 1)
def sim_bullet(self, table, mode='GUI'): if mode == "GUI": self.mode = "GUI" p.connect( p.GUI, 1234 ) # use build-in graphical user interface, p.DIRECT: pass the final results p.resetDebugVisualizerCamera( cameraDistance=0.45, cameraYaw=-90.0, cameraPitch=-89, cameraTargetPosition=[1.55, 0.85, 1.4]) elif mode == "DIRECT": p.connect(p.DIRECT) else: print("Input wrong simulation mode ") p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setGravity(0., 0., -9.81) p.setAdditionalSearchPath(pybullet_data.getDataPath()) path_robots = "/home/hszlyw/PycharmProjects/ahky/robots" # tablesize [2.14 x 1.22] file1 = os.path.join(path_robots, "models", "air_hockey_table", "model.urdf") # self.table = p.loadURDF(file, [1.7, 0.85, 0.117], [0, 0, 0.0, 1.0]) self.table = p.loadURDF(file1, list(table[:3]), list(table[3:])) file2 = os.path.join(path_robots, "models", "puck", "model2.urdf") # self.puck = p.loadURDF(file, puck_poses[0, 0:3], [0, 0, 0.0, 1.0], flags=p.URDF_USE_IMPLICIT_CYLINDER) self.puck = p.loadURDF(file2, self.init_pos, [0, 0, 0.0, 1.0], flags=p.URDF_USE_INERTIA_FROM_FILE or p.URDF_MERGE_FIXED_LINKS) j_puck = self.get_joint_map(self.puck) j_table = self.get_joint_map(self.table) tableshape = p.getCollisionShapeData(self.table, j_table.get(b'base_joint')) puckshape = p.getCollisionShapeData(self.puck, -1) p.changeDynamics(self.puck, -1, lateralFriction=1) p.changeDynamics(self.puck, -1, restitution=1) p.changeDynamics(self.puck, -1, linearDamping=self.damp) # p.changeDynamics(self.puck, -1, restitution=0.8) # load parameters p.changeDynamics(self.table, j_table.get(b'base_joint'), lateralFriction=self.t_lateral_f) p.changeDynamics(self.table, j_table.get(b'base_down_rim_l'), lateralFriction=self.four_side_rim_latf) p.changeDynamics(self.table, j_table.get(b'base_down_rim_r'), lateralFriction=self.four_side_rim_latf) p.changeDynamics(self.table, j_table.get(b'base_down_rim_top'), lateralFriction=1) # no collision p.changeDynamics(self.table, j_table.get(b'base_left_rim'), lateralFriction=self.left_rim_f) p.changeDynamics(self.table, j_table.get(b'base_right_rim'), lateralFriction=self.left_rim_f) p.changeDynamics(self.table, j_table.get(b'base_up_rim_l'), lateralFriction=self.four_side_rim_latf) p.changeDynamics(self.table, j_table.get(b'base_up_rim_r'), lateralFriction=self.four_side_rim_latf) p.changeDynamics(self.table, j_table.get(b'base_up_rim_top'), lateralFriction=1) # no collision p.changeDynamics(self.table, j_table.get(b'base_joint'), restitution=0.2) p.changeDynamics(self.table, j_table.get(b'base_down_rim_l'), restitution=self.four_side_rim_res) p.changeDynamics(self.table, j_table.get(b'base_down_rim_r'), restitution=self.four_side_rim_res) p.changeDynamics(self.table, j_table.get(b'base_down_rim_top'), restitution=self.four_side_rim_res) # no collision p.changeDynamics(self.table, j_table.get(b'base_left_rim'), restitution=self.left_rim_res) p.changeDynamics(self.table, j_table.get(b'base_right_rim'), restitution=self.right_rim_res) p.changeDynamics(self.table, j_table.get(b'base_up_rim_l'), restitution=self.four_side_rim_res) p.changeDynamics(self.table, j_table.get(b'base_up_rim_r'), restitution=self.four_side_rim_res) p.changeDynamics(self.table, j_table.get(b'base_up_rim_top'), restitution=self.four_side_rim_res) # no collision # init puck self.collision_filter(self.puck, self.table) p.resetBasePositionAndOrientation(self.puck, self.init_pos, [0, 0, 0, 1.]) p.resetBaseVelocity(self.puck, linearVelocity=self.lin_vel, angularVelocity=self.ang_vel) # START pre_pos = self.init_pos pos = [pre_pos] # record position vel = [self.lin_vel] # record velocity t_sim = [0] # record time t = 0 p.setTimeStep(1 / 120.) while (np.abs(np.array(p.getBaseVelocity(self.puck)[0][:-1])) > .1).any() and .115 < np.abs( p.getBasePositionAndOrientation(self.puck)[0][2]) < 1.2: # while True: p.stepSimulation() # time.sleep(1/120.) # t += 1/120 t_sim.append(t) new_pos = p.getBasePositionAndOrientation(self.puck)[0] pos.append(new_pos) vel.append(p.getBaseVelocity(self.puck)[0]) if mode == 'GUI': p.addUserDebugLine(pre_pos, new_pos, lineColorRGB=[0.5, 0.5, 0.5], lineWidth=3) pre_pos = new_pos p.disconnect() return np.array(t_sim)[:, None], np.array(pos), np.array(vel)
def get_collision_data(body, link=BASE_LINK): # TODO: try catch return [CollisionShapeData(*tup) for tup in p.getCollisionShapeData(body, link, physicsClientId=CLIENT)]