def _set_init_pose(self):
"""Sets the Robot in its init pose
"""
self._log()
cpose = self.ur3e_arm.end_effector()
deltax = np.array([0., 0., 0.02, 0., 0., 0.])
cpose = transformations.pose_from_angular_veloticy(cpose,
deltax,
dt=self.reset_time,
ee_rotation=True)
self.ur3e_arm.set_target_pose(pose=cpose, wait=True, t=self.reset_time)
self._add_uncertainty_error()
if self.random_initial_pose:
self._randomize_initial_pose()
self.ur3e_arm.set_target_pose(pose=self.rand_init_cpose,
wait=True,
t=self.reset_time)
else:
qc = self.init_q
self.ur3e_arm.set_joint_positions(position=qc,
wait=True,
t=self.reset_time)
self.max_distance = spalg.translation_rotation_error(
self.ur3e_arm.end_effector(), self.target_pos) * 1000.
self.max_dist = None
def _is_done(self, observations):
if self.target_pose_uncertain_per_step:
self._add_uncertainty_error()
true_error = spalg.translation_rotation_error(
self.true_target_pose, self.ur3e_arm.end_effector())
true_error[:3] *= 1000.0
true_error[3:] = np.rad2deg(true_error[3:])
success = np.linalg.norm(true_error[:3],
axis=-1) < self.distance_threshold
self._log_message = "Final distance: " + str(np.round(
true_error, 3)) + (' success!' if success else '')
return success
def get_points_and_vels(self, joint_angles):
"""
Helper function that gets the cartesian positions
and velocities from ROS."""
if self._previous_joints is None:
self._previous_joints = self.ur3e_arm.joint_angles()
# Current position
ee_pos_now = self.ur3e_arm.end_effector(joint_angles=joint_angles)
# Last position
ee_pos_last = self.ur3e_arm.end_effector(
joint_angles=self._previous_joints)
self._previous_joints = joint_angles # update
# Use the past position to get the present velocity.
linear_velocity = (ee_pos_now[:3] -
ee_pos_last[:3]) / self.agent_control_dt
angular_velocity = transformations.angular_velocity_from_quaternions(
ee_pos_now[3:], ee_pos_last[3:], self.agent_control_dt)
velocity = np.concatenate((linear_velocity, angular_velocity))
# Shift the present poistion by the End Effector target.
# Since we subtract the target point from the current position, the optimal
# value for this will be 0.
error = spalg.translation_rotation_error(self.target_pos, ee_pos_now)
# scale error error, for more precise motion
# (numerical error with small numbers?)
error *= [1000, 1000, 1000, 1000., 1000., 1000.]
# Extract only positions of interest
if self.tgt_pose_indices is not None:
error = np.array([error[i] for i in self.tgt_pose_indices])
velocity = np.array([velocity[i] for i in self.tgt_pose_indices])
return error, velocity
def ground_true(self):
error = spalg.translation_rotation_error(self.true_target_pose, self.ur3e_arm.end_effector())*1000.0
return np.linalg.norm(error[:3], axis=-1) < self.distance_threshold