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)
