def _disable_simobj_collisions(self): """ Disables all collisions between simobjects and robots. """ for sim_obj in self.simulator._objects.keys(): if sim_obj not in self.excluded_bodies: p.setCollisionFilterGroupMask(sim_obj, 0, 0, 0)
def set_collisions(self, links, group=1, mask=1): """Activate/Deactivate leg collisions""" for link in links: p.setCollisionFilterGroupMask(bodyUniqueId=self.animal, linkIndexA=self.link_id[link], collisionFilterGroup=group, collisionFilterMask=mask)
def __init__(self, size): uVer = math.pi/2 uHor = 0 ver = p.getQuaternionFromEuler([0,0,math.radians(90)]) hor = p.getQuaternionFromEuler([0,0,0]) self.pos = [0,0,0] #make floor self.floor = p.loadURDF(mv.prefabToURDF["plane"], [0,0,0], hor, useFixedBase=1, globalScaling=size/200) hu.unitySpawn(self.floor, "plane", [0,0,0], uHor, size/10) #set prefab as plane hsm.objIDToObject[self.floor] = self p.setCollisionFilterGroupMask(self.floor, -1, mv.TERRAIN_GROUP, mv.TERRAIN_MASK) #make walls self.walls = [] self.walls.append(p.loadURDF(mv.prefabToURDF["wall"], [-0.59*size, 0, size/10.0], ver, useFixedBase=1, globalScaling = size*10)) # 0 self.walls.append(p.loadURDF(mv.prefabToURDF["wall"], [0.59*size, 0, size/10.0], ver, useFixedBase=1, globalScaling = size*10)) #180 self.walls.append(p.loadURDF(mv.prefabToURDF["wall"], [0, 0.59*size, size/10.0], hor, useFixedBase=1, globalScaling = size*10)) # 90 self.walls.append(p.loadURDF(mv.prefabToURDF["wall"], [0, -0.59*size, size/10.0], hor, useFixedBase=1, globalScaling = size*10)) #270 hu.unitySpawn(self.walls[0], "wall", [-(size + mv.WALL_WIDTH)/2.0, 0, mv.WALL_HEIGHT/2.0], uVer, size) #set prefab as cube hu.unitySpawn(self.walls[1], "wall", [(size + mv.WALL_WIDTH)/2.0, 0, mv.WALL_HEIGHT/2.0], uVer, size) hu.unitySpawn(self.walls[2], "wall", [0, (size + mv.WALL_WIDTH)/2.0, mv.WALL_HEIGHT/2.0], uHor, size) hu.unitySpawn(self.walls[3], "wall", [0, -(size + mv.WALL_WIDTH)/2.0, mv.WALL_HEIGHT/2.0], uHor, size) for wallID in self.walls: hsm.objIDToObject[wallID] = self hsm.terrainWallIDs.append(wallID) p.setCollisionFilterGroupMask(wallID, -1, mv.TERRAIN_GROUP, mv.TERRAIN_MASK)
def __init__(self, n_joints, pos, radius, parent=None): ''' Create a pybullet tentacle ''' mass = 1 visualShapeId = -1 self.particles = [None] * n_joints self.radius = radius if parent is None: base_shape = p.createCollisionShape(p.GEOM_PLANE) self.base = p.createMultiBody(0, base_shape, -1, pos) else: self.base = parent colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=self.radius) p.setCollisionFilterGroupMask(self.base,-1,0,0) for i in range (n_joints): self.particles[i] = p.createMultiBody(mass, colSphereId ,visualShapeId, [pos[0], pos[1], pos[2] + i*2*self.radius + self.radius]) if i > 0: p.createConstraint(self.particles[i-1], -1, self.particles[i], -1, p.JOINT_FIXED,[0,0,0], [0,0, 2*self.radius], [0,0,0]) else: p.setCollisionFilterPair(self.base, self.particles[i], -1, -1, True) p.createConstraint(self.base, -1, self.particles[i], -1, p.JOINT_FIXED, [0,0,0], [0,0,self.radius], [0,0,0], [0,0,0,1]) p.changeDynamics(self.particles[i],-1, mass=1, linearDamping=1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
def set_collisions_whole_body(self, group=1, mask=1): """Activate/Deactivate leg collisions""" for link in range(-1, p.getNumJoints(self.animal)): p.setCollisionFilterGroupMask(bodyUniqueId=self.animal, linkIndexA=link, collisionFilterGroup=group, collisionFilterMask=mask)
def _initialize_bullet(self): """ Initialize physics world in headless mode or with GUI (debug). """ GUI = False if GUI: self.world = pybullet.connect(pybullet.GUI) pybullet.setRealTimeSimulation(0) else: self.world = pybullet.connect( pybullet.DIRECT) # non-graphical client # set-up simulation pybullet.setGravity(0, 0, -9.81) pybullet.setTimeStep(config.TIME_STEP) pybullet.setPhysicsEngineParameter( fixedTimeStep=config.TIME_STEP, numSolverIterations=config.SOLVER_ITERATIONS, numSubSteps=config.SOLVER_SUB_STEPS) # add ground plane pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) self.plane = pybullet.loadURDF("plane.urdf", globalScaling=1 / 10.0) pybullet.setCollisionFilterGroupMask(self.plane, -1, 1, 1) pybullet.resetBasePositionAndOrientation(self.plane, [0, 0, 0], [0, 0, 0, 1], self.world)
def __init__(self, action_repeat=10, render=False): self._action_repeat = action_repeat self.robot = Robot('ur5e_stick', pb=True, pb_cfg={'gui': render, 'realtime':False}) self.ee_ori = [-np.sqrt(2) / 2, np.sqrt(2) / 2, 0, 0] self._action_bound = 1.0 self._ee_pos_scale = 0.02 self._ee_ori_scale = np.pi / 36.0 self._action_high = np.array([self._action_bound] * 2) self.action_space = spaces.Box(low=-self._action_high, high=self._action_high, dtype=np.float32) self.goal = np.array([0.75, -0.3, 1.0]) self.init = np.array([0.5, 0.1, 1.0]) self.robot.arm.reset() ori = euler2quat([0, 0, np.pi / 2]) self.table_id = self.robot.pb_client.load_urdf('table/table.urdf', [.5, 0, 0.4], ori, scaling=0.9) self.marker_id = self.robot.pb_client.load_geom('box', size=0.05, mass=1, base_pos=self.goal.tolist(), rgba=[0, 1, 0, 0.4]) client_id = self.robot.pb_client.get_client_id() p.setCollisionFilterGroupMask(self.marker_id, -1, 0, 0, physicsClientId=client_id) p.setCollisionFilterPair(self.marker_id, self.table_id, -1, -1, 1, physicsClientId=client_id) self.reset() state_low = np.full(len(self._get_obs()), -float('inf')) state_high = np.full(len(self._get_obs()), float('inf')) self.observation_space = spaces.Box(state_low, state_high, dtype=np.float32)
def __init__(self, objPos): self.yaw = random.uniform(0.0, 2.0 * np.pi) self.pos = objPos self.objID = hsm.create("food", mv.prefabToURDF["food"], self.pos, self.yaw, mv.FOOD_SIZE) hsm.objIDToObject[self.objID] = self p.setCollisionFilterGroupMask(self.objID, -1, mv.FOOD_GROUP, mv.FOOD_MASK)
def setCollision(self): for joint in range( p.getNumJoints(bodyUniqueId=self.robotId, physicsClientId=self.clientId)): p.setCollisionFilterGroupMask(bodyUniqueId=self.robotId, linkIndexA=joint, collisionFilterGroup=1, collisionFilterMask=1)
def _enable_robot_collisions(self): """ Enables all self collisions for all robots. """ for robot in self.simulator._robots.keys(): links = [idx for idx in range(-1, p.getNumJoints(robot))] for link in links: p.setCollisionFilterGroupMask(robot, link, 0, 1)
def init(self): for joint in range(pybullet.getNumJoints(self.URDF)): name = self.name + " - " + str(joint) self.addDebugParams(name, -5, 5, 0) #collisiones correctas pybullet.setCollisionFilterGroupMask(self.URDF, -1, 0, 0) for e in list([3,4]): for i in range(6): pybullet.setCollisionFilterPair(self.URDF, self.URDF, e, i, 0)
def _disable_robot_collisions(self): """ Disables all self collisions for all robots. """ for robot in self.simulator._robots.keys(): if robot not in self.excluded_bodies: links = [idx for idx in range(-1, p.getNumJoints(robot))] for link in links: if (robot, link) not in self.excluded_body_link_pairs: p.setCollisionFilterGroupMask(robot, link, 0, 0)
def __init__(self, objPos): super().__init__("prey", mv.prefabToURDF["prey"], objPos, mv.PREY_MAX_STAMINA, mv.PREY_SIZE) self.isEaten = False # True if a predator has eaten this prey, False otherwise self.foodTimeStamps = [] self.isTargeted = False self.targetList = [[], [], [], [], []] p.setCollisionFilterGroupMask(self.objID, -1, mv.PREY_GROUP, mv.PREY_MASK) self.hallucinatedPred = None self.hallucinationTimer = 0
def load_collision(self): # Doit être appelé après avoir générer les robots et le plateau for i in range(0, len(self.robots)): # On désactive les collisions entre les robots, # en pratique chaque robot a son propore groupe et mask # avec i, donc aucun ne peuvent avoir de collision entre eux p.setCollisionFilterGroupMask(self.robots[i].robotId, -1, i, i) # On rend la collision possible avec le sol p.setCollisionFilterPair(self.robots[i].robotId, self.planeId, -1, -1, 1)
def createModel(self, nodeId, baseMass, basePosition, baseQuaternionOrientation): linkMasses = [] linkCollisionShapeIndices = [] linkVisualShapeIndices = [] linkPositions = [] linkOrientations = [] linkInertialFramePositions = [] linkInertialFrameOrientations = [] linkParentIndices = [] linkJointTypes = [] linkJointAxis = [] linkJointAxis = [] for i in range(self.jointNum): linkMasses.append(baseMass) # 0 makes link static linkCollisionShapeIndices.append(nodeId) linkVisualShapeIndices.append(-1) linkPositions.append([0, 0, self.nodeLength + self.gap]) linkOrientations.append([0, 0, 0, 1]) linkInertialFramePositions.append([0, 0, 0]) linkInertialFrameOrientations.append([0, 0, 0, 1]) linkParentIndices.append(i) linkJointTypes.append(p.JOINT_SPHERICAL) linkJointAxis.append([0, 1, 0]) # create rigid objects by shapes boneId = p.createMultiBody( baseMass, nodeId, -1, basePosition, baseQuaternionOrientation, linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis) # remove collisions for i in range(-1, self.jointNum): p.setCollisionFilterGroupMask(boneId, i, 0, 0) p.setCollisionFilterPair(0, boneId, -1, i, 1) return boneId
def reset_objects(self): # activate collisions - or move out of sim area and deactivate for obj_str in self.models.keys(): if obj_str in self.objects_to_use: pybullet.changeDynamics(self.models[obj_str], -1, mass=1.0) pybullet.setCollisionFilterGroupMask(self.models[obj_str], -1, 1, 1) else: pybullet.resetBasePositionAndOrientation( self.models[obj_str], [0, 0, -5], [0, 0, 0, 1], self.world) pybullet.changeDynamics(self.models[obj_str], -1, mass=0) # -> static pybullet.setCollisionFilterGroupMask(self.models[obj_str], -1, 0, 0)
def load_robot(self): flags = p.URDF_USE_SELF_COLLISION table = p.loadURDF(TABLE_URDF_PATH, [0.5, 0, -0.6300], [0, 0, 0, 1]) robot = p.loadURDF(ROBOT_URDF_PATH, [0, 0, 0], [0, 0, 0, 1], flags=flags) state_object= [random.uniform(0.3,0.6),random.uniform(-0.2,0.2),random.uniform(0.3,0.6)] self.objectUid = p.loadURDF(SPHERE_URDF_PATH, useMaximalCoordinates=False,globalScaling=0.1,basePosition=state_object,useFixedBase=True) collisionFilterGroup = 0 collisionFilterMask = 0 p.setCollisionFilterGroupMask(self.objectUid, -1, collisionFilterGroup, collisionFilterMask) enableCollision = 0 p.setCollisionFilterPair(robot, self.objectUid, -1, -1, enableCollision) return robot
def create_arena(self): arenaVis = p.createVisualShape(shapeType=p.GEOM_CYLINDER, radius=self.arena_radius, length=0.01, visualFramePosition=[0, 0, 0], visualFrameOrientation=[0, 0, 0, 1], rgbaColor=[0.5, 0.5, 0.5, 0.5], physicsClientId=self.clientId) arena = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=-1, baseVisualShapeIndex=arenaVis, basePosition=[0, 0, 0.01]) p.setCollisionFilterGroupMask(bodyUniqueId=arena, linkIndexA=0, collisionFilterGroup=0, collisionFilterMask=0) return arena
def draw_marker(self, pos): print("drawing marker...") client_id = self.robot.pb_client.get_client_id() self.marker_id = self.robot.pb_client.load_geom('box', size=self.box_size, mass=1, base_pos=pos, rgba=[0, 1, 0, 0.4]) p.setCollisionFilterGroupMask(self.marker_id, -1, 0, 0, physicsClientId=client_id) p.setCollisionFilterPair(self.marker_id, self.table_id, -1, -1, 1, physicsClientId=client_id)
timeStep=1./500 p.setTimeStep(timeStep) #p.setDefaultContactERP(0) #urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS urdfFlags = p.URDF_USE_SELF_COLLISION startPos=[0.007058990464444105, 0.03149299192130908, 0.4918981912395484] startOrn=[0.005934649695708604, 0.7065453990917289, 0.7076373820553712, -0.0027774940359030264] quadruped = p.loadURDF("laikago/laikago.urdf",startPos,startOrn, flags = urdfFlags,useFixedBase=False) p.resetBasePositionAndOrientation(quadruped,startPos,startOrn) #This cube is added as a soft constraint to keep the laikago from falling #since we didn't train it yet, it doesn't balance cube = p.loadURDF("cube_no_rotation.urdf",[0,0,-0.5],[0,0.5,0.5,0]) p.setCollisionFilterGroupMask(cube,-1,0,0) for j in range(p.getNumJoints(cube)): p.setJointMotorControl2(cube,j,p.POSITION_CONTROL,force=0) p.setCollisionFilterGroupMask(cube,j,0,0) p.changeVisualShape(cube,j,rgbaColor=[1,0,0,0]) cid = p.createConstraint(cube,p.getNumJoints(cube)-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,1,0],[0,0,0]) p.changeConstraint(cid, maxForce=10) jointIds=[] paramIds=[] jointOffsets=[] jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1] jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0] for i in range (4):
import pybullet as p import time import pybullet_data p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False) cubeId = p.loadURDF("cube_collisionfilter.urdf", [0, 0, 3], useMaximalCoordinates=False) collisionFilterGroup = 0 collisionFilterMask = 0 p.setCollisionFilterGroupMask(cubeId, -1, collisionFilterGroup, collisionFilterMask) enableCollision = 1 p.setCollisionFilterPair(planeId, cubeId, -1, -1, enableCollision) p.setRealTimeSimulation(1) p.setGravity(0, 0, -10) while (p.isConnected()): time.sleep(1. / 240.) p.setGravity(0, 0, -10)
maiskolben_1.append(pixel_5) maiskolben_1.append(pixel_6) maiskolben_1.append(pixel_7) maiskolben_1.append(pixel_8) j = 0 for pixel in maiskolben_1: body = p.getBodyUniqueId(pixel) while len(maiskolben_1_hit) < body: maiskolben_1_hit.append(j) maiskolben_1_hit.append(j) collisionFilterGroup = 0 collisionFilterMask = 0 p.setCollisionFilterGroupMask(col, -1, collisionFilterGroup, collisionFilterMask) p.setCollisionFilterGroupMask(col2, -1, collisionFilterGroup, collisionFilterMask) collisionFilterGroup = 0 collisionFilterMask = 0 p.setCollisionFilterGroupMask(pixel_1, -1, collisionFilterGroup, collisionFilterMask) p.setCollisionFilterGroupMask(pixel_2, -1, collisionFilterGroup, collisionFilterMask) p.setCollisionFilterGroupMask(pixel_3, -1, collisionFilterGroup, collisionFilterMask) p.setCollisionFilterGroupMask(pixel_4, -1, collisionFilterGroup, collisionFilterMask) p.setCollisionFilterGroupMask(pixel_5, -1, collisionFilterGroup,
def add_robot(self, physics_active=True): if not physics_active: flags = p.URDF_USE_INERTIA_FROM_FILE elif self.robot_type in ["wolfgang", "sigmaban"]: # we have a better inertia estimation from onshape in our model flags = p.URDF_USE_SELF_COLLISION + p.URDF_USE_INERTIA_FROM_FILE + \ p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS else: # most other URDFs from the internet have issues with their inertia values flags = p.URDF_USE_SELF_COLLISION + p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS if self.urdf_path is None: # use standards rospack = rospkg.RosPack() if self.robot_type == "op2": self.urdf_path = rospack.get_path( "robotis_op2_description") + "/urdf/robot.urdf" elif self.robot_type == "sigmaban": self.urdf_path = rospack.get_path( "sigmaban_description") + "/urdf/robot.urdf" else: self.urdf_path = rospack.get_path( "wolfgang_description") + "/urdf/robot.urdf" robot_index = p.loadURDF(self.urdf_path, self.start_position, self.start_orientation, flags=flags, useFixedBase=not physics_active) self.robot_indexes.append(robot_index) # disable physics for robots that dont need it (mostly used to display reference trajectories) if not physics_active: p.changeDynamics(robot_index, -1, linearDamping=0, angularDamping=0) p.setCollisionFilterGroupMask(robot_index, -1, collisionFilterGroup=0, collisionFilterMask=0) p.changeDynamics(robot_index, -1, activationState=p.ACTIVATION_STATE_SLEEP + p.ACTIVATION_STATE_ENABLE_SLEEPING + p.ACTIVATION_STATE_DISABLE_WAKEUP) num_joints = p.getNumJoints(robot_index) for j in range(num_joints): p.setCollisionFilterGroupMask(robot_index, j, collisionFilterGroup=0, collisionFilterMask=0) p.changeDynamics(robot_index, j, activationState=p.ACTIVATION_STATE_SLEEP + p.ACTIVATION_STATE_ENABLE_SLEEPING + p.ACTIVATION_STATE_DISABLE_WAKEUP) # Retrieving joints and foot pressure sensors joints = {} joint_stall_torques = [] joint_max_vels = [] link_masses = [] link_inertias = [] pressure_sensors = {} links = {} # Collecting the available joints for i in range(p.getNumJoints(robot_index)): joint_info = p.getJointInfo(robot_index, i) name = joint_info[1].decode('utf-8') # we can get the links by seeing where the joint is attached. we only get parent link so do +1 links[joint_info[12].decode('utf-8')] = joint_info[16] + 1 if name in self.initial_joint_positions.keys(): # remember joint joints[name] = Joint(i, robot_index) joint_stall_torques.append(joint_info[10]) joint_max_vels.append(joint_info[11]) elif name in [ "LLB", "LLF", "LRF", "LRB", "RLB", "RLF", "RRF", "RRB" ]: p.enableJointForceTorqueSensor(robot_index, i) pressure_sensors[name] = PressureSensor( name, i, robot_index, 10, 5) for link_id in links.values(): dynamics_info = p.getDynamicsInfo(robot_index, link_id) link_masses.append(dynamics_info[0]) link_inertias.append(dynamics_info[2]) # set dynamics for feet, maybe overwritten later by domain randomization for link_name in links.keys(): if link_name in [ "llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb" ]: p.changeDynamics(robot_index, links[link_name], lateralFriction=1, spinningFriction=0.1, rollingFriction=0.1, restitution=0.9) self.joints[robot_index] = joints self.joint_stall_torques[robot_index] = joint_stall_torques self.joint_max_vels[robot_index] = joint_max_vels self.link_masses[robot_index] = link_masses self.link_inertias[robot_index] = link_inertias self.pressure_sensors[robot_index] = pressure_sensors self.links[robot_index] = links self.torso_ids[robot_index] = self.links[robot_index]["torso"] # reset robot to initial position self.reset(robot_index) return robot_index
def plotGranular(self): x_value = -1.0 y_min = -1.0 y_max = 4.0 z_min = 0.0 z_max = self.granularDepth z_values = np.linspace(z_min, z_max, num=100) delta = 0.1 # Plot Plane 1 for z_value in z_values: self.granular_points.append( p.addUserDebugLine([x_value - delta, y_min - delta, z_value], [x_value - delta, y_max + delta, z_value], [1.0, 0, 0])) # Plot Plane 2 x_min = -1.0 x_max = 1.0 y_value = -1.0 for z_value in z_values: self.granular_points.append( p.addUserDebugLine([x_min - delta, y_value - delta, z_value], [x_max + delta, y_value - delta, z_value], [1.0, 0, 0])) # Plot Plane 3 x_value = 1.0 for z_value in z_values: self.granular_points.append( p.addUserDebugLine([x_value + delta, y_min - delta, z_value], [x_value + delta, y_max + delta, z_value], [1.0, 0, 0])) # Plot Plane 4 x_min = -1.0 x_max = 1.0 y_value = y_max for z_value in z_values: self.granular_points.append( p.addUserDebugLine([x_min - delta, y_value + delta, z_value], [x_max + delta, y_value + delta, z_value], [1.0, 0, 0])) # Add the sand sphere_radius = 0.03 x_values = np.linspace(x_min, x_max, num=int( (x_max - x_min) / (2.5 * sphere_radius))) y_values = np.linspace(y_min, y_max, num=int( (y_max - y_min) / (2.5 * sphere_radius))) for x in x_values: for y in y_values: nextSphere = p.loadURDF( "/home/peterjochem/Desktop/Deep_RL/DDPG/h3pper/gym-hopping_robot/gym_hopping_robot/envs/hopping_robot/urdf/sphere_1cm.urdf", [x, y, sphere_radius], useFixedBase=False) p.setCollisionFilterGroupMask(nextSphere, -1, 0, 0) enableCollision = 1 p.setCollisionFilterPair(self.plane, nextSphere, -1, -1, enableCollision)
def __init__(self): # Two environments? One for graphics, one w/o graphics? self.physicsClient = p.connect( p.GUI) #or p.DIRECT for non-graphical version self.visualizeTrajectory = False self.jointIds = [] self.paramIds = [] #self.neural_net = keras.models.load_model(absolute_path_neural_net) p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally p.setGravity(0, 0, -10) self.plane = p.loadURDF("plane.urdf") p.setCollisionFilterGroupMask(self.plane, -1, 0, 0) self.hopper = p.loadURDF(absolute_path_urdf, [0.0, 0.0, 1.35], useFixedBase=False) p.setCollisionFilterGroupMask(self.hopper, -1, 0, 0) self.gravId = p.addUserDebugParameter("gravity", -10, 10, -10) self.homePositionAngles = [0.0, 0.0, 0.0] # Setup the debugParam sliders self.gravId = p.addUserDebugParameter("gravity", -10, 10, -10) self.homePositionAngles = [0.0, 0.0, 0.0] self.granularDepth = 0.3 # The height of the granular material/bed self.foot_points = [] self.body_points = [] self.granular_points = [] # For visualizing the granular material activeJoint = 0 for j in range(p.getNumJoints(self.hopper)): # Why set the damping factors to 0? p.changeDynamics(self.hopper, j, linearDamping=0, angularDamping=0) info = p.getJointInfo(self.hopper, j) jointName = info[1] jointType = info[2] if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): self.jointIds.append(j) self.paramIds.append( p.addUserDebugParameter( jointName.decode("utf-8"), -4, 4, self.homePositionAngles[activeJoint])) #p.resetJointState(self.hopper, j, self.homePositionAngles[activeJoint]) activeJoint = activeJoint + 1 p.setRealTimeSimulation( 0) # Must do this to apply forces/torques with PyBullet method self.plotGranular() enableCollision = 1 p.setCollisionFilterPair(self.plane, self.hopper, -1, 3, enableCollision) p.setCollisionFilterPair(self.plane, self.hopper, -1, 2, enableCollision) p.setCollisionFilterPair(self.plane, self.hopper, -1, 1, enableCollision) self.stateId = p.saveState( ) # Stores state in memory rather than on disk
test = np.array(sv.translation()) test_ = np.reshape(test, (3, )) print(test) print(test_) c = np.zeros([1, 2, 3, 3]) print(c[0, 1, 2, :]) c[0, 1, 2, :] = test_ print(c[0, 1, 2, :]) for bi, bd in enumerate(robot.kine_dyn.mb.bodies()): print('body index: {}, body name = {}, body position = {}'.format( bi, bd.name().decode("utf-8"), robot.kine_dyn.mbc.bodyPosW[bi].translation().transpose())) # p = e.Vector3d.UnitX() # p_ = sv.rotation().transpose() * p + sv.translation() # print(p_) # example to add in a cube link1_id = b.loadURDF("./assets/cube.urdf") b.setCollisionFilterGroupMask(link1_id, -1, 0, 0) # b.resetBasePositionAndOrientation(position=) while sim.get_time() < total_time: # sim.step_simulation() sim.step_simulation(q) # s = Simulation(0.001, r)
rep = p.connect(p.GUI) # Add a path where we want to search for data p.setAdditionalSearchPath(pybullet_data.getDataPath()) # Load a plane and a cube from URDF plane_id = p.loadURDF("plane.urdf", useMaximalCoordinates=False) cube_id = p.loadURDF("cube_collisionfilter.urdf", [0, 0, 3], useMaximalCoordinates=False) # Filter group and mask collision_filter_group = 0 collision_filter_mask = 0 # Set filter group and mask p.setCollisionFilterGroupMask(cube_id, -1, collision_filter_group, collision_filter_mask) # Collision enabled enable_collision = True # Collision between plane and cube enabled p.setCollisionFilterPair(plane_id, cube_id, -1, -1, enable_collision) # Set real time p.setRealTimeSimulation(True) # Set gravity # p.setGravity(0, 0, -9,8) # Run while connected while p.isConnected():
import pybullet as p import time p.connect(p.GUI) planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False) cubeId = p.loadURDF("cube_collisionfilter.urdf", [0, 0, 3], useMaximalCoordinates=False) collisionFilterGroup = 0 collisionFilterMask = 0 p.setCollisionFilterGroupMask(cubeId, -1, collisionFilterGroup, collisionFilterMask) enableCollision = 1 p.setCollisionFilterPair(planeId, cubeId, -1, -1, enableCollision) p.setRealTimeSimulation(1) p.setGravity(0, 0, -10) while (p.isConnected()): time.sleep(1. / 240.) p.setGravity(0, 0, -10)
def setup_inverse_kinematics(self, load_urdf=True): """ This function is responsible for doing any setup for inverse kinematics. Inverse Kinematics maps end effector (EEF) poses to joint angles that are necessary to achieve those poses. load_urdf specifies whether the robot urdf should be loaded into the sim. Useful flag that should be cleared in the case of multi-armed robots which might have multiple IK controller instances but should all reference the same (single) robot urdf within the bullet sim """ # get paths to urdfs self.robot_urdf = pjoin( os.path.join(robosuite.models.assets_root, "bullet_data"), "{}_description/urdf/{}_arm.urdf".format(self.robot_name.lower(), self.robot_name.lower())) # import reference to the global pybullet server and load the urdfs from . import get_pybullet_server if load_urdf: self.ik_robot = p.loadURDF(fileName=self.robot_urdf, useFixedBase=1, physicsClientId=self.bullet_server_id) # Add this to the pybullet server get_pybullet_server().bodies[self.ik_robot] = self.robot_name else: # We'll simply assume the most recent robot (robot with highest pybullet id) is the relevant robot and # mark this controller as belonging to that robot body self.ik_robot = max(get_pybullet_server().bodies) # load the number of joints from the bullet data self.num_bullet_joints = p.getNumJoints( self.ik_robot, physicsClientId=self.bullet_server_id) # Disable collisions between all the joints for joint in range(self.num_bullet_joints): p.setCollisionFilterGroupMask( bodyUniqueId=self.ik_robot, linkIndexA=joint, collisionFilterGroup=0, collisionFilterMask=0, physicsClientId=self.bullet_server_id) # TODO: Very ugly initialization - any way to automate this? Maybe move the hardcoded magic numbers to the robot model files? # For now, hard code baxter bullet eef idx if self.robot_name == "Baxter": if "right" in self.eef_name: self.bullet_ee_idx = 27 self.bullet_joint_indexes = [13, 14, 15, 16, 17, 19, 20] self.ik_command_indexes = np.arange(1, self.joint_dim + 1) elif "left" in self.eef_name: self.bullet_ee_idx = 45 self.bullet_joint_indexes = [31, 32, 33, 34, 35, 37, 38] self.ik_command_indexes = np.arange(self.joint_dim + 1, self.joint_dim * 2 + 1) else: # Error with inputted id print( "Error loading ik controller for Baxter -- arm id's must contain 'right' or 'left'!" ) else: # Default assumes pybullet has same number of joints compared to mujoco sim self.bullet_ee_idx = self.num_bullet_joints - 1 self.bullet_joint_indexes = np.arange(self.joint_dim) self.ik_command_indexes = np.arange(self.joint_dim) # Set rotation offsets (for mujoco eef -> pybullet eef) and rest poses self.rest_poses = list(self.initial_joint) if self.robot_name == "Sawyer": self.rotation_offset = T.rotation_matrix(angle=-np.pi / 2, direction=[0., 0., 1.], point=None) elif self.robot_name == "Panda": self.rotation_offset = T.rotation_matrix(angle=np.pi / 4, direction=[0., 0., 1.], point=None) elif self.robot_name == "Baxter": self.rotation_offset = T.rotation_matrix(angle=0, direction=[0., 0., 1.], point=None) else: # No other robots supported, print out to user print( "ERROR: Unsupported robot requested for ik controller. Only Sawyer, Panda, and Baxter " "currently supported.") # Simulation will update as fast as it can in real time, instead of waiting for # step commands like in the non-realtime case. p.setRealTimeSimulation(1, physicsClientId=self.bullet_server_id)
maiskolben_1_hit = [] maiskolben_2 = [] maiskolben_2_hit = [] maiskolben_3 = [] maiskolben_3_hit = [] createMaiskolben(0, 0, 0, maiskolben_1, maiskolben_1_hit) createMaiskolben(5, 0, 0, maiskolben_2, maiskolben_2_hit) createMaiskolben(-5, 0, 0, maiskolben_3, maiskolben_3_hit) collisionFilterGroup = 0 collisionFilterMask = 0 p.setCollisionFilterGroupMask(col, -1, collisionFilterGroup, collisionFilterMask) p.setCollisionFilterGroupMask(col2, -1, collisionFilterGroup, collisionFilterMask) collisionFilterGroup = 0 collisionFilterMask = 0 enableCollision = 1 p.setCollisionFilterPair(planeId, col, -1, -1, enableCollision) p.setRealTimeSimulation(1) # p.setGravity(0, 0, -10) cid = p.createConstraint(col, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1])
def __init__(self, objPos): super().__init__("predator", mv.prefabToURDF["predator"], objPos, mv.PREDATOR_MAX_STAMINA, mv.PREDATOR_SIZE) self.lastTargetedPrey = None self.preyTimeStamps = [] p.setCollisionFilterGroupMask(self.objID, -1, mv.PREDATOR_GROUP, mv.PREDATOR_MASK)