def _test_last_configuration(robot, theta, last_theta):
pose_1 = rox.fwdkin(robot, theta)
theta2 = rox.robot6_sphericalwrist_invkin(robot, pose_1, last_theta)
pose_2 = rox.fwdkin(robot, theta2[0])
if not pose_1 == pose_2:
return False
if not np.allclose(theta2[0], last_theta, atol=np.deg2rad(10)):
return False
return True
def _test_configuration(robot, theta):
pose_1 = rox.fwdkin(robot, theta)
theta2 = rox.robot6_sphericalwrist_invkin(robot, pose_1)
if not len(theta2) > 0:
return False
for theta2_i in theta2:
pose_2 = rox.fwdkin(robot, theta2_i)
if not pose_1 == pose_2:
return False
return True
def compute_ik(self, pose, current_joint = None):
if isinstance(pose, PoseStamped):
pose=rox_msg.msg2transform(pose.pose)
elif isinstance(pose, Pose):
pose=rox_msg.msg2transform(pose)
joint_targets=rox.robot6_sphericalwrist_invkin(self.rox_robot, pose)
if current_joint is None:
current_joint=self.moveit_group.get_current_joint_values()
joint_target=None
d_max=1e10
for j in joint_targets:
d=np.linalg.norm(j-current_joint)
if d < d_max:
d_max = d
joint_target=np.copy(j)
if (joint_target is None):
raise Exception("Could not find target joint values")
return joint_target
def first_half(input, num_iter):
# stop the active RAPID program
rapid.stop()
# reset the pointer to main
rapid.resetpp()
print 'first half'
print 'reset PP to main'
time.sleep(2)
# start the RAPID program
rapid.start()
# determine if robot has reached the initial configuration
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init))
if np.fabs(sum(state.joint_angles) - sum(init)) < 1e-4:
tag = False
time.sleep(1)
# out is composed of 5 velocity and 1 force in z
out = np.zeros((6, n))
force_out = np.zeros((1, n))
# pos of eef
eef_pos = np.zeros((3, n))
# orientation of eef (quaternion)
eef_orien = np.zeros((4, n))
# timestamp
tim = np.zeros((1, n))
q_hat = np.zeros((6, 1))
qhat_dot = np.zeros((6, 1))
# for observer k should be symmetric and positive definite
kl = 0.5
### drain the EGM buffer ###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
time.sleep(2)
cnt = 0
step_done = False
while cnt < n:
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
q_new = np.deg2rad(state.joint_angles)
# step-over
if not step_done:
print '--------start step-over motion--------'
# do step-over of 0.25 mm in +x in world frame
# current eef pose
pose_cur = rox.fwdkin(abb_robot, q_new)
pose_cur.p[0] = pose_cur.p[0] + num_iter * 2 * step_over
# solve for inverse kinematics and pick the one that is closest to current configuration
sol = rox.robot6_sphericalwrist_invkin(abb_robot, pose_cur, q_new)
try:
tar = sol[0] # raise exception if no solution
except:
tar = q_new
# move to the new position after step-over
egm.send_to_robot(tar)
step_done = True
q_new = tar
### drain the EGM buffer, or it will use the obsolete EGM feedback###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
print '--------step-over motion done--------'
time.sleep(2)
# forward kinematics to calculate current position of eef
pose = rox.fwdkin(abb_robot, q_new)
R = pose.R
Fd0 = 50
if pose.p[2] >= pos_obj:
F = 0
v_z = Kp * 30 * (F - Fd0) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, 0, 0, v_z])
else:
F = -k * (pose.p[2] - pos_obj)
if F < Fdz - 0.5 and cnt == 0:
v_z = Kp * (F - Fdz) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, 0, 0, v_z])
else:
# formalize entire twist
# nominal input composed of F and v
spatial_velocity_command = input[:, cnt]
v_z = Kp * (F - spatial_velocity_command[5])
# nominal input only contains v
spatial_velocity_command[5] = v_z
# approximation of joint velocity
#q_new = np.deg2rad(state.joint_angles)
######### change here, use observer instead of approximation to calculate q_dot ########
if cnt == 0:
q_hat = q_new
qhat_dot = joints_vel + kl * (q_new - q_hat)
#q_dot = (q_new - q_pre)/delta_t
J = rox.robotjacobian(abb_robot, q_new)
# estimate velocity
v_est = J.dot(qhat_dot)
# formalize the nominal output composed of F and v
out[:, cnt] = np.append(v_est[0:5], F)
force_out[:, cnt] = F
eef_pos[:, cnt] = pose.p
R = pose.R
quat = rox.R2q(R)
eef_orien[:, cnt] = quat
tim[0, cnt] = time.time()
cnt = cnt + 1
print F
# solve for joint velocity
# Jacobian inverse
J = rox.robotjacobian(abb_robot, q_new)
joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# QP
#joints_vel = quadprog.compute_joint_vel_cmd_qp(q_new, spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = q_new + joints_vel * delta_t
egm.send_to_robot(q_c)
# joint angle at previous time step
######### change here ########
q_hat = q_hat + qhat_dot * delta_t
error = out - desired
# flip the error
err_flip = np.fliplr(error)
print np.linalg.norm(error, 'fro')
return out, err_flip, np.linalg.norm(
error, 'fro'), force_out, eef_pos, eef_orien, tim
def first_half(input, num_iter):
# stop the active RAPID program
#rapid.stop()
# reset the pointer to main
#rapid.resetpp()
print 'first half'
print 'reset PP to main'
time.sleep(5)
# start the RAPID program
#rapid.start()
# determine if robot has reached the initial configuration
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init))
if np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init)) < 1e-4:
tag = False
time.sleep(1)
# out is composed of 5 velocity and 1 force in z
out = np.zeros((6, n))
force_out = np.zeros((6, n))
# pos of eef
eef_pos = np.zeros((3, n))
# orientation of eef (quaternion)
eef_orien = np.zeros((4, n))
# timestamp
tim = np.zeros((1, n))
############### change ################ or there will be errors that q_pre referred before assigned
#q_pre = np.deg2rad(state.joint_angles)
q_hat = np.zeros((6, 1))
qhat_dot = np.zeros((6, 1))
# for observer k should be symmetric and positive definite
kl = 0.1
### drain the force sensor buffer ###
count = 0
while count < 1000:
flag, ft = netft.read_ft_streaming(.1)
#print ft[5]
count = count + 1
### drain the EGM buffer ###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
# substract the initial force for bias
flag, ft = netft.read_ft_streaming(.1)
F0 = ft[5]
print F0
time.sleep(3)
cnt = 0
step_done = False
while cnt < n:
#t_pre = time.time()
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
q_new = state.joint_angles
# step-over
if not step_done:
print '--------start step-over motion--------'
# do step-over of 0.25 mm in +x in world frame
# current eef pose
pose_cur = rox.fwdkin(abb_robot, q_new)
pose_cur.p[0] = pose_cur.p[0] + num_iter * 2 * step_over
# solve for inverse kinematics and pick the one that is closest to current configuration
sol = rox.robot6_sphericalwrist_invkin(abb_robot, pose_cur, q_new)
try:
tar = sol[0] # raise exception if no solution
except:
tar = q_new
# move to the new position after step-over
egm.send_to_robot(tar)
step_done = True
q_new = tar
### drain the EGM buffer, or it will use the obsolete EGM feedback###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
print '--------step-over motion done--------'
time.sleep(2)
# forward kinematics to calculate current position of eef
pose = rox.fwdkin(abb_robot, q_new)
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] # - F0# first three torques and then three forces
# start motion in y direction when robot barely touches coupon
Fd0 = 50 # this should be equal to the Fdz[0]
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) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, 0, 0, v_z])
else:
# formalize entire twist
# nominal input composed of F and v
spatial_velocity_command = input[:,
cnt] #np.array([0, 0, 0, vdx, 0, Fd])
z = pose.p[2]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
v_z = Kp * (F - spatial_velocity_command[5]
) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# nominal input only contains v
spatial_velocity_command[5] = v_z
# approximation of joint velocity
#q_new = np.deg2rad(state.joint_angles)
######### change here, use observer instead of approximation to calculate q_dot ########
if cnt == 0:
q_hat = q_new
qhat_dot = joints_vel + kl * (q_new - q_hat)
#q_dot = (q_new - q_pre)/delta_t
J = rox.robotjacobian(abb_robot, q_new)
# estimate velocity
v_est = J.dot(qhat_dot)
# formalize the nominal output composed of F and v
out[:, cnt] = np.append(v_est[0:5], F)
force_out[:, cnt] = np.concatenate((T_b, F_b), axis=0)
eef_pos[:, cnt] = pose.p
#eef_pos[2, cnt] = z
R = pose.R
quat = rox.R2q(R)
eef_orien[:, cnt] = quat
tim[0, cnt] = time.time()
cnt = cnt + 1
print F
# solve for joint velocity
# Jacobian inverse
#J = rox.robotjacobian(abb_robot, q_new)
#joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# 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..."
# QP
joints_vel = quadprog.compute_joint_vel_cmd_qp(
q_new, spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = q_new + joints_vel * delta_t
egm.send_to_robot(q_c)
# joint angle at previous time step
#q_pre = q_new
############ change here ##############
# make the time interval 0.004 s
#t_new = time.time()
#if t_new - t_pre < delta_t:
# time.sleep(delta_t - t_new + t_pre)
######### change here ########
q_hat = q_hat + qhat_dot * delta_t
# interpolate to filter the output
t_inter = np.arange(0, n * delta_t, delta_t)
# interpolate each row of output
for i in range(6):
y = out[i, :]
tck = interpolate.splrep(t_inter, y,
s=0.01) # s = 0 means no interpolation
ynew = interpolate.splev(t_inter, tck, der=0)
out[i, :] = ynew
error = out - desired
# flip the error
err_flip = np.fliplr(error)
print np.linalg.norm(error, 'fro')
return out, err_flip, np.linalg.norm(
error, 'fro'), force_out, eef_pos, eef_orien, tim
