def step(self, a):
if self.ctrl is None:
self.ctrl = CtrlUtils(self.sim)
# self.do_simulation(a, self.frame_skip)
qposd_robot = np.array([0, 0, 0, -np.pi / 2, -np.pi / 2, 0, 0])
self.ctrl.calculate_errors(self.sim, qpos_ref=qposd_robot)
u = self.ctrl.ctrl_action(self.sim) + a[:N_JOINTS]
u[JOINT_6] += a[JOINT_6]
u[JOINT_3] += a[JOINT_3]
self.sim.data.ctrl[:self.ctrl.nv] = u
self.sim.data.ctrl[-1] = a[-1]
self.sim.step()
if (self.ep > 60):
self.sim.render(mode="window")
ob = self._get_obs()
#Modelando a reward
#Primeiramente, pegamos o target e o erro
desired_angle = self.desired_angle
erro = self.erro
self.current_step += 1
#Angulo relativo entre a ferramenta e o gripper
angle_error = ob[0]
current_angle = angle_error + desired_angle
# print("ANGULO ATUAL: ", current_angle)
#Checa se está na região de sucesso
if (current_angle >= (desired_angle - erro) and current_angle <=
(desired_angle + erro)):
reward = (-1) * np.abs(current_angle - desired_angle) / 200
self.counter = self.counter + 1
done = 0
#Caso fique uma quantidade de tempo na região de sucesso
if (self.counter > 120):
print("*********************Completou***********************")
print(" || Episodio: ", self.ep, " || Reward: ",
np.round(self.ep_ret, 0), " || Steps: ", self.ep_len,
" || Angulo Final: ",
np.round(angle_error + desired_angle,
3), " || Target: ", desired_angle)
reward = 10
#Zerando o contador de tempo na zona de sucesso
self.counter = 0
#Zerando o tempo do episódio
self.ep_len = 0
#Zerando a recompensa acumulada
self.ep_ret = 0
done = 1
return ob, reward, done, {}
#Se não completar
else:
self.counter = 0
reward = (-1) * np.abs(current_angle - desired_angle) / 100
done = 0
#Reward total e duração do episodio
self.ep_ret += reward
self.ep_len += 1
if (self.current_step == MAX_EP_LEN):
#Printando resultados
print(" || Episodio: ", self.ep, " || Reward: ",
np.round(self.ep_ret, 0), " || Steps: ",
self.ep_len, " || Angulo Final: ",
np.round(angle_error + desired_angle,
3), " || Target: ", desired_angle)
#Zerando a duração do episodio
self.ep_len = 0
#Atualizando o episodio atual
self.current_ep += 1
#Zerando a recompensa acumulada
self.ep_ret = 0
return ob, reward, done, {}
class PivotingEnv(mujoco_env.MujocoEnv, utils.EzPickle):
def __init__(self):
self.ep = 0
self.current_ep = 0
self.counter = 0
self.desired_angle = 0
self.erro = 0
self.current_step = 0
self.ep_ret = 0
self.ep_len = 0
self.ctrl = None
utils.EzPickle.__init__(self)
mujoco_env.MujocoEnv.__init__(self, 'pivoting_kuka.xml', 8)
def step(self, a):
if self.ctrl is None:
self.ctrl = CtrlUtils(self.sim)
# self.do_simulation(a, self.frame_skip)
qposd_robot = np.array([0, 0, 0, -np.pi / 2, -np.pi / 2, 0, 0])
self.ctrl.calculate_errors(self.sim, qpos_ref=qposd_robot)
u = self.ctrl.ctrl_action(self.sim) + a[:N_JOINTS]
u[JOINT_6] += a[JOINT_6]
u[JOINT_3] += a[JOINT_3]
self.sim.data.ctrl[:self.ctrl.nv] = u
self.sim.data.ctrl[-1] = a[-1]
self.sim.step()
if (self.ep > 60):
self.sim.render(mode="window")
ob = self._get_obs()
#Modelando a reward
#Primeiramente, pegamos o target e o erro
desired_angle = self.desired_angle
erro = self.erro
self.current_step += 1
#Angulo relativo entre a ferramenta e o gripper
angle_error = ob[0]
current_angle = angle_error + desired_angle
# print("ANGULO ATUAL: ", current_angle)
#Checa se está na região de sucesso
if (current_angle >= (desired_angle - erro) and current_angle <=
(desired_angle + erro)):
reward = (-1) * np.abs(current_angle - desired_angle) / 200
self.counter = self.counter + 1
done = 0
#Caso fique uma quantidade de tempo na região de sucesso
if (self.counter > 120):
print("*********************Completou***********************")
print(" || Episodio: ", self.ep, " || Reward: ",
np.round(self.ep_ret, 0), " || Steps: ", self.ep_len,
" || Angulo Final: ",
np.round(angle_error + desired_angle,
3), " || Target: ", desired_angle)
reward = 10
#Zerando o contador de tempo na zona de sucesso
self.counter = 0
#Zerando o tempo do episódio
self.ep_len = 0
#Zerando a recompensa acumulada
self.ep_ret = 0
done = 1
return ob, reward, done, {}
#Se não completar
else:
self.counter = 0
reward = (-1) * np.abs(current_angle - desired_angle) / 100
done = 0
#Reward total e duração do episodio
self.ep_ret += reward
self.ep_len += 1
if (self.current_step == MAX_EP_LEN):
#Printando resultados
print(" || Episodio: ", self.ep, " || Reward: ",
np.round(self.ep_ret, 0), " || Steps: ",
self.ep_len, " || Angulo Final: ",
np.round(angle_error + desired_angle,
3), " || Target: ", desired_angle)
#Zerando a duração do episodio
self.ep_len = 0
#Atualizando o episodio atual
self.current_ep += 1
#Zerando a recompensa acumulada
self.ep_ret = 0
return ob, reward, done, {}
def reset_model(self):
# print("epoch =", self.ep)
#Recebe qpos e qvel do modelo do mujoco, com vários dados que não serão utilizados
qpos_init_robot = [0, 0, 0, -np.pi / 2, -np.pi / 2, 0, 0]
qpos_init_gripper = [0]
qpos_init_tool = [0.1, 0, 1.785, 1, 0, 0, 0]
qpos = np.concatenate(
(qpos_init_robot, qpos_init_gripper, qpos_init_tool))
qvel = self.init_qvel
#Atualizando o numero do episódio e steps atuais. Está com gambiarra
self.ep += 1
self.counter = 0
self.current_step = 0
#Definindo o angulo desejado(target)
self.desired_angle = np.random.randint(-30, 30)
while (self.desired_angle == 0):
self.desired_angle = np.random.randint(-30, 30)
#Definindo o erro aceitável para a conclusão do objetivo
self.erro = min(np.abs(self.desired_angle / 7), 3)
if ((self.desired_angle) <= 6 or (self.desired_angle) >= -6):
self.erro = self.erro + 0.3
self.set_state(qpos, qvel)
return self._get_obs()
def _get_obs(self):
#Pegando o Angulo da ferramenta em radianos
obs = self.sim.data.get_joint_qpos("tool")
tools_angle = np.arctan2(2 * (obs[3] * obs[6] + obs[4] * obs[5]),
(1 - 2 * (obs[5]**2 + obs[6]**2)))
tools_angle = 180 * tools_angle / np.pi
#Gripper's angle
grippers_angle = self.sim.data.get_joint_qpos("kuka_joint_6")
grippers_angle = 180 * grippers_angle / np.pi
#Angle error between tool and gripper with relation to the desired angle
relative_angle = tools_angle - grippers_angle - self.desired_angle
#Gripper's Distance
grippers_dist = obs[1]
#Gripper's velocity
grippers_vel = self.sim.data.get_joint_qvel("kuka_joint_6")
#Tool's velocity relative to gripper
tools_vel = self.sim.data.get_joint_qvel("tool")
tools_vel = tools_vel[5] #Tool's global velocity
tools_vel = tools_vel - grippers_vel
if (obs[2] < 1.1):
drop = 1
else:
drop = 0
obs = np.concatenate([[relative_angle], [grippers_dist],
[grippers_vel], [tools_vel], [drop]]).ravel()
return obs
def viewer_setup(self):
v = self.viewer
v.cam.trackbodyid = 0
v.cam.distance = self.model.stat.extent + 0.7
use_gravity = True # If False, loads the model without gravity.
plot_2d = True # Show 2D plots at the end. But still need to fix after all new features.
use_kd = True # If True, use PD feedback. If False, just P.
use_ki = True # If True use a PID feedback. If False, just PD.
simulation_time = 10 # [s]. This is the maximum time the robot will wait at the same position.
# Choose with controller should be used. CtrlType.INV_DYNAMICS is the default
# controller_type = CtrlType.INDEP_JOINTS
# controller_type = CtrlType.INV_DYNAMICS
# controller_type = CtrlType.INV_DYNAMICS_OP_SPACE
sim, viewer = load_model_mujoco(show_simulation, use_gravity)
sim.step() # single step for operational space update
ctrl = CtrlUtils(sim,
simulation_time=simulation_time,
use_gravity=use_gravity,
plot_2d=plot_2d,
use_kd=use_kd,
use_ki=use_ki)
# ctrl.controller_type = CtrlType.INDEP_JOINTS
print(ctrl.name_tcp)
print(ctrl.name_ft_sensor)
# Inverse dynamics in joint space
qd = np.array([0, 0.461, 0, -0.817, 0, 0.69, 0])
ctrl.move_to_joint_pos(sim, qd=qd, viewer=viewer)
# ctrl.controller_type = CtrlType.INV_DYNAMICS
# ctrl.get_pd_matrices()
qd = np.array([0, 0, 0, -np.pi / 2, -np.pi / 2, 0, 0])
ctrl.move_to_joint_pos(sim, qd=qd, viewer=viewer)
# qd = np.array([0, -0.4, 0, -0.3, .5, 0.69, 0])
# ctrl.move_to_joint_pos(sim, qd=qd, viewer=viewer)
use_gravity = True # If False, loads the model without gravity.
plot_2d = True # Show 2D plots at the end. But still need to fix after all new features.
use_kd = True # If True, use PD feedback. If False, just P.
use_ki = True # If True use a PID feedback. If False, just PD.
simulation_time = 10 # [s]. This is the maximum time the robot will wait at the same position.
# Choose with controller should be used. CtrlType.INV_DYNAMICS is the default
# controller_type = CtrlType.INDEP_JOINTS
# controller_type = CtrlType.INV_DYNAMICS
# controller_type = CtrlType.INV_DYNAMICS_OP_SPACE
sim, viewer = load_model_mujoco(show_simulation, use_gravity)
sim.step() # single step for operational space update
ctrl = CtrlUtils(sim,
simulation_time=simulation_time,
use_gravity=use_gravity,
plot_2d=plot_2d,
use_kd=use_kd,
use_ki=use_ki)
# Inverse dynamics in joint space
# qd = np.array([0, 0.461, 0, -0.817, 0, 0.69, 0])
qd = np.array([0, 0, 0, -np.pi / 2, 0, np.pi / 2, 0])
ctrl.move_to_joint_pos(sim, qd=qd, viewer=viewer)
# qd = np.array([0, -0.4, 0, -0.3, .5, 0.69, 0])
# ctrl.move_to_joint_pos(qd, sim, viewer=viewer)
# qd = np.array([0, 0.1, 0, -0.6, 0, 0.13, 0])
# ctrl.move_to_joint_pos(qd, sim, viewer=viewer)
# qd = np.array([np.pi/3, 0, 0, -np.pi/2, 0, 0, 0])
# ctrl.move_to_joint_pos(qd, sim, viewer=viewer)
# q0 = np.zeros(7)
# ctrl.move_to_joint_pos(q0, sim, viewer=viewer)
def step(self, a):
if self.ctrl is None:
self.ctrl = CtrlUtils(self.sim)
# self.do_simulation(a, self.frame_skip)
qposd_robot = np.array([0, 0, 0, -np.pi / 2, -np.pi / 2, 0, 0])
# TODO: NAO USAR ESSES AQUI POR ENQUANTO, TEMOS QUE ALTERAR O XML
# PPO ATUANDO NA POSICAO (comentar aqui se for usar torque)
# qpos_ppo = self.ctrl_action_position(a)
# self.ctrl.calculate_errors(self.sim, qpos_ref=qpos_ppo)
# u = self.ctrl.ctrl_action(self.sim)
# PPO ATUANDO NO TORQUE
self.ctrl.calculate_errors(self.sim, qpos_ref=qposd_robot)
u = self.ctrl_action_torque(a, qposd_robot=qposd_robot)
# NAO EDITAR DAQUI PRA BAIXO
self.sim.data.ctrl[:self.ctrl.
nv] = u # jogando ação de controle nas juntas
self.sim.data.ctrl[-1] = a[
-1] # ação de controle na garra. Esse aqui é tudo por conta do PPO
self.sim.step()
if (self.ep > 1):
self.sim.render(mode="window")
ob = self._get_obs()
#Modelando a reward
#Primeiramente, pegamos o target e o erro
desired_angle = self.desired_angle
erro = self.erro
self.current_step += 1
#Angulo relativo entre a ferramenta e o gripper
angle_error = ob[0]
current_angle = angle_error + desired_angle
# print("ANGULO ATUAL: ", current_angle)
#Checa se está na região de sucesso
if (current_angle >= (desired_angle - erro) and current_angle <=
(desired_angle + erro)):
reward = (-1) * np.abs(current_angle - desired_angle) / 200
self.counter = self.counter + 1
done = 0
#Caso fique uma quantidade de tempo na região de sucesso
if (self.counter > 120):
print("*********************Completou***********************")
print(" || Episodio: ", self.ep, " || Reward: ",
np.round(self.ep_ret, 0), " || Steps: ", self.ep_len,
" || Angulo Final: ",
np.round(angle_error + desired_angle,
3), " || Target: ", desired_angle)
reward = 10
#Zerando o contador de tempo na zona de sucesso
self.counter = 0
#Zerando o tempo do episódio
self.ep_len = 0
#Zerando a recompensa acumulada
self.ep_ret = 0
done = 1
return ob, reward, done, {}
#Se não completar
else:
self.counter = 0
reward = (-1) * np.abs(current_angle - desired_angle) / 100
done = 0
#Reward total e duração do episodio
self.ep_ret += reward
self.ep_len += 1
if (self.current_step == MAX_EP_LEN):
#Printando resultados
print(" || Episodio: ", self.ep, " || Reward: ",
np.round(self.ep_ret, 0), " || Steps: ",
self.ep_len, " || Angulo Final: ",
np.round(angle_error + desired_angle,
3), " || Target: ", desired_angle)
#Zerando a duração do episodio
self.ep_len = 0
#Atualizando o episodio atual
self.current_ep += 1
#Zerando a recompensa acumulada
self.ep_ret = 0
return ob, reward, done, {}
class PivotingEnv(mujoco_env.MujocoEnv, utils.EzPickle):
def __init__(self):
self.ep = 0
self.current_ep = 0
self.counter = 0
self.desired_angle = 0
self.erro = 0
self.current_step = 0
self.ep_ret = 0
self.ep_len = 0
self.ctrl = None
utils.EzPickle.__init__(self)
mujoco_env.MujocoEnv.__init__(self, 'pivoting_kuka.xml', 8)
def ctrl_action_torque(self, a, qposd_robot):
'''Métodos de torque'''
# método 1: nessa config o PPO vai atuar JUNTO com meu controlador em torque EM TODAS AS JUNTAS
# u = self.ctrl.ctrl_action(self.sim) + a[:N_JOINTS]
# método 2: nessa config o PPO vai controlar tudo em torque sem ajuda do meu controlador
# u = a[:N_JOINTS]
# método 3: aqui o PPO vai atuar só nas juntas "planares" em torque
u = self.ctrl.ctrl_action(self.sim)
u[JOINT_6] += a[JOINT_6]
u[JOINT_3] += a[JOINT_3]
return u
# def ctrl_action_position(self, a):
# '''Métodos de posição'''
# # método 1: nessa config o PPO vai atuar JUNTO com meu controlador em posição em TODAS AS JUNTAS
# # qposd_ppo = np.array([0, 0, 0, -np.pi / 2, -np.pi / 2, 0, 0]) + a[:N_JOINTS]
#
# # método 2: nessa config o PPO vai controlar tudo em posição. Aprendizado mais lento!
# # qposd_ppo = a[:N_JOINTS]
#
# # método 3: aqui o PPO vai atuar só nas juntas "planares" em posição
# # qposd_ppo = np.array([0, 0, 0, -np.pi / 2, -np.pi / 2, 0, 0])
# # qposd_ppo[JOINT_6] += a[JOINT_6]
# # qposd_ppo[JOINT_3] += a[JOINT_3]
#
# return qposd_ppo
def step(self, a):
if self.ctrl is None:
self.ctrl = CtrlUtils(self.sim)
# self.do_simulation(a, self.frame_skip)
qposd_robot = np.array([0, 0, 0, -np.pi / 2, -np.pi / 2, 0, 0])
# TODO: NAO USAR ESSES AQUI POR ENQUANTO, TEMOS QUE ALTERAR O XML
# PPO ATUANDO NA POSICAO (comentar aqui se for usar torque)
# qpos_ppo = self.ctrl_action_position(a)
# self.ctrl.calculate_errors(self.sim, qpos_ref=qpos_ppo)
# u = self.ctrl.ctrl_action(self.sim)
# PPO ATUANDO NO TORQUE
self.ctrl.calculate_errors(self.sim, qpos_ref=qposd_robot)
u = self.ctrl_action_torque(a, qposd_robot=qposd_robot)
# NAO EDITAR DAQUI PRA BAIXO
self.sim.data.ctrl[:self.ctrl.
nv] = u # jogando ação de controle nas juntas
self.sim.data.ctrl[-1] = a[
-1] # ação de controle na garra. Esse aqui é tudo por conta do PPO
self.sim.step()
if (self.ep > 1):
self.sim.render(mode="window")
ob = self._get_obs()
#Modelando a reward
#Primeiramente, pegamos o target e o erro
desired_angle = self.desired_angle
erro = self.erro
self.current_step += 1
#Angulo relativo entre a ferramenta e o gripper
angle_error = ob[0]
current_angle = angle_error + desired_angle
# print("ANGULO ATUAL: ", current_angle)
#Checa se está na região de sucesso
if (current_angle >= (desired_angle - erro) and current_angle <=
(desired_angle + erro)):
reward = (-1) * np.abs(current_angle - desired_angle) / 200
self.counter = self.counter + 1
done = 0
#Caso fique uma quantidade de tempo na região de sucesso
if (self.counter > 120):
print("*********************Completou***********************")
print(" || Episodio: ", self.ep, " || Reward: ",
np.round(self.ep_ret, 0), " || Steps: ", self.ep_len,
" || Angulo Final: ",
np.round(angle_error + desired_angle,
3), " || Target: ", desired_angle)
reward = 10
#Zerando o contador de tempo na zona de sucesso
self.counter = 0
#Zerando o tempo do episódio
self.ep_len = 0
#Zerando a recompensa acumulada
self.ep_ret = 0
done = 1
return ob, reward, done, {}
#Se não completar
else:
self.counter = 0
reward = (-1) * np.abs(current_angle - desired_angle) / 100
done = 0
#Reward total e duração do episodio
self.ep_ret += reward
self.ep_len += 1
if (self.current_step == MAX_EP_LEN):
#Printando resultados
print(" || Episodio: ", self.ep, " || Reward: ",
np.round(self.ep_ret, 0), " || Steps: ",
self.ep_len, " || Angulo Final: ",
np.round(angle_error + desired_angle,
3), " || Target: ", desired_angle)
#Zerando a duração do episodio
self.ep_len = 0
#Atualizando o episodio atual
self.current_ep += 1
#Zerando a recompensa acumulada
self.ep_ret = 0
return ob, reward, done, {}
def reset_model(self):
# print("epoch =", self.ep)
#Recebe qpos e qvel do modelo do mujoco, com vários dados que não serão utilizados
qpos_init_robot = [0, 0, 0, -np.pi / 2, -np.pi / 2, 0, 0]
qpos_init_gripper = [0]
qpos_init_tool = [0.04, 0, 1.785, 1, 0, 0, 0]
qpos = np.concatenate(
(qpos_init_robot, qpos_init_gripper, qpos_init_tool))
qvel = self.init_qvel
#Atualizando o numero do episódio e steps atuais. Está com gambiarra
self.ep += 1
self.counter = 0
self.current_step = 0
#Definindo o angulo desejado(target)
self.desired_angle = np.random.randint(-30, 30)
while (self.desired_angle == 0):
self.desired_angle = np.random.randint(-30, 30)
#Definindo o erro aceitável para a conclusão do objetivo
self.erro = min(np.abs(self.desired_angle / 7), 3)
if ((self.desired_angle) <= 6 or (self.desired_angle) >= -6):
self.erro = self.erro + 0.3
self.set_state(qpos, qvel)
return self._get_obs()
def _get_obs(self):
#Pegando o Angulo da ferramenta em radianos
obs = self.sim.data.get_joint_qpos("tool")
tools_angle = np.arctan2(2 * (obs[3] * obs[6] + obs[4] * obs[5]),
(1 - 2 * (obs[5]**2 + obs[6]**2)))
tools_angle = 180 * tools_angle / np.pi
#Gripper's angle
grippers_angle = self.sim.data.get_joint_qpos("kuka_joint_6")
grippers_angle = 180 * grippers_angle / np.pi
#Angle error between tool and gripper with relation to the desired angle
relative_angle = tools_angle - grippers_angle - self.desired_angle
#Gripper's Distance
grippers_dist = obs[1]
#Gripper's velocity
grippers_vel = self.sim.data.get_joint_qvel("kuka_joint_6")
#Tool's velocity relative to gripper
tools_vel = self.sim.data.get_joint_qvel("tool")
tools_vel = tools_vel[5] #Tool's global velocity
tools_vel = tools_vel - grippers_vel
if (obs[2] < 1.1):
drop = 1
else:
drop = 0
obs = np.concatenate([[relative_angle], [grippers_dist],
[grippers_vel], [tools_vel], [drop]]).ravel()
return obs
def viewer_setup(self):
v = self.viewer
v.cam.trackbodyid = 0
v.cam.distance = self.model.stat.extent + 0.7