示例#1
0
def main(args):
    print "Running demo"
    rospy.init_node("sock_demo_node")
    PrimitiveUtils.call_primitive("init")
    pt = pickup_clump("r")
    pt = pickup_sock("l")
    open_sock()
    """   
示例#2
0
def main(args):
    print "Running demo"
    rospy.init_node("sock_demo_node")
    PrimitiveUtils.call_primitive("init")
    pt = pickup_clump("r")
    pt = pickup_sock("l")
    open_sock()
    """   
    def init_stances(self):
        my_stances = {}
        
        folding_height_stance = Stance  (set_torso_height=True,torso_height=0.3,arms="n")
        my_stances["folding_height"] = folding_height_stance
        viewing_height_stance = Stance  (set_torso_height=True,torso_height=0.2,arms="n")
        my_stances["viewing_height"] = viewing_height_stance
        folding_stance = Stance(x_left=0.65,y_left=0.3,z_left=0.2,roll_left=pi/2,pitch_left=pi/4,yaw_left=0,grip_left=False,frame_left="torso_lift_link"
                                    ,x_right=0.65,y_right=-0.3,z_right=0.2,roll_right=pi/2,pitch_right=pi/4,yaw_right=0,grip_right=False,frame_right="torso_lift_link"
                                    ,arms="b")                                 
        my_stances["folding"] = folding_stance
        folding_right_stance = Stance(x_left=0.65,y_left=0.3,z_left=0.2,roll_left=pi/2,pitch_left=pi/4,yaw_left=0,grip_left=False,frame_left="torso_lift_link"
                                    ,x_right=0.65,y_right=-0.3,z_right=0.2,roll_right=pi/2,pitch_right=pi/4,yaw_right=0,grip_right=False,frame_right="torso_lift_link"
                                    ,arms="r")
        my_stances["folding_right"] = folding_right_stance  
        folding_left_stance = Stance(x_left=0.65,y_left=0.3,z_left=0.2,roll_left=pi/2,pitch_left=pi/4,yaw_left=0,grip_left=False,frame_left="torso_lift_link"
                                    ,x_right=0.65,y_right=-0.3,z_right=0.2,roll_right=pi/2,pitch_right=pi/4,yaw_right=0,grip_right=False,frame_right="torso_lift_link"
                                    ,arms="l")
        my_stances["folding_left"] = folding_left_stance        
        viewing_stance = Stance (x_left=0.1,y_left=0.7,z_left=0.3,roll_left=pi/2,pitch_left=-pi/4,yaw_left=0,grip_left=False,frame_left='torso_lift_link'
                                ,x_right=0.1,y_right=-0.7,z_right=0.3,roll_right=pi/2,pitch_right=-pi/4,yaw_right=0,grip_right=False,frame_right='torso_lift_link'
                                ,arms="b")
        my_stances["viewing"] = viewing_stance
        viewing_stance_left = Stance (x_left=0.1,y_left=0.7,z_left=0.3,roll_left=pi/2,pitch_left=-pi/4,yaw_left=0,grip_left=False,frame_left='torso_lift_link'
                                ,x_right=0.1,y_right=-0.7,z_right=0.3,roll_right=pi/2,pitch_right=-pi/4,yaw_right=0,grip_right=False,frame_right='torso_lift_link'
                                ,arms="l")
        my_stances["viewing_left"] = viewing_stance_left
        viewing_stance_right = Stance (x_left=0.1,y_left=0.7,z_left=0.3,roll_left=pi/2,pitch_left=-pi/4,yaw_left=0,grip_left=False,frame_left='torso_lift_link'
                                ,x_right=0.1,y_right=-0.7,z_right=0.3,roll_right=pi/2,pitch_right=-pi/4,yaw_right=0,grip_right=False,frame_right='torso_lift_link'
                                ,arms="r")
        my_stances["viewing_right"] = viewing_stance_right
        #Head stances
        my_stances["look_bottom_left"] = Stance(arms="n",set_head_angle=True,head_pan=0.6,head_tilt=1.25)
        my_stances["look_top_left"] = Stance(arms="n",set_head_angle=True,head_pan=0.6,head_tilt=0.9)
        my_stances["look_bottom_right"] = Stance(arms="n",set_head_angle=True,head_pan=-0.6,head_tilt=1.25)
        my_stances["look_top_right"] = Stance(arms="n",set_head_angle=True,head_pan=-0.6,head_tilt=0.9)
        my_stances["look_center"] = Stance(arms="n",set_head_angle=True,head_pan=0.0,head_tilt=1.0)
        StanceUtils.add_stances(my_stances)
        initialize = PrimitiveUtils.make_primitive(stances=["folding_height","viewing"],durs=[5.0,5.0],params=[[],[]])
        PrimitiveUtils.add_primitive(primitive=initialize,name="initialize")
        scan_cloth = PrimitiveUtils.make_primitive(
                                                    stances=["viewing","look_bottom_left","pause","look_top_left","pause","look_top_right","pause","look_bottom_right","pause"],
                                                    durs=[5.0,5.0,25.0,5.0,25.0,5.0,25.0,5.0,25.0],
                                                    params=[[],[],[],[],[],[],[],[],[]]
                                                    )

        PrimitiveUtils.add_primitive(primitive=scan_cloth,name="scan_cloth")
示例#4
0
    def init_stances(self):
        my_stances = {}

        folding_height_stance = Stance(set_torso_height=True,
                                       torso_height=0.3,
                                       arms="n")
        my_stances["folding_height"] = folding_height_stance
        viewing_height_stance = Stance(set_torso_height=True,
                                       torso_height=0.2,
                                       arms="n")
        my_stances["viewing_height"] = viewing_height_stance
        folding_stance = Stance(x_left=0.65,
                                y_left=0.3,
                                z_left=0.2,
                                roll_left=pi / 2,
                                pitch_left=pi / 4,
                                yaw_left=0,
                                grip_left=False,
                                frame_left="torso_lift_link",
                                x_right=0.65,
                                y_right=-0.3,
                                z_right=0.2,
                                roll_right=pi / 2,
                                pitch_right=pi / 4,
                                yaw_right=0,
                                grip_right=False,
                                frame_right="torso_lift_link",
                                arms="b")
        my_stances["folding"] = folding_stance
        folding_right_stance = Stance(x_left=0.65,
                                      y_left=0.3,
                                      z_left=0.2,
                                      roll_left=pi / 2,
                                      pitch_left=pi / 4,
                                      yaw_left=0,
                                      grip_left=False,
                                      frame_left="torso_lift_link",
                                      x_right=0.65,
                                      y_right=-0.3,
                                      z_right=0.2,
                                      roll_right=pi / 2,
                                      pitch_right=pi / 4,
                                      yaw_right=0,
                                      grip_right=False,
                                      frame_right="torso_lift_link",
                                      arms="r")
        my_stances["folding_right"] = folding_right_stance
        folding_left_stance = Stance(x_left=0.65,
                                     y_left=0.3,
                                     z_left=0.2,
                                     roll_left=pi / 2,
                                     pitch_left=pi / 4,
                                     yaw_left=0,
                                     grip_left=False,
                                     frame_left="torso_lift_link",
                                     x_right=0.65,
                                     y_right=-0.3,
                                     z_right=0.2,
                                     roll_right=pi / 2,
                                     pitch_right=pi / 4,
                                     yaw_right=0,
                                     grip_right=False,
                                     frame_right="torso_lift_link",
                                     arms="l")
        my_stances["folding_left"] = folding_left_stance
        viewing_stance = Stance(x_left=0.1,
                                y_left=0.7,
                                z_left=0.3,
                                roll_left=pi / 2,
                                pitch_left=-pi / 4,
                                yaw_left=0,
                                grip_left=False,
                                frame_left='torso_lift_link',
                                x_right=0.1,
                                y_right=-0.7,
                                z_right=0.3,
                                roll_right=pi / 2,
                                pitch_right=-pi / 4,
                                yaw_right=0,
                                grip_right=False,
                                frame_right='torso_lift_link',
                                arms="b")
        my_stances["viewing"] = viewing_stance
        viewing_stance_left = Stance(x_left=0.1,
                                     y_left=0.7,
                                     z_left=0.3,
                                     roll_left=pi / 2,
                                     pitch_left=-pi / 4,
                                     yaw_left=0,
                                     grip_left=False,
                                     frame_left='torso_lift_link',
                                     x_right=0.1,
                                     y_right=-0.7,
                                     z_right=0.3,
                                     roll_right=pi / 2,
                                     pitch_right=-pi / 4,
                                     yaw_right=0,
                                     grip_right=False,
                                     frame_right='torso_lift_link',
                                     arms="l")
        my_stances["viewing_left"] = viewing_stance_left
        viewing_stance_right = Stance(x_left=0.1,
                                      y_left=0.7,
                                      z_left=0.3,
                                      roll_left=pi / 2,
                                      pitch_left=-pi / 4,
                                      yaw_left=0,
                                      grip_left=False,
                                      frame_left='torso_lift_link',
                                      x_right=0.1,
                                      y_right=-0.7,
                                      z_right=0.3,
                                      roll_right=pi / 2,
                                      pitch_right=-pi / 4,
                                      yaw_right=0,
                                      grip_right=False,
                                      frame_right='torso_lift_link',
                                      arms="r")
        my_stances["viewing_right"] = viewing_stance_right
        #Head stances
        my_stances["look_bottom_left"] = Stance(arms="n",
                                                set_head_angle=True,
                                                head_pan=0.6,
                                                head_tilt=1.25)
        my_stances["look_top_left"] = Stance(arms="n",
                                             set_head_angle=True,
                                             head_pan=0.6,
                                             head_tilt=0.9)
        my_stances["look_bottom_right"] = Stance(arms="n",
                                                 set_head_angle=True,
                                                 head_pan=-0.6,
                                                 head_tilt=1.25)
        my_stances["look_top_right"] = Stance(arms="n",
                                              set_head_angle=True,
                                              head_pan=-0.6,
                                              head_tilt=0.9)
        my_stances["look_center"] = Stance(arms="n",
                                           set_head_angle=True,
                                           head_pan=0.0,
                                           head_tilt=1.0)
        StanceUtils.add_stances(my_stances)
        initialize = PrimitiveUtils.make_primitive(
            stances=["folding_height", "viewing"],
            durs=[5.0, 5.0],
            params=[[], []])
        PrimitiveUtils.add_primitive(primitive=initialize, name="initialize")
        scan_cloth = PrimitiveUtils.make_primitive(
            stances=[
                "viewing", "look_bottom_left", "pause", "look_top_left",
                "pause", "look_top_right", "pause", "look_bottom_right",
                "pause"
            ],
            durs=[5.0, 5.0, 25.0, 5.0, 25.0, 5.0, 25.0, 5.0, 25.0],
            params=[[], [], [], [], [], [], [], [], []])

        PrimitiveUtils.add_primitive(primitive=scan_cloth, name="scan_cloth")