baxter_continuous_scan.py 文件源码

def move_cup(self):
        self.gripper.close()

        rotate = long(random.randrange(20, 180))*(3.14156)/(180) 
        print("rotate: " + str(rotate))
        t = self.tl.getLatestCommonTime("/root", "/right_gripper")
        cur_position, quaternion = self.tl.lookupTransform("/root",
                                                           "/right_gripper",
                                                           t)
        quaternion = [1,0,0,0]
        requrd_rot = (0,0,rotate) # in radians
        requrd_trans = (0,0,0)                
        rotated_pose = self.getOffsetPoses(cur_position, quaternion, requrd_rot, requrd_trans)
        trans_5= tuple(rotated_pose[:3])
        quat_5= tuple(rotated_pose[3:])
        br = tf.TransformBroadcaster()
        br.sendTransform(trans_5,
                                  quat_5,
                                  rospy.Time.now(),
                                  "pick_pose",
                                  "/root")  
        pick_pose = PoseStamped(Header(0, rospy.Time(0), n.robot.base),
                                Pose(Point(*trans_5),
                                Quaternion(*quat_5)))
        self.move_ik(pick_pose)
        self.gripper.open()

        t = self.tl.getLatestCommonTime("/root", "/right_gripper")
        cur_position, quaternion = self.tl.lookupTransform("/root",
                                                        "/right_gripper",
                                                           t)
        cur_position = list(cur_position)
        cur_position[2] = cur_position[2] + 0.08
        pose = Pose(Point(*cur_position),
                        Quaternion(*quaternion))
        while True:
            try:
                stamped_pose = PoseStamped( Header(0, rospy.Time(0), n.robot.base), pose)
                self.move_ik(stamped_pose)
                break
            except AttributeError:
                print("can't find valid pose for gripper cause I'm dumb")
                return False

        rospy.sleep(2)

        home = PoseStamped(
                    Header(0, rospy.Time(0), n.robot.base),
                    Pose(Point(0.60558, -0.24686, 0.093535),
                    Quaternion(0.99897, -0.034828, 0.027217, 0.010557)))
        self.move_ik(home) 
        rospy.sleep(2)