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 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 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=None, rot=None, use_ik=True, wait_callback=True):
if pos is None:
pos = []
if rot is None:
rot = []
if self.comp_hysteresis:
joint = dvrkKinematics.pose_to_joint(pos, rot)
return self.set_joint(joint=joint, wait_callback=wait_callback, use_interpolation=False)
else:
return super().set_pose(pos=pos, rot=rot, wait_callback=wait_callback)
joint_ang1.append(list(msg.position))
elif topic == topics[1]:
joint_ang2.append(list(msg.position))
print('length (joint): ', len(joint_ang1))
bag.close()
# post-process data
from FLSpegtransfer.motion.dvrkKinematics import dvrkKinematics
joint_ang1 = np.array(joint_ang1)[::8]
joint_ang2 = np.array(joint_ang2)[::8]
joint_ang1 = joint_ang1[100:]
joint_ang2 = joint_ang2[100:]
pose1 = []
pose2 = []
joint_ang1_new = []
joint_ang2_new = []
for q in joint_ang1:
pos, rot = dvrkKinematics.joint_to_pose(q)
pos[2] += 0.005
joint_ang1_new.append(dvrkKinematics.pose_to_joint(pos, rot)[0])
for q in joint_ang2:
pos, rot = dvrkKinematics.joint_to_pose(q)
pos[2] += 0.005
joint_ang2_new.append(dvrkKinematics.pose_to_joint(pos, rot)[0])
print(len(joint_ang1_new))
print(len(joint_ang2_new))
# np.save('210405_cal_random_traj_PSM1', joint_ang1_new)
np.save('210405_cal_random_traj_PSM2', joint_ang2_new)
def set_pose(self, pos, rot, wait_callback=True):
assert not np.isnan(np.sum(pos))
assert not np.isnan(np.sum(rot))
joint = np.squeeze(dvrkKinematics.pose_to_joint(pos, rot)) # SAM: sometimes need to squeeze to avoid ROS error
self.set_joint(joint, wait_callback=wait_callback)
def transfer_block(optimizer, pos_pick, rot_pick, pos_place, rot_place,
vel_limit, acc_limit, method):
offset_grasp = [-0.005, +0.002]
height_ready = -0.120
height_drop = -0.15
q0 = dvrkKinematics.pose_to_joint(
pos=[pos_pick[0], pos_pick[1], pos_pick[2] + 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], 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], 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], height_drop],
rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])))
if method == 'optimization':
J1 = dvrkKinematics.jacobian(qw1)
J2 = dvrkKinematics.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)
st = time.time()
q_pos, q_vel, q_acc, t =\
optimizer.optimize_motion(q0, qw1, dqw1, qw2, dqw2, qf,
max_vel=vel_limit, max_acc=acc_limit, t_step=0.01,
horizon=50, print_out=False, visualize=False)
cal_time = time.time() - st
elif method == 'cubic_optimizer':
J1 = dvrkKinematics.jacobian(qw1)
J2 = dvrkKinematics.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)
st = time.time()
q_pos, t = optimizer.optimize(q0,
qw1,
qw2,
qf,
dqw1,
dqw2,
max_vel=vel_limit,
max_acc=acc_limit,
t_step=0.01,
print_out=False,
visualize=False)
cal_time = time.time() - st
elif method == 'cubic_cartesian':
# [x, y, z, yaw, pitch, roll]
pose0 = [pos_pick[0], pos_pick[1], pos_pick[2] + offset_grasp[0]
] + [np.deg2rad(rot_pick), 0.0, 0.0]
posew1 = [pos_pick[0], pos_pick[1], height_ready
] + [np.deg2rad(rot_pick), 0.0, 0.0]
posew2 = [pos_place[0], pos_place[1], height_ready
] + [np.deg2rad(rot_place), 0.0, 0.0]
posef = [pos_place[0], pos_place[1], height_drop
] + [np.deg2rad(rot_place), 0.0, 0.0]
q_pos = []
st = time.time()
tf1, traj1 = dvrkArm.cubic_cartesian(pose0,
posew1,
vel_limit=vel_limit,
acc_limit=acc_limit)
tf2, traj2 = dvrkArm.cubic_cartesian(posew1,
posew2,
vel_limit=vel_limit,
acc_limit=acc_limit)
tf3, traj3 = dvrkArm.cubic_cartesian(posew2,
posef,
vel_limit=vel_limit,
acc_limit=acc_limit)
cal_time = time.time() - st
q_pos.append(traj1)
q_pos.append(traj2)
q_pos.append(traj3)
else:
raise ValueError
ref_waypoints = np.concatenate((q0, qw1, qw2, qf), axis=0).reshape(4, 6)
return q_pos, ref_waypoints, cal_time