Add moving Baxter testing scripts

src_main/n1_work_flow_control.py

@@ -42,21 +42,12 @@ def moveBaxterToJointAngles(pos):
 def readBaxterEndeffectPose():
     if DEBUG_MODE: # Manually define the T4x4 matrix
         pos = Point(-1, 1, 0)
-        pos = Point(0,0, 0)
-        quat = quaternion_from_euler(0, 0, 0, 'rxyz')
+        pos = Point(0, 0, 0)
+        quaternion = quaternion_from_euler(0, 0, 0, 'rxyz')
     else:
-        pos = Point()
-        quat = Quaternion()
-
-    # Trans to 4x4 matrix, and then trans to 1x16 array
-    R=quaternion_to_SO3(quat) # this is my func. support both list and quat-struct.
-    p=[pos.x, pos.y, pos.z]
-    T=form_T(R, p)
-    pose=[]
-    for i in range(4):
-        for j in range(4):
-            pose+=[T[i,j]]
-    return pose
+        (pos, quaternion) = self.tf_listener.lookupTransform(
+            '/base', '/left_hand_camera', rospy.Time(0))
+    return Pose(pos, quaternion)
 
 def getCloudSize(open3d_cloud):
     return np.asarray(open3d_cloud.points).shape[0]
@@ -75,6 +66,18 @@ def getCloudSize(open3d_cloud):
     #   sends the pose to node2 to tell it to take the picture.
     pub_pose = rospy.Publisher(topic_endeffector_pos,
                                T4x4, queue_size=10)
+    def publishPose(pose):
+        # Trans to 4x4 matrix
+        R = quaternion_to_SO3(pose.orientation) # this is my func. support both list and quat-struct.
+        p = [pose.position.x, pose.position.y, pose.position.z]
+        T = form_T(R, p)
+
+        # Trans to 1x16 array
+        pose_1x16 = []
+        for i in range(4):
+            for j in range(4):
+                pose_1x16+=[T[i,j]]
+        pub_pose.publish(pose_1x16)
 
     # Boot up Baxter
     if not DEBUG_MODE:
@@ -121,8 +124,8 @@ def sim_pub_point_cloud(ith_goalpose):
 
         rospy.loginfo("--------------------------------")
         rospy.loginfo("node1: publish pose "+str(ith_goalpose))
-        pub_pose.publish(pose)
-        
+        publishPose(pose)
+
         if DEBUG_MODE: # simulate publishing a point cloud
             rospy.sleep(0.01)
             sim_pub_point_cloud(ith_goalpose)

src_python/README.md

@@ -0,0 +1,6 @@
+
+Contents: Python library functions and small pieces of scripts.
+
+For other python scripts, see:
+* MAIN scripts are in src_main
+* Baxter related python scripts are in ./baxter/

src_python/baxter/README.md

@@ -0,0 +1,8 @@
+
+Baxter related functions and scripts are stored here, either for testing Baxter, or for providing lib functions for the main nodes.
+
+# Reference
+
+Files start with "copied_" are directly copied from Baxter's official tutorial.
+
+Many other small pieces of codes related to Baxter are borrowed from their official example.

src_python/baxter/lib_baxter.py

@@ -0,0 +1,79 @@
+#!/usr/bin/python
+
+import rospy
+import baxter_interface
+from baxter_interface import CHECK_VERSION
+from baxter_pykdl import baxter_kinematics
+from tf.transformations import euler_from_quaternion, quaternion_from_euler
+
+class MyBaxter(object):
+    def __init__(self, arm_name):
+        self.limb_kinematics = baxter_kinematics(arm_name)
+        self.limb = baxter_interface.Limb(arm_name)
+        self.limb_interface = baxter_interface.limb.Limb(arm_name)
+        self.joint_names = limb_interface.joint_names()
+
+        def enableBaxter():
+            rs = self.baxter_interface.RobotEnable(CHECK_VERSION)
+            print("Enabling robot... ")
+            rs.enable()
+            print("Running. Ctrl-c to quit")
+        enableBaxter()
+
+
+    def getJointAngles(self):
+        angles = self.limb.joint_angles().values()
+        return angles
+
+    def getEndEffectorPose(self, flag_return_euler=True):
+        tmp = self.limb_kinematics.forward_position_kinematics()
+        # fk returned type: [x, y, z, quat_i, quat_j, quat_k, quat_w]
+        position = tmp[0:3]
+        quaternion = tmp[3:]
+        if flag_return_euler:
+            euler=list(euler_from_quaternion(quaternion))
+            return position, euler
+        else:
+            return position, quaternion
+
+    def moveToJointAngles(self, joint_angles):
+        print "Moving robot to joint angles : ", joint_angles
+        output_angles = dict(zip(self.joint_names, joint_angles))
+        self.limb.set_joint_positions(output_angles)
+
+    def computeIK(self, pos, orientation=None):
+        print("Computing IK:\n")
+        print(pos)
+        print(orientation)
+        len_pos = len(pos)
+        len_ori = len(orientation)
+        if len_ori == 0:
+            print("Using only position")
+            joint_angles = self.limb_kinematics.inverse_kinematics(pos)
+        if len_pos == 3 and len_ori == 3:
+            print("Using both position and euler angle")
+            euler = orientation
+            quaternion = list(quaternion_from_euler(euler[0], euler[1], euler[2]))
+            joint_angles = self.limb_kinematics.inverse_kinematics(pos, quaternion)
+        if len_pos == 3 and len_ori == 3:
+            print("Using both position and quaternion")
+            quaternion = orientation
+            joint_angles = self.limb_kinematics.inverse_kinematics(pos, quaternion)
+        else:
+            print("Wrong input!")
+            joint_angles = None
+            return joint_angles
+
+        if joint_angles is None:
+            print("my WARNING: IK solution not found.")
+        return joint_angles
+
+
+if __name__ == "__main__":
+    rospy.init_node('GUI_for_Baxter')
+
+    # Setting Baxter
+    arm_name = ['left', 'right'][0]
+    my_baxter = MyBaxter(arm_name)
+
+    rospy.spin()

src_python/baxter/reference/copied_joint_position_keyboard.py

@@ -0,0 +1,157 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2013-2015, Rethink Robotics
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+#    this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+# 3. Neither the name of the Rethink Robotics nor the names of its
+#    contributors may be used to endorse or promote products derived from
+#    this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""
+Baxter RSDK Joint Position Example: keyboard
+"""
+import argparse
+
+import rospy
+
+import baxter_interface
+import baxter_external_devices
+
+from baxter_interface import CHECK_VERSION
+
+
+def map_keyboard():
+    left = baxter_interface.Limb('left')
+    right = baxter_interface.Limb('right')
+    grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
+    grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
+    lj = left.joint_names()
+    rj = right.joint_names()
+
+    def set_j(limb, joint_name, delta):
+        current_position = limb.joint_angle(joint_name)
+        joint_command = {joint_name: current_position + delta}
+        limb.set_joint_positions(joint_command)
+
+    bindings = {
+    #   key: (function, args, description)
+        '9': (set_j, [left, lj[0], 0.1], "left_s0 increase"),
+        '6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"),
+        '8': (set_j, [left, lj[1], 0.1], "left_s1 increase"),
+        '7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"),
+        'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"),
+        'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"),
+        'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"),
+        'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"),
+        'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"),
+        'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"),
+        'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"),
+        'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"),
+        '.': (set_j, [left, lj[6], 0.1], "left_w2 increase"),
+        'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
+        ',': (grip_left.close, [], "left: gripper close"),
+        'm': (grip_left.open, [], "left: gripper open"),
+        '/': (grip_left.calibrate, [], "left: gripper calibrate"),
+
+        '4': (set_j, [right, rj[0], 0.1], "right_s0 increase"),
+        '1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"),
+        '3': (set_j, [right, rj[1], 0.1], "right_s1 increase"),
+        '2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"),
+        'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"),
+        'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"),
+        'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"),
+        'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"),
+        'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"),
+        'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"),
+        'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"),
+        's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"),
+        'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"),
+        'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"),
+        'c': (grip_right.close, [], "right: gripper close"),
+        'x': (grip_right.open, [], "right: gripper open"),
+        'b': (grip_right.calibrate, [], "right: gripper calibrate"),
+     }
+    done = False
+    print("Controlling joints. Press ? for help, Esc to quit.")
+    while not done and not rospy.is_shutdown():
+        c = baxter_external_devices.getch()
+        if c:
+            #catch Esc or ctrl-c
+            if c in ['\x1b', '\x03']:
+                done = True
+                rospy.signal_shutdown("Example finished.")
+            elif c in bindings:
+                cmd = bindings[c]
+                #expand binding to something like "set_j(right, 's0', 0.1)"
+                cmd[0](*cmd[1])
+                print("command: %s" % (cmd[2],))
+            else:
+                print("key bindings: ")
+                print("  Esc: Quit")
+                print("  ?: Help")
+                for key, val in sorted(bindings.items(),
+                                       key=lambda x: x[1][2]):
+                    print("  %s: %s" % (key, val[2]))
+
+
+def main():
+    """RSDK Joint Position Example: Keyboard Control
+
+    Use your dev machine's keyboard to control joint positions.
+
+    Each key corresponds to increasing or decreasing the angle
+    of a joint on one of Baxter's arms. Each arm is represented
+    by one side of the keyboard and inner/outer key pairings
+    on each row for each joint.
+    """
+    epilog = """
+See help inside the example with the '?' key for key bindings.
+    """
+    arg_fmt = argparse.RawDescriptionHelpFormatter
+    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
+                                     description=main.__doc__,
+                                     epilog=epilog)
+    parser.parse_args(rospy.myargv()[1:])
+
+    print("Initializing node... ")
+    rospy.init_node("rsdk_joint_position_keyboard")
+    print("Getting robot state... ")
+    rs = baxter_interface.RobotEnable(CHECK_VERSION)
+    init_state = rs.state().enabled
+
+    def clean_shutdown():
+        print("\nExiting example...")
+        if not init_state:
+            print("Disabling robot...")
+            rs.disable()
+    rospy.on_shutdown(clean_shutdown)
+
+    print("Enabling robot... ")
+    rs.enable()
+
+    map_keyboard()
+    print("Done.")
+
+
+if __name__ == '__main__':
+    main()