def _reset(self): if (self.physicsClientId<0): conInfo = p.getConnectionInfo() if (conInfo['isConnected']): self.ownsPhysicsClient = False self.physicsClientId = 0 else: self.ownsPhysicsClient = True self.physicsClientId = p.connect(p.SHARED_MEMORY) if (self.physicsClientId<0): if (self.isRender): self.physicsClientId = p.connect(p.GUI) else: self.physicsClientId = p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) if self.scene is None: self.scene = self.create_single_player_scene() if not self.scene.multiplayer and self.ownsPhysicsClient: self.scene.episode_restart() self.robot.scene = self.scene self.frame = 0 self.done = 0 self.reward = 0 dump = 0 s = self.robot.reset() self.potential = self.robot.calc_potential() return s
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"plane_stadium.sdf") self.ground_plane_mjcf=p.loadSDF(filename) #filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") #self.ground_plane_mjcf = p.loadSDF(filename) # for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8]) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
def _reset(self):
if (self.stateId>=0):
#print("restoreState self.stateId:",self.stateId)
p.restoreState(self.stateId)
r = MJCFBaseBulletEnv._reset(self)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene(
self.stadium_scene.ground_plane_mjcf)
self.ground_ids = set([(self.parts[f].bodies[self.parts[f].bodyIndex], self.parts[f].bodyPartIndex) for f in
self.foot_ground_object_names])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
if (self.stateId<0):
self.stateId=p.saveState()
#print("saving state self.stateId:",self.stateId)
return r
def __init__(
self,
renderer='tiny', # ('tiny', 'egl', 'debug')
):
self.action_space = spaces.Discrete(2)
self.iter = cycle(range(0, 360, 10))
# how we want to show
assert renderer in ('tiny', 'egl', 'debug', 'plugin')
self._renderer = renderer
self._render_width = 84
self._render_height = 84
# connecting
if self._renderer == "tiny" or self._renderer == "plugin":
optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
p.connect(p.DIRECT, options=optionstring)
if self._renderer == "plugin":
plugin_fn = os.path.join(
p.__file__.split("bullet3")[0],
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
plugin = p.loadPlugin(plugin_fn, "_tinyRendererPlugin")
if plugin < 0:
print("\nPlugin Failed to load! Try installing via `pip install -e .`\n")
sys.exit()
print("plugin =", plugin)
elif self._renderer == "egl":
optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
optionstring += ' --window_backend=2 --render_device=0'
p.connect(p.GUI, options=optionstring)
elif self._renderer == "debug":
#print("Connection: SHARED_MEMORY")
#cid = p.connect(p.SHARED_MEMORY)
#if (cid<0):
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
def __enter__(self):
print("connecting")
optionstring='--width={} --height={}'.format(pixelWidth,pixelHeight)
optionstring += ' --window_backend=2 --render_device=0'
print(self.connection_mode, optionstring,*self.argv)
cid = pybullet.connect(self.connection_mode, options=optionstring,*self.argv)
if cid < 0:
raise ValueError
print("connected")
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0)
pybullet.resetSimulation()
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
pybullet.loadURDF("duck_vhacd.urdf")
pybullet.setGravity(0,0,-10)
import pybullet as p
import pybullet_data as pd
import math
import time
import numpy as np
import panda_sim_grasp as panda_sim
#video requires ffmpeg available in path
createVideo=False
if createVideo:
p.connect(p.GUI, options="--mp4=\"pybullet_grasp.mp4\", --mp4fps=240")
else:
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP,1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.resetDebugVisualizerCamera(cameraDistance=1.3, cameraYaw=38, cameraPitch=-22, cameraTargetPosition=[0.35,-0.13,0])
p.setAdditionalSearchPath(pd.getDataPath())
timeStep=1./120.#240.
p.setTimeStep(timeStep)
p.setGravity(0,-9.8,0)
panda = panda_sim.PandaSimAuto(p,[0,0,0])
logId = panda.bullet_client.startStateLogging(panda.bullet_client.STATE_LOGGING_PROFILE_TIMINGS, "log.json")
panda.bullet_client.submitProfileTiming("start")
for i in range (100000):
panda.bullet_client.submitProfileTiming("full_step")
panda.step()
import pybullet as p
import time
p.connect(p.GUI)
useCollisionShapeQuery = True
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2])
baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0]) #[0,0.5,0.5,0]
basePositionB = [1.5, 0, 1]
obA = -1
obB = -1
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geomBox,
basePosition=basePositionB,
baseOrientation=baseOrientationB)
lineWidth = 3
colorRGB = [1, 0, 0]
lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
lineToXYZ=[0, 0, 0],
lineColorRGB=colorRGB,
lineWidth=lineWidth,
lifeTime=0)
pitch = 0
yaw = 0
while (p.isConnected()):
pitch += 0.01
if (pitch >= 3.1415 * 2.):
p.connect(p.GUI)
amplitudeId= p.addUserDebugParameter("amplitude",0,3.14,0.5)
timeScaleId = p.addUserDebugParameter("timeScale",0,10,1)
jointTypeNames={}
jointTypeNames[p.JOINT_REVOLUTE]="JOINT_REVOLUTE"
jointTypeNames[p.JOINT_FIXED]="JOINT_FIXED"
p.setPhysicsEngineParameter(numSolverIterations=100)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
jointNamesToIndex={}
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
for j in range(p.getNumJoints(vision)):
jointInfo = p.getJointInfo(vision,j)
jointInfoName = jointInfo[1].decode("utf-8")
print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]])
jointNamesToIndex[jointInfoName ]=j
def build_scene(
floor_textures=FLOOR_TEXTURES,
wall_textures=WALL_TEXTURES,
fix_light_position=False,
gui=False):
"""
Builds the scene
"""
if gui:
physics_client = pybullet.connect(pybullet.GUI)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,
0,
physicsClientId=physics_client)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,
0,
physicsClientId=physics_client)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,
0,
physicsClientId=physics_client)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_SHADOWS,
1,
physicsClientId=physics_client)
else:
physics_client = pybullet.connect(pybullet.DIRECT)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_SHADOWS,
1,
physicsClientId=physics_client)
# Add current folder path to the bullet research path
pybullet.setAdditionalSearchPath(
os.path.dirname(os.path.realpath(__file__)))
# load the floor
floorId = pybullet.loadURDF("urdf/floor.urdf",
globalScaling=1,
basePosition=[0., 0., 0.],
baseOrientation=pybullet.getQuaternionFromEuler([0, 0, 0]),
useFixedBase=1)
floor_texture = pybullet.loadTexture(np.random.choice(floor_textures))
pybullet.changeVisualShape(floorId, -1, textureUniqueId=floor_texture)
# load the walls
wallsId = pybullet.loadURDF("urdf/walls.urdf",
globalScaling=1,
basePosition=[0., 0., 0.],
baseOrientation=pybullet.getQuaternionFromEuler([0, 0, 0]),
useFixedBase=1)
wall_texture = pybullet.loadTexture(np.random.choice(wall_textures))
pybullet.changeVisualShape(wallsId, -1, textureUniqueId=wall_texture)
# Light position
if fix_light_position:
translation = [1, -1, 1.6]
else:
xz = np.random.uniform(-1, 1, size=2)
translation = np.array([xz[0], 1, xz[1]])
pybullet.configureDebugVisualizer(
lightPosition=translation,
physicsClientId=physics_client)
import pybullet as p
import pybullet_data_local as pd
import math
import time
import numpy as np
from pybullet_envs_local.examples import panda_sim
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
p.setAdditionalSearchPath(pd.getDataPath())
timeStep = 1. / 60.
p.setTimeStep(timeStep)
p.setGravity(0, -9.8, 0)
panda = panda_sim.PandaSim(p, [0, 0, 0])
while (1):
panda.step()
p.stepSimulation()
time.sleep(timeStep)
rcs_rear = 9
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setTimeStep(1 / timestep)
p.setRealTimeSimulation(realTime)
p.setGravity(0, 0, -9.81)
planeId = p.loadURDF("plane.urdf")
rocketStartPos = [r.randrange(-20, 20, 1), 0, 20]
rocketStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
rocket = p.loadURDF("rocketwithrcs.urdf", rocketStartPos,
rocketStartOrientation)
p.setRealTimeSimulation(0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, syntheticCamera)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,
syntheticCamera)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, syntheticCamera)
heightPID = PID.PID(0.5, 0, 3, 0, 1)
pitchPID = PID.PID(1, 0.02, 2, -0.2, 0.2)
XPID = PID.PID(0.01, 0, 0.02, -1, 1)
#Debug
drymass = p.getDynamicsInfo(rocket, -1)[0] + p.getDynamicsInfo(
rocket, 1)[0] + p.getDynamicsInfo(rocket, 2)[0] * 4
throttleInput = p.addUserDebugParameter("Throttle Position", 0.4, 1, 0.4)
nozzleInput = p.addUserDebugParameter("Nozzle Position", -0.2, 0.2, 0)
import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
useMaximalCoordinates = False
p.setGravity(0, 0, -10)
plane = p.loadURDF("plane.urdf", [0, 0, -1],
useMaximalCoordinates=useMaximalCoordinates)
p.setRealTimeSimulation(0)
velocity = 1
num = 40
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1) #disable this to make it faster
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
p.setPhysicsEngineParameter(enableConeFriction=1)
for i in range(num):
print("progress:", i, num)
x = velocity * math.sin(2. * 3.1415 * float(i) / num)
y = velocity * math.cos(2. * 3.1415 * float(i) / num)
print("velocity=", x, y)
sphere = p.loadURDF("sphere_small_zeroinertia.urdf",
flags=p.URDF_USE_INERTIA_FROM_FILE,
useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphere, -1, lateralFriction=0.02)
import pybullet as p
import time
import math
useGui = True
if (useGui):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.loadURDF("samurai.urdf")
p.loadURDF("r2d2.urdf", [3, 3, 1])
rayFrom = []
rayTo = []
rayIds = []
numRays = 1024
rayLen = 13
rayHitColor = [1, 0, 0]
rayMissColor = [0, 1, 0]
replaceLines = True
for i in range(numRays):
import pybullet as p
import pybullet_data
from time import sleep, time
from mathutils import Vector #https://github.com/majimboo/py-mathutils
from math import sqrt, pi
from numpy import cross, diagonal
import numpy as np
from scipy.spatial import ConvexHull, distance
from pyquaternion import Quaternion
#physicsClient = p.connect(p.DIRECT) #p.DIRECT for non-graphical version
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
# This is to change the visualizer window settings
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, enable=0)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, enable=0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, enable=0)
# change init camera distance/location to view scene
p.resetDebugVisualizerCamera(cameraDistance=.5, cameraYaw=45, cameraPitch=-20, cameraTargetPosition=[0.0, 0.0, 0.0])
rID = p.loadURDF("/Users/carlyndougherty/PycharmProjects/cr_grasper/RobotURDFs/barrett_hand_description/urdf/bh.urdf", basePosition=[0,0,-.1], globalScaling=1, useFixedBase=True)
oID = p.loadURDF("/Users/carlyndougherty/PycharmProjects/cr_grasper/ObjectURDFs/cube/cube_small.urdf", globalScaling=1, useFixedBase=True)
p.addUserDebugLine([0, 0, 0], [.2, 0, 0], [1, 0, 0], parentObjectUniqueId=rID, parentLinkIndex=-1, lineWidth=500)
p.addUserDebugLine([0, 0, 0], [0, .2, 0], [0, 1, 0], parentObjectUniqueId=rID, parentLinkIndex=-1, lineWidth=500)
p.addUserDebugLine([0, 0, 0], [0, 0, .2], [0, 0, 1], parentObjectUniqueId=rID, parentLinkIndex=-1, lineWidth=500)
active_grasp_joints = [3,6,9]
max_grasp_force = 100
target_grasp_velocity = .7
def __init__(self, robot_desc_file, ee_joint_name, ee_link_name,
control_mode, base_pos, rest_arm_qpos=None,
left_ee_joint_name=None, left_ee_link_name=None,
left_fing_link_prefix=None, left_joint_suffix=None,
left_rest_arm_qpos=None, output_noise_var=0.0,
dt=1.0/240.0, kp=1.0, kd=0.1, min_z=0.0,
visualize=False, cam_dist=1.5, cam_yaw=25, cam_pitch=-35,
cam_target=(0.5, 0, 0), debug_level=0):
assert(control_mode in
('ee_position', 'position', 'velocity', 'torque'))
self.control_mode = control_mode
# Variance of normal noise in the output action of the manipulator
# Noise is proportional to max allowed value for each action
self.output_noise_var = output_noise_var
self.dt = dt; self.kp = kp; self.kd = kd; self.min_z = min_z
self.debug_level = debug_level
self.visualize = visualize; self.cam_dist = cam_dist
self.cam_yaw = cam_yaw; self.cam_pitch = cam_pitch
self.cam_target = list(cam_target)
# Create and connect bullet simulation client.
if visualize:
self.sim = bclient.BulletClient(connection_mode=pybullet.GUI)
# disable aux menus in the gui
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0)
# don't render during init
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING,0)
else:
self.sim = bclient.BulletClient(connection_mode=pybullet.DIRECT)
self._aux_sim = bclient.BulletClient(
connection_mode=pybullet.DIRECT)
# Load ground.
self.sim.setAdditionalSearchPath(pybullet_data.getDataPath())
self.plane_id = self.sim.loadURDF("plane.urdf",[0,0,0])
self._aux_sim.setAdditionalSearchPath(pybullet_data.getDataPath())
self._aux_sim.loadURDF("plane.urdf", [0, 0, 0])
# Load robot from URDF.
if not os.path.isabs(robot_desc_file):
robot_description_folder = os.path.split(__file__)[0]
data_path = os.path.join(robot_description_folder, 'data')
robot_desc_file = os.path.join(data_path, robot_desc_file)
self.info = self.load_robot(
robot_desc_file, ee_joint_name, ee_link_name,
left_ee_joint_name, left_ee_link_name,
left_fing_link_prefix, left_joint_suffix,
base_pos=base_pos, base_quat=[0,0,0,1])
# Set simulation parameters.
# time step: https://github.com/bulletphysics/bullet3/issues/1460
self.sim.setRealTimeSimulation(0)
self.sim.setGravity(0, 0, BulletManipulator.GRAVITY)
self.sim.setPhysicsEngineParameter(
fixedTimeStep=self.dt, numSolverIterations=5, numSubSteps=2)
# Init aux sim.
self._aux_sim.setRealTimeSimulation(0)
self._aux_sim.setGravity(0, 0, BulletManipulator.GRAVITY)
self._aux_sim.setPhysicsEngineParameter(
fixedTimeStep=self.dt, numSolverIterations=5, numSubSteps=2)
# Turn on FT sensors (for model-free gravity compensation compute).
#BulletManipulatorSim.toggle_ft_sensors(self.sim, self.robots, on=True)
# Reset to initial position and visualize.
self.rest_qpos = (self.info.joint_maxpos+self.info.joint_minpos)/2
if rest_arm_qpos is not None:
assert(len(self.info.arm_jids_lst)==len(rest_arm_qpos))
self.rest_qpos[self.info.arm_jids_lst] = rest_arm_qpos[:]
if left_rest_arm_qpos is not None:
assert(len(self.info.left_arm_jids_lst)==len(left_rest_arm_qpos))
self.rest_qpos[self.info.left_arm_jids_lst] = left_rest_arm_qpos[:]
self.reset()
if self.visualize:
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING,1)
self.refresh_viz()
def remove_created_bodies(self, list_ids, n_bodies):
for i in range(n_bodies):
p.removeBody(list_ids[i])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
def main():
pixelWidth, pixelHeight = 1920 // 2, 1080 // 2
window_name = 'frame'
scn = Scene(pixelHeight, pixelWidth, window_name)
# VTK STUFF
sphere = vtk.vtkSphereSource()
sphere.SetRadius(0.01 / 2)
mapper = vtk.vtkPolyDataMapper()
if vtk.VTK_MAJOR_VERSION <= 5:
mapper.SetInput(sphere.GetOutput())
else:
mapper.SetInputConnection(sphere.GetOutputPort())
actors = {}
poses = {}
dataDir = '/home/biomed/Art/bullet3/data'
#p.loadPlugin("eglRendererPlugin")
p.loadURDF(os.path.join(dataDir, "plane.urdf"), [0, 0, .4])
# p.loadURDF(os.path.join(dataDir, "r2d2.urdf"))
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
ob = {}
count = 0
for i in range(5):
for j in range(5):
for k in range(5):
ob[count] = p.loadURDF(os.path.join(
dataDir, "sphere_1cm.urdf"), [
0.02 * i + np.random.random_sample(1) * 0.02,
0.02 * j + np.random.random_sample(1) * 0.01, 0.02 * k
],
useMaximalCoordinates=True)
actors[count] = vtk.vtkActor()
actors[count].SetMapper(mapper)
scn.ren.AddActor(actors[count])
count = count + 1
main_start = time.time()
cap = cv2.VideoCapture(0)
res = set_res(cap, 1280 // 2, 960 // 2)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
count = 0
scn.start()
while True:
ret, frame = cap.read()
for k in actors.keys():
pos, rot = p.getBasePositionAndOrientation(ob[k])
actors[k].SetPosition(pos)
cv2.imshow(window_name, scn.render(count % 2))
count = count + 1
if cv2.waitKey(1) & 0xFF == ord('q'):
play = False
break
stop = time.time()
scn.stop()
cap.release()
cv2.destroyAllWindows()
p.resetSimulation()
import pybullet as p
import math
import time
dt = 1. / 240.
p.connect(p.SHARED_MEMORY_GUI)
p.loadURDF("r2d2.urdf", [0, 0, 1])
p.loadURDF("plane.urdf")
p.setGravity(0, 0, -10)
radius = 5
t = 0
p.configureDebugVisualizer(shadowMapWorldSize=5)
p.configureDebugVisualizer(shadowMapResolution=8192)
while (1):
t += dt
p.configureDebugVisualizer(
lightPosition=[radius * math.sin(t), radius * math.cos(t), 3])
p.stepSimulation()
time.sleep(dt)
def reset(self):
self.step_counter = 0
p.resetSimulation()
p.configureDebugVisualizer(
p.COV_ENABLE_RENDERING,
0) # we will enable rendering after we loaded everything
p.resetDebugVisualizerCamera(cameraDistance=3,
cameraYaw=30,
cameraPitch=-32,
cameraTargetPosition=[0, 0, 0])
urdfRootPath = pybullet_data.getDataPath()
p.setGravity(0, 0, -10)
planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"),
basePosition=[0, 0, 0])
rest_poses = [0, -0.2, 0, -1.2, 0, 0, 0]
radius = 0.8
self.arm1Uid = p.loadURDF(os.path.join(urdfRootPath,
"kuka_iiwa/model.urdf"),
useFixedBase=True,
basePosition=[radius, 0, 0],
baseOrientation=[0, 0, 1, 0])
self.arm2Uid = p.loadURDF(
os.path.join(urdfRootPath, "kuka_iiwa/model.urdf"),
useFixedBase=True,
basePosition=[-radius * 0.5, radius * 0.866, 0],
baseOrientation=[0, 0, 0.5, -0.8660254])
self.arm3Uid = p.loadURDF(
os.path.join(urdfRootPath, "kuka_iiwa/model.urdf"),
useFixedBase=True,
basePosition=[-radius * 0.5, -radius * 0.866, 0],
baseOrientation=[0, 0, -0.5, -0.8660254])
self.nJointsPerArm = p.getNumJoints(self.arm1Uid)
for i in range(7):
p.resetJointState(self.arm1Uid, i, rest_poses[i])
p.resetJointState(self.arm2Uid, i, rest_poses[i])
p.resetJointState(self.arm3Uid, i, rest_poses[i])
#we need to first set force limit to zero to use torque control!!
#see: https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_dynamics.py
p.setJointMotorControlArray(self.arm1Uid,
range(self.nJointsPerArm),
p.VELOCITY_CONTROL,
forces=np.zeros(self.nJointsPerArm))
p.setJointMotorControlArray(self.arm2Uid,
range(self.nJointsPerArm),
p.VELOCITY_CONTROL,
forces=np.zeros(self.nJointsPerArm))
p.setJointMotorControlArray(self.arm3Uid,
range(self.nJointsPerArm),
p.VELOCITY_CONTROL,
forces=np.zeros(self.nJointsPerArm))
#create a base
baseUid = p.loadURDF(os.path.join(urdfRootPath,
"table_square/table_square.urdf"),
useFixedBase=True)
#create a box
state_object = [
np.random.uniform(-0.1, 0.1),
np.random.uniform(-0.1, 0.1), 1.0
]
half_ext = 0.35
cuid = p.createCollisionShape(p.GEOM_BOX, halfExtents=[half_ext] * 3)
vuid = p.createVisualShape(p.GEOM_BOX,
halfExtents=[half_ext] * 3,
rgbaColor=[0, 0, 1, 0.8])
mass_box = 0.5
self.objectUid = p.createMultiBody(mass_box,
cuid,
vuid,
basePosition=state_object)
p.changeDynamics(self.objectUid,
-1,
lateralFriction=2.0,
spinningFriction=0.8,
rollingFriction=0.8)
#get observations, only joint positions are considered
arm1_joint_state = getMotorJointStates(self.arm1Uid)
arm2_joint_state = getMotorJointStates(self.arm2Uid)
arm3_joint_state = getMotorJointStates(self.arm3Uid)
arm1_joint_pos = arm1_joint_state[0]
arm2_joint_pos = arm2_joint_state[0]
arm3_joint_pos = arm3_joint_state[0]
self.observation = np.concatenate(
(arm1_joint_pos, arm2_joint_pos, arm3_joint_pos))
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
return np.array(self.observation).astype(np.float32)
def reset(self):
super(FeedingEnv, self).reset()
self.build_assistive_env('wheelchair')
self.furniture.set_on_ground()
if self.robot.wheelchair_mounted:
wheelchair_pos, wheelchair_orient = self.furniture.get_base_pos_orient(
)
self.robot.set_base_pos_orient(
wheelchair_pos +
np.array(self.robot.toc_base_pos_offset[self.task]),
[0, 0, -np.pi / 2.0])
if self.general_model:
# Randomize the human body shape
gender = self.np_random.choice(['male', 'female'])
# body_shape = self.np_random.randn(1, self.human.num_body_shape)
body_shape = self.np_random.uniform(-2, 5,
(1, self.human.num_body_shape))
# human_height = self.np_random.uniform(1.59, 1.91) if gender == 'male' else self.np_random.uniform(1.47, 1.78)
human_height = self.np_random.uniform(1.5, 1.9)
else:
gender = self.gender
body_shape = self.body_shape_filename
human_height = self.human_height
# Randomize human pose
joint_angles = [(self.human.j_left_hip_x, -90),
(self.human.j_right_hip_x, -90),
(self.human.j_left_knee_x, 70),
(self.human.j_right_knee_x, 70),
(self.human.j_left_shoulder_z, -45),
(self.human.j_right_shoulder_z, 45),
(self.human.j_left_elbow_y, -90),
(self.human.j_right_elbow_y, 90)]
# u = self.np_random.uniform
# joint_angles += [(self.human.j_waist_x, u(-30, 45)), (self.human.j_waist_y, u(-45, 45)), (self.human.j_waist_z, u(-30, 30)), (self.human.j_lower_neck_x, u(-30, 30)), (self.human.j_lower_neck_y, u(-30, 30)), (self.human.j_lower_neck_z, u(-10, 10)), (self.human.j_upper_neck_x, u(-45, 45)), (self.human.j_upper_neck_y, u(-30, 30)), (self.human.j_upper_neck_z, u(-30, 30))]
# joint_angles += [(self.human.j_waist_x, u(-20, 30)), (self.human.j_waist_y, u(-45, 0)), (self.human.j_waist_z, u(0, 30)), (self.human.j_lower_neck_x, u(-30, 30)), (self.human.j_lower_neck_y, u(-30, 30)), (self.human.j_lower_neck_z, u(-10, 10)), (self.human.j_upper_neck_x, u(-30, 30)), (self.human.j_upper_neck_y, u(-30, 30)), (self.human.j_upper_neck_z, u(-30, 30))]
joint_angles += [
(j, self.np_random.uniform(-10, 10))
for j in (self.human.j_waist_x, self.human.j_waist_y,
self.human.j_waist_z, self.human.j_lower_neck_x,
self.human.j_lower_neck_y, self.human.j_lower_neck_z,
self.human.j_upper_neck_x, self.human.j_upper_neck_y,
self.human.j_upper_neck_z)
]
self.human.init(self.directory,
self.id,
self.np_random,
gender=gender,
height=human_height,
body_shape=body_shape,
joint_angles=joint_angles,
position=[0, 0, 0],
orientation=[0, 0, 0])
# Place human in chair
chair_seat_position = np.array([0, 0.05, 0.6])
self.human.set_base_pos_orient(
self.furniture.get_base_pos_orient()[0] + chair_seat_position -
self.human.get_vertex_positions(self.human.bottom_index),
[0, 0, 0, 1])
# Create a table
self.table = Furniture()
self.table.init('table', self.directory, self.id, self.np_random)
self.generate_target()
p.resetDebugVisualizerCamera(cameraDistance=1.10,
cameraYaw=40,
cameraPitch=-45,
cameraTargetPosition=[-0.2, 0, 0.75],
physicsClientId=self.id)
# Open gripper to hold the tool
self.robot.set_gripper_open_position(self.robot.right_gripper_indices,
self.robot.gripper_pos[self.task],
set_instantly=True)
# Initialize the tool in the robot's gripper
self.tool.init(self.robot,
self.task,
self.directory,
self.id,
self.np_random,
right=True,
mesh_scale=[0.08] * 3)
target_ee_orient = np.array(
p.getQuaternionFromEuler(np.array(
self.robot.toc_ee_orient_rpy[self.task]),
physicsClientId=self.id))
while True:
# Continually resample initial end effector poses until we find one where the robot isn't colliding with the person
# target_ee_pos = np.array([-0.1, -0.6, 1.2]) + np.array([self.np_random.uniform(-0.1, 0.1), self.np_random.uniform(-0.2, 0.3), self.np_random.uniform(-0.1, 0.1)])
mouth_pos = self.human.get_pos_orient(self.human.mouth)[0]
target_ee_pos = mouth_pos + np.array([
self.np_random.uniform(-0.3, 0),
self.np_random.uniform(-0.6, -0.3),
self.np_random.uniform(-0.3, 0.1)
])
if self.robot.mobile:
# Randomize robot base pose
pos = np.array(self.robot.toc_base_pos_offset[self.task])
pos[:2] += self.np_random.uniform(-0.1, 0.1, size=2)
orient = np.array(self.robot.toc_ee_orient_rpy[self.task])
orient[2] += self.np_random.uniform(-np.deg2rad(30),
np.deg2rad(30))
self.robot.set_base_pos_orient(pos, orient)
# Randomize starting joint angles
self.robot.set_joint_angles(
[3], [0.75 + self.np_random.uniform(-0.1, 0.1)])
# Randomly set friction of the ground
self.plane.set_frictions(
self.plane.base,
lateral_friction=self.np_random.uniform(0.025, 0.5),
spinning_friction=0,
rolling_friction=0)
elif self.robot.wheelchair_mounted:
# Use IK to find starting joint angles for mounted robots
self.robot.ik_random_restarts(right=True,
target_pos=target_ee_pos,
target_orient=target_ee_orient,
max_iterations=1000,
max_ik_random_restarts=40,
success_threshold=0.03,
step_sim=False,
check_env_collisions=False)
else:
# Use TOC with JLWKI to find an optimal base position for the robot near the person
self.robot.position_robot_toc(
self.task,
'right', [(target_ee_pos, target_ee_orient)],
[(self.target_pos, None)],
self.human,
step_sim=False,
check_env_collisions=False,
attempts=50)
# Check if the robot is colliding with the person or table
self.tool.reset_pos_orient()
_, _, _, _, dists_human = self.robot.get_closest_points(self.human,
distance=0)
_, _, _, _, dists_table = self.robot.get_closest_points(self.table,
distance=0)
_, _, _, _, dists_tool = self.tool.get_closest_points(self.human,
distance=0)
if not dists_human and not dists_table and not dists_tool:
break
# Open gripper to hold the tool
self.robot.set_gripper_open_position(self.robot.right_gripper_indices,
self.robot.gripper_pos[self.task],
set_instantly=True)
# Initialize the tool in the robot's gripper
# self.tool.init(self.robot, self.task, self.directory, self.id, self.np_random, right=True, mesh_scale=[0.08]*3)
# Place a bowl on a table
self.bowl = Furniture()
self.bowl.init('bowl', self.directory, self.id, self.np_random)
if not self.robot.mobile:
self.robot.set_gravity(0, 0, 0)
self.human.set_gravity(0, 0, 0)
self.tool.set_gravity(0, 0, 0)
p.setPhysicsEngineParameter(numSubSteps=4,
numSolverIterations=10,
physicsClientId=self.id)
# Generate food
spoon_pos, spoon_orient = self.tool.get_base_pos_orient()
food_radius = 0.005
food_mass = 0.001
batch_positions = []
for i in range(2):
for j in range(2):
for k in range(2):
batch_positions.append(
np.array([
i * 2 * food_radius - 0.005, j * 2 *
food_radius, k * 2 * food_radius + 0.01
]) + spoon_pos)
self.foods = self.create_spheres(radius=food_radius,
mass=food_mass,
batch_positions=batch_positions,
visual=False,
collision=True)
colors = [[60. / 256., 186. / 256., 84. / 256., 1],
[244. / 256., 194. / 256., 13. / 256., 1],
[219. / 256., 50. / 256., 54. / 256., 1],
[72. / 256., 133. / 256., 237. / 256., 1]]
for i, f in enumerate(self.foods):
p.changeVisualShape(f.body,
-1,
rgbaColor=colors[i % len(colors)],
physicsClientId=self.id)
self.total_food_count = len(self.foods)
self.foods_active = [f for f in self.foods]
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
# Drop food in the spoon
for _ in range(25):
p.stepSimulation(physicsClientId=self.id)
self.init_env_variables()
return self._get_obs()
def birrt_smoothing():
################################################################
# TODO your code to implement the birrt algorithm with smoothing
################################################################
pass
if __name__ == "__main__":
args = get_args()
# set up simulator
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(enableFileCaching=0)
p.setGravity(0, 0, -9.8)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, False)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, True)
p.resetDebugVisualizerCamera(cameraDistance=1.400,
cameraYaw=58.000,
cameraPitch=-42.200,
cameraTargetPosition=(0.0, 0.0, 0.0))
# load objects
plane = p.loadURDF("plane.urdf")
ur5 = p.loadURDF('assets/ur5/ur5.urdf',
basePosition=[0, 0, 0.02],
useFixedBase=True)
obstacle1 = p.loadURDF('assets/block.urdf',
basePosition=[1 / 4, 0, 1 / 2],
useFixedBase=True)
obstacle2 = p.loadURDF('assets/block.urdf',
def reset(self):
self.step_counter = 0
p.resetSimulation()
p.configureDebugVisualizer(
p.COV_ENABLE_RENDERING,
0) # we will enable rendering after we loaded everything
p.resetDebugVisualizerCamera(cameraDistance=3,
cameraYaw=30,
cameraPitch=-32,
cameraTargetPosition=[0, 0, 0])
urdfRootPath = pybullet_data.getDataPath()
p.setGravity(0, 0, -10)
planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"),
basePosition=[0, 0, 0])
rest_poses = [0, 0, 0, 0, 0, 0, 0]
radius = 0.5
self.agent1Uid = p.loadURDF(os.path.join(urdfRootPath,
"sphere_small.urdf"),
useFixedBase=False,
basePosition=[radius, 0, 0.8])
self.agent2Uid = p.loadURDF(
os.path.join(urdfRootPath, "sphere_small.urdf"),
useFixedBase=False,
basePosition=[-radius * 0.5, radius * 0.866, 0.8])
self.agent3Uid = p.loadURDF(
os.path.join(urdfRootPath, "sphere_small.urdf"),
useFixedBase=False,
basePosition=[-radius * 0.5, -radius * 0.866, 0.8])
p.changeDynamics(self.agent1Uid,
-1,
lateralFriction=2.0,
spinningFriction=0.8,
rollingFriction=0.8)
p.changeDynamics(self.agent2Uid,
-1,
lateralFriction=2.0,
spinningFriction=0.8,
rollingFriction=0.8)
p.changeDynamics(self.agent3Uid,
-1,
lateralFriction=2.0,
spinningFriction=0.8,
rollingFriction=0.8)
#create a base
baseUid = p.loadURDF(os.path.join(urdfRootPath,
"table_square/table_square.urdf"),
useFixedBase=True)
#create a box
state_object = [
np.random.uniform(-0.1, 0.1),
np.random.uniform(-0.1, 0.1), 1.0
]
half_ext = 0.35
cuid = p.createCollisionShape(p.GEOM_BOX, halfExtents=[half_ext] * 3)
vuid = p.createVisualShape(p.GEOM_BOX,
halfExtents=[half_ext] * 3,
rgbaColor=[0, 0, 1, 0.8])
mass_box = 0.5
self.objectUid = p.createMultiBody(mass_box,
cuid,
vuid,
basePosition=state_object)
p.changeDynamics(self.objectUid,
-1,
lateralFriction=2.0,
spinningFriction=0.8,
rollingFriction=0.8)
agent1_pose = np.concatenate(
p.getBasePositionAndOrientation(self.agent1Uid))
agent2_pose = np.concatenate(
p.getBasePositionAndOrientation(self.agent2Uid))
agent3_pose = np.concatenate(
p.getBasePositionAndOrientation(self.agent3Uid))
self.observation = np.concatenate(
[agent1_pose, agent2_pose, agent3_pose])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
return np.array(self.observation).astype(np.float32)
def __init__(
self,
use_GUI=False, # False
gravity=-9.81, # -9.81
tracking_cam=True, # True
timestep=1. / 60, # 1./240
decision_interval=.25, # 1.0,
center_link_mass=None, # None
outer_link_mass=None, # None
wheel_mass=None, # None
rolling_friction=None, # None
lateral_fricition=None, # None
spinning_fricition=None, # None
friction_anchor=None, # 1
wheel_restitution=None, # 0
a_upper=7 * np.pi / 18, # np.pi/2 # limits for robot 7*np.pi/18
a_lower=-7 * np.pi / 18): # -np.pi/2 # -7*np.pi/18
self.tracking_cam = tracking_cam
self.timestep = timestep
self.decision_interval = decision_interval
self.a_upper = a_upper
self.a_lower = a_lower
# initalize pybullet simulation enviroment,
if use_GUI:
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
else:
p.connect(p.DIRECT)
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Load "floor", otherwise unfixed based will "fall through plane"
p.loadURDF("plane.urdf")
p.createCollisionShape(p.GEOM_PLANE)
# Define starting position and orientation for robot
StartingOrientation = p.getQuaternionFromEuler([0, 0, 0])
StartingPosition = [1, 1, .03] # x,y,z
# Load robot URDF file created in solidworks
self.robot = p.loadURDF(
r'C:\Users\Jesse\Desktop\DeepRobots\Snakebot_PyBullet\Snakebot_urdf.SLDASM\Snakebot_urdf.SLDASM\urdf\Snakebot_urdf.SLDASM.urdf',
StartingPosition,
StartingOrientation,
useFixedBase=0)
pf.color_code_joints(self.robot)
# Modify default robot model parameters
if center_link_mass is not None:
p.changeDynamics(self.robot, linkIndex=-1, mass=center_link_mass)
if outer_link_mass is not None:
p.changeDynamics(self.robot, linkIndex=0, mass=outer_link_mass)
p.changeDynamics(self.robot, linkIndex=3, mass=outer_link_mass)
if wheel_mass is not None:
pf.add_wheels_mass(self.robot, wheel_mass=wheel_mass)
if rolling_friction is not None:
pf.add_rolling_friction(self.robot, friction=rolling_friction)
if lateral_fricition is not None:
pf.add_lateral_friction(self.robot, friction=lateral_fricition)
if spinning_fricition is not None:
pf.add_spinning_friction(self.robot, friction=spinning_fricition)
if friction_anchor is not None:
pf.add_friction_anchor(self.robot, friction_anchor=friction_anchor)
if wheel_restitution is not None:
pf.add_wheel_restitution(self.robot,
wheel_restitution=wheel_restitution)
# Initalize Physics Engine parameters (always after modifiying robot parameters)
p.setGravity(0, 0, gravity)
p.setTimeStep(timestep)
p.setRealTimeSimulation(1) # 1 = True, 0 = False
p.setPhysicsEngineParameter(fixedTimeStep=timestep,
numSolverIterations=1,
useSplitImpulse=1,
numSubSteps=1,
enableConeFriction=1,
collisionFilterMode=0)
# Get current state of robot
self.update_system_params()
self._set_init_system_params()
self.theta_displacement = 0 # for theta reward functions
self.a1dot = 0
self.a2dot = 0
self.state = (self.theta, self.a1, self.a2)
import math
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4,
minimumSolverIslandSize=1024)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,
"createMultiBodyBatch.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000, -1.000000,
-1.000000], [1.000000, -1.000000, -1.000000],
[1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, -1.000000, -1.000000],
[-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
def __init__(self, gui):
self.gui = gui
self.paused = False
self.gravity = True
# config values
self.ball_start_position = [0.5, 0, 0.05]
self.ball_start_orientation = p.getQuaternionFromEuler((0, 0.25, 0))
self.start_position = [0, 0, 0.43]
self.start_orientation = p.getQuaternionFromEuler((0, 0.25, 0))
self.initial_joints_positions = {
"LAnklePitch": -30,
"LAnkleRoll": 0,
"LHipPitch": 30,
"LHipRoll": 0,
"LHipYaw": 0,
"LKnee": 60,
"RAnklePitch": 30,
"RAnkleRoll": 0,
"RHipPitch": -30,
"RHipRoll": 0,
"RHipYaw": 0,
"RKnee": -60,
"LShoulderPitch": 0,
"LShoulderRoll": 0,
"LElbow": 45,
"RShoulderPitch": 0,
"RShoulderRoll": 0,
"RElbow": -45,
"HeadPan": 0,
"HeadTilt": 0
}
# Instantiating Bullet
if self.gui:
self.client_id = p.connect(p.GUI)
else:
self.client_id = p.connect(p.DIRECT)
p.setGravity(0, 0, -9.81)
self.time = 0
# disable debug interface, only show robot
p.configureDebugVisualizer(p.COV_ENABLE_GUI, False)
# Loading floor
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
self.plane_index = p.loadURDF('plane.urdf')
p.changeDynamics(self.plane_index,
-1,
lateralFriction=1,
spinningFriction=-1,
rollingFriction=-1,
restitution=0.9)
# Loading ball
rospack = rospkg.RosPack()
self.ball_index = p.loadURDF(
rospack.get_path("bitbots_throw") + '/urdf/ball.urdf',
self.ball_start_position, self.ball_start_orientation)
# Loading robot
path = rospack.get_path("wolfgang_description")
flags = p.URDF_USE_INERTIA_FROM_FILE + p.URDF_USE_SELF_COLLISION + p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
self.robot_index = p.loadURDF(path + "/urdf/robot.urdf",
self.start_position,
self.start_orientation,
flags=flags)
# Engine parameters
# time step should be at 240Hz (due to pyBullet documentation)
self.timestep = 1 / 240
# standard parameters seem to be best. leave them like they are
# p.setPhysicsEngineParameter(fixedTimeStep=self.timestep, numSubSteps=1)
# no real time, as we will publish own clock
self.real_time = False
p.setRealTimeSimulation(0)
# Retrieving joints and foot pressure sensors
self.joints = {}
self.pressure_sensors = {}
self.links = {}
# Collecting the available joints
link_index = 0
for i in range(p.getNumJoints(self.robot_index)):
joint_info = p.getJointInfo(self.robot_index, i)
name = joint_info[1].decode('utf-8')
# we can get the links by seeing where the joint is attached
self.links[joint_info[12].decode('utf-8')] = link_index
link_index += 1
if name in self.initial_joints_positions.keys():
# remember joint
self.joints[name] = Joint(i, self.robot_index)
elif name in [
"LLB", "LLF", "LRF", "LRB", "RLB", "RLF", "RRF", "RRB"
]:
p.enableJointForceTorqueSensor(self.robot_index, i)
self.pressure_sensors[name] = PressureSensor(
name, i, self.robot_index, 10, 5)
# set friction for feet
self.set_foot_dynamics(0.0, 0.0, 0.0)
# reset robot to initial position
self.reset()
def __init__(self, gui):
self.gui = gui
self.paused = False
self.follow_camera = False
self.gravity = True
# config values
self.start_position = [0, 0, 0.43]
self.start_orientation = p.getQuaternionFromEuler((0, 0.13, 0))
self.initial_joints_positions = {"LAnklePitch": -26, "LAnkleRoll": 0, "LHipPitch": 26, "LHipRoll": 0,
"LHipYaw": 0, "LKnee": -60, "RAnklePitch": 26, "RAnkleRoll": 0,
"RHipPitch": -26, "RHipRoll": 0, "RHipYaw": 0, "RKnee": 60,
"LShoulderPitch": 0, "LShoulderRoll": 0, "LElbow": 45, "RShoulderPitch": 0,
"RShoulderRoll": 0, "RElbow": -45, "HeadPan": 0, "HeadTilt": 0}
# Instantiating Bullet
if self.gui:
self.client_id = p.connect(p.GUI)
else:
self.client_id = p.connect(p.SHARED_MEMORY_SERVER)
p.setGravity(0, 0, -9.81)
self.time = 0
# disable DEBUG interface, only show robot
p.configureDebugVisualizer(p.COV_ENABLE_GUI, False)
# set camera angle
p.resetDebugVisualizerCamera(cameraDistance=0.7, cameraYaw=0, cameraPitch=-10,
cameraTargetPosition=self.start_position)
# Loading floor
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
self.plane_index = p.loadURDF('plane.urdf')
p.changeDynamics(self.plane_index, -1, lateralFriction=100000000000000, spinningFriction=100000000000,
rollingFriction=0.1, restitution=0.9)
# Loading robot
flags = p.URDF_USE_INERTIA_FROM_FILE
self.robot_index = p.loadURDF("/urdf/robot.urdf",
self.start_position, self.start_orientation, flags=flags)
# Engine parameters
# time step should be at 240Hz (due to pyBullet documentation)
self.time_step = 1 / 240
# standard parameters seem to be best. leave them like they are
# p.setPhysicsEngineParameter(fixedTimeStep=self.time_step, numSubSteps=1)
# no real time, as we will publish own clock
p.setRealTimeSimulation(0)
# Retrieving joints and foot pressure sensors
self.joints = {}
self.pressure_sensors = {}
self.links = {}
# Collecting the available joints
for i in range(p.getNumJoints(self.robot_index)):
joint_info = p.getJointInfo(self.robot_index, i)
name = joint_info[1].decode('utf-8')
# we can get the links by seeing where the joint is attached
self.links[joint_info[12].decode('utf-8')] = joint_info[16]
if name in self.initial_joints_positions.keys():
# remember joint
self.joints[name] = Joint(i, self.robot_index)
elif name in ["LLB", "LLF", "LRF", "LRB", "RLB", "RLF", "RRF", "RRB"]:
self.pressure_sensors[name] = PressureSensor(name, i, self.robot_index)
# set friction for feet
for link_name in self.links.keys():
if link_name in ["l_foot", "r_foot", "llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb"]:
# print(self.parts[part].body_name)
p.changeDynamics(self.robot_index, self.links[link_name],
lateralFriction=10000000000000000,
spinningFriction=1000000000000000,
rollingFriction=0.1)
# reset robot to initial position
self.reset()
time.sleep(2)
rviz = subprocess.Popen('rviz')
time.sleep(2)
print("Initializing node... ")
rospy.init_node('bullet_point_cloud')
pcl_node = PointCloudPublisher()
#pcl_node.run_thread()
br = tf.TransformBroadcaster()
##########################################################
model_dir = os.getcwd() # Hack
pcid = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0, physicsClientId=pcid)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0, physicsClientId=pcid)
p.resetDebugVisualizerCamera(cameraDistance=1.2, cameraYaw=90, cameraPitch=-15,
cameraTargetPosition=[0.5, -0.35, 0.2], physicsClientId=pcid)
p.resetSimulation(physicsClientId=pcid)
p.setGravity(0, 0, -9.81, physicsClientId=pcid)
p.setPhysicsEngineParameter(numSolverIterations=1000, physicsClientId=pcid)
time_step = 1./200.
p.setTimeStep(time_step, physicsClientId=pcid)
p.loadURDF(model_dir + '/models/table/table.urdf', [0.5,0.0,-0.33],
useFixedBase=True, physicsClientId=pcid)
object_id = p.loadURDF(model_dir + '/models/object_models/urdf/bowl.urdf',
useFixedBase=False, physicsClientId=pcid)
import pybullet as p
import time
import math
p.connect(p.GUI)
#don't create a ground plane, to allow for gaps etc
p.resetSimulation()
#p.createCollisionShape(p.GEOM_PLANE)
#p.createMultiBody(0,0)
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
stoneId = p.createCollisionShape(p.GEOM_SPHERE)
boxHalfLength = 0.5
boxHalfWidth = 2.5
boxHalfHeight = 0.1
segmentLength = 5
colBoxId = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
mass = 1
visualShapeId = -1
segmentStart = 0
def __init__(
self,
display=True,
seed=None,
pos_noise=0.05,
rpy_noise=0.1,
num_rings=5,
ring_separation=5.,
):
# Create random number generator
self.rng = np.random.default_rng(seed)
# Choose the time step
self.dt = 0.01
# Create empty list of drones
self.drones = []
self.max_num_drones = 40
# Connect to and configure pybullet
self.display = display
if self.display:
pybullet.connect(pybullet.GUI)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
else:
pybullet.connect(pybullet.DIRECT)
pybullet.setGravity(0, 0, -9.81)
pybullet.setPhysicsEngineParameter(fixedTimeStep=self.dt,
numSubSteps=4,
restitutionVelocityThreshold=0.05,
enableFileCaching=0)
# Load plane
self.plane_id = pybullet.loadURDF(
os.path.join('.', 'urdf', 'plane.urdf'),
basePosition=np.array([0., 0., 0.]),
baseOrientation=pybullet.getQuaternionFromEuler([0., 0., 0.]),
useFixedBase=1)
# Load rings
self.num_rings = num_rings
self.ring_separation = ring_separation
self.rings = []
self.add_ring([0., 0., 0.25], [0., -np.pi / 2, 0.], 2.5, 0.5,
'big-ring.urdf')
for i in range(num_rings):
x = (i + 1) * ring_separation
y = self.rng.uniform(low=-5., high=5.)
z = self.rng.uniform(low=1., high=3.)
self.add_ring([x, y, z], [0., 0., 0.], 1., 0.25, 'ring.urdf')
self.add_ring([(num_rings + 1) * ring_separation, 0., 0.25],
[0., np.pi / 2, 0.], 2.5, 0.5, 'big-ring.urdf')
# # Eliminate linear and angular damping (a poor model of drag)
# pybullet.changeDynamics(self.robot_id, -1, linearDamping=0., angularDamping=0.)
# Set contact parameters
object_ids = [self.plane_id]
for ring in self.rings:
object_ids.append(ring['id'])
for object_id in object_ids:
pybullet.changeDynamics(object_id,
-1,
lateralFriction=1.0,
spinningFriction=0.0,
rollingFriction=0.0,
restitution=0.5,
contactDamping=-1,
contactStiffness=-1)
self.pos_noise = pos_noise
self.rpy_noise = rpy_noise
self.camera_drone_name = None
self.camera_drone_yaw = None
self.camera_viewfromstart = True
self.camera()
self.update_display()
self.l = 0.175
self.kF = 7e-6
self.kM = 1e-7
self.min_spin_rate = 100 # <-- rad/s
self.max_spin_rate = 900 # <-- rad/s
self.s_min = self.min_spin_rate**2
self.s_max = self.max_spin_rate**2
# motor[0]: front (+z spin)
# motor[1]: rear (+z spin)
# motor[2]: left (-z spin)
# motor[3]: right (-z spin)
self.M = linalg.inv(
np.array([[0., 0., self.kF * self.l, -self.kF * self.l],
[-self.kF * self.l, self.kF * self.l, 0., 0.],
[-self.kM, -self.kM, self.kM, self.kM],
[self.kF, self.kF, self.kF, self.kF]]))
def wrapped_load_func(*args, **kwargs):
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, False)
res = load_func(*args, **kwargs)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True)
return res
def reset(self):
super(ArmManipulationEnv, self).reset()
self.build_assistive_env('bed',
fixed_human_base=False,
human_impairment='no_tremor')
# Update robot and human motor gains
self.robot.motor_forces = 20.0
self.human.motor_forces = 2.0
self.furniture.set_friction(self.furniture.base, friction=5)
# Setup human in the air and let them settle into a resting pose on the bed
joints_positions = [(self.human.j_right_shoulder_x, 30)]
self.human.setup_joints(joints_positions,
use_static_joints=False,
reactive_force=None)
self.human.set_base_pos_orient([-0.25, 0.2, 0.95],
[-np.pi / 2.0, 0, 0])
p.setGravity(0, 0, -1, physicsClientId=self.id)
if not self.robot.mobile:
self.robot.set_gravity(0, 0, 0)
# Add small variation in human joint positions
motor_indices, motor_positions, motor_velocities, motor_torques = self.human.get_motor_joint_states(
)
self.human.set_joint_angles(
motor_indices,
self.np_random.uniform(-0.1, 0.1, size=len(motor_indices)))
# Let the person settle on the bed
for _ in range(100):
p.stepSimulation(physicsClientId=self.id)
self.furniture.set_friction(self.furniture.base, friction=0.3)
# Lock human joints and set velocities to 0
joints_positions = [(self.human.j_right_shoulder_x, 60),
(self.human.j_right_shoulder_y, -60),
(self.human.j_right_elbow, 0)]
self.human.setup_joints(joints_positions,
use_static_joints=True,
reactive_force=0.01)
self.human.set_mass(self.human.base, mass=0)
self.human.set_base_velocity(linear_velocity=[0, 0, 0],
angular_velocity=[0, 0, 0])
# Let the right arm fall to the ground
for _ in range(100):
p.stepSimulation(physicsClientId=self.id)
elbow_pos = self.human.get_pos_orient(self.human.right_elbow)[0]
wrist_pos = self.human.get_pos_orient(self.human.right_wrist)[0]
stomach_pos = self.human.get_pos_orient(self.human.stomach)[0]
waist_pos = self.human.get_pos_orient(self.human.waist)[0]
# Initialize the tool in the robot's gripper
self.tool_right.init(self.robot,
self.task,
self.directory,
self.id,
self.np_random,
right=True,
mesh_scale=[0.001] * 3)
if not self.robot.has_single_arm:
self.tool_left.init(self.robot,
self.task,
self.directory,
self.id,
self.np_random,
right=False,
mesh_scale=[0.001] * 3)
target_ee_right_pos = np.array([
-1, 0.4 if self.robot.has_single_arm else -0.3, 0.8
]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_ee_left_pos = np.array([-1, 0.7, 0.8]) + self.np_random.uniform(
-0.05, 0.05, size=3)
target_ee_orient = self.get_quaternion(
self.robot.toc_ee_orient_rpy[self.task])
if self.robot.has_single_arm:
base_position = self.init_robot_pose(
target_ee_right_pos,
target_ee_orient, [(target_ee_right_pos, target_ee_orient)],
[(wrist_pos, None), (waist_pos, None), (elbow_pos, None),
(stomach_pos, None)],
arm='right',
tools=[self.tool_right],
collision_objects=[self.human, self.furniture],
wheelchair_enabled=False)
else:
base_position = self.init_robot_pose(
target_ee_right_pos,
target_ee_orient, [[(target_ee_right_pos, target_ee_orient)],
[(target_ee_left_pos, target_ee_orient)]],
[[(wrist_pos, None),
(waist_pos, None)], [(elbow_pos, None),
(stomach_pos, None)]],
arm=['right', 'left'],
tools=[self.tool_right, self.tool_left],
collision_objects=[self.human, self.furniture],
wheelchair_enabled=False)
if self.robot.wheelchair_mounted:
# Load a nightstand in the environment for mounted arms
self.nightstand = Furniture()
self.nightstand.init('nightstand', self.directory, self.id,
self.np_random)
self.nightstand.set_base_pos_orient(
np.array([-1.2, 0.7, 0]) + base_position, [0, 0, 0, 1])
# Open gripper to hold the tools
self.robot.set_gripper_open_position(self.robot.right_gripper_indices,
self.robot.gripper_pos[self.task],
set_instantly=True)
if not self.robot.has_single_arm:
self.robot.set_gripper_open_position(
self.robot.left_gripper_indices,
self.robot.gripper_pos[self.task],
set_instantly=True)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
self.tool_right.set_gravity(0, 0, 0)
if not self.robot.has_single_arm:
self.tool_left.set_gravity(0, 0, 0)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
self.init_env_variables()
return self._get_obs()
def stop_recording(self):
p.stopStateLogging(self.rec_id)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
import pybullet as p
import time
import math
p.connect(p.GUI)
useMaximalCoordinates = False
p.setGravity(0,0,-10)
plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinates)
p.setRealTimeSimulation(0)
velocity = 1
num = 40
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.setPhysicsEngineParameter(enableConeFriction=1)
for i in range (num):
print("progress:",i,num)
x = velocity*math.sin(2.*3.1415*float(i)/num)
y = velocity*math.cos(2.*3.1415*float(i)/num)
print("velocity=",x,y)
sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphere,-1,lateralFriction=0.02)
#p.changeDynamics(sphere,-1,rollingFriction=10)
p.changeDynamics(sphere,-1,linearDamping=0)
p.changeDynamics(sphere,-1,angularDamping=0)
ls = p.getLinkState(jacoId, jacoEndEffectorIndex)
p.setGravity(0, 0, -10)
t = 0.
prevPose = [0, 0, 0]
prevPose1 = [0, 0, 0]
hasPrevPose = 0
useNullSpace = 0
useOrientation = 1
useSimulation = 1
useRealTimeSimulation = 1
ikSolver = 0
p.setRealTimeSimulation(useRealTimeSimulation)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
trailDuration = 5
pg.init()
os.environ['SDL_VIDEO_WINDOW_POS'] = "%d, %d" % (0, 0)
pg.display.set_mode((500, 500))
pg.display.set_caption("Control Interface")
runUI = UI(logData)
# positions and orientations of end-effector
pos = list(ls[4])
orn = list(ls[5])
# grab the initial positions of the end-effector for autonomous grasp
# grab the initial euler orientations of the end-effector for autonomous grasp
X_0 = pos[0]
# Panda config
panda_end_effector_index = 11 #8
panda_num_dofs = 7
base_orientation = [-0.707107, 0.0, 0.0, 0.707107]
base_offset = [0, 0, 0] # Base position offset [0,0,0] (meters)
# General PyBullet config
gui = True
use_simulation = False
# Startup PyBullet
if gui:
p.connect(p.GUI)
# p.connect(p.GUI, options="--opengl2")
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
else:
p.connect(p.DIRECT)
p.setAdditionalSearchPath(pd.getDataPath())
flags = p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES
# Set simulation time step
timeStep = 1. / 60.
p.setTimeStep(timeStep)
# Set gravity
p.setGravity(0, -9.81, 0)
import pybullet as p import json import time useGUI = True if useGUI: p.connect(p.GUI) else: p.connect(p.DIRECT) useZUp = False useYUp = not useZUp if useYUp: p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1) from pdControllerExplicit import PDControllerExplicitMultiDof from pdControllerStable import PDControllerStableMultiDof explicitPD = PDControllerExplicitMultiDof(p) stablePD = PDControllerStableMultiDof(p) p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) import pybullet_data p.setTimeOut(10000) useMotionCapture=False useMotionCaptureReset=False#not useMotionCapture useExplicitPD = True p.setAdditionalSearchPath(pybullet_data.getDataPath())
import pybullet as p
import numpy as np
sim = 4
physicsClient = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
# p.setPhysicsEngineParameter(numSolverIterations=10)
# p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
time_step = 0.005
p.setTimeStep(time_step)
p.setGravity(0, 0, -9.81)
# p.setGravity(0, 0, 0)
plane = p.loadURDF('plane.urdf')
pos1 = np.array([-0.5, 0, 0.5])
pos2 = np.array([0.5, 0, 0.5])
pos3 = np.array([0.5, 0, 1.5])
pos4 = np.array([-0.5, 0, 1.5])
gripper_visual = p.createVisualShape(p.GEOM_SPHERE,
radius=0.02,
rgbaColor=[1, 0, 0, 1])
gripper_collision = p.createCollisionShape(p.GEOM_SPHERE, radius=0.0001)
# gripper_collision = -1
mass = 1.0 if sim in [3, 4] else 0.0
cloth_attachment1 = p.createMultiBody(
baseMass=mass,
import pybullet as p
import time
import math
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
def __init__(self, time_step: SimulationTime, gui: bool = False):
self._gui = gui
if self._INSTANCE is not None and p.isConnected(self._INSTANCE._physics_client_id):
# Instance could already exist when running test, just to be sure resetting it here.
p.resetSimulation(self._INSTANCE._physics_client_id)
self._physics_client_id = -1
if gui:
self._physics_client_id = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_KEYBOARD_SHORTCUTS, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
else:
self._physics_client_id = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setTimeStep(time_step.seconds)
p.setGravity(0, 0, -GRAVITY)
import pybullet as p
import time
#p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
pr2_gripper = objects[0]
print ("pr2_gripper=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
def reset(self):
if self._physics_client_id < 0:
if self._renders:
self._p = bc.BulletClient(connection_mode=pb.GUI)
self._p.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
else:
self._p = bc.BulletClient()
# self._p.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
self._physics_client_id = self._p._client
p = self._p
p.resetSimulation()
# load ramp
# load soccerbot
# p.loadURDF("/home/shahryar/catkin_ws/src/soccer_ws/soccer_description/models/soccerbot_stl.urdf",
# p.loadURDF("/home/shahryar/PycharmProjects/DeepRL/gym-soccerbot/gym_soccerbot/soccer_description/models/soccerbot_stl.urdf",
# "/home/shahryar/PycharmProjects/DeepRL/gym-soccerbot/gym_soccerbot/soccer_description/models/soccerbot_stl.urdf",
urdfBotPath = gym_soccerbot.getDataPath()
self.soccerbotUid = p.loadURDF(os.path.join(urdfBotPath, "soccer_description/models/soccerbot_stl.urdf"),
useFixedBase=False,
useMaximalCoordinates=False,
basePosition=[0, 0, self.STANDING_HEIGHT],
baseOrientation=[0., 0., 0., 1.],
flags=pb.URDF_USE_INERTIA_FROM_FILE
| pb.URDF_USE_MATERIAL_COLORS_FROM_MTL
| pb.URDF_USE_SELF_COLLISION
| pb.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
)
# |p.URDF_USE_SELF_COLLISION|p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
urdfRootPath = pybullet_data.getDataPath()
self.planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane_implicit.urdf"),
useMaximalCoordinates=True,
basePosition=[0, 0, 0])
# TODO change dynamics ...
# for i in range(p.getNumJoints(bodyUniqueId=self.soccerbotUid)):
# print(p.getJointInfo(bodyUniqueId=self.soccerbotUid, jointIndex=i)[1])
p.changeDynamics(self.planeUid, linkIndex=-1, lateralFriction=0.9, spinningFriction=0.9, rollingFriction=0)
# p.changeDynamics(self.soccerbotUid, linkIndex=Links.IMU, mass=0.01, localInertiaDiagonal=[7e-7, 7e-7, 7e-7])
# p.changeDynamics(self.soccerbotUid, linkIndex=Links.IMU, mass=0., localInertiaDiagonal=[0., 0., 0.])
'''
p.changeDynamics(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6,
frictionAnchor=1, lateralFriction=1,
rollingFriction=1, spinningFriction=1)
p.changeDynamics(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6,
frictionAnchor=1, lateralFriction=1,
rollingFriction=1, spinningFriction=1)
'''
# TODO change timestep ...
self.timeStep = 1./240
p.setTimeStep(self.timeStep)
p.setGravity(0, 0, -9.81)
self.gravity = [0, 0, -9.81]
p.setRealTimeSimulation(0) # To manually step simulation later
p = self._p
# TODO reset joint state
p.resetBasePositionAndOrientation(self.soccerbotUid, [0, 0, self.STANDING_HEIGHT], [0., 0., 0., 1.])
p.resetBaseVelocity(self.soccerbotUid, [0, 0, 0], [0, 0, 0])
self.prev_lin_vel = np.array([0, 0, 0])
#p.resetJointStates(self.soccerbotUid, list(range(0, 18, 1)), 0)
#pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
#standing_poses = [0] * (self.JOINT_DIM + 2)
standing_poses = self._standing_poses()
for i in range(self.JOINT_DIM + 2):
p.resetJointState(self.soccerbotUid, i, standing_poses[i])
# WARM UP SIMULATION
for _ in range(self.WARM_UP_STEPS):
p.stepSimulation()
# TODO set state???
# TODO get observation
feet = self._feet()
imu = self._imu()
joint_states = p.getJointStates(self.soccerbotUid, list(range(0, self.JOINT_DIM, 1)))
joints_pos = np.array([state[0] for state in joint_states], dtype=np.float32)
start_pos = np.array([0, 0, self.STANDING_HEIGHT], dtype=np.float32)
observation = np.concatenate((joints_pos, imu, start_pos, feet))
#pb.resetSimulation()
"""
From core.py in gym:
Returns:
observation (object): the initial observation.
"""
if self._renders:
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
return observation
