def reset(self):
self.ordered_joints = []
for ob in self.objects:
p.removeBody(ob)
if self.self_collision:
self.objects = p.loadMJCF(
os.path.join(pybullet_data.getDataPath(), "mjcf",
self.model_xml),
flags=p.URDF_USE_SELF_COLLISION +
p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
self.objects)
else:
self.objects = p.loadMJCF(
os.path.join(pybullet_data.getDataPath(), "mjcf",
self.model_xml))
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
self.objects)
self.robot_specific_reset()
s = self.calc_state(
) # optimization: calc_state() can calculate something in self.* for calc_potential() to use
return s
def main():
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setTimeStep(1./240.)
floor = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
p.loadMJCF(floor)
cabinet_0007 = os.path.join(
gibson2.assets_path, 'models/cabinet2/cabinet_0007.urdf')
cabinet_0004 = os.path.join(
gibson2.assets_path, 'models/cabinet/cabinet_0004.urdf')
obj1 = ArticulatedObject(filename=cabinet_0007)
obj1.load()
obj1.set_position([0, 0, 0.5])
obj2 = ArticulatedObject(filename=cabinet_0004)
obj2.load()
obj2.set_position([0, 0, 2])
obj3 = YCBObject('003_cracker_box')
obj3.load()
obj3.set_position_orientation([0, 0, 1.2], [0, 0, 0, 1])
for _ in range(24000): # at least 100 seconds
p.stepSimulation()
time.sleep(1./240.)
p.disconnect()
def _set_physics_client(self, render=0):
if self.physicsClient_ID != -1:
pybullet.disconnect(self.physicsClient_ID)
self.physicsClient_ID = pybullet.connect(
pybullet.GUI) if render else pybullet.connect(pybullet.DIRECT)
# Load door
pybullet.setAdditionalSearchPath(self.xml_path)
pybullet.loadURDF(fileName="room_with_door.urdf",
basePosition=self.door_init_pos,
physicsClientId=self.physicsClient_ID)
# flags=pybullet.URDF_USE_INERTIA_FROM_FILE | pybullet.URDF_USE_SELF_COLLISION | pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
# Load humanoid
#pybullet.loadURDF(fileName='humanoid.urdf', basePosition=self.robot_init_pos, physicsClientId=self.physicsClient_ID)
# flags=pybullet.URDF_USE_INERTIA_FROM_FILE | pybullet.URDF_USE_SELF_COLLISION | pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
pybullet.loadMJCF("mjcf/humanoid_symmetric.xml")
#cubeId = pybullet.loadURDF("cube_small.urdf", 0, 0, 3)
#cid = pybullet.createConstraint(cubeId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1])
# Set and store physics parameters
pybullet.setDefaultContactERP(.90)
pybullet.setGravity(0, 0, -0.981)
pybullet.setPhysicsEngineParameter(
fixedTimeStep=self.time_step * self.frame_skip,
numSolverIterations=self.num_solver_iterations,
numSubSteps=self.frame_skip)
def _reset(self):
if self.scene is None:
self.scene = self.create_single_player_scene()
if not self.scene.multiplayer:
self.scene.episode_restart()
self.ordered_joints = []
self.frame = 0
self.done = 0
self.reward = 0
dump = 0
if self.self_collision:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
p.loadMJCF(
os.path.join(os.path.dirname(__file__), "mujoco_assets",
self.model_xml),
flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS))
else:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
p.loadMJCF(
os.path.join(os.path.dirname(__file__), "mujoco_assets",
self.model_xml)))
self.robot_specific_reset()
s = self.calc_state(
) # optimization: calc_state() can calculate something in self.* for calc_potential() to use
self.potential = self.calc_potential()
return s
def spawnPepper(self,
physics_client,
translation=[0, 0, 0],
quaternion=[0, 0, 0, 1],
spawn_ground_plane=False):
"""
Loads a Pepper model in the simulation
Parameters:
physics_client - The id of the simulated instance in which the
robot is supposed to be spawned
translation - List containing 3 elements, the spawning translation
[x, y, z] in the WORLD frame
quaternions - List containing 4 elements, the spawning rotation as
a quaternion [x, y, z, w] in the WORLD frame
spawn_ground_plane - If True, the pybullet_data ground plane will
be spawned
Returns:
pepper - A PepperVirtual object, the Pepper simulated instance
"""
pepper = PepperVirtual()
if spawn_ground_plane:
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadMJCF("mjcf/ground_plane.xml",
physicsClientId=physics_client)
pepper.loadRobot(translation,
quaternion,
physicsClientId=physics_client)
return pepper
def main():
p.connect(p.GUI)
p.setGravity(0,0,-9.8)
p.setTimeStep(1./240.)
floor = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
p.loadMJCF(floor)
#scene = BuildingScene('Placida',
# is_interactive=True,
# build_graph=True,
# pybullet_load_texture=True)
#scene.load()
robots = []
config = parse_config('../configs/jr2_reaching.yaml')
#jr = JR2_Kinova(config)
#robots.append(jr)
jr_head = JR2_Kinova_Head(config)
robots.append(jr_head)
positions = [[0, 0, 0]]#, [0,2,0]]
for robot, position in zip(robots, positions):
robot.load()
robot.set_position(position)
robot.robot_specific_reset()
robot.keep_still()
marker_position = [0,4,1]
marker = VisualMarker(visual_shape=p.GEOM_SPHERE, radius=0.1)
marker.load()
marker.set_position(marker_position)
print("Wait")
for _ in range(120): # keep still for 10 seconds
p.stepSimulation()
time.sleep(1./240.)
print("Move")
action_base = np.random.uniform(0, 1, robot.wheel_dim)
for _ in range(2400): # move with small random actions for 10 seconds
for robot, position in zip(robots, positions):
action_arm = np.random.uniform(-1, 1, robot.arm_dim)
action = np.concatenate([action_base, action_arm])
robot.apply_action([0,0,0.2,0.2,0,0,0,0,0])
p.stepSimulation()
time.sleep(1./240.0)
p.disconnect()
def test(args):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
fileName = os.path.join("mjcf", args.mjcf)
print("fileName")
print(fileName)
p.loadMJCF(fileName)
while (1):
p.stepSimulation()
p.getCameraImage(320,240)
time.sleep(0.01)
def test(args):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data_local.getDataPath())
fileName = os.path.join("mjcf", args.mjcf)
print("fileName")
print(fileName)
p.loadMJCF(fileName)
while (1):
p.stepSimulation()
p.getCameraImage(320, 240)
time.sleep(0.01)
def _spawnGroundPlane(self, physics_client):
"""
INTERNAL METHOD, Loads a ground plane
Parameters:
physics_client - The id of the simulated instance in which the
ground plane is supposed to be spawned
"""
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadMJCF("mjcf/ground_plane.xml",
physicsClientId=physics_client)
def reset(self): self.ordered_joints = [] if self.self_collision: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)) else: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml))) self.robot_specific_reset() s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use return s
def reset(self):
# reset the simulation < check
self._p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
self._p.resetSimulation()
self._p.setGravity(0, 0, -10)
p.setTimeStep(self.dt)
urdfRootPath = "/Users/rysul/URDFs"
_, self.botId = p.loadMJCF(
os.path.join(urdfRootPath, "biped.xml"),
flags=p.URDF_USE_SELF_COLLISION
| p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
| p.URDF_GOOGLEY_UNDEFINED_COLORS)
#self.rewards = 0
#self.done = 0
self.linkDict, self.jointDict, self.orderedJoints, self.robotBase = self.robot(
self._p)
# reset the joints at arbitrary position and 0 velocity
for j in self.orderedJoints:
j.reset_state(np.random.uniform(low=-1, high=1), 0)
self.initialZ = self.robotBase.current_position()[2] # modify?
observation = self.get_observation()
self.potential = -self.targetDist / self.dt
# add contact features later
self._p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
return observation
def __init__(self,
render=False,
torque_limits=[100] * 6,
init_noise=.005,
):
self.render = render
self.torque_limits = np.array(torque_limits)
self.init_noise = init_noise
low = -np.ones(6,dtype=np.float32)
self.action_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32)
low = -np.ones(17, dtype=np.float32)*np.inf
self.observation_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32)
if render:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self.plane_id = p.loadSDF(pybullet_data.getDataPath() + "/plane_stadium.sdf")[0]
self.walker_id = p.loadMJCF(pybullet_data.getDataPath() + "/mjcf/walker2d.xml")[0]
p.setGravity(0, 0, -9.8)
self.dt = p.getPhysicsEngineParameters()['fixedTimeStep']
self.reset()
def reset(self):
p.removeBody(self.world_id)
p.removeBody(self.arm_id)
p.removeBody(self.ball_id)
# self.plane_id = p.loadSDF(pybullet_data.getDataPath() + "/plane_stadium.sdf")[0]
self.world_id, self.arm_id, self.ball_id = p.loadMJCF('/home/sgillen/work/bball/python/assets/bball3.xml', flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
def set_motors_zero(body_id):
p.setJointMotorControlArray(body_id,
[i for i in range(p.getNumJoints(body_id))],
p.POSITION_CONTROL,
# positionGain=0.1,
# velocityGain=0.1,
forces=[0 for _ in range(p.getNumJoints(body_id))]
)
# world_id, arm_id, ball_id = p.loadMJCF('/home/sgillen/work/bball/python/assets/bball3.xml')
p.setGravity(0, -9.8, 0.0)
#p.resetBasePositionAndOrientation(self.ball_id, [0, 1.2, 0], [0, 0, 0, 1.0])
p.resetJointState(self.arm_id, 4, -np.pi / 4)
set_motors_zero(self.arm_id)
set_motors_zero(self.ball_id)
self.cur_step = 0
return self._get_obs()
def __init__(self, model_xml, robot_name, timestep, frame_skip, action_dim,
obs_dim, repeats):
self.action_space = gym.spaces.Box(-1.0, 1.0, shape=(action_dim, ))
float_max = np.finfo(np.float32).max
# obs space for problem is (R, obs_dim)
# R = number of repeats
# obs_dim d tuple
self.state_shape = (repeats, obs_dim)
self.observation_space = gym.spaces.Box(-float_max,
float_max,
shape=self.state_shape)
# no state until reset.
self.state = np.empty(self.state_shape, dtype=np.float32)
self.frame_skip = frame_skip
self.timestep = timestep
self.model_xml = model_xml
self.parts, self.joints, = self.getScene(p.loadMJCF(model_xml))
self.robot_name = robot_name
self.dt = timestep * frame_skip
self.metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second':
int(np.round(1.0 / timestep / frame_skip))
}
self._seed()
def __init__(self):
self.physicsClient = p.connect(
p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=0,
cameraPitch=0,
cameraTargetPosition=[0, 0, 0.6])
p.setGravity(0, 0, -9.81) # Explicitly set gravity.
self.ground = p.loadURDF("plane.urdf")
self.robot = p.loadMJCF("sustech_biped2d.xml",
flags=p.MJCF_COLORS_FROM_FILE)[0]
self.base_dof = 3 # degree of freedom of the base
self.simu_f = 500 # Simulation frequency, Hz
self.motion_f = 2 # Controlled motion frequency, Hz
self.zh = 0.55 # height of robot COM.
self.stance_idx = 0
self.pre_foot_contact = np.array([1, 0])
self.foot_contact = np.array([1, 0])
self.joints = self.get_joints()
self.n_j = len(self.joints)
self.q_vec = np.zeros(self.n_j)
self.dq_vec = np.zeros(self.n_j)
self.q_mat = np.zeros((self.simu_f * 3, self.n_j))
self.q_d_mat = np.zeros((self.simu_f * 3, self.n_j - self.base_dof))
self.t = 0
self.init_pos_and_vel_of_robot()
def robot_specific_reset(self):
WalkerBase.robot_specific_reset(self)
humanoidId = -1
numBodies = p.getNumBodies()
for i in range (numBodies):
bodyInfo = p.getBodyInfo(i)
if bodyInfo[1].decode("ascii") == 'humanoid':
humanoidId = i
## Spherical radiance/glass shield to protect the robot's camera
if self.glass_id is None:
glass_id = p.loadMJCF(os.path.join(self.physics_model_dir, "glass.xml"))[0]
#print("setting up glass", glass_id, humanoidId)
p.changeVisualShape(glass_id, -1, rgbaColor=[0, 0, 0, 0])
cid = p.createConstraint(humanoidId, -1, glass_id,-1,p.JOINT_FIXED,[0,0,0],[0,0,1.4],[0,0,1])
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
def reset(self):
global IDX
p.resetSimulation()
body_ids = p.loadMJCF('mjcf/point_mass.xml')
self._world_id = 0
self._goal_body_id = None
p.syncBodyInfo()
self.object_ids = []
for info in WALL_INFO:
self._load_box(info)
for info in OBSTACLE_INFO:
self._load_box(info)
self._setup_top_view()
p.setTimeStep(0.1)
dict_copy = {}
for key, val in GOAL.items():
dict_copy[key] = val
# x_min = 0.2
# x_max = 0.8
# xs = np.linspace(x_min, x_max, 10, endpoint=True)
dict_copy['x'] = 0.2
# IDX = (1 + IDX) % 10
self.goal_pos = np.asarray([dict_copy['x'], dict_copy['y']])
if self._goal_body_id is not None:
p.removeBody(self._goal_body_id)
self._load_goal(dict_copy)
rgb_img = self._get_top_view()
if self.img_obs:
obs = rgb_img
else:
obs = p.getLinkState(self._world_id, 1)[4][:2]
return obs
def episode_restart(self):
Scene.episode_restart(self) # contains cpp_world.clean_everything()
# 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
self.stadium = p.loadSDF("stadium.sdf")
self.ground_plane_mjcf = p.loadMJCF("mjcf/ground_plane.xml")
def init_pybullet():
# No need for GUI here. In fact, is this even necessary?
p.connect(p.DIRECT)
objs = p.loadMJCF(
"isaac/mjcf/humanoid_symmetric_no_ground.xml", # TODO fix path
flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
human = objs[0]
joints = []
center_of_mass_joint_id = -1
for j in range(p.getNumJoints(human)):
info = p.getJointInfo(human, j)
# Retrieve joints
# The following could be used to get only joints which are not fixed (can be activated)
is_fixed = info[2] == p.JOINT_FIXED
jname = info[1].decode('ascii')
lname = info[12].decode('ascii')
# print('joint: %s, link: %s' % (jname, lname))
parent_index = info[16]
joints.append((jname, j, parent_index, lname, is_fixed))
# Save the center of mass joint id
print('jname: %s -- com: %s' % (jname, mujoco_center_of_mass_joint))
if jname == mujoco_center_of_mass_joint:
center_of_mass_joint_id = j
return human, joints, center_of_mass_joint_id
def load(self):
"""
Load the robot model into pybullet
:return: body id in pybullet
"""
# flags = p.URDF_USE_MATERIAL_COLORS_FROM_MTL
flags = 0
if self.self_collision:
flags = flags | p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT
if self.model_type == "MJCF":
self.robot_ids = p.loadMJCF(os.path.join(self.physics_model_dir,
self.model_file),
flags=flags)
if self.model_type == "URDF":
self.robot_ids = (p.loadURDF(os.path.join(self.physics_model_dir,
self.model_file),
globalScaling=self.scale,
flags=flags), )
self.parts, self.jdict, self.ordered_joints, self.robot_body, self.robot_mass = self.parse_robot(
self.robot_ids)
assert "eyes" in self.parts, 'Please add a link named "eyes" in your robot URDF file with the same pose as the onboard camera. Feel free to check out assets/models/turtlebot/turtlebot.urdf for an example.'
self.eyes = self.parts["eyes"]
return self.robot_ids
def load_model(self, path, pose=None, fixed_base=True, scaling=1.0):
"""
Load the objects in the scene
:param path: str
Path to the object
:param pose: Pose
Position and Quaternion
:param fixed_base: str
Fixed in the scene
:param scaling: float
Scale object
:return: int
ID of the loaded object
"""
if path.endswith('.urdf'):
body = p.loadURDF(path, useFixedBase=fixed_base, flags=0, globalScaling=scaling,
physicsClientId=self.client_id)
elif path.endswith('.sdf'):
body = p.loadSDF(path, physicsClientId=self.client_id)
elif path.endswith('.xml'):
body = p.loadMJCF(path, physicsClientId=self.client_id)
elif path.endswith('.bullet'):
body = p.loadBullet(path, physicsClientId=self.client_id)
else:
raise ValueError(path)
if pose is not None:
self.set_pose(body, pose)
return body
def __init__(self,
xml_path,
freq=240,
headless=False,
kp=0.25,
kv=0.5,
max_torque=10,
g=-9.81):
# Set up PyBullet Simulator
if not headless:
pybullet.connect(
pybullet.GUI) # or p.DIRECT for non-graphical version
pybullet.resetDebugVisualizerCamera(
cameraDistance=1,
cameraYaw=45,
cameraPitch=-45,
cameraTargetPosition=[0, 0.0, 0])
else:
pybullet.connect(
pybullet.DIRECT) # or p.DIRECT for non-graphical version
pybullet.setAdditionalSearchPath(
pybullet_data.getDataPath()) # optionally
pybullet.setGravity(0, 0, g)
pybullet.setTimeStep(1 / freq)
self.model = pybullet.loadMJCF(xml_path)
print("")
print("Pupper body IDs:", self.model)
numjoints = pybullet.getNumJoints(self.model[1])
print("Number of joints in converted MJCF: ", numjoints)
print("Joint Info: ")
for i in range(numjoints):
print(pybullet.getJointInfo(self.model[1], i))
self.joint_indices = list(range(0, 24, 2))
def load(self):
self.build_graph = False
self.is_interactive = False
planeName = os.path.join(pybullet_data.getDataPath(),
"mjcf/ground_plane.xml")
self.ground = p.loadMJCF(planeName)[0]
p.changeDynamics(self.ground, -1, lateralFriction=1)
return [self.ground]
def main():
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 240.)
floor = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
p.loadMJCF(floor)
robots = []
config = parse_config('../configs/fetch_p2p_nav.yaml')
fetch = Fetch(config)
robots.append(fetch)
config = parse_config('../configs/jr_p2p_nav.yaml')
jr = JR2_Kinova(config)
robots.append(jr)
config = parse_config('../configs/locobot_p2p_nav.yaml')
locobot = Locobot(config)
robots.append(locobot)
config = parse_config('../configs/turtlebot_p2p_nav.yaml')
turtlebot = Turtlebot(config)
robots.append(turtlebot)
positions = [[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0]]
for robot, position in zip(robots, positions):
robot.load()
robot.set_position(position)
robot.robot_specific_reset()
robot.keep_still()
for _ in range(2400): # keep still for 10 seconds
p.stepSimulation()
time.sleep(1. / 240.)
for _ in range(2400): # move with small random actions for 10 seconds
for robot, position in zip(robots, positions):
action = np.random.uniform(-1, 1, robot.action_dim)
robot.apply_action(action)
p.stepSimulation()
time.sleep(1. / 240.0)
p.disconnect()
def reset(self): full_path = os.path.join(os.path.dirname(__file__), "..", "assets", "mjcf", self.model_xml) if (self.doneLoading==0): self.ordered_joints = [] self.doneLoading=1 if self.self_collision: self.objects = p.loadMJCF(full_path, flags=p.URDF_USE_SELF_COLLISION|p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects ) else: self.objects = p.loadMJCF(full_path) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects) self.robot_specific_reset() s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use return s
def reset(self):
self.ordered_joints = []
if self.self_collision:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
p.loadMJCF(
os.path.join("mjcf", self.model_xml),
flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS))
else:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
p.loadMJCF(os.path.join("mjcf", self.model_xml)))
self.robot_specific_reset()
s = self.calc_state(
) # optimization: calc_state() can calculate something in self.* for calc_potential() to use
return s
def load_agent(self):
dir_path = os.path.dirname(os.path.realpath(__file__))
agent_path = dir_path + "/data/MPL/MPL.xml"
objects = pb.loadMJCF(agent_path, flags=0)
self.agent = objects[0] #1 total
#if self.obj_to_classify:
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
self.hand_cid = pb.createConstraint(self.agent, -1, -1, -1,
pb.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], [0, 0, 0])
def load_agent(self):
objects = pb.loadMJCF("MPL/MPL.xml", flags=0)
self.agent = objects[0] #1 total
#if self.obj_to_classify:
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
#hand_po = pb.getBasePositionAndOrientation(self.agent)
#distance = math.sqrt(sum([(xi-yi)**2 for xi,yi in zip(obj_po[0],hand_po[0])])) #TODO faster euclidean
pb.resetBasePositionAndOrientation(
self.agent, (obj_po[0][0], obj_po[0][1] + 0.5, obj_po[0][2]),
obj_po[1])
def reset(self):
p.resetSimulation()
self.reacher = p.loadMJCF("Reacher.xml")
self.kp = 1
self.kd = .1
self.maxForce = 200
target_x = np.random.rand() * 0.6 - 0.4
target_y = np.random.rand() * 0.6 - 0.2
p.resetBasePositionAndOrientation(7, [target_x, target_y, .01],
[0.0, 0.0, 0.0, 1.0])
def robot_specific_reset(self):
"""
Humanoid robot specific reset
Add spherical radiance/glass shield to protect the robot's camera
"""
humanoidId = -1
numBodies = p.getNumBodies()
for i in range(numBodies):
bodyInfo = p.getBodyInfo(i)
if bodyInfo[1].decode("ascii") == 'humanoid':
humanoidId = i
# Spherical radiance/glass shield to protect the robot's camera
super(Humanoid, self).robot_specific_reset()
if self.glass_id is None:
glass_path = os.path.join(self.physics_model_dir,
"humanoid/glass.xml")
glass_id = p.loadMJCF(glass_path)[0]
self.glass_id = glass_id
p.changeVisualShape(self.glass_id, -1, rgbaColor=[0, 0, 0, 0])
p.createMultiBody(baseVisualShapeIndex=glass_id,
baseCollisionShapeIndex=-1)
cid = p.createConstraint(
humanoidId,
-1,
self.glass_id,
-1,
p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, self.glass_offset],
childFramePosition=[0, 0, 0])
robot_pos = list(self.get_position())
robot_pos[2] += self.glass_offset
robot_orn = self.get_orientation()
p.resetBasePositionAndOrientation(self.glass_id, robot_pos, robot_orn)
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += [
"right_hip_x", "right_hip_z", "right_hip_y", "right_knee"
]
self.motor_power += [100, 100, 300, 200]
self.motor_names += [
"left_hip_x", "left_hip_z", "left_hip_y", "left_knee"
]
self.motor_power += [100, 100, 300, 200]
self.motor_names += [
"right_shoulder1", "right_shoulder2", "right_elbow"
]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
def __init__(
self,
render=False,
torque_limits=[100] * 6,
init_noise=.005,
physics_params=None,
dynamics_params=None,
):
self.args = locals()
self.torque_limits = np.array(torque_limits)
self.init_noise = init_noise
self.cur_step = 0
low = -np.ones(6)
self.action_space = gym.spaces.Box(low=low,
high=-low,
dtype=np.float32)
low = -np.ones(19) * np.inf
self.observation_space = gym.spaces.Box(low=low,
high=-low,
dtype=np.float32)
if render:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self.plane_id = p.loadSDF(pybullet_data.getDataPath() +
"/plane_stadium.sdf")[0]
self.walker_id = p.loadMJCF(pybullet_data.getDataPath() +
"/mjcf/walker2d.xml")[0]
#flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)[0] # TODO not sure the self collision needs to be here..
p.setGravity(0, 0, -9.8)
if physics_params is None:
physics_params = {}
if dynamics_params is None:
dynamics_params = {}
p.changeDynamics(self.plane_id, -1, **dynamics_params)
for i in range(p.getNumJoints(self.walker_id)):
p.changeDynamics(self.walker_id, i, **dynamics_params)
p.changeDynamics(self.walker_id, -1, **dynamics_params)
p.setPhysicsEngineParameter(**physics_params)
self.dt = p.getPhysicsEngineParameters()['fixedTimeStep']
self.reset()
def load(self):
"""
Load the scene into pybullet
"""
plane_file = os.path.join(
pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
self.floor_body_ids += [p.loadMJCF(plane_file)[0]]
p.changeDynamics(self.floor_body_ids[0], -1, lateralFriction=1)
# white floor plane for visualization purpose if needed
p.changeVisualShape(self.floor_body_ids[0], -1, rgbaColor=[1, 1, 1, 1])
return self.floor_body_ids
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() # 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(),"stadium_no_collision.sdf") self.stadium = p.loadSDF(filename) planeName = os.path.join(pybullet_data.getDataPath(),"mjcf/ground_plane.xml") self.ground_plane_mjcf = p.loadMJCF(planeName) for i in self.ground_plane_mjcf: p.changeVisualShape(i,-1,rgbaColor=[0,0,0,0])
def reset(self):
self.initial_z = None
objs = p.loadMJCF(os.path.join(self.urdfRootPath,"mjcf/humanoid_symmetric_no_ground.xml"),flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.human = objs[0]
self.jdict = {}
self.ordered_joints = []
self.ordered_joint_indices = []
for j in range( p.getNumJoints(self.human) ):
info = p.getJointInfo(self.human, j)
link_name = info[12].decode("ascii")
if link_name=="left_foot": self.left_foot = j
if link_name=="right_foot": self.right_foot = j
self.ordered_joint_indices.append(j)
if info[2] != p.JOINT_REVOLUTE: continue
jname = info[1].decode("ascii")
self.jdict[jname] = j
lower, upper = (info[8], info[9])
self.ordered_joints.append( (j, lower, upper) )
p.setJointMotorControl2(self.human, j, controlMode=p.VELOCITY_CONTROL, force=0)
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
print("self.motors")
print(self.motors)
print("num motors")
print(len(self.motors))
import pybullet as p
import pybullet_data
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadSDF("stadium.sdf")
p.setGravity(0, 0, -10)
objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0]
p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1])
p.changeDynamics(sphere, -1, linearDamping=0.9)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
cid = p.connect(p.GUI)
p.resetSimulation()
useRealTime = 0
p.setRealTimeSimulation(useRealTime)
p.setGravity(0,0,-10)
p.loadSDF("stadium.sdf")
obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
human = obUids[0]
for i in range (p.getNumJoints(human)):
p.setJointMotorControl2(human,i,p.POSITION_CONTROL,targetPosition=0,force=500)
kneeAngleTargetId = p.addUserDebugParameter("kneeAngle",-4,4,-1)
maxForceId = p.addUserDebugParameter("maxForce",0,500,10)
kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL",-4,4,-1)
maxForceLeftId = p.addUserDebugParameter("maxForceL",0,500,10)
kneeJointIndex=11
#vr glove was custom build using Spectra Symbolflex sensors (4.5")
#inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle
#with BLE Link to receive serial (for wireless bluetooth serial)
import serial
import time
import pybullet as p
#first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY)
print(c)
if (c < 0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/MPL.xml")
hand = objects[0]
#clamp in range 400-600
#minV = 400
#maxV = 600
minVarray = [275, 280, 350, 290]
maxVarray = [450, 550, 500, 400]
pinkId = 0
middleId = 1
indexId = 2
thumbId = 3
p.setRealTimeSimulation(1)
p.setGravity(0,0,-10)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
hand=objects[0]
ho = p.getQuaternionFromEuler([3.14,-3.14/2,0])
hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[-0.05,0,0.02],[0.500000,0.300006,0.700000],ho)
print ("hand_cid")
print (hand_cid)
for i in range (p.getNumJoints(hand)):
p.setJointMotorControl2(hand,i,p.POSITION_CONTROL,0,0)
#clamp in range 400-600
#minV = 400
#maxV = 600
minV = 250
maxV = 450
import pybullet as p
import time
p.connect(p.DIRECT)
p.setGravity(0,0,-10)
p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.)
p.setPhysicsEngineParameter(numSubSteps=1)
objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000])
ob = objects[1]
p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923])
jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
#first let the humanoid fall
#p.setRealTimeSimulation(1)
#time.sleep(5)
p.setRealTimeSimulation(0)
#p.saveWorld("lyiing.py")
#now do a benchmark
print("Starting benchmark")
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"pybullet_humanoid_timings.json")
for i in range(1000):
p.stepSimulation()
p.stopStateLogging(logId)
