Resetting simulation

edit

When I run my program and my robot meets the reward requirement the program resets the robot to its original pose, but when I try to run the program again it doesn't print the reward and when it visually looks like the robot hits the reward requirement the simulation doesn't reset itself. I'm trying to get to the point where the simulation keeps resetting itself in a loop so I can collect data. Here is my code.

class RobotControl:

def __init__(self):
    rospy.init_node('robot_control_node', anonymous=True)
    self.talker = rospy.Publisher('/rrbot/joint1_position_controller/command', Float64, queue_size=10)
    rospy.Subscriber('/rrbot/joint_states', JointState, self.joint_callback)
    self.force = None
    self.joint = None
    self.rate = rospy.Rate(10)
    reward = None

    while not rospy.is_shutdown():
        self.give_force()
        self.rate.sleep()
        '''
        if self.reward is not None:
            reset_simulation()
            self.rate.sleep()
            RobotControl()
        #rospy.spin()
        '''



def give_force(self):
    self.force = random.choice([-5,5])  # Changes force every time, right before publishing
    self.talker.publish(self.force)

    '''
    if self.observation_check() is True:
        self.force = 0
    else:
        pass
    '''


def joint_callback(self, data):
    self.joint = data.position[0]
    reward = None
    if self.observation_check() is True:
        reward = (time.time() - start_time)
        #reset_world()
        reset_simulation()
        #sys.exit("restarting simulation")


    else:
        pass

    print(reward)
    #return reward

def observation_check(self):
    max_height = -3.3
    if self.joint < max_height:
        done = True
    else:
        done = False
    return done

def robotcontrolloop(): RobotControl()

robotcontrolloop()