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 go_pick_traj(self, obj, obj_opt, pos_pick, rot_pick, optimized,
optimizer, which_arm):
self.lock.acquire()
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[which_arm][1]
],
rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0])))
if optimizer == 'cubic':
q_pos, _ = obj_opt.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[which_arm][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))
self.lock.release()
return q_pos
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 set_pose_interpolate(self, pos, rot, tf_init=0.5, t_step=0.01):
"""
:param pos: [x,y,z]
:param rot: [qx,qy,qz,qw]
:param tf_init: initial guess to be minimized
:param t_step:
:return:
"""
assert not np.isnan(np.sum(pos))
assert not np.isnan(np.sum(rot))
# Define q0 and qf
pos0, rot0 = self.get_current_pose(wait_callback=True)
if len(pos)==0:
posf = pos0
else:
posf = pos
if len(rot)==0:
rotf = rot0
else:
rotf = rot
rot0 = U.quaternion_to_euler(rot0)
rotf = U.quaternion_to_euler(rotf)
pose0 = np.concatenate((pos0, rot0))
posef = np.concatenate((posf, rotf))
# Define trajectory
if np.allclose(pose0, posef):
return False
else:
_, traj = self.cubic_cartesian(pose0, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max,
tf_init=tf_init, t_step=0.01)
# Execute trajectory
for q in traj:
self.set_joint_direct(q)
rospy.sleep(t_step)
return True
def go_place_traj(self, obj, obj_opt, pos_place, rot_place, optimized):
self.lock.acquire()
# if optimized:
# # trajectory to pick
# q0 = obj.get_current_joint()
# qw = 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])))
# J = dvrkKinematics.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, _ = obj_opt.optimize(q0, qw, qf, dqw, max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max,
# t_step=0.01, print_out=False, visualize=False)
# else:
pos0, quat0 = obj.get_current_pose()
rot0 = U.quaternion_to_euler(quat0)
pose0 = np.concatenate((pos0, rot0))
posw = [pos_place[0], pos_place[1], self.height_ready]
rotw = [np.deg2rad(rot_place), 0.0, 0.0]
posew = np.concatenate((posw, rotw))
# 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,
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))
q_pos = q_pos1
self.lock.release()
return q_pos
def set_pose_interpolate(self, pos1=[], rot1=[], pos2=[], rot2=[], method='cubic'):
# Define q0 and qf for arm 1
pos10, rot10 = self.arm1.get_current_pose(wait_callback=True)
if len(pos1) == 0:
pos1f = pos10
else:
pos1f = pos1
if len(rot1) == 0:
rot1f = rot10
else:
rot1f = rot1
rot10 = U.quaternion_to_euler(rot10)
rot1f = U.quaternion_to_euler(rot1f)
q10 = np.concatenate((pos10, rot10))
q1f = np.concatenate((pos1f, rot1f))
# Define q0 and qf for arm 2
pos20, rot20 = self.arm2.get_current_pose(wait_callback=True)
if len(pos2) == 0:
pos2f = pos20
else:
pos2f = pos2
if len(rot2) == 0:
rot2f = rot20
else:
rot2f = rot2
rot20 = U.quaternion_to_euler(rot20)
rot2f = U.quaternion_to_euler(rot2f)
q20 = np.concatenate((pos20, rot20))
q2f = np.concatenate((pos2f, rot2f))
# Define trajectory
v_max = [0.1, 0.1, 0.1, 1.0, 1.0, 1.0]
a_max = [0.1, 0.1, 0.1, 1.0, 1.0, 1.0]
if method == 'cubic':
time1, traj1 = self.arm1.cubic(q10, q1f, v_max=v_max, a_max=a_max, t_step=0.01)
time2, traj2 = self.arm2.cubic(q20, q2f, v_max=v_max, a_max=a_max, t_step=0.01)
elif method == 'LSPB':
time1, traj1 = self.arm1.LSPB(q10, q1f, v_max=v_max, a_max=a_max, t_step=0.01)
time2, traj2 = self.arm2.LSPB(q20, q2f, v_max=v_max, a_max=a_max, t_step=0.01)
else:
raise IndexError
# Execute trajectory
len1 = len(traj1)
len2 = len(traj2)
len_max = max(len1, len2)
traj_add1 = np.repeat([traj1[-1]], len_max-len1, axis=0)
traj_add2 = np.repeat([traj2[-1]], len_max-len2, axis=0)
traj1 = np.concatenate((traj1, traj_add1), axis=0)
traj2 = np.concatenate((traj2, traj_add2), axis=0)
for i, (q1, q2) in enumerate(zip(traj1, traj2)):
super().set_pose(pos1=q1[:3], rot1=U.euler_to_quaternion(q1[3:]),
pos2=q2[:3], rot2=U.euler_to_quaternion(q2[3:]), wait_callback=False)
if self.stop_collision:
if self.detect_collision():
# when collide, temporary make the threshold higher to escape in next move.
self.curr_threshold = self.curr_threshold_high
return False
time.sleep(0.01)
# wait until goal reached
super().set_pose(pos1=traj1[-1][:3], rot1=U.euler_to_quaternion(traj1[-1][3:]),
pos2=traj2[-1][:3], rot2=U.euler_to_quaternion(traj2[-1][3:]), wait_callback=True)
self.curr_threshold = self.curr_threshold_low
return True
