# jointboi.move(testpos)
# rospy.sleep(1)
# TODO fix reset function
# rospy.sleep(1)
# print(envboi.get_latest_state())
# envboi.reset()
# rospy.sleep(2)
# #rospy.spin()
#position_command.pos_commander(testpos)
rewards = np.array([])
while True:
current_distance = envboi.get_goal_distance()
print("Current distance is: " + str(current_distance))
action = (raw_input("Enter an action from -1 to 1: \n"))
action = action.split(',')
action = [float(i) for i in action]
action = np.array(action)
print(action)
(state, reward, done) = envboi.step(action)
"Reward is {}, Done: {}".format(reward, done)
rewards = np.append(rewards, [reward])
if (done):
total_rewards = str(np.sum(rewards))
print("Well done! you reached the goal \n Total reward: " +
total_rewards)
break
import rospy
from arm_env import ArmEnvironment, AllJoints
import numpy as np
rospy.init_node("test")
env = ArmEnvironment(static_goal=True)
env.reset()
rospy.sleep(1)
print("Goal Distance: ", env.get_goal_distance())
while (True):
action = input("Enter an action:").split(' ')
floatarr = []
for i in action:
if (i == '0'):
floatarr.append(0)
else:
floatarr.append(float(i))
action = np.array(floatarr)
env.step(floatarr)
print("Goal Distance: ", env.get_goal_distance())
reset = eval(input("Reset goal?"))
if (reset):
env.set_new_goal()
# alljoints.move(pos)
# #rospy.sleep(3)
# env.pause_physics()
# env.unpause_physics()