pr2_pitcher.world

Go to the documentation of this file.

[CONSTANTS]

DEG2RAD:=PI/180

[LINKS]

  world

  fridge_base: body "bodies/Fridge/friA-01.off"
               body "bodies/Fridge/friA-02.off"
               body "bodies/Fridge/friA-03.off"
               body "bodies/Fridge/friA-04.off"
               % cylinder 0.02 (0.35,-0.5,0) (0.35,-0.5,1.5) blue

  fridge_door:  body "bodies/Fridge/friB-01.off"
                body "bodies/Fridge/friB-02.off"
                body "bodies/Fridge/friB-03.off"

  pitcher: body "bodies/pitcher.off"

  % pitcher: body "bodies/components/ice1.off"
  %          body "bodies/components/lemon3.off"
  %          body "bodies/components/jar_with_liquid1.off"

  % pitcher:  body "bodies/components/jar_with_liquid2.off"
  %           body "bodies/components/3lemon.off"
  %           body "bodies/components/4lemon.off"
  %           body "bodies/components/liquid2.off"
  %           body "bodies/components/pitcher2.off"
  %           body "bodies/components/spoon1.off"

  pitcher_virtual1
  pitcher_virtual2
  pitcher_virtual3

  base_footprint: body "bodies/base_link.off" white
  % body "bodies/friA_01.off"
  
  base:  body "bodies/base_link.off" white

  laser_tilt: body "bodies/laser_tilt_mount_link.off" white

  wheel_rotation_1: body "bodies/bl_caster_rotation_link.off" white
  wheel_1_left:  body "bodies/bl_caster_l_wheel_link.off" black
  wheel_1_right: body "bodies/bl_caster_r_wheel_link.off" black

  wheel_rotation_2: body "bodies/br_caster_rotation_link.off" white
  wheel_2_left:  body "bodies/br_caster_l_wheel_link.off" black
  wheel_2_right: body "bodies/br_caster_r_wheel_link.off" black

  wheel_rotation_3: body "bodies/fl_caster_rotation_link.off" white
  wheel_3_left:  body "bodies/fl_caster_l_wheel_link.off" black
  wheel_3_right: body "bodies/fl_caster_r_wheel_link.off" black

  wheel_rotation_4: body "bodies/fr_caster_rotation_link.off" white
  wheel_4_left:  body "bodies/fr_caster_l_wheel_link.off" black
  wheel_4_right: body "bodies/fr_caster_r_wheel_link.off" black

  torso: body "bodies/torso_lift_link.off" white

  head_pan: body "bodies/head_pan_link.off" white
  head_tilt: body "bodies/head_tilt_link.off" white

  l_shoulder_pan : body "bodies/l_shoulder_pan_link.off" white
  l_shoulder_lift: body "bodies/l_shoulder_lift_link.off" white
  l_upper_arm_roll: body "bodies/l_upper_arm_roll_link.off" white
  l_upper_arm: body "bodies/l_upper_arm_link.off" white
  l_elbow_flex: body "bodies/l_elbow_flex_link.off"  white
  l_forearm_roll: body "bodies/l_forearm_roll_link.off" white
  l_forearm: body "bodies/l_forearm_link.off" white
  l_wrist_flex: body "bodies/l_wrist_flex_link.off" white
  l_wrist_roll: body "bodies/l_wrist_roll_link.off" white
  l_gripper_palm: body "bodies/l_gripper_palm_link.off" white
  l_gripper_l_finger: body "bodies/l_gripper_l_finger_link.off" white
  l_gripper_l_finger_tip: body "bodies/l_gripper_l_finger_tip_link.off" white
  l_gripper_r_finger: body "bodies/l_gripper_r_finger_link.off" white
  l_gripper_r_finger_tip: body "bodies/l_gripper_r_finger_tip_link.off" white


  r_shoulder_pan : body "bodies/r_shoulder_pan_link.off" white
  r_shoulder_lift: body "bodies/r_shoulder_lift_link.off" white
  r_upper_arm_roll: body "bodies/r_upper_arm_roll_link.off" white
  r_upper_arm: body "bodies/r_upper_arm_link.off" white
  r_elbow_flex: body "bodies/r_elbow_flex_link.off"  white
  r_forearm_roll: body "bodies/r_forearm_roll_link.off" white
  r_forearm: body "bodies/r_forearm_link.off" white
  r_wrist_flex: body "bodies/r_wrist_flex_link.off" white
  r_wrist_roll: body "bodies/r_wrist_roll_link.off" white
  r_gripper_palm: body "bodies/r_gripper_palm_link.off" white
  r_gripper_l_finger: body "bodies/r_gripper_l_finger_link.off" white
  r_gripper_l_finger_tip: body "bodies/r_gripper_l_finger_tip_link.off" white
  r_gripper_r_finger: body "bodies/r_gripper_r_finger_link.off" white
  r_gripper_r_finger_tip: body "bodies/r_gripper_r_finger_tip_link.off" white

[JOINTS]

  FIX:  world fridge_base Rz(Pi/2)
        
  REVOLUTE: fridge_base (-0.3,-0.8,0)+(0,0,1)  
            fridge_door(-0.3,-0.8,0)+(0,0,1)
            range [-0.06,2.3]
            +(1,0,0)
            +(1,0,0)

  FIX: world base_footprint  Id

  FIX: base_footprint base
       Txyz(0,0,0.051)

  FIX: torso laser_tilt
       Txyz (0.09893,0,0.227)% (0.275,0,0.252)

  FIX: torso head_pan
       Txyz(-0.01707,0,0.38145)

  FIX: head_pan head_tilt
       Txyz(0.068,0,0)

  /* Wheels */
  FIX: base wheel_rotation_1
       Txyz (-0.2246,0.2246,0.0282)
  FIX: wheel_rotation_1 wheel_1_left
       Txyz(0,0.049,0)
  FIX: wheel_rotation_1 wheel_1_right
       Txyz(0,-0.049,0)

  FIX: base wheel_rotation_2
       Txyz (-0.2246,-0.2246,0.0282)
  FIX: wheel_rotation_2 wheel_2_left
       Txyz(0,0.049,0)
  FIX: wheel_rotation_2 wheel_2_right
       Txyz(0,-0.049,0)

  FIX: base wheel_rotation_3
       Txyz (0.2246,0.2246,0.0282)
  FIX: wheel_rotation_3 wheel_3_left
       Txyz(0,0.049,0)
  FIX: wheel_rotation_3 wheel_3_right
       Txyz(0,-0.049,0)

  FIX: base wheel_rotation_4
       Txyz (0.2246,-0.2246,0.0282)
  FIX: wheel_rotation_4 wheel_4_left
       Txyz(0,0.049,0)
  FIX: wheel_rotation_4 wheel_4_right
       Txyz(0,-0.049,0)
 
  /* Torso and arms (with grippers) */
  % FIX: base torso
  %      Txyz(-0.05,0,0.739675)
  PRISMATIC: base (-0.05,0,0.739675)+(0,0,1)
             torso (0,0,0)+(0,0,1)
             range [0,0.31]

  %  FIX: torso l_shoulder_pan 
  %       Txyz(0,0.188,0)*Rz(Pi/2)
  REVOLUTE: torso (0,0.188,0)+(0,0,1)
            l_shoulder_pan (0,0,0)+(0,0,1)
            range [-40.9437*DEG2RAD,130.944*DEG2RAD]
                  +(1,0,0)
                  +(1,0,0)

  %  FIX: l_shoulder_pan l_shoulder_lift
  %       Txyz(0.1,0,0)*Ry(Pi/4)
  REVOLUTE: l_shoulder_pan (0.1,0,0)+(0,1,0)
            l_shoulder_lift (0,0,0)+(0,1,0)
            range [-30.0001*DEG2RAD,80.0021*DEG2RAD]
                  +(1,0,0)
                  +(1,0,0)

  % FIX: l_shoulder_lift l_upper_arm_roll
  %      Rx(Pi)
  REVOLUTE: l_shoulder_lift (0,0,0)+(1,0,0)
            l_upper_arm_roll (0,0,0)+(1,0,0)
            range [-45.8366*DEG2RAD,223.454*DEG2RAD]
                  +(0,1,0)
                  +(0,1,0)

  FIX: l_upper_arm_roll l_upper_arm
       Id

  % FIX: l_upper_arm l_elbow_flex
  %      Txyz(0.4,0,0)*Ry(-Pi/4)
  REVOLUTE: l_upper_arm (0.4,0,0)+(0,1,0)
            l_elbow_flex (0,0,0)+(0,1,0)
            range [-133.001*DEG2RAD,0*DEG2RAD]
                 +(1,0,0)
                  +(1,0,0)

  % FIX: l_elbow_flex l_forearm_roll
  %      Id
  REVOLUTE: l_elbow_flex (0,0,0)+(1,0,0)
            l_forearm_roll (0,0,0)+(1,0,0)
            
  FIX: l_forearm_roll l_forearm
       Id

  % FIX: l_forearm l_wrist_flex
  %      Txyz(0.321,0,0)
  REVOLUTE: l_forearm (0.321,0,0)+(0,1,0)
            l_wrist_flex (0,0,0)+(0,1,0)
            range [-124.905*DEG2RAD,0*DEG2RAD]
                  +(1,0,0)
                  +(1,0,0)

  % FIX: l_wrist_flex l_wrist_roll
  %      Id
  REVOLUTE: l_wrist_flex (0,0,0)+(1,0,0)
            l_wrist_roll (0,0,0)+(1,0,0)

  FIX: l_wrist_roll l_gripper_palm
       Id

  FIX: l_gripper_palm l_gripper_l_finger
        Txyz(0.07691,0.01,0)% *Rz(pi/20) 
  %    REVOLUTE: l_gripper_palm (0.07691,0.01,0)+(0,0,1)
  %              l_gripper_l_finger (0,0,0)+(0,0,1)
  %              range [0*DEG2RAD,31.3981*DEG2RAD]
  %              +(1,0,0)
  %              +(1,0,0)

  FIX: l_gripper_l_finger l_gripper_l_finger_tip
        Txyz(0.09137,0.00495,0)
  %    CREVOLUTE: l_gripper_l_finger (0.09137,0.00495,0)+(0,0,-1)
  %               l_gripper_l_finger_tip (0,0,0)+(0,0,-1)
  %               range [0*DEG2RAD,31.3981*DEG2RAD]
  %               +(1,0,0)
  %               +(1,0,0)

  FIX: l_gripper_palm l_gripper_r_finger
       Txyz(0.07691,-0.01,0)*Rx(pi) %*Rz(pi/20)   % !!!
  %    CREVOLUTE: l_gripper_palm (0.07691,-0.01,0)+(0,0,-1)
  %               l_gripper_r_finger (0,0,0)+(0,0,1)
  %               range [0*DEG2RAD,31.3981*DEG2RAD]
  %               +(1,0,0)
  %               +(1,0,0)

  FIX: l_gripper_r_finger l_gripper_r_finger_tip
       Txyz(0.09137,-0.00495,0)
  %    CREVOLUTE: l_gripper_r_finger (0.09137,0.00495,0)+(0,0,-1)
  %               l_gripper_r_finger_tip (0,0,0)+(0,0,-1)
  %               range [0*DEG2RAD,31.3981*DEG2RAD]
  %               +(1,0,0)
  %               +(1,0,0)


  % FIX: torso r_shoulder_pan 
  %      Txyz(0,-0.188,0)
  REVOLUTE: torso (0,-0.188,0)+(0,0,1)
            r_shoulder_pan (0,0,0)+(0,0,1)
            range [-130.944*DEG2RAD,40.9437*DEG2RAD]
                  +(1,0,0)
                  +(1,0,0)

  % FIX: r_shoulder_pan r_shoulder_lift
  %      Txyz(0.1,0,0)
  REVOLUTE: r_shoulder_pan (0.1,0,0)+(0,1,0)
            r_shoulder_lift (0,0,0)+(0,1,0)
            range [-30.0001*DEG2RAD,80.0021*DEG2RAD]
                  +(1,0,0)
                  +(1,0,0)

  % FIX: r_shoulder_lift r_upper_arm_roll
  %      Id
  REVOLUTE: r_shoulder_lift (0,0,0)+(1,0,0)
            r_upper_arm_roll (0,0,0)+(1,0,0)
            range [-223.454*DEG2RAD,45.8366*DEG2RAD]
                  +(0,1,0)
                  +(0,1,0)

  FIX: r_upper_arm_roll r_upper_arm
       Id

  % FIX: r_upper_arm r_elbow_flex
  %      Txyz(0.4,0,0)
  REVOLUTE: r_upper_arm (0.4,0,0)+(0,1,0)
            r_elbow_flex (0,0,0)+(0,1,0)
            range [-133.001*DEG2RAD,0*DEG2RAD]
                  +(1,0,0)
                  +(1,0,0)

  % FIX: r_elbow_flex r_forearm_roll
  %      Id
  REVOLUTE: r_elbow_flex (0,0,0)+(1,0,0)
            r_forearm_roll (0,0,0)+(1,0,0)

  FIX: r_forearm_roll r_forearm
       Id

  % FIX: r_forearm r_wrist_flex
  %      Txyz(0.321,0,0)
  REVOLUTE: r_forearm (0.321,0,0)+(0,1,0)
            r_wrist_flex (0,0,0)+(0,1,0)
            range [-124.905*DEG2RAD,0*DEG2RAD]
                  +(1,0,0)
                  +(1,0,0)

  % FIX: r_wrist_flex r_wrist_roll
  %      Id
  REVOLUTE: r_wrist_flex (0,0,0)+(1,0,0)
            r_wrist_roll (0,0,0)+(1,0,0)

  FIX: r_wrist_roll r_gripper_palm
       Id

  FIX: r_gripper_palm r_gripper_l_finger
       Txyz(0.07691,0.01,0)*Rz(pi/18)
  %    REVOLUTE: r_gripper_palm (0.07691,0.01,0)+(0,0,1)
  %              r_gripper_l_finger (0,0,0)+(0,0,1)
  %               range [0*DEG2RAD,31.3981*DEG2RAD]
  %               +(1,0,0)
  %               +(1,0,0)

  FIX: r_gripper_l_finger r_gripper_l_finger_tip
       Txyz(0.09137,0.00495,0)
  %    CREVOLUTE: r_gripper_l_finger (0.09137,0.00495,0)+(0,0,-1)
  %               r_gripper_l_finger_tip (0,0,0)+(0,0,-1)
  %               range [0*DEG2RAD,31.3981*DEG2RAD]
  %               +(1,0,0)
  %               +(1,0,0)

  FIX: r_gripper_palm r_gripper_r_finger
        Txyz(0.07691,-0.01,0)*Rx(pi)*Rz(pi/18)  % !!!
  %     CREVOLUTE: r_gripper_palm (0.07691,-0.01,0)+(0,0,-1)
  %                r_gripper_r_finger (0,0,0)+(0,0,1)
  %                range [0*DEG2RAD,31.3981*DEG2RAD]
  %                +(1,0,0)
  %                +(1,0,0)

  FIX: r_gripper_r_finger r_gripper_r_finger_tip
        Txyz(0.09137,-0.00495,0)
  %     CREVOLUTE: r_gripper_r_finger (0.09137,0.00495,0)+(0,0,-1)
  %                r_gripper_r_finger_tip (0,0,0)+(0,0,-1)
  %                range [0*DEG2RAD,31.3981*DEG2RAD]
  %                +(1,0,0)
  %                +(1,0,0)

  % REVOLUTE: r_wrist_roll (0.18,0,0)+(0,0,1)  
  %           fridge_door(0.225,-0.748,0.93)+(0,0,1)
  %           range [-0.01,0.01]

  % FIX: r_wrist_roll (0.18,0,0)+(0,0,1)  
  %      fridge_door(0.225,-0.748,0.93)+(0,0,1)

  FIX: r_wrist_roll fridge_door 
       Txyz(-0.588,-0.220,-0.93)*Rz(Pi/2)

  FIX: l_wrist_roll pitcher    
       Txyz(0.355,0.08,0)*Rz(Pi)

  PRISMATIC: world (0,0,0)+(1,0,0) 
             pitcher_virtual1 (0,0,0)+(1,0,0)
             range [0,1.5]

  PRISMATIC: pitcher_virtual1 (0,0,0)+(0,1,0) 
             pitcher_virtual2 (0,0,0)+(0,1,0)
             range [-0.5,1]

  PRISMATIC: pitcher_virtual2 (0,0,0)+(0,0,1) 
             pitcher_virtual3 (0,0,0)+(0,0,1)
             range [0.5,1.5]

  REVOLUTE: pitcher_virtual3 (0,0,0)+(0,0,1) 
            pitcher (0,0,0)+(0,0,1)

[COLLISIONS]

  Do not check: all
  Check : fridge_door,pitcher
  Check : fridge_base, pitcher
  Check : r_gripper_palm, fridge_door
  Check : r_gripper_l_finger, fridge_door
  % Check : r_gripper_l_finger_tip, fridge_door
  Check : r_gripper_r_finger, fridge_door
  % Check : r_gripper_r_finger_tip, fridge_door
  Check : pitcher, torso
  Check :  r_shoulder_pan, fridge_door
  Check :  r_shoulder_lift, fridge_door
  Check :  r_upper_arm_roll, fridge_door
  Check :  r_upper_arm, fridge_door
  Check :  r_elbow_flex, fridge_door
  Check :  r_forearm_roll, fridge_door
  Check :  r_forearm, fridge_door
  Check :  r_wrist_flex, fridge_door
  Check :  r_wrist_roll , fridge_door
  Check :  l_shoulder_pan, fridge_door
  Check :  l_shoulder_lift, fridge_door
  Check :  l_upper_arm_roll, fridge_door
  Check :  l_upper_arm, fridge_door
  Check :  l_elbow_flex, fridge_door
  Check :  l_forearm_roll, fridge_door
  Check :  l_forearm, fridge_door
  Check :  l_wrist_flex, fridge_door
  Check :  l_wrist_roll , fridge_door
  
  Check :  l_shoulder_pan, r_shoulder_pan
  Check :  l_shoulder_lift, r_shoulder_pan
  Check :  l_upper_arm_roll, r_shoulder_pan
  Check :  l_upper_arm, r_shoulder_pan
  Check :  l_elbow_flex, r_shoulder_pan
  Check :  l_forearm_roll, r_shoulder_pan
  Check :  l_forearm, r_shoulder_pan
  Check :  l_wrist_flex, r_shoulder_pan
  Check :  l_wrist_roll, r_shoulder_pan
  
  Check :  l_shoulder_pan, r_upper_arm_roll
  Check :  l_shoulder_lift, r_upper_arm_roll
  Check :  l_upper_arm_roll, r_upper_arm_roll
  Check :  l_upper_arm, r_upper_arm_roll
  Check :  l_elbow_flex, r_upper_arm_roll
  Check :  l_forearm_roll, r_upper_arm_roll
  Check :  l_forearm, r_upper_arm_roll
  Check :  l_wrist_flex, r_upper_arm_roll
  Check :  l_wrist_roll, r_upper_arm_roll
  
  Check :  l_shoulder_pan,  r_shoulder_lift
  Check :  l_shoulder_lift,  r_shoulder_lift
  Check :  l_upper_arm_roll,  r_shoulder_lift
  Check :  l_upper_arm,  r_shoulder_lift
  Check :  l_elbow_flex,  r_shoulder_lift
  Check :  l_forearm_roll,  r_shoulder_lift
  Check :  l_forearm,  r_shoulder_lift
  Check :  l_wrist_flex,  r_shoulder_lift
  Check :  l_wrist_roll,  r_shoulder_lift
  
  Check :  l_shoulder_pan, r_upper_arm
  Check :  l_shoulder_lift, r_upper_arm
  Check :  l_upper_arm_roll, r_upper_arm
  Check :  l_upper_arm, r_upper_arm
  Check :  l_elbow_flex, r_upper_arm
  Check :  l_forearm_roll, r_upper_arm
  Check :  l_forearm, r_upper_arm
  Check :  l_wrist_flex, r_upper_arm
  Check :  l_wrist_roll, r_upper_arm
  
  Check :  l_shoulder_pan,  r_elbow_flex
  Check :  l_shoulder_lift,  r_elbow_flex
  Check :  l_upper_arm_roll,  r_elbow_flex
  Check :  l_upper_arm,  r_elbow_flex
  Check :  l_elbow_flex,  r_elbow_flex
  Check :  l_forearm_roll,  r_elbow_flex
  Check :  l_forearm,  r_elbow_flex
  Check :  l_wrist_flex,  r_elbow_flex
  Check :  l_wrist_roll,  r_elbow_flex
  
  Check :  l_shoulder_pan,  r_forearm_roll
  Check :  l_shoulder_lift,  r_forearm_roll
  Check :  l_upper_arm_roll,  r_forearm_roll
  Check :  l_upper_arm,  r_forearm_roll
  Check :  l_elbow_flex,  r_forearm_roll
  Check :  l_forearm_roll,  r_forearm_roll
  Check :  l_forearm,  r_forearm_roll
  Check :  l_wrist_flex,  r_forearm_roll
  Check :  l_wrist_roll,  r_forearm_roll
  
  Check :  l_shoulder_pan, r_forearm
  Check :  l_shoulder_lift, r_forearm
  Check :  l_upper_arm_roll, r_forearm
  Check :  l_upper_arm, r_forearm
  Check :  l_elbow_flex, r_forearm
  Check :  l_forearm_roll, r_forearm
  Check :  l_forearm, r_forearm
  Check :  l_wrist_flex, r_forearm
  Check :  l_wrist_roll, r_forearm
  
  Check :  l_shoulder_pan, r_wrist_flex
  Check :  l_shoulder_lift, r_wrist_flex
  Check :  l_upper_arm_roll, r_wrist_flex
  Check :  l_upper_arm, r_wrist_flex
  Check :  l_elbow_flex, r_wrist_flex
  Check :  l_forearm_roll, r_wrist_flex
  Check :  l_forearm, r_wrist_flex
  Check :  l_wrist_flex, r_wrist_flex
  Check :  l_wrist_roll, r_wrist_flex
  
  Check :  l_shoulder_pan,  r_wrist_roll
  Check :  l_shoulder_lift,  r_wrist_roll
  Check :  l_upper_arm_roll,  r_wrist_roll
  Check :  l_upper_arm,  r_wrist_roll
  Check :  l_elbow_flex,  r_wrist_roll
  Check :  l_forearm_roll,  r_wrist_roll
  Check :  l_forearm,  r_wrist_roll
  Check :  l_wrist_flex,  r_wrist_roll
  Check :  l_wrist_roll,  r_wrist_roll
  
  Check :  l_shoulder_pan, pitcher 
  Check :  l_shoulder_lift, pitcher
  Check :  l_upper_arm_roll, pitcher
  Check :  l_upper_arm, pitcher
  Check :  l_elbow_flex, pitcher
  Check :  l_forearm_roll, pitcher
  Check :  l_forearm, pitcher
  Check :  l_wrist_flex, pitcher
  Check :  l_wrist_roll, pitcher
  
  Check :  l_shoulder_pan, fridge_base
  Check :  l_shoulder_lift, fridge_base
  Check :  l_upper_arm_roll, fridge_base
  Check :  l_upper_arm, fridge_base
  Check :  l_elbow_flex, fridge_base
  Check :  l_forearm_roll, fridge_base
  Check :  l_forearm, fridge_base
  Check :  l_wrist_flex, fridge_base
  Check :  l_wrist_roll, fridge_base
  Check :  l_gripper_palm, fridge_base
  Check :  l_gripper_l_finger, fridge_base
  Check :  l_gripper_l_finger_tip, fridge_base
  Check :  l_gripper_r_finger, fridge_base
  Check :  l_gripper_r_finger_tip, fridge_base
  
  Check :  r_shoulder_pan,  pitcher 
  Check :  r_shoulder_lift,  pitcher 
  Check :  r_upper_arm_roll,  pitcher 
  Check :  r_upper_arm,  pitcher 
  Check :  r_elbow_flex,  pitcher 
  Check :  r_forearm_roll,  pitcher 
  Check :  r_forearm,  pitcher 
  Check :  r_wrist_flex,  pitcher 
  Check :  r_wrist_roll ,  pitcher 
  Check :  r_gripper_palm,  pitcher 
  Check :  r_gripper_l_finger,  pitcher 
  Check :  r_gripper_l_finger_tip,  pitcher 
  Check :  r_gripper_r_finger,  pitcher 
  Check :  r_gripper_r_finger_tip,  pitcher 
  
  Check : head_pan, pitcher
  Check : head_tilt, pitcher