def first_reset(self):
# print("first reset")
jointtrajpub = JointTrajPub()
for update in range(500):
jointtrajpub.jointTrajectoryCommand_reset(self.arr_init_pos0)
time.sleep(0.5)
for update in range(300):
jointtrajpub.jointTrajectoryCommand_reset(self.arr_init_pos1)
time.sleep(0.5)
def reset(self):
self.max_knob_rotation = 0
self.max_door_rotation = 0
self.max_wirst3 = 0
self.min_wirst3 = 0
self.max_wirst2 = 0
self.min_wirst2 = 0
self.max_wirst1 = 0
self.min_wirst1 = 0
self.max_elb = 0
self.min_elb = 0
self.max_shl = 0
self.min_shl = 0
self.max_shp = 0
self.min_shp = 0
self.max_force_x = 0
self.min_force_x = 0
self.max_force_y = 0
self.min_force_y = 0
self.max_force_z = 0
self.min_force_z = 0
self.max_torque_x = 0
self.min_torque_x = 0
self.max_torque_y = 0
self.min_torque_y = 0
self.max_torque_z = 0
self.min_torque_z = 0
# Go to initial position
self._gz_conn.unpauseSim()
# rospy.logdebug("set_init_pose init variable...>>>" + str(self.init_joint_pose0))
jointtrajpub = JointTrajPub()
for update in range(200):
jointtrajpub.GrpCommand(self.arr_init_g_pos1)
# time.sleep(1)
for update in range(300):
jointtrajpub.jointTrajectoryCommand_reset(self.arr_init_pos2)
time.sleep(1)
for update in range(300):
jointtrajpub.jointTrajectoryCommand_reset(self.arr_init_pos1)
time.sleep(1)
# 0st: We pause the Simulator
# rospy.logdebug("Pausing SIM...")
self._gz_conn.pauseSim()
# 1st: resets the simulation to initial values
# rospy.logdebug("Reset SIM...")
self._gz_conn.resetSim()
# 2nd: We Set the gravity to 0.0 so that we dont fall when reseting joints
# It also UNPAUSES the simulation
# rospy.logdebug("Remove Gravity...")
self._gz_conn.change_gravity_zero()
# EXTRA: Reset JoinStateControlers because sim reset doesnt reset TFs, generating time problems
# rospy.logdebug("reset_ur_joint_controllers...")
self._ctrl_conn.reset_ur_joint_controllers(self._ctrl_type)
# 3rd: resets the robot to initial conditions
# rospy.logdebug("set_init_pose init variable...>>>" + str(self.init_joint_pose1))
# rospy.logdebug("set_init_pose init variable...>>>" + str(self.init_joint_pose2))
self.force = self.wrench_stamped.wrench.force
self.torque = self.wrench_stamped.wrench.torque
# print("self.force", self.force)
# print("self.torque", self.torque)
self.force_ini = copy.deepcopy(self.force)
self.torque_ini = copy.deepcopy(self.torque)
# We save that position as the current joint desired position
# 4th: We Set the init pose to the jump topic so that the jump control can update
# We check the jump publisher has connection
if self._ctrl_type == 'traj_pos':
self._joint_traj_pubisher.check_publishers_connection()
elif self._ctrl_type == 'pos':
self._joint_pubisher.check_publishers_connection()
elif self._ctrl_type == 'traj_vel':
self._joint_traj_pubisher.check_publishers_connection()
elif self._ctrl_type == 'vel':
self._joint_pubisher.check_publishers_connection()
else:
rospy.logwarn("Controller type is wrong!!!!")
# 5th: Check all subscribers work.
# Get the state of the Robot defined by its RPY orientation, distance from
# desired point, contact force and JointState of the three joints
# rospy.logdebug("check_all_systems_ready...")
self.check_all_systems_ready()
# 6th: We restore the gravity to original
# rospy.logdebug("Restore Gravity...")
self._gz_conn.adjust_gravity()
for update in range(300):
jointtrajpub.jointTrajectoryCommand_reset(self.arr_init_pos2)
time.sleep(1)
for update in range(200):
jointtrajpub.GrpCommand(self.arr_init_g_pos2)
time.sleep(1)
# 7th: pauses simulation
# rospy.logdebug("Pause SIM...")
self._gz_conn.pauseSim()
self.right_image_ini = copy.deepcopy(self.right_image)
self.left_image_ini = copy.deepcopy(self.left_image)
# 8th: Get the State Discrete Stringuified version of the observations
# rospy.logdebug("get_observations...")
observation = self.get_observations()
# print("[observations]", observation)
return observation