def __init__(self, motion_file, enable_draw=False, pybullet_client=None, fall_contact_bodies=[]):
super().__init__(None, enable_draw)
self._num_agents = 1
self._pybullet_client = pybullet_client
self._useStablePD = True
if self.enable_draw:
self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_GUI, 0)
else:
self._pybullet_client = bullet_client.BulletClient()
self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
z2y = self._pybullet_client.getQuaternionFromEuler([-math.pi * 0.5, 0, 0])
self._planeId = self._pybullet_client.loadURDF("plane_implicit.urdf", [0, 0, 0], z2y, useMaximalCoordinates=True)
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
self._pybullet_client.setGravity(0, -9.8, 0)
self._pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)
self._pybullet_client.changeDynamics(self._planeId, linkIndex=-1, lateralFriction=0.9)
self._mocapData = motion_capture_data.MotionCaptureData()
motionPath = pybullet_data.getDataPath() + "/" + motion_file[0]
self._mocapData.Load(motionPath)
timeStep = 1. / 240.
useFixedBase = False
self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData, timeStep, useFixedBase, fall_contact_bodies)
self._pybullet_client.setTimeStep(timeStep)
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1)
self.reset()
def __init__(self, gui, controlled_joints, ee_idx, torque_limits,
target_pos):
if gui:
self.sim = bc.BulletClient(connection_mode=pybullet.GUI)
else:
self.sim = bc.BulletClient(connection_mode=pybullet.DIRECT)
self.sim.setAdditionalSearchPath(pybullet_data.getDataPath())
self.ee_idx = ee_idx
self.cur_joint_pos = None
self.cur_joint_vel = None
self.curr_ee = None
self.babbling_torque_limits = None
self.logger = None
self.controlled_joints = controlled_joints
self.torque_limits = torque_limits
self.n_dofs = len(controlled_joints)
#pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
# TODO: the following should be extracted into some world model that is loaded independently of the robot
#self.planeId = pybullet.loadURDF("plane.urdf",[0,0,0])
if target_pos is not None:
self.cubeId = pybullet.loadURDF("sphere_small.urdf", target_pos)
def initialize(self, is_render=False):
self.is_render = is_render
if self.is_render:
self.pybullet_client = bullet_client.BulletClient(
connection_mode=p1.GUI)
self.pybullet_client.configureDebugVisualizer(
self.pybullet_client.COV_ENABLE_GUI, 0)
else:
self.pybullet_client = bullet_client.BulletClient()
self.pybullet_client.configureDebugVisualizer(
self.pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
self.pybullet_client.setAdditionalSearchPath('{}/urdf'.format(
os.path.dirname(os.path.realpath(__file__))))
self.pybullet_client.setPhysicsEngineParameter(
numSolverIterations=self.num_solver_iterations) #10
self.pybullet_client.setTimeStep(self.time_step)
#make vertical plane(bottom plane)
z2y = self.pybullet_client.getQuaternionFromEuler([-np.pi * 0.5, 0, 0])
planeId = self.pybullet_client.loadURDF("plane_implicit.urdf",
[0, 0, 0],
z2y,
useMaximalCoordinates=True)
self.pybullet_client.setGravity(0, -9.8, 0)
self.pybullet_client.changeDynamics(planeId,
linkIndex=-1,
lateralFriction=1.0)
self.model = HalfCheetah(self.pybullet_client)
def reset(self):
if (self.physicsClientId < 0):
self.ownsPhysicsClient = True
if self.isRender:
self._p = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
self._p = bullet_client.BulletClient()
self.physicsClientId = self._p._client
self._p.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
if self.scene is None:
self.scene = self.create_single_player_scene(self._p)
if not self.scene.multiplayer and self.ownsPhysicsClient:
self.scene.episode_restart(self._p)
self.robot.scene = self.scene
self.frame = 0
self.done = 0
self.reward = 0
dump = 0
s = self.robot.reset(self._p)
if hasattr(self, 'reset_dynamics'):
self.reset_dynamics()
#for i in range(20):
# #print(i)
# mass = self._p.getDynamicsInfo(self.robot.objects[0], i)[0]
# new_mass = max(0, mass - 5)
# self._p.changeDynamics(self.robot.objects[0], i, mass=new_mass)
self.potential = self.robot.calc_potential()
return s
def _load_with_tool(self, tool):
p0 = bc.BulletClient(connection_mode=p.DIRECT)
p1 = bc.BulletClient(connection_mode=p.DIRECT)
kuka = p0.loadURDF(get_resource_path('kuka_iiwa_insertion','robot', 'urdf', 'iiwa14.urdf'))
if tool is "square":
tool = p1.loadURDF(get_resource_path('kuka_iiwa_insertion','models', 'square10x10', 'tool9x9.urdf'))
elif tool is "triangular":
tool = p1.loadURDF(get_resource_path('kuka_iiwa_insertion','models', 'triangle10x10', 'TriangleTool9x9.urdf'))
elif tool is "zylindric":
tool = p1.loadURDF(get_resource_path('kuka_iiwa_insertion','models', 'zylinder10x10', 'ZylinderTool9x9.urdf'))
else:
raise NotImplementedError("This tool has not been implemented")
ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(kuka, p0._client)
ed1 = ed.UrdfEditor()
ed1.initializeFromBulletBody(tool, p1._client)
jointPivotXYZInParent = [0, 0, 0.026]
jointPivotRPYInParent = [0, 0, 0]
jointPivotXYZInChild = [0, 0, 0]
jointPivotRPYInChild = [0, 0, 0]
newjoint = ed0.joinUrdf(ed1, self.ee_index, jointPivotXYZInParent, jointPivotRPYInParent,
jointPivotXYZInChild, jointPivotRPYInChild, p0._client, p1._client)
newjoint.joint_type = p.JOINT_FIXED
self.kuka_uid = ed0.createMultiBody([0, 0, 0], [0,0,0,1], self.client)
def setup_pybullet(self):
if self.parallel:
if self.gui:
p = bc.BulletClient(connection_mode=pybullet.GUI)
else:
p = bc.BulletClient(connection_mode=pybullet.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
else:
if self.gui:
pybullet.connect(pybullet.GUI)
p = pybullet
# This just makes sure that the sphere is not visible (we only use the sphere for collision checking)
else:
pybullet.connect(pybullet.DIRECT)
p = pybullet
p.setGravity(0, 0, 0)
self.ground = p.loadURDF("./URDFs/plane.urdf") # Ground plane
# p.changeVisualShape(self.ground, -1, rgbaColor=[0.9,0.9,0.9,0.7])
self.init_position = [0, 0, 2]
self.init_orientation = p.getQuaternionFromEuler([0., 0., np.pi/4])
quadrotor = p.loadURDF("./URDFs/Quadrotor/quadrotor.urdf",
basePosition=self.init_position,
baseOrientation=self.init_orientation,
useFixedBase=1,
globalScaling=1) # Load robot from URDF
p.changeVisualShape(quadrotor, -1, rgbaColor=[0.5,0.5,0.5,1])
self.p = p
self.quadrotor = quadrotor
def __init__(self):
self.physics_client = bullet_client.BulletClient(pb.GUI)
self.ik_back = bullet_client.BulletClient(pb.DIRECT)
self.object_list = {}
self.physics_client.setAdditionalSearchPath(pb_data.getDataPath())
self.physics_client.loadURDF("plane.urdf")
self.physics_client.setGravity(0, 0, -9.81)
self.ik_back.setAdditionalSearchPath(pb_data.getDataPath())
self.ik_back.loadURDF("plane.urdf")
self.ik_back.setGravity(0, 0, -9.81)
self.end_time = 0
self.start_time = 0
self.time_consumed = 0.0
self.viewMatrix = self.physics_client.computeViewMatrix(
cameraEyePosition=[0.5, 0.35, 0.98],
cameraTargetPosition=[0.5, 0.35, .6],
cameraUpVector=[0, 1, 0])
self.projectionMatrix = self.physics_client.computeProjectionMatrixFOV(
fov=45.0, # degrees
aspect=1.0,
nearVal=0.1,
farVal=1.5)
def __init__(self, render_mode='DIRECT', time_step=1. / 240., seed=None, thresholds=np.array([0.03, 0.03, 0.03]), assets_path=assets_path, n_substeps=5):
self.time_step = time_step
self.render_mode = render_mode
self.thresholds = thresholds
self.n_substeps = n_substeps
self.seed(seed)
if render_mode == 'DIRECT':
bullet_instance = bc.BulletClient(p.DIRECT)
elif render_mode == 'GUI':
bullet_instance = bc.BulletClient(p.GUI)
bullet_instance.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
bullet_instance.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
bullet_instance.setPhysicsEngineParameter(maxNumCmdPer1ms=1000)
bullet_instance.resetDebugVisualizerCamera(cameraDistance=1.3, cameraYaw=38, cameraPitch=-22,
cameraTargetPosition=[0.35, -0.13, 0])
else:
sys.exit(f'FetchBulletEnv - render mode not supported: {render_mode}')
# bullet_instance.setAdditionalSearchPath(pd.getDataPath())
bullet_instance.setAdditionalSearchPath(assets_path)
bullet_instance.setTimeStep(time_step)
bullet_instance.setGravity(0, -9.8, 0)
self.sim = FetchBulletSim(bullet_instance, [0, 0, 0], self.np_random)
obs = self.sim.reset()
self.action_space = spaces.Box(-1., 1., shape=(4,), dtype='float32')
self.observation_space = spaces.Dict(dict(
desired_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'),
achieved_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'),
observation=spaces.Box(-np.inf, np.inf, shape=obs['observation'].shape, dtype='float32'),
))
def __init__(self, config):
self.config = config
# Usage mode
if config['simulation']['headless']:
self.p = bc.BulletClient(connection_mode=p.DIRECT)
else:
options = '--background_color_red=0.85 --background_color_green=0.85 --background_color_blue=0.85' # noqa
self.p = bc.BulletClient(connection_mode=p.GUI, options=options)
self.p.resetDebugVisualizerCamera(cameraDistance=150,
cameraYaw=0,
cameraPitch=-89.999,
cameraTargetPosition=[0, 30, 0])
# self.p.getCameraImage(1200, 1200)
self.p.configureDebugVisualizer(self.p.COV_ENABLE_GUI, 0)
# self.p.configureDebugVisualizer(shadowMapWorldSize=10)
# self.p.configureDebugVisualizer(lightPosition=[0, 0, 500])
# Set gravity
self.p.setGravity(0, 0, -9.81)
# Set parameters for simulation
self.p.setPhysicsEngineParameter(
fixedTimeStep=config['simulation']['time_step'] / 10,
numSubSteps=1,
numSolverIterations=5)
return None
def reset(self):
# print("-----------reset simulation---------------")
if self._physics_client_id < 0:
if self._renders:
self._p = bc.BulletClient(connection_mode=p2.GUI)
else:
self._p = bc.BulletClient()
self._physics_client_id = self._p._client
p = self._p
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "cartpole.urdf"),
[0, 0, 0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0, 0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
p = self._p
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
def _reset(self):
if (self.physicsClientId < 0):
self.ownsPhysicsClient = True
if self.isRender:
self._p = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
self._p = bullet_client.BulletClient()
self.physicsClientId = self._p._client
self._p.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
if self.scene is None:
self.scene = self.create_single_player_scene(self._p)
if not self.scene.multiplayer and self.ownsPhysicsClient:
self.scene.episode_restart(self._p)
self.robot.scene = self.scene
self.frame = 0
self.done = 0
self.reward = 0
dump = 0
s = self.robot.reset(self._p)
self.potential = self.robot.calc_potential()
return s
def __init__(self, config):
self.config = config
# Usage mode
if config['simulation']['headless']:
self.p = bc.BulletClient(connection_mode=p.DIRECT)
else:
self.p = bc.BulletClient(connection_mode=p.GUI)
self.p.resetDebugVisualizerCamera(cameraDistance=150,
cameraYaw=0,
cameraPitch=-89.999,
cameraTargetPosition=[0, 80, 0])
# Set gravity
self.p.setGravity(0, 0, -9.81)
self.p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optional
# Set parameters for simulation
self.p.setPhysicsEngineParameter(
fixedTimeStep=config['simulation']['time_step'], numSubSteps=1)
# Setup ground
plane = self.p.loadURDF("plane.urdf", [0, 0, 0],
self.p.getQuaternionFromEuler(
[0, 0, math.pi / 2]),
useFixedBase=True,
globalScaling=20)
self.p.changeVisualShape(plane, -1)
return None
def __init__(self, renders=False, actionRepeat=1500, discrete_actions=False):
# start the bullet physics server
self._renders = renders
self._discrete_actions = discrete_actions
self._render_height = 400
self._render_width = 420
self._actionRepeat = actionRepeat
self._physics_client_id = -1
self._cam_dist = 40
self._cam_pitch = -21 # degree
self._cam_yaw = 200#200 # degree
if self._renders:
self._p = bc.BulletClient(connection_mode=p2.GUI)
else:
self._p = bc.BulletClient()
self.v = 0.1111 # nmi/s
# action space
self.min_t1 = 100
self.max_t1 = 400
self.min_h = 30
self.max_h = 40
self.min_t2 = 200
self.max_t2 = 400
# max area: Isosceles Right Triangle
self.max_area = (self.max_t2 * self.v) ** 2 / 4 * 2
self.low_action = np.array([self.min_t1, self.min_h, self.min_t2],
dtype=np.float32) # t1, h, t2
self.high_action = np.array([self.max_t1, self.max_h, self.max_t2],
dtype=np.float32)
self.action_space = spaces.Box(self.low_action,
self.high_action,
dtype=np.float32)
print('high_action', self.high_action)
# observation space
self.seperationStatus_min = 3
self.seperationStatus_max = 100
self.low_state = np.array([self.seperationStatus_min],
dtype=np.float32)
self.high_state = np.array([self.seperationStatus_max],
dtype=np.float32)
self.observation_space = spaces.Box(low=self.low_state,
high=self.high_state,
dtype=np.float32)
self.seed()
# self.reset()
self.viewer = None
self._configure()
def reset(self):
self._timestep = 0
""" @Brief: reset everything. Note: We need to think about reference state
initialization (RSI) in the deepmimic paper
return ob, reward, done, info
"""
# TODO
if (self._pybullet_client == None):
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
self._pybullet_client = bullet_client.BulletClient()
self._pybullet_client.setAdditionalSearchPath(
pybullet_data.getDataPath())
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
self._motion = MotionCaptureData()
motionPath = pybullet_data.getDataPath(
) + "/motions/humanoid3d_walk.txt" #humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt"
self._motion.Load(motionPath)
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_RENDERING, 0)
self._pybullet_client.resetSimulation()
self._pybullet_client.setGravity(0, -9.8, 0)
y2zOrn = self._pybullet_client.getQuaternionFromEuler([-1.57, 0, 0])
self._ground_id = self._pybullet_client.loadURDF(
"%s/plane.urdf" % self._urdf_root, [0, 0, 0], y2zOrn)
#self._pybullet_client.changeVisualShape(self._ground_id,-1,rgbaColor=[1,1,1,0.8])
#self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,self._ground_id)
shift = [0, 0, 0]
self._humanoid = Humanoid(self._pybullet_client, self._motion, shift)
self._humanoid.Reset()
simTime = random.randint(0, self._motion.NumFrames() - 2)
self._humanoid.SetSimTime(simTime)
pose = self._humanoid.InitializePoseFromMotionData()
self._humanoid.ApplyPose(pose, True, True, self._humanoid._humanoid,
self._pybullet_client)
self._env_step_counter = 0
self._objectives = []
self._pybullet_client.resetDebugVisualizerCamera(
self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0])
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_RENDERING, 1)
self._current_step = 0
# self._env.reset()
# the following is a hack, there is some precision issue in mujoco_py
# self._old_ob = self._get_observation()
# self._env.reset()
# self.set_state({'start_state': self._old_ob.copy()})
self._old_ob = self._get_observation()
return self._old_ob.copy(), 0.0, False, {}
def reset(self):
self._current_step = 0
if self._physics_client_id < 0:
if self._renders:
self._p = bc.BulletClient(connection_mode=p2.GUI)
else:
self._p = bc.BulletClient()
self._physics_client_id = self._p._client
self._p.setAdditionalSearchPath(pybullet_data.getDataPath())
p = self._p
#p.resetSimulation()
if self._state_id < 0:
# load plane
planeId = p.loadURDF("plane.urdf")
# load sawyer
self.sawyerStartPos = [0, 1, 1]
self.sawyerStartOrientation = p.getQuaternionFromEuler(
[np.pi / 2, 0, 0])
self.sawyer = p.loadURDF(
"/data/yxjin/robot/sawyer/sawyer_description/urdf/gripper.urdf",
globalScaling=1.5)
p.resetBasePositionAndOrientation(self.sawyer, self.sawyerStartPos,
self.sawyerStartOrientation)
self.base_constraint = p.createConstraint(
parentBodyUniqueId=self.sawyer,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=self.sawyerStartPos,
parentFrameOrientation=p.getQuaternionFromEuler([0, 0, 0]),
childFrameOrientation=self.sawyerStartOrientation)
# load drawer
self.drawerStartPos = [0, 2.2, 1]
self.drawerStartOrientation = p.getQuaternionFromEuler(
[0, 0, -np.pi / 2])
self.drawer = p.loadURDF(
"/data/yxjin/robot/rotation/drawer_one_sided_handle.urdf",
useFixedBase=1,
flags=p.URDF_USE_SELF_COLLISION)
p.resetBasePositionAndOrientation(self.drawer, self.drawerStartPos,
self.drawerStartOrientation)
self._state_id = p.saveState()
else:
p.restoreState(self._state_id)
self.handle_origin = p.getLinkState(self.drawer, 3)[0][1]
self.max_pull_dist = self.handle_origin - self.handle_max
# compute init state
self.state = p.getLinkState(self.sawyer, 0)[0] + p.getLinkState(
self.sawyer, 0)[1] + p.getLinkState(self.drawer, 3)[0]
return np.array(self.state)
def __init__(self, render=False):
self.action_repeat = 8
self.timestep_length = 1 / 240
self.time_limit = 10
self.target_pos = np.array([10, 0, 0])
self.last_goal_distance = None
if render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
else:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.DIRECT)
self._pybullet_client.setAdditionalSearchPath(
os.path.dirname(__file__) + '/bullet_data')
self._plane_id = self._pybullet_client.loadURDF(
"plane_narrow.urdf", [13, 0, 0],
self._pybullet_client.getQuaternionFromEuler(
[-math.pi * 0.5, 0, 0]),
globalScaling=1.00)
track_id = self._pybullet_client.loadSDF(
'RoboyParkourTrack/model.sdf')[0]
self._pybullet_client.resetBasePositionAndOrientation(
track_id, [5, 0, -1.8], [0, 0, 0, 1])
self.humanoid = Humanoid(self._pybullet_client,
time_step_length=self.timestep_length)
self._pybullet_client.setGravity(0, -9.8, 0)
self._pybullet_client.changeDynamics(self._plane_id,
linkIndex=-1,
lateralFriction=0.95)
self._pybullet_client.setTimeStep(self.timestep_length)
self.action_dim = 43
self.obs_dim = 196
self.max_num_steps = 2000
self.action_space = gym.spaces.Box(low=-1, high=1, shape=(43, ))
observation_example = self.get_observation()
self.observation_space = gym.spaces.Dict({
'state':
gym.spaces.Box(low=-1,
high=1,
shape=observation_example['state'].shape),
'goal':
gym.spaces.Dict({
'camera':
gym.spaces.Box(
low=-1,
high=1,
shape=observation_example['goal']['camera'].shape),
'relative_target':
gym.spaces.Box(low=-100, high=100, shape=(2, ))
})
})
self.last_100_goal_distances = None
def reset(self):
self._current_step = 0
if self._physics_client_id < 0:
if self._renders:
self._p = bc.BulletClient(connection_mode=p2.GUI)
else:
self._p = bc.BulletClient()
self._physics_client_id = self._p._client
self._p.setAdditionalSearchPath(pybullet_data.getDataPath())
p = self._p
if self._state_id < 0:
# load plane
planeId = p.loadURDF("plane.urdf")
# load hand
self.handStartPos = [0.05, 0, 1.05]
self.handStartOrientation = p.getQuaternionFromEuler(
[-np.pi / 2, 0, np.pi])
self.dex_hand = p.loadURDF(
"/data/yxjin/robot/inmoov_ros/inmoov_description/robots/inmoov_shadow_hand_v2_3.urdf"
)
p.resetBasePositionAndOrientation(self.dex_hand, self.handStartPos,
self.handStartOrientation)
self.hand_constraint = p.createConstraint(
self.dex_hand, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], self.handStartPos,
p.getQuaternionFromEuler([0, 0, 0]), self.handStartOrientation)
self.ll = np.array([
p.getJointInfo(self.dex_hand, joint)[8]
for joint in self.joint_id_list
])
self.ul = np.array([
p.getJointInfo(self.dex_hand, joint)[9]
for joint in self.joint_id_list
])
# load drawer
self.drawerStartPos = [0, 2.2, 1]
self.drawerStartOrientation = p.getQuaternionFromEuler(
[0, 0, -np.pi / 2])
self.drawer = p.loadURDF(
"/data/yxjin/robot/rotation/drawer_one_sided_handle.urdf",
useFixedBase=1,
flags=p.URDF_USE_SELF_COLLISION)
p.resetBasePositionAndOrientation(self.drawer, self.drawerStartPos,
self.drawerStartOrientation)
self._state_id = p.saveState()
else:
p.restoreState(self._state_id)
self.handle_origin = p.getLinkState(self.drawer, self.handle_idx)[0][1]
self.max_pull_dist = self.handle_origin - self.handle_max
# compute init state
self.state = self._get_obs()
return self.state
def _init(self):
with open('OpenRoboRL/config/pybullet_sim_param.yaml') as f:
sim_params_dict = yaml.safe_load(f)
if "quadruped_robot" in list(sim_params_dict.keys()):
self._sim_params = sim_params_dict["quadruped_robot"]
else:
raise ValueError(
"Hyperparameters not found for pybullet_sim_config.yaml")
self.action_space = self._robot[0].action_space
self.observation_space = self._flatten_observation_spaces(
self.robot[0].observation_space)
# Simulation related parameters.
self._num_action_repeat = self._robot[0].action_repeat
self._sim_time_step = self._sim_params["sim_time_step_s"]
self._env_time_step = self._num_action_repeat * self._sim_time_step
self._env_step_counter = 0
self._num_bullet_solver_iterations = int(self._sim_params["num_sim_iter_step"] /
self._num_action_repeat)
self._is_render = self._sim_params["enable_rendering"]
# The wall-clock time at which the last frame is rendered.
self._last_frame_time = 0.0
self._show_reference_id = -1
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_GUI,
self._sim_params["enable_rendering_gui"])
self._delay_id = pybullet.addUserDebugParameter("delay", 0, 0.3, 0)
else:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.DIRECT)
self._pybullet_client.setAdditionalSearchPath(pd.getDataPath())
self._pybullet_client.resetSimulation()
self._pybullet_client.setPhysicsEngineParameter(
numSolverIterations=self._num_bullet_solver_iterations)
self._pybullet_client.setTimeStep(self._sim_time_step)
self._pybullet_client.setGravity(0, 0, -10)
# Rebuild the world.
self._world_dict = {
"ground": self._pybullet_client.loadURDF("plane_implicit.urdf")
}
for i in range(self.num_robot):
self._robot[i].init_robot(self._pybullet_client)
self._task[i]._init_task(
self._robot[i], self._pybullet_client, self.get_ground(), self._env_time_step)
self.reset()
def _connect_to_physics_server(self):
"""
Connect to the PyBullet physics server in SHARED_MEMORY, GUI or DIRECT mode
"""
if self.gui_on:
self.p = bc.BulletClient(connection_mode=pybullet.GUI)
# if (self.p < 0):
# self.p = bc.BulletClient(connection_mode=p.GUI)
self._set_gui_mode()
else:
self.p = bc.BulletClient(connection_mode=pybullet.DIRECT)
self.p.setPhysicsEngineParameter(enableFileCaching=0)
def _setup_client_and_physics(
self,
graphics=False
) -> bullet_client.BulletClient:
"""Creates a PyBullet process instance.
The parameters for the physics simulation are determined by the
get_physics_parameters() function.
Parameters
----------
graphics: bool
If True PyBullet shows graphical user interface with 3D OpenGL
rendering.
Returns
-------
bc: BulletClient
The instance of the created PyBullet client process.
"""
if graphics or self.use_graphics:
bc = bullet_client.BulletClient(connection_mode=pb.GUI)
else:
bc = bullet_client.BulletClient(connection_mode=pb.DIRECT)
# optionally enable EGL for faster headless rendering
try:
if os.environ["PYBULLET_EGL"]:
con_mode = bc.getConnectionInfo()['connectionMethod']
if con_mode == bc.DIRECT:
egl = pkgutil.get_loader('eglRenderer')
if egl:
bc.loadPlugin(egl.get_filename(),
"_eglRendererPlugin")
print('LOADED EGL...')
else:
bc.loadPlugin("eglRendererPlugin")
except KeyError:
# print('Note: could not load egl...')
pass
# add bullet_safety_gym/envs/data to the PyBullet data path
bc.setAdditionalSearchPath(bases.get_data_path())
# disable GUI debug visuals
bc.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0)
bc.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
bc.setPhysicsEngineParameter(
fixedTimeStep=self.time_step * self.frame_skip,
numSolverIterations=self.number_solver_iterations,
deterministicOverlappingPairs=1,
numSubSteps=self.frame_skip)
bc.setGravity(0, 0, -9.81)
bc.setDefaultContactERP(0.9)
return bc
def reset(self):
self._current_step = 0
if self._physics_client_id < 0:
if self._renders:
self._p = bc.BulletClient(connection_mode=p2.GUI)
else:
self._p = bc.BulletClient()
self._physics_client_id = self._p._client
self._p.setAdditionalSearchPath(pybullet_data.getDataPath())
p = self._p
#p.resetSimulation()
if self._state_id < 0:
# load plane
planeId = p.loadURDF("plane.urdf")
# load sawyer
self.sawyerStartPos = [0.05, 0.5, 1.05]
self.sawyerStartOrientation = p.getQuaternionFromEuler(
[0, 0, np.pi / 2])
self.sawyer = p.loadURDF(
"/data/yxjin/robot/sawyer/sawyer_description/urdf/sawyer_with_gripper.urdf",
basePosition=self.sawyerStartPos,
baseOrientation=self.sawyerStartOrientation,
useFixedBase=1)
# set sawyer init pos
pos_new = p.calculateInverseKinematics(self.sawyer,
17, [0, 1, 1],
jointDamping=self.jd)
for it in range(p.getNumJoints(self.sawyer)):
jointInfo = p.getJointInfo(self.sawyer, it)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(self.sawyer, it, pos_new[qIndex - 7])
# load drawer
self.drawerStartPos = [0, 2.2, 1]
self.drawerStartOrientation = p.getQuaternionFromEuler(
[0, 0, -np.pi / 2])
self.drawer = p.loadURDF(
"/data/yxjin/robot/rotation/drawer_one_sided_handle.urdf",
basePosition=self.drawerStartPos,
baseOrientation=self.drawerStartOrientation,
useFixedBase=1,
flags=p.URDF_USE_SELF_COLLISION)
self._state_id = p.saveState()
else:
p.restoreState(self._state_id)
self.handle_origin = p.getLinkState(self.drawer, 3)[0][1]
self.max_pull_dist = self.handle_origin - self.handle_max
# compute init state
self.state = p.getLinkState(self.sawyer, 17)[0] + p.getLinkState(
self.sawyer, 17)[1] + p.getLinkState(self.drawer, 3)[0]
return np.array(self.state)
def __init__(self,
urdf_root=pybullet_data.getDataPath(),
render=False,
distance_limit=10,
forward_reward_cap=float("inf"),
distance_weight=3.0,
energy_weight=0.02,
drift_weight=0.0,
shake_weight=0.0,
hard_reset=True,
hexapod_urdf_root="/home/czbfy/hexapod_rl/urdf"):
super(HexapodGymEnv, self).__init__()
self._urdf_root = urdf_root
self._hexapod_urdf_root = hexapod_urdf_root
self._observation = []
self._norm_observation = []
self._env_step_counter = 0
self._is_render = render
self._cam_dist = 1.0
self._cam_yaw = 0
self._cam_pitch = -30
self._last_frame_time = 0.0
self.control_time_step = 0.01
self._distance_limit = distance_limit
self._forward_reward_cap = forward_reward_cap
self._time_step = 0.01
self._objective_weights = [
distance_weight, energy_weight, drift_weight, shake_weight
]
self._objectives = []
if self._is_render:
self._pybullet_client = bc.BulletClient(
connection_mode=pybullet.GUI)
else:
self._pybullet_client = bc.BulletClient()
self._hard_reset = True
self.seed()
self.reset()
self._action_bound = 1.0
action_dim = NUM_MOTORS
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
observation_high = self._get_observation_upper_bound()
observation_low = -observation_high
self.observation_space = spaces.Box(observation_low, observation_high)
self._hard_reset = hard_reset
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
isDiscrete=False,
renders=False,
point_goal=True,
rgb_camera_state=False,
depth_camera_state=False):
self._timeStep = 0.01
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._ballUniqueId = -1
self._envStepCounter = 0
self._renders = renders
self._point_goal = point_goal
self._depth_camera_state = depth_camera_state
self._rgb_camera_state = rgb_camera_state
self._isDiscrete = isDiscrete
self._cam_dist = 4
self._cam_yaw = 0
self._cam_pitch = 0
if self._renders:
self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
else:
self._p = bullet_client.BulletClient()
self.seed()
observationDim = 2 # len(self.getExtendedObservation()) # todo make variable and do right
print("observationDim")
print(observationDim) # todo change but now it's a dict/list
# observation_high = np.array([np.finfo(np.float32).max] * observationDim)
observation_high = np.ones(observationDim) * 1000 # np.inf
if (isDiscrete):
self.action_space = spaces.Discrete(4)
else:
# todo continuous as well make sure it works
action_dim = 2
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high,
action_high,
dtype=np.float32)
self.observation_space = spaces.Box(-observation_high,
observation_high,
dtype=np.float32)
self.viewer = None
def _init_pybullet_client(self, record_video):
pybullet_client = pybullet
if record_video:
# recording video requires ffmpeg in the path
pybullet_client.connect(pybullet.GUI,
options=f"--width={sim_constants.RENDER_WIDTH} "
f"--height={sim_constants.RENDER_HEIGHT} "
f"--mp4=\"test.mp4\" --mp4fps=100")
pybullet_client.configureDebugVisualizer(pybullet.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
else:
if self._is_render:
pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI)
else:
pybullet_client = bullet_client.BulletClient()
return pybullet_client
def __init__(self, config: configs.Solo8BaseConfig, use_gui: bool):
"""Create a solo8 env.
Args:
config (configs.Solo8BaseConfig): The SoloConfig. Defaults to None.
use_gui (bool): Whether or not to show the pybullet GUI. Defaults to
False.
"""
self.config = config
self.client = bc.BulletClient(
connection_mode=p.GUI if use_gui else p.DIRECT)
self.client.setAdditionalSearchPath(pbd.getDataPath())
self.client.setGravity(*self.config.gravity)
if self.config.dt:
self.client.setPhysicsEngineParameter(fixedTimeStep=self.config.dt,
numSubSteps=1)
else:
self.client.setRealTimeSimulation(1)
self.client_configuration()
self.plane = self.client.loadURDF('plane.urdf')
self.load_bodies()
self.obs_factory = obs.ObservationFactory(self.client)
self.reward_factory = rewards.RewardFactory(self.client)
self.termination_factory = terms.TerminationFactory()
self.reset(init_call=True)
def __init__(self,
render: bool = False,
n_substeps: int = 20,
background_color: Optional[np.ndarray] = None) -> None:
background_color = background_color if background_color is not None else np.array(
[223.0, 54.0, 45.0])
self.background_color = background_color.astype(np.float64) / 255
options = "--background_color_red={} \
--background_color_green={} \
--background_color_blue={}".format(*self.background_color)
self.connection_mode = p.GUI if render else p.DIRECT
self.physics_client = bc.BulletClient(
connection_mode=self.connection_mode, options=options)
self.physics_client.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
self.physics_client.configureDebugVisualizer(
p.COV_ENABLE_MOUSE_PICKING, 0)
self.n_substeps = n_substeps
self.timestep = 1.0 / 500
self.physics_client.setTimeStep(self.timestep)
self.physics_client.resetSimulation()
self.physics_client.setAdditionalSearchPath(
pybullet_data.getDataPath())
self.physics_client.setGravity(0, 0, -9.81)
self._bodies_idx = {}
def __init__(self, fps=120):
""" init function
Keyword arguments:
fps -- frames per second of the trajectories (default 120)
"""
self.p = bc.BulletClient(connection_mode=pybullet.GUI)
#p.connect(p.GUI)
#p.setGravity(0,0,-100)
self._humans = {}
self._hidehumans = {}
self._objects = {}
self._fps = fps
self._playbackTrajs = []
self._playbackTrajs_interp = []
self._playbackTrajsObj = []
self._start_playback = None
self._end_playback = None
self.gaze_trajs = []
self.gaze_objs = []
self.trans_world = np.array(
[0., 0., 0.]) # this can be used to translate the world
self.p.configureDebugVisualizer(self.p.COV_ENABLE_RENDERING, 0)
self.plane = self.p.loadURDF(
os.path.dirname(os.path.realpath(__file__)) + "/data/plane.urdf",
basePosition=[0, 0, 0] + self.trans_world)
self.p.configureDebugVisualizer(self.p.COV_ENABLE_RENDERING, 1)
def convert_mjcf_to_urdf(input_mjcf, output_path):
"""Convert MuJoCo mjcf to URDF format and save.
Parameters
----------
input_mjcf : str
input path of mjcf file.
output_path : str
output directory path of urdf.
"""
client = bulllet_client.BulletClient()
objs = client.loadMJCF(
input_mjcf, flags=client.URDF_USE_IMPLICIT_CYLINDER)
# create output directory
mjcf2urdf.makedirs(output_path)
for obj in objs:
humanoid = objs[obj]
ue = urdfEditor.UrdfEditor()
ue.initializeFromBulletBody(humanoid, client._client)
robot_name = str(client.getBodyInfo(obj)[1], 'utf-8')
part_name = str(client.getBodyInfo(obj)[0], 'utf-8')
save_visuals = False
outpath = osp.join(
output_path, "{}_{}.urdf".format(robot_name, part_name))
ue.saveUrdf(outpath, save_visuals)
def __init__(self, plane_spawn=True, bullet_gui=True):
self.verbose = True
self.physics_ts = 1.0 / 240.0 #Time step for the bullet environment for physics simulation
if bullet_gui:
# physicsClient = p.connect(p.GUI)
p0 = bc.BulletClient(connection_mode=p.GUI)
p0.addUserDebugParameter("Disconnect", 1, 0, 1)
p0.resetDebugVisualizerCamera(cameraDistance=1.5,
cameraYaw=55,
cameraPitch=-10,
cameraTargetPosition=[0, 0, 0.5])
self.physicsClient = p0
else:
physicsClient = p.connect(p.DIRECT)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0, 0, -9.81)
if plane_spawn:
planeId = p.loadURDF("plane.urdf")
self.robotIDs = []
self.objectIDs = []
self.torque_control_mode = False
self.visualization_realtime = True
# Add button for
p.addUserDebugParameter("Disconnect", 1, 0, 1)
# Set default camera position
p.resetDebugVisualizerCamera(cameraDistance=1.5,
cameraYaw=55,
cameraPitch=-10,
cameraTargetPosition=[0, 0, 0.5])
def __init__(self, config, evaluate, test, validate):
"""Initialize a new simulated world.
Args:
config: A dict containing values for the following keys:
real_time (bool): Flag whether to run the simulation in real time.
visualize (bool): Flag whether to open the bundled visualizer.
"""
self._rng = self.seed(evaluate=evaluate)
config_scene = config['scene']
self.scene_type = config_scene.get('scene_type', "OnTable")
if self.scene_type == "OnTable":
self._scene = scene.OnTable(self, config, self._rng, test,
validate)
elif self.scene_type == "OnFloor":
self._scene = scene.OnFloor(self, config, self._rng, test,
validate)
else:
self._scene = scene.OnTable(self, config, self._rng, test,
validate)
self.sim_time = 0.
self._time_step = 1. / 240.
self._solver_iterations = 150
config = config['simulation']
visualize = config.get('visualize', True)
self._real_time = config.get('real_time', True)
self.physics_client = bullet_client.BulletClient(
p.GUI if visualize else p.DIRECT)
self.models = []
self._callbacks = {World.Events.RESET: [], World.Events.STEP: []}
