def _add_uncertainty_error(self):
if self.target_pose_uncertain:
if len(self.uncertainty_std) == 2:
translation_error = np.random.normal(
scale=self.uncertainty_std[0], size=3)
translation_error[2] = 0.0
rotation_error = np.random.normal(
scale=self.uncertainty_std[1], size=3)
rotation_error = np.deg2rad(rotation_error)
error = np.concatenate([translation_error, rotation_error])
elif len(self.uncertainty_std) == 6:
if self.fixed_uncertainty_error:
error = self.uncertainty_std.copy()
error[3:] = np.deg2rad(error[3:])
else:
translation_error = np.random.normal(
scale=self.uncertainty_std[:3])
rotation_error = np.random.normal(
scale=self.uncertainty_std[3:])
rotation_error = np.deg2rad(rotation_error)
error = np.concatenate([translation_error, rotation_error])
else:
print("Warning: invalid uncertanty error",
self.uncertainty_std)
return
self.target_pos = transformations.pose_euler_to_quaternion(
self.true_target_pose, error, ee_rotation=True)
def move_back():
cpose = arm.end_effector()
deltax = np.array([0., 0., 0.04, 0., 0., 0.])
cpose = transformations.pose_euler_to_quaternion(cpose,
deltax,
ee_rotation=True)
arm.set_target_pose(pose=cpose, wait=True, t=3)
def circular_trajectory():
initial_q = [5.1818, 0.3954, -1.5714, -0.3902, 1.5448, -0.5056]
arm.set_joint_positions(initial_q, wait=True, t=2)
target_position = [0.20, -0.30, 0.80]
target_orienation = [0.01504, 0.01646, -0.01734, 0.9996]
target_orienation = [0.18603, 0.01902, -0.01464, 0.98225]
target_pose = target_position + target_orienation
deltax = np.array([0., 0.05, 0.1, 0., 0., 0.])
initial_pose = transformations.pose_euler_to_quaternion(target_pose, deltax, ee_rotation=True)
initial_pose = initial_pose[:3]
final_pose = target_position[:3]
target_q = transformations.vector_to_pyquaternion(target_orienation)
p1 = target_q.rotate(initial_pose - final_pose)
p2 = np.zeros(3)
steps = 20
duration = 10
traj = traj_utils.circunference(p1, p2, steps)
traj = np.apply_along_axis(target_q.rotate, 1, traj)
trajectory = traj + final_pose
for position in trajectory:
cmd = np.concatenate([position, target_orienation])
arm.set_target_pose(cmd, wait=True, t=(duration/steps))
def move_endeffector(wait=True):
# get current position of the end effector
cpose = arm.end_effector()
# define the desired translation/rotation
deltax = np.array([0., 0., 0.04, 0., 0., 0.])
# add translation/rotation to current position
cpose = transformations.pose_euler_to_quaternion(cpose, deltax, ee_rotation=True)
# execute desired new pose
# may fail if IK solution is not found
arm.set_target_pose(pose=cpose, wait=True, t=1.0)
def _set_action(self, action):
assert action.shape == (self.n_actions, )
if np.any(np.isnan(action)):
rospy.logerr("Invalid NAN action(s)" + str(action))
sys.exit()
assert np.all(action >= -1.0001) and np.all(action <= 1.0001)
# ensure that we don't change the action outside of this scope
actions = np.copy(action)
if self.n_actions == 3:
cmd = self.ur3e_arm.end_effector()
cmd[:3] += actions * 0.001
if self.n_actions == 4:
cpose = self.ur3e_arm.end_effector()
delta = actions * 0.001
delta = np.concatenate(
(delta[:3], [0, 0], delta[3:])) # Do not change ax and ay
# cmd = cpose + delta
cmd = transformations.pose_euler_to_quaternion(cpose, actions)
cmd = apply_workspace_contraints(cmd, self.workspace)
if self.n_actions == 6:
cpose = self.ur3e_arm.end_effector()
delta = actions * 0.001
cmd = transformations.pose_euler_to_quaternion(cpose, delta)
# print("cmd", cmd)
# cmd = apply_workspace_contraints(cmd, self.workspace)
# print("cmd2", cmd)
self.ur3e_arm.set_target_pose_flex(cmd, t=self.agent_control_dt)
self.rate.sleep()
def move_arm(wait=True):
q = [2.37191, -1.88688, -1.82035, 0.4766, 2.31206, 3.18758]
arm.set_joint_positions(position=q, wait=wait, t=0.5)
initial_ee = arm.end_effector(q)
deltas = [
[0.0, 0.03, 0.08, 0.13, 0.18], # x
[0.0, 0.03, 0.08, 0.13, 0.18], # y
[0.0, 0.03, 0.08, 0.13, 0.18], # z
]
X = 5
Y = 5
Z = 3
pose_changes = [
[[4, 2.05], [5, 3.20]],
[[4, 2.3], [5, 3.20]],
[[4, 2.1936], [5, 3.8]],
[[4, 2.05], [5, 3.8]],
[[4, 2.3], [5, 3.8]],
[[4, 2.1936], [5, 2.6]],
[[4, 2.05], [5, 2.6]],
[[4, 2.3], [5, 2.6]],
]
for i in range(Z):
x = y = 0
dx = 0
dy = -1
for _ in range(max(X, Y)**2):
if (-X / 2 < x <= X / 2) and (-Y / 2 < y <= Y / 2):
delta = np.zeros(6)
delta[0] = deltas[0][x + 1]
delta[2] = deltas[2][y + 1]
delta[1] = deltas[1][i]
cpose = transformations.pose_euler_to_quaternion(
initial_ee, delta, ee_rotation=False)
arm.set_target_pose(pose=cpose, wait=True, t=0.5)
# input("Enter to continue")
append_tf_data()
q = arm.joint_angles()
rotate_wrist(q, pose_changes)
if x == y or (x < 0 and x == -y) or (x > 0 and x == 1 - y):
dx, dy = -dy, dx
x, y = x + dx, y + dy
np.save(savefolder + "calibration_data_apriltag", tf_data)
def circular_trajectory2():
initial_q = [2.159, -1.8183, -1.9143, 0.4768, 2.5238, 4.6963]
# print(arm.end_effector().tolist())
arm.set_joint_positions(initial_q, wait=True, t=1)
target_position = [0.35951, -0.54521, 0.34393]
target_orienation = spalg.face_towards(target_position, arm.end_effector()[:3])[3:].tolist()
target_pose = target_position + target_orienation
deltax = np.array([0., 0.0, 0.2, 0., 0., 0.])
initial_pose_ = transformations.pose_euler_to_quaternion(target_pose, deltax, ee_rotation=True)
initial_pose = initial_pose_[:3]
final_pose = target_position[:3]
circle_orientation = [0,0,0,1.0]
target_q = transformations.vector_to_pyquaternion(circle_orientation)
p1 = target_q.rotate(initial_pose - final_pose)
p2 = np.zeros(3)
steps = 20
duration = 10
traj = traj_utils.circunference(p1, p2, steps, axes=[0,2,1])
traj = np.apply_along_axis(target_q.rotate, 1, traj)
trajectory = traj + final_pose
rot = spalg.face_towards(target_position, trajectory[0])[3:].tolist()
cmd = np.concatenate([trajectory[0], rot])
arm.set_target_pose(cmd, wait=True, t=(duration/steps))
for position in trajectory:
rot = spalg.face_towards(target_position, position)[3:].tolist()
cmd = np.concatenate([position, rot])
cmd_q = arm._solve_ik(cmd)
if check_ik(arm.joint_angles(), cmd_q):
arm.set_target_pose(cmd, wait=True, t=(duration/steps))
from ur_gazebo.gazebo_spawner import GazeboModels
spawner = GazeboModels('ur3_gazebo')
m1 = create_gazebo_marker(initial_pose_, "base_link", marker_id="obj")
spawner.load_models([m1])
def admittance_control(method="integration"):
print("Admitance Control", method)
e = 40
# K = np.ones(6)*100000.0
# M = np.ones(6)
K = 300.0 # 3000
M = 1.
B = e * 2 * np.sqrt(K * M)
dt = 0.002
x0 = arm.end_effector()
admittance_ctrl = AdmittanceModel(M, K, B, dt, method=method)
target = x0[:]
# target[2] = 0.03
delta_x = np.array([0., -0.0, -0.0, 0., 0., 0.])
target = transformations.pose_euler_to_quaternion(target, delta_x)
print("Impedance model", admittance_ctrl)
arm.set_impedance_control(target,
admittance_ctrl,
timeout=10.,
max_force=30,
indices=[1])
go_to(True)
def rotation():
go_to(True)
target = [
-0.09108, -0.51868, 0.47657, -0.49215, -0.48348, 0.51335, -0.5104
]
target_quaternion = Quaternion(np.roll(target[3:], 1))
current_quaternion = arm.end_effector()[3:]
from_quaternion = Quaternion(np.roll(current_quaternion, 1))
rotate = Quaternion(axis=[1, 0, 0], degrees=00.0)
to_quaternion = from_quaternion * rotate
# rotate = Quaternion(axis=[0,1,0], degrees=-50.0)
# to_quaternion *= rotate
rotate = Quaternion(axis=[0, 0, 1], degrees=50.0)
to_quaternion *= rotate
old_q = from_quaternion
for q in Quaternion.intermediates(q0=from_quaternion,
q1=target_quaternion,
n=10):
w = transformations.angular_velocity_from_quaternions(
old_q, q, 1.0 / 10.0)
old_q = q
delta = np.concatenate((np.zeros(3), w.vector))
to_pose = transformations.pose_euler_to_quaternion(arm.end_effector(),
delta,
dt=1.0 / 10.0)
arm.set_target_pose_flex(to_pose, t=1.0 / 10.0)
rospy.sleep(1.0 / 10.0)
dist = Quaternion.distance(target_quaternion,
Quaternion(np.roll(arm.end_effector()[3:], 1)))
print(dist)