def take_observation(self):
"""
Take observation from the environment and return it.
TODO: define return type
"""
# Take an observation
# done = False
obs_message = self._observation_msg
if obs_message is None:
# print("last_observations is empty")
return None
# Collect the end effector points and velocities in
# cartesian coordinates for the process_observationsstate.
# Collect the present joint angles and velocities from ROS for the state.
last_observations = self.process_observations(obs_message,
self.environment)
# # # Get Jacobians from present joint angles and KDL trees
# # # The Jacobians consist of a 6x6 matrix getting its from from
# # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw])
ee_link_jacobians = self.get_jacobians(last_observations)
if self.environment['link_names'][-1] is None:
print("End link is empty!!")
return None
else:
# print(self.environment['link_names'][-1])
trans, rot = forward_kinematics(
self.scara_chain,
self.environment['link_names'],
last_observations[:self.scara_chain.getNrOfJoints()],
base_link=self.environment['link_names'][0],
end_link=self.environment['link_names'][-1])
# #
rotation_matrix = np.eye(4)
rotation_matrix[:3, :3] = rot
rotation_matrix[:3, 3] = trans
# angle, dir, _ = rotation_from_matrix(rotation_matrix)
# #
# current_quaternion = np.array([angle]+dir.tolist())#
# # I need this calculations for the new reward function, need to send them back to the run mara or calculate them here
# current_quaternion = quaternion_from_matrix(rotation_matrix)
# tgt_quartenion = quaternion_from_matrix(self.target_orientation)
current_quaternion = quat.from_rotation_matrix(rotation_matrix)
# print("current_quaternion: ", current_quaternion)
tgt_quartenion = quat.from_rotation_matrix(self.target_orientation)
# A = np.vstack([current_quaternion, np.ones(len(current_quaternion))]).T
#quat_error = np.linalg.lstsq(A, tgt_quartenion)[0]
# this is wrong!!!! Substraction should be replaced by: quat_error = current_quaternion * tgt_quartenion.conjugate()
# quat_error = current_quaternion - tgt_quartenion
quat_error = current_quaternion * tgt_quartenion.conjugate()
# convert quat to np arrays
quat_error = quat.as_float_array(quat_error)
# RK: revisit this later, we only take one angle difference here!
angle_diff = 2 * np.arccos(np.clip(quat_error[..., 0], -1., 1.))
# print("quat error: ", quat_error)
# print("quat_error[..., 0]: ", quat_error[..., 0])
# print("quat_error: ",quat_error)
# print("angle_diff: ", angle_diff)
# print("self.realgoal: ", self.realgoal)
# print("curr quat: ", current_quaternion)
current_ee_tgt = np.ndarray.flatten(
get_ee_points(self.environment['end_effector_points'], trans,
rot).T)
ee_points = current_ee_tgt - self.realgoal #self.environment['ee_points_tgt']
ee_points_jac_trans, _ = self.get_ee_points_jacobians(
ee_link_jacobians, self.environment['end_effector_points'],
rot)
ee_velocities = self.get_ee_points_velocities(
ee_link_jacobians, self.environment['end_effector_points'],
rot, last_observations)
# Concatenate the information that defines the robot state
# vector, typically denoted asrobot_id 'x'.
state = np.r_[np.reshape(last_observations, -1),
np.reshape(ee_points, -1),
np.reshape(angle_diff, -1),
np.reshape(ee_velocities, -1), ]
# print("quat_error: ", quat_error)
# print("ee_points:", ee_points)
# print("angle_diff: ", angle_diff)
return np.r_[np.reshape(last_observations, -1),
np.reshape(ee_points, -1),
np.reshape(angle_diff, -1),
np.reshape(ee_velocities, -1), ]
def step(self, action):
state = []
reward = []
done = False
data_arm_state = None
while data_arm_state is None:
try:
data_arm_state = rospy.wait_for_message('/ariac/arm/state', JointTrajectoryControllerState, timeout=5)
except:
pass
data_arm_state2 = JointTrajectoryControllerState()
data_arm_state2.joint_names = data_arm_state.joint_names[1:]
data_arm_state2.desired.positions = data_arm_state.desired.positions[1:]
data_arm_state2.desired.velocities = data_arm_state.desired.velocities[1:]
data_arm_state2.desired.accelerations = data_arm_state.desired.accelerations[1:]
data_arm_state2.actual.positions = data_arm_state.actual.positions[1:]
data_arm_state2.actual.velocities = data_arm_state.actual.velocities[1:]
data_arm_state2.actual.accelerations = data_arm_state.actual.accelerations[1:]
data_arm_state2.error.positions = data_arm_state.error.positions[1:]
data_arm_state2.error.velocities = data_arm_state.error.velocities[1:]
data_arm_state2.error.accelerations = data_arm_state.error.accelerations[1:]
# Collect the end effector points and velocities in
# cartesian coordinates for the process_observationsstate.
# Collect the present joint angles and velocities from ROS for the state.
last_observations = self.process_observations(data_arm_state2, self.environment)
# # # Get Jacobians from present joint angles and KDL trees
# # # The Jacobians consist of a 6x6 matrix getting its from from
# # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw])
ee_link_jacobians = self.get_jacobians(last_observations)
if self.environment['link_names'][-1] is None:
print("End link is empty!!")
return None
else:
# print(self.environment['link_names'][-1])
trans, rot = forward_kinematics(self.scara_chain,
self.environment['link_names'],
last_observations[:self.scara_chain.getNrOfJoints()],
base_link=self.environment['link_names'][0],
end_link=self.environment['link_names'][-1])
# #
rotation_matrix = np.eye(4)
rotation_matrix[:3, :3] = rot
rotation_matrix[:3, 3] = trans
# angle, dir, _ = rotation_from_matrix(rotation_matrix)
# #
# current_quaternion = np.array([angle]+dir.tolist())#
# I need this calculations for the new reward function, need to send them back to the run scara or calculate them here
current_quaternion = quaternion_from_matrix(rotation_matrix)
current_ee_tgt = np.ndarray.flatten(get_ee_points(self.environment['end_effector_points'],
trans,
rot).T)
ee_points = current_ee_tgt - self.realgoal#self.environment['ee_points_tgt']
ee_points_jac_trans, _ = self.get_ee_points_jacobians(ee_link_jacobians,
self.environment['end_effector_points'],
rot)
ee_velocities = self.get_ee_points_velocities(ee_link_jacobians,
self.environment['end_effector_points'],
rot,
last_observations)
# Concatenate the information that defines the robot state
# vector, typically denoted asrobot_id 'x'.
state = np.r_[np.reshape(last_observations, -1),
np.reshape(ee_points, -1),
np.reshape(ee_velocities, -1),]
self._pub.publish(self.get_trajectory_message([0.018231524968342513,
3.2196073949389703 + np.random.rand(1)[0] - .5,
-0.6542426695478509 + np.random.rand(1)[0] - .5,
1.7401498071871018 + np.random.rand(1)[0] - .5,
3.7354939354077596 + np.random.rand(1)[0] - .5,
-1.510707301072792 + np.random.rand(1)[0] - .5,
0.00014565660628829136 + np.random.rand(1)[0] - .5]))
data_vacuum_state = None
while data_vacuum_state is None:
try:
data_vacuum_state = rospy.wait_for_message('/ariac/gripper/state', VacuumGripperState, timeout=5)
except:
pass
data_camera = None
while data_camera is None:
try:
data_camera = rospy.wait_for_message('/ariac/logical_camera_1', LogicalCameraImage, timeout=5)
except:
pass
try:
self.vacuum_gripper_control(enable=bool(state[0]))
except (rospy.ServiceException) as e:
print ("/gazebo/reset_simulation service call failed")
state = [data_arm_state, data_camera, data_vacuum_state]
return state, reward, done, {}
def take_observation(self):
"""
Take observation from the environment and return it.
TODO: define return type
"""
# Take an observation
# done = False
obs_message = self._observation_msg
if obs_message is None:
# print("last_observations is empty")
return None
# Collect the end effector points and velocities in
# cartesian coordinates for the process_observationsstate.
# Collect the present joint angles and velocities from ROS for the state.
last_observations = self.process_observations(obs_message,
self.environment)
# # # Get Jacobians from present joint angles and KDL trees
# # # The Jacobians consist of a 6x6 matrix getting its from from
# # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw])
ee_link_jacobians = self.get_jacobians(last_observations)
if self.environment['link_names'][-1] is None:
print("End link is empty!!")
return None
else:
# print(self.environment['link_names'][-1])
trans, rot = forward_kinematics(
self.scara_chain,
self.environment['link_names'],
last_observations[:self.scara_chain.getNrOfJoints()],
base_link=self.environment['link_names'][0],
end_link=self.environment['link_names'][-1])
# #
rotation_matrix = np.eye(4)
rotation_matrix[:3, :3] = rot
rotation_matrix[:3, 3] = trans
# angle, dir, _ = rotation_from_matrix(rotation_matrix)
# #
# current_quaternion = np.array([angle]+dir.tolist())#
# I need this calculations for the new reward function, need to send them back to the run scara or calculate them here
current_quaternion = quaternion_from_matrix(rotation_matrix)
current_ee_tgt = np.ndarray.flatten(
get_ee_points(self.environment['end_effector_points'], trans,
rot).T)
ee_points = current_ee_tgt - self.environment['ee_points_tgt']
ee_points_jac_trans, _ = self.get_ee_points_jacobians(
ee_link_jacobians, self.environment['end_effector_points'],
rot)
ee_velocities = self.get_ee_points_velocities(
ee_link_jacobians, self.environment['end_effector_points'],
rot, last_observations)
# Concatenate the information that defines the robot state
# vector, typically denoted asrobot_id 'x'.
state = np.r_[np.reshape(last_observations, -1),
np.reshape(ee_points, -1),
np.reshape(ee_velocities, -1), ]
return np.r_[np.reshape(last_observations, -1),
np.reshape(ee_points, -1),
np.reshape(ee_velocities, -1), ]
def take_observation(self):
"""
Take observation from the environment and return it.
TODO: define return type
"""
obs_message = self._observation_msg
if obs_message is None:
return None
# Collect the end effector points and velocities in
# cartesian coordinates for the process_observationsstate.
# Collect the present joint angles and velocities from ROS for the state.
last_observations = self.process_observations(obs_message,
self.environment)
# # # Get Jacobians from present joint angles and KDL trees
# # # The Jacobians consist of a 6x6 matrix getting its from from
# # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw])
ee_link_jacobians = self.get_jacobians(last_observations)
if self.environment['link_names'][-1] is None:
print("End link is empty!!")
return None
else:
trans, rot = forward_kinematics(
self.scara_chain,
self.environment['link_names'],
last_observations[:self.scara_chain.getNrOfJoints()],
base_link=self.environment['link_names'][0],
end_link=self.environment['link_names'][-1])
rotation_matrix = np.eye(4)
rotation_matrix[:3, :3] = rot
rotation_matrix[:3, 3] = trans
# angle, dir, _ = rotation_from_matrix(rotation_matrix)
# current_quaternion = np.array([angle]+dir.tolist())#
# # I need this calculations for the new reward function, need to send them back to the run mara or calculate them here
# current_quaternion = quaternion_from_matrix(rotation_matrix)
# tgt_quartenion = quaternion_from_matrix(self.target_orientation)
current_quaternion = tf.quaternions.mat2quat(rot) #[w, x, y ,z]
tgt_quartenion = tf.quaternions.mat2quat(self.target_orientation)
# A = np.vstack([current_quaternion, np.ones(len(current_quaternion))]).T
#quat_error = np.linalg.lstsq(A, tgt_quartenion)[0]
quat_error = current_quaternion * tgt_quartenion.conjugate()
vec, angle = tf.quaternions.quat2axangle(quat_error)
rot_vec_err = [vec[0], vec[1], vec[2], np.float64(angle)]
# convert quat to np arrays
# quat_error = np.asarray(quat_error, dtype=np.quaternion).view((np.double, 4))
# RK: revisit this later, we only take one angle difference here!
# angle_diff = 2 * np.arccos(np.clip(quat_error[..., 0], -1., 1.))
current_ee_tgt = np.ndarray.flatten(
get_ee_points(self.environment['end_effector_points'], trans,
rot).T)
ee_points = current_ee_tgt - self.realgoal #self.environment['ee_points_tgt']
ee_points_jac_trans, _ = self.get_ee_points_jacobians(
ee_link_jacobians, self.environment['end_effector_points'],
rot)
ee_velocities = self.get_ee_points_velocities(
ee_link_jacobians, self.environment['end_effector_points'],
rot, last_observations)
# Concatenate the information that defines the robot state
# vector, typically denoted asrobot_id 'x'.
state = np.r_[np.reshape(last_observations, -1),
np.reshape(ee_points, -1),
np.reshape(rot_vec_err, -1),
np.reshape(ee_velocities, -1), ]
return state
