def __init__(self,
render=False,
torque_limits=[100] * 6,
init_noise=.005,
):
self.render = render
self.torque_limits = np.array(torque_limits)
self.init_noise = init_noise
low = -np.ones(6,dtype=np.float32)
self.action_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32)
low = -np.ones(17, dtype=np.float32)*np.inf
self.observation_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32)
if render:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self.plane_id = p.loadSDF(pybullet_data.getDataPath() + "/plane_stadium.sdf")[0]
self.walker_id = p.loadMJCF(pybullet_data.getDataPath() + "/mjcf/walker2d.xml")[0]
p.setGravity(0, 0, -9.8)
self.dt = p.getPhysicsEngineParameters()['fixedTimeStep']
self.reset()
def print_physics_engine_params(self):
"""Prints the parametes of the physics engine.
"""
params = pybullet.getPhysicsEngineParameters(self.physicsClient)
print("physics_engine_params:")
for key in params:
print(" - ", key, ": ", params[key])
def __init__(self, floor_height=0.3, bullet_direct=False):
self.controlled_joints = 6
self.tau_min = -2.0
self.tau_max = 2.0
if bullet_direct:
physicsClient = p.connect(p.DIRECT)
else:
physicsClient = p.connect(p.GUI)
urdf_base_string = str(os.path.dirname(os.path.abspath(__file__)))
urdf_robot_string = (os.path.join(
rospkg.RosPack().get_path("robot_properties_solo"), "urdf",
"solo.urdf"))
planeId = p.loadURDF(
os.path.join(urdf_base_string, "urdf",
"plane_with_restitution.urdf"))
cubeStartPos = [0, 0, floor_height]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
self.robotId = p.loadURDF(urdf_robot_string,
cubeStartPos,
cubeStartOrientation,
flags=p.URDF_USE_INERTIA_FROM_FILE)
cubePos, cubeOrn = p.getBasePositionAndOrientation(self.robotId)
useRealTimeSimulation = False
# Query all the joints.
num_joints = p.getNumJoints(self.robotId)
print("Number of joints={}".format(num_joints))
for ji in range(num_joints):
p.changeDynamics(self.robotId,
ji,
linearDamping=.04,
angularDamping=0.04,
restitution=0.0,
lateralFriction=0.5)
p.setGravity(0, 0, -9.81)
p.setPhysicsEngineParameter(1e-3, numSubSteps=1)
print(p.getPhysicsEngineParameters())
# Create the pinocchio robot.
self.robot = QuadrupedWrapper()
self.controlled_joints = [
'BL_HFE', 'BL_KFE', 'BR_HFE', 'BR_KFE', 'FL_HFE', 'FL_KFE',
'FR_HFE', 'FR_KFE'
]
# Create the simulator for easier mapping between
self.sim = Simulator(self.robotId, self.robot, self.controlled_joints,
[
'BL_END',
'BR_END',
'FL_END',
'FR_END',
])
def __init__(self, robot, config, break_condition=None):
self._robot = robot # robot sim should not be in real-time. Step simulation will be called by controller.
self._P_pos = np.diag(config['P_pos'])
self._D_pos = np.diag(config['D_pos'])
self._P_ori = np.diag(config['P_ori'])
self._D_ori = np.diag(config['D_ori'])
self._error_thresh = config['error_thresh']
self._start_err = config['start_err']
self._robot.set_ctrl_mode('tor')
self._run_ctrl = False
if break_condition is not None and callable(break_condition):
self._break_condition = break_condition
else:
self._break_condition = lambda: False
self._ctrl_thread = threading.Thread(target=self._control_thread)
self._mutex = threading.Lock()
self._sim_timestep = pb.getPhysicsEngineParameters()['fixedTimeStep']
self._sim_time = 0.0
if 'rate' not in config:
self._ctrl_rate = 1. / self._sim_timestep
else:
self._ctrl_rate = float(config['rate'])
def __init__(
self,
render=False,
torque_limits=[100] * 6,
init_noise=.005,
physics_params=None,
dynamics_params=None,
):
self.args = locals()
self.torque_limits = np.array(torque_limits)
self.init_noise = init_noise
self.cur_step = 0
low = -np.ones(6)
self.action_space = gym.spaces.Box(low=low,
high=-low,
dtype=np.float32)
low = -np.ones(19) * np.inf
self.observation_space = gym.spaces.Box(low=low,
high=-low,
dtype=np.float32)
if render:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self.plane_id = p.loadSDF(pybullet_data.getDataPath() +
"/plane_stadium.sdf")[0]
self.walker_id = p.loadMJCF(pybullet_data.getDataPath() +
"/mjcf/walker2d.xml")[0]
#flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)[0] # TODO not sure the self collision needs to be here..
p.setGravity(0, 0, -9.8)
if physics_params is None:
physics_params = {}
if dynamics_params is None:
dynamics_params = {}
p.changeDynamics(self.plane_id, -1, **dynamics_params)
for i in range(p.getNumJoints(self.walker_id)):
p.changeDynamics(self.walker_id, i, **dynamics_params)
p.changeDynamics(self.walker_id, -1, **dynamics_params)
p.setPhysicsEngineParameter(**physics_params)
self.dt = p.getPhysicsEngineParameters()['fixedTimeStep']
self.reset()
def run(self):
"""
Use a soft real-time clock (Soft RTC) to step through the simulation.If
the p.stepSimulation takes too long, slow down the clock by decreasing
the real time factor.
"""
time_step = p.getPhysicsEngineParameters()["fixedTimeStep"]
clock = SoftRealTimeClock(period=time_step / self.real_time_factor)
while threading.main_thread().is_alive():
p.stepSimulation()
clock.sleep()
def _initialize_(self):
# Set the gravity
self.setGravity(numpy.array([0, 0, 0]))
# Current relative time
self.time = 0
# Get the time step
self.setStepsize(pybullet.getPhysicsEngineParameters()["fixedTimeStep"])
# Set the visualization time
self.sleep_time = 1e-3
# Store the simulation data
self.measurements = pandas.DataFrame()
self._measurements = pandas.DataFrame()
def init_sim():
if not p.isConnected():
globals.physicsClient = p.connect(
p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
else:
p.resetSimulation(globals.physicsClient)
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
globals.sph1 = p.loadURDF("robots/capsule01.urdf", [0, 0, 3],
cubeStartOrientation)
globals.sph2 = p.loadURDF("robots/sphere01_man.urdf", [0, 0, 5],
cubeStartOrientation)
globals.t_step = p.getPhysicsEngineParameters()['fixedTimeStep']
globals.curr_step = 0
p.setJointMotorControl2(globals.sph1,
0,
controlMode=p.VELOCITY_CONTROL,
force=0)
planeId = p.loadURDF("plane.urdf")
#initial position of a robot
cubeStartPos = [0, 0, 1]
#initial orientation of a robot
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
#load a robot
robotId = p.loadURDF("urdf/test_walker1.urdf", cubeStartPos,
cubeStartOrientation)
mode = p.POSITION_CONTROL
angle = 0
add_angle = 3.14 / 50
jindex = 0
count = -30
simpara = p.getPhysicsEngineParameters(physicsClient)
print('simulation parameters')
print(simpara)
for i in range(10000):
#get Base position
basePos, baseQua = p.getBasePositionAndOrientation(robotId)
#VRPos = (basePos[0] + 0.2, basePos[1] - 0.5, basePos[0] + 0.2)
#VRQua = (baseQua[0], baseQua[1], baseQua[2])
#VRtarget = (basePos[0], basePos[1], basePos[2])
#print('VRPos type')
#print(type(VRPos))
#set Camera state
#p.setVRCameraState(VRPos, VRQua, VRtarget)
p.resetDebugVisualizerCamera(2.0, 10, -30, basePos)
def init_simulation(self,
floor_height=0.3,
visualize=True,
sim_timestep=0.001,
cont_timestep_mult=16,
lateral_friction=1.0,
joint_damping=0.0,
contact_stiffness=10000.0,
contact_damping=200.0,
contact_damping_multiplier=None):
self.visualize = visualize
if self.visualize:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT)
dir_path = os.path.dirname(os.path.realpath(__file__))
cubeStartPos = [0, 0, 0]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
planeId = p.loadURDF(dir_path + '/urdf/plane_with_restitution.urdf')
self.robotId = p.loadURDF(dir_path + '/urdf/teststand.urdf',
cubeStartPos,
cubeStartOrientation,
flags=p.URDF_USE_INERTIA_FROM_FILE)
cubePos, cubeOrn = p.getBasePositionAndOrientation(self.robotId)
if isinstance(lateral_friction, list):
self.random_lateral_friction = True
self.lateral_friction_range = lateral_friction
self.lateral_friction = np.random.uniform(
self.lateral_friction_range[0], self.lateral_friction_range[1])
else:
self.random_lateral_friction = False
self.lateral_friction = lateral_friction
if isinstance(contact_stiffness, list):
self.random_contact_stiffness = True
self.contact_stiffness_range = contact_stiffness
self.contact_stiffness = np.random.uniform(
self.contact_stiffness_range[0],
self.contact_stiffness_range[1])
else:
self.random_contact_stiffness = False
self.contact_stiffness = contact_stiffness
useRealTimeSimulation = False
# Query all the joints.
num_joints = p.getNumJoints(self.robotId)
print("Number of joints={}".format(num_joints))
for ji in range(num_joints):
p.changeDynamics(self.robotId,
ji,
linearDamping=.04,
angularDamping=0.04,
restitution=0.0,
lateralFriction=self.lateral_friction,
maxJointVelocity=1000)
p.changeDynamics(self.robotId, ji, jointDamping=joint_damping)
p.changeDynamics(planeId, -1, lateralFriction=self.lateral_friction)
self.contact_damping = contact_damping
self.contact_damping_multiplier = contact_damping_multiplier
if self.random_contact_stiffness:
self.contact_stiffness = np.random.uniform(
self.contact_stiffness_range[0],
self.contact_stiffness_range[1])
if self.contact_damping_multiplier is not None:
contact_damping = self.contact_damping_multiplier * 2.0 * np.sqrt(
self.contact_stiffness)
else:
if self.contact_damping is None:
contact_damping = 2.0 * np.sqrt(self.contact_stiffness)
else:
contact_damping = self.contact_damping
p.changeDynamics(planeId,
-1,
contactStiffness=self.contact_stiffness,
contactDamping=contact_damping)
p.setGravity(0.0, 0.0, -9.81)
#p.setPhysicsEngineParameter(1e-3, numSubSteps=1)
self.sim_timestep = sim_timestep
self.cont_timestep_mult = cont_timestep_mult
self.dt = self.cont_timestep_mult * self.sim_timestep
p.setPhysicsEngineParameter(fixedTimeStep=self.sim_timestep)
print(p.getPhysicsEngineParameters())
return p, self.robotId, planeId, [1, 2, 3], [2, 3]
goal_info = {
'achieved': achieved_goal,
'desired': desired_goal,
}
done = self.step_counter >= self.max_steps
return observation, goal_info, done
if __name__ == "__main__":
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
p.loadURDF("plane.urdf")
task = Pick_Place(p)
time_step = p.getPhysicsEngineParameters()["fixedTimeStep"]
while True:
obs = task.reset()
for _ in np.arange(1. / time_step):
p.stepSimulation()
time.sleep(time_step)
print(obj_names) # simulate: p.setGravity(0,0,-9.81) ''' #show this for 10 seconds now = time.time() while (time.time() < now+10): p.stepSimulation() ''' p.setRealTimeSimulation(0) p.setPhysicsEngineParameter(numSubSteps = 10) p.setPhysicsEngineParameter(numSolverIterations = 100) print(p.getPhysicsEngineParameters()) t0 = time.time() tau = 8 # [seconds] jointIndex = 4 torque = joint_dict[jointIndex]["torque"] from dataclasses import dataclass @dataclass class KeyCommand: key: 'str' #ord: 'int' joint: 'int' torque: 'float' def __post_init__(self): self.ord = ord(self.key)
def get_time_step():
# {'gravityAccelerationX', 'useRealTimeSimulation', 'gravityAccelerationZ', 'numSolverIterations',
# 'gravityAccelerationY', 'numSubSteps', 'fixedTimeStep'}
return p.getPhysicsEngineParameters(
physicsClientId=CLIENT)['fixedTimeStep']
def physics_engine_parameters(self):
return pb.getPhysicsEngineParameters(physicsClientId=self.bullet_cid)
def get_parameters(self):
"""Gets several parameters of the physics engine. Refer to the pybullet
Quickstart Guide for the list of exposed parameters."""
return p.getPhysicsEngineParameters(physicsClientId=self.eid)
