def updateSimulation(self, action, botId, jointIndex, overspeed=False):
last_err = self.arm_target_prev - self.arm_angle_jittered
s2r.networkStep(err=last_err)
activation = self.decodeAction(action)
if overspeed:
activation = 0.5
self.activation_buf.insert(0, activation)
self.activation_buf.pop()
target = cm.wrapRad(self.arm_angle_real + activation * cm.MAX_RADIANS)
self.arm_target_prev = target
err = s2r.networkPredictPos(target)
target = cm.wrapRad(target + err)
p.setJointMotorControl2(bodyUniqueId=self.botId,
jointIndex=jointIndex,
controlMode=p.POSITION_CONTROL,
targetPosition=target,
positionGain=1,
velocityGain=1,
maxVelocity=cm.VEL_MAX_ARM)
self.arm_angle_real_prev = self.arm_angle_real
def compute_observation(self):
arm_angle_real, self.arm_vel_real, _, _ = p.getJointState(
self.botId, self.armId)
self.arm_angle_real = cm.wrapRad(arm_angle_real)
arm_to_axle_id = self.poleId
pole_angle_real, self.pole_vel_real, _, _ = p.getJointState(
self.botId, self.poleId)
self.pole_angle_real = cm.wrapRad(pole_angle_real)
self.arm_angle_real = s2r.noise(self.arm_angle_real, cm.NOISE_SENSE)
self.arm_vel_real = s2r.noise(self.arm_vel_real, cm.NOISE_SENSE)
self.pole_angle_real = s2r.noise(self.pole_angle_real, cm.NOISE_SENSE)
self.pole_vel_real = s2r.noise(self.pole_vel_real, cm.NOISE_SENSE)
obs = [
self.arm_angle_real, self.arm_vel_real, self.pole_angle_real,
self.pole_vel_real, self.arm_angle_target
]
return obs
def updateObs(render=False):
global arm_last, pole_last, time_last, obs
now = time.time()
diff = now - time_last
pole = getPole()
arm = getArm()
pole = cm.rad2Norm(cm.wrapRad(cm.norm2Rad(pole) + cm.norm2Rad(pole_offset)))
arm_vel = getVel(arm, arm_last, diff)
pole_vel = getVel(pole, pole_last, diff)
arm_last = arm
pole_last = pole
time_last = now
if render:
print("arm: %.6f %.6f \tpole: %.6f %.6f" % (arm, arm_vel, pole, pole_vel))
print("arm: %.3f %.3f \t\tpole: %.3f %.3f" % (cm.rad2Deg(cm.norm2Rad(arm)), np.sign(arm_vel)*cm.rad2Deg(abs(arm_vel)), cm.rad2Deg(cm.norm2Rad(pole)), np.sign(pole_vel)*cm.rad2Deg(abs(pole_vel))))
obs = [arm, arm_vel, pole, pole_vel, ARM_TARGET_RAD]
return obs
def calculateTarget(delta):
return cm.wrapRad(cm.norm2Rad(arm_last) + delta)
def timeLinear(start, now, pos):
target = cm.wrapRad((now - start) * 2 * cm.PI)
return cm.difRads(pos, target)
if time_last > start + CYCLE_PERIOD_S:
break
action, _ = p.predict(np.array(obs))
activation = THROTTLE_PROFILE[action]
delta = activation * cm.MAX_RADIANS / 2 # reality factor
print("%d\t%.6f\t%.3f\t%.3f\t%.6f" %
(count, obs[0], cm.rad2Deg(obs[0]), activation, delta))
buf_time.append(time_last)
buf_pos.append(obs[0])
buf_act.append(activation)
buf_d_p.append(delta)
pos = cm.wrapRad(cm.norm2Rad(obs[0]) + delta)
pos = cm.rad2Norm(pos)
obs[0] = pos
count += 1
now = time.time()
dif = now - time_last
time.sleep(cm.STEP - dif)
plt.plot(buf_time, buf_pos)
plt.plot(buf_time, buf_act)
plt.plot(buf_time, buf_d_p)
plt.show()
def rad2rip(rad):
if not S2R_RIPPLE:
return 0
rad = cm.wrapRad(rad)
deg = int(cm.rad2Deg(rad))
return rip[deg]
