def place_block(self, placing_pose1=[], placing_pose2=[]):
pp1 = placing_pose1
pp2 = placing_pose2
if pp1 != []:
# be ready to place & close jaw
pos = [pp1[0], pp1[1], self.height_ready]
rot = np.deg2rad([pp1[3], 0.0, 0.0])
rot = U.euler_to_quaternion(rot)
jaw = self.jaw_closing
self.dvrk_CM.set_pose(pos, rot, jaw)
# go down toward peg & open jaw
pos = [pp1[0], pp1[1], self.height_drop]
rot = np.deg2rad([pp1[3], 0.0, 0.0])
rot = U.euler_to_quaternion(rot)
jaw = self.jaw_opening_drop
self.dvrk_CM.set_pose(pos, rot, jaw)
# go up
pos = [pp1[0], pp1[1], self.height_ready]
rot = np.deg2rad([pp1[3], 0.0, 0.0])
rot = U.euler_to_quaternion(rot)
jaw = self.jaw_opening_drop
self.dvrk_CM.set_pose(pos, rot, jaw)
if pp2 != []:
raise NotImplementedError
def pickup_block(self, grasping_pose1=[], grasping_pose2=[]):
gp1 = grasping_pose1
gp2 = grasping_pose2
if gp1 != []:
# go above block to pickup
pos = [gp1[0], gp1[1], self.height_ready]
rot = np.deg2rad([gp1[3], 0.0, 0.0])
rot = U.euler_to_quaternion(rot)
jaw = self.jaw_closing
self.dvrk_CM.set_pose(pos, rot, jaw)
# approach block & open jaw
pos = [gp1[0], gp1[1], gp1[2] + self.height_grasp_offset_above]
rot = np.deg2rad([gp1[3], 0.0, 0.0])
rot = U.euler_to_quaternion(rot)
jaw = self.jaw_opening
self.dvrk_CM.set_pose(pos, rot, jaw)
# go down toward block & close jaw
pos = [gp1[0], gp1[1], gp1[2] + self.height_grasp_offset_below]
rot = np.deg2rad([gp1[3], 0.0, 0.0])
rot = U.euler_to_quaternion(rot)
jaw = self.jaw_closing
self.dvrk_CM.set_pose(pos, rot, jaw)
# go up with block
pos = [gp1[0], gp1[1], self.height_ready]
rot = np.deg2rad([gp1[3], 0.0, 0.0])
rot = U.euler_to_quaternion(rot)
jaw = self.jaw_closing
self.dvrk_CM.set_pose(pos, rot, jaw)
if gp2 != []:
raise NotImplementedError
def cubic_cartesian(cls, pose0, posef, vel_limit, acc_limit, tf_init=0.5, t_step=0.01):
tf = tf_init
while True:
dt = 0.01
_, pose_traj = dvrkArm.__cubic_time(pose0=pose0, posef=posef, tf=tf, t_step=dt)
# pose_traj in Cartesian, q_pos in Joint space
q_pos = dvrkKinematics.pose_to_joint(pos=pose_traj[:, :3], rot=U.euler_to_quaternion(pose_traj[:, 3:]))
q_pos_prev = np.insert(q_pos, 0, q_pos[0], axis=0)
q_pos_prev = np.delete(q_pos_prev, -1, axis=0)
q_vel = (q_pos - q_pos_prev) / dt
q_vel_prev = np.insert(q_vel, 0, q_vel[0], axis=0)
q_vel_prev = np.delete(q_vel_prev, -1, axis=0)
q_acc = (q_vel - q_vel_prev) / dt
# find maximum values
vel_max = np.max(abs(q_vel), axis=0)
acc_max = np.max(abs(q_acc), axis=0)
if np.any(vel_max > vel_limit) or np.any(acc_max > acc_limit):
if tf_init == tf:
tf_init += 0.5
tf = tf_init
else:
tf += 0.02
break
else:
if tf < 0.1:
break
tf -= 0.01
_, pose_traj = dvrkArm.__cubic_time(pose0=pose0, posef=posef, tf=tf, t_step=t_step)
q_pos = dvrkKinematics.pose_to_joint(pos=pose_traj[:, :3], rot=U.euler_to_quaternion(pose_traj[:, 3:]))
return tf, q_pos
def move_above_block(self, pos1, rot1, pos2, rot2):
# be ready to place & close jaw
t1 = threading.Thread(target=self.arm1.set_pose_interpolate,
args=([pos1[0], pos1[1], self.height_ready],
U.euler_to_quaternion(
np.deg2rad([rot1, 0.0, 0.0]))))
t2 = threading.Thread(target=self.arm2.set_pose_interpolate,
args=([pos2[0], pos2[1], self.height_ready],
U.euler_to_quaternion(
np.deg2rad([rot2, 0.0, 0.0]))))
t3 = threading.Thread(target=self.arm1.set_jaw,
args=[[self.jaw_closing[0]]])
t4 = threading.Thread(target=self.arm2.set_jaw,
args=[[self.jaw_closing[1]]])
t1.daemon = True
t2.daemon = True
t3.daemon = True
t4.daemon = True
t1.start()
t2.start()
t3.start()
t4.start()
t1.join()
t2.join()
t3.join()
t4.join()
def set_pose_interpolate(self, pos=None, rot=None, tf_init=0.5, t_step=0.01):
pos0, quat0 = dvrkKinematics.joint_to_pose(self.last_joint_cmd)
if len(pos) == 0:
posf = pos0
else:
posf = pos
if len(rot) == 0:
quatf = quat0
else:
quatf = rot
rot0 = U.quaternion_to_euler(quat0)
rotf = U.quaternion_to_euler(quatf)
pose0 = np.concatenate((pos0, rot0))
posef = np.concatenate((posf, rotf))
# Define trajectory
if np.allclose(pose0, posef):
return False
else:
_, q_pos = self.cubic_cartesian(pose0, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max,
tf_init=tf_init, t_step=t_step)
# Execute trajectory
for q in q_pos:
start = time.perf_counter()
self.set_joint_direct(q)
end = time.perf_counter()
delta = 0.01 - (end - start)
if delta > 0:
time.sleep(delta)
return True
def pick_block(self, pos, rot, which_arm='PSM1'):
if which_arm == 'PSM1' or which_arm == 0:
obj = self.arm1
index = 0
elif which_arm == 'PSM2' or which_arm == 1:
obj = self.arm2
index = 1
else:
raise ValueError
# approach block & open jaw
t1 = threading.Thread(
target=obj.set_pose_interpolate,
args=([pos[0], pos[1], pos[2] + self.offset_grasp[which_arm][1]],
U.euler_to_quaternion(np.deg2rad([rot, 0.0, 0.0]))))
t2 = threading.Thread(target=obj.set_jaw_interpolate,
args=[[self.jaw_opening[index]]])
t1.daemon = True
t2.daemon = True
t1.start()
t2.start()
t1.join()
t2.join()
# go down toward block & close jaw
obj.set_pose_interpolate(
pos=[pos[0], pos[1], pos[2] + self.offset_grasp[which_arm][0]],
rot=U.euler_to_quaternion(np.deg2rad([rot, 0.0, 0.0])))
obj.set_jaw_interpolate(jaw=[self.jaw_closing[index]])
def fit_model(self, sample):
# calculate coefficients from three points
p1 = sample[0]
p2 = sample[1]
p3 = sample[2]
p4 = sample[3]
A = np.array([[0, 0, 0, 0, 1],
[p1[0]**2+p1[1]**2+p1[2]**2, p1[0], p1[1], p1[2], 1],
[p2[0]**2+p2[1]**2+p2[2]**2, p2[0], p2[1], p2[2], 1],
[p3[0]**2+p3[1]**2+p3[2]**2, p3[0], p3[1], p3[2], 1],
[p4[0]**2+p4[1]**2+p4[2]**2, p4[0], p4[1], p4[2], 1]])
M11 = np.linalg.det(U.minor(A,0,0))
if M11 == 0:
return []
else:
M12 = np.linalg.det(U.minor(A,0,1))
M13 = np.linalg.det(U.minor(A,0,2))
M14 = np.linalg.det(U.minor(A,0,3))
M15 = np.linalg.det(U.minor(A,0,4))
xc = 0.5*M12/M11
yc = -0.5*M13/M11
zc = 0.5*M14/M11
rc = np.sqrt(xc**2 + yc**2 + zc**2 - M15/M11)
return xc, yc, zc, rc
def __LSPB(self, q0, qf, t, tf, v):
if np.allclose(q0,qf): return q0
elif np.all(v)==0: return q0
elif tf==0: return q0
elif tf<0: return []
elif t<0: return []
else:
v_limit = (np.array(qf) - np.array(q0)) / tf
if np.allclose(U.normalize(v),U.normalize(v_limit)):
if np.linalg.norm(v) < np.linalg.norm(v_limit) or np.linalg.norm(2*v_limit) < np.linalg.norm(v):
return []
else:
tb = np.linalg.norm(np.array(q0)-np.array(qf)+np.array(v)*tf) / np.linalg.norm(v)
a = np.array(v)/tb
if 0 <= t and t < tb:
q = np.array(q0) + np.array(a)/2*t*t
elif tb < t and t <= tf - tb:
q = (np.array(qf)+np.array(q0)-np.array(v)*tf)/2 + np.array(v)*t
elif tf - tb < t and t <= tf:
q = np.array(qf)-np.array(a)*tf*tf/2 + np.array(a)*tf*t - np.array(a)/2*t*t
else:
return []
return q
else:
return []
def return_to_origin(self, pos_place, rot_place):
# trajectory to place
q0 = self.dvrk_model.pose_to_joint(
pos=[pos_place[0], pos_place[1], self.height_drop],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
qw = self.dvrk_model.pose_to_joint(
pos=[pos_place[0], pos_place[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
qf = self.dvrk_model.pose_to_joint(pos=self.pos_org1,
rot=self.rot_org1)
J = self.dvrk_model.jacobian(qw)
dvw = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
dqw = np.linalg.inv(J).dot(dvw)
q_pos, t = self.motion_opt_1wp.optimize(q0,
qw,
qf,
dqw,
max_vel=dvrkVar.v_max,
max_acc=dvrkVar.a_max,
t_step=0.01,
print_out=False,
visualize=False)
for joint in q_pos[:-1]:
self.dvrk.set_joint(joint=joint, wait_callback=False)
if self.use_controller:
time.sleep(0.005)
else:
time.sleep(0.01)
self.dvrk.set_joint(joint=q_pos[-1], wait_callback=True)
self.dvrk.set_jaw(jaw=self.jaw_closing)
def go_pick(self, pos_pick, rot_pick):
# trajectory to pick
q0 = self.dvrk.get_current_joint(wait_callback=True)
qw = self.dvrk_model.pose_to_joint(
pos=[pos_pick[0], pos_pick[1], self.height_ready - 0.005],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
qf = self.dvrk_model.pose_to_joint(
pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[1]],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
J = self.dvrk_model.jacobian(qw)
dvw = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
dqw = np.linalg.inv(J).dot(dvw)
q_pos, t = self.motion_opt_1wp.optimize(q0,
qw,
qf,
dqw,
max_vel=dvrkVar.v_max,
max_acc=dvrkVar.a_max,
t_step=0.01,
print_out=False,
visualize=False)
for joint in q_pos[:-1]:
self.dvrk.set_joint(joint=joint, wait_callback=False)
if self.use_controller:
time.sleep(0.005)
else:
time.sleep(0.01)
self.dvrk.set_joint(joint=q_pos[-1], wait_callback=True)
return q_pos, t
def loop(self):
while True:
# Low pass filtering
fc = 1 # (Hz)
dt = self.__interval_ms * 0.001 # (sec)
jaw1_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[5],
self.__jaw1_curr_PSM1_prev, fc, dt)
self.__jaw1_curr_PSM1_prev = jaw1_curr_PSM1
jaw2_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[6],
self.__jaw2_curr_PSM1_prev, fc, dt)
self.__jaw2_curr_PSM1_prev = jaw2_curr_PSM1
jaw1_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[5],
self.__jaw1_curr_PSM2_prev, fc, dt)
self.__jaw1_curr_PSM2_prev = jaw1_curr_PSM2
jaw2_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[6],
self.__jaw2_curr_PSM2_prev, fc, dt)
self.__jaw2_curr_PSM2_prev = jaw2_curr_PSM2
# Current thresholding
if (self.__state_jaw_current_PSM1[0] <= 0) and (
jaw1_curr_PSM1 - jaw2_curr_PSM1 > self.threshold_PSM1):
self.grasp_detected_PSM1 = True
else:
self.grasp_detected_PSM1 = False
if (self.__state_jaw_current_PSM2[0] <= 0) and (
jaw1_curr_PSM2 - jaw2_curr_PSM2 > self.threshold_PSM2):
self.grasp_detected_PSM2 = True
else:
self.grasp_detected_PSM2 = False
self.rate.sleep()
def transfer_block_traj(self, pos_pick, rot_pick, pos_place, rot_place, optimized, optimizer):
if optimized:
# trajectory to transferring block from peg to peg
q0 = dvrkKinematics.pose_to_joint(pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[0]],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
qw1 = dvrkKinematics.pose_to_joint(pos=[pos_pick[0], pos_pick[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
qw2 = dvrkKinematics.pose_to_joint(pos=[pos_place[0], pos_place[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
qf = dvrkKinematics.pose_to_joint(pos=[pos_place[0], pos_place[1], self.height_drop],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
q0 = np.array(q0)
qw1 = np.array(qw1)
qw2 = np.array(qw2)
qf = np.array(qf)
st = time.time()
if optimizer == 'cubic':
q_pos, _ = self.motion_opt_2wp.optimize(q0, qw1, qw2, qf)
# q_pos, _ = self.motion_opt_1wp.optimize(q0, qw1, qw2)
elif optimizer == 'qp':
# x, H = self.motion_opt.optimize_lift_to_handover_motion(q0[0], qw1[0], qw2[0])
x, H = self.motion_opt.optimize_motion(q0[0], qw1[0], qw2[0], qf[0])
if x is not None:
dim = 6
q_pos = np.array([x[t * dim:(t + 1) * dim] for t in range(H + 1)])
elif optimizer == 'mtsqp':
x, H = self.motion_opt.optimize_motion(q0[0], qw1[0], qw2[0], qf[0])
if x is not None:
dim = 6
q_pos = np.array([x[t * dim:(t + 1) * dim] for t in range(H + 1)])
self.time_computing.append(time.time() - st)
else:
# trajectory to transferring block from peg to peg
pos0 = [pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[0]]
rot0 = [np.deg2rad(rot_pick), 0.0, 0.0]
pose0 = np.concatenate((pos0, rot0))
posw1 = [pos_pick[0], pos_pick[1], self.height_ready]
rotw1 = [np.deg2rad(rot_pick), 0.0, 0.0]
posew1 = np.concatenate((posw1, rotw1))
posw2 = [pos_place[0], pos_place[1], self.height_ready]
rotw2 = [np.deg2rad(rot_place), 0.0, 0.0]
posew2 = np.concatenate((posw2, rotw2))
# posf = [pos_place[0], pos_place[1], self.height_drop]
# rotf = [np.deg2rad(rot_place), 0.0, 0.0]
# posef = np.concatenate((posf, rotf))
# Define trajectory
_, q_pos1 = dvrkArm.cubic_cartesian(pose0, posew1, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max,
tf_init=0.5, t_step=0.01)
_, q_pos2 = dvrkArm.cubic_cartesian(posew1, posew2, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max,
tf_init=0.5, t_step=0.01)
# _, q_pos3 = dvrkArm.cubic_cartesian(posew2, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max,
# tf_init=0.5, t_step=0.01)
# q_pos = np.concatenate((q_pos1, q_pos2, q_pos3))
q_pos = np.concatenate((q_pos1, q_pos2))
return q_pos
def go_handover_traj(self, obj_opt, pos_pick, rot_pick, pos_place,
rot_place, optimized, optimizer, which_arm):
self.lock.acquire()
if optimized:
# trajectory to transferring block from peg to peg
q0 = dvrkKinematics.pose_to_joint(
pos=[
pos_pick[0], pos_pick[1],
pos_pick[2] + self.offset_grasp[which_arm][0]
],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
qw = dvrkKinematics.pose_to_joint(
pos=[pos_pick[0], pos_pick[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
qf = dvrkKinematics.pose_to_joint(
pos=[pos_place[0], pos_place[1], self.height_handover],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
if optimizer == 'cubic':
q_pos, _ = obj_opt.optimize(q0, qw, qf)
elif optimizer == 'qp':
x, H = self.motion_opt.optimize_lift_to_handover_motion(
q0[0], qw[0], qf[0])
if x is not None:
dim = 6
q_pos = np.array(
[x[t * dim:(t + 1) * dim] for t in range(H + 1)])
else:
# trajectory to transferring block from peg to peg
pos0 = [
pos_pick[0], pos_pick[1],
pos_pick[2] + self.offset_grasp[which_arm][0]
]
rot0 = [np.deg2rad(rot_pick), 0.0, 0.0]
pose0 = np.concatenate((pos0, rot0))
posw = [pos_pick[0], pos_pick[1], self.height_ready]
rotw = [np.deg2rad(rot_pick), 0.0, 0.0]
posew = np.concatenate((posw, rotw))
posf = [pos_place[0], pos_place[1], self.height_handover]
rotf = [np.deg2rad(rot_place), 0.0, 0.0]
posef = np.concatenate((posf, rotf))
# Define trajectory
_, q_pos1 = dvrkArm.cubic_cartesian(pose0,
posew,
vel_limit=dvrkVar.v_max,
acc_limit=dvrkVar.a_max,
tf_init=0.5,
t_step=0.01)
_, q_pos2 = dvrkArm.cubic_cartesian(posew,
posef,
vel_limit=dvrkVar.v_max,
acc_limit=dvrkVar.a_max,
tf_init=0.5,
t_step=0.01)
q_pos = np.concatenate((q_pos1, q_pos2))
self.lock.release()
return q_pos
def servoing_block(self, pos_place, rot_place):
# go down toward block & open jaw
self.arm1.set_pose_interpolate(pos=[pos_place[0], pos_place[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
pos = [pos_place[0], pos_place[1], self.height_drop]
rot = U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))
self.arm1.set_pose_interpolate(pos=pos, rot=rot)
self.arm1.set_jaw_interpolate(jaw=self.jaw_opening_drop)
def insertion_sampling(dvrk_model, insertion_number, dvrk_motion):
pos_org = [0.080, 0.0, -0.095]
rot_org = [0.0, 0.0, 0.0, 1.0]
pos_min = [0.172, 0.036, -0.150]
pos_max = [0.080, -0.051, -0.110]
height_ready = -0.120
height_block = [-0.155, -0.150]
traj_pos = []
traj_joint = []
# dvrk_motion.set_jaw(jaw1=[0.0])
for i in range(insertion_number):
# pick
pos_above = np.random.uniform(pos_min, pos_max)
pos_above[2] = height_ready
pos_grasp = [
pos_above[0], pos_above[1],
np.random.uniform(height_block[0], height_block[1])
]
# action 1
traj_pos.append(pos_org)
q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_org, rot_org)
traj_joint.append([q1, q2, q3, q4, q5, q6])
# dvrk_motion.set_joint(joint1=[q1,q2,q3,q4,q5,q6])
# action 2
traj_pos.append(pos_above)
q4_rand = np.random.uniform(-np.pi / 2, np.pi / 2)
rot_q = U.euler_to_quaternion([q4_rand, 0.0, 0.0])
q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_above, rot_q)
traj_joint.append([q1, q2, q3, q4, q5, q6])
# dvrk_motion.set_joint(joint1=[q1, q2, q3, q4, q5, q6])
# action 3
traj_pos.append(pos_grasp)
q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_grasp, rot_q)
traj_joint.append([q1, q2, q3, q4, q5, q6])
# dvrk_motion.set_joint(joint1=[q1, q2, q3, q4, q5, q6])
# action 4
traj_pos.append(pos_above)
q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_above, rot_q)
traj_joint.append([q1, q2, q3, q4, q5, q6])
# dvrk_motion.set_joint(joint1=[q1, q2, q3, q4, q5, q6])
# place
pos_above = np.random.uniform(pos_min, pos_max)
pos_above[2] = height_ready
# action 5
traj_pos.append(pos_above)
rot_q = U.euler_to_quaternion([0.0, 0.0, 0.0])
q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_above, rot_q)
traj_joint.append([q1, q2, q3, q4, q5, q6])
# dvrk_motion.set_joint(joint1=[q1, q2, q3, q4, q5, q6])
print(i, "th insertion traj. sampled")
return traj_pos, traj_joint
def run(self, stop):
while True:
# Resizing images
img1 = cv2.resize(self.__img_raw_left,
(self.__img_width, self.__img_height))
img2 = cv2.resize(self.__img_raw_right,
(self.__img_width, self.__img_height))
# Low pass filtering
fc = 1 # (Hz)
dt = 0.01 # (ms)
jaw1_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[5],
self.__jaw1_curr_PSM1_prev, fc, dt)
self.__jaw1_curr_PSM1_prev = jaw1_curr_PSM1
jaw2_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[6],
self.__jaw2_curr_PSM1_prev, fc, dt)
self.__jaw2_curr_PSM1_prev = jaw2_curr_PSM1
jaw1_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[5],
self.__jaw1_curr_PSM2_prev, fc, dt)
self.__jaw1_curr_PSM2_prev = jaw1_curr_PSM2
jaw2_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[6],
self.__jaw2_curr_PSM2_prev, fc, dt)
self.__jaw2_curr_PSM2_prev = jaw2_curr_PSM2
# Current thresholding
self.__threshold_PSM1 = 0.3 # (A)
self.__threshold_PSM2 = 0.25 # (A)
if self.__state_jaw_current_PSM1[0] <= 0:
if jaw1_curr_PSM1 - jaw2_curr_PSM1 > self.__threshold_PSM1:
cv2.circle(img1, (540, 300), 30, (0, 255, 0), -1)
else:
cv2.circle(img1, (540, 300), 30, (0, 0, 255), -1)
else:
cv2.circle(img1, (540, 300), 30, (0, 0, 255), -1)
if self.__state_jaw_current_PSM2[0] <= 0:
if jaw1_curr_PSM2 - jaw2_curr_PSM2 > self.__threshold_PSM2:
cv2.circle(img1, (100, 300), 30, (0, 255, 0), -1)
else:
cv2.circle(img1, (100, 300), 30, (0, 0, 255), -1)
else:
cv2.circle(img1, (100, 300), 30, (0, 0, 255), -1)
cv2.imshow("original1", img1)
if self.__video_recording is True:
self.out.write(img1)
self.rate.sleep()
if cv2.waitKey(1) & 0xFF == ord('q'):
if self.__video_recording is True:
print("finishing recording")
self.out.release()
cv2.destroyAllWindows()
stop()
break
def return_to_peg_traj(self, obj, pos_pick, rot_pick, optimized, optimizer):
# trajectory of returning to another peg to pick-up
if optimized:
p0, quat0 = self.arm1.get_current_pose()
q0 = self.arm1.get_current_joint()
qw1 = dvrkKinematics.pose_to_joint(pos=[p0[0], p0[1], self.height_ready], rot=quat0)
qw2 = dvrkKinematics.pose_to_joint(pos=[pos_pick[0], pos_pick[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
qf = dvrkKinematics.pose_to_joint(pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[1]],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
q0 = np.array(q0)
qw1 = np.array(qw1)
qw2 = np.array(qw2)
qf = np.array(qf)
st = time.time()
if optimizer == 'cubic':
q_pos, _ = self.motion_opt_2wp.optimize(q0, qw1, qw2, qf)
# q_pos, _ = self.motion_opt_1wp.optimize(q0, qw1, qw2, dqw1)
elif optimizer == 'qp':
x, H = self.motion_opt.optimize_motion(q0, qw1[0], qw2[0], qf[0])
if x is not None:
dim = 6
q_pos = np.array([x[t * dim:(t + 1) * dim] for t in range(H + 1)])
elif optimizer == 'mtsqp':
x, H = self.motion_opt.optimize_motion(q0, qw1[0], qw2[0], qf[0])
if x is not None:
dim = 6
q_pos = np.array([x[t * dim:(t + 1) * dim] for t in range(H + 1)])
self.time_computing.append(time.time() - st)
else:
pos0, quat0 = obj.get_current_pose()
rot0 = U.quaternion_to_euler(quat0)
pose0 = np.concatenate((pos0, rot0))
posw1 = [pos0[0], pos0[1], self.height_ready]
rotw1 = rot0
posew1 = np.concatenate((posw1, rotw1))
posw2 = [pos_pick[0], pos_pick[1], self.height_ready]
rotw2 = [np.deg2rad(rot_pick), 0.0, 0.0]
posew2 = np.concatenate((posw2, rotw2))
posf = [pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[1]]
rotf = [np.deg2rad(rot_pick), 0.0, 0.0]
posef = np.concatenate((posf, rotf))
# Define trajectory
_, q_pos1 = dvrkArm.cubic_cartesian(pose0, posew1, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max,
tf_init=0.5, t_step=0.01)
_, q_pos2 = dvrkArm.cubic_cartesian(posew1, posew2, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max,
tf_init=0.5, t_step=0.01)
_, q_pos3 = dvrkArm.cubic_cartesian(posew2, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max,
tf_init=0.5, t_step=0.01)
q_pos = np.concatenate((q_pos1, q_pos2, q_pos3))
return q_pos
def go_pick_traj(self, obj, pos_pick, rot_pick, optimized, optimizer):
if optimized:
# trajectory to pick
q0 = obj.get_current_joint()
qw = dvrkKinematics.pose_to_joint(
pos=[pos_pick[0], pos_pick[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
qf = dvrkKinematics.pose_to_joint(
pos=[
pos_pick[0], pos_pick[1],
pos_pick[2] + self.offset_grasp[1]
],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
if optimizer == 'cubic':
q_pos, _ = self.motion_opt_1wp.optimize(q0, qw, qf)
elif optimizer == 'qp':
x, H = self.motion_opt.optimize_handover_to_drop_motion(
q0, qw[0], qf[0])
if x is not None:
dim = 6
q_pos = np.array(
[x[t * dim:(t + 1) * dim] for t in range(H + 1)])
else:
pos0, quat0 = obj.get_current_pose()
rot0 = U.quaternion_to_euler(quat0)
pose0 = np.concatenate((pos0, rot0))
posw = [pos_pick[0], pos_pick[1], self.height_ready]
rotw = [np.deg2rad(rot_pick), 0.0, 0.0]
posew = np.concatenate((posw, rotw))
posf = [
pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[1]
]
rotf = [np.deg2rad(rot_pick), 0.0, 0.0]
posef = np.concatenate((posf, rotf))
# Define trajectory
_, q_pos1 = dvrkArm.cubic_cartesian(pose0,
posew,
vel_limit=dvrkVar.v_max,
acc_limit=dvrkVar.a_max,
tf_init=0.5,
t_step=0.01)
_, q_pos2 = dvrkArm.cubic_cartesian(posew,
posef,
vel_limit=dvrkVar.v_max,
acc_limit=dvrkVar.a_max,
tf_init=0.5,
t_step=0.01)
q_pos = np.concatenate((q_pos1, q_pos2))
return q_pos
def set_pose(self, pos, rot, unit='rad', wait_callback=True):
"""
:param pos_des: position array [x,y,z]
:param rot_des: rotation array [Z,Y,X euler angle]
:param unit: 'rad' or 'deg'
:param wait_callback: True or False
"""
if unit == 'deg':
rot = U.deg_to_rad(rot)
# limiting range of motion
limit_angle = 89 * np.pi / 180
if rot[1] > limit_angle: rot[1] = limit_angle
elif rot[1] < -limit_angle: rot[1] = -limit_angle
if rot[2] > limit_angle: rot[2] = limit_angle
elif rot[2] < -limit_angle: rot[2] = -limit_angle
# set in position cartesian mode
frame = self.NumpyArraytoPyKDLFrame(pos, rot)
msg = posemath.toMsg(frame)
print msg
# go to that position by goal
if wait_callback:
return self.__set_position_goal_cartesian_publish_and_wait(msg)
else:
self.goal_reached = False
self.__set_position_goal_cartesian_pub.publish(msg)
return True
def NumpyArraytoPyKDLFrame(self, pos, rot, unit='rad'):
if unit == 'deg':
rot = U.deg_to_rad(rot)
px, py, pz = pos
rz, ry, rx = np.array([-np.pi / 2, np.pi, 0]) - np.array(rot)
return PyKDL.Frame(PyKDL.Rotation.EulerZYX(rz, ry, rx),
PyKDL.Vector(px, py, pz))
def sampling(nb_pos, nb_rot):
pos_min = [0.180, 0.017, -0.150] # workspace for peg transfer
pos_max = [0.087, -0.040, -0.117]
pos_traj = []
joint_traj = []
roll_st = 0.0
for i in range(nb_pos):
pos_rand = np.random.uniform(pos_min, pos_max)
for j in range(nb_rot):
# select random roll angle
roll_ed = np.random.uniform(-90, 90)
if roll_st > roll_ed:
roll = np.arange(roll_st, roll_ed, step=-10)
else:
roll = np.arange(roll_st, roll_ed, step=10)
rot = [[r, 0.0, 0.0] for r in roll]
quat = [U.euler_to_quaternion(r, 'deg') for r in rot]
jaw = [0 * np.pi / 180.]
for q in quat:
joints = dvrk_model.pose_to_joint(pos_rand, q)
# dvrk.set_joint(joint1=joints, jaw1=jaw)
joints_deg = [
joint * 180. / np.pi if i != 2 else joint
for i, joint in enumerate(joints)
]
print(i, roll_ed, pos_rand, joints_deg)
joint_traj.append(joints)
pos_traj.append(pos_rand)
return joint_traj, pos_traj
def find_block_servo_handover(self, img_color, img_point):
# color masking & transform points to pegboard coordinate
pnt_masked = self.mask_color(img_color, img_point,
[self.lower_red, self.upper_red])
pnt_masked = self.remove_nan(pnt_masked)
pnt_transformed = U.transform(pnt_masked * 0.001, self.Tpc) * 1000
# block image masking by depth
pnt_blk = self.mask_depth(pnt_transformed, self.depth_block_servo)
# remove outliers
# pnt_blk, _ = PCLRegistration.remove_outliers(pnt_blk, nb_points=20, radius=2, visualize=False)
pcl_blk, pnt_blk = PCLRegistration.convert(pnt_blk)
# registration
if len(pnt_blk) < 100:
pose_blk = []
pnt_mask = []
else:
T = PCLRegistration.registration(pcl_blk,
self.pcl_model_servo,
downsample=2,
use_svr=False,
save_image=False,
visualize=False)
T = np.linalg.inv(T) # transform from model to block
blk_ang = self.get_pose(T)
pose_blk = [blk_ang, T]
pnt_mask = T[:3, :3].dot(
self.pnt_model_servo.T).T + T[:3,
-1].T # transformed mask points
return pose_blk, pnt_blk, pnt_mask
def move_origin(self):
# Daniel note: debug positions/orientations by changing origins and exiting after this.
print('Moving dvrk to origin, should have closed grippers.')
rot_temp1 = (np.array(self.__rot_org1) +
np.array(self.__rot_offset1)) * np.pi / 180.
rot_temp2 = (np.array(self.__rot_org2) +
np.array(self.__rot_offset2)) * np.pi / 180.
rot_org1 = U.euler_to_quaternion(rot_temp1)
rot_org2 = U.euler_to_quaternion(rot_temp2)
jaw_org1 = np.array(self.__jaw_org1) * np.pi / 180.
jaw_org2 = np.array(self.__jaw_org2) * np.pi / 180.
self.__dvrk.set_pose(self.__pos_org1, rot_org1, self.__pos_org2,
rot_org2)
print('Changing jaws to: {}, {}'.format(jaw_org1, jaw_org2))
self.__dvrk.set_jaw(jaw_org1, jaw_org2)
print('Changed jaws! Proceeding...')
def transfer_block(self, pos_pick, rot_pick, pos_place, rot_place):
# open jaw, go down toward block, and close jaw
self.dvrk.set_jaw(jaw=self.jaw_opening)
self.dvrk.set_pose(
pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[0]],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
self.dvrk.set_jaw(jaw=self.jaw_closing)
# trajectory to transferring block from peg to peg
q0 = self.dvrk_model.pose_to_joint(
pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[0]],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
qw1 = self.dvrk_model.pose_to_joint(
pos=[pos_pick[0], pos_pick[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
qw2 = self.dvrk_model.pose_to_joint(
pos=[pos_place[0], pos_place[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
qf = self.dvrk_model.pose_to_joint(
pos=[pos_place[0], pos_place[1], self.height_drop],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
J1 = self.dvrk_model.jacobian(qw1)
J2 = self.dvrk_model.jacobian(qw2)
dvw = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
dqw1 = np.linalg.inv(J1).dot(dvw)
dqw2 = np.linalg.inv(J2).dot(dvw)
q_pos, t = self.motion_opt_2wp.optimize(q0,
qw1,
qw2,
qf,
dqw1,
dqw2,
max_vel=dvrkVar.v_max,
max_acc=dvrkVar.a_max,
t_step=0.01,
print_out=False,
visualize=False)
for joint in q_pos[:-1]:
self.dvrk.set_joint(joint=joint, wait_callback=False)
if self.use_controller:
time.sleep(0.005)
else:
time.sleep(0.01)
self.dvrk.set_joint(joint=q_pos[-1], wait_callback=True)
self.dvrk.set_jaw(jaw=self.jaw_opening)
return q_pos, t
def get_pose(self, transform):
if transform == []:
pose = []
else:
R = transform[:3, :3]
euler = np.rad2deg(U.R_to_euler(R))
pose = euler[0]
return pose # block angle
def servoing_block(self, pos_place, rot_place, which_arm):
if which_arm == 'PSM1' or which_arm == 0:
index = 0
obj = self.arm1
elif which_arm == 'PSM2' or which_arm == 1:
index = 1
obj = self.arm2
# go down toward block & open jaw
obj.set_pose_interpolate(
pos=[pos_place[0], pos_place[1], self.height_ready],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
pos = [pos_place[0], pos_place[1], self.height_drop]
rot = U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))
obj.set_pose_interpolate(pos=pos, rot=rot)
obj.set_jaw_interpolate([self.jaw_opening[index]])
def get_current_jaw(self, unit='rad'):
"""
:param unit: 'rad' or 'deg'
:return: Numpy.float64
"""
jaw = np.float64(self.__position_jaw_current)
if unit == "deg":
jaw = U.rad_to_deg(self.__position_jaw_current)
return jaw
def get_poses(self, transform):
pose = []
for n, T in enumerate(transform):
if T == []:
pose.append([n, 0.0, []])
else:
R = T[:3, :3]
euler = np.rad2deg(U.R_to_euler(R))
pose.append([n, euler[2], T])
return pose # [nb_block, euler angle, T]
def servoing_block(self, pos_place1, rot_place1, pos_place2, rot_place2):
# go down toward block & open jaw
t1 = threading.Thread(
target=self.arm1.set_pose_interpolate,
args=([pos_place1[0], pos_place1[1], self.height_ready],
U.euler_to_quaternion(np.deg2rad([rot_place1, 0.0, 0.0]))))
t2 = threading.Thread(
target=self.arm2.set_pose_interpolate,
args=([pos_place2[0], pos_place2[1], self.height_ready],
U.euler_to_quaternion(np.deg2rad([rot_place2, 0.0, 0.0]))))
t1.daemon = True
t2.daemon = True
t1.start()
t2.start()
t1.join()
t2.join()
t3 = threading.Thread(
target=self.arm1.set_pose_interpolate,
args=([pos_place1[0], pos_place1[1], self.height_drop],
U.euler_to_quaternion(np.deg2rad([rot_place1, 0.0, 0.0]))))
t4 = threading.Thread(
target=self.arm2.set_pose_interpolate,
args=([pos_place2[0], pos_place2[1], self.height_drop],
U.euler_to_quaternion(np.deg2rad([rot_place2, 0.0, 0.0]))))
t3.daemon = True
t4.daemon = True
t3.start()
t4.start()
t3.join()
t4.join()
t5 = threading.Thread(target=self.arm1.set_jaw_interpolate,
args=[[self.jaw_opening[0]]])
t6 = threading.Thread(target=self.arm2.set_jaw_interpolate,
args=[[self.jaw_opening[1]]])
t5.daemon = True
t6.daemon = True
t5.start()
t6.start()
t5.join()
t6.join()
def remove_outliers(Rs, Ts):
Eulers = np.array([U.R_to_euler(R) for R in Rs])
Ts = np.array(Ts)
# for translation
clustering = DBSCAN(eps=0.3, min_samples=2).fit(Ts)
index = clustering.labels_
# for rotation
clustering2 = DBSCAN(eps=0.7, min_samples=2).fit(Eulers)
index2 = clustering2.labels_
# cross sectional index
index_cross = index + index2
Ts = Ts[index_cross == 0]
T = np.average(Ts, axis=0)
Euler = Eulers[index_cross == 0]
Euler = np.average(Euler, axis=0)
R = U.euler_to_R(Euler)
return R, T
