def _robo_init(self, model_xml, robot_name, action_dim):
if self.servo:
RoboschoolForwardWalkerServo.__init__(self, power=2.5)
else:
RoboschoolForwardWalker.__init__(self, power=2.5)
RoboschoolMujocoXmlEnv.__init__(self,
model_xml,
robot_name,
action_dim=action_dim, obs_dim=70)
def __init__(self):
RoboschoolForwardWalker.__init__(self, power=0.30)
RoboschoolUrdfEnv.__init__(
self,
"atlas_description/urdf/atlas_v4_with_multisense.urdf",
"pelvis",
action_dim=30,
obs_dim=70,
fixed_base=False,
self_collision=True)
def _robo_init(self, model_urdf, robot_name, action_dim):
if self.servo:
RoboschoolForwardWalkerServo.__init__(self, power=0.30)
else:
RoboschoolForwardWalker.__init__(self, power=0.30)
RoboschoolUrdfEnv.__init__(self,
model_urdf,
robot_name,
action_dim=action_dim,
obs_dim=70,
fixed_base=False,
self_collision=True)
def reset(self):
if self.scene is None:
self.scene = self.create_single_player_scene()
if not self.scene.multiplayer:
self.scene.episode_restart()
self.mjcf = self.scene.cpp_world.load_mjcf(
os.path.join(os.path.dirname(__file__), "mujoco_assets",
self.model_xml))
self.ordered_joints = []
self.jdict = {}
self.parts = {}
self.frame = 0
self.done = 0
self.reward = 0
dump = 0
for r in self.mjcf:
if dump: print("ROBOT '%s'" % r.root_part.name)
if r.root_part.name == self.robot_name:
self.cpp_robot = r
self.robot_body = r.root_part
for part in r.parts:
if dump: print("\tPART '%s'" % part.name)
self.parts[part.name] = part
if part.name == self.robot_name:
self.cpp_robot = r
self.robot_body = part
for j in r.joints:
if dump:
print(
"\tALL JOINTS '%s' limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f"
% ((j.name, ) + j.limits()))
if j.name[:6] == "ignore":
j.set_motor_torque(0)
continue
j.power_coef = 100.0
self.ordered_joints.append(j)
self.jdict[j.name] = j
assert (self.cpp_robot)
self.robot_specific_reset()
self.body_xyz = [-0.3, 0, 0.25]
RoboschoolForwardWalker.move_robot(self, self.body_xyz[0],
self.body_xyz[1], self.body_xyz[2])
for r in self.mjcf:
r.query_position()
s = self.calc_state(
) # optimization: calc_state() can calculate something in self.* for calc_potential() to use
self.potential = self.calc_potential()
self.camera = self.scene.cpp_world.new_camera_free_float(
self.VIDEO_W, self.VIDEO_H, "video_camera")
return s
def __init__(self, fn, robot_name, action_dim, obs_dim, power):
RoboschoolMujocoXmlEnv.__init__(self, fn, robot_name, action_dim,
obs_dim)
RoboschoolForwardWalker.__init__(self, power)
def __init__(self, power, state_dim):
self.last_state = np.random.rand(state_dim)
self.last_dist = 0
self.still_counter = 0
RoboschoolForwardWalker.__init__(self, power)
def _robot_specific_reset(self):
if self.servo:
RoboschoolForwardWalkerServo.robot_specific_reset(self)
else:
RoboschoolForwardWalker.robot_specific_reset(self)
self.set_initial_orientation(yaw_center=0, yaw_random_spread=np.pi)
def robot_specific_reset(self):
self.rewards_progress = []
self.reward_angleDiff = []
self.reward_alive = []
self.ordered_joints = []
self.jdict = {}
self.jointToUse = [
"fl1", "fl2", "fl3", "fr1", "fr2", "fr3", "bl1", "bl2", "bl3",
"br1", "br2", "br3"
]
for j in self.urdf.joints:
if j.name in self.jointToUse:
#print("\tJoint '%s' limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((j.name,) + j.limits()) )
j.servo_target = 0.0
j.set_servo_target(j.servo_target, self.servo_kp,
self.servo_kd, self.servo_maxTorque)
j.power_coef, j.max_velocity = j.limits()[2:4]
self.ordered_joints.append(j)
self.jdict[j.name] = j
continue
self.iklinks_fl = [
self.jdict["fl1"], self.jdict["fl2"], self.jdict["fl3"]
]
self.iklinks_fr = [
self.jdict["fr1"], self.jdict["fr2"], self.jdict["fr3"]
]
self.iklinks_bl = [
self.jdict["bl1"], self.jdict["bl2"], self.jdict["bl3"]
]
self.iklinks_br = [
self.jdict["br1"], self.jdict["br2"], self.jdict["br3"]
]
self.setupEnergyCost("fl")
self.setupEnergyCost("fr")
self.setupEnergyCost("bl")
self.setupEnergyCost("br")
RoboschoolForwardWalker.robot_specific_reset(self)
self.body_xyz = [-0.3, 0, 0.18]
RoboschoolForwardWalker.move_robot(self, self.body_xyz[0],
self.body_xyz[1], self.body_xyz[2])
self.numStepsPerSecond = 1.0 / self.physics_time_step
self.numSecondsToTrack = 1.0
self.walked_distance = deque(maxlen=int(self.numSecondsToTrack *
self.numStepsPerSecond))
self.progressHistory = deque(maxlen=int(self.numSecondsToTrack *
self.numStepsPerSecond))
if settings.history1Len > 0.0:
self.history1 = deque(maxlen=int(settings.history1Len *
self.numStepsPerSecond))
if settings.history2Len > 0.0:
self.history2 = deque(maxlen=int(settings.history2Len *
self.numStepsPerSecond))
if settings.history3Len > 0.0:
self.history3 = deque(maxlen=int(settings.history3Len *
self.numStepsPerSecond))
if self.robotLibOn:
self.robotLib.executeCommand("cmd#zero")
def __init__(self, fn, robot_name, action_dim, power):
urdfFilename = os.path.join(os.path.dirname(__file__), "models_robot",
fn)
self.velObservations = 0
sensorsObservations = 4 #4 legs from 0 or 1
rotationObservation = 3 #roll pith yaw
sinCosToTarget = 2 # sin cos from angle to target
servoObservation = 12 # rel angles
numObservation = sensorsObservations + rotationObservation + sinCosToTarget + servoObservation + self.velObservations
if hasattr(settings, "history1Len") == False:
settings.history1Len = 0.0
if hasattr(settings, "history2Len") == False:
settings.history2Len = 0.0
if hasattr(settings, "history3Len") == False:
settings.history3Len = 0.0
baseObservations = numObservation
if settings.history1Len > 0.0:
numObservation += baseObservations
if settings.history2Len > 0.0:
numObservation += baseObservations
if settings.history3Len > 0.0:
numObservation += baseObservations
RoboschoolUrdfEnv.__init__(self,
urdfFilename,
robot_name,
action_dim,
numObservation,
fixed_base=False,
self_collision=False)
RoboschoolForwardWalker.__init__(self, power)
self.servo_kp = 0.3
self.servo_kd = 0.9
self.servo_maxTorque = 2.0
self.time = 0.0
self.max_servo_speed = math.radians(230.0)
self.physics_time_step = 1.0 / 240.0
self.walk_target_x = 1000.0 # 1km meters
self.walk_target_z = 0.15
self.debugStats = 0
self.movement_dir = [1, 0, 0]
self.robotLibOn = 0
self.jointsLocalPos = {}
tree = ET.parse(urdfFilename)
root = tree.getroot()
for robot in root.iter('robot'):
for joint in robot.iter('joint'):
origin = joint.find("origin")
posStr = origin.attrib["xyz"].split(" ")
pos = [float(posStr[0]), float(posStr[1]), float(posStr[2])]
self.jointsLocalPos[joint.attrib["name"]] = pos
self.link1 = self.jointsLocalPos["fl3"]
self.link2 = self.jointsLocalPos["fl4"]
self.extLenX = 0.15 # 15 cm
self.extLenY = 0.15 # 15 cm
self.minZLeg = -0.2
self.maxZLeg = -0.02
self.minArea = [-self.extLenX, -self.extLenY,
self.minZLeg] # 20 cm away from chassis
self.maxArea = [+self.extLenX, +self.extLenY,
self.maxZLeg] # 2 cm from chassis
self.flMinArea = glm.vec3(self.minArea)
self.flMaxArea = glm.vec3(self.maxArea)
self.frMinArea = glm.vec3(self.minArea)
self.frMaxArea = glm.vec3(self.maxArea)
self.blMinArea = glm.vec3(self.minArea)
self.blMaxArea = glm.vec3(self.maxArea)
self.brMinArea = glm.vec3(self.minArea)
self.brMaxArea = glm.vec3(self.maxArea)
self.areaSize = [
self.maxArea[0] - self.minArea[0],
self.maxArea[1] - self.minArea[1],
self.maxArea[2] - self.minArea[2]
]
self.len1 = abs(self.link1[2])
self.len2 = abs(self.link2[2])
self.ActionIsAngles = False
self.ActionsIsAdditive = False
self.actionsMult = 1.0
self.chassisSpaceX = 0.30
self.chassisSpaceY = 0.20
self.chassisSpaceMin = [-self.chassisSpaceX, -self.chassisSpaceY, -0.2]
self.chassisSpaceMax = [
+self.chassisSpaceX, +self.chassisSpaceY, -0.02
]
self.inputsInChassisSpace = False
self.actionsInChassisSpace = False
self.inputsIsIKTargets = True
self.perLegSpace = True
if self.perLegSpace:
self.extLenYOut = 0.1 # 10 cm
self.extLenYIn = 0.03 # 2 cm
self.flMinArea = glm.vec3(-self.extLenX, -self.extLenYIn,
self.minZLeg)
self.flMaxArea = glm.vec3(self.extLenX, self.extLenYOut,
-self.maxZLeg)
self.blMinArea = glm.vec3(-self.extLenX, -self.extLenYIn,
self.minZLeg)
self.blMaxArea = glm.vec3(self.extLenX, self.extLenYOut,
-self.maxZLeg)
self.frMinArea = glm.vec3(-self.extLenX, -self.extLenYOut,
self.minZLeg)
self.frMaxArea = glm.vec3(self.extLenX, self.extLenYIn,
-self.maxZLeg)
self.brMinArea = glm.vec3(-self.extLenX, -self.extLenYOut,
self.minZLeg)
self.brMaxArea = glm.vec3(self.extLenX, self.extLenYIn,
-self.maxZLeg)
if settings.robotLibAvailable and hasattr(settings, "robotNN"):
if settings.robotNN == 0:
self.robotLibOn = 1
else:
self.robotLibOn = 0
self.robotLib = pyRobotLib.Robot()
self.robotLib.load(
"/Users/pavlogryb/Dropbox/Projects/Robot/Robot/Robot/")
print(os.getpid())
def robot_specific_reset(self):
RoboschoolForwardWalker.robot_specific_reset(self)
self.set_initial_orientation(yaw_center=0, yaw_random_spread=np.pi)
self.head = self.parts["head"]