def find_normal_orientation():
U1 = UR10Controller('10.1.1.6')
Sim = ur10_simulator()
Sim2 = ur10_simulator()
U1.read_joints()
print("UR10 joint values", U1.joints)
Sim2.set_joints(copy(U1.joints))
xyzR = copy(Sim2.joints2pose())
print("UR10 xyzR", xyzR)
Sim.set_joints(copy(U1.joints))
Sim.joints2pose()
_, R1 = Sim.get_Translation_and_Rotation_Matrix()
U1.joints[4] = copy(U1.joints[4] - 90)
Sim.set_joints(copy(U1.joints))
Sim.joints2pose()
_, R2 = Sim.get_Translation_and_Rotation_Matrix()
R1 = np.array(R1)
R2 = np.array(R2)
rotatedx = R1[:, 2]
rotatedy = R2[:, 2]
rotatedz = np.cross(rotatedx, rotatedy)
print("Normal orientation (rotated z)", rotatedz)
def move_joints_with_grasp_constraints(self, joints_vec, dist_pivot,grasp_pivot,constant_axis):
self.read_joints()
# time.sleep(0.5)
self.read_xyz()
S1 = ur10_simulator()
S1.set_joints(self.joints)
S1.tcp_vec = S1.joints2pose()
S1.set_tcp(self.xyzR)
pivot_curr,unit_vector = copy(S1.grasp_position_endaxis(dist_pivot,grasp_pivot,constant_axis))
# print(pivot_curr)
S1.set_joints(joints_vec)
S1.tcp_vec = copy(S1.joints2pose())
pivot_new,unit_vector = copy(S1.grasp_position_endaxis(dist_pivot,grasp_pivot,constant_axis))
xyz_shift = pivot_curr[0:3] - pivot_new[0:3]
new_xyzR = copy(S1.tcp_vec)
new_xyzR[0:3] = np.add(S1.tcp_vec[0:3],xyz_shift)
S1.tcp_vec = copy(new_xyzR)
# print(S1.position_along_endaxis(dist_pivot))
return new_xyzR
def do_circular_pivot_motion(self, angle, dist_pivot,axis,t,correction):
Sim = ur10_simulator()
self.read_joints()
wrist1 = copy(self.joints[5])
print("Wrist_old",wrist1)
Sim.set_joints(self.joints)
useless = copy(Sim.joints2pose())
new_xyzR = self.circular_pivot_motion(angle,dist_pivot,axis)
self.movej(new_xyzR,t)
time.sleep(t + 0.2)
self.read_joints()
newjoints = copy(self.joints)
# newjoints[5] = wrist1+correction
newjoints[5] = newjoints[5] + correction
self.movejoint(np.deg2rad(newjoints),2)
time.sleep(2.1)
self.read_joints()
print("Wrist_new",self.joints[5])
def pivot_move(U1, dist_pivot, delta_joints, t, orient, dist, Yoffset):
Sim = ur10_simulator()
U1.read_joints()
print(U1.joints)
Sim.set_joints(U1.joints)
U1.xyzR = Sim.joints2pose()
new_joints = copy(U1.joints)
new_joints = new_joints + delta_joints
new_xyzR = U1.move_joint_with_constraints(new_joints, dist_pivot)
aux = copy(new_xyzR)
aux[1] += (dist * np.cos(np.deg2rad(-orient)) -
Yoffset * np.sin(np.deg2rad(-orient)))
aux[0] += -(dist * np.sin(np.deg2rad(-orient)) +
Yoffset * np.cos(np.deg2rad(-orient)))
print(aux)
a = input("check")
U1.movej(aux, t)
# self.UR10Cont.movej(new_xyzR,t)
time.sleep(t + 0.2)
return new_xyzR
def circular_pivot_motion(self, angle, dist_pivot,axis):
self.read_joints()
# time.sleep(0.5)
self.read_xyz()
S1 = ur10_simulator()
S1.set_joints(self.joints)
S1.tcp_vec = S1.joints2pose()
S1.set_tcp(self.xyzR)
pivot_curr,unit_vector = copy(S1.position_along_endaxis(dist_pivot))
pivot_new = S1.circular_motion(dist_pivot,angle,axis)
xyz_shift = pivot_curr[0:3] - pivot_new[0:3]
new_xyzR = copy(S1.tcp_vec)
new_xyzR[0:3] = np.add(S1.tcp_vec[0:3],xyz_shift)
S1.tcp_vec = copy(new_xyzR)
return new_xyzR
U1 = UR10Controller(ip1)
# U2 = UR10Controller(ip2)
# U1.read_joints()
# print(U1.joints)
# U1.read_joints()
# Sim = ur10_simulator()
# Sim.set_joints(U1.joints)
# U1.xyzR = Sim.joints2pose()
# print(U1.xyzR)
# new_joints = copy(U1.joints)
mult = 1
Sim = ur10_simulator()
U1.do_circular_pivot_motion(-20, 180,"z",10,00)
time.sleep(3)
U1.do_circular_pivot_motion(20, 180,"z",10,0)
time.sleep(3)
# U1.do_circular_pivot_motion(40, 190,"z",3,-20)
# # time.sleep(3)
# U1.do_circular_pivot_motion(-40, 190,"z",3,20)
# # time.sleep(3)
# U1.do_circular_pivot_motion(-40, 190,"z",3,-20)
# # time.sleep(3)
# for i in range(100):
def compute_coordinates():
global ur10, iLimb
ur10.read_joints_and_xyzR()
xyzR = copy(ur10.xyzR)
joints = copy(ur10.joints)
sim = ur10_simulator()
sim.set_joints(joints)
_ = sim.joints2pose()
_, rm = sim.get_Translation_and_Rotation_Matrix()
# Calculate the direction in which the end effector is pointing
# aVlue corresponding to z-direction is ignored
direction = rm[:2, 2] # x and y direction vector only
direction /= np.linalg.norm(direction)
# Calculate unit vector direction
dir_ang = np.arctan(abs(direction[1] / direction[0]))
if direction[0] < 0:
if direction[1] < 0:
dir_ang += np.pi
else:
dir_ang = np.pi - dir_ang
else:
if direction[1] < 0:
dir_ang = 2 * np.pi - dir_ang
# Find point of contact for index finger
idx_control = STATE.CONTROL_POS[MAPPING['index']]
if idx_control > 0:
theta = 30 + 60 / 500 * idx_control
if idx_control < 210:
# Normal circular motion
rel_theta = 30
else:
rel_theta = 30 + 60 / 290 * (idx_control - 210)
# rel_theta = 30 + 60/500 * idx_control
axis = IDX_0 * np.cos(np.deg2rad(theta)) + IDX_1 * np.cos(
np.deg2rad(theta + rel_theta))
perp = IDX_0 * np.sin(np.deg2rad(theta)) + IDX_1 * np.sin(
np.deg2rad(theta + rel_theta))
axis += IDX_TO_BASE
pt_1 = [
axis * np.cos(dir_ang) - perp * np.sin(dir_ang) + xyzR[0],
axis * np.sin(dir_ang) + perp * np.cos(dir_ang) + xyzR[1], xyzR[2]
]
STATE.NUM_POINTS += 1
STATE.CONTACT_POINTS.append(pt_1)
# Find point of contact for thumb
thb_control = STATE.CONTROL_POS[MAPPING['thumb']]
if thb_control > 0:
theta = 90 * (1 - thb_control / 500)
axis = THB * np.cos(np.deg2rad(theta)) + THB_TO_BASE
perp = THB * np.sin(np.deg2rad(theta))
pt_2 = [
axis * np.cos(dir_ang) - perp * np.sin(dir_ang) + xyzR[0],
axis * np.sin(dir_ang) + perp * np.cos(dir_ang) + xyzR[1], xyzR[2]
]
STATE.NUM_POINTS += 1
STATE.CONTACT_POINTS.append(pt_2)
#--------------------------------------------------
# Collect information
STATE.XYZR.append(xyzR)
STATE.UNIT_VECTOR.append(direction)
