Skip to content

Commit a23f233

Browse files
committed
Improved Forward_kinematics
1 parent f262e8d commit a23f233

File tree

4 files changed

+23
-75
lines changed

4 files changed

+23
-75
lines changed

scripts/Assignment3_Camera_Calibration.py

Lines changed: 16 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ def main(noiter=15):
4848
rospy.sleep(2)
4949
Calib = rospy.wait_for_message("/camera/color/camera_info", CameraInfo)
5050
K_matrix = np.array(Calib.K)
51-
fk = forward_kinematics_extrinsics.forwardKinematics('arm_base_link', 'ar_tag')
51+
fk = forward_kinematics_extrinsics.forwardKinematics('bottom_plate', 'ar_tag')
5252

5353
positions = np.zeros((noiter, 3))
5454
orientations = np.zeros((noiter, 4))
@@ -81,18 +81,17 @@ def artagcallback(i):
8181
# # Send nodes of the path as commands to controller
8282
count = 0
8383
while count < noiter:
84-
8584
joint_angle = generatesample()
8685
joint_angles[count, :] = joint_angle
8786
print(joint_angles)
8887
joint_angle = (joint_angle*rad_from_deg).tolist()
8988
set_arm_joint(pub, np.array(joint_angle))
9089

91-
joint_angle = np.append(joint_angle, 0)
92-
93-
M = fk.getJointForwardKinematics(joint_angle.reshape((1,6)))
94-
end_effector_positions[count, :, :] = np.matmul(np.linalg.inv(M[0,:,:]),M[5,:,:])
90+
joint_angle = np.append(np.append(0, joint_angle), 0)
9591

92+
M = fk.getJointForwardKinematics(joint_angle.reshape((1,7)))
93+
end_effector_positions[count, :, :] = np.matmul(np.linalg.inv(M[0,:,:]),M[6,:,:])
94+
print(end_effector_positions[count, :, :])
9695
rospy.sleep(4)
9796

9897
try:
@@ -111,16 +110,16 @@ def artagcallback(i):
111110
np.savez("../data/calibration/calibration_info.npz", position=positions, orientation=orientations, camerainfo=K_matrix, \
112111
joint_angles=joint_angles, forward_kinematics=end_effector_positions)
113112

114-
R = calculate_extrinsic(positions, K_matrix, end_effector_positions)
113+
R = calculate_extrinsic(positions, K_matrix, end_effector_positions, noiter)
115114
print(R)
116115

117116

118-
def calculate_extrinsic(positions, K_matrix, end_effector_positions):
119-
min_error = 10000000000000000
117+
def calculate_extrinsic(positions, K_matrix, end_effector_positions, noiter):
118+
min_error = np.inf
120119
p = get_p(K_matrix, positions)
121120
P = get_P(end_effector_positions)
122-
for i in range(0,1000):
123-
indices = np.random.randint(0, 100, size=6)
121+
for i in range(0,5000):
122+
indices = np.random.randint(0, noiter, size=6)
124123
M = get_projection_matrix(p[indices,:], P[indices,:])
125124
homogeneous_P = np.hstack((P, np.ones((P.shape[0], 1))))
126125
estimated_p = np.matmul(M,homogeneous_P.T)
@@ -132,9 +131,10 @@ def calculate_extrinsic(positions, K_matrix, end_effector_positions):
132131
final_M = M
133132
K,R = linalg.rq(final_M[:,0:3])
134133
print(K/K[2,2])
135-
T = np.matmul(np.linalg.inv(K),M[:,3])
134+
T = np.matmul(np.linalg.inv(K),final_M[:,3])
136135
H = np.vstack((np.hstack((R,T.reshape((3,1)))),np.array([[0,0,0,1]])))
137-
print(H)
136+
print(np.linalg.inv(H))
137+
print(min_error)
138138
return H
139139

140140
def get_projection_matrix(p, P):
@@ -168,10 +168,6 @@ def get_P(end_effector_position):
168168
return point_3D_world_frame
169169

170170
if __name__ == "__main__":
171-
<<<<<<< HEAD
172-
main(30)
173-
=======
174-
main(100)
175-
# calib_info = np.load("calibration_info.npz")
176-
# calculate_extrinsic(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'])
177-
>>>>>>> 00f3b8d0a97eda761ca978e46306ced5397a027d
171+
# main(50)
172+
calib_info = np.load("../data/calibration/calibration_info.npz")
173+
calculate_extrinsic(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'], 20)

scripts/command_joints.py

Lines changed: 0 additions & 48 deletions
This file was deleted.

scripts/forward_kinematics_extrinsics.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66

77
# Class for storing information about the robot and computing the forward kinematics
88
class forwardKinematics:
9-
def __init__(self, base_link='arm_base_link', end_link='ar_tag', \
9+
def __init__(self, base_link='bottom_plate', end_link='ar_tag', \
1010
path="/media/karmesh/DATA/CMU/Courses/Robot_Autonomy/Homework/hw3_release/hw3_release/code/locobot_description/urdf/locobot_description.urdf"):
1111

1212
self.axis = None
@@ -122,8 +122,8 @@ def createTransformationMatrix(O,T):
122122

123123
def urdfParser(self, robot, base_link, end_link):
124124
joint_list = robot.get_chain(base_link,end_link,links=False)
125-
self.position = np.zeros((6,3))
126-
self.axis = np.zeros((6,3))
125+
self.position = np.zeros((len(joint_list),3))
126+
self.axis = np.zeros((len(joint_list),3))
127127
print(joint_list)
128128
for i in range(len(joint_list)):
129129
self.position[i,:] = robot.joint_map[joint_list[i]].origin.xyz
@@ -143,7 +143,7 @@ def loadURDFinfo(self):
143143
# joint_angles = calibration_info[calibration_info.files[3]][0].reshape((1,5))
144144
# fk = forwardKinematics()
145145
# M = fk.getJointForwardKinematics(joint_angles)
146-
X = np.array([[ 0., 20., 50., -80., 45., 0.]])
146+
X = np.array([[0., 90., 0., -90., 90., 0., 0.]])
147147
fk = forwardKinematics()
148148
M = fk.getJointForwardKinematics(X*np.pi/180)
149149
for i in range(M.shape[0]):

scripts/test_code.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -40,9 +40,9 @@ def main():
4040
print("Robot moving. Please wait.")
4141
home_arm(pub)
4242

43-
X = np.array([[ 0., 20., 50., -80., 45.]])
44-
fk = forward_kinematics_extrinsics.forwardKinematics('arm_base_link', 'ar_tag')
45-
M = fk.getJointForwardKinematics(X[0,0:,4]*rad_from_deg)
43+
X = np.array([[ 90., 0., -90., 90., 0.]])
44+
fk = forward_kinematics_extrinsics.forwardKinematics('bottom_plate', 'ar_tag')
45+
M = fk.getJointForwardKinematics(X*rad_from_deg)
4646
for i in range(M.shape[0]):
4747
print(np.matmul(np.linalg.inv(M[0,:,:]),M[i,:,:]))
4848

0 commit comments

Comments
 (0)