def move_group_python_interface_tutorial():

  ## First initialize moveit_commander and rospy.
  print "============ Starting tutorial setup"
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('move_group_python_interface_tutorial',
                  anonymous=True)

  ## Instantiate a RobotCommander object.  This object is an interface to
  ## the robot as a whole.
  robot = moveit_commander.RobotCommander()

  ## Instantiate a PlanningSceneInterface object.  This object is an interface
  ## to the world surrounding the robot.
  scene = moveit_commander.PlanningSceneInterface()

  ## Instantiate a MoveGroupCommander object.  This object is an interface
  ## to one group of joints.  In this case the group is the joints in the left
  ## arm.  This interface can be used to plan and execute motions on the left
  ## arm.
  group = moveit_commander.MoveGroupCommander("arm")

  ## Sometimes for debugging it is useful to print the entire state of the

  print "============ Printing robot end-effector pose"
  #print group.get_current_pose()
  print group.get_current_pose().pose.position.x, group.get_current_pose().pose.position.y, group.get_current_pose().pose.position.z
  print group.get_current_rpy()
  ## When finished shut down moveit_commander.
  moveit_commander.roscpp_shutdown()

def init():
  global robot
  global scene
  global left_arm
  global right_arm
  global left_gripper
  global right_gripper

  robot = None
  scene = None
  left_arm = None
  right_arm = None
  left_gripper = None
  right_gripper = None
  gc.collect()

  #Initialize moveit_commander
  moveit_commander.roscpp_initialize(sys.argv)

  #Initialize both arms
  robot = moveit_commander.RobotCommander()
  scene = moveit_commander.PlanningSceneInterface()
  left_arm = moveit_commander.MoveGroupCommander('left_arm')
  right_arm = moveit_commander.MoveGroupCommander('right_arm')
  left_arm.set_planner_id('RRTConnectkConfigDefault')
  left_arm.set_planning_time(20)
  right_arm.set_planner_id('RRTConnectkConfigDefault')
  right_arm.set_planning_time(20)

	def __init__(self):
		## First initialize moveit_commander and rospy.
		moveit_commander.roscpp_initialize(sys.argv)
		rospy.init_node('move_piece_interface',
				anonymous=True)

		## Instantiate a RobotCommander object.  This object is an interface to
		## the robot as a whole.
		self.robot = moveit_commander.RobotCommander()

		## Instantiate a PlanningSceneInterface object.  This object is an interface
		## to the world surrounding the robot.
		self.scene = moveit_commander.PlanningSceneInterface()

		## Instantiate a MoveGroupCommander object.  This object is an interface
		## to one group of joints.  In this case the group is the joints in the left
		## arm.  This interface can be used to plan and execute motions on the left
		## arm.
		self.left_group = moveit_commander.MoveGroupCommander("left_arm")
		self.right_group = moveit_commander.MoveGroupCommander("right_arm")
		#self.listener = tf.TransformListener()
		#self.listener.waitForTransform("/base", 'left_gripper', rospy.Time(0), rospy.Duration(3.0));
		#self._limb = baxter_interface.Limb('left')
		self._left_gripper = baxter_interface.Gripper('left')
		self._right_gripper = baxter_interface.Gripper('right')
		self.gripper_close()

    def __init__(self):
        smach.State.__init__(self, outcomes=['done'],
                                   input_keys=['goal_pose'])
        
        moveit_commander.roscpp_initialize(sys.argv)

        ## Instantiate a RobotCommander object.  This object is an interface to
        ## the robot as a whole.
        self.robot = moveit_commander.RobotCommander()

        ## Instantiate a PlanningSceneInterface object.  This object is an interface
        ## to the world surrounding the robot.
        rospy.loginfo('Instantiate PlanningSceneInterface')
        self.scene = moveit_commander.PlanningSceneInterface()

        ## Instantiate a MoveGroupCommander object.  This object is an interface
        ## to one group of joints.  In this case the group is the joints in the left
        ## arm.  This interface can be used to plan and execute motions on the left
        ## arm.
        rospy.loginfo('Instantiate MoveGroupCommander')
        self.group = moveit_commander.MoveGroupCommander("manipulator")


        ## We create this DisplayTrajectory publisher which is used below to publish
        ## trajectories for RVIZ to visualize.
        rospy.loginfo('Create display_trajectory_publisher')
        self.display_trajectory_publisher = rospy.Publisher(
                                            '/move_group/display_planned_path',
                                            moveit_msgs.msg.DisplayTrajectory)

def move_robot():
	print "Starting rospy and moveit..."
	moveit_commander.roscpp_initialize(sys.argv)
	rospy.init_node('move_group_py')

	robot = moveit_commander.RobotCommander()
	scene = moveit_commander.PlanningSceneInterface()
	group1 = moveit_commander.MoveGroupCommander("both_arms")



	display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory, queue_size=1)
	group1.allow_replanning(True)
	group1.set_planning_time(10.0)


	print "Waiting for RViz"
	rospy.sleep(10)
	print "Initializing..."

	print "Reference frame: %s" %group1.get_planning_frame()

	print "Generating plan..."
	group_values = group1.get_current_joint_values()
	group_values[1] = 0
	group_values[2] = 0
	group1.set_joint_value_target(group_values)


	plan1 = group1.plan()
	rospy.sleep(5)

	print "Executing..."
	plan1 = group1.go()

    def __init__(self):
        self.state = "INITIALIZING"
        moveit_commander.roscpp_initialize(sys.argv)

        #Start a node
        rospy.init_node('moveit_node')
        
        #Initialize subscriber
        self.sub = rospy.Subscriber("game_state", String, self.state_machine)
        
        #Initialize both arms
        self.robot = moveit_commander.RobotCommander()
        self.scene = moveit_commander.PlanningSceneInterface()
        self.left_arm = moveit_commander.MoveGroupCommander('left_arm')
        self.right_arm = moveit_commander.MoveGroupCommander('right_arm')
        self.left_arm.set_planner_id('RRTConnectkConfigDefault')
        self.left_arm.set_planning_time(10)
        self.right_arm.set_planner_id('RRTConnectkConfigDefault')
        self.right_arm.set_planning_time(10)

        #Set up the grippers
        self.left_gripper = baxter_gripper.Gripper('left')
        self.right_gripper = baxter_gripper.Gripper('right')

        #Calibrate the gripper (other commands won't work unless you do this first)
        print('Calibrating...')
        self.right_gripper.calibrate()
        rospy.sleep(0.5)

        #Initialize class variables
        self.all_pieces = [0,1,2,3,4,5,6,7,8,9,13,14,15,16,20]
        self.available_pieces = self.all_pieces

        self.state = "PLAY"

def starting_pose():
	rospy.sleep(20)
	moveit_commander.roscpp_initialize(sys.argv)
	rospy.init_node('move_to_starting_pose',
		              anonymous=True)
		              
	robot = moveit_commander.RobotCommander()

	scene = moveit_commander.PlanningSceneInterface()

	group = moveit_commander.MoveGroupCommander("arm_gp")

	display_trajectory_publisher = rospy.Publisher(
		                                  '/move_group/display_planned_path',
		                                  moveit_msgs.msg.DisplayTrajectory)
		                                  
	print "============ Waiting for RVIZ..."
	
	
	pose_target = geometry_msgs.msg.Pose()
	pose_target.orientation.x = 0.0
	pose_target.orientation.y = -1.0
	pose_target.orientation.z = 0.0
	pose_target.orientation.w = 0.0
	pose_target.position.x = 0.3
	pose_target.position.y = 0.0
	pose_target.position.z = 0.38
	group.set_pose_target(pose_target)
	
	group.go(wait=True)
	
	## When finished shut down moveit_commander.
	moveit_commander.roscpp_shutdown()

def move_arm():
    print "Starting tutorial setup"
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_arm_node')
    print "move_arm_node should be initialized"
    print "Initializing Robot Commander"
    robot = moveit_commander.RobotCommander()
    print "Initializing Planning Scene Interface"
    scene = moveit_commander.PlanningSceneInterface()
    print "Let's move left arm first"
    group = moveit_commander.MoveGroupCommander("left_arm")
    
    print "Create publisher to display_planned_path"
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
    
    robot_pose = geometry_msgs.msg.Pose()
    robot_pose.orientation.x = 1

    robot_pose.position.x = 0.7
    robot_pose.position.y = 0.4
    robot_pose.position.z = 0.3
    
    group.set_pose_target(robot_pose)
    plan1 = group.plan()
    rospy.sleep(5)
    group.go(wait=True)
    
    moveit_commander.roscpp_shutdown()

    print "Finishing"

    def setUp(self):
        # create a action client of move group
        self._move_client = SimpleActionClient('move_group', MoveGroupAction)
        self._move_client.wait_for_server()

        moveit_commander.roscpp_initialize(sys.argv)
        group_name = moveit_commander.RobotCommander().get_group_names()[0]
        group = moveit_commander.MoveGroupCommander(group_name)

        # prepare a joint goal
        self._goal = MoveGroupGoal()
        self._goal.request.group_name = group_name
        self._goal.request.max_velocity_scaling_factor = 0.1
        self._goal.request.max_acceleration_scaling_factor = 0.1
        self._goal.request.start_state.is_diff = True

        goal_constraint = Constraints()
        joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
        joint_names = group.get_active_joints()
        for i in range(len(joint_names)):
            joint_constraint = JointConstraint()
            joint_constraint.joint_name = joint_names[i]
            joint_constraint.position = joint_values[i]
            joint_constraint.weight = 1.0
            goal_constraint.joint_constraints.append(joint_constraint)

        self._goal.request.goal_constraints.append(goal_constraint)
        self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True

 def __init__(self):
     # Initialize the move_group API
     moveit_commander.roscpp_initialize(sys.argv)
     
     rospy.init_node('moveit_demo')
             
     # Initialize the MoveIt! commander for the right arm
     arm = moveit_commander.MoveGroupCommander('arm_with_torso')
     
     arm.set_end_effector_link('wrist_roll_link')
     
     # Get the name of the end-effector link
     end_effector_link = arm.get_end_effector_link()
                     
     # Set the reference frame for pose targets
     reference_frame = 'base_link'
     
     # Set the right arm reference frame accordingly
     arm.set_pose_reference_frame(reference_frame)
             
     # Allow replanning to increase the odds of a solution
     arm.allow_replanning(True)
     
     # Allow some leeway in position (meters) and orientation (radians)
     arm.set_goal_position_tolerance(0.01)
     arm.set_goal_orientation_tolerance(0.05)
            
     # Set the start pose.  This particular pose has the gripper oriented horizontally
     # 0.75 meters above the base and 0.72 meters in front of the robot.
     target_pose = PoseStamped()
     target_pose.header.frame_id = reference_frame
     target_pose.header.stamp = rospy.Time.now()     
     target_pose.pose.position.x = 0.609889
     target_pose.pose.position.y = 0.207002
     target_pose.pose.position.z = 1.10677
     target_pose.pose.orientation.x = 0.0
     target_pose.pose.orientation.y = 0.0
     target_pose.pose.orientation.z = 0.0
     target_pose.pose.orientation.w = 1.0
     
     # Set the start state to the current state
     arm.set_start_state_to_current_state()
     
     # Set the goal pose of the end effector to the stored pose
     arm.set_pose_target(target_pose, end_effector_link)
     
     # Plan the trajectory to the goal
     traj = arm.plan()
     
     # Execute the planned trajectory
     arm.execute(traj)
 
     # Pause for a couple of seconds
     rospy.sleep(2)
      
     # Shut down MoveIt cleanly
     moveit_commander.roscpp_shutdown()
     
     # Exit MoveIt
     moveit_commander.os._exit(0)

    def __init__(self):
        rospy.init_node("moveit_cartesian_path", anonymous=False)
        rospy.loginfo("Starting node moveit_cartesian_path")
        rospy.on_shutdown(self.cleanup)
        moveit_commander.roscpp_initialize(sys.argv)
        self._arm = moveit_commander.MoveGroupCommander('manipulator')
        rospy.loginfo("End effector: " + self._arm.get_end_effector_link())
        self._arm.set_pose_reference_frame("/base")

        # Allow replanning to increase the odds of a solution
        self._arm.allow_replanning(True)

        # Allow some leeway in position (meters) and orientation (radians)
        self._arm.set_goal_position_tolerance(0.01)
        self._arm.set_goal_orientation_tolerance(0.1)

        self._joint_order = ('shoulder_pan_joint',
                             'shoulder_lift_joint',
                             'elbow_joint',
                             'wrist_1_joint',
                             'wrist_2_joint',
                             'wrist_3_joint')
        self._joint_sub = rospy.Subscriber("/joint_states", JointState, self.cb_js)
        self._last_check = 0
        self._tcp_location = None
        self._ur_kin = ur5()

def InitializeMoveItCommander():
    moveit_commander.roscpp_initialize(sys.argv)
    # Instantiate a RobotCommander object.  This object is an interface to
    # the robot as a whole.
    robot = moveit_commander.RobotCommander()
    rospy.sleep(1)

    ## Instantiate a PlanningSceneInterface object.  This object is an interface
    ## to the world surrounding the robot.
    global scene
    scene = moveit_commander.PlanningSceneInterface()
    rospy.sleep(1)

    ## Instantiate a MoveGroupCommander object.  This object is an interface
    ## to one group of joints.  In this case the group is the joints in the left
    ## arm.  This interface can be used to plan and execute motions on the left
    ## arm.
    global group
    group = MoveGroupCommander("left_arm")
    
    global left_gripper
    left_gripper = baxter_interface.Gripper('left')
    left_gripper.calibrate()

    rospy.sleep(2)

def launch_script():
    print "====== l_leg_inverse_ik.py started ========"
    print "======= Start initializing move commander for l_leg_inverse ==========="
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('l_leg_inverse_ik', anonymous=True)


    lleg_group = moveit_commander.MoveGroupCommander("l_leg_inverse")
    lleg_group.set_planner_id("RRTConnectkConfigDefault")

    print "\n =========== Some basic informations of Move Commander=================="
    print "Current Pose of end effector %s %s" %(lleg_group.get_end_effector_link(), lleg_group.get_current_pose())
    lleg_group.set_end_effector_link("pelvis")
    print "The end effector is: %s" %lleg_group.get_end_effector_link()
    current_pose = lleg_group.get_current_pose()

    random_target =    lleg_group.get_random_pose()
    print "==============================================================="
    print "Random pose set: %s" %random_target
    lleg_group.set_pose_target(random_target)
    print "==============================================================="
    print "Planning"
    lleg_group.plan()
    print "==============================================================="
    print "Running"
    lleg_group.go(wait=True)

    print "==============================================================="

    def followCartTrajMoveit(self, x_vec, dt):

        moveit_commander.roscpp_initialize(sys.argv)
        group = moveit_commander.MoveGroupCommander("left_arm")

        waypoints = []
        waypoints.append(group.get_current_pose().pose)
        

        for i in range(len(x_vec)): 
        
            #Make sure quaternion is normalized
            x_vec[i][3:7] = x_vec[i][3:7] / la.norm(x_vec[i][3:7])

            wpose = geometry_msgs.msg.Pose()
            wpose.position.x = x_vec[i][0]
            wpose.position.y = x_vec[i][1]
            wpose.position.z = x_vec[i][2]
            wpose.orientation.x = x_vec[i][3]
            wpose.orientation.y = x_vec[i][4]
            wpose.orientation.z = x_vec[i][5]
            wpose.orientation.w = x_vec[i][6]
            waypoints.append(copy.deepcopy(wpose))

        (plan, fraction) = group.compute_cartesian_path(
                             waypoints,   # waypoints to follow
                             dt,          # eef_step
                             0.0)         # jump_threshold
        print "fraction: ", fraction
        print "plan: ", plan
        group.execute(plan)        

def main():
    global left_arm
    global left_gripper
    #Initialize moveit_commander
    moveit_commander.roscpp_initialize(sys.argv)

    #Start a node
    rospy.init_node('moveit_interface')

    #Set up Baxter Arms
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    left_arm = moveit_commander.MoveGroupCommander('left_arm')
    left_arm.set_planner_id('RRTConnectkConfigDefault')
    left_arm.set_planning_time(10)

    #Calibrate the end effector on the left arm
    left_gripper = baxter_gripper.Gripper('left')
    print('Calibrating...')
    left_gripper.calibrate()
    rospy.sleep(2.0)
    print('Ready to go')


    #make subscriber to position topic
    rospy.Subscriber('new_position', Pose, move_arm)

    #make subscriber to gripper topic
    rospy.Subscriber('gripper_control', Bool, actuate_gripper)

    rospy.spin()

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('listener', anonymous=True)
    
    global robot
    global scene
    global arm
    global gripper
    
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    arm = moveit_commander.MoveGroupCommander("arm")
    gripper = moveit_commander.MoveGroupCommander("gripper")

    rospy.Subscriber("/pick_pose", Point, callback)
    # sub = rospy.Subscriber("/clicked_point", geometry_msgs.msg.PoseStamped, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

    def __init__(self):
        rospy.loginfo("Initializing ChipBehaviour")

        moveit_commander.roscpp_initialize(sys.argv)

        robot = moveit_commander.RobotCommander()

        scene = moveit_commander.PlanningSceneInterface()

        self.group = moveit_commander.MoveGroupCommander("right_arm_torso")

        self.pose_pub = rospy.Publisher('/arm_pointing_pose',
                                        geometry_msgs.msg.PoseStamped,
                                        queue_size=1)

        self.tts_ac = SimpleActionClient('/tts', TtsAction)
        self.tts_ac.wait_for_server()

        self.point_head_ac = SimpleActionClient('/head_controller/point_head_action',
                                                PointHeadAction)
        self.point_head_ac.wait_for_server(rospy.Duration(10.0))

        self.faces_sub = rospy.Subscriber('/pal_face/faces',
                                          FaceDetections,
                                          self.faces_cb,
                                          queue_size=1)

    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('subtask_grasp')
        rospy.on_shutdown(self.shutdown)

        self.grasp_srv = rospy.Service('subtask_grasp', rp_grasp, self.graspSrvCallback)

    def __init__(self):
        # rospy.sleep(10) # wait for other files to be launched successfully

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cw3_q1')

        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        # self.touch = [0,0,0,0]

        # rospy.Subscriber("/contacts/lh_ff/distal", ContactsState, self.call_ff)
        # rospy.Subscriber("/contacts/lh_mf/distal", ContactsState, self.call_mf)
        # rospy.Subscriber("/contacts/lh_rf/distal", ContactsState, self.call_rf)
        # rospy.Subscriber("/contacts/lh_th/distal", ContactsState, self.call_th)
        # # Robot contains the entire state of the robot (iiwa and shadow hand)
        # robot = moveit_commander.RobotCommander()
        # # We can get a list of all the groups in the robot
        # print('============ Robot Groups:')
        # print('{}\n\n'.format(robot.get_group_names()))

        self.iiwa_group = moveit_commander.MoveGroupCommander('hand_iiwa')
        self.hand_group = moveit_commander.MoveGroupCommander('sr_hand')

        rospy.sleep(1)

        # object tracking
        [all_object_name,all_object_pose] = self.get_object_position()

        # create a service proxy for add and remove objects to allowed collision matrix
        add_to_acm = rospy.ServiceProxy('/add_object_acm', ChangeCollisionObject)
        remove_from_acm = rospy.ServiceProxy('/remove_object_acm', ChangeCollisionObject)

        # open the hand
        self.hand_open()

        for i in range(0,3):
            # i = 2 # object no.

            # move the iiwa to just above the object
            self.move_iiwa_to_object(all_object_pose,i)

            # add object to allowed collision matrix
            add_to_acm(all_object_name[i])

            # close the hand (grasping)
            self.hand_close()

            # hold the grasping position for 10 seconds
            rospy.sleep(10)

            # open the hand (releasing)
            self.hand_open()

            # remove object from allowed collision matrix
            remove_from_acm(all_object_name[i])

        moveit_commander.roscpp_shutdown()
        rospy.loginfo("Task finished")

def move_group_python_interface_tutorial():
    global group_left
    global group_right
    global p
    print "============ Starting tutorial setup"
    moveit_commander.roscpp_initialize(sys.argv)
    #rospy.init_node('move_group_python_interface_tutorial',anonymous=True)


    ## Instantiate a RobotCommander object.  Interface to the robot.
    robot = moveit_commander.RobotCommander()
    ## Instantiate a PlanningSceneInterface object.  Interface to the world.
    scene = moveit_commander.PlanningSceneInterface()
    ## Instantiate a MoveGroupCommander object.  Interface to groups of joints.
    group_left = moveit_commander.MoveGroupCommander("left_arm")
    #group_right = moveit_commander.MoveGroupCommander("right_arm")
    ## We create this DisplayTrajectory publisher which is used below to publish
    ## trajectories for RVIZ to visualize.
    display_trajectory_publisher = rospy.Publisher(
                                      '/move_group/display_planned_path',
                                      moveit_msgs.msg.DisplayTrajectory,
                                      queue_size=20)
    ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
    print "============ Waiting for RVIZ..."
    #rospy.sleep(2)
    print "============ Starting tutorial "
    ## We can get the name of the reference frame for this robot
    print "============ Reference frame: %s" % group_left.get_planning_frame()
    ## We can also print the name of the end-effector link for this group
    print "============ End effector: %s" % group_left.get_end_effector_link()
    ## We can get a list of all the groups in the robot
    print "============ Robot Groups:"
    print robot.get_group_names()
    ## Sometimes for debugging it is useful to print the entire state of the
    ## robot.
    print "============ Printing robot state"
    print robot.get_current_state()
    print "============"
    p = PlanningSceneInterface("base_link")
    p.addBox("table",0.2 ,1.25 ,0.55 ,0.65 ,0.775 ,0.275)
    p.addCylinder("redCylinder",0.3,0.03,0.65,0.7,0.7)
    p.addCylinder("blueCylinder",0.3,0.03,0.65,0.8,0.7)
    p.addCylinder("greenCylinder",0.3,0.03,0.65,0.9,0.7)
    p.addBox("obstacle1_1", 0.6, 0.2, 0.4, 0.65, 0.45, 0.95)
    p.addBox("obstacle1_2", 0.45, 0.01, 0.50, 0.3, 0.4, 0.3)
    print group_left.get_planning_time()
    group_left.set_planning_time(15)
    print group_left.get_planning_time()
    group_left.set_num_planning_attempts(20)
    '''p.addBox("obstacle1_1", 0.15, 0.1, 0.3, 0.6, 0.4, 0.7)#llega hasta 0.85
    p.addBox("obstacle1_2", 0.25, 0.1, 0.70, 0.35, 0.4, 0.35)
    #p.addBox("obstacle1_3", 0.75, 0.8, 0.3, 0.6, 0.4, 1.32)
    p.addBox("obstacle1_3", 0.75, 0.8, 0.1, 0.6, 0.4, 1.18)
    #p.addBox("obstacle1_3_1", 0.3, 0.1, 0.04, 0.225, 0.4, 1.09)
    p.addBox("obstacle1_1_bis", 0.3, 0.1, 0.4, 0.6, 0.00, 0.7)'''
    #p.addBox("obstacle1_2_bis", 0.25, 0.1, 0.1, 0.35, 0.4, 1.12)
    #p.addBox("obstacle1_11", 0.2, 0.1, 0.65, 0.6, 0.4, 1.175)
    #p.addBox("obstacle1_21", 0.25, 0.1, 0.65, 0.35, 0.4, 1.275)
    '''#PRUEBA 1