def take_observation(self, action):
"""
Take observation from the environment and return it.
:return: state.
"""
# # Take an observation
rclpy.spin_once(self.node)
obs_message = self._observation_msg
# Collect the present joint angles and velocities from ROS for the state.
motor_flag = True
cnt = 0
while (motor_flag):
rclpy.spin_once(self.node)
obs_message = self._observation_msg
lastObservations = ut_mara.processObservations(
obs_message, self.environment)
motor1_err = abs(lastObservations[0] - action[0])
motor2_err = abs(lastObservations[1] - action[1])
motor3_err = abs(lastObservations[2] - action[2])
if motor1_err < 0.01 and motor2_err < 0.01 and motor3_err < 0.01:
motor_flag = False
#print ('Motor observation:', lastObservations)
cnt += 1
#print ("Counter Value: ", cnt)
if cnt == 1000:
break
#print ('Motor observation:', lastObservations)
#Set observation to None after it has been read.
self._observation_msg = None
state = np.r_[np.reshape(lastObservations, -1), ]
return state
def take_observation(self):
"""
Take observation from the environment and return it.
:return: state.
"""
# # Take an observation
rclpy.spin_once(self.node)
obs_message = self._observation_msg
# Check that the observation is not prior to the action
while obs_message is None or int(
str(obs_message.header.stamp.sec) +
(str(obs_message.header.stamp.nanosec))) < self.ros_clock:
rclpy.spin_once(self.node)
obs_message = self._observation_msg
# Collect the end effector points and velocities in cartesian coordinates for the processObservations state.
# Collect the present joint angles and velocities from ROS for the state.
lastObservations = ut_mara.processObservations(obs_message,
self.environment)
#Set observation to None after it has been read.
self._observation_msg = None
# 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 = ut_mara.getJacobians(lastObservations,
self.numJoints,
self.jacSolver)
if self.environment['linkNames'][-1] is None:
print("End link is empty!!")
return None
else:
translation, rot = general_utils.forwardKinematics(
self.mara_chain,
self.environment['linkNames'],
lastObservations[:self.numJoints],
baseLink=self.environment['linkNames']
[0], # make the table as the base to get the world coordinate system
endLink=self.environment['linkNames'][-1])
current_eePos_tgt = np.ndarray.flatten(
general_utils.getEePoints(
self.environment['end_effector_points'], translation,
rot).T)
eePos_points = current_eePos_tgt - self.targetPosition
eeVelocities = ut_mara.getEePointsVelocities(
ee_link_jacobians, self.environment['end_effector_points'],
rot, lastObservations)
# Concatenate the information that defines the robot state
# vector, typically denoted asrobot_id 'x'.
state = np.r_[np.reshape(lastObservations, -1),
np.reshape(eePos_points, -1),
np.reshape(eeVelocities, -1), ]
return state