def compute_lift(self, q0, q1, H=25):
dim = 6
robot = dvrkKinematics()
qp = self._new_qp(H, a_prev=np.zeros(6))
qp.constrain_q(0, q0)
qp.constrain_v(0, np.zeros(6))
qp.constrain_q(H, q1)
x = qp.solve()
p0 = robot.fk(q0)[:3,3]
p1 = robot.fk(q1)[:3,3]
for h in range(H-1, 3, -1):
#print(f"solve h={h}, lift")
# for iter_no in range(3):
qp = self._new_qp(h, a_prev=np.zeros(6))
qp.constrain_q(0, q0)
qp.constrain_v(0, np.zeros(6))
qp.constrain_q(h, q1)
for t in range(1,h):
q_prev = x[qp._qidx(t):qp._qidx(t+1)]
p_prev = (p0*(h-t) + p1*t)/h
qp.constrain_pos(t, q_prev, p_prev, tolerance=1e-3/4, penalty=1e+3)
x_prev = None # interpolate(x, H, self.t_step, h, self.t_step, n_slack=(h-1)*4)
x_next = qp.solve(x_prev) #x if iter_no > 0 else None)
if x_next is None:
break
x,H = x_next,h
return x,H
def __init__(self, use_simulation=True, which_camera='inclined'):
self.use_simulation = use_simulation
# load transform
self.Trc = np.load(root + 'calibration_files/Trc_' + which_camera + '_PSM1.npy') # robot to camera
self.Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera + '.npy') # pegboard to camera
# import modules
if use_simulation:
pass
else:
self.zivid = ZividCapture(which_camera=which_camera)
self.zivid.start()
self.block = BlockDetection3D(self.Tpc)
self.gp = GraspingPose3D()
self.vd = VisualizeDetection()
self.pegboard = PegboardCalibration()
self.dvrk_motion = dvrkPegTransferMotionSingleArmOptimized()
self.dvrk_model = dvrkKinematics()
# action ordering
self.action_list = np.array([[1, 7], [0, 6], [3, 8], [2, 9], [5, 11], [4, 10], # left to right
[7, 1], [6, 0], [8, 3], [9, 2], [11, 5], [10, 4]]) # right to left
self.action_order = 0
# data members
self.color = []
self.point = []
self.state = ['initialize']
self.main()
def compute_drop(self, q2, q3, H=25):
robot = dvrkKinematics()
qp = self._new_qp(H, a_next=np.zeros(6))
qp.constrain_q(0, q2)
qp.constrain_q(H, q3)
qp.constrain_v(H, np.zeros(6))
x = qp.solve()
p2 = robot.fk(q2)[:3,3]
p3 = robot.fk(q3)[:3,3]
for h in range(H-1, 3, -1):
#print(f"solve h={h}, drop")
qp = self._new_qp(h, a_next=np.zeros(6))
qp.constrain_q(0, q2)
qp.constrain_q(h, q3)
qp.constrain_v(h, np.zeros(6))
for t in range(1,h):
q_prev = x[qp._qidx(t):qp._qidx(t+1)]
p_prev = (p2*(h-t) + p3*t)/h
qp.constrain_pos(t, q_prev, p_prev, tolerance=1e-3/4, penalty=1e+3)
x_next = qp.solve() # x if iter_no > 0 else None)
if x_next is None:
break
x,H = x_next,h
return x, H
def __init__(self):
super(dvrkMotionBridgeC, self).__init__()
root = "/home/hwangmh/pycharmprojects/FLSpegtransfer/"
dir = "calibration_files/model_grey_history_peg_sampled/"
# self.NNController = dvrkNNController(root+dir+"model_hysteresis.out", nb_ensemble=10)
self.NNController = dvrkNNController(root + dir + "model_grey_history_peg_sampled.out", nb_ensemble=10)
self.dvrk_model = dvrkKinematics()
def __init__(self, H, t_step, max_vel, max_acc, max_jerk=None, a_prev=None, a_next=None, minimize="a"):
assert minimize == "v" or minimize == "a" or minimize == "j"
self.robot = dvrkKinematics()
self.max_vel = max_vel # [1.0, 1.0, 1.0, 8.0, 8.0, 8.0]
self.max_acc = max_acc # [1.0, 1.0, 1.0, 8.0, 8.0, 8.0]
self.minimize = minimize
self.max_jerk = max_jerk
self.t_step = t_step
self.H = H
self.dim = 6
# H+1 q, H+1 v, H a
self._n_vars = (H+1) * self.dim * 3 - self.dim
self._v0 = (H+1) * self.dim
self._a0 = (H+1) * self.dim * 2
self.init(a_prev, a_next)
def __init__(self, use_simulation=True, which_camera='inclined'):
self.use_simulation = use_simulation
# load transform
self.Trc1 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM1.npy') # robot to camera
self.Trc2 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM2.npy') # robot to camera
self.Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera +
'.npy') # pegboard to camera
# import modules
if use_simulation:
pass
else:
self.zivid = ZividCapture(which_camera=which_camera)
self.zivid.start()
self.block = BlockDetection3D(self.Tpc)
self.gp = {
'PSM1': GraspingPose3D(which_arm='PSM1'),
'PSM2': GraspingPose3D(which_arm='PSM2')
}
self.vd = VisualizeDetection()
self.pegboard = PegboardCalibration()
self.dvrk_motion = dvrkPegTransferMotionDualArm()
self.dvrk_model = dvrkKinematics()
# action ordering
self.action_list = np.array([
[[0, 1], [7, 8]],
[[2, 3], [6, 11]],
[[4, 5], [9, 10]], # left to right
[[7, 8], [0, 1]],
[[6, 11], [2, 3]],
[[9, 10], [4, 5]]
]) # right to left
self.action_order = 0
# data members
self.color = []
self.point = []
self.state = ['initialize']
self.main()
def __init__(self, comp_hysteresis=False, stop_collision=False):
super(dvrkController, self).__init__()
# Members
self.stop_collision = stop_collision
self.comp_hysteresis = comp_hysteresis
self.curr_threshold_low = np.array([[0.1, 0.1, 0.18], [0.1, 0.1, 0.18]])
self.curr_threshold_high = self.curr_threshold_low*1.5
self._curr_threshold = self.curr_threshold_low
self.filter = LPF(fc=3, fs=100, order=2, nb_axis=3)
# Load models
if stop_collision:
self.force_est = dvrkCurrEstDNN(history=50, nb_ensemble=10, nb_axis=3) # torque prediction model
for i in range(100): # model start-up by padding dummy histories
self.detect_collision()
if comp_hysteresis:
self.hyst_comp = dvrkHystCompDNN(history=6) # hysteresis compensation model
self.dvrk_model = dvrkKinematics()
def __init__(self, model='None'):
# load model
self.model = model
if self.model=='neural net':
from FLSpegtransfer.motion.NNModel import NNModel
self.NN = NNModel(ensemble=3, horizon=4, iter=3, alpha=0.5, model='forward')
elif self.model=='linear':
from FLSpegtransfer.motion.LinearModel import LinearModel
self.LM = LinearModel(4)
# define instances
self.dvrk = dvrkMotionBridgeP()
self.dvrk_model = dvrkKinematics()
# limit joints
self.joint_limits_min = np.deg2rad([-80, -60, 0.08, -100, -90, -100])
self.joint_limits_min[2] = 0.08
self.joint_limits_max = np.deg2rad([80, 60, 0.245, 100, 90, 100])
self.joint_limits_max[2] = 0.245
def __init__(self, use_controller):
# motion library
self.dvrk = dvrkController(arm_name='/PSM1',
comp_hysteresis=True,
stop_collision=False)
self.use_controller = True
# self.dvrk = dvrkArm('/PSM1')
self.dvrk_model = dvrkKinematics()
self.motion_opt_2wp = CubicOptimizer_2wp()
self.motion_opt_1wp = CubicOptimizer_1wp()
# Motion variables
self.pos_org1 = [0.060, 0.0, -0.095]
self.rot_org1 = [0.0, 0.0, 0.0, 1.0]
self.jaw_org1 = [0.0]
self.pos_org2 = [-0.060, 0.0, -0.095]
self.rot_org2 = [0.0, 0.0, 0.0, 1.0]
self.jaw_org2 = [0.0]
self.offset_grasp = [-0.003, +0.003]
self.height_ready = -0.125
self.height_drop = -0.14
self.jaw_opening = [np.deg2rad(40)]
self.jaw_closing = [np.deg2rad(0)]
const[i * H + H - 2] = 1.0
const[i * H + H - 1] = -1.0
A_eq = np.insert(A_eq, len(A_eq), const, axis=0)
b_eq = np.insert(b_eq, len(b_eq), 0.0)
return Q, c, A_eq, b_eq
def solve_QP(Q, c, A_eq, b_eq):
A = np.block([[Q, A_eq.T], [A_eq, np.eye(np.shape(A_eq)[0])]])
b = np.concatenate((-c, b_eq))
result = np.linalg.lstsq(A, b)
return result[0][:np.shape(Q)[0]]
from FLSpegtransfer.motion.dvrkKinematics import dvrkKinematics
dvrk_model = dvrkKinematics()
# pose information of pick & place
pos1 = np.array(
[0.14635528297124054, -0.02539699498070678, -0.15560356171404516])
rot1 = np.array([0.0, 0.0, 0.2985186426947612, 0.9544038034101067])
joint1 = dvrk_model.pose_to_joint(pos1, rot1)
pos2 = np.array([0.14635528297124054, -0.02539699498070678, -0.12])
rot2 = np.array([0.0, 0.0, 0.2985186426947612, 0.9544038034101067])
joint2 = dvrk_model.pose_to_joint(pos2, rot2)
pos3 = np.array([0.11511456574520817, -0.03390017669363312, -0.12])
rot3 = np.array([0.0, 0.0, 0.0, 1.0])
joint3 = dvrk_model.pose_to_joint(pos3, rot3)
def __init__(self, use_simulation=True, which_camera='inclined'):
self.use_simulation = use_simulation
# load transform
self.Trc1 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM1.npy') # robot to camera
self.Trc2 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM2.npy') # robot to camera
self.Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera +
'.npy') # pegboard to camera
# import modules
if use_simulation:
pass
else:
self.zivid = ZividCapture(which_camera=which_camera)
self.zivid.start()
self.block = BlockDetection3D(self.Tpc)
# self.gp = [GraspingPose3D(which_arm='PSM1'), GraspingPose3D(which_arm='PSM2')]
self.gp = {
'PSM1': GraspingPose3D(which_arm='PSM1'),
'PSM2': GraspingPose3D(which_arm='PSM2')
}
self.vd = VisualizeDetection()
self.pegboard = PegboardCalibration()
self.dvrk_motion = dvrkPegTransferMotionHandOver()
self.dvrk_model = dvrkKinematics()
# action ordering (pick, place) pair
self.action_list = [
['PSM2', 1, 'PSM1', 7], # left to right
['PSM2', 0, 'PSM1', 6],
['PSM2', 3, 'PSM1', 8],
['PSM2', 2, 'PSM1', 9],
['PSM2', 5, 'PSM1', 11],
['PSM2', 4, 'PSM1', 10],
['PSM1', 7, 'PSM2', 1], # right to left
['PSM1', 6, 'PSM2', 0],
['PSM1', 8, 'PSM2', 3],
['PSM1', 9, 'PSM2', 2],
['PSM1', 11, 'PSM2', 5],
['PSM1', 10, 'PSM2', 4]
]
self.action_order = [-1, -1] # [pick action, place action]
# data members
self.color = []
self.point = []
self.state = [] # [pick state, place state]
self.pnt_handover = []
# event
self.event_handover = threading.Event()
self.event_waiting = threading.Event()
# parallelize
self.initialize()
# self.thread1 = threading.Thread(target=self.PSM1)
self.thread2 = threading.Thread(target=self.run_pick)
# self.thread1.start()
self.thread2.start()
self.run_place()
def __init__(self):
self.arm1 = dvrkArm('/PSM1')
self.arm2 = dvrkArm('/PSM2')
self.dvrk_kin = dvrkKinematics()
