def GetState(self, request, context):
try:
msg = robot_server_pb2.State()
return self.rosbridge.get_state()
except:
rospy.logerr('Failed to get state', exc_info=True)
return robot_server_pb2.State(success=0)
def GetState(self, request, context):
try:
msg = robot_server_pb2.State()
msg.state.extend(self.rosbridge.get_state())
msg.success = 1
return msg
except:
return robot_server_pb2.State(success = 0)
def SendActionGetState(self, request, context):
try:
executed_action = self.rosbridge.publish_env_arm_cmd(request.action)
return self.rosbridge.get_state()
except:
rospy.logerr('Failed to send action and get state', exc_info=True)
return robot_server_pb2.State(success = 0)
def _set_initial_robot_server_state(
self,
rs_state,
fixed_object_position=None) -> robot_server_pb2.State:
if fixed_object_position:
state_msg = super()._set_initial_robot_server_state(
rs_state=rs_state, fixed_object_position=fixed_object_position)
return state_msg
z_amplitude = np.random.default_rng().uniform(low=0.09, high=0.35)
z_frequency = 0.125
z_offset = np.random.default_rng().uniform(low=0.2, high=0.6)
string_params = {"object_0_function": "triangle_wave"}
float_params = {
"object_0_x": 0.12,
"object_0_y": 0.34,
"object_0_z_amplitude": z_amplitude,
"object_0_z_frequency": z_frequency,
"object_0_z_offset": z_offset
}
state = {}
state_msg = robot_server_pb2.State(state=state,
float_params=float_params,
string_params=string_params,
state_dict=rs_state)
return state_msg
def get_state(self, ):
msg = self.robot_server_stub.GetState(robot_server_pb2.State(),
timeout=20)
if msg.success == 1:
return msg.state
else:
raise Exception('Error while retrieving state')
def get_state(self):
self.get_state_event.clear()
# # currently only working on a fixed target mode
if self.target_mode == FIXED_TARGET_MODE:
target = copy.deepcopy(self.target)
else:
raise ValueError
panda_state = copy.deepcopy(self.panda_state)
# # TODO is ee_to_base_transform value correctly loaded and set
# (position, quaternion) = self.tf_listener.lookupTransform(
# self.reference_frame)
# ee_to_base_transform = position + quaternion
# # TODO currently not needed
# # if self.real_robot:
# # panda_collision = False
# # else:
# # panda_collision = any(self.collision_sensors.values())
self.get_state_event.set()
# Create and fill State message
msg = robot_server_pb2.State()
msg.state.extend(target)
msg.state.extend(panda_state)
# msg.state.extend(ee_to_base_transform)
# msg.state.extend([panda_collision])
msg.success = True
return msg
def get_state(self):
self.get_state_event.clear()
# Get environment state
state = []
target = copy.deepcopy(self.target)
mir_pose = copy.deepcopy(self.mir_pose)
mir_twist = copy.deepcopy(self.mir_twist)
mir_f_scan = copy.deepcopy(self.f_scan)
mir_b_scan = copy.deepcopy(self.b_scan)
in_collision = copy.deepcopy(self.collision)
obstacles = [0.0] * 9
self.get_state_event.set()
# Create and fill State message
msg = robot_server_pb2.State()
msg.state.extend(target)
msg.state.extend(mir_pose)
msg.state.extend(mir_twist)
msg.state.extend(mir_f_scan)
msg.state.extend(mir_b_scan)
msg.state.extend([in_collision])
msg.state.extend(obstacles)
msg.success = 1
return msg
def reset(self, start_pose = None, target_pose = None):
"""Environment reset.
Args:
start_pose (list[3] or np.array[3]): [x,y,yaw] initial robot position.
target_pose (list[3] or np.array[3]): [x,y,yaw] target robot position.
Returns:
np.array: Environment state.
"""
self.elapsed_steps = 0
self.prev_base_reward = None
# Initialize environment state
self.state = np.zeros(self._get_env_state_len())
rs_state = np.zeros(self._get_robot_server_state_len())
# Set Robot starting position
if start_pose:
assert len(start_pose)==3
else:
start_pose = self._get_start_pose()
rs_state[3:6] = start_pose
# Set target position
if target_pose:
assert len(target_pose)==3
else:
target_pose = self._get_target(start_pose)
rs_state[0:3] = target_pose
# Generate obstacles positions
self._generate_obstacles_positions()
rs_state[1021:1024] = self.sim_obstacles[0]
rs_state[1024:1027] = self.sim_obstacles[1]
rs_state[1027:1030] = self.sim_obstacles[2]
# Set initial state of the Robot Server
state_msg = robot_server_pb2.State(state = rs_state.tolist())
if not self.client.set_state_msg(state_msg):
raise RobotServerError("set_state")
# Get Robot Server state
rs_state = copy.deepcopy(np.nan_to_num(np.array(self.client.get_state_msg().state)))
# Check if the length of the Robot Server state received is correct
if not len(rs_state)== self._get_robot_server_state_len():
raise InvalidStateError("Robot Server state received has wrong length")
# Convert the initial state from Robot Server format to environment format
self.state = self._robot_server_state_to_env_state(rs_state)
# Check if the environment state is contained in the observation space
if not self.observation_space.contains(self.state):
raise InvalidStateError()
return self.state
def _set_initial_robot_server_state(self,
rs_state) -> robot_server_pb2.State:
string_params = {}
float_params = {}
state = {}
state_msg = robot_server_pb2.State(state=state,
float_params=float_params,
string_params=string_params,
state_dict=rs_state)
return state_msg
def _set_initial_robot_server_state(
self, rs_state, ee_target_pose) -> robot_server_pb2.State:
string_params = {"object_0_function": "fixed_position"}
float_params = {
"object_0_x": ee_target_pose[0],
"object_0_y": ee_target_pose[1],
"object_0_z": ee_target_pose[2]
}
state = {}
state_msg = robot_server_pb2.State(state=state,
float_params=float_params,
string_params=string_params,
state_dict=rs_state)
return state_msg
def _set_initial_robot_server_state(
self,
rs_state,
fixed_object_position=None) -> robot_server_pb2.State:
if fixed_object_position:
state_msg = super()._set_initial_robot_server_state(
rs_state=rs_state, fixed_object_position=fixed_object_position)
return state_msg
string_params = {"object_0_function": "fixed_trajectory"}
float_params = {"object_0_trajectory_id": self.ep_n % 50}
state = {}
state_msg = robot_server_pb2.State(state=state,
float_params=float_params,
string_params=string_params,
state_dict=rs_state)
return state_msg
def get_state(self):
self.get_state_event.clear()
# Get environment state
state = []
if self.target_mode == 'fixed':
target = copy.deepcopy(self.target)
elif self.target_mode == 'moving':
if self.real_robot:
(t_position, t_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, self.target_frame, rospy.Time(0))
target = t_position + [0, 0, 0]
else:
pose = self.get_model_state_pose(self.target_model_name)
# Convert orientation target from Quaternion to RPY
quaternion = PyKDL.Rotation.Quaternion(pose[3], pose[4],
pose[5], pose[6])
r, p, y = quaternion.GetRPY()
target = pose[0:3] + [r, p, y]
else:
raise ValueError
ur_state = copy.deepcopy(self.ur_state)
(position, quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, self.ee_frame, rospy.Time(0))
ee_to_base_transform = position + quaternion
if self.real_robot:
ur_collision = False
else:
ur_collision = any(self.collision_sensors.values())
self.get_state_event.set()
# Create and fill State message
msg = robot_server_pb2.State()
msg.state.extend(target)
msg.state.extend(ur_state)
msg.state.extend(ee_to_base_transform)
msg.state.extend([ur_collision])
msg.success = 1
return msg
def get_state(self):
self.get_state_event.clear()
# Get environment state
state = []
mir_pose = copy.deepcopy(self.mir_pose)
mir_twist = copy.deepcopy(self.mir_twist)
is_collision = copy.deepcopy(self.collision)
is_change_room = 0
is_change_mir_pose = 0
targets = copy.deepcopy(self.targets)
if len(targets) != 0:
targets = targets.flatten()
map_size = copy.deepcopy(self.map_size)
map_data = copy.deepcopy(self.map_data)
map_data_trueth = copy.deepcopy(self.map_data_trueth)
rgp = [
copy.deepcopy(self.room_generator_params[tag])
for tag in self.rgp_tags
]
self.get_state_event.set()
# Create and fill State message
msg = robot_server_pb2.State()
msg.state.extend([map_size])
msg.state.extend(map_data)
msg.state.extend(map_data_trueth)
msg.state.extend(mir_pose)
msg.state.extend(mir_twist)
msg.state.extend([is_collision])
msg.state.extend([is_change_room])
msg.state.extend([is_change_mir_pose])
msg.state.extend(rgp)
msg.state.extend(targets)
msg.success = 1
return msg
def _set_initial_robot_server_state(
self, rs_state, fixed_object_position) -> robot_server_pb2.State:
string_params = {}
float_params = {}
state = {}
# Set initial state of the Robot Server
if fixed_object_position:
# Object in a fixed position
string_params = {"object_0_function": "fixed_position"}
float_params = {
"object_0_x": fixed_object_position[0],
"object_0_y": fixed_object_position[1],
"object_0_z": fixed_object_position[2]
}
state_msg = robot_server_pb2.State(state=state,
float_params=float_params,
string_params=string_params,
state_dict=rs_state)
return state_msg
def _set_initial_robot_server_state(
self,
rs_state,
fixed_object_position=None) -> robot_server_pb2.State:
if fixed_object_position:
state_msg = super()._set_initial_robot_server_state(
rs_state=rs_state, fixed_object_position=fixed_object_position)
return state_msg
n_sampling_points = int(np.random.default_rng().uniform(low=8000,
high=12000))
string_params = {"object_0_function": "3d_spline_ur5_workspace"}
float_params = {"object_0_x_min": -1.0, "object_0_x_max": 1.0, "object_0_y_min": -1.0, "object_0_y_max": 1.0, \
"object_0_z_min": 0.1, "object_0_z_max": 1.0, "object_0_n_points": 10, \
"n_sampling_points": n_sampling_points}
state = {}
state_msg = robot_server_pb2.State(state=state,
float_params=float_params,
string_params=string_params,
state_dict=rs_state)
return state_msg
def reset(self,
initial_joint_positions=None,
ee_target_pose=None,
type='random'):
"""Environment reset.
Args:
initial_joint_positions (list[6] or np.array[6]): robot joint positions in radians.
ee_target_pose (list[6] or np.array[6]): [x,y,z,r,p,y] target end effector pose.
Returns:
np.array: Environment state.
"""
self.elapsed_steps = 0
self.last_action = None
self.prev_base_reward = None
# Initialize environment state
self.state = np.zeros(self._get_env_state_len())
rs_state = np.zeros(self._get_robot_server_state_len())
print(self.last_position_on_success)
# Set initial robot joint positions
if initial_joint_positions:
assert len(initial_joint_positions) == 6
ur5_initial_joint_positions = initial_joint_positions
elif (len(self.last_position_on_success) != 0) and (type
== 'continue'):
ur5_initial_joint_positions = self.last_position_on_success
else:
ur5_initial_joint_positions = self._get_initial_joint_positions()
rs_state[6:12] = self.ur5._ur_5_joint_list_to_ros_joint_list(
ur5_initial_joint_positions)
# Set target End Effector pose
if ee_target_pose:
assert len(ee_target_pose) == 6
else:
ee_target_pose = self._get_target_pose()
rs_state[0:6] = ee_target_pose
# Set initial state of the Robot Server
state_msg = robot_server_pb2.State(state=rs_state.tolist())
if not self.client.set_state_msg(state_msg):
raise RobotServerError("set_state")
# Get Robot Server state
rs_state = copy.deepcopy(
np.nan_to_num(np.array(self.client.get_state_msg().state)))
# Check if the length of the Robot Server state received is correct
if not len(rs_state) == self._get_robot_server_state_len():
raise InvalidStateError(
"Robot Server state received has wrong length")
# Convert the initial state from Robot Server format to environment format
self.state = self._robot_server_state_to_env_state(rs_state)
# Check if the environment state is contained in the observation space
if not self.observation_space.contains(self.state):
raise InvalidStateError()
# check if current position is in the range of the initial joint positions
if (len(self.last_position_on_success) == 0) or (type == 'random'):
joint_positions = self.ur5._ros_joint_list_to_ur5_joint_list(
rs_state[6:12])
tolerance = 0.1
for joint in range(len(joint_positions)):
if (joint_positions[joint] + tolerance <
self.initial_joint_positions_low[joint]) or (
joint_positions[joint] - tolerance >
self.initial_joint_positions_high[joint]):
raise InvalidStateError(
'Reset joint positions are not within defined range')
# go one empty action and check if there is a collision
action = self.state[3:3 + len(self.action_space.sample())]
_, _, done, info = self.step(action)
self.elapsed_steps = 0
if done:
raise InvalidStateError('Reset started in a collision state')
return self.state
def get_state(self):
self.get_state_event.clear()
# Get environment state
state =[]
state_dict = {}
if self.rs_mode == 'only_robot':
# Joint Positions and Joint Velocities
joint_position = copy.deepcopy(self.joint_position)
joint_velocity = copy.deepcopy(self.joint_velocity)
state += self._get_joint_ordered_value_list(joint_position)
state += self._get_joint_ordered_value_list(joint_velocity)
state_dict.update(self._get_joint_states_dict(joint_position, joint_velocity))
# ee to ref transform
ee_to_ref_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.ee_frame, rospy.Time(0))
ee_to_ref_trans_list = self._transform_to_list(ee_to_ref_trans)
state += ee_to_ref_trans_list
state_dict.update(self._get_transform_dict(ee_to_ref_trans, 'ee_to_ref'))
# Collision sensors
ur_collision = any(self.collision_sensors.values())
state += [ur_collision]
state_dict['in_collision'] = float(ur_collision)
elif self.rs_mode == '1object':
# Object 0 Pose
object_0_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.objects_frame[0], rospy.Time(0))
object_0_trans_list = self._transform_to_list(object_0_trans)
state += object_0_trans_list
state_dict.update(self._get_transform_dict(object_0_trans, 'object_0_to_ref'))
# Joint Positions and Joint Velocities
joint_position = copy.deepcopy(self.joint_position)
joint_velocity = copy.deepcopy(self.joint_velocity)
state += self._get_joint_ordered_value_list(joint_position)
state += self._get_joint_ordered_value_list(joint_velocity)
state_dict.update(self._get_joint_states_dict(joint_position, joint_velocity))
# ee to ref transform
ee_to_ref_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.ee_frame, rospy.Time(0))
ee_to_ref_trans_list = self._transform_to_list(ee_to_ref_trans)
state += ee_to_ref_trans_list
state_dict.update(self._get_transform_dict(ee_to_ref_trans, 'ee_to_ref'))
# Collision sensors
ur_collision = any(self.collision_sensors.values())
state += [ur_collision]
state_dict['in_collision'] = float(ur_collision)
elif self.rs_mode == '1moving2points':
# Object 0 Pose
object_0_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.objects_frame[0], rospy.Time(0))
object_0_trans_list = self._transform_to_list(object_0_trans)
state += object_0_trans_list
state_dict.update(self._get_transform_dict(object_0_trans, 'object_0_to_ref'))
# Joint Positions and Joint Velocities
joint_position = copy.deepcopy(self.joint_position)
joint_velocity = copy.deepcopy(self.joint_velocity)
state += self._get_joint_ordered_value_list(joint_position)
state += self._get_joint_ordered_value_list(joint_velocity)
state_dict.update(self._get_joint_states_dict(joint_position, joint_velocity))
# ee to ref transform
ee_to_ref_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.ee_frame, rospy.Time(0))
ee_to_ref_trans_list = self._transform_to_list(ee_to_ref_trans)
state += ee_to_ref_trans_list
state_dict.update(self._get_transform_dict(ee_to_ref_trans, 'ee_to_ref'))
# Collision sensors
ur_collision = any(self.collision_sensors.values())
state += [ur_collision]
state_dict['in_collision'] = float(ur_collision)
# forearm to ref transform
forearm_to_ref_trans = self.tf2_buffer.lookup_transform(self.reference_frame, 'forearm_link', rospy.Time(0))
forearm_to_ref_trans_list = self._transform_to_list(forearm_to_ref_trans)
state += forearm_to_ref_trans_list
state_dict.update(self._get_transform_dict(forearm_to_ref_trans, 'forearm_to_ref'))
else:
raise ValueError
self.get_state_event.set()
# Create and fill State message
msg = robot_server_pb2.State(state=state, state_dict=state_dict, success= True)
return msg
def set_state(self, state):
msg = self.robot_server_stub.SetState(
robot_server_pb2.State(state=state), timeout=60)
return msg.success
def GetState(self, request, context):
try:
return self.rosbridge.get_state()
except:
return robot_server_pb2.State(success=0)
def set_state(self, state):
# Old method, to be gradually replaced in all the stack
msg = self.robot_server_stub.SetState(robot_server_pb2.State(state = state), timeout = 60)
return msg.success
def get_state(self):
self.get_state_event.clear()
# Get environment state
state = []
if self.target_mode == 'fixed':
trans = self.tf2_buffer.lookup_transform(self.reference_frame,
'target', rospy.Time(0))
target = [
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z
] + [0, 0, 0]
elif self.target_mode == 'moving':
if self.real_robot:
(t_position, t_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, self.objects_frame[0], rospy.Time(0))
target = t_position + [0, 0, 0]
else:
pose = self.get_model_state_pose(self.objects_model_name[0])
# Convert orientation target from Quaternion to RPY
quaternion = PyKDL.Rotation.Quaternion(pose[3], pose[4],
pose[5], pose[6])
r, p, y = quaternion.GetRPY()
target = pose[0:3] + [r, p, y]
elif self.target_mode == '2moving':
if self.real_robot:
(t_position, t_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, self.objects_frame[0], rospy.Time(0))
target = t_position + [0, 0, 0]
(o2_position,
o2_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, self.objects_frame[1],
rospy.Time(0))
object2 = o2_position + [0, 0, 0]
else:
# Target
t_pose = self.get_model_state_pose(self.objects_model_name[0])
# Convert orientation target from Quaternion to RPY
t_quaternion = PyKDL.Rotation.Quaternion(
t_pose[3], t_pose[4], t_pose[5], t_pose[6])
t_r, t_p, t_y = t_quaternion.GetRPY()
target = t_pose[0:3] + [t_r, t_p, t_y]
# Object 02
o2_pose = self.get_model_state_pose(self.objects_model_name[1])
# Convert orientation target from Quaternion to RPY
o2_quaternion = PyKDL.Rotation.Quaternion(
o2_pose[3], o2_pose[4], o2_pose[5], o2_pose[6])
o2_r, o2_p, o2_y = o2_quaternion.GetRPY()
object2 = o2_pose[0:3] + [o2_r, o2_p, o2_y]
elif self.target_mode == '2moving2points':
(forearm_position,
forearm_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, 'forearm_link', rospy.Time(0))
forearm_to_ref_tf = forearm_position + forearm_quaternion
if self.real_robot:
(t_position, t_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, self.objects_frame[0], rospy.Time(0))
target = t_position + [0, 0, 0]
(o2_position,
o2_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, self.objects_frame[1],
rospy.Time(0))
object2 = o2_position + [0, 0, 0]
else:
# Target
t_pose = self.get_model_state_pose(self.objects_model_name[0])
# Convert orientation target from Quaternion to RPY
t_quaternion = PyKDL.Rotation.Quaternion(
t_pose[3], t_pose[4], t_pose[5], t_pose[6])
t_r, t_p, t_y = t_quaternion.GetRPY()
target = t_pose[0:3] + [t_r, t_p, t_y]
# Object 02
o2_pose = self.get_model_state_pose(self.objects_model_name[1])
# Convert orientation target from Quaternion to RPY
o2_quaternion = PyKDL.Rotation.Quaternion(
o2_pose[3], o2_pose[4], o2_pose[5], o2_pose[6])
o2_r, o2_p, o2_y = o2_quaternion.GetRPY()
object2 = o2_pose[0:3] + [o2_r, o2_p, o2_y]
elif self.target_mode == '1moving2points':
(forearm_position,
forearm_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, 'forearm_link', rospy.Time(0))
forearm_to_ref_tf = forearm_position + forearm_quaternion
if self.real_robot:
(t_position, t_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, self.objects_frame[0], rospy.Time(0))
target = t_position + [0, 0, 0]
else:
# Target
t_pose = self.get_model_state_pose(self.objects_model_name[0])
# Convert orientation target from Quaternion to RPY
t_quaternion = PyKDL.Rotation.Quaternion(
t_pose[3], t_pose[4], t_pose[5], t_pose[6])
t_r, t_p, t_y = t_quaternion.GetRPY()
target = t_pose[0:3] + [t_r, t_p, t_y]
elif self.target_mode == '1moving1point_2_2_4_voxel':
(forearm_position,
forearm_quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, 'forearm_link', rospy.Time(0))
forearm_to_ref_tf = forearm_position + forearm_quaternion
if self.real_robot:
# (t_position, t_quaternion) = self.tf_listener.lookupTransform(self.reference_frame,self.objects_frame[0],rospy.Time(0))
raise NotImplementedError(
"voxelisation of real robot not yet implemented")
else:
pose = self.get_model_state_pose(self.objects_model_name[0])
# Convert orientation target from Quaternion to RPY
quaternion = PyKDL.Rotation.Quaternion(pose[3], pose[4],
pose[5], pose[6])
r, p, y = quaternion.GetRPY()
target = pose[0:3] + [r, p, y]
voxel_occupancy = self.voxel_occupancy
else:
raise ValueError
ur_state = copy.deepcopy(self.ur_state)
(position, quaternion) = self.tf_listener.lookupTransform(
self.reference_frame, self.ee_frame, rospy.Time(0))
ee_to_base_transform = position + quaternion
if self.real_robot:
ur_collision = False
else:
ur_collision = any(self.collision_sensors.values())
self.get_state_event.set()
# Create and fill State message
msg = robot_server_pb2.State()
msg.state.extend(target)
msg.state.extend(ur_state)
msg.state.extend(ee_to_base_transform)
msg.state.extend([ur_collision])
if self.target_mode == '2moving':
msg.state.extend(object2)
if self.target_mode == '2moving2points':
msg.state.extend(object2)
msg.state.extend(forearm_to_ref_tf)
if self.target_mode == '1moving2points':
msg.state.extend(forearm_to_ref_tf)
if self.target_mode == '1moving1point_2_2_4_voxel':
msg.state.extend(forearm_to_ref_tf) # keep it in for now
msg.state.extend(voxel_occupancy)
msg.success = 1
return msg
def reset(self, initial_joint_positions=None, type='random'):
"""Environment reset.
Args:
initial_joint_positions (list[6] or np.array[6]): robot joint positions in radians.
ee_target_pose (list[6] or np.array[6]): [x,y,z,r,p,y] target end effector pose.
Returns:
np.array: Environment state.
"""
self.elapsed_steps = 0
self.last_action = None
self.prev_base_reward = None
# Initialize environment state
self.state = np.zeros(self._get_env_state_len())
rs_state = np.zeros(self._get_robot_server_state_len())
# Set initial robot joint positions
if initial_joint_positions:
assert len(initial_joint_positions) == 6
ur5_initial_joint_positions = initial_joint_positions
elif (len(self.last_position_on_success) != 0) and (type
== 'continue'):
ur5_initial_joint_positions = self.last_position_on_success
else:
ur5_initial_joint_positions = self._get_initial_joint_positions()
rs_state[6:12] = self.ur5._ur_5_joint_list_to_ros_joint_list(
ur5_initial_joint_positions)
# Set initial state of the Robot Server
# z_amplitude = 0.25
# z_frequency = 0.125
# z_offset = 0.35
z_amplitude = np.random.default_rng().uniform(low=0.09, high=0.35)
z_frequency = 0.125
z_offset = np.random.default_rng().uniform(low=0.2, high=0.6)
float_params = {
"x": 0.13,
"y": -0.30,
"z_amplitude": z_amplitude,
"z_frequency": z_frequency,
"z_offset": z_offset
}
state_msg = robot_server_pb2.State(state=rs_state.tolist(),
float_params=float_params)
if not self.client.set_state_msg(state_msg):
raise RobotServerError("set_state")
# Get Robot Server state
rs_state = copy.deepcopy(
np.nan_to_num(np.array(self.client.get_state_msg().state)))
self.prev_rs_state = copy.deepcopy(rs_state)
# Check if the length of the Robot Server state received is correct
if not len(rs_state) == self._get_robot_server_state_len():
raise InvalidStateError(
"Robot Server state received has wrong length")
# Convert the initial state from Robot Server format to environment format
self.state = self._robot_server_state_to_env_state(rs_state)
# save start position
self.start_position = self.state[3:9]
# Check if the environment state is contained in the observation space
if not self.observation_space.contains(self.state):
raise InvalidStateError()
# check if current position is in the range of the initial joint positions
if (len(self.last_position_on_success) == 0) or (type == 'random'):
joint_positions = self.ur5._ros_joint_list_to_ur5_joint_list(
rs_state[6:12])
tolerance = 0.1
for joint in range(len(joint_positions)):
if (joint_positions[joint] + tolerance <
self.initial_joint_positions_low[joint]) or (
joint_positions[joint] - tolerance >
self.initial_joint_positions_high[joint]):
raise InvalidStateError(
'Reset joint positions are not within defined range')
# go one empty action and check if there is a collision
action = self.state[4:7]
_, _, done, info = self.step(action)
self.elapsed_steps = 0
if done:
raise InvalidStateError('Reset started in a collision state')
return self.state
def reset(self, initial_joint_positions=None, ee_target_pose=None):
"""Environment reset.
Args:
initial_joint_positions (list[6] or np.array[6]): robot joint positions in radians.
ee_target_pose (list[6] or np.array[6]): [x,y,z,r,p,y] target end effector pose.
Returns:
np.array: Environment state.
"""
self.elapsed_steps = 0
self.last_action = None
self.prev_base_reward = None
# Initialize environment state
self.state = np.zeros(self._get_env_state_len())
rs_state = np.zeros(self._get_robot_server_state_len())
# Set initial robot joint positions
if initial_joint_positions:
assert len(initial_joint_positions) == 6
ur10_initial_joint_positions = initial_joint_positions
else:
ur10_initial_joint_positions = self._get_initial_joint_positions()
rs_state[6:12] = self.ur10._ur_10_joint_list_to_ros_joint_list(
ur10_initial_joint_positions)
# Set target End Effector pose
if ee_target_pose:
assert len(ee_target_pose) == 6
else:
ee_target_pose = self._get_target_pose()
rs_state[0:6] = ee_target_pose
# Set initial state of the Robot Server
state_msg = robot_server_pb2.State(state=rs_state.tolist())
if not self.client.set_state_msg(state_msg):
raise RobotServerError("set_state")
# Get Robot Server state
rs_state = copy.deepcopy(
np.nan_to_num(np.array(self.client.get_state_msg().state)))
# Check if the length of the Robot Server state received is correct
if not len(rs_state) == self._get_robot_server_state_len():
raise InvalidStateError(
"Robot Server state received has wrong length")
# Convert the initial state from Robot Server format to environment format
self.state = self._robot_server_state_to_env_state(rs_state)
# Check if the environment state is contained in the observation space
if not self.observation_space.contains(self.state):
raise InvalidStateError()
# go one empty action and check if there is a collision
action = self.state[3:3 + len(self.action_space.sample())]
_, _, done, _ = self.step(action)
self.elapsed_steps = 0
if done:
raise InvalidStateError('Reset started in a collision state')
return self.state
