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)