Added camera and gripper control

Author: ykarmesh

scripts/Assignment3_Camera_Calibration.py

@@ -10,12 +10,26 @@
 from sensor_msgs.msg import (JointState,
                              Image,
                              CameraInfo)
+from std_msgs.msg import Float64
 from cv_bridge import CvBridge, CvBridgeError
 from ar_track_alvar_msgs.msg import AlvarMarkers
 import forward_kinematics_extrinsics
 
 ROSTOPIC_SET_ARM_JOINT = '/goal_dynamixel_position'
-#flag = 0
+ROSTOPIC_SET_PAN_JOINT = '/pan/command'
+ROSTOPIC_SET_TILT_JOINT = '/tilt/command'
+
+def set_camera_pan(pub, pan_rad):
+    pan_msg = Float64()
+    pan_msg.data = pan_rad
+    rospy.loginfo('Going to camera pan: {} rad'.format(pan_rad))
+    pub.publish(pan_msg)
+
+def set_camera_tilt(pub, tilt_rad):
+    tilt_msg = Float64()
+    tilt_msg.data = tilt_rad
+    rospy.loginfo('Going to camera tilt: {} rad'.format(tilt_rad))
+    pub.publish(tilt_msg)
 
 def home_arm(pub):
     set_arm_joint(pub, np.zeros(5))
@@ -45,6 +59,9 @@ def main(noiter=15):
 
     bridge = CvBridge()
     pub = rospy.Publisher(ROSTOPIC_SET_ARM_JOINT, JointState, queue_size=1)
+    pan_pub = rospy.Publisher(ROSTOPIC_SET_PAN_JOINT, Float64, queue_size=1)
+    tilt_pub = rospy.Publisher(ROSTOPIC_SET_TILT_JOINT, Float64, queue_size=1)
+
     rospy.sleep(2)
     Calib = rospy.wait_for_message("/camera/color/camera_info", CameraInfo)
     K_matrix = np.array(Calib.K)
@@ -60,6 +77,9 @@ def main(noiter=15):
     raw_input("Robot ready to move to HOME POSITION. Press Enter to continue.")
     print("Robot moving. Please wait.")
     home_arm(pub)
+    set_camera_pan(pan_pub, rad_from_deg*0.)
+    set_camera_tilt(tilt_pub, rad_from_deg*0.)
+    rospy.sleep(4)
 
     def imgcallback(i):
         data = rospy.wait_for_message("/camera/color/image_raw", Image, timeout=5)
@@ -168,6 +188,6 @@ def get_P(end_effector_position):
     return point_3D_world_frame
 
 if __name__ == "__main__":
-    # main(50)
-    calib_info = np.load("../data/calibration/calibration_info.npz")
-    calculate_extrinsic(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'], 20)
+    main(50)
+    # calib_info = np.load("../data/calibration/calibration_info.npz")
+    # calculate_extrinsic(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'], 20)

scripts/ik_control.py

@@ -9,14 +9,22 @@
 import rospy
 from geometry_msgs.msg import Pose, Point, Quaternion
 from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64
+from std_msgs.msg import Empty
 from trac_ik_python.trac_ik import IK
 
 GRIPPER_LINK = "gripper_link"
 ARM_BASE_LINK = "arm_base_link"
 MOVE_GROUP_NAME = 'arm'
+
 ROSTOPIC_SET_ARM_JOINT = '/goal_dynamixel_position'
+ROSTOPIC_SET_PAN_JOINT = '/pan/command'
+ROSTOPIC_SET_TILT_JOINT = '/tilt/command'
+ROSTOPIC_OPEN_GRIPPER = '/gripper/open'
+ROSTOPIC_CLOSE_GRIPPER = '/gripper/close'
+
 IK_POSITION_TOLERANCE = 0.01
-IK_ORIENTATION_TOLERANCE = np.pi/6
+IK_ORIENTATION_TOLERANCE = np.pi/9
 current_joint_state = None
 
 
@@ -25,6 +33,38 @@ def home_arm(pub):
     set_arm_joint(pub, np.zeros(5))
     rospy.sleep(5)
 
+def set_camera_pan(pub, pan_rad):
+    pan_msg = Float64()
+    pan_msg.data = pan_rad
+    rospy.loginfo('Going to camera pan: {} rad'.format(pan_rad))
+    pub.publish(pan_msg)
+
+def set_camera_tilt(pub, tilt_rad):
+    tilt_msg = Float64()
+    tilt_msg.data = tilt_rad
+    rospy.loginfo('Going to camera tilt: {} rad'.format(tilt_rad))
+    pub.publish(tilt_msg)
+
+def set_arm_joint(pub, joint_target):
+    joint_state = JointState()
+    joint_state.position = tuple(joint_target)
+    pub.publish(joint_state)
+
+def get_joint_state(data):
+    global current_joint_state
+    if len(data.position) == 11:
+    	current_joint_state = data.position[4:9]
+
+def open_gripper(pub):
+    empty_msg = Empty()
+    rospy.loginfo('Opening gripper')
+    pub.publish(empty_msg)
+
+def close_gripper(pub):
+    empty_msg = Empty()
+    rospy.loginfo('Closing gripper')
+    pub.publish(empty_msg)
+
 
 def compute_ik(ik_solver, target_pose, current_joint):
     """
@@ -59,17 +99,6 @@ def compute_ik(ik_solver, target_pose, current_joint):
         rospy.logerr('No IK solution found')
     return result
 
-
-def set_arm_joint(pub, joint_target):
-    joint_state = JointState()
-    joint_state.position = tuple(joint_target)
-    pub.publish(joint_state)
-
-def get_joint_state(data):
-    global current_joint_state
-    if len(data.position) == 11:
-    	current_joint_state = data.position[4:9]
-
 def main():
     rospy.init_node('IK_Control', anonymous=True)
 
@@ -79,33 +108,45 @@ def main():
     #                     Quaternion(0.245, 0.613, -0.202, 0.723))]
 
     target_poses = [Pose(Point(0.279, 0.176, 0.217),
-                         Quaternion(-0.4940158, -0.4228743, -0.4940158, 0.5771257)),
-                    Pose(Point(0.239, 0.0116, 0.2),
+                         Quaternion(0, 0, 0, 1)),
+                    Pose(Point(0.239, 0.0116, 0.05),
                          Quaternion(0, 0.8509035, 0, 0.525322))]
 
     ik_solver = IK(ARM_BASE_LINK, GRIPPER_LINK)
 
     rospy.Subscriber('/joint_states', JointState, get_joint_state)
-    pub = rospy.Publisher(ROSTOPIC_SET_ARM_JOINT,
-                          JointState, queue_size=1)
+
+    arm_pub = rospy.Publisher(ROSTOPIC_SET_ARM_JOINT, JointState, queue_size=1)
+    pan_pub = rospy.Publisher(ROSTOPIC_SET_PAN_JOINT, Float64, queue_size=1)
+    tilt_pub = rospy.Publisher(ROSTOPIC_SET_TILT_JOINT, Float64, queue_size=1)
+    gripper_open_pub = rospy.Publisher(ROSTOPIC_OPEN_GRIPPER, Empty, queue_size=1)
+    gripper_close_pub = rospy.Publisher(ROSTOPIC_CLOSE_GRIPPER, Empty, queue_size=1)
     rospy.sleep(2)
-    home_arm(pub)
 
+    home_arm(arm_pub)
+    close_gripper(gripper_close_pub)
 
     for pose in target_poses:
 
         rospy.loginfo('Commanding arm to pose {}'.format(pose))
 
-	print(current_joint_state)
-	if current_joint_state:
+        print(current_joint_state)
+        if current_joint_state:
             target_joint = compute_ik(
             ik_solver, pose, current_joint_state)
 
         if target_joint:
-            set_arm_joint(pub, target_joint)
+            set_arm_joint(arm_pub, target_joint)
             rospy.sleep(5)
 
-    home_arm(pub)
+    open_gripper(gripper_open_pub)
+
+    raw_input("Robot ready to close gripper. Press Enter to continue.")
+    print("Robot moving. Please wait.")
+    close_gripper(gripper_close_pub)
+    rospy.sleep(4)
+
+    home_arm(arm_pub)
 
 
 if __name__ == "__main__":

scripts/lab2_motion_demo.py

@@ -9,6 +9,7 @@
 import rospy
 
 from sensor_msgs.msg import JointState
+import PRM
 
 
 ROSTOPIC_SET_ARM_JOINT = '/goal_dynamixel_position'
@@ -28,34 +29,67 @@ def set_arm_joint(pub, joint_target):
 def main():
     rospy.init_node('command_joints_example', anonymous=True)
 
-    rad_from_deg = np.pi/180.
-    start_joints_deg = [  0., 20., 50., -80., 0.]
-    final_joints_deg = [135., 20., 50., -80., 0.]
-
     pub = rospy.Publisher(ROSTOPIC_SET_ARM_JOINT,
                           JointState, queue_size=1)
     rospy.sleep(2)
-    
-    raw_input("Robot ready to move to HOME POSITION. Press Enter to continue.")
-    print("Robot moving. Please wait.")
-    home_arm(pub)
-    rospy.sleep(5)
 
-    raw_input("Robot ready to move to START POSITION. Press Enter to continue.")
-    print("Robot moving. Please wait.")
-    set_arm_joint(pub, rad_from_deg*np.array(start_joints_deg))
-    rospy.sleep(5)
-    
-    raw_input("Robot ready to move to FINAL POSITION. Press Enter to continue.")
-    print("Robot moving. Please wait.")
-    set_arm_joint(pub, rad_from_deg*np.array(final_joints_deg))
-    rospy.sleep(10)
+    rad_from_deg = np.pi/180.
 
     raw_input("Robot ready to move to HOME POSITION. Press Enter to continue.")
     print("Robot moving. Please wait.")
     home_arm(pub)
     rospy.sleep(5)
 
+    start = np.array([[  0., 20., 50., -80., 0.]])
+    goal = np.array([[85., 20., 50., -80., 0.]])
+    
+    # Create PRM variable and get path from start to goal
+    obstacles_cuboids = np.array([[[0.0604, 0.1854, 0.214], [0, 0, 0], [0.1, 0.125, 0.225]], \
+                                  [[0.0604, 0.1854, 0.349], [0, 0, 0], [0.125, 0.225, 0.1]]])
+
+    prm = PRM.ProbabilisticRoadMap(obstacles_cuboids)
+
+    path = prm.makePlan(start, goal)
+
+    # # Send nodes of the path as commands to controller 
+    for joint_angles in path:
+        print("Robot moving. Please wait.")
+        joint_angles = (joint_angles*rad_from_deg).tolist()
+        set_arm_joint(pub, np.array(joint_angles))
+        print(joint_angles)
+        rospy.sleep(4)
+
+
+    path = prm.makePlan(goal, start)
+
+    # # Send nodes of the path as commands to controller 
+    for joint_angles in path:
+        print("Robot moving. Please wait.")
+        joint_angles = (joint_angles*rad_from_deg).tolist()
+        set_arm_joint(pub, np.array(joint_angles))
+        print(joint_angles)
+        rospy.sleep(4)
+
+    # raw_input("Robot ready to move to HOME POSITION. Press Enter to continue.")
+    # print("Robot moving. Please wait.")
+    # home_arm(pub)
+    # 
+
+    # raw_input("Robot ready to move to START POSITION. Press Enter to continue.")
+    # print("Robot moving. Please wait.")
+    # set_arm_joint(pub, rad_from_deg*np.array(start_joints_deg))
+    # rospy.sleep(5)
+    
+    # raw_input("Robot ready to move to FINAL POSITION. Press Enter to continue.")
+    # print("Robot moving. Please wait.")
+    # set_arm_joint(pub, rad_from_deg*np.array(final_joints_deg))
+    # rospy.sleep(10)
+
+    # raw_input("Robot ready to move to HOME POSITION. Press Enter to continue.")
+    # print("Robot moving. Please wait.")
+    # home_arm(pub)
+    # rospy.sleep(5)
+
 
 if __name__ == "__main__":
     main()