3,
2]
T_base2leftgripper = np.dot(robot_parameters.T_base2leftarmmount,
T_leftarmmount2leftgripper)
# Compute peg frame wrt base frame
T_base2peg = T_base2leftgripper.copy()
T_base2peg[:3, 3] += robot_parameters.l_peg * T_base2peg[:3, 2]
###########################################################################
########################### PART 3 ########################################
## Compute Hole Pose wrt Base Frame using Right arm joint solution ##
###########################################################################
# Compute gripper frame wrt base frame
T_rightarmmount2rightgripper = np.vstack(
(ikModuleRight.compFK(right_js_noisy), np.array([0, 0, 0, 1])))
T_rightarmmount2rightgripper[:3,
3] += correction_length * T_rightarmmount2rightgripper[:
3,
2]
T_base2rightgripper = np.dot(robot_parameters.T_base2rightarmmount,
T_rightarmmount2rightgripper)
# Compute peg frame wrt base frame
T_base2hole = T_base2rightgripper.copy()
T_base2hole[:3, 3] += robot_parameters.l_hole * T_base2hole[:3, 2]
############################################################################
############################ PART 4 ########################################
################ Move peg along its z-axis towards hole ##################
############################################################################
pd_r = np.array([0.385, 0.692, 0.350])
qd_r = np.array([0.316, 0.188, -0.698, -0.614])
# Get the inverse kinematics solutions
print("Start of computing IK solutions for left-arm")
get_iksol(pd_l, qd_l, adjust_len=0.067, side="left", config_num=1)
print("\n\n\n\n")
print("Start of computing IK solutions for right-arm")
get_iksol(pd_r, qd_r, adjust_len=0.067, side="right", config_num=2)
# # Cross validate forward kinematics of left arm
print(
"Warning: You need to type left and right arm solutions manually. Other wise following result has no meaning"
)
left_sol = [-0.514, 0.312, 2.837, 0.580, 1.692, 2.028, -0.507]
left_FK = ikModuleLeft.compFK(left_sol)
left_FK[:3, 3] += 0.067 * left_FK[:3, 2]
print("\n => Left arm position: ", left_FK[:3, 3])
print("=> Left arm orientation(quaternion): ", rotm2quat(left_FK[:3, :3]))
# # Cross validate forward kinematics of right arm
print(
"Warning: You need to type left and right arm solutions manually. Other wise following result has no meaning"
)
right_sol = [0.287, -0.717, 0.647, 1.179, 1.082, 1.390, -0.093]
right_FK = ikModuleRight.compFK(right_sol)
right_FK[:3, 3] += 0.067 * right_FK[:3, 2]
print("\n => Right arm position: ", right_FK[:3, 3])
print("=> Right arm orientation(quaternion): ", rotm2quat(right_FK[:3, :3]))
2]
T_base2leftgripper = np.dot(
robot_parameters.T_base2leftarmmount,
T_leftarmmount2leftgripper)
# Compute peg frame wrt base frame
T_base2peg = T_base2leftgripper.copy()
T_base2peg[:3, 3] += robot_parameters.l_peg * T_base2peg[:3, 2]
###########################################################################
########################### PART 3 ########################################
## Compute Hole Pose wrt Base Frame using Right arm joint solution ##
###########################################################################
# Compute gripper frame wrt base frame
T_rightarmmount2rightgripper = np.vstack(
(ikModuleRight.compFK(right_js_noisy),
np.array([0, 0, 0, 1])))
T_rightarmmount2rightgripper[:3,
3] += correction_length * T_rightarmmount2rightgripper[:
3,
2]
T_base2rightgripper = np.dot(
robot_parameters.T_base2rightarmmount,
T_rightarmmount2rightgripper)
# Compute peg frame wrt base frame
T_base2hole = T_base2rightgripper.copy()
T_base2hole[:3,
3] += robot_parameters.l_hole * T_base2hole[:3, 2]
############################################################################
###########################################################################
# Compute gripper frame wrt base frame
T_leftarmmount2leftgripper = np.vstack((ikModuleLeft.compFK(left_js_noisy), np.array([0, 0, 0, 1])))
T_leftarmmount2leftgripper[:3, 3] += correction_length * T_leftarmmount2leftgripper[:3, 2]
T_base2leftgripper = np.dot(robot_parameters.T_base2leftarmmount, T_leftarmmount2leftgripper)
# Compute peg frame wrt base frame
T_base2peg = T_base2leftgripper.copy()
T_base2peg[:3, 3] += robot_parameters.l_peg*T_base2peg[:3, 2]
###########################################################################
########################### PART 3 ########################################
## Compute Hole Pose wrt Base Frame using Right arm joint solution ##
###########################################################################
# Compute gripper frame wrt base frame
T_rightarmmount2rightgripper = np.vstack((ikModuleRight.compFK(right_js_noisy), np.array([0, 0, 0, 1])))
T_rightarmmount2rightgripper[:3, 3] += correction_length * T_rightarmmount2rightgripper[:3, 2]
T_base2rightgripper = np.dot(robot_parameters.T_base2rightarmmount, T_rightarmmount2rightgripper)
# Compute peg frame wrt base frame
T_base2hole = T_base2rightgripper.copy()
T_base2hole[:3, 3] += robot_parameters.l_hole*T_base2hole[:3, 2]
############################################################################
############################ PART 4 ########################################
################ Move peg along its z-axis towards hole ##################
############################################################################
dist = kinematic_utilities.norm(T_base2peg[:3, 3] - T_base2hole[:3, 3])
# Update the position of peg frame after sliding it along z-axis by dist
peg_final_position = T_base2peg[:3, 3] + dist * T_base2peg[:3, 2]
abs_error = math.sqrt(error[0]**2 + error[1]**2 + error[2]**2)
print("Absolute error %2.6f" % abs_error)
print("============================================\n")
###########################################################
# Test case right arm with standard narrow gripper
###########################################################
pd_r = np.array([0.350, 0.584, 0.193])
qd_r = np.array([-0.239, -0.355, 0.576, 0.697])
js = [
0.630801096432741, -0.602499540215156, -0.105711918042191,
1.400339041322102, 1.463158873826231, 1.910764999999990, -1.036010254667002
]
# js = [0.2001, -0.4832, 0.5717, 1.3610, 1.0143, 1.5708, -0.9092]
fk_sol = ikModuleRight.compFK(js)
pa = fk_sol[:, 3]
error = pa - pd_r
abs_error = math.sqrt(error[0]**2 + error[1]**2 + error[2]**2)
print("Absolute error %2.6f" % abs_error)
print("============================================\n")
###########################################
# Test case 4
###########################################
pd = np.array([0.3866647738, -0.7262986804, 0.2122544445])
qd = [-0.3063825941, 0.3034236009, 0.6363355209, -0.6396412505]
js = [
-0.3976845193, -0.1514806028, -0.8233641879, 0.5361262854, -0.7179030087,
1.6682041068, 0.2143738151
]
