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)