def reset(self): self.ordered_joints = [] print(os.path.join(os.path.dirname(__file__), "data", self.model_urdf)) if self.self_collision: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base, flags=p.URDF_USE_SELF_COLLISION)) else: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base)) 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 reset(self, bullet_client):
self._p = bullet_client
#print("Created bullet_client with id=", self._p._client)
if (self.doneLoading == 0):
self.ordered_joints = []
self.doneLoading = 1
if self.self_collision:
self.objects = self._p.loadMJCF(os.path.join(pybullet_data.getDataPath(), "mjcf",
self.model_xml),
flags=pybullet.URDF_USE_SELF_COLLISION |
pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
self._p, self.objects)
else:
self.objects = self._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._p, self.objects)
self.robot_specific_reset(self._p)
s = self.calc_state(
) # optimization: calc_state() can calculate something in self.* for calc_potential() to use
return s
def reset(self):
self.t = 0
if not self._isInitialized:
if self.enable_draw:
self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
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.urdf",[0,0,0],z2y)
#print("planeId=",self._planeId)
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()+"/motions/humanoid3d_walk.txt"
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
self._mocapData.Load(motionPath)
timeStep = 1./600
useFixedBase=False
self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData, timeStep, useFixedBase)
self._isInitialized = True
self._pybullet_client.setTimeStep(timeStep)
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
selfCheck = False
if (selfCheck):
curTime = 0
while self._pybullet_client.isConnected():
self._humanoid.setSimTime(curTime)
state = self._humanoid.getState()
#print("state=",state)
pose = self._humanoid.computePose(self._humanoid._frameFraction)
for i in range (10):
curTime+=timeStep
#taus = self._humanoid.computePDForces(pose)
#self._humanoid.applyPDForces(taus)
#self._pybullet_client.stepSimulation()
time.sleep(timeStep)
#print("numframes = ", self._humanoid._mocap_data.NumFrames())
startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
rnrange = 1000
rn = random.randint(0,rnrange)
startTime = float(rn)/rnrange * self._humanoid.getCycleTime()
self._humanoid.setSimTime(startTime)
self._humanoid.resetPose()
#this clears the contact points. Todo: add API to explicitly clear all contact points?
self._pybullet_client.stepSimulation()
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.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 __init__(self,
urdf_root=pybullet_data.getDataPath(),
render=False):
"""Initialize the gym environment.
Args:
urdf_root: The path to the urdf data folder.
render: Whether to render the simulation.
Raises:
ValueError: If the urdf_version is not supported.
"""
# Set up logging.
self._urdf_root = urdf_root
self._observation = []
self._env_step_counter = 0
self._is_render = render
self._cam_dist = 1.0
self._cam_yaw = 0
self._cam_pitch = -30
self._ground_id = None
self._pybullet_client = None
self._humanoid = None
self._control_time_step = 8.*(1./240.)#0.033333
self.seed()
observation_high = (self._get_observation_upper_bound())
observation_low = (self._get_observation_lower_bound())
action_dim = 36
self._action_bound = 3.14 #todo: check this
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_low, observation_high, dtype=np.float32)
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=50,
isEnableSelfCollision=True,
renders=True):
print("init")
self._timeStep = 0.01
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._p = p
if self._renders:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self._seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(9)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
def __init__(self,
urdf_root=pybullet_data.getDataPath(),
self_collision_enabled=True,
pd_control_enabled=False,
leg_model_enabled=True,
on_rack=False,
render=False):
"""Initialize the minitaur and ball gym environment.
Args:
urdf_root: The path to the urdf data folder.
self_collision_enabled: Whether to enable self collision in the sim.
pd_control_enabled: Whether to use PD controller for each motor.
leg_model_enabled: Whether to use a leg motor to reparameterize the action
space.
on_rack: Whether to place the minitaur on rack. This is only used to debug
the walking gait. In this mode, the minitaur's base is hanged midair so
that its walking gait is clearer to visualize.
render: Whether to render the simulation.
"""
super(MinitaurBallGymEnv, self).__init__(
urdf_root=urdf_root,
self_collision_enabled=self_collision_enabled,
pd_control_enabled=pd_control_enabled,
leg_model_enabled=leg_model_enabled,
on_rack=on_rack,
render=render)
self._cam_dist = 2.0
self._cam_yaw = -70
self._cam_pitch = -30
self.action_space = spaces.Box(np.array([-1]), np.array([1]))
self.observation_space = spaces.Box(np.array([-math.pi, 0]),
np.array([math.pi, 100]))
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=True):
print("init")
self._timeStep = 1./240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._width = 341
self._height = 256
self.terminated = 0
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
self._seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(7)
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
self.viewer = None
def __init__(self,
urdf_root=pybullet_data.getDataPath(),
action_repeat=1,
observation_noise_stdev=minitaur_gym_env.SENSOR_NOISE_STDDEV,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False,
render=False):
"""Initialize the minitaur standing up gym environment.
Args:
urdf_root: The path to the urdf data folder.
action_repeat: The number of simulation steps before actions are applied.
observation_noise_stdev: The standard deviation of observation noise.
self_collision_enabled: Whether to enable self collision in the sim.
motor_velocity_limit: The velocity limit of each motor.
pd_control_enabled: Whether to use PD controller for each motor.
render: Whether to render the simulation.
"""
super(MinitaurStandGymEnv, self).__init__(
urdf_root=urdf_root,
action_repeat=action_repeat,
observation_noise_stdev=observation_noise_stdev,
self_collision_enabled=self_collision_enabled,
motor_velocity_limit=motor_velocity_limit,
pd_control_enabled=pd_control_enabled,
accurate_motor_model_enabled=True,
motor_overheat_protection=True,
render=render)
# Set the action dimension to 1, and reset the action space.
action_dim = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
def __init__(self, urdfRootPath=pybullet_data.getDataPath(), timeStep=0.01): self.urdfRootPath = urdfRootPath self.timeStep = timeStep self.maxVelocity = .35 self.maxForce = 200. self.fingerAForce = 2 self.fingerBForce = 2.5 self.fingerTipForce = 2 self.useInverseKinematics = 1 self.useSimulation = 1 self.useNullSpace =21 self.useOrientation = 1 self.kukaEndEffectorIndex = 6 self.kukaGripperIndex = 7 #lower limits for null space self.ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] #upper limits for null space self.ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] #joint ranges for null space self.jr=[5.8,4,5.8,4,5.8,4,6] #restposes for null space self.rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] #joint damping coefficents self.jd=[0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001] self.reset()
def get_cube(_p, x, y, z):
body = _p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), [x, y, z])
_p.changeDynamics(body,-1, mass=1.2)#match Roboschool
part_name, _ = _p.getBodyInfo(body)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(_p, part_name, bodies, 0, -1)
def build_world(args, enable_draw):
arg_parser = build_arg_parser(args)
print("enable_draw=", enable_draw)
env = PyBulletDeepMimicEnv(arg_parser, enable_draw)
world = RLWorld(env, arg_parser)
#world.env.set_playback_speed(playback_speed)
motion_file = arg_parser.parse_string("motion_file")
print("motion_file=", motion_file)
bodies = arg_parser.parse_ints("fall_contact_bodies")
print("bodies=", bodies)
int_output_path = arg_parser.parse_string("int_output_path")
print("int_output_path=", int_output_path)
agent_files = pybullet_data.getDataPath() + "/" + arg_parser.parse_string("agent_files")
AGENT_TYPE_KEY = "AgentType"
print("agent_file=", agent_files)
with open(agent_files) as data_file:
json_data = json.load(data_file)
print("json_data=", json_data)
assert AGENT_TYPE_KEY in json_data
agent_type = json_data[AGENT_TYPE_KEY]
print("agent_type=", agent_type)
agent = PPOAgent(world, id, json_data)
agent.set_enable_training(False)
world.reset()
return world
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 build_arg_parser(args):
arg_parser = ArgParser()
arg_parser.load_args(args)
arg_file = arg_parser.parse_string('arg_file', '')
if (arg_file != ''):
path = pybullet_data.getDataPath() + "/args/" + arg_file
succ = arg_parser.load_file(path)
Logger.print2(arg_file)
assert succ, Logger.print2('Failed to load args from: ' + arg_file)
return arg_parser
def build_agent(world, id, file):
agent = None
with open(pybullet_data.getDataPath()+"/"+file) as data_file:
json_data = json.load(data_file)
assert AGENT_TYPE_KEY in json_data
agent_type = json_data[AGENT_TYPE_KEY]
if (agent_type == PPOAgent.NAME):
agent = PPOAgent(world, id, json_data)
else:
assert False, 'Unsupported agent type: ' + agent_type
return agent
def reset(self):
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)
return self._get_observation()
def __init__(self, pybullet_client, urdf_root=pybullet_data.getDataPath(), time_step=0.01):
"""Constructs an example simulation and reset it to the initial states.
Args:
pybullet_client: The instance of BulletClient to manage different
simulations.
urdf_root: The path to the urdf folder.
time_step: The time step of the simulation.
"""
self._pybullet_client = pybullet_client
self._urdf_root = urdf_root
self.m_actions_taken_since_reset = 0
self.time_step = time_step
self.stateId = -1
self.Reset(reload_urdf=True)
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(),"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) for j in range(p.getNumJoints(i)): p.changeDynamics(i,j,lateralFriction=0)
def __init__(self, pybullet_client, motion_data, baseShift):
"""Constructs a humanoid and reset it to the initial states.
Args:
pybullet_client: The instance of BulletClient to manage different
simulations.
"""
self._baseShift = baseShift
self._pybullet_client = pybullet_client
self.kin_client = BulletClient(
pybullet_client.DIRECT
) # use SHARED_MEMORY for visual debugging, start a GUI physics server first
self.kin_client.resetSimulation()
self.kin_client.setAdditionalSearchPath(pybullet_data.getDataPath())
self.kin_client.configureDebugVisualizer(self.kin_client.COV_ENABLE_Y_AXIS_UP, 1)
self.kin_client.setGravity(0, -9.8, 0)
self._motion_data = motion_data
print("LOADING humanoid!")
self._humanoid = self._pybullet_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0],
globalScaling=0.25,
useFixedBase=False)
self._kinematicHumanoid = self.kin_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0],
globalScaling=0.25,
useFixedBase=False)
#print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid))
pose = HumanoidPose()
for i in range(self._motion_data.NumFrames() - 1):
frameData = self._motion_data._motion_data['Frames'][i]
pose.PostProcessMotionData(frameData)
self._pybullet_client.resetBasePositionAndOrientation(self._humanoid, self._baseShift,
[0, 0, 0, 1])
self._pybullet_client.changeDynamics(self._humanoid, -1, linearDamping=0, angularDamping=0)
for j in range(self._pybullet_client.getNumJoints(self._humanoid)):
ji = self._pybullet_client.getJointInfo(self._humanoid, j)
self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0)
self._pybullet_client.changeVisualShape(self._humanoid, j, rgbaColor=[1, 1, 1, 1])
#print("joint[",j,"].type=",jointTypes[ji[2]])
#print("joint[",j,"].name=",ji[1])
self._initial_state = self._pybullet_client.saveState()
self._allowed_body_parts = [11, 14]
self.Reset()
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=False,
isDiscrete=False,
maxSteps = 1000):
#print("KukaGymEnv __init__")
self._isDiscrete = isDiscrete
self._timeStep = 1./240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._p = p
if self._renders:
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])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
self.seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([largeValObservation] * observationDim)
if (self._isDiscrete):
self.action_space = spaces.Discrete(7)
else:
action_dim = 3
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
self.timeStep = 0.01
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -10)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
p.resetJointState(self.cartpole, 1, initialAngle)
p.resetJointState(self.cartpole, 0, initialCartPos)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
return np.array(self.state)
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=10,
isEnableSelfCollision=True,
isDiscrete=False,
renders=True):
print("init")
self._timeStep = 0.01
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._ballUniqueId = -1
self._envStepCounter = 0
self._renders = renders
self._width = 100
self._height = 10
self._isDiscrete = isDiscrete
if self._renders:
self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
else:
self._p = bullet_client.BulletClient()
self.seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
if (isDiscrete):
self.action_space = spaces.Discrete(9)
else:
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(low=0,
high=255,
shape=(self._height, self._width, 4),
dtype=np.uint8)
self.viewer = None
def episode_restart(self, bullet_client):
self._p = bullet_client
Scene.episode_restart(self, bullet_client) # 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 = self._p.loadSDF(filename)
#filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf")
#self.ground_plane_mjcf = self._p.loadSDF(filename)
#
for i in self.ground_plane_mjcf:
self._p.changeDynamics(i, -1, lateralFriction=0.8, restitution=0.5)
self._p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.8])
self._p.configureDebugVisualizer(pybullet.COV_ENABLE_PLANAR_REFLECTION, 1)
def _reset(self):
# print("-----------reset simulation---------------")
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)
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 build_agents(self):
num_agents = self.env.get_num_agents()
print("num_agents=",num_agents)
self.agents = []
Logger.print2('')
Logger.print2('Num Agents: {:d}'.format(num_agents))
agent_files = self.arg_parser.parse_strings('agent_files')
print("len(agent_files)=",len(agent_files))
assert(len(agent_files) == num_agents or len(agent_files) == 0)
model_files = self.arg_parser.parse_strings('model_files')
assert(len(model_files) == num_agents or len(model_files) == 0)
output_path = self.arg_parser.parse_string('output_path')
int_output_path = self.arg_parser.parse_string('int_output_path')
for i in range(num_agents):
curr_file = agent_files[i]
curr_agent = self._build_agent(i, curr_file)
if curr_agent is not None:
curr_agent.output_dir = output_path
curr_agent.int_output_dir = int_output_path
Logger.print2(str(curr_agent))
if (len(model_files) > 0):
curr_model_file = model_files[i]
if curr_model_file != 'none':
curr_agent.load_model(pybullet_data.getDataPath()+"/"+curr_model_file)
self.agents.append(curr_agent)
Logger.print2('')
self.set_enable_training(self.enable_training)
return
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=50,
isEnableSelfCollision=True,
isDiscrete=False,
renders=False):
print("init")
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._isDiscrete = isDiscrete
if self._renders:
self._p = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
self._p = bullet_client.BulletClient()
self._seed()
#self.reset()
observationDim = 2 #len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
# 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(9)
else:
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)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=80,
isEnableSelfCollision=True,
renders=False,
isDiscrete=False,
maxSteps=8,
dv=0.06,
removeHeightHack=False,
blockRandom=0.3,
cameraRandom=0,
width=48,
height=48,
numObjects=5,
isTest=False):
"""Initializes the KukaDiverseObjectEnv.
Args:
urdfRoot: The diretory from which to load environment URDF's.
actionRepeat: The number of simulation steps to apply for each action.
isEnableSelfCollision: If true, enable self-collision.
renders: If true, render the bullet GUI.
isDiscrete: If true, the action space is discrete. If False, the
action space is continuous.
maxSteps: The maximum number of actions per episode.
dv: The velocity along each dimension for each action.
removeHeightHack: If false, there is a "height hack" where the gripper
automatically moves down for each action. If true, the environment is
harder and the policy chooses the height displacement.
blockRandom: A float between 0 and 1 indicated block randomness. 0 is
deterministic.
cameraRandom: A float between 0 and 1 indicating camera placement
randomness. 0 is deterministic.
width: The image width.
height: The observation image height.
numObjects: The number of objects in the bin.
isTest: If true, use the test set of objects. If false, use the train
set of objects.
"""
self._isDiscrete = isDiscrete
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._dv = dv
self._p = p
self._removeHeightHack = removeHeightHack
self._blockRandom = blockRandom
self._cameraRandom = cameraRandom
self._width = width
self._height = height
self._numObjects = numObjects
self._isTest = isTest
if self._renders:
self.cid = p.connect(p.SHARED_MEMORY)
if (self.cid < 0):
self.cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
else:
self.cid = p.connect(p.DIRECT)
self.seed()
if (self._isDiscrete):
if self._removeHeightHack:
self.action_space = spaces.Discrete(9)
else:
self.action_space = spaces.Discrete(7)
else:
self.action_space = spaces.Box(low=-1, high=1,
shape=(3, )) # dx, dy, da
if self._removeHeightHack:
self.action_space = spaces.Box(low=-1, high=1,
shape=(4, )) # dx, dy, dz, da
self.viewer = None
def __init__(self,
max_steps=500,
seed=1234,
gui=False,
map_width=4,
chamber_fraction=1/3,
observation_height=15,
iters=2000,
render_shape=(128*2, 128*2, 3),
observation_shape=(64, 64, 3),
obs_fov=60,
obs_aspect=1.0,
obs_nearplane=0.01,
obs_farplane=100,
reset_upon_touch=False,
touch_thresh=1.,
n_particles=5,
):
super(MaxwellsDemonEnv, self).__init__()
self._GRAVITY = -9.8
self._dt = 1/100.0
self.sim_steps = 5
# Temp. Doesn't currently make sense if smaller.
assert(map_width >= 4.)
self._map_area = self._map_width ** 2
# This constant doesn't affect the created width.
self._cube_width = 1.
self.dt = self._dt
self._game_settings = {"include_egocentric_vision": True}
# self.action_space = gym.spaces.Box(low=np.array([-1.2, -1.2, 0]), high=np.array([1.2,1.2,1]))
if gui:
# self._object.setPosition([self._x[self._step], self._y[self._step], 0.0] )
self._physicsClient = pybullet.connect(pybullet.GUI)
MaxwellsDemonEnv.count += 1
else:
self._physicsClient = pybullet.connect(pybullet.DIRECT)
RLSIMENV_PATH = os.environ['RLSIMENV_PATH']
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.resetSimulation()
#pybullet.setRealTimeSimulation(True)
pybullet.setGravity(0,0,self._GRAVITY)
pybullet.setTimeStep(self._dt)
# _planeId = pybullet.loadURDF("plane.urdf", )
pybullet.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 0.5]
cubeStartOrientation = pybullet.getQuaternionFromEuler([0.,0,0])
# These exist as part of the pybullet installation.
self._demon = pybullet.loadURDF(DATA_DIR + "/sphere2_yellow.urdf", cubeStartPos, cubeStartOrientation, useFixedBase=0)
self._particles = []
for _ in range(self._n_particles):
self._particles.append(pybullet.loadURDF("sphere2red.urdf", cubeStartPos, cubeStartOrientation, useFixedBase=0))
pybullet.setCollisionFilterPair(self._demon, self._particles[-1], -1, -1, enableCollision=0)
pybullet.setAdditionalSearchPath(RLSIMENV_PATH + '/rlsimenv/data')
# Add walls
self._blocks = []
self._wall_heights = [0.5, 1.5]
self.action_space = gym.spaces.Box(low=np.array([-5, -5, -10]), high=np.array([5, 5, 10])) #self._wall_heights[-1]]))
# self._wall_heights = (0.5, 1.5)
x_full_right = (1+self._chamber_fraction) * self._map_width + self._cube_width
x_inner_right = self._map_width
for z in self._wall_heights:
cube_locations = []
# Right walls
# cube_locations.extend([[self._map_width, y, z] for y in range(-self._map_width, -2)])
# cube_locations.extend([[self._map_width, y, z] for y in range(2, self._map_width)])
# Right wall
cube_locations.extend([[x_full_right, y, z] for y in np.arange(-self._map_width, self._map_width)])
# Left wall
cube_locations.extend([[-self._map_width, y, z] for y in range(-self._map_width, self._map_width)])
# Top Wall
cube_locations.extend([[x, self._map_width, z] for x in np.arange(-self._map_width, x_full_right + self._cube_width)])
# Bottom Wall
cube_locations.extend([[x, -self._map_width, z] for x in np.arange(-self._map_width, x_full_right + self._cube_width)])
# Add small room
# Add Right wall
# "Small" small room
# cube_locations.extend([[self._map_width+(self._map_width//2), y, z] for y in range(-self._map_width//2, self._map_width//2)])
# cube_locations.extend([[self._map_width, y, z] for y in range(-self._map_width, self._map_width)])
# Top wall
# cube_locations.extend([[x, self._map_width//2, z] for x in range(self._map_width, self._map_width+(self._map_width//2))])
# Bottom wall
# cube_locations.extend([[x, -self._map_width//2, z] for x in range(self._map_width, self._map_width+(self._map_width//2))])
# print ("cube_locations: ", cube_locations)
for loc in cube_locations:
blockId = pybullet.loadURDF(DATA_DIR + "/cube2.urdf", loc, cubeStartOrientation, useFixedBase=1)
self._blocks.append(blockId)
self._doors = []
# door_locations = [[self._map_width, y, 0] for y in range(-2, 2)]
door_locations = [[self._map_width, y, z] for y in range(-self._map_width, self._map_width)]
for loc in door_locations:
blockId = pybullet.loadURDF(DATA_DIR + "/door_cube.urdf", loc, cubeStartOrientation, useFixedBase=1)
self._doors.append(blockId)
pybullet.setCollisionFilterPair(self._demon, self._doors[-1], -1, -1, enableCollision=0)
self._roof = []
for x in np.arange(-self._map_width, x_full_right):
for y in range(-self._map_width, self._map_width):
loc = [x, y, self._wall_heights[-1]+0.5]
self._roof.append(pybullet.loadURDF(DATA_DIR + "/cube2.urdf",
loc,
cubeStartOrientation,
useFixedBase=1))
pybullet.changeVisualShape(self._roof[-1], -1, rgbaColor=[.4, .4, .4, 0.0])
for body in self._particles + [self._demon] + self._doors + self._blocks + self._roof:
pybullet.changeDynamics(body,
-1,
rollingFriction=0.,
spinningFriction=0.0,
lateralFriction=0.0,
linearDamping=0.0,
angularDamping=0.0,
restitution=1.0,
maxJointVelocity=10)
# disable the default velocity motors
#and set some position control with small force to emulate joint friction/return to a rest pose
jointFrictionForce = 1
for joint in range(pybullet.getNumJoints(self._demon)):
pybullet.setJointMotorControl2(self._demon,joint,pybullet.POSITION_CONTROL,force=jointFrictionForce)
#for i in range(10000):
# pybullet.setJointMotorControl2(botId, 1, pybullet.TORQUE_CONTROL, force=1098.0)
# pybullet.stepSimulation()
#import ipdb
#ipdb.set_trace()
pybullet.setRealTimeSimulation(1)
# lo = self.getObservation()["pixels"] * 0.0
# hi = lo + 1.0
lo = 0.
hi = 1.
self._game_settings['state_bounds'] = [lo, hi]
# self._observation_space = ActionSpace(self._game_settings['state_bounds'])
# self.observation_space = gym.spaces.Box(low=lo, high=hi, shape=(64,64,3))
self.observation_space = gym.spaces.Box(low=lo, high=hi, shape=(64,64,3))
def _generate_field(self, env, heightPerturbationRange=0.08):
env.pybullet_client.setAdditionalSearchPath(pd.getDataPath())
env.pybullet_client.configureDebugVisualizer(
env.pybullet_client.COV_ENABLE_RENDERING, 0)
heightPerturbationRange = heightPerturbationRange
if heightfieldSource == useProgrammatic:
for j in range(int(numHeightfieldColumns / 2)):
for i in range(int(numHeightfieldRows / 2)):
height = random.uniform(0, heightPerturbationRange)
self.heightfieldData[2 * i +
2 * j * numHeightfieldRows] = height
self.heightfieldData[2 * i + 1 +
2 * j * numHeightfieldRows] = height
self.heightfieldData[2 * i + (2 * j + 1) *
numHeightfieldRows] = height
self.heightfieldData[2 * i + 1 + (2 * j + 1) *
numHeightfieldRows] = height
terrainShape = env.pybullet_client.createCollisionShape(
shapeType=env.pybullet_client.GEOM_HEIGHTFIELD,
# meshScale=[.5, .5, 1.5],
meshScale=[.06, .06, .8],
heightfieldTextureScaling=(numHeightfieldRows - 1) / 2,
heightfieldData=self.heightfieldData,
numHeightfieldRows=numHeightfieldRows,
numHeightfieldColumns=numHeightfieldColumns)
terrain = env.pybullet_client.createMultiBody(0, terrainShape)
env.pybullet_client.resetBasePositionAndOrientation(
terrain, [0, 0, 0.0], [0, 0, 0, 1])
env.pybullet_client.changeDynamics(terrain,
-1,
lateralFriction=1.0)
if heightfieldSource == useDeepLocoCSV:
terrainShape = env.pybullet_client.createCollisionShape(
shapeType=env.pybullet_client.GEOM_HEIGHTFIELD,
meshScale=[.5, .5, 2.5],
fileName="heightmaps/ground0.txt",
heightfieldTextureScaling=128)
terrain = env.pybullet_client.createMultiBody(0, terrainShape)
env.pybullet_client.resetBasePositionAndOrientation(
terrain, [0, 0, 0], [0, 0, 0, 1])
env.pybullet_client.changeDynamics(terrain,
-1,
lateralFriction=1.0)
if heightfieldSource == useTerrainFromPNG:
terrainShape = env.pybullet_client.createCollisionShape(
shapeType=env.pybullet_client.GEOM_HEIGHTFIELD,
meshScale=[.05, .05, 1.8],
fileName="heightmaps/wm_height_out.png")
textureId = env.pybullet_client.loadTexture(
"heightmaps/gimp_overlay_out.png")
terrain = env.pybullet_client.createMultiBody(0, terrainShape)
env.pybullet_client.changeVisualShape(terrain,
-1,
textureUniqueId=textureId)
env.pybullet_client.resetBasePositionAndOrientation(
terrain, [0, 0, 0.1], [0, 0, 0, 1])
env.pybullet_client.changeDynamics(terrain,
-1,
lateralFriction=1.0)
self.hf_id = terrainShape
self.terrainShape = terrainShape
print("TERRAIN SHAPE: {}".format(terrainShape))
env.pybullet_client.changeVisualShape(terrain,
-1,
rgbaColor=[1, 1, 1, 1])
env.pybullet_client.configureDebugVisualizer(
env.pybullet_client.COV_ENABLE_RENDERING, 1)
clientId = p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0, 0, -10)
useRealTimeSim = 1 #if set to 0, stepping function must be used to step simulation
p.setRealTimeSimulation(useRealTimeSim)
#plane = p.loadURDF( os.path.join( pybullet_data.getDataPath(), "plane.urdf" ))
#plane = p.loadURDF( os.path.join( pybullet_data.getDataPath(), "romans_urdf_files/octopus_files/octopus_simple_2_troubleshoot.urdf" ) )
#default urdf to troubleshoot is octopus_simple_2_troubleshoot.urdf
#plane = p.loadURDF( os.path.join( pybullet_data.getDataPath(), "romans_urdf_files/octopus_files/python_scripts_edit_urdf/output2.urdf" ) )
plane = p.loadURDF(
os.path.join(
pybullet_data.getDataPath(),
"romans_urdf_files/octopus_files/python_scripts_edit_urdf/octopus_generated_8_links.urdf"
))
#plane = p.loadURDF("octopus_generated_3_links.urdf")
#plane = p.loadURDF( os.path.join(os.getcwd(), "output2.urdf") )
#blockPos = [0,0,3]
#plane = p.loadURDF( os.path.join( pybullet_data.getDataPath(), "cube_small.urdf" ) , basePosition = blockPos )
#p.resetSimulation()
#robotId = p.loadURDF( os.path.join(pybullet_data.getDataPath(), "romans_urdf_files/octopus_files/my_robot.urdf" ) )
##################velocity??????? 2pm jan 6, 2019
#angles = p.calculateInverseKinematics(bodyUniqueId=plane, endEffectorLinkIndex=0, targetPosition = [-0.5 , 0, 0 ], solver=p.IK_DLS )
def set_minimal_environment():
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF("plane.urdf")
pybullet.setGravity(0, 0, -9.81)
import pybullet as p
import json
import time
import pybullet_data
useGUI = True
if useGUI:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
useZUp = False
useYUp = not useZUp
showJointMotorTorques = False
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.4,
cameraYaw=-94,
cameraPitch=-14,
cameraTargetPosition=[0.24, -0.02, -0.09])
def __init__(self,
robot,
model_id,
gravity,
timestep,
frame_skip,
collision_enabled=True,
env=None):
Scene.__init__(self, gravity, timestep, frame_skip, env)
# 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(get_model_path(model_id), "mesh_z_up.obj")
#filename = os.path.join(get_model_path(model_id), "3d", "blender.obj")
#textureID = p.loadTexture(os.path.join(get_model_path(model_id), "3d", "rgb.mtl"))
if robot.model_type == "MJCF":
MJCF_SCALING = robot.mjcf_scaling
scaling = [
1.0 / MJCF_SCALING, 1.0 / MJCF_SCALING, 0.6 / MJCF_SCALING
]
else:
scaling = [1, 1, 1]
magnified = [2, 2, 2]
if collision_enabled:
collisionId = p.createCollisionShape(
p.GEOM_MESH,
fileName=filename,
meshScale=scaling,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
view_only_mesh = os.path.join(get_model_path(model_id),
"mesh_view_only_z_up.obj")
if os.path.exists(view_only_mesh):
visualId = p.createVisualShape(
p.GEOM_MESH,
fileName=view_only_mesh,
meshScale=scaling,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
else:
visualId = -1
boundaryUid = p.createMultiBody(
baseCollisionShapeIndex=collisionId,
baseVisualShapeIndex=visualId)
else:
visualId = p.createVisualShape(p.GEOM_MESH,
fileName=filename,
meshScale=scaling,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
boundaryUid = p.createMultiBody(baseCollisionShapeIndex=-1,
baseVisualShapeIndex=visualId)
p.changeDynamics(boundaryUid, -1, lateralFriction=1)
#print(p.getDynamicsInfo(boundaryUid, -1))
self.scene_obj_list = [(boundaryUid, -1)] # baselink index -1
planeName = os.path.join(pybullet_data.getDataPath(),
"mjcf/ground_plane.xml")
self.ground_plane_mjcf = p.loadMJCF(planeName)
if "z_offset" in self.env.config:
z_offset = self.env.config["z_offset"]
else:
z_offset = -10 #with hole filling, we don't need ground plane to be the same height as actual floors
p.resetBasePositionAndOrientation(self.ground_plane_mjcf[0],
posObj=[0, 0, z_offset],
ornObj=[0, 0, 0, 1])
p.changeVisualShape(
boundaryUid,
-1,
rgbaColor=[168 / 255.0, 164 / 255.0, 92 / 255.0, 1.0],
specularColor=[0.5, 0.5, 0.5])
def reset(self):
startTime = 0. #float(rn)/rnrange * self._humanoid.getCycleTime()
self.t = startTime
if not self._isInitialized:
if self.enable_draw:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=p1.GUI)
#disable 'GUI' since it slows down a lot on Mac OSX and some other platforms
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)
#print("planeId=",self._planeId)
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()+"/motions/humanoid3d_walk.txt"
motionPath = pybullet_data.getDataPath(
) + "/motions/humanoid3d_backflip.txt"
self._mocapData.Load(motionPath)
timeStep = 1. / 600
useFixedBase = False
self._humanoid = humanoid_stable_pd.HumanoidStablePD(
self._pybullet_client, self._mocapData, timeStep, useFixedBase)
self._isInitialized = True
self._pybullet_client.setTimeStep(timeStep)
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
selfCheck = False
if (selfCheck):
curTime = 0
while self._pybullet_client.isConnected():
self._humanoid.setSimTime(curTime)
state = self._humanoid.getState()
#print("state=",state)
pose = self._humanoid.computePose(
self._humanoid._frameFraction)
for i in range(10):
curTime += timeStep
#taus = self._humanoid.computePDForces(pose)
#self._humanoid.applyPDForces(taus)
#self._pybullet_client.stepSimulation()
time.sleep(timeStep)
#print("numframes = ", self._humanoid._mocap_data.NumFrames())
#startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
rnrange = 1000
rn = random.randint(0, rnrange)
self._humanoid.setSimTime(startTime)
self._humanoid.resetPose()
#this clears the contact points. Todo: add API to explicitly clear all contact points?
#self._pybullet_client.stepSimulation()
self._humanoid.resetPose()
self.needs_update_time = self.t - 1 #force update
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=3.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):
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
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)
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(random=True)
for i in range(self.JOINT_DIM + 2):
p.resetJointState(self.soccerbotUid, i, standing_poses[i])
# WARM UP SIMULATION
if self.WARM_UP:
warm_up = self.np_random.randint(0, 11)
for _ in range(warm_up):
p.stepSimulation()
p.stepSimulation()
# TODO set state???
# TODO get observation
# self.st = RollingAvg(256, 0.01, 0.01)
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=self.dtype)
start_pos = np.array([0, 0, self.STANDING_HEIGHT], dtype=self.dtype)
observation = np.concatenate((joints_pos, imu, start_pos, feet))
# Roll Stats
if self._renders:
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
return observation
import os
import pybullet as pb
import pybullet_data
import matplotlib.pyplot as plt
from mimic.dataset import KinematicsDataset
from mimic.models import KinemaNet
from mimic.trainer import TrainCache
pbdata_path = pybullet_data.getDataPath()
urdf_path = os.path.join(pbdata_path, 'kuka_iiwa', 'model.urdf')
joint_names = ['lbr_iiwa_joint_{}'.format(idx + 1) for idx in range(7)]
link_names = ['lbr_iiwa_link_7']
dataset = KinematicsDataset.from_urdf(urdf_path,
joint_names,
link_names,
n_sample=10)
tcache = TrainCache[KinemaNet].load('kinematics', KinemaNet)
tcache.visualize()
plt.show()
model = tcache.best_model
inp, out = dataset[1]
out_exp = model.layer(inp)
print(out)
print(out_exp)
#rudimentary MuJoCo mjcf to ROS URDF converter using the UrdfEditor
import pybullet_utils.bullet_client as bc
import pybullet_data as pd
import pybullet_utils.urdfEditor as ed
import argparse
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--mjcf', help='MuJoCo xml file to be converted to URDF', default='mjcf/humanoid.xml')
args = parser.parse_args()
p = bc.BulletClient()
p.setAdditionalSearchPath(pd.getDataPath())
objs = p.loadMJCF(args.mjcf, flags=p.URDF_USE_IMPLICIT_CYLINDER)
for o in objs:
#print("o=",o, p.getBodyInfo(o), p.getNumJoints(o))
humanoid = objs[o]
ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(humanoid, p._client)
robotName = str(p.getBodyInfo(o)[1],'utf-8')
partName = str(p.getBodyInfo(o)[0], 'utf-8')
print("robotName=",robotName)
print("partName=",partName)
saveVisuals=False
ed0.saveUrdf(robotName+"_"+partName+".urdf", saveVisuals)
def __init__(self, config: EnvContext):
self.max_steps_one_episode = config["max_steps_one_episode"]
self.is_render = config["is_render"]
self.is_good_view = config["is_good_view"]
if self.is_render:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self.x_low_obs = 0.2
self.x_high_obs = 0.7
self.y_low_obs = -0.3
self.y_high_obs = 0.3
self.z_low_obs = 0
self.z_high_obs = 0.55
self.x_low_obs_for_obs_space = 0
self.x_high_obs_for_obs_space = 1
self.y_low_obs_for_obs_space = -1
self.y_high_obs_for_obs_space = 1
self.z_low_obs_for_obs_space = -0.2
self.z_high_obs_for_obs_space = 0.8
self.x_low_action = -0.4
self.x_high_action = 0.4
self.y_low_action = -0.4
self.y_high_action = 0.4
self.z_low_action = -0.6
self.z_high_action = 0.3
p.resetDebugVisualizerCamera(cameraDistance=1.5,
cameraYaw=0,
cameraPitch=-40,
cameraTargetPosition=[0.55, -0.35, 0.2])
self.action_space = spaces.Box(low=np.array(
[self.x_low_action, self.y_low_action, self.z_low_action]),
high=np.array([
self.x_high_action,
self.y_high_action,
self.z_high_action
]),
dtype=np.float32)
self.observation_space = spaces.Box(low=np.array([
self.x_low_obs_for_obs_space, self.y_low_obs_for_obs_space,
self.z_low_obs_for_obs_space
]),
high=np.array([
self.x_high_obs_for_obs_space,
self.y_high_obs_for_obs_space,
self.z_high_obs_for_obs_space
]),
dtype=np.float32)
self.step_counter = 0
self.urdf_root_path = pybullet_data.getDataPath()
# lower limits for null space
self.lower_limits = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
# upper limits for null space
self.upper_limits = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
# joint ranges for null space
self.joint_ranges = [5.8, 4, 5.8, 4, 5.8, 4, 6]
# restposes for null space
self.rest_poses = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
# joint damping coefficents
self.joint_damping = [
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001
]
self.init_joint_positions = [
0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684,
-0.006539
]
self.orientation = p.getQuaternionFromEuler(
[0., -math.pi, math.pi / 2.])
self.seed()
def reset(self):
if (self.physicsClientId <
0): #if it is the first time we are loading the simulations
self.ownsPhysicsClient = True #this
if self.isRender:
self._p = bullet_client.BulletClient(
connection_mode=pybullet.GUI
) # connect to physics server, and render through Graphical User Interface (GUI)
else:
self._p = bullet_client.BulletClient(
) #connect to physics server, and DO NOT render through graphical user interface
self.physicsClientId = self._p._client # get the client ID from physics server, this makes self.physicsClientId become a value greater than 0 (this value indicates the server that this environement instance is connected to)
self._p.resetSimulation(
) # reset physics server and remove all objects (urdf files, or mjcf, or )
#self.number_of_links_urdf = number_of_links_urdf
self.model_urdf = "romans_urdf_files/octopus_files/python_scripts_edit_urdf/octopus_generated_" + str(
self.number_of_links_urdf) + "_links.urdf"
#load URDF into pybullet physics simulator
self.octopusBodyUniqueId = self._p.loadURDF(
fileName=os.path.join(pybullet_data.getDataPath(),
self.model_urdf),
flags=self._p.URDF_USE_SELF_COLLISION
| self._p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
print("Loading urdf from: ",
os.path.join(pybullet_data.getDataPath(), self.model_urdf))
#turn off all motors so that joints are not stiff for the rest of the simulation
self._p.setJointMotorControlArray(
bodyUniqueId=self.octopusBodyUniqueId,
jointIndices=list(range(self.number_of_joints_urdf)),
controlMode=self._p.POSITION_CONTROL,
positionGains=[0.1] * self.number_of_links_urdf,
velocityGains=[0.1] * self.number_of_links_urdf,
forces=[0] * self.number_of_links_urdf)
#this loop unlocks all of the arm's joints
for i in range(self.number_of_joints_urdf):
if self.constraintUniqueIdsList[i] is not None:
self._p.removeConstraint(self.constraintUniqueIdsList[i])
self.constraintUniqueIdsList[i] = None
#this loop locks all of the arm's joints
for i in range(self.number_of_joints_urdf - 1):
#get joint's child link name (for sanity check to ensure that we are indexing correct values from the list)
#jointChildLinkName=jointParentFramePosition=p.getJointInfo(self.octopusBodyUniqueId, jointIndex=0, physicsClientId=self.physicsClientId)[12]
#get joint frame position, relative to parent link frame
#jointParentLinkFramePosition=p.getJointInfo(octopusBodyUniqueId, jointIndex=0, physicsClientId=physicsClientId)[14]
#get joint frame orientation, relative to parent CoM link frame
#jointParentLinkFrameOrientation=p.getJointInfo(octopusBodyUniqueId, jointIndex=0, physicsClientId=physicsClientId)[15]
#get position of the joint frame, relative to a given child CoM Coordidinate Frame, (other posibility: get world origin (0,0,0) if no child specified for this joint)
#jointChildLinkFramePosition=p.getjointState
#get position of the joint frame, relative to a given child center of mass coordinate frame, (or get the world origin if no child is specified for this joint)
#jointChildLinkFrameOrientation=p.getLinkState
#constraintUniqueIds.append( self._p.createConstraint(parentBodyUniqueId=self.octopusBodyUniqueId , parentLinkIndex=i childBodyUniqueId=self.octopusBodyUniqueId , childLinkIndex=i+1 , jointType=JOINT_FIXED , jointAxis=[0,0,1], parentFramePosition= [0,0,1], childFramePosition=[0,0,1], parentFrameOrientation=[0,0,1], childFrameOrientation=[0,0,1], physicsClientId=self.physicsClientId ) )
#constraintUniqueIds.append( p.createConstraint( parentBodyUniqueId=octopusBodyUniqueId, parentLinkIndex=6, childBodyUniqueId=octopusBodyUniqueId, childLinkIndex=6+1, jointType=p.JOINT_FIXED, jointAxis=[1,0,0], parentFramePosition= [0,0,1], childFramePosition=[0,0,-1], parentFrameOrientation=p.getJointInfo(bodyUniqueId=octopusBodyUniqueId, jointIndex=7, physicsClientId=physicsClientId)[15], childFrameOrientation= p.getQuaternionFromEuler(eulerAngles=[-p.getJointState(bodyUniqueId=octopusBodyUniqueId, jointIndex=7, physicsClientId=physicsClientId)[0],-0,-0]), physicsClientId=physicsClientId ) )
self.constraintUniqueIdsList[i] = self._p.createConstraint(
parentBodyUniqueId=self.octopusBodyUniqueId,
parentLinkIndex=i - 1,
childBodyUniqueId=self.octopusBodyUniqueId,
childLinkIndex=i,
jointType=self._p.JOINT_FIXED,
jointAxis=[1, 0, 0],
parentFramePosition=[0, 0, 1],
childFramePosition=[0, 0, -1],
parentFrameOrientation=self._p.getJointInfo(
bodyUniqueId=self.octopusBodyUniqueId,
jointIndex=i,
physicsClientId=self.physicsClientId)[15],
childFrameOrientation=self._p.
getQuaternionFromEuler(eulerAngles=[
-self._p.getJointState(
bodyUniqueId=self.octopusBodyUniqueId,
jointIndex=i,
physicsClientId=self.physicsClientId)[0], -0, -0
]),
physicsClientId=self.physicsClientId)
#unlock last 3 joints
for i in range(3):
if self.constraintUniqueIdsList[self.number_of_joints_urdf -
1 - i] is not None:
self._p.removeConstraint(
userConstraintUniqueId=self.constraintUniqueIdsList[
self.number_of_joints_urdf - 1 - i],
physicsClientId=self.physicsClientId)
self.constraintUniqueIdsList[self.number_of_joints_urdf -
1 - i] = None
#indicate urdf file for visualizing goal point and load it
self.goal_point_urdf = "sphere8cube.urdf"
self.goalPointUniqueId = self._p.loadURDF(
fileName=os.path.join(pybullet_data.getDataPath(),
self.goal_point_urdf),
basePosition=[self.target_x, self.target_y, self.target_z],
useFixedBase=1
) #flags=self._p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
#self.goalPointUniqueId = self._p.createVisualShape(physicsClientId=self.physicsClientId, shapeType=self._p.GEOM_SPHERE, radius=4, specularColor=[0.5,0.5,0.5]) #secularcolor = [r,g,b]
#self._p.resetBasePositionAndOrientation(bodyUniqueId=self.goalPointUniqueId, physicsClientId=self.physicsClientId, posObj=[self.target_x, self.target_y, self.target_z], ornObj=[0,0,0,1])
#function usage example: 'bodyUniqueId = pybullet.loadURDF(fileName="path/to/file.urdf", basePosition=[0.,0.,0.], baseOrientation=[0.,0.,0.,1.], useMaximalCoordinates=0, useFixedBase=0, flags=0, globalScaling=1.0, physicsClientId=0)\nCreate a multibody by loading a URDF file.'
#optionally enable EGL for faster headless rendering
try:
if os.environ["PYBULLET_EGL"]:
con_mode = self._p.getConnectionInfo()['connectionMethod']
if con_mode == self._p.DIRECT:
egl = pkgutil.get_loader('eglRenderer')
if (egl):
self._p.loadPlugin(egl.get_filename(),
"_eglRendererPlugin")
else:
self._p.loadPlugin("eglRendererPlugin")
except:
pass
# get Physics Client Id
self.physicsClientId = self._p._client
# enable gravity
self._p.setGravity(gravX=0,
gravY=0,
gravZ=-9.8 * 0 / 1,
physicsClientId=self.physicsClientId)
self._p.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
self.joints_and_links = robotJointsandLinks(
number_of_joints=self.number_of_joints_urdf,
number_of_links=self.number_of_links_urdf,
bodyUniqueId=self.octopusBodyUniqueId,
physicsClientId=self.physicsClientId
) #make dictionaries of joints and links (refer to robotJointsandLinks() class)
#end of "if first loading pybullet client"
####non-first-time reset code starts here
#reset goal target to random one
self.target_x = self.target_x # no x comoponent for 2D case
self.target_y = random.uniform(
self.y_lower_limit,
self.y_upper_limit) # choose y coordinates such that arm can reach
self.target_z = random.uniform(
self.z_lower_limit,
self.z_upper_limit) # choose z coordinates such that arm can reach
#correspondingly move visual representation of goal target
self._p.resetBasePositionAndOrientation(
bodyUniqueId=self.goalPointUniqueId,
posObj=[self.target_x, self.target_y, self.target_z],
ornObj=[0, 0, 0, 1],
physicsClientId=self.physicsClientId)
#reset joint positions and velocities
for i in range(self.number_of_joints_urdf):
#ROLLOUT1: all positions=0, all velocities=0 #(initially stretched and static)
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=0, targetVelocity=0 ) #tagetValue is angular (or xyz) position #targetvelocity is angular (or xyz) veloity
#ROLLOUT2: all positions=pi, all velocities=0 #(initially contracted and static)
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=np.pi, targetVelocity=0 )
#ROLLOUT3: all positions=0, all velocities=(-pi*c, pi*c) # (initially stretched and dynamic)
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=0, targetVelocity=random.uniform(-np.pi*0.5, np.pi*0.5) )
#ROLLOUT4: all positions = pi (IDEA: try uniform sampling around it?), all velocities=(-pi*c,*pi*c) (initially contracted and dynamic)
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=np.pi, targetVelocity=random.uniform(-np.pi*2, np.pi*2) )
#TRAINING1: initial positions=random, initial velocities=0 #essentialltially arm is reset to (semi) contracted and dynamic
self._p.resetJointState(
bodyUniqueId=self.octopusBodyUniqueId,
physicsClientId=self.physicsClientId,
jointIndex=i,
targetValue=random.uniform(-np.pi * -1, np.pi * 1),
targetVelocity=random.uniform(-np.pi * 0.5 * 0,
np.pi * 0.5 * 0)
) #tagetValue is angular (or xyz) position #targetvelocity is angular (or xyz) veloity
#TRAINING2: random initial positions=(-pi, pi), velocities=0
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=random.uniform(-np.pi, np.pi), targetVelocity=random.uniform(-np.pi*0.5*0, np.pi*0.5*0) ) #tagetValue is angular (or xyz) position #targetvelocity is angular (or xyz) veloity
#LAST LEFT OFF HERE
#TRAINING3: random initial positions=(-pi, pi), velocities=(-pi*c, pi*c)
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=random.uniform(-np.pi, np.pi), targetVelocity=random.uniform(-np.pi/4, np.pi/4) ) #tagetValue is angular (or xyz) position #targetvelocity is angular (or xyz) veloity
#TRAINING4: random initial positions=0, velocities=(-pi*c, pi*c)
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=random.uniform(-np.pi, np.pi), targetVelocity=random.uniform(-np.pi*0.5*0, np.pi*0.5*0) ) #tagetValue is angular (or xyz) position #targetvelocity is angular (or xyz) veloity
#TRAINING5: random initial positions=(-pi*c, pi*c), velocities=0
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=random.uniform(-np.pi/4, np.pi/4), targetVelocity=random.uniform(-np.pi*0.5*0, np.pi*0.5*0) ) #tagetValue is angular (or xyz) position #targetvelocity is angular (or xyz) veloity
#TRAINING6: random initial positions=(-pi*c, pi*c), velocities=(-pi*c, pi*c)
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=random.uniform(-np.pi/4, np.pi/4), targetVelocity=random.uniform(-np.pi*0.5*0, np.pi*0.5*0) ) #tagetValue is angular (or xyz) position #targetvelocity is angular (or xyz) veloity
#TRAINING7: all initial positions=0, velocities=(pi*c,pi*c) #essentially arm is is reset to stretched and dynamic
#TRAINING8: all initial positions=pi, velocities=(pi*c,pi*c) #initiall arm is reset to contracted and dynamic
self.time_stamp = 0
#make first link angle face downward
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=0, targetValue=random.uniform(np.pi/2*2, np.pi/2*2), targetVelocity=random.uniform(-np.pi*0, np.pi*0) ) #reset base link
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=0, targetValue=random.uniform(-np.pi/2*1, np.pi/2*1), targetVelocity=random.uniform(-np.pi*0, np.pi*0) ) #reset base link
#randomize joint positions and velocities
#for i in range(self.number_of_joints_urdf):
# pass
#self._p.resetJointState(bodyUniqueId = self.octopusBodyUniqueId, physicsClientId=self.physicsClientId , jointIndex=i, targetValue=random.uniform(-np.pi/2,np.pi/2), targetVelocity=0 ) #tagetValue is angular (or xyz) position #targetvelocity is angular (or xyz) veloity
#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
self.states = None
self.step_observations = self.states
dump = 0
#s = self.robot.reset(self._p)
#self.potential = self.robot.calc_potential()
self.states = self.get_states()
self.step_observations = self.states
return self.step_observations
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
def __init__(self,
urdf_root=pybullet_data.getDataPath(),
urdf_version=None,
distance_weight=1.0,
energy_weight=0.005,
shake_weight=0.0,
drift_weight=0.0,
distance_limit=float("inf"),
observation_noise_stdev=SENSOR_NOISE_STDDEV,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False,
leg_model_enabled=True,
accurate_motor_model_enabled=False,
remove_default_joint_damping=False,
motor_kp=1.0,
motor_kd=0.02,
control_latency=0.0,
pd_latency=0.0,
torque_control_enabled=False,
motor_overheat_protection=False,
hard_reset=True,
on_rack=False,
render=False,
num_steps_to_log=1000,
action_repeat=1,
control_time_step=None,
env_randomizer=None,
forward_reward_cap=float("inf"),
reflection=True,
log_path=None):
"""Initialize the minitaur gym environment.
Args:
urdf_root: The path to the urdf data folder.
urdf_version: [DEFAULT_URDF_VERSION, DERPY_V0_URDF_VERSION,
RAINBOW_DASH_V0_URDF_VERSION] are allowable
versions. If None, DEFAULT_URDF_VERSION is used. DERPY_V0_URDF_VERSION
is the result of first pass system identification for derpy.
We will have a different URDF and related Minitaur class each time we
perform system identification. While the majority of the code of the
class remains the same, some code changes (e.g. the constraint location
might change). __init__() will choose the right Minitaur class from
different minitaur modules based on
urdf_version.
distance_weight: The weight of the distance term in the reward.
energy_weight: The weight of the energy term in the reward.
shake_weight: The weight of the vertical shakiness term in the reward.
drift_weight: The weight of the sideways drift term in the reward.
distance_limit: The maximum distance to terminate the episode.
observation_noise_stdev: The standard deviation of observation noise.
self_collision_enabled: Whether to enable self collision in the sim.
motor_velocity_limit: The velocity limit of each motor.
pd_control_enabled: Whether to use PD controller for each motor.
leg_model_enabled: Whether to use a leg motor to reparameterize the action
space.
accurate_motor_model_enabled: Whether to use the accurate DC motor model.
remove_default_joint_damping: Whether to remove the default joint damping.
motor_kp: proportional gain for the accurate motor model.
motor_kd: derivative gain for the accurate motor model.
control_latency: It is the delay in the controller between when an
observation is made at some point, and when that reading is reported
back to the Neural Network.
pd_latency: latency of the PD controller loop. PD calculates PWM based on
the motor angle and velocity. The latency measures the time between when
the motor angle and velocity are observed on the microcontroller and
when the true state happens on the motor. It is typically (0.001-
0.002s).
torque_control_enabled: Whether to use the torque control, if set to
False, pose control will be used.
motor_overheat_protection: Whether to shutdown the motor that has exerted
large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
(OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more
details.
hard_reset: Whether to wipe the simulation and load everything when reset
is called. If set to false, reset just place the minitaur back to start
position and set its pose to initial configuration.
on_rack: Whether to place the minitaur on rack. This is only used to debug
the walking gait. In this mode, the minitaur's base is hanged midair so
that its walking gait is clearer to visualize.
render: Whether to render the simulation.
num_steps_to_log: The max number of control steps in one episode that will
be logged. If the number of steps is more than num_steps_to_log, the
environment will still be running, but only first num_steps_to_log will
be recorded in logging.
action_repeat: The number of simulation steps before actions are applied.
control_time_step: The time step between two successive control signals.
env_randomizer: An instance (or a list) of EnvRandomizer(s). An
EnvRandomizer may randomize the physical property of minitaur, change
the terrrain during reset(), or add perturbation forces during step().
forward_reward_cap: The maximum value that forward reward is capped at.
Disabled (Inf) by default.
log_path: The path to write out logs. For the details of logging, refer to
minitaur_logging.proto.
Raises:
ValueError: If the urdf_version is not supported.
"""
# Set up logging.
self._log_path = log_path
self.logging = minitaur_logging.MinitaurLogging(log_path)
# PD control needs smaller time step for stability.
if control_time_step is not None:
self.control_time_step = control_time_step
self._action_repeat = action_repeat
self._time_step = control_time_step / action_repeat
else:
# Default values for time step and action repeat
if accurate_motor_model_enabled or pd_control_enabled:
self._time_step = 0.002
self._action_repeat = 5
else:
self._time_step = 0.01
self._action_repeat = 1
self.control_time_step = self._time_step * self._action_repeat
# TODO(b/73829334): Fix the value of self._num_bullet_solver_iterations.
self._num_bullet_solver_iterations = int(
NUM_SIMULATION_ITERATION_STEPS / self._action_repeat)
self._urdf_root = urdf_root
self._self_collision_enabled = self_collision_enabled
self._motor_velocity_limit = motor_velocity_limit
self._observation = []
self._true_observation = []
self._objectives = []
self._objective_weights = [
distance_weight, energy_weight, drift_weight, shake_weight
]
self._env_step_counter = 0
self._num_steps_to_log = num_steps_to_log
self._is_render = render
self._last_base_position = [0, 0, 0]
self._distance_weight = distance_weight
self._energy_weight = energy_weight
self._drift_weight = drift_weight
self._shake_weight = shake_weight
self._distance_limit = distance_limit
self._observation_noise_stdev = observation_noise_stdev
self._action_bound = 1
self._pd_control_enabled = pd_control_enabled
self._leg_model_enabled = leg_model_enabled
self._accurate_motor_model_enabled = accurate_motor_model_enabled
self._remove_default_joint_damping = remove_default_joint_damping
self._motor_kp = motor_kp
self._motor_kd = motor_kd
self._torque_control_enabled = torque_control_enabled
self._motor_overheat_protection = motor_overheat_protection
self._on_rack = on_rack
self._cam_dist = 1.0
self._cam_yaw = 0
self._cam_pitch = -30
self._forward_reward_cap = forward_reward_cap
self._hard_reset = True
self._last_frame_time = 0.0
self._control_latency = control_latency
self._pd_latency = pd_latency
self._urdf_version = urdf_version
self._ground_id = None
self._reflection = reflection
self._env_randomizers = convert_to_list(
env_randomizer) if env_randomizer else []
self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
self._pybullet_client = bullet_client.BulletClient()
if self._urdf_version is None:
self._urdf_version = DEFAULT_URDF_VERSION
self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
self._seed()
self.reset()
observation_high = (self._get_observation_upper_bound() +
OBSERVATION_EPS)
observation_low = (self._get_observation_lower_bound() -
OBSERVATION_EPS)
action_dim = NUM_MOTORS
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(observation_low, observation_high)
self.viewer = None
self._hard_reset = hard_reset # This assignment need to be after reset()
def __init__(self,
urdf_root=pybullet_data.getDataPath(),
action_repeat=5,
distance_weight=1.0,
energy_weight=0.02,
shake_weight=0.0,
drift_weight=0.0,
distance_limit=float("inf"),
observation_noise_stdev=0.0,
self_collision_enabled=False,
motor_velocity_limit=np.inf,
pd_control_enabled=False,#not needed to be true if accurate motor model is enabled (has its own better PD)
leg_model_enabled=True,
accurate_motor_model_enabled=True,
motor_kp=1.0,
motor_kd=0.02,
torque_control_enabled=False,
motor_overheat_protection=True,
hard_reset=False,
on_rack=False,
render=False,
kd_for_pd_controllers=0.3,
gait='spine',
velocity_idx=2,
max_spine_angle = 10.0,
env_randomizer=None):
"""Initialize the stoch gym environment.
Args:
urdf_root: The path to the urdf data folder.
action_repeat: The number of simulation steps before actions are applied.
distance_weight: The weight of the distance term in the reward.
energy_weight: The weight of the energy term in the reward.
shake_weight: The weight of the vertical shakiness term in the reward.
drift_weight: The weight of the sideways drift term in the reward.
distance_limit: The maximum distance to terminate the episode.
observation_noise_stdev: The standard deviation of observation noise.
self_collision_enabled: Whether to enable self collision in the sim.
motor_velocity_limit: The velocity limit of each motor.
pd_control_enabled: Whether to use PD controller for each motor.
leg_model_enabled: Whether to use a leg motor to reparameterize the action
space.
accurate_motor_model_enabled: Whether to use the accurate DC motor model.
motor_kp: proportional gain for the accurate motor model.
motor_kd: derivative gain for the accurate motor model.
torque_control_enabled: Whether to use the torque control, if set to
False, pose control will be used.
motor_overheat_protection: Whether to shutdown the motor that has exerted
large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
(OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in stoch.py for more
details.
hard_reset: Whether to wipe the simulation and load everything when reset
is called. If set to false, reset just place the stoch back to start
position and set its pose to initial configuration.
on_rack: Whether to place the stoch on rack. This is only used to debug
the walking gait. In this mode, the stoch's base is hanged midair so
that its walking gait is clearer to visualize.
render: Whether to render the simulation.
kd_for_pd_controllers: kd value for the pd controllers of the motors
env_randomizer: An EnvRandomizer to randomize the physical properties
during reset().
"""
self._time_step = 0.01
self._action_repeat = action_repeat
self._num_bullet_solver_iterations = 300
self._urdf_root = urdf_root
self._self_collision_enabled = self_collision_enabled
self._motor_velocity_limit = motor_velocity_limit
self._observation = []
self._env_step_counter = 0
self._is_render = render
self._last_base_position = [0, 0, 0]
self._distance_weight = distance_weight
self._energy_weight = energy_weight
self._drift_weight = drift_weight
self._shake_weight = shake_weight
self._distance_limit = distance_limit
self._observation_noise_stdev = observation_noise_stdev
self._action_bound = 1
self._pd_control_enabled = pd_control_enabled
self._leg_model_enabled = leg_model_enabled
self._accurate_motor_model_enabled = accurate_motor_model_enabled
self._motor_kp = motor_kp
self._motor_kd = motor_kd
self._torque_control_enabled = torque_control_enabled
self._motor_overheat_protection = motor_overheat_protection
self._on_rack = on_rack
self._cam_dist = 0.6
self._cam_yaw = 0.0
self._cam_pitch = -12.0
self._hard_reset = True
self._kd_for_pd_controllers = kd_for_pd_controllers
self._last_frame_time = 0.0
print("urdf_root=" + self._urdf_root)
self._env_randomizer = None
# PD control needs smaller time step for stability.
if pd_control_enabled or accurate_motor_model_enabled:
self._time_step /= NUM_SUBSTEPS
self._num_bullet_solver_iterations /= NUM_SUBSTEPS
self._action_repeat *= NUM_SUBSTEPS
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
self._pybullet_client = bullet_client.BulletClient()
# Data logging during inference
self.cg_list = list()
self.log_buff = False
self.c = 0
self._seed()
self.reset()
#State space
observation_high = (
self.stoch.GetObservationUpperBound() + OBSERVATION_EPS)
observation_low = (
self.stoch.GetObservationLowerBound() - OBSERVATION_EPS)
# Gait selection and correspoding attributes
self.trot, self.bound, self.spine = False, False, False
if gait == 'trot':
self.trot = True
elif gait == 'bound':
self.bound = True
elif gait == 'spine':
self.spine= True
velocities = [0.5, 1, 1.5, 2, 2.5] # velocities in (m/s)
sd = [0.1, 0.2, 0.4, 0.55, 0.7] # correspoding gaussian standard deviation
self.velocity = velocities[velocity_idx]
self.vel_sd = sd[velocity_idx]
self.max_spine_angle = max_spine_angle #maximum bend in spine
# Action space
action_dim = 5
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_low, observation_high, dtype=np.float32)
self.viewer = None
self._hard_reset = hard_reset # This assignment need to be after reset()
def setup(self, env_config, training_config, gui):
self.action_interval = env_config["action_interval"]
self.episode_length = env_config["episode_length"]
self.simulation_steps_per_action_step = int(
self.action_interval / BaseEnv.SIMULATION_TIMESTEP)
self.episode_counts = 0
self.action_type = 'delta'\
if 'action_type' not in env_config \
else env_config['action_type']
self.observations = None
self.gui = gui
self.ray_id = None
# Get variables from config
self.max_ur5s_count = env_config['max_ur5s_count']
self.max_task_ur5s_count = env_config['max_ur5s_count']
self.min_task_ur5s_count = env_config['min_ur5s_count']
self.survival_penalty = env_config['reward']['survival_penalty']
self.workspace_radius = env_config['workspace_radius']
self.individually_reach_target = \
env_config['reward']['individually_reach_target']
self.collectively_reach_target = \
env_config['reward']['collectively_reach_target']
self.cooperative_individual_reach_target = \
env_config['reward']['cooperative_individual_reach_target']
self.collision_penalty = env_config['reward']['collision_penalty']
proximity_config = env_config['reward']['proximity_penalty']
self.proximity_penalty_distance = proximity_config['max_distance']
self.proximity_penalty = proximity_config['penalty']
self.delta_reward = env_config['reward']['delta']
self.terminate_on_collectively_reach_target = env_config[
'terminate_on_collectively_reach_target']
self.terminate_on_collision = env_config['terminate_on_collision']
self.position_tolerance = env_config['position_tolerance']
self.orientation_tolerance = env_config['orientation_tolerance']
self.stop_ur5_after_reach = False\
if 'stop_ur5_after_reach' not in env_config \
else env_config['stop_ur5_after_reach']
self.finish_task_in_episode = False
self.centralized_policy = False \
if 'centralized_policy' not in training_config\
else training_config['centralized_policy']
self.centralized_critic = False \
if 'centralized_critic' not in training_config\
else training_config['centralized_critic']
self.curriculum = env_config['curriculum']
self.retry_on_fail = env_config['retry_on_fail']
self.failed_in_task = False
self.task_manager = None
self.episode_policy_instance_keys = None
self.memory_cluster_map = {}
self.collision_distance = \
env_config['collision_distance']
self.curriculum_level = -1
self.min_task_difficulty = 0.
self.max_task_difficulty = 100.0
if self.logger is not None:
self.set_level(ray.get(self.logger.get_curriculum_level.remote()))
self.expert_trajectory_threshold = 0.4\
if 'expert_trajectory_threshold' not in env_config\
else env_config['expert_trajectory_threshold']
if self.gui:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf", [0, 0, -self.collision_distance - 0.01])
p.setRealTimeSimulation(0)
p.setGravity(0, 0, -9.81)
self.all_ur5s = [] # reference to all UR5's in scene
self.ur5_episode_memories = None
# Keep track of episode progress
self.current_step = 0
self.terminate_episode = False
# Visualization
if self.gui:
self.real_time_debug = p.addUserDebugParameter(
'real-time', 0.0, 1.0, 1.0)
self.viewer = None
self.on_setup(env_config, training_config)
self.setup_action_observation(training_config['observations'])
self.setup_ur5s(env_config)
def __init__(self):
#self.physicsClient = p.connect(p.DIRECT)
self.physicsClient = bc.BulletClient()
#self.physicsClient = bc.BulletClient(connection_mode=p.DIRECT)
self.physicsClient.setAdditionalSearchPath(pybullet_data.getDataPath())
self.physicsClient.setGravity(0,0,0)
self.startPos = [0,0,1]
self.startOrientation = self.physicsClient.getQuaternionFromEuler([0,0,0])
filepath = os.path.abspath(os.path.dirname(__file__))
path = os.path.join(filepath, "lm50.urdf")
self.scID = self.physicsClient.loadURDF(path, self.startPos, self.startOrientation)
high = np.array([np.pi, np.pi, np.pi, 1, 1, 1])
self.action_space = spaces.Discrete(7)
self.observation_space = spaces.Box(-high, high, dtype=np.float32)
self.nsteps = 0
#---thresholds for episode-----
self.max_epsiode_steps = 500
self.maxAngVelo = 10
#---------------------------
#initial command/goal
self.goalEuler = (np.random.randint(-np.pi, high=np.pi, size=3))
while np.array_equal(self.goalEuler, np.array([0, 0, 0])):
self.goalEuler = (np.random.randint(-np.pi, high=np.pi, size=3))
self.goalQuat = self.physicsClient.getQuaternionFromEuler(self.goalEuler)
self.goalVec = self._getVectorFromOrn(self.goalQuat)
self.initialAngle = self._getAngleFromOrn(self.startOrientation, self.goalQuat)
#avoid div by zero, just in case
if self.initialAngle < 0.0001:
self.initialAngle = 0.0001
#getting state. state vector will be (goalVec, currVec), so (1x6) of floats
# (Xgoal, Ygoal, Zgoal; Xcurr, Ycurr, Zcurr)
scPosition, scOrientation = self.physicsClient.getBasePositionAndOrientation(self.scID)
currVec = self._getVectorFromOrn(scOrientation)
scTransVelo, scAngVelo = self.physicsClient.getBaseVelocity(self.scID) #leave this out for now
self.state = (self.goalVec[0], self.goalVec[1], self.goalVec[2], currVec[0], currVec[1], currVec[2])
self.steps_beyond_done = None
def __init__(self,
controller,
urdf='/urdf/hexapod_simplified.urdf',
visualiser=False,
follow=True,
collision_fatal=True,
failed_legs=[],
camera_position=[0, 0, 0],
camera_distance=0.7,
camera_yaw=20,
camera_pitch=-30):
self.t = 0 #: float: Current time of the simulator
self.dt = 1.0 / 240.0 #: float: Timestep of simulator. Default is 1/240s for PyBullet.
self.n_step = 0
self.gravity = -9.81 #: float: Magnitude of gravity vector in the positive z direction
self.foot_friction = 0.7
self.controller = controller
self.visualiser_enabled = visualiser
self.follow = follow
self.collision_fatal = collision_fatal
self.failed_joints = []
# set up joints to be locked to simulate failure
self.locked_joints = []
for failed_leg in failed_legs:
self.locked_joints += [
failed_leg * 3 - 3, failed_leg * 3 - 2, failed_leg * 3 - 1
]
self.camera_position = [
0.7, 0, 0
] #: list of float: GUI camera focus position in cartesian coordinates (self.controller.body_height)
self.camera_distance = 0.8 #: float: GUI camera distance from camera position
self.camera_yaw = 0 #: float: GUI camera yaw in degrees (-20)
self.camera_pitch = -45 #: float: GUI camera pitch in degrees (-30)
self.camera_position = camera_position
self.camera_distance = camera_distance
self.camera_yaw = camera_yaw
self.camera_pitch = camera_pitch
# connect a client to a pybullet physics server
if self.visualiser_enabled:
self.client = bc.BulletClient(connection_mode=p.GUI)
self.client.resetDebugVisualizerCamera(
cameraDistance=self.camera_distance,
cameraYaw=self.camera_yaw,
cameraPitch=self.camera_pitch,
cameraTargetPosition=self.camera_position)
self.client.configureDebugVisualizer(
p.COV_ENABLE_GUI, False) # somestimes useful to turn on
self.client.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True)
self.client.configureDebugVisualizer(
p.COV_ENABLE_SHADOWS, False,
shadowMapResolution=8192) # for nice rendering
self.client.configureDebugVisualizer(
p.COV_ENABLE_WIREFRAME, False) # somestimes useful to turn on
self.client.configureDebugVisualizer(
p.COV_ENABLE_RGB_BUFFER_PREVIEW, False)
self.client.configureDebugVisualizer(
p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, False)
self.client.configureDebugVisualizer(
p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, False)
else:
self.client = bc.BulletClient(connection_mode=p.DIRECT)
self.client.setAdditionalSearchPath(pybullet_data.getDataPath())
self.client.setGravity(0, 0, self.gravity)
self.client.setRealTimeSimulation(
False) # simulation needs to be explicitly stepped
# Add ground plane and set lateral friction coefficient
self.groundId = self.client.loadURDF(
'plane.urdf') #: int: Body ID of ground
self.client.changeDynamics(self.groundId,
-1,
lateralFriction=self.foot_friction)
# Add hexapod URDF
position = [0, 0, self.controller.body_height]
orientation = self.client.getQuaternionFromEuler(
[0, 0, -controller.crab_angle])
filepath = os.path.abspath(os.path.dirname(__file__)) + urdf
self.hexId = self.client.loadURDF(
filepath,
position,
orientation,
flags=p.URDF_USE_INERTIA_FROM_FILE
| p.URDF_USE_SELF_COLLISION) #: int: Body ID of hexapod robot
# get joint and link info from model
self.joints = self.__get_joints(
self.hexId) #: list of int: List of joint indeces
self.links = self.__get_links(
self.joints) #: list of int: List of link indeces
# initialise joints and links
self.__init_joints(self.controller, self.joints, self.locked_joints)
self.__init_links(self.links)
def __init__(
self,
gui=False,
urdf=(os.path.dirname(os.path.abspath(__file__)) +
'/urdf/pexod.urdf'),
dt=1. / 240., # the default for pybullet (see doc)
control_dt=0.05,
video=''):
self.GRAVITY = -9.81
self.dt = dt
self.control_dt = control_dt
# we call the controller every control_period steps
self.control_period = int(control_dt / dt)
self.t = 0
self.i = 0
self.safety_turnover = True
self.video_output_file = video
# the final target velocity is computed using:
# kp*(erp*(desiredPosition-currentPosition)/dt)+currentVelocity+kd*(m_desiredVelocity - currentVelocity).
# here we set kp to be likely to reach the target position
# in the time between two calls of the controller
self.kp = 1. / 12. # * self.control_period
self.kd = 0.4
# the desired position for the joints
self.angles = np.zeros(18)
# setup the GUI (disable the useless windows)
if gui:
self.physics = bc.BulletClient(connection_mode=p.GUI)
self.physics.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
self.physics.configureDebugVisualizer(
p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
self.physics.configureDebugVisualizer(
p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
self.physics.configureDebugVisualizer(
p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
self.physics.resetDebugVisualizerCamera(
cameraDistance=1,
cameraYaw=20,
cameraPitch=-20,
cameraTargetPosition=[1, -0.5, 0.8])
else:
self.physics = bc.BulletClient(connection_mode=p.DIRECT)
self.physics.setAdditionalSearchPath(pybullet_data.getDataPath())
self.physics.resetSimulation()
self.physics.setGravity(0, 0, self.GRAVITY)
self.physics.setTimeStep(self.dt)
self.physics.setPhysicsEngineParameter(fixedTimeStep=self.dt)
self.planeId = self.physics.loadURDF("plane.urdf")
start_pos = [0, 0, 0.15]
start_orientation = self.physics.getQuaternionFromEuler([0., 0, 0])
self.botId = self.physics.loadURDF(urdf, start_pos, start_orientation)
self.joint_list = self._make_joint_list(self.botId)
# bullet links number corresponding to the legs
self.leg_link_ids = [17, 14, 2, 5, 8, 11]
self.descriptor = {17: [], 14: [], 2: [], 5: [], 8: [], 11: []}
# video makes things much slower
if (video != ''):
self._stream_to_ffmpeg(self.video_output_file)
# put the hexapod on the ground (gently)
self.physics.setRealTimeSimulation(0)
jointFrictionForce = 1
for joint in range(self.physics.getNumJoints(self.botId)):
self.physics.setJointMotorControl2(self.botId,
joint,
p.POSITION_CONTROL,
force=jointFrictionForce)
for t in range(0, 100):
self.physics.stepSimulation()
self.physics.setGravity(0, 0, self.GRAVITY)
def __init__(
self,
xml_path=None,
debug=False,
gain_pos=0.125,
gain_vel=0.25,
max_torque=1,
g=-9.81,
start_standing=True,
img_size=(84, 84),
enable_new_floor=False,
frequency=FREQ_SIM,
):
"""
:param str xml_path: Path to Mujoco robot XML definition file
:param bool debug: Set to True to enable visual inspection
:param float gain_pos: Gain position value for low-level controller
:param float gain_vel: Gain velocity value for low-level controller
:param float max_torque: Max torque for PID controller
:param float g: Gravity value
"""
if xml_path is None:
xml_path = os.path.join(ASSET_DIR, "pupper_pybullet_out.xml")
self.gain_pos = gain_pos
self.gain_vel = gain_vel
self.max_torque = max_torque
self.img_size = img_size
if debug:
startup_flag = pybullet.GUI
else:
startup_flag = pybullet.DIRECT
self.p = bc.BulletClient(connection_mode=startup_flag)
self.p.setTimeStep(1 / frequency)
self.p.setAdditionalSearchPath(pybullet_data.getDataPath())
self.p.setGravity(0, 0, g)
if debug:
self.p.resetDebugVisualizerCamera(cameraDistance=1.5,
cameraYaw=180,
cameraPitch=-45,
cameraTargetPosition=[0, 0.0, 0])
# self.p.loadURDF("plane.urdf")
self.floor, self.model = self.p.loadMJCF(xml_path)
if debug:
numjoints = self.p.getNumJoints(self.model)
for i in range(numjoints):
print(self.p.getJointInfo(self.model, i))
self.joint_indices = list(range(0, 24, 2))
self.cam_proj = self.p.computeProjectionMatrixFOV(
fov=90,
aspect=self.img_size[0] / self.img_size[1],
nearVal=0.001,
farVal=10)
self.collision_floor = None
if enable_new_floor:
self.collision_floor = self.create_box((0, 0, 0.001), (0, 0, 0),
(1, 1, 0.0005),
visible=False)
self.p.createConstraint(
parentBodyUniqueId=self.collision_floor,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=self.p.JOINT_FIXED,
jointAxis=(1, 1, 1),
parentFramePosition=(0, 0, 0.0),
childFramePosition=(0, 0, 0.0005),
)
if start_standing:
self.reset(True)
for _ in range(10):
self.step()
#video requires ffmpeg available in path
createVideo = False
# connect to engine servers
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)
# add search path for loadURDF
p.setAdditionalSearchPath(pd.getDataPath())
# define world
timeStep = 1. / 120. #240.
p.setTimeStep(timeStep)
p.setGravity(0, 0, -9.8)
p.resetDebugVisualizerCamera(cameraDistance=0.7,
cameraYaw=0,
cameraPitch=0,
cameraTargetPosition=[0.5, -0.5, 0.5])
ur3 = ur3SimAuto(p, [0, 0, 0])
logId = ur3.bullet_client.startStateLogging(
ur3.bullet_client.STATE_LOGGING_PROFILE_TIMINGS, "log.json")
ur3.bullet_client.submitProfileTiming("start")
for i in range(100000):
# allow the manipulator to render slowly
def __init__(
self,
urdf_root=pybullet_data.getDataPath(),
action_repeat=1,
distance_weight=1.0,
energy_weight=0.005,
shake_weight=0.001,
drift_weight=0.001,
jerky_decay=0.95,
jerky_weight=0.001,
distance_limit=float("inf"),
observation_noise_stdev=0.0,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False,
# not needed to be true if accurate motor model is enabled (has its own better PD)
leg_model_enabled=True,
accurate_motor_model_enabled=True,
motor_kp=1.0,
motor_kd=0.02,
torque_control_enabled=False,
motor_overheat_protection=True,
hard_reset=True,
on_rack=False,
render=False,
kd_for_pd_controllers=0.3,
env_randomizer=MinitaurEnvRandomizer()):
"""Initialize the minitaur gym environment.
Args:
urdf_root: The path to the urdf data folder.
action_repeat: The number of simulation steps before actions are applied.
distance_weight: The weight of the distance term in the reward.
energy_weight: The weight of the energy term in the reward.
shake_weight: The weight of the vertical shakiness term in the reward.
drift_weight: The weight of the sideways drift term in the reward.
distance_limit: The maximum distance to terminate the episode.
observation_noise_stdev: The standard deviation of observation noise.
self_collision_enabled: Whether to enable self collision in the sim.
motor_velocity_limit: The velocity limit of each motor.
pd_control_enabled: Whether to use PD controller for each motor.
leg_model_enabled: Whether to use a leg motor to reparameterize the action
space.
accurate_motor_model_enabled: Whether to use the accurate DC motor model.
motor_kp: proportional gain for the accurate motor model.
motor_kd: derivative gain for the accurate motor model.
torque_control_enabled: Whether to use the torque control, if set to
False, pose control will be used.
motor_overheat_protection: Whether to shutdown the motor that has exerted
large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
(OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more
details.
hard_reset: Whether to wipe the simulation and load everything when reset
is called. If set to false, reset just place the minitaur back to start
position and set its pose to initial configuration.
on_rack: Whether to place the minitaur on rack. This is only used to debug
the walking gait. In this mode, the minitaur's base is hanged midair so
that its walking gait is clearer to visualize.
render: Whether to render the simulation.
kd_for_pd_controllers: kd value for the pd controllers of the motors
env_randomizer: An EnvRandomizer to randomize the physical properties
during reset().
"""
self._time_step = 0.01
self._action_repeat = action_repeat
self._num_bullet_solver_iterations = 300
self._urdf_root = urdf_root
self._self_collision_enabled = self_collision_enabled
self._motor_velocity_limit = motor_velocity_limit
self._observation = []
self._env_step_counter = 0
self._is_render = render
self._last_base_position = [0, 0, 0]
self._distance_weight = distance_weight
self._energy_weight = energy_weight
self._drift_weight = drift_weight
self._shake_weight = shake_weight
self._distance_limit = distance_limit
self._observation_noise_stdev = observation_noise_stdev
self._action_bound = 1
self._pd_control_enabled = pd_control_enabled
self._leg_model_enabled = leg_model_enabled
self._accurate_motor_model_enabled = accurate_motor_model_enabled
self._motor_kp = motor_kp
self._motor_kd = motor_kd
self._torque_control_enabled = torque_control_enabled
self._motor_overheat_protection = motor_overheat_protection
self._on_rack = on_rack
self._cam_dist = 1.0
self._cam_yaw = 0
self._cam_pitch = -30
self._hard_reset = True
self._kd_for_pd_controllers = kd_for_pd_controllers
self._last_frame_time = 0.0
print("urdf_root=" + self._urdf_root)
self._env_randomizer = env_randomizer
# PD control needs smaller time step for stability.
if pd_control_enabled or accurate_motor_model_enabled:
self._time_step /= NUM_SUBSTEPS
self._num_bullet_solver_iterations /= NUM_SUBSTEPS
self._action_repeat *= NUM_SUBSTEPS
if self._is_render:
self._pybullet_client = BulletClient(connection_mode=pybullet.GUI)
else:
self._pybullet_client = BulletClient()
self._seed()
self.reset()
observation_high = (self.minitaur.GetObservationUpperBound() +
OBSERVATION_EPS)
observation_low = (self.minitaur.GetObservationLowerBound() -
OBSERVATION_EPS)
action_dim = 8
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_low,
observation_high,
dtype=np.float32)
self.viewer = None
self._hard_reset = hard_reset # This assignment need to be after reset()
# ==kir
self._jerky_decay = jerky_decay
self._jerky_weight = jerky_weight
self._jerky_penalty = 0
self._action_dimention = self.minitaur.GetActionDimension()
self._prev_action = np.zeros(self._action_dimention)
def _run_example(max_time=_MAX_TIME_SECONDS):
"""Runs the locomotion controller example."""
#recording video requires ffmpeg in the path
record_video = False
if record_video:
p = pybullet
p.connect(
p.GUI,
options="--width=1280 --height=720 --mp4=\"test.mp4\" --mp4fps=100"
)
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
else:
p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.setAdditionalSearchPath(pd.getDataPath())
num_bullet_solver_iterations = 30
p.setPhysicsEngineParameter(
numSolverIterations=num_bullet_solver_iterations)
p.setPhysicsEngineParameter(enableConeFriction=0)
p.setPhysicsEngineParameter(numSolverIterations=30)
simulation_time_step = 0.001
p.setTimeStep(simulation_time_step)
p.setGravity(0, 0, -9.8)
p.setPhysicsEngineParameter(enableConeFriction=0)
p.setAdditionalSearchPath(pd.getDataPath())
#random.seed(10)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
heightPerturbationRange = 0.06
plane = True
if plane:
p.loadURDF("plane.urdf")
#planeShape = p.createCollisionShape(shapeType = p.GEOM_PLANE)
#ground_id = p.createMultiBody(0, planeShape)
else:
numHeightfieldRows = 256
numHeightfieldColumns = 256
heightfieldData = [0] * numHeightfieldRows * numHeightfieldColumns
for j in range(int(numHeightfieldColumns / 2)):
for i in range(int(numHeightfieldRows / 2)):
height = random.uniform(0, heightPerturbationRange)
heightfieldData[2 * i + 2 * j * numHeightfieldRows] = height
heightfieldData[2 * i + 1 +
2 * j * numHeightfieldRows] = height
heightfieldData[2 * i +
(2 * j + 1) * numHeightfieldRows] = height
heightfieldData[2 * i + 1 +
(2 * j + 1) * numHeightfieldRows] = height
terrainShape = p.createCollisionShape(
shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[.05, .05, 1],
heightfieldTextureScaling=(numHeightfieldRows - 1) / 2,
heightfieldData=heightfieldData,
numHeightfieldRows=numHeightfieldRows,
numHeightfieldColumns=numHeightfieldColumns)
ground_id = p.createMultiBody(0, terrainShape)
#p.resetBasePositionAndOrientation(ground_id,[0,0,0], [0,0,0,1])
#p.changeDynamics(ground_id, -1, lateralFriction=1.0)
robot_uid = p.loadURDF(robot_sim.URDF_NAME, robot_sim.START_POS)
robot = robot_sim.SimpleRobot(p,
robot_uid,
simulation_time_step=simulation_time_step)
controller = _setup_controller(robot)
controller.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
#while p.isConnected():
# pos,orn = p.getBasePositionAndOrientation(robot_uid)
# print("pos=",pos)
# p.stepSimulation()
# time.sleep(1./240)
current_time = robot.GetTimeSinceReset()
#logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "mpc.json")
while current_time < max_time:
#pos,orn = p.getBasePositionAndOrientation(robot_uid)
#print("pos=",pos, " orn=",orn)
p.submitProfileTiming("loop")
# Updates the controller behavior parameters.
lin_speed, ang_speed = _generate_example_linear_angular_speed(
current_time)
#lin_speed, ang_speed = (0., 0., 0.), 0.
_update_controller_params(controller, lin_speed, ang_speed)
# Needed before every call to get_action().
controller.update()
hybrid_action, info = controller.get_action()
robot.Step(hybrid_action)
if record_video:
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
#time.sleep(0.003)
current_time = robot.GetTimeSinceReset()
p.submitProfileTiming()
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=False,
isDiscrete=False):
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
#self._cam_dist = 1.3
#self._cam_yaw = 180
#self._cam_pitch = -40
self._cam_dist = 0.3
#self._cam_yaw = 45
self._cam_roll = 0
self._cam_yaw = 90
self._cam_pitch = -40
#self._width = 341
#self._height = 256
self._kinect_rgb_width = 1920
self._kinect_rgb_height = 1080
self._kinect_d_width = 512
self._kinect_d_height = 424
self._handcamera_width = 640
self._handcamera_height = 480
self._isDiscrete = isDiscrete
#self._isBox = isBox
self.terminated = 0
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
self.seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] *
observationDim)
if (self._isDiscrete):
self.action_space = spaces.Discrete(7)
else:
action_dim = 12
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._proximity_low = np.array([0] * 3)
self._proximity_high = np.array([1] * 3)
self._force_low = np.array([0] * 3)
self._force_high = np.array([10] * 3)
self.observation_space = spaces.Tuple(
(spaces.Box(low=0,
high=255,
shape=(self._kinect_rgb_height, self._kinect_rgb_width,
4),
dtype=np.uint8),
spaces.Box(self._proximity_low,
self._proximity_high,
dtype=np.float32),
spaces.Box(self._force_low, self._force_high, dtype=np.float32)))
self.viewer = None
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()
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_SELF_COLLISION
| pb.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
# load ramp
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)
'''
# Simulation Physics General Settings
self.timeStep = 1. / 240
p.setTimeStep(self.timeStep)
p.setGravity(*self._GRAVITY_VECTOR)
p.setRealTimeSimulation(0) # To manually step simulation later
p = self._p
# Bring robot back to origin
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(self.np_random)
# standing_poses = self._standing_poses()
# MX-28s:
for i in range(Joints.LEFT_LEG_1, Joints.HEAD_1):
p.resetJointState(self.soccerbotUid, i, standing_poses[i])
p.changeDynamics(self.soccerbotUid,
i,
jointLowerLimit=-self._joint_limit_low[i],
jointUpperLimit=self._joint_limit_high[i],
jointLimitForce=self._MX_28_force)
# AX-12s:
for i in range(Joints.LEFT_ARM_1, Joints.LEFT_LEG_1):
p.resetJointState(self.soccerbotUid, i, standing_poses[i])
p.changeDynamics(self.soccerbotUid,
i,
jointLowerLimit=-self._joint_limit_low[i],
jointUpperLimit=self._joint_limit_high[i],
jointLimitForce=self._AX_12_force)
p.resetJointState(self.soccerbotUid, Joints.HEAD_1,
standing_poses[Joints.HEAD_1])
p.changeDynamics(self.soccerbotUid,
Joints.HEAD_1,
jointLowerLimit=-np.pi,
jointUpperLimit=np.pi,
jointLimitForce=self._AX_12_force)
p.resetJointState(self.soccerbotUid, Joints.HEAD_2,
standing_poses[Joints.HEAD_2])
p.changeDynamics(self.soccerbotUid,
Joints.HEAD_2,
jointLowerLimit=-np.pi,
jointUpperLimit=np.pi,
jointLimitForce=self._AX_12_force)
for i in range(20):
p.stepSimulation()
p.stepSimulation()
if np.sum(self._feet()[0:4]) > 0.5 or np.sum(
self._feet()[4:8]) > 0.5:
break
# WARM UP SIMULATION
'''
if self.WARM_UP:
warm_up = self.np_random.randint(0, 10)
for _ in range(warm_up):
p.stepSimulation()
p.stepSimulation()
'''
# Get Observation
# self.st = RollingAvg(256, 0.01, 0.01)
self.prev_lin_vel = np.array([0, 0, 0])
self.imu_bias = np.concatenate(
(self.np_random.normal(0, self._IMU_LIN_STDDEV_BIAS,
int(self._IMU_DIM / 2)),
self.np_random.normal(0, self._IMU_ANG_STDDEV_BIAS,
int(self._IMU_DIM / 2))))
# Construct Observation
imu = self._imu()
feet = self._feet()
joints_pos = self._joints_pos()
joints_vel = self._joints_vel()
height = np.array([self._global_pos()[2]], dtype=self.DTYPE)
orn = self._global_orn()
observation = np.concatenate(
(joints_pos, joints_vel, imu, orn, height, feet))
# To keep up with the pipeline - 120Hz
p.stepSimulation()
p.stepSimulation()
if self._renders:
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
return observation
import pybullet_utils.bullet_client as bc
import pybullet_utils.urdfEditor as ed
import pybullet
import pybullet_data
import time
p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p0.setAdditionalSearchPath(pybullet_data.getDataPath())
p1 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p1.setAdditionalSearchPath(pybullet_data.getDataPath())
#can also connect using different modes, GUI, SHARED_MEMORY, TCP, UDP, SHARED_MEMORY_SERVER, GUI_SERVER
husky = p1.loadURDF("husky/husky.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
kuka = p0.loadURDF("kuka_iiwa/model.urdf")
ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(husky, p1._client)
ed1 = ed.UrdfEditor()
ed1.initializeFromBulletBody(kuka, p0._client)
#ed1.saveUrdf("combined.urdf")
parentLinkIndex = 0
jointPivotXYZInParent = [0,0,0]
jointPivotRPYInParent = [0,0,0]
def init(self):
if (self._game_settings['render']):
# self._object.setPosition([self._x[self._step], self._y[self._step], 0.0] )
self._physicsClient = p.connect(p.GUI)
else:
self._physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0, 0, self._GRAVITY)
p.setTimeStep(self._dt)
# p.connect(p.GUI)
RLSIMENV_PATH = os.environ['RLSIMENV_PATH']
p.loadURDF("plane.urdf")
self._agent = p.loadURDF(RLSIMENV_PATH +
"/rlsimenv/data/kinova/jaco_on_table.urdf",
[0, 0, 0.8],
useFixedBase=False)
# gravId = p.addUserDebugParameter("gravity",-10,10,-10)
self._jointIds = []
paramIds = []
self._init_root_vel = p.getBaseVelocity(self._agent)
self._init_root_pos = p.getBasePositionAndOrientation(self._agent)
self._init_pose = []
p.setPhysicsEngineParameter(numSolverIterations=100)
p.changeDynamics(self._agent, -1, linearDamping=0, angularDamping=0)
self._jointAngles = [
0, 0, 1.0204, -1.97, -0.084, 2.06, -1.9, 0, 0, 1.0204, -1.97,
-0.084, 2.06, -1.9, 0
]
activeJoint = 0
self._action_bounds = [[], []]
for j in range(p.getNumJoints(self._agent)):
p.changeDynamics(self._agent, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(self._agent, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC
or jointType == p.JOINT_REVOLUTE):
self._jointIds.append(j)
### Update action bounds
self._action_bounds[0].append(info[8])
self._action_bounds[1].append(info[9])
# print ("self._action_bounds: ", self._action_bounds)
# paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,jointAngles[activeJoint]))
# self._paramIds.append()
p.resetJointState(self._agent, j,
self._jointAngles[activeJoint])
activeJoint += 1
p.setRealTimeSimulation(0)
lo = [0.0 for l in self.getObservation()[0]]
hi = [1.0 for l in self.getObservation()[0]]
state_bounds = [lo, hi]
state_bounds = [state_bounds]
self._game_settings['state_bounds'] = [lo, hi]
self._state_length = len(self._game_settings['state_bounds'][0])
# print ("self._state_length: ", self._state_length)
self._observation_space = ActionSpace(
self._game_settings['state_bounds'])
self._game_settings['action_bounds'] = self._action_bounds
self._action_space = ActionSpace(self._action_bounds)
self._last_state = self.getState()
self._last_pose = p.getBasePositionAndOrientation(self._agent)[0]
def get_sphere(_p, x, y, z):
body = _p.loadURDF(os.path.join(pybullet_data.getDataPath(), "sphere2red_nocol.urdf"), [x, y, z])
part_name, _ = _p.getBodyInfo(body)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(_p, part_name, bodies, 0, -1)
'''
test file for checking functions
and values in realtion to
pybullet
'''
import pybullet as p
import os
import time
import numpy as np
import pybullet_data
file_path = "./rsc/anymal/anymal.urdf"
p.connect(p.GUI)
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), 0, 0,
0) #5,10,15,20,
p.setGravity(0, 0, -10)
geo_anymal = p.loadURDF(file_path)
quat = p.getQuaternionFromEuler([0, 0, 0])
#p.resetBasePositionAndOrientation(anymal, [0, 0, 0.6], quat)
def save_towr_angles(base_pos, base_quat, ee1, ee2, ee3, ee4):
base_pos = list(base_pos)
base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]]
angles_12 = []
for i in range(20):
p.resetBasePositionAndOrientation(geo_anymal, base_pos, base_quat)
leg_LF = p.calculateInverseKinematics(geo_anymal, 5, ee1)
leg_RF = p.calculateInverseKinematics(geo_anymal, 10, ee2)
def __init__(self,pybullet_sim_factory = boxstack_pybullet_sim,
render=True,
render_sleep=False,
debug_visualization=True,
hard_reset = False,
render_width=240,
render_height=240,
action_repeat=1,
time_step = 1./240.,
num_bullet_solver_iterations=50,
urdf_root=pybullet_data.getDataPath()):
"""Initialize the gym environment.
Args:
urdf_root: The path to the urdf data folder.
"""
self._pybullet_sim_factory = pybullet_sim_factory
self._time_step = time_step
self._urdf_root = urdf_root
self._observation = []
self._action_repeat=action_repeat
self._num_bullet_solver_iterations = num_bullet_solver_iterations
self._env_step_counter = 0
self._is_render = render
self._debug_visualization = debug_visualization
self._render_sleep = render_sleep
self._render_width=render_width
self._render_height=render_height
self._cam_dist = .3
self._cam_yaw = 50
self._cam_pitch = -35
self._hard_reset = True
self._last_frame_time = 0.0
optionstring='--width={} --height={}'.format(render_width,render_height)
print("urdf_root=" + self._urdf_root)
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI, options=optionstring)
else:
self._pybullet_client = bullet_client.BulletClient()
if (debug_visualization==False):
self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_GUI,enable=0)
self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_RGB_BUFFER_PREVIEW,enable=0)
self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_DEPTH_BUFFER_PREVIEW,enable=0)
self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,enable=0)
self._pybullet_client.setAdditionalSearchPath(urdf_root)
self._seed()
self.reset()
observation_high = (
self._example_sim.GetObservationUpperBound())
observation_low = (
self._example_sim.GetObservationLowerBound())
action_dim = self._example_sim.GetActionDimension()
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(observation_low, observation_high)
self.viewer = None
self._hard_reset = hard_reset # This assignment need to be after reset()
def relative_ee_pose_to_ee_world_pose1(robotId, eeTargetPos, eeTargetOrn):
ee_link_state = p.getLinkState(robotId,
linkIndex=7,
computeForwardKinematics=1)
ee_pos_W = ee_link_state[-2]
ee_ort_W = ee_link_state[-1]
return p.multiplyTransforms(ee_pos_W, ee_ort_W, eeTargetPos, eeTargetOrn)
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
time_step = 0.001
gravity_constant = -9.81
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf", [0, 0, -0.3])
cubeStartPos = [0, 0, 0]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
gripper_left = p.addUserDebugParameter('Gripper_left', -0.5, 0.5, 0)
gripper_right = p.addUserDebugParameter('Gripper_right', -0.5, 0.5, 0)
