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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 transfer_block(self, pos_pick, rot_pick, pos_place, rot_place):
# open jaw
self.arm1.set_jaw_interpolate(self.jaw_opening)
# go down toward block & close jaw
self.arm1.set_pose_interpolate(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.arm1.set_jaw_interpolate(self.jaw_closing)
# transfer block from peg to peg
q_pos = self.transfer_block_traj(pos_pick, rot_pick, pos_place, rot_place, self.use_optimization, self.optimizer)
self.move_trajectory(self.arm1, q_pos)
def drop_block(self, pos1, rot1, pos2, rot2):
# be ready to place with jaw closing
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]))))
t1.daemon = True
t2.daemon = True
t1.start()
t2.start()
t1.join()
t2.join()
# go down toward peg
t3 = threading.Thread(target=self.arm1.set_pose_interpolate,
args=([pos1[0], pos1[1], self.height_drop],
U.euler_to_quaternion(
np.deg2rad([rot1, 0.0, 0.0]))))
t4 = threading.Thread(target=self.arm2.set_pose_interpolate,
args=([pos2[0], pos2[1], self.height_drop],
U.euler_to_quaternion(
np.deg2rad([rot2, 0.0, 0.0]))))
t3.daemon = True
t4.daemon = True
t3.start()
t4.start()
t3.join()
t4.join()
# # open jaw
t5 = threading.Thread(target=self.arm1.set_jaw,
args=[[self.jaw_opening[0]]])
t6 = threading.Thread(target=self.arm2.set_jaw,
args=[[self.jaw_opening[1]]])
t5.daemon = True
t6.daemon = True
t5.start()
t6.start()
t5.join()
t6.join()
# go up
t7 = 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]))))
t8 = 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]))))
t7.daemon = True
t8.daemon = True
t7.start()
t8.start()
t7.join()
t8.join()
def move_block(self, grasping_pose1, placing_pose1, pick_number1,
place_number1, grasping_pose2, placing_pose2, pick_number2,
place_number2):
pos_pick1 = grasping_pose1[pick_number1 - 1][1:]
rot_pick1 = [
-grasping_pose1[pick_number1 - 1][0], 0, 0
] # (-) is added because the direction of pose perception and motion is oppoiste.
pos_place1 = placing_pose1[place_number1 - 1][1:]
rot_place1 = [-placing_pose1[place_number1 - 1][0], 0, 0]
pos_pick2 = grasping_pose2[pick_number2 - 1][1:]
rot_pick2 = [-grasping_pose2[pick_number2 - 1][0], 0, 0]
pos_place2 = placing_pose2[place_number2 - 1][1:]
rot_place2 = [-placing_pose2[place_number2 - 1][0], 0, 0]
# conversion to quaternion
q_pick1 = U.euler_to_quaternion(rot_pick1)
q_place1 = U.euler_to_quaternion(rot_place1)
q_pick2 = U.euler_to_quaternion(rot_pick2)
q_place2 = U.euler_to_quaternion(rot_place2)
self.__dvrk.pickup(pos_pick1, q_pick1, pos_pick2, q_pick2, 'PSM2')
self.__dvrk.place(pos_place1, q_place1, pos_place2, q_place2, 'PSM2')
def return_to_peg(self, pos_pick, rot_pick):
p0, quat0 = self.dvrk.get_current_pose()
self.dvrk.set_jaw(jaw=self.jaw_closing)
# trajectory of returning to another peg to pick-up
q0 = self.dvrk.get_current_joint(wait_callback=True)
qw1 = self.dvrk_model.pose_to_joint(
pos=[p0[0], p0[1], self.height_ready - 0.01], rot=quat0)
qw2 = 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])))
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)
return q_pos, t
def go_handover(self, pos_pick, rot_pick, pos_place, rot_place, which_arm):
if which_arm == 'PSM1':
obj = self.arm1
if self.optimizer == 'cubic':
obj_opt = self.motion_opt_1wp_PSM1
elif self.optimizer == 'qp':
obj_opt = self.motion_opt
index = 0
elif which_arm == 'PSM2':
obj = self.arm2
if self.optimizer == 'cubic':
obj_opt = self.motion_opt_1wp_PSM2
elif self.optimizer == 'qp':
obj_opt = self.motion_opt
index = 1
else:
raise ValueError
# open jaw
obj.set_jaw_interpolate([self.jaw_opening[index]])
# go down toward block & close jaw
obj.set_pose_interpolate(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])))
obj.set_jaw_interpolate([self.jaw_closing[index]])
# pick-up and handover
q_pos = self.go_handover_traj(obj_opt, pos_pick, rot_pick, pos_place,
rot_place, self.use_optimization,
self.optimizer, which_arm)
if which_arm == 'PSM1':
t1 = threading.Thread(target=self.move_trajectory,
args=(obj, q_pos))
elif which_arm == 'PSM2':
t1 = threading.Thread(target=self.move_trajectory2,
args=(obj, q_pos))
t2 = threading.Thread(target=obj.set_jaw_interpolate,
args=[[self.jaw_closing[index]]])
t1.daemon = True
t2.daemon = True
t1.start()
t2.start()
t1.join()
t2.join()
def insertion_sampling(dvrk_model, insertion_number):
# pos_low_min_z = -0.170
# pos_low_max_z = -0.135
# pos_high_min = [0.02, -0.08, -0.115]
# pos_high_max = [0.18, 0.08, -0.095]
pos_org = [0.055, 0.0, -0.113]
rot_org = [0.0, 0.0, 0.0, 1.0]
pos_min = [0.170, 0.027, -0.145]
pos_max = [0.082, -0.051, -0.106]
height_ready = [-0.115, -0.095]
height_block = [-0.155, -0.135]
traj_pos = []
traj_joint = []
for i in range(insertion_number):
pos_above = np.random.uniform(pos_min, pos_max)
pos_above[2] = np.random.uniform(height_ready[0], height_ready[1])
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])
# 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])
# 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])
# 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])
return traj_pos, traj_joint
def move_upright(self,
pos,
rot,
jaw='close',
which_arm='PSM1',
interpolate=False):
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
pos = [pos[0], pos[1], pos[2]]
if rot == []: pass
else: rot = U.euler_to_quaternion(np.deg2rad([rot, 0.0, 0.0]))
if jaw == 'close':
jaw = self.jaw_closing[index]
elif jaw == 'open':
jaw = self.jaw_opening[index]
else:
raise ValueError
if interpolate:
t1 = threading.Thread(target=obj.set_pose_interpolate,
args=(pos, rot))
else:
t1 = threading.Thread(target=obj.set_pose_interpolate,
args=(pos, rot))
t2 = threading.Thread(target=obj.set_jaw_interpolate, args=[[jaw]])
t1.daemon = True
t2.daemon = True
t1.start()
t2.start()
t1.join()
t2.join()
def transfer_block(self, pos_pick1, rot_pick1, pos_place1, rot_place1,
pos_pick2, rot_pick2, pos_place2, rot_place2):
# open jaw
t3 = threading.Thread(target=self.arm1.set_jaw_interpolate,
args=[[self.jaw_opening[0]]])
t4 = threading.Thread(target=self.arm2.set_jaw_interpolate,
args=[[self.jaw_opening[1]]])
t3.daemon = True
t4.daemon = True
t3.start()
t4.start()
t3.join()
t4.join()
# go down toward block & close jaw
t5 = threading.Thread(
target=self.arm1.set_pose_interpolate,
args=([
pos_pick1[0], pos_pick1[1], pos_pick1[2] + self.offset_grasp[0]
], U.euler_to_quaternion(np.deg2rad([rot_pick1, 0.0, 0.0]))))
t6 = threading.Thread(
target=self.arm2.set_pose_interpolate,
args=([
pos_pick2[0], pos_pick2[1], pos_pick2[2] + self.offset_grasp[0]
], U.euler_to_quaternion(np.deg2rad([rot_pick2, 0.0, 0.0]))))
t5.daemon = True
t6.daemon = True
t5.start()
t6.start()
t5.join()
t6.join()
t7 = threading.Thread(target=self.arm1.set_jaw_interpolate,
args=[[self.jaw_closing[0]]])
t8 = threading.Thread(target=self.arm2.set_jaw_interpolate,
args=[[self.jaw_closing[1]]])
t7.daemon = True
t8.daemon = True
t7.start()
t8.start()
t7.join()
t8.join()
# transfer block from peg to peg
q_pos1 = self.transfer_block_traj(pos_pick1, rot_pick1, pos_place1,
rot_place1, self.use_optimization,
self.optimizer)
q_pos2 = self.transfer_block_traj(pos_pick2, rot_pick2, pos_place2,
rot_place2, self.use_optimization,
self.optimizer)
t1 = threading.Thread(target=self.move_trajectory,
args=(self.arm1, q_pos1))
t2 = threading.Thread(target=self.move_trajectory,
args=(self.arm2, q_pos2))
t3 = threading.Thread(target=self.arm1.set_jaw_interpolate,
args=[[self.jaw_closing[0]]])
t4 = threading.Thread(target=self.arm2.set_jaw_interpolate,
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()
self.des_pos1 = []
self.des_rot1 = []
self.des_jaw1 = []
self.des_pos2 = []
self.des_rot2 = []
self.des_jaw2 = []
self.des_joint1 = []
self.des_joint2 = []
self.input_flag = [True, True, True, True, True, True, True, True]
if __name__ == "__main__":
perception = dvrkMotionBridgeP()
pos1 = [0.1, 0.1, -0.13]
rot1 = [-0.3, -0.4, -0.3]
q1 = U.euler_to_quaternion(rot1, unit='deg')
jaw1 = [5 * np.pi / 180.]
# perception.set_pose(pos1=pos1, rot1=q1, jaw1=jaw1)
perception.set_pose(jaw1=jaw1)
pos2 = [0.0, 0.0, -0.13]
rot2 = [0.0, 0.0, 0.0]
q2 = U.euler_to_quaternion(rot2, unit='deg')
jaw2 = [0.0 * np.pi / 180.]
# perception.set_pose(jaw1=jaw1)
# joint1 = [0.8, .7, 0.12, 0.0, -0.5, 0.0]
# joint2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# perception.set_joint(joint1=joint1)
# t = 0.0
# import time
# while True:
from FLSpegtransfer.motion.dvrkKinematics import dvrkKinematics import numpy as np import FLSpegtransfer.utils.CmnUtil as U model = dvrkKinematics() pos = [0.3, 1.03, -0.15] rot = [0.0, -20.0, -3.0] quat = U.euler_to_quaternion(rot, 'deg') import time st = time.time() ans = model.pose_to_joint(pos, quat, method='analytic') ans2 = model.pose_to_joint(pos, quat, method='numerical') # print (ans) # print (ans2) L1 = 0.1 L2 = 0.2 L3 = 0.02 L4 = 0.05 st = time.time() a = dvrkKinematics.fk(ans, L1, L2, L3, L4) print(time.time() - st) st = time.time() b = dvrkKinematics.fk_orientation(ans) print(time.time() - st) print(a)
10) and self.arm2._dvrkArm__goal_reached_event.wait(
10) # 10 seconds at most
else:
self.arm1._dvrkArm__set_position_goal_joint_pub.publish(msg1)
self.arm2._dvrkArm__set_position_goal_joint_pub.publish(msg2)
return True
if __name__ == "__main__":
p = dvrkDualArm()
import numpy as np
while True:
pos11 = [-0.1, -0.1, -0.14]
rot11 = np.array([0, -60, -60
]) * np.pi / 180. # ZYX Euler angle in (deg)
q11 = U.euler_to_quaternion(rot11)
jaw11 = [0]
pos12 = [-0.05, -0.05, -0.14]
rot12 = np.array([0, -60, -60
]) * np.pi / 180. # ZYX Euler angle in (deg)
q12 = U.euler_to_quaternion(rot12)
jaw12 = [0]
p.set_pose(pos1=pos11, rot1=q11, pos2=pos12, rot2=q12)
# p.set_pose(pos1=pos11, rot1=q11)
p.set_jaw(jaw1=jaw11, jaw2=jaw12)
# p.set_jaw(jaw2=jaw12)
pos21 = [0.1, 0.1, -0.14]
rot21 = np.array([0, 60, 60
]) * np.pi / 180. # ZYX Euler angle in (deg)
q21 = U.euler_to_quaternion(rot21)
pos3 = pose3[:3]
opt = PegMotionOptimizer()
pos, vel, acc, t = opt.optimize_motion(pose1,
pose2,
pose3,
max_vel=dvrkVar.v_max,
max_acc=dvrkVar.a_max,
t_step=0.01,
horizon=50,
print_out=True,
visualize=False)
from FLSpegtransfer.motion.dvrkDualArm import dvrkDualArm
import time
dvrk = dvrkDualArm()
while True:
q = U.euler_to_quaternion([pose1[3], 0.0, 0.0])
dvrk.set_pose(pos1=pose1[:3], rot1=q)
dvrk.set_jaw(jaw1=[0.5])
# time.sleep(2)
for pose in pos:
q = U.euler_to_quaternion([pose[3], 0.0, 0.0])
dvrk.set_pose(pos1=pose[:3], rot1=q, wait_callback=False)
time.sleep(0.01)
# time.sleep(2)
q = U.euler_to_quaternion([pose3[3], 0.0, 0.0])
dvrk.set_pose(pos1=pose3[:3], rot1=q)
dvrk.set_jaw(jaw1=[0.5])