Skip to content

Commit e24d970

Browse files
ykarmeshykarmesh
committed
ik_working
1 parent 9b1ffeb commit e24d970

File tree

3 files changed

+213
-32
lines changed

3 files changed

+213
-32
lines changed

‎scripts/Assignment3_Camera_Calibration.py

Lines changed: 14 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
from std_msgs.msg import Float64
1414
from cv_bridge import CvBridge, CvBridgeError
1515
from ar_track_alvar_msgs.msg import AlvarMarkers
16-
from forward_kinematics_extrinsics import forwardKinematics
16+
import forward_kinematics_extrinsics
1717

1818
ROSTOPIC_SET_ARM_JOINT = '/goal_dynamixel_position'
1919
ROSTOPIC_SET_PAN_JOINT = '/pan/command'
@@ -48,7 +48,9 @@ def generatesample():
4848
if i == 3:
4949
sample[i] = np.random.randint(-10, 10, size=1) - 90
5050
elif i == 1:
51-
sample[i] = np.random.randint(-5, 30, size=1)
51+
sample[i] = np.random.randint(-10, 15, size=1)
52+
elif i == 2:
53+
sample[i] = np.random.randint(-35, 15, size=1)
5254
else:
5355
sample[i] = np.random.randint(-30, 30, size=1)
5456
print(sample)
@@ -65,7 +67,7 @@ def main(noiter=15):
6567
rospy.sleep(2)
6668
Calib = rospy.wait_for_message("/camera/color/camera_info", CameraInfo)
6769
K_matrix = np.array(Calib.K)
68-
fk = forwardKinematics('bottom_plate', 'ar_tag')
70+
fk = forward_kinematics_extrinsics.forwardKinematics('bottom_plate', 'ar_tag')
6971

7072
positions = np.zeros((noiter, 3))
7173
orientations = np.zeros((noiter, 4))
@@ -161,7 +163,7 @@ def calculate_extrinsic(positions, K_matrix, end_effector_positions, noiter):
161163
final_M = get_projection_matrix(p[final_inliers,:], P[final_inliers,:])
162164
# RQ decomposition
163165
K,R = linalg.rq(final_M[:,0:3])
164-
rvec = forwardKinematics.getEulerAngles(np.linalg.inv(R))
166+
rvec = forward_kinematics_extrinsics.forwardKinematics.getEulerAngles(np.linalg.inv(R))
165167
tvec = np.matmul(np.linalg.inv(K),final_M[:,3])
166168
H = np.vstack((np.hstack((R,tvec.reshape((3,1)))),np.array([[0,0,0,1]])))
167169
print("Using PnP Algorithm without non-linear optimization ")
@@ -205,12 +207,16 @@ def calculate_extrinsic_opencv(positions, K_matrix, end_effector_positions):
205207
p = get_p(K_matrix, positions)
206208
P = get_P(end_effector_positions)
207209
success, rvec, tvec, inliers = cv2.solvePnPRansac(P, p, K_matrix.reshape((3,3)), distCoeffs=None)
210+
H = forward_kinematics_extrinsics.forwardKinematics.createTransformationMatrix(rvec,tvec.reshape((-1)))
211+
print(success)
212+
print(len(inliers))
208213
print("Using OpenCV solvePnPRansac")
209214
print("Orientation : {0}".format(rvec.T[0]))
210215
print("Position : {0}".format(tvec.T[0]))
216+
print("Transformation : {0}".format(np.linalg.inv(H)))
211217

212218
if __name__ == "__main__":
213-
main(50)
214-
# calib_info = np.load("../data/calibration/calibration_info.npz")
215-
# calculate_extrinsic(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'], 20)
216-
# calculate_extrinsic_opencv(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'])
219+
main(100)
220+
calib_info = np.load("../data/calibration/calibration_info.npz")
221+
calculate_extrinsic(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'], 20)
222+
calculate_extrinsic_opencv(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'])

‎scripts/camera_forward_kinematics.py

Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
import numpy as np
2+
import math
3+
import roslib; roslib.load_manifest('urdfdom_py')
4+
import rospy
5+
from urdf_parser_py.urdf import URDF
6+
7+
# Class for storing information about the robot and computing the forward kinematics
8+
class camerakinematics:
9+
def __init__(self, base_link='bottom_plate', end_link='camera_link', \
10+
path="/media/karmesh/DATA/CMU/Courses/Robot_Autonomy/Homework/hw3_release/hw3_release/code/locobot_description/urdf/locobot_description.urdf"):
11+
12+
self.axis = None
13+
self.position = None
14+
self.camera_link_to_camera_depth_frame = self.createTransformationMatrix([0, 0, 0], [0, 0.0149, 0])
15+
robot = URDF.from_xml_file(path)
16+
self.urdfParser(robot, base_link, end_link)
17+
self.loadURDFinfo()
18+
self.extrinsics_H = None
19+
20+
# Creates Rotation Matrix given roll, pitch and yaw
21+
@staticmethod
22+
def createRotationMatrix(orientation):
23+
r,p,y = orientation
24+
25+
R_x = np.array([[1, 0, 0 ],
26+
[0, math.cos(r), -math.sin(r)],
27+
[0, math.sin(r), math.cos(r)]])
28+
29+
R_y = np.array([[math.cos(p), 0, math.sin(p)],
30+
[0, 1, 0 ],
31+
[-math.sin(p), 0, math.cos(p)]])
32+
33+
R_z = np.array([[math.cos(y), -math.sin(y), 0],
34+
[math.sin(y), math.cos(y), 0],
35+
[0, 0, 1]])
36+
37+
R = np.matmul(R_z, np.matmul( R_y, R_x ))
38+
39+
return R
40+
41+
# Returns the Homogeneous Transformation Matrix between joint i and ground, where i = [1,2,3,4,5,6,7]
42+
def cameraForwardKinematics(self, joint_angles):
43+
Hij = np.identity(4)
44+
H = []
45+
46+
for i in range(joint_angles.shape[1]):
47+
H21 = self.createTransformationMatrix(self.axis[i,:]*joint_angles[0,i],self.position[i,:])
48+
Hij = np.matmul(Hij, H21)
49+
H.append(Hij)
50+
print(Hij)
51+
52+
# H = np.stack(H)
53+
self.extrinsics_H = np.matmul(Hij, self.camera_link_to_camera_depth_frame)
54+
return self.extrinsics_H
55+
56+
def getTransformedPose(self, R, T):
57+
H = self.createTransformationMatrix(R, T)
58+
new_H = np.matmul(self.extrinsics_H, H)
59+
return self.getEulerAngles(new_H[0:3, 0:3]), H[0:3, 3]
60+
61+
# Checks if a matrix is a valid rotation matrix.
62+
@staticmethod
63+
def isRotationMatrix(R):
64+
Rt = np.transpose(R)
65+
shouldBeIdentity = np.dot(Rt, R)
66+
I = np.identity(3, dtype = R.dtype)
67+
n = np.linalg.norm(I - shouldBeIdentity)
68+
return n < 1e-6
69+
70+
# Calculates rotation matrix to euler angles
71+
@staticmethod
72+
def getEulerAngles(R):
73+
74+
assert(camerakinematics.isRotationMatrix(R)), \
75+
'Not a rotation matrix'
76+
77+
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
78+
79+
singular = sy < 1e-6
80+
81+
if not singular:
82+
x = math.atan2(R[2,1], R[2,2])
83+
y = math.atan2(-R[2,0], sy)
84+
z = math.atan2(R[1,0], R[0,0])
85+
else :
86+
x = math.atan2(-R[1,2], R[1,1])
87+
y = math.atan2(-R[2,0], sy)
88+
z = 0
89+
90+
return np.array([x, y, z])
91+
92+
# Creates Homogeneous Transformation Matrix from orientation and translation information
93+
@staticmethod
94+
def createTransformationMatrix(O,T):
95+
H = np.identity(4)
96+
H[0:3,0:3] = camerakinematics.createRotationMatrix(O)
97+
H[0:3,3] = T
98+
return H
99+
100+
def urdfParser(self, robot, base_link, end_link):
101+
joint_list = robot.get_chain(base_link,end_link,links=False)
102+
self.position = np.zeros((len(joint_list),3))
103+
self.axis = np.zeros((len(joint_list),3))
104+
print(joint_list)
105+
for i in range(len(joint_list)):
106+
self.position[i,:] = robot.joint_map[joint_list[i]].origin.xyz
107+
self.axis[i,:] = robot.joint_map[joint_list[i]].axis
108+
109+
np.savez('../data/cameraurdfinfo.npz', position=self.position, axis=self.axis)
110+
111+
def loadURDFinfo(self):
112+
file = np.load('../data/cameraurdfinfo.npz')
113+
self.position = file[file.files[0]]
114+
self.axis = file[file.files[1]]
115+
# print(self.position)
116+
# print(self.axis)
117+
118+
if __name__ == "__main__":
119+
# calibration_info = np.load('../data/calibration/calibration_info.npz')
120+
# joint_angles = calibration_info[calibration_info.files[3]][0].reshape((1,5))
121+
# fk = forwardKinematics()
122+
# M = fk.getJointForwardKinematics(joint_angles)
123+
X = np.array([[90., 0., 0.]])
124+
ck = camerakinematics()
125+
M = ck.getCameraForwardKinematics(X*np.pi/180)
126+
# for i in range(M.shape[0]):
127+
# print(np.matmul(np.linalg.inv(M[0,:,:]),M[i,:,:]))
128+
print(M)

‎scripts/ik_control.py

Lines changed: 71 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,11 @@
1212
from std_msgs.msg import Float64
1313
from std_msgs.msg import Empty
1414
from trac_ik_python.trac_ik import IK
15+
import camera_forward_kinematics
16+
import tf
1517

1618
GRIPPER_LINK = "gripper_link"
17-
ARM_BASE_LINK = "arm_base_link"
19+
ARM_BASE_LINK = "bottom_plate"
1820
MOVE_GROUP_NAME = 'arm'
1921

2022
ROSTOPIC_SET_ARM_JOINT = '/goal_dynamixel_position'
@@ -24,7 +26,7 @@
2426
ROSTOPIC_CLOSE_GRIPPER = '/gripper/close'
2527

2628
IK_POSITION_TOLERANCE = 0.01
27-
IK_ORIENTATION_TOLERANCE = np.pi/9
29+
IK_ORIENTATION_TOLERANCE = np.pi/6
2830
current_joint_state = None
2931

3032

@@ -33,17 +35,18 @@ def home_arm(pub):
3335
set_arm_joint(pub, np.zeros(5))
3436
rospy.sleep(5)
3537

36-
def set_camera_pan(pub, pan_rad):
38+
def set_camera_angles(pan_pub, tilt_pub, pan_rad, tilt_rad, ck):
3739
pan_msg = Float64()
3840
pan_msg.data = pan_rad
3941
rospy.loginfo('Going to camera pan: {} rad'.format(pan_rad))
40-
pub.publish(pan_msg)
42+
pan_pub.publish(pan_msg)
4143

42-
def set_camera_tilt(pub, tilt_rad):
4344
tilt_msg = Float64()
4445
tilt_msg.data = tilt_rad
4546
rospy.loginfo('Going to camera tilt: {} rad'.format(tilt_rad))
46-
pub.publish(tilt_msg)
47+
tilt_pub.publish(tilt_msg)
48+
49+
print("Camera Extrinsics", ck.cameraForwardKinematics(np.array([[pan_rad, tilt_rad, 0.]])))
4750

4851
def set_arm_joint(pub, joint_target):
4952
joint_state = JointState()
@@ -52,8 +55,8 @@ def set_arm_joint(pub, joint_target):
5255

5356
def get_joint_state(data):
5457
global current_joint_state
55-
if len(data.position) == 11:
56-
current_joint_state = data.position[4:9]
58+
# if len(data.position) == 9:
59+
current_joint_state = data.position[0:5]
5760

5861
def open_gripper(pub):
5962
empty_msg = Empty()
@@ -101,36 +104,72 @@ def compute_ik(ik_solver, target_pose, current_joint):
101104

102105
def main():
103106
rospy.init_node('IK_Control', anonymous=True)
104-
105-
#target_poses = [Pose(Point(0.279, 0.176, 0.217),
106-
# Quaternion(-0.135, 0.350, 0.329, 0.866)),
107-
# Pose(Point(0.339, 0.0116, 0.255),
108-
# Quaternion(0.245, 0.613, -0.202, 0.723))]
109-
110-
target_poses = [Pose(Point(0.279, 0.176, 0.217),
111-
Quaternion(0, 0, 0, 1)),
112-
Pose(Point(0.239, 0.0116, 0.05),
113-
Quaternion(0, 0.8509035, 0, 0.525322))]
107+
global current_joint_state
108+
109+
rad_from_deg = np.pi/180.
114110

115111
ik_solver = IK(ARM_BASE_LINK, GRIPPER_LINK)
116-
112+
ck = camera_forward_kinematics.camerakinematics()
113+
114+
# Describing the publisher subscribers
117115
rospy.Subscriber('/joint_states', JointState, get_joint_state)
118116

119117
arm_pub = rospy.Publisher(ROSTOPIC_SET_ARM_JOINT, JointState, queue_size=1)
120118
pan_pub = rospy.Publisher(ROSTOPIC_SET_PAN_JOINT, Float64, queue_size=1)
121119
tilt_pub = rospy.Publisher(ROSTOPIC_SET_TILT_JOINT, Float64, queue_size=1)
122120
gripper_open_pub = rospy.Publisher(ROSTOPIC_OPEN_GRIPPER, Empty, queue_size=1)
123121
gripper_close_pub = rospy.Publisher(ROSTOPIC_CLOSE_GRIPPER, Empty, queue_size=1)
122+
tf_listener = tf.TransformListener()
124123
rospy.sleep(2)
125124

126-
home_arm(arm_pub)
125+
# Homing of all servos
127126
close_gripper(gripper_close_pub)
127+
set_camera_angles(pan_pub, tilt_pub, rad_from_deg*0., rad_from_deg*40.0, ck)
128+
129+
rospy.sleep(5)
130+
# while not rospy.is_shutdown():
131+
# try:
132+
# tf_listener.waitForTransform("/camera_depth_frame", "/icp_cuboid_frame", rospy.Time.now(), rospy.Duration(4.0))
133+
# position, quaternion = tf_listener.lookupTransform("/camera_depth_frame", "/icp_cuboid_frame", rospy.Time(0))
134+
# print("Box in depth frame")
135+
# print(position, quaternion)
136+
# break
137+
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
138+
# print(e)
139+
# # return
140+
141+
# # for pose in target_poses:
142+
# O = tf.transformations.euler_from_quaternion(quaternion)
143+
# R, T = ck.getTransformedPose(np.array([O[0], O[1], O[2]]), np.array([position[0], position[1], position[2]]))
144+
# print("Box in bottom plate frame" ,R, T)
145+
# q = tf.transformations.quaternion_from_euler(R[0], R[1], R[2])
146+
# poses = [Pose(Point(T[0], T[1], T[2]+0.1), Quaternion(q[0], q[1], q[2], q[3])),
147+
# Pose(Point(T[0], T[1], T[2]), Quaternion(q[0], q[1], q[2], q[3]))]
148+
# rospy.loginfo('Commanding arm to pose {}'.format(poses))
149+
150+
# q = tf.transformations.quaternion_from_euler(0, 90, 0)
151+
# poses = [Pose(Point(0.3, 0.000, 0.25), Quaternion(q[0], q[1], q[2], q[3])),
152+
# Pose(Point(0.3, 0.000, 0.10), Quaternion(q[0], q[1], q[2], q[3]))]
153+
154+
while not rospy.is_shutdown():
155+
try:
156+
tf_listener.waitForTransform("/bottom_plate", "/icp_cuboid_frame", rospy.Time.now(), rospy.Duration(4.0))
157+
P, Q = tf_listener.lookupTransform("/bottom_plate", "/icp_cuboid_frame", rospy.Time(0))
158+
print("Box in bottom plate frame")
159+
print(P, Q)
160+
break
161+
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
162+
print(e)
163+
164+
poses = [Pose(Point(P[0], P[1], P[2]+0.1), Quaternion(Q[0], Q[1], Q[2], Q[3])),
165+
Pose(Point(P[0], P[1], P[2]), Quaternion(Q[0], Q[1], Q[2], Q[3]))]
128166

129-
for pose in target_poses:
167+
target_joint = None
130168

131-
rospy.loginfo('Commanding arm to pose {}'.format(pose))
132-
169+
home_arm(arm_pub)
170+
for pose in poses:
133171
print(current_joint_state)
172+
print("Reaching a joint state")
134173
if current_joint_state:
135174
target_joint = compute_ik(
136175
ik_solver, pose, current_joint_state)
@@ -139,7 +178,8 @@ def main():
139178
set_arm_joint(arm_pub, target_joint)
140179
rospy.sleep(5)
141180

142-
open_gripper(gripper_open_pub)
181+
open_gripper(gripper_open_pub)
182+
rospy.sleep(4)
143183

144184
raw_input("Robot ready to close gripper. Press Enter to continue.")
145185
print("Robot moving. Please wait.")
@@ -151,3 +191,10 @@ def main():
151191

152192
if __name__ == "__main__":
153193
main()
194+
195+
196+
# Some values
197+
# target_poses = [Pose(Point(0.279, 0.176, 0.217),
198+
# Quaternion(0, 0, 0, 1)),
199+
# Pose(Point(0.239, 0.0116, 0.05),
200+
# Quaternion(0, 0.8509035, 0, 0.525322))]

0 commit comments

Comments
 (0)