def main():
# initialize EGM interface instance
egm = rpi_abb_irc5.EGM()
# initialize a robot instance
abb_robot = abb_irb6640_180_255_robot()
# desired force
#Fd = 1000
# desired velocity in y
#vdy = -0.5
# feedback gain
Kp = 0.0002
Kd = 0.0008
Ki = 0.0004
# time step
delta_t = 0.004
# initial configuration in degree
init = [-91.85, 2.53, 38.20, 0.00, 49.27, -1.85]
n = 2000
# quadprog to solve for joint velocity
quadprog = qp.QuadProg(abb_robot)
# force sensor interface
try:
if (len(sys.argv) < 2):
raise Exception('IP address of ATI Net F/T sensor required')
host = sys.argv[1]
netft = rpi_ati_net_ft.NET_FT(host)
netft.set_tare_from_ft()
netft.start_streaming()
except KeyboardInterrupt:
pass
####### trapezoidal desired force in z #######
tt = np.linspace(0, 4 * np.pi, n)
desired_f = np.zeros((1, n))
vdy = np.zeros((1, n))
# form trap force and trap motion
for i in range(n):
if tt[i] >= 0 and tt[i] < np.pi:
desired_f[0, i] = 50 + 302 * tt[i]
vdy[0, i] = -0.2 * tt[i]
elif tt[i] >= np.pi and tt[i] < 3 * np.pi:
desired_f[0, i] = 50 + 302 * np.pi
vdy[0, i] = -0.2 * np.pi
else:
desired_f[0, i] = 50 + 302 * np.pi - 302 * (tt[i] - 3 * np.pi)
vdy[0, i] = -0.2 * np.pi + 0.2 * (tt[i] - 3 * np.pi)
#plt.plot(vdy[0, :])
#plt.show()
######## change here ########
#pos = 0
#acce = 0
#v_l_pre = 0
#########
# output force
force_out = np.zeros((6, n))
# pos of eef
eef_pos = np.zeros((3, n))
eef_orien = np.zeros((4, n))
# timestamp
tim = np.zeros((1, n))
# referece height of coupon that achieves desired force
z_ref = 0.89226
x_ref = 0
y_ref = -1.35626
# determine if robot has reached the initial configuration init
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print sum(np.rad2deg(state.joint_angles))- sum(init)
if np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init)) < 1e-3:
tag = False
time.sleep(1)
print '--------start force control--------'
### drain the force sensor buffer ###
for i in range(1000):
flag, ft = netft.read_ft_streaming(.1)
### drain the EGM buffer ###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
flag, ft = netft.read_ft_streaming(.1)
F0 = ft[5]
print F0
time.sleep(2)
cnt = 0
while cnt < n:
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
# forward kinematics
pose = rox.fwdkin(abb_robot, state.joint_angles)
R = pose.R
flag, ft = netft.read_ft_streaming(.1)
# map torque/force from sensor frame to base frame
T_b = np.matmul(R, ft[0:3])
F_b = np.matmul(R, ft[3:None])
F = F_b[2] # first three torques and then three forces
Fd0 = 50
# do not start motion in y until robot barely touches coupon (50 N)
if F < Fd0 - 0.1 and cnt == 0:
z = pose.p[2]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
# will shake if gain too large, here use 0.0002
v_z = Kp * 10 * (F - Fd0)
v_l = np.array([0, 0, v_z])
else:
# deadzone for Fx
if abs(F_b[0]) < 30:
F_x = 0
# deadzone for Fy
if abs(F_b[1]) < 30:
F_y = 0
v_x = Kp / 2 * (F_x - 0)
v_y = vdy[0, cnt] + Kp / 2 * (F_y - 0)
z = pose.p[2]
#print desired_f[0, cnt]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
v_z = Kp * (F - desired_f[0, cnt])
v_l = np.array([v_x, v_y, v_z])
force_out[:, cnt] = np.concatenate((T_b, F_b), axis=0)
eef_pos[:, cnt] = pose.p
quat = rox.R2q(R)
eef_orien[:, cnt] = quat
tim[0, cnt] = time.time()
cnt += 1
print F
#### change here ####
#pos = pos + v_l[2]*delta_t
#acce = (v_l[2]-v_l_pre)/delta_t
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, v_l[0], v_l[1], v_l[2]])
# emergency stop if force too large
if abs(F) > 2000:
spatial_velocity_command = np.array([0, 0, 0, 0, 0, 0])
print "force too large, stop..."
# solve for joint velocity
# Jacobian inverse
#J = rox.robotjacobian(abb_robot, state.joint_angles)
#joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# QP
joints_vel = quadprog.compute_joint_vel_cmd_qp(
state.joint_angles, spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = state.joint_angles + joints_vel * delta_t
egm.send_to_robot(q_c)
####### change here ########
#v_l_pre = v_l[2]
#if t_new - t_pre < delta_t:
# time.sleep(delta_t - t_new + t_pre)
if cnt == n:
csv_dat = np.hstack((desired_f.T, vdy.T, force_out.T, eef_pos.T,
eef_orien.T, tim.T))
np.savetxt(
'trap_force_trap_motion_020520.csv',
csv_dat,
fmt='%6.5f',
delimiter=',') #, header='desired joint, optimal input')
print "done"
def main():
#Init the joystick
pygame.init()
pygame.joystick.init()
joy = pygame.joystick.Joystick(0)
joy.init()
clock = pygame.time.Clock()
egm = rpi_abb_irc5.EGM()
OpenRAVE_obj = OpenRAVEObject()
# Initialize Robot Parameters
ex, ey, ez, n, P, q_ver, H, ttype, dq_bounds = robotParams()
# Initialize Control Parameters
# initial joint angles
""" """
#q = np.zeros((6, 1))
q = np.array([0, 0, 0, 0, np.pi / 2, 0]).reshape(6, 1)
R, pos = fwdkin(q, ttype, H, P, n)
orien = Quaternion(matrix=R)
orien = np.array([orien[0], orien[1], orien[2], orien[3]]).reshape(1, 4)
pos_v = np.zeros((3, 1))
ang_v = np.array([1, 0, 0, 0])
dq = np.zeros((int(n), 1))
# joint limits
lower_limit = np.transpose(
np.array([
-170 * np.pi / 180, -65 * np.pi / 180, -np.pi, -300 * np.pi / 180,
-120 * np.pi / 180, -2 * np.pi
]))
upper_limit = np.transpose(
np.array([
170 * np.pi / 180, 85 * np.pi / 180, 70 * np.pi / 180,
300 * np.pi / 180, 120 * np.pi / 180, 2 * np.pi
]))
# inequality constraints
h = np.zeros((15, 1))
sigma = np.zeros((13, 1))
dhdq = np.vstack((np.hstack((np.eye(6), np.zeros((6, 1)), np.zeros(
(6, 1)))), np.hstack((-np.eye(6), np.zeros((6, 1)), np.zeros(
(6, 1)))), np.zeros((1, 8))))
# velocities
w_t = np.zeros((3, 1))
v_t = np.zeros((3, 1))
# keyboard controls
# define position and angle step
inc_pos_v = 0.01 # m/s
inc_ang_v = 0.5 * np.pi / 180 # rad/s
# optimization params
er = 0.05
ep = 0.05
epsilon = 0 # legacy param for newton iters
# parameters for inequality constraints
c = 0.5
eta = 0.1
epsilon_in = 0.15
E = 0.005
Ke = 1
# create a handle of these parameters for interactive modifications
obj = ControlParams(ex, ey, ez, n, P, H,
ttype, dq_bounds, q, dq, pos, orien, pos_v,
ang_v.reshape(1, 4), w_t, v_t, epsilon, inc_pos_v,
inc_ang_v, 0, er, ep, 0)
dt = 0
counter = 0
while not obj.params['controls']['stop']:
# Loop reading the joysticks
for event in pygame.event.get():
pass
if counter != 0:
toc = timeit.default_timer()
dt = toc - tic
tic = timeit.default_timer()
counter = counter + 1
if counter != 0:
obj.params['controls']['q'] = obj.params['controls'][
'q'] + obj.params['controls']['dq'] * dt * 0.1
res, state = egm.receive_from_robot(0.01)
if res:
a = np.array(state.joint_angles)
a = a * 180 / np.pi
print "Joints: " + str(a)
egm.send_to_robot([
float(x) * 180 / np.pi for x in obj.params['controls']['q']
])
print "Target Joints: " + str([
float(x) * 180 / np.pi for x in obj.params['controls']['q']
])
pp, RR = fwdkin_alljoints(obj.params['controls']['q'], ttype,
obj.params['defi']['H'],
obj.params['defi']['P'],
obj.params['defi']['n'])
# parameters for qp
obj.params['controls']['pos'] = pp[:, -1]
orien_tmp = Quaternion(matrix=RR[:, :, -1])
obj.params['controls']['orien'] = np.array(
[orien_tmp[0], orien_tmp[1], orien_tmp[2],
orien_tmp[3]]).reshape(1, 4)
stop, Closest_Pt, Closest_Pt_env = OpenRAVE_obj.CollisionReport(
obj.params['controls']['q'][0], obj.params['controls']['q'][1],
obj.params['controls']['q'][2], obj.params['controls']['q'][3],
obj.params['controls']['q'][4], obj.params['controls']['q'][5])
# check self-collision
if (stop):
print 'robot is about to self-collide, robot stopped.'
obj.params['controls']['pos_v'] = np.array([0, 0,
0]).reshape(3, 1)
obj.params['controls']['ang_v'] = np.array([1, 0, 0,
0]).reshape(1, 4)
J2C_Joint = Joint2Collision(Closest_Pt, pp)
J_eef = getJacobian(obj.params['controls']['q'],
obj.params['defi']['ttype'],
obj.params['defi']['H'],
obj.params['defi']['P'],
obj.params['defi']['n'])
v_tmp = Closest_Pt - obj.params['controls']['pos']
v_tmp2 = (pp[:, -1] - pp[:, -3])
p_norm2 = norm(v_tmp2)
v_tmp2 = v_tmp2 / p_norm2
# determine if the closest point is on the panel
if (norm(v_tmp) < 1.5
and np.arccos(np.inner(v_tmp, v_tmp2) / norm(v_tmp)) *
180 / np.pi < 95):
print '---the closest point is on the panel---'
J2C_Joint = 6
J = getJacobian3(obj.params['controls']['q'],
obj.params['defi']['ttype'],
obj.params['defi']['H'],
obj.params['defi']['P'],
obj.params['defi']['n'], Closest_Pt,
J2C_Joint)
#J,p_0_tmp = getJacobian2(obj.params['controls']['q'], obj.params['defi']['ttype'], obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n'],Closest_Pt,J2C_Joint)
#if (J2C_Joint < 4):
else:
J, p_0_tmp = getJacobian2(obj.params['controls']['q'],
obj.params['defi']['ttype'],
obj.params['defi']['H'],
obj.params['defi']['P'],
obj.params['defi']['n'], Closest_Pt,
J2C_Joint)
#else:
# J = getJacobian3(obj.params['controls']['q'], obj.params['defi']['ttype'], obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n'], Closest_Pt,J2C_Joint)
x = joy.get_axis(0)
if (abs(x) < .2):
x = 0
else:
x = (abs(x) - .2) / .8 * cmp(x, 0)
# control of linear velocity
b1 = joy.get_button(0)
b2 = joy.get_button(1)
b3 = joy.get_button(2)
# control of angular velocity
b4 = joy.get_button(3)
b5 = joy.get_button(4)
b6 = joy.get_button(5)
# emergency stop
b9 = joy.get_button(8)
if (b9 == 1):
print 'robot stopped'
obj.params['controls']['pos_v'] = np.array([0, 0,
0]).reshape(3, 1)
obj.params['controls']['ang_v'] = np.array([1, 0, 0,
0]).reshape(1, 4)
button = [x, b1, b2, b3, b4, b5, b6]
func_xbox(button, obj)
# update joint velocities
axang = quat2axang(obj.params['controls']['ang_v'])
# desired rotational velocity
vr = axang[3] * axang[0:3]
# desired linear velocity
V_desired = obj.params['controls']['pos_v']
Q = getqp_H(obj.params['controls']['dq'], J_eef, vr.reshape(3, 1),
obj.params['controls']['pos_v'],
obj.params['opt']['er'], obj.params['opt']['ep'])
# make sure Q is symmetric
Q = 0.5 * (Q + Q.T)
f = getqp_f(obj.params['controls']['dq'], obj.params['opt']['er'],
obj.params['opt']['ep'])
Q = matrix(Q, tc='d')
f = matrix(f, tc='d')
# bounds for qp
if obj.params['opt']['upper_dq_bounds']:
bound = obj.params['defi']['dq_bounds'][1, :]
else:
bound = obj.params['defi']['dq_bounds'][0, :]
LB = np.vstack((-0.1 * bound.reshape(6, 1), 0, 0))
UB = np.vstack((0.1 * bound.reshape(6, 1), 1, 1))
LB = matrix(LB, tc='d')
UB = matrix(UB, tc='d')
# inequality constrains A and b
h[0:6] = obj.params['controls']['q'] - lower_limit.reshape(6, 1)
h[6:12] = upper_limit.reshape(6, 1) - obj.params['controls']['q']
dx = Closest_Pt_env[0] - Closest_Pt[0]
dy = Closest_Pt_env[1] - Closest_Pt[1]
dz = Closest_Pt_env[2] - Closest_Pt[2]
""" """
dist = np.sqrt(dx**2 + dy**2 + dz**2)
# derivative of dist w.r.t time
der = np.array([
dx * (dx**2 + dy**2 + dz**2)**(-0.5),
dy * (dx**2 + dy**2 + dz**2)**(-0.5),
dz * (dx**2 + dy**2 + dz**2)**(-0.5)
])
""" """
h[12] = dist - 0.05
""" """ """ """
#dhdq[12, 0:6] = np.dot(-der.reshape(1, 3), J_eef2[3:6,:])
dhdq[12, 0:6] = np.dot(-der[None, :], J[3:6, :])
sigma[0:12] = inequality_bound(h[0:12], c, eta, epsilon_in, E)
sigma[12] = inequality_bound(h[12], c, eta, epsilon_in, E)
A = -dhdq
b = -sigma
A = matrix([
matrix(A, tc='d'),
matrix(np.eye(8), tc='d'),
matrix(-np.eye(8), tc='d')
])
b = matrix([matrix(b, tc='d'), UB, -LB])
""" """
solvers.options['show_progress'] = False
sol = solvers.qp(Q, f, A, b)
dq_sln = sol['x']
b7 = joy.get_button(6)
b8 = joy.get_button(7)
# set desired eef position and orientation
if (b7 == 1 and b8 == 1):
x_des = pp[:, -1]
R_des = RR[:, :, -1]
print 'desired position set'
print 'desired position set'
# it seems that the equality constraints works better when close to the obstacle
# equality constraints for maintaining end-effector position (pure rotation)
if (b7 == 1):
print 'pure rotational movement'
A_eq_pos = np.hstack((J_eef[3:6, :], np.zeros((3, 2))))
b_eq_pos = 0.05 * Ke * (x_des - pp[:, -1])
A_eq_pos = matrix(A_eq_pos, tc='d')
b_eq_pos = matrix(b_eq_pos, (3, 1))
dq_sln = solvers.qp(Q, f, A, b, A_eq_pos, b_eq_pos)['x']
# equality constraints for maintaining end-effector orientation (pure translation)
if (b8 == 1):
print 'pure translational movement'
A_eq = np.hstack((J_eef[0:3, :], np.zeros((3, 2))))
w_skew = logm(np.dot(RR[:, :, -1], R_des.T))
w = np.array([w_skew[2, 1], w_skew[0, 2], w_skew[1, 0]])
b_eq = -0.05 * Ke * w
A_eq = matrix(A_eq, tc='d')
b_eq = matrix(b_eq, (3, 1))
dq_sln = solvers.qp(Q, f, A, b, A_eq, b_eq)['x']
if len(dq_sln) < obj.params['defi']['n']:
obj.params['controls']['dq'] = np.zeros((6, 1))
V_scaled = 0
print 'No Solution'
else:
obj.params['controls']['dq'] = dq_sln[
0:int(obj.params['defi']['n'])]
V_scaled = dq_sln[-1] * V_desired
vr_scaled = dq_sln[-2] * vr.reshape(3, 1)
V_linear = np.dot(J_eef[3:6, :], obj.params['controls']['dq'])
V_rot = np.dot(J_eef[0:3, :], obj.params['controls']['dq'])
print '------Scaled desired linear velocity------'
print V_scaled
print '------Real linear velocity by solving quadratic programming------'
print V_linear
print '------Scaled desired angular velocity------'
print vr_scaled
print '------Real angular velocity by solving quadratic programming------'
print V_rot
pygame.quit()
def main():
# initialize EGM interface instance
egm = rpi_abb_irc5.EGM()
# initialize a robot instance
abb_robot = abb_irb6640_180_255_robot()
# desired force
Fd = -1000
# desired velocity in x
vdx = 1
# spring constant
k = 50000
# position of object in z
pos_obj = 1.5
# feedback gain
Kp = 0.005
Kd = 0.2
Ki = 0.001
# time step
delta_t = 0.004 * 4
# initial configuration in degree
init = [0, 0, 0, 0, 90, 0]
# quadprog to solve for joint velocity
quadprog = qp.QuadProg(abb_robot)
# determine if robot has reached the initial configuration init
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print np.fabs(sum(state.joint_angles) - sum(b))
if np.fabs(sum(state.joint_angles) - sum(init)) < 1e-2:
tag = False
time.sleep(0.5)
print '--------start force control--------'
pose = rox.fwdkin(abb_robot, np.deg2rad(state.joint_angles))
while pose.p[0] < 2:
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
# forward kinematics to calculate current position of eef
pose = rox.fwdkin(abb_robot, np.deg2rad(state.joint_angles))
if pose.p[2] >= pos_obj:
F = 0
v_l = np.array([0, 0, -Kp * (F - Fd)])
else:
F = k * (pose.p[2] - pos_obj)
v_l = np.array([vdx, 0, -Kp * (F - Fd)])
print F
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, v_l[0], v_l[1], v_l[2]])
# solve for joint velocity
# Jacobian inverse
#J = rox.robotjacobian(abb_robot, np.deg2rad(state.joint_angles))
#joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# QP
joints_vel = quadprog.compute_joint_vel_cmd_qp(
np.deg2rad(state.joint_angles), spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = np.deg2rad(state.joint_angles) + joints_vel * delta_t
egm.send_to_robot(q_c)
egm.send_to_robot(q_c)
# joint angle at previous time step
#q_pre = q_new
#t_pre = t_new
######### change here ########
q_hat = q_hat + qhat_dot * delta_t
err = out - out_pre
err_flip2 = np.fliplr(err)
return err_flip2
# initialize EGM interface instance
egm = rpi_abb_irc5.EGM()
# initialize a robot instance
abb_robot = abb_irb6640_180_255_robot()
# desired force
Fdz = 1000
# desired velocity in y
vdy = -1
# spring constant N/m (the real one is 2.4378e7)
k = 2.4378e5
# position of object in z (m)
pos_obj = 1.05
def track(camera):
# EVENT LISTENER
octave.addpath('/home/Shuyang/Downloads/update/Velocity_Control_Constrained_ABB_OpenRAVE')
egm=rpi_abb_irc5.EGM()
# Initialize Robot Parameters
[ex,ey,ez,n,P,q_nouse,H,ttype,dq_bounds] = octave.feval('robotParams', nout=9)
# Initialize Control Parameters
# initial joint angles
q = np.zeros((6, 1))
[R,pos] = octave.feval('fwdkin', q,ttype,H,P,n, nout=2)
dq = np.zeros((int(n),1))
# velocities
w_t = np.zeros((3, 1))
v_t = np.zeros((3, 1))
# create a handle of these parameters for interactive modifications
obj = ControlParams(ex,ey,ez,n,P,H,ttype,dq_bounds,q,dq,pos,orien,pos_v,ang_v[None, :],w_t,v_t,epsilon,view_port,axes_lim,inc_pos_v,inc_ang_v,0,er,ep,0)
dt = 0
counter = 0
req_exit = []
StateVector = []
listener = threading.Thread(target=input_thread, args=(req_exit,))
listener.start()
tag_index = 0
Roc = np.array([[0.4860,-0.1868,-0.8537], [0.0583, -0.9678, 0.2450], [-0.8720, -0.1688, -0.4595]])
Poc = np.array([[3579.5], [-826.5], [1416.7]])
# Kalman Filter Parameters
Phi = np.matrix('1 0 0 1 0 0; 0 1 0 0 1 0; 0 0 1 0 0 1; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1')
R = np.eye(3,3)*1000
Q = np.eye(6)*1000
H = np.eye(3,6)
Cov = np.eye(6,6)*1500
while not req_exit:
if counter != 0:
end = time.time()
dt = end-start
start = time.time()
counter = counter + 1
if counter != 0:
J_eef = octave.feval('getJacobian', obj.params['controls']['q'], obj.params['defi']['ttype'], obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n'])
data = []
# Search if the camera module fails to find the tag for 30 consecutive frames
for x in range(0,30):
data = camera.get_all_poses()[tag_index]
if data != []:
break;
# SEARCH TERMINATED DUE TO BREAK SIGNAL
if data == []:
print "No data."
return
# Follow tag while it is in view
else:
Z = data[0]
# Kalman Filter: Initialization
if StateVector == []:
StateVector = np.concatenate( (Z,np.array([[10],[10],[10]])),axis =0 )
# Kalman Filter: Prediction
StateVector = np.matmul(Phi, StateVector)
Cov = np.matmul(np.matmul(Phi, Cov),np.transpose(Phi)) + Q
# Kalman Filter: Update
K = np.matmul(np.matmul(Cov, np.transpose(H)),np.linalg.inv(np.matmul(H,np.matmul(Cov, np.transpose(H))) +R))
StateVector = StateVector + np.matmul(K,(Z-np.matmul(H,StateVector)))
Cov = (np.eye(6,6)-K*H)*Cov
Z_est = np.array(StateVector[:3])
Poa = np.matmul(Roc, Z_est) + Poc
Poa = Coordinate_mapping(Poa)
print "Poa:", np.transpose(Poa)
obj.params['controls']['dq'] = np.linalg.inv(J_eef[3:6, :])*(Poa - obj.params['controls']['q'])
# desired joint velocity
obj.params['controls']['q'] = obj.params['controls']['q'] + obj.params['controls']['dq']*dt
res, state = egm.receive_from_robot(0.01)
if res:
a = np.array(state.joint_angles)
a = a * 180 / np.pi
print "Joints: " + str(a)
egm.send_to_robot([float(x)*180/np.pi for x in obj.params['controls']['q']])
print "Target Joints: " + str([float(x)*90/np.pi for x in obj.params['controls']['q']])