def send_arm_goal(self, arm_goal):
        arm_joint_names = ['joint_lift',
                           'joint_waist',
                           'joint_big_arm',
                           'joint_forearm',
                           'joint_wrist_pitching',
                           'joint_wrist_rotation']

        rospy.loginfo("Waiting for follow_joint_trajectory server")
        self.arm_client.wait_for_server()
        rospy.loginfo("Connected to follow_joint_trajectory server")
        rospy.sleep(1)
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joint_names
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].time_from_start = rospy.Duration(10)
        rospy.loginfo("Preparing for moving the arm to goal position!")
        rospy.sleep(1)
        arm_goal_pos = FollowJointTrajectoryGoal()
        arm_goal_pos.trajectory = arm_trajectory

        arm_goal_pos.goal_time_tolerance = rospy.Duration(0)
        self.arm_client.send_goal(arm_goal_pos)
        rospy.loginfo("Send goal to the trajectory server successfully!")
        self.arm_client.wait_for_result()
Пример #2
0
    def move_to(self, result_trajectory, duration=5.0):
        trajectory = result_trajectory
        follow_goal = FollowJointTrajectoryGoal()
        follow_goal.trajectory = trajectory

        self.client.send_goal(follow_goal)
        self.client.wait_for_result()
    def __init__(self):
        rospy.init_node('arm_simple_trajectory')
        
        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)
        
        # Which joints define the arm?
        arm_joints = ['joint_lift',
                      'joint_waist',
                      'joint_big_arm', 
                      'joint_forearm',
                      'joint_wrist_pitching',
                      'joint_wrist_rotation']
        
        if reset:
            # Set the arm back to the resting position
            arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        else:
            # Set a goal configuration for the arm
            arm_goal = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0]
    
        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for xm arm trajectory controller...')
        
        arm_client = actionlib.SimpleActionClient('xm_arm_controller/follow_joint_trajectory',
                                                  FollowJointTrajectoryAction)
        
        arm_client.wait_for_server()
        
        rospy.loginfo('...connected.')
        rospy.sleep(1)
    
        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3)
    
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')
        rospy.sleep(1)
        
        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()
        
        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory
        
        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0)
    
        # Send the goal to the action server
        arm_client.send_goal(arm_goal)

        rospy.loginfo('...done')
        rospy.sleep(1)
Пример #4
0
 def move_head(self, pan , tilt):   
     # Which joints define the head?
     head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
     # Create a single-point head trajectory with the head_goal as the end-point
     head_trajectory = JointTrajectory()
     head_trajectory.joint_names = head_joints
     head_trajectory.points.append(JointTrajectoryPoint())
     head_trajectory.points[0].positions = pan , tilt
     head_trajectory.points[0].velocities = [0.0 for i in head_joints]
     head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
     head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
 
     # Send the trajectory to the head action server
     rospy.loginfo('Moving the head to goal position...')
     
     head_goal = FollowJointTrajectoryGoal()
     head_goal.trajectory = head_trajectory
     head_goal.goal_time_tolerance = rospy.Duration(0.0)
 
     # Send the goal
     self.head_client.send_goal(head_goal)
     
     # Wait for up to 5 seconds for the motion to complete 
     self.head_client.wait_for_result(rospy.Duration(2.0))
     
     rospy.loginfo('...done')
Пример #5
0
 def execute_trajectory(self, trajectory, duration=5.0):
     follow_goal = FollowJointTrajectoryGoal()
     follow_goal.trajectory = trajectory
     self.client.send_goal(follow_goal)
     #rospy.sleep(1.5)
     #self.client.cancel_goal()
     self.client.wait_for_result()
     self.finish_time = rospy.Time.now() 
     self.believed_state = trajectory.points[-1].positions
Пример #6
0
    def __init__(self):
        rospy.init_node('trajectory_demo')
        
        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)
        
        # Which joints define the arm?
        arm_joints = ['link_1_joint',
                      'servo_2_joint',
                      'link_3_joint', 
                      'link_4_joint',
                      'link_5_joint']
        
        if reset:
            # Set the arm back to the resting position
            arm_goal  = [0, 0, 0, 0, 0]
  
        else:
            # Set a goal configuration for the arm
            arm_goal  = [-0.525487, 0.904972, 0.480017, -1.331166, 0.731413]

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')
        
        arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        
        arm_client.wait_for_server()
        
        rospy.loginfo('...connected.')
        
    
        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
    
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')
        
        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()
        
        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory
        
        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    
        # Send the goal to the action server
        arm_client.send_goal(arm_goal)
                
        rospy.loginfo('...done')
Пример #7
0
 def move_to(self, positions, duration=5.0):
     print "============= Moving"
     trajectory = JointTrajectory()
     trajectory.joint_names = self.joint_names
     trajectory.points.append(JointTrajectoryPoint())
     trajectory.points[0].positions = positions
     trajectory.points[0].velocities = [0.0 for _ in positions]
     trajectory.points[0].accelerations = [0.0 for _ in positions]
     trajectory.points[0].time_from_start = rospy.Duration(duration)
     follow_goal = FollowJointTrajectoryGoal()
     follow_goal.trajectory = trajectory
Пример #8
0
 def build_follow_trajectory(self, traj):
     # Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
     traj_goal = FollowJointTrajectoryGoal()
     traj_goal.trajectory = traj
     tolerance = JointTolerance()
     tolerance.position = 0.05
     tolerance.velocity = 0.1
     traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
     traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
     traj_goal.goal_time_tolerance = rospy.Duration(3)
     return traj_goal
Пример #9
0
    def btnPlay(self):
        if not self.ui.chkAllMotors.isChecked():
            print("all motors should have torque")
            return
        if self.ui.spinSetFrame.value()>self.ui.spinPlayTo.value() or self.ui.spinSetFrame.value()<1 or self.ui.spinPlayTo.value()>self.totalFrames:
            print("\nwrong range in play\n")
            return

        trajectories=[]
        for cat in self.motorTrajectoryTopics:
            jointTraj=JointTrajectory()
            jts_names=[]
            for name in self.names:
                #if self.motorNamesAndCat[name]==cat and name!='l_hip_yaw':
                if self.motorNamesAndCat[name]==cat:
                    jointTraj.joint_names.append(name+"_joint")
                    jts_names.append(name)

            fcount=self.ui.spinPlayTo.value()-self.ui.spinSetFrame.value()+1
            totalDelay=0.0
            for n in range(0,fcount):
                pos_arr=JointTrajectoryPoint()
                readLoc=n+self.ui.spinSetFrame.value()
                delaySecs=float(self.TimeDelayFromPrevious[readLoc-1])/1000.0
                totalDelay=totalDelay+delaySecs
                pos_arr.time_from_start=rospy.Duration().from_sec(totalDelay)
                for name in jts_names:
                    pos_arr.positions.append(self.TrajectoryInfo[name][readLoc-1])
                    print("\n "+name+": "+str(self.TrajectoryInfo[name][readLoc-1]))
                    velocity=1.0#2*pi rad/sec
                    if n!=0:
                        velocity=float(self.TrajectoryInfo[name][readLoc-1]-self.TrajectoryInfo[name][readLoc-2])/delaySecs
                    ##pos_arr.velocities.append(velocity)
                jointTraj.points.append(pos_arr)
            jointTraj.header.stamp=rospy.Time.now()
            trajectories.append(jointTraj)

        trajClients=[]
        mm=0
        for cat in self.motorTrajectoryTopics:
            trajClients.append(actionlib.SimpleActionClient(self.motorTrajectoryTopics[cat],FollowJointTrajectoryAction))
            trajClients[mm].wait_for_server()
            mm=mm+1
            #trajectories[nn] .. how to send this goal

        nn=0
        for traj in trajClients:
            goal=FollowJointTrajectoryGoal()
            #goal.trajectory.points=trajectories[nn].points
            goal.trajectory=trajectories[nn]
            traj.send_goal(goal)
            nn=nn+1

        print ("\nDone Sending Play Info...\n")
Пример #10
0
    def execute(self, trajectory, test=None, epsilon=0.1):
        """
        Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
        This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
        Non-blocking needs should deal with the JointTrajectory action server
        :param trajectory: either a RobotTrajectory or a JointTrajectory
        :param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
        :param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
        :return: True if execution ended successfully, False otherwise
        """
        def distance_to_first_point(point):
            joint_pos = np.array(point.positions)
            return np.linalg.norm(current_array - joint_pos)

        self.display(trajectory)
        with self._stop_lock:
            self._stop_reason = ''
        if isinstance(trajectory, RobotTrajectory):
            trajectory = trajectory.joint_trajectory
        elif not isinstance(trajectory, JointTrajectory):
            raise TypeError("Execute only accept RobotTrajectory or JointTrajectory")
        ftg = FollowJointTrajectoryGoal()
        ftg.trajectory = trajectory

        # check if it is necessary to move to the first point
        current = self.get_current_state()
        current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names])

        if distance_to_first_point(trajectory.points[0]) > epsilon:
            # convert first point to robot state
            rs = RobotState()
            rs.joint_state.name = trajectory.joint_names
            rs.joint_state.position = trajectory.points[0].positions
            # move to the first point
            self.move_to_controlled(rs)

        # execute the input trajectory
        self.client.send_goal(ftg)
        # Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory

        while self.client.simple_state != SimpleGoalState.DONE:
            if callable(test) and test():
                self.client.cancel_goal()
                return True

            if self._stop_reason!='':
                self.client.cancel_goal()
                return False

            self._rate.sleep()

        return True
    def __init__(self):
        rospy.init_node("gripper_trajectory_demo")

        # Set to True to move back to the starting configurations
        reset = rospy.get_param("~reset", False)

        # Set the duration of the trajectory in seconds
        duration = rospy.get_param("~duration", 2.0)

        # Which joints define the head?
        gripper_joints = ["right_gripper_finger_joint"]

        # Set a goal configuration for the head
        if reset:
            gripper_goal = [0]
        else:
            gripper_goal = [0.25]

        # Connect to the head trajectory action server
        rospy.loginfo("Waiting for gripper trajectory controller...")

        gripper_client = actionlib.SimpleActionClient(
            "right_gripper_controller/follow_joint_trajectory", FollowJointTrajectoryAction
        )

        gripper_client.wait_for_server()

        rospy.loginfo("...connected.")

        # Create a head trajectory with a single end point using the gripper_goal positions
        trajectory = JointTrajectory()
        trajectory.joint_names = gripper_joints
        trajectory.points.append(JointTrajectoryPoint())
        trajectory.points[0].positions = gripper_goal
        trajectory.points[0].velocities = [0.0 for i in gripper_joints]
        trajectory.points[0].accelerations = [0.0 for i in gripper_joints]
        trajectory.points[0].time_from_start = rospy.Duration(duration)

        # Send the trajectory to the head action server
        rospy.loginfo("Moving the gripper to goal position...")

        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        goal.goal_time_tolerance = rospy.Duration(0.0)

        # Send the goal
        gripper_client.send_goal(goal)

        # Wait for up to 5 seconds for the motion to complete
        gripper_client.wait_for_result(rospy.Duration(5.0))

        rospy.loginfo("...done")
Пример #12
0
    def move_to_start(self):

        self.moving_mode = True
        # Look down
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = '/base_link'
        goal.target.point.x = 1.5
        goal.target.point.y = 0.0
        goal.target.point.z = -0.2
        goal.min_duration = rospy.Duration(0.5)
        if 0: logger.info('Point head to %s...', goal);
        self.head_point_client.send_goal(goal)
        if 0: logger.info('Point head sent')

        goal = GripperCommandGoal()
        goal.command.max_effort = 60
        goal.command.position = 0.1
        self.gripper_client.send_goal(goal)

        joints = [
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
        ]
        pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
        result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
        if result.error_code.val == MoveItErrorCodes.SUCCESS:
            if 0: logger.info('Got trajectory %s', result.planned_trajectory)
            follow_goal = FollowJointTrajectoryGoal()
            follow_goal.trajectory = result.planned_trajectory.joint_trajectory
            logger.info('sending trajectory to arm...')
            self.arm_trajectory_client.send_goal(follow_goal)
            result = self.arm_trajectory_client.wait_for_result()
            logger.info('arm followed trajectory %s', result)
        else:
            logger.warn('moveToJointPosition returned %s', result)
            return

        result = self.head_point_client.wait_for_result()
        logger.info('head followed trajectory %s', result)

        logger.info('sending empty arm goal')
        empty_goal = FollowJointTrajectoryGoal()
        self.arm_trajectory_client.send_goal(empty_goal)

        logger.info('Point head done')
        self.moving_mode = False
Пример #13
0
def get_hand_goal(goal_positions):
    grasp_msg = FollowJointTrajectoryGoal()

    point1 = JointTrajectoryPoint(
        positions=goal_positions,
        velocities=[0.0, 0.0, 0.0],
        time_from_start = rospy.Duration(1))

    grasp_msg.trajectory = JointTrajectory(
        joint_names = ['hand_right_thumb_joint', 'hand_right_index_1_joint', 'hand_right_middle_1_joint'],
        points = [point1])

    return grasp_msg
Пример #14
0
  def execute_traj_moveit(self, waypoints):
    # Cycle through waypoints
    for point in waypoints:
      plannedTraj = self.arm_planner.plan_jointTargetInput(point)
      if plannedTraj == None or len(plannedTraj.joint_trajectory.points) < 1:
        print "Error: no plan found"
	return -1
      else:
        traj_goal = FollowJointTrajectoryGoal()
        traj_goal.trajectory = plannedTraj.joint_trajectory
        self.smooth_joint_trajectory_client.send_goal(traj_goal)
        self.smooth_joint_trajectory_client.wait_for_result()   
        self.smooth_joint_trajectory_client.get_result() 
	return 1
    def __init__(self):
        rospy.init_node('head_trajectory_demo')

        # Set to True to move back to the starting configurations    
        reset = rospy.get_param('~reset', False)
        
        # Set the duration of the trajectory in seconds
        duration = rospy.get_param('~duration', 3.0)
                
        # Which joints define the head?
        head_joints = ['head_pan_joint', 'head_tilt_joint']
        
        # Set a goal configuration for the head
        if reset:
            head_goal = [0, 0]
        else:
            head_goal = [-1.3, -0.3]
        
        # Connect to the head trajectory action server
        rospy.loginfo('Waiting for head trajectory controller...')
    
        head_client = actionlib.SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
       
        head_client.wait_for_server()
        
        rospy.loginfo('...connected.')    
        
        # Create a head trajectory with a single end point using the head_goal positions
        trajectory = JointTrajectory()
        trajectory.joint_names = head_joints
        trajectory.points.append(JointTrajectoryPoint())
        trajectory.points[0].positions = head_goal
        trajectory.points[0].velocities = [0.0 for i in head_joints]
        trajectory.points[0].accelerations = [0.0 for i in head_joints]
        trajectory.points[0].time_from_start = rospy.Duration(duration)
            
        # Send the trajectory to the head action server
        rospy.loginfo('Moving the head to goal position...')
        
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        goal.goal_time_tolerance = rospy.Duration(0.0)
    
        # Send the goal
        head_client.send_goal(goal)
        
        # Wait for up to 5 seconds for the motion to complete 
        head_client.wait_for_result(rospy.Duration(5.0))
        
        rospy.loginfo('...done')
	def on_enter(self, userdata):
		self._failed = False

		# create goal
		goal = FollowJointTrajectoryGoal()
		goal.trajectory = userdata.joint_trajectory
		goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)

		# execute the motion
		try: 
			self._client.send_goal(self._action_topic, goal)
		except Exception as e:
			Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e))
			self._failed = True
Пример #17
0
    def move_to(self, positions, duration=5.0):
        if len(self.joint_names) != len(positions):
            print("Invalid trajectory position")
            return False
        trajectory = JointTrajectory()
        trajectory.joint_names = self.joint_names
        trajectory.points.append(JointTrajectoryPoint())
        trajectory.points[0].positions = positions
        trajectory.points[0].velocities = [0.0 for _ in positions]
        trajectory.points[0].accelerations = [0.0 for _ in positions]
        trajectory.points[0].time_from_start = rospy.Duration(duration)
        follow_goal = FollowJointTrajectoryGoal()
        follow_goal.trajectory = trajectory

        self.client.send_goal(follow_goal)
        self.client.wait_for_result()
Пример #18
0
    def move_to_initial(self, positions, duration):    

        assert(len(self.joint_names) == len(positions))
        trajectory = JointTrajectory()
        trajectory.joint_names = self.joint_names

        p = JointTrajectoryPoint()
        p.positions = positions

        p.time_from_start = rospy.Duration(duration)
        trajectory.points.append(p)

        follow_goal = FollowJointTrajectoryGoal()
        follow_goal.trajectory = trajectory

        self.client.send_goal(follow_goal)
        self.client.wait_for_result()
Пример #19
0
    def __init__(self):
        rospy.init_node('trajectory_demo')

        # Which joints define the arm?
        arm_joints = ['x_joint']
        # Set the arm back to the resting position
        arm_goal  = [2]

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        arm_client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)

        arm_client.wait_for_server()

        rospy.loginfo('...connected.')

        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0]
        arm_trajectory.points[0].accelerations = [0.0]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)

        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')

        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)

        # Send the goal to the action server
        arm_client.send_goal(arm_goal)

        # Wait for up to 5 seconds for the motion to complete
        arm_client.wait_for_result(rospy.Duration(5.0))

        rospy.loginfo('...done')
Пример #20
0
    def move_to(self, positions, velocities, accelerations, delta_t):
        assert(len(self.joint_names) == positions.shape[1])
        trajectory = JointTrajectory()
        trajectory.joint_names = self.joint_names

        for i in range(positions.shape[0]): # for each row i
            p = JointTrajectoryPoint()
            p.positions = list(positions[i])
            p.velocities = list(velocities[i])
            p.accelerations = list(accelerations[i])
            p.time_from_start = rospy.Duration(i * delta_t)
            trajectory.points.append(p)


        follow_goal = FollowJointTrajectoryGoal()
        follow_goal.trajectory = trajectory

        self.client.send_goal(follow_goal)
        self.client.wait_for_result()
Пример #21
0
    def enviarTrayectoria(self, arm_goal1):
        # arm_goal = [0, 0, 0, 0, 0, 0]
        # arm_goal = [-0.3, 0.5, -1.0, 1.8, 0.3, 0.6]
        arm_joints = ['joint_1',
                      'joint_2',
                      'joint_3',
                      'joint_4',
                      'joint_5',
                      'joint_6']
        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        rospy.loginfo('...connected.')

        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal1
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(1.5)

        rospy.loginfo('Moving the arm to goal position...')

        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)

        # Send the goal to the action server
        self.arm_client.send_goal(arm_goal)

        # Wait for up to 5 seconds for the motion to complete
        self.arm_client.wait_for_result(rospy.Duration(5.0))
        self.actualizar_IK(arm_goal1)

        rospy.loginfo('...done')
    def execute(self, traj_msg):
        """ Execute a JointTrajectory message and return a TrajectoryFuture.

        @param  traj_msg: requested trajectory
        @type   traj_msg: trajectory_msgs.msg.JointTrajectory
        @return future representing the execution of the trajectory
        @rtype  TrajectoryFuture
        """
        from control_msgs.msg import FollowJointTrajectoryGoal

        goal_msg = FollowJointTrajectoryGoal()
        goal_msg.trajectory = traj_msg

        traj_future = TrajectoryFuture(traj_msg)
        traj_future._handle = self._client.send_goal(
            goal_msg,
            transition_cb=traj_future.on_transition,
            feedback_cb=traj_future.on_feedback
        )
        return traj_future
Пример #23
0
    def tuck_arm(self):
        while not self.state_recv:
            rospy.loginfo("Waiting for controllers to be up...")
            rospy.sleep(0.1)

        trajectory = JointTrajectory()
        trajectory.joint_names = self.joint_names
        trajectory.points.append(JointTrajectoryPoint())
        trajectory.points[0].positions = self.tucked
        trajectory.points[0].velocities = [0.0 for i in self.joint_names]
        trajectory.points[0].accelerations = [0.0 for i in self.joint_names]
        trajectory.points[0].time_from_start = rospy.Duration(5.0)

        rospy.loginfo("Tucking arm...")
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        goal.goal_time_tolerance = rospy.Duration(0.0)

        self.client.send_goal(goal)
        self.client.wait_for_result(rospy.Duration(6.0))
        rospy.loginfo("...done")
    def execute_cb(self, goal):
        """
        :type goal: FollowJointTrajectoryGoal
        """
        rospy.loginfo("Got a goal!")
        # Just resend the goal to the real as
        # while checking for cancel goals

        # Create goal
        g = FollowJointTrajectoryGoal()
        # We ignore path_tolerance and goal_tolerance... does not make sense in
        # a gripper
        g.goal_time_tolerance = goal.goal_time_tolerance
        jt = self.substitute_trajectory(goal.trajectory)
        g.trajectory = jt

        if not self._ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr("Action server for gripper not found!")
            self._as.set_aborted(FollowJointTrajectoryResult())

        # Send goal subscribing to feedback to republish it
        self._ac.send_goal(g, feedback_cb=self._feedback_cb)

        # wait for goal to finish while checking if cancel is requested
        while not self._ac.wait_for_result(rospy.Duration(0.1)):
            rospy.loginfo("Waiting for goal to finish...")
            # Deal with goal cancelled
            if self._as.is_preempt_requested():
                self._ac.cancel_all_goals()
                self._as.set_preempted()
                return

        result = self._ac.get_result()
        res = FollowJointTrajectoryResult()
        if result:
            res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
            self._as.set_succeeded(res)
        else:
            res.error_code = result
            self._as.set_aborted(res)
    def update(self):
        points = []
        tick = 0
        self.phase = np.random.uniform(np.pi)
        while tick <= self.time:
            x = 2*np.pi*self.freq*tick + self.phase
            positions = [np.sin(x)]*len(self.joint_names)
            velocities = [np.cos(x)]*len(self.joint_names)
            accelerations = [-np.sin(x)]*len(self.joint_names)
            points.append(JointTrajectoryPoint(positions=positions,
                                               velocities=velocities,
                                               accelerations=accelerations,
                                               time_from_start=rospy.Time.from_sec(tick)))
            tick += self.step
        msg = JointTrajectory(joint_names=self.joint_names, points=points)
        msg.header.stamp = rospy.Time.now()
        self.pub.publish(msg)

        # joint_trajectroy_action
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = msg
        self.client.send_goal(goal)
def move_to_joint(end_pos, duration):

  print end_pos
  traj = build_traj(get_cur_pos(), end_pos, duration)

  # wait for subscribers to connect
  # pub = rospy.Publisher('joint_path_command', JointTrajectory)
  # if not wait_for_subs(pub, 1, 0.5, 2.0):
  #   rospy.logwarn('Timeout while waiting for subscribers.  Publishing trajectory anyway.')
  # pub.publish(traj)

  _left_client = actionlib.SimpleActionClient(
            '/joint_trajectory_action',
            FollowJointTrajectoryAction,
        )

  print "Waiting..."
  _left_client.wait_for_server();
  print "Done waiting"

  goal = FollowJointTrajectoryGoal()
  goal.trajectory = traj
  goal.trajectory.header.stamp = rospy.Time.now()
  _left_client.send_goal(goal)

  print "Sent goal"


  rate = rospy.Rate(60) # hz
  counter = 0
  while counter < duration*70:
    rospy.is_shutdown()
    rate.sleep()
    counter = counter+1


  print "Received Result: " 
Пример #27
0
    def __init__(self):
        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        arm_joints = [
            'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint',
            'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint',
            'wrist_roll_joint'
        ]

        # Which joints define the head?
        head_joints = ['head_pan_joint', 'head_tilt_joint']

        # Which joint defines the torso?
        torso_joints = ['torso_lift_joint']

        if reset:
            # Set the arm back to the tucked position
            arm_goal = [-1.3901, 1.3439, -2.8327, -1.8119, 0.0, -1.6571, 0.0]

            # Re-center the head
            head_goal = [0.0, 0.0]

            # Bring the toros back down
            torso_goal = [0.0]
        else:
            # Set a goal configuration for the arm
            arm_goal = [-1.0, 0, 0, -1.0, 0, 0, 0]

            # Set a goal configuration for the head
            head_goal = [-0.85, -0.25]

            # Move the torso up
            torso_goal = [0.35]

        # Connect to the arm trajectory action server
        rospy.loginfo('Waiting for arm trajectory controller...')

        arm_client = actionlib.SimpleActionClient(
            'arm_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction)

        arm_client.wait_for_server()

        rospy.loginfo('...connected.')

        # Connect to the head trajectory action server
        rospy.loginfo('Waiting for head trajectory controller...')

        head_client = actionlib.SimpleActionClient(
            'head_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction)

        head_client.wait_for_server()

        rospy.loginfo('...connected.')

        # Connect to the torso trajectory action server
        rospy.loginfo('Waiting for headtorso trajectory controller...')

        torso_client = actionlib.SimpleActionClient(
            'torso_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction)

        torso_client.wait_for_server()

        rospy.loginfo('...connected.')

        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(5.0)

        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')

        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)

        # Send the goal to the action server
        arm_client.send_goal(arm_goal)

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            arm_client.wait_for_result(rospy.Duration(5.0))

        # Create a single-point head trajectory with the head_goal as the end-point
        head_trajectory = JointTrajectory()
        head_trajectory.joint_names = head_joints
        head_trajectory.points.append(JointTrajectoryPoint())
        head_trajectory.points[0].positions = head_goal
        head_trajectory.points[0].velocities = [0.0 for i in head_joints]
        head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
        head_trajectory.points[0].time_from_start = rospy.Duration(5.0)

        # Send the trajectory to the head action server
        rospy.loginfo('Moving the head to goal position...')

        head_goal = FollowJointTrajectoryGoal()
        head_goal.trajectory = head_trajectory
        head_goal.goal_time_tolerance = rospy.Duration(0.0)

        # Send the goal
        head_client.send_goal(head_goal)

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            head_client.wait_for_result(rospy.Duration(5.0))

        # Create a single-point torso trajectory with the torso_goal as the end-point
        torso_trajectory = JointTrajectory()
        torso_trajectory.joint_names = torso_joints
        torso_trajectory.points.append(JointTrajectoryPoint())
        torso_trajectory.points[0].positions = torso_goal
        torso_trajectory.points[0].velocities = [0.0 for i in torso_joints]
        torso_trajectory.points[0].accelerations = [0.0 for i in torso_joints]
        torso_trajectory.points[0].time_from_start = rospy.Duration(5.0)

        # Send the trajectory to the head action server
        rospy.loginfo('Moving the head to goal position...')

        torso_goal = FollowJointTrajectoryGoal()
        torso_goal.trajectory = torso_trajectory
        torso_goal.goal_time_tolerance = rospy.Duration(0.0)

        # Send the goal
        torso_client.send_goal(torso_goal)

        # Wait for up to 8 seconds for the motion to complete
        torso_client.wait_for_result(rospy.Duration(8.0))

        rospy.loginfo('...done')
Пример #28
0
    torso_client = actionlib.SimpleActionClient(
        "torso_controller/follow_joint_trajectory",
        FollowJointTrajectoryAction)
    torso_client.wait_for_server()
    rospy.loginfo("...connected.")

    trajectory = JointTrajectory()
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.joint_names = torso_joint_names
    trajectory.points[0].positions = torso_joint_positions
    trajectory.points[0].velocities = [0.0] * len(torso_joint_positions)
    trajectory.points[0].accelerations = [0.0] * len(torso_joint_positions)
    trajectory.points[0].time_from_start = rospy.Duration(5.0)

    torso_goal = FollowJointTrajectoryGoal()
    torso_goal.trajectory = trajectory
    torso_goal.goal_time_tolerance = rospy.Duration(0.0)

    torso_client.send_goal(torso_goal)
    torso_client.wait_for_result(rospy.Duration(5.0))
    """
    rospy.loginfo("Waiting for head_controller...")
    head_client = actionlib.SimpleActionClient("head_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    head_client.wait_for_server()
    rospy.loginfo("...connected.")

    rospy.loginfo("Waiting for arm_controller...")
    arm_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    arm_client.wait_for_server()
    rospy.loginfo("...connected.")
Пример #29
0
 def execute_plan_traj(self, plannedTraj):
     traj_goal = FollowJointTrajectoryGoal()
     traj_goal.trajectory = plannedTraj.joint_trajectory
     self.smooth_joint_trajectory_client.send_goal(traj_goal)
     self.smooth_joint_trajectory_client.wait_for_result()
     self.smooth_joint_trajectory_client.get_result()
Пример #30
0
 def follow_trajectory(self, trajectory):
     follow_goal = FollowJointTrajectoryGoal()
     follow_goal.trajectory = trajectory
     self.client.send_goal(follow_goal)
     self.client.wait_for_result()
def get_default_pose():
    arm_joint_names = [
        "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
        "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
        "wrist_roll_joint"
    ]
    # arm_intermediate_positions  = [1.32, 0, -1.4, 1.72, 0.0, 1.66, 0.0]
    arm_intermediate_positions = [0.0, -0.62, 0, 0, 0.0, 0.62, 0.0]
    # arm_joint_positions  = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
    # arm_joint_positions  = [1.1, -0.64, -1.83, 0.96, 1.13, -.96, 0.0]
    arm_joint_positions = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
    head_joint_names = ["head_pan_joint", "head_tilt_joint"]
    head_joint_positions = [0.0, 0.0]

    #rospy.init_node("prepare_simulated_robot_pick_place")

    # Check robot serial number, we never want to run this on a real robot!
    if rospy.get_param("robot/serial") != "ABCDEFGHIJKLMNOPQRSTUVWX":
        rospy.logerr("This script should not be run on a real robot")
        sys.exit(-1)

    # rospy.loginfo("Waiting for move_base...")
    # move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
    # move_base.wait_for_server()
    # rospy.loginfo("...connected.")

    rospy.loginfo("Waiting for head_controller...")
    head_client = actionlib.SimpleActionClient(
        "head_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    head_client.wait_for_server()
    rospy.loginfo("...connected.")

    rospy.loginfo("Waiting for arm_controller...")
    arm_client = actionlib.SimpleActionClient(
        "arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    arm_client.wait_for_server()
    rospy.loginfo("...connected.")

    rospy.loginfo("Waiting for gripper_controller...")
    gripper_client = actionlib.SimpleActionClient(
        "gripper_controller/gripper_action", GripperCommandAction)
    gripper_client.wait_for_server()
    rospy.loginfo("...connected.")

    # move_goal = MoveBaseGoal()
    # move_goal.target_pose.pose.position.x = x
    # move_goal.target_pose.pose.position.y = y
    # move_goal.target_pose.pose.orientation.z = sin(theta/2.0)
    # move_goal.target_pose.pose.orientation.w = cos(theta/2.0)
    # move_goal.target_pose.header.frame_id = frame
    # move_goal.target_pose.header.stamp = rospy.Time.now()
    # move_base.send_goal(move_goal)
    # move_base.wait_for_result()

    trajectory = JointTrajectory()
    trajectory.joint_names = head_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = head_joint_positions
    trajectory.points[0].velocities = [0.0] * len(head_joint_positions)
    trajectory.points[0].accelerations = [0.0] * len(head_joint_positions)
    trajectory.points[0].time_from_start = rospy.Duration(5.0)

    head_goal = FollowJointTrajectoryGoal()
    head_goal.trajectory = trajectory
    head_goal.goal_time_tolerance = rospy.Duration(0.0)

    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names

    # trajectory.points.append(JointTrajectoryPoint())
    # trajectory.points[0].positions = [0.0] * len(arm_joint_positions)
    # trajectory.points[0].velocities =  [0.0] * len(arm_joint_positions)
    # trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions)
    # duration = 5.0
    # trajectory.points[0].time_from_start = rospy.Duration(duration)
    #
    # trajectory.points.append(JointTrajectoryPoint())
    # trajectory.points[1].positions = arm_intermediate_positions
    # trajectory.points[1].velocities =  [0.0] * len(arm_joint_positions)
    # trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions)
    # duration += 5.0
    # trajectory.points[1].time_from_start = rospy.Duration(duration)
    #
    # trajectory.points.append(JointTrajectoryPoint())
    # trajectory.points[2].positions = arm_joint_positions
    # trajectory.points[2].velocities =  [0.0] * len(arm_joint_positions)
    # trajectory.points[2].accelerations = [0.0] * len(arm_joint_positions)
    # duration += 5.0
    # trajectory.points[2].time_from_start = rospy.Duration(duration)

    i = 0
    duration = 5.0
    trajectory.points.append(JointTrajectoryPoint())
    arm_joint_positions = [0.0, -1.22, 0.0, 1.20, 0.0, 1.6,
                           0.0]  # above the block, READY TO PICK
    trajectory.points[i].positions = arm_joint_positions
    trajectory.points[i].velocities = [0.0] * len(arm_joint_positions)
    trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[i].time_from_start = rospy.Duration(duration)

    i += 1
    duration += 5.0
    trajectory.points.append(JointTrajectoryPoint())
    arm_joint_positions = [0.0, -1.20, 0.0, 1.45, 0.0, 1.3,
                           0.0]  # PERFECT - PICK POSE
    trajectory.points[i].positions = arm_joint_positions
    trajectory.points[i].velocities = [0.0] * len(arm_joint_positions)
    trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[i].time_from_start = rospy.Duration(duration)

    # i += 1
    # duration += 5.0
    # trajectory.points.append(JointTrajectoryPoint())
    # arm_joint_positions  = [0.0, -1.22, 0.0, 1.20, 0.0, 1.6, 0.0] # above the block, READY TO PICK
    # trajectory.points[i].positions = arm_joint_positions
    # trajectory.points[i].velocities =  [0.0] * len(arm_joint_positions)
    # trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions)
    # trajectory.points[i].time_from_start = rospy.Duration(duration)

    arm_goal = FollowJointTrajectoryGoal()
    arm_goal.trajectory = trajectory
    arm_goal.goal_time_tolerance = rospy.Duration(0.0)

    gripper_goal = GripperCommandGoal()
    gripper_goal.command.max_effort = 10.0
    gripper_goal.command.position = 0.1

    rospy.loginfo("Setting positions...")
    head_client.send_goal(head_goal)
    arm_client.send_goal(arm_goal)
    gripper_client.send_goal(gripper_goal)
    gripper_client.wait_for_result(rospy.Duration(5.0))
    arm_client.wait_for_result(rospy.Duration(6.0))
    head_client.wait_for_result(rospy.Duration(6.0))
    rospy.loginfo("...done")

    gripper_goal = GripperCommandGoal()
    gripper_goal.command.max_effort = 10.0
    gripper_goal.command.position = 0.0
    #gripper_client.send_goal(gripper_goal)
    time.sleep(5.0)
    gripper_client.send_goal_and_wait(gripper_goal, rospy.Duration(5.0))
    #gripper_client.wait_for_result(rospy.Duration(100.0))
    rospy.loginfo("...done")

    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names
    i = 0
    duration = 5.0
    trajectory.points.append(JointTrajectoryPoint())
    arm_joint_positions = [0.0, -1.22, 0.0, 1.20, 0.0, 1.6,
                           0.0]  # above the block, READY TO PICK
    trajectory.points[i].positions = arm_joint_positions
    trajectory.points[i].velocities = [0.0] * len(arm_joint_positions)
    trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[i].time_from_start = rospy.Duration(duration)

    arm_goal = FollowJointTrajectoryGoal()
    arm_goal.trajectory = trajectory
    arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    arm_client.send_goal(arm_goal)
    arm_client.wait_for_result(rospy.Duration(6.0))
Пример #32
0
 def __init__(self):
     rospy.init_node('trajectory_demo')
     
     # Set to True to move back to the starting configurations
     reset = rospy.get_param('~reset', False)
     
     # Set to False to wait for arm to finish before moving head
     sync = rospy.get_param('~sync', True)
     
     # Which joints define the arm?
     arm_joints = ['right_e0',
                   'right_e1',
                   'right_s0', 
                   'right_s1',
                   'right_w0',
                   'right_w1',
                   'right_w2']
     
     # Which joints define the head?
     #head_joints = ['head_pan_joint', 'head_tilt_joint']
     
     if reset:
         # Set the arm back to the resting position
         arm_goal  = [0, 0, 0, 0, 0, 0]
         
         # Re-center the head
         head_goal = [0, 0]  
     else:
         # Set a goal configuration for the arm
         arm_goal  = [-0.3, -2.0, -1.0, 0.8, 1.0, -0.7]
         
         # Set a goal configuration for the head
         head_goal = [-1.3, -0.1]
 
     # Connect to the right arm trajectory action server
     rospy.loginfo('Waiting for right arm trajectory controller...')
     
     arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
     
     arm_client.wait_for_server()
     
     rospy.loginfo('...connected.')
     
     # Connect to the head trajectory action server
     rospy.loginfo('Waiting for head trajectory controller...')
 
     head_client = actionlib.SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
    
     head_client.wait_for_server()
     
     rospy.loginfo('...connected.')    
 
     # Create a single-point arm trajectory with the arm_goal as the end-point
     arm_trajectory = JointTrajectory()
     arm_trajectory.joint_names = arm_joints
     arm_trajectory.points.append(JointTrajectoryPoint())
     arm_trajectory.points[0].positions = arm_goal
     arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
     arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
     arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
 
     # Send the trajectory to the arm action server
     rospy.loginfo('Moving the arm to goal position...')
     
     # Create an empty trajectory goal
     arm_goal = FollowJointTrajectoryGoal()
     
     # Set the trajectory component to the goal trajectory created above
     arm_goal.trajectory = arm_trajectory
     
     # Specify zero tolerance for the execution time
     arm_goal.goal_time_tolerance = rospy.Duration(0.0)
 
     # Send the goal to the action server
     arm_client.send_goal(arm_goal)
     
     if not sync:
         # Wait for up to 5 seconds for the motion to complete 
         arm_client.wait_for_result(rospy.Duration(5.0))
     
     # Create a single-point head trajectory with the head_goal as the end-point
     head_trajectory = JointTrajectory()
     head_trajectory.joint_names = head_joints
     head_trajectory.points.append(JointTrajectoryPoint())
     head_trajectory.points[0].positions = head_goal
     head_trajectory.points[0].velocities = [0.0 for i in head_joints]
     head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
     head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
 
     # Send the trajectory to the head action server
     rospy.loginfo('Moving the head to goal position...')
     
     head_goal = FollowJointTrajectoryGoal()
     head_goal.trajectory = head_trajectory
     head_goal.goal_time_tolerance = rospy.Duration(0.0)
 
     # Send the goal
     head_client.send_goal(head_goal)
     
     # Wait for up to 5 seconds for the motion to complete 
     head_client.wait_for_result(rospy.Duration(5.0))
     
     rospy.loginfo('...done')
Пример #33
0
    rospy.loginfo("Waiting for gripper_controller...")
    gripper_client = actionlib.SimpleActionClient(
        "gripper_controller/gripper_action", GripperCommandAction)
    gripper_client.wait_for_server()
    rospy.loginfo("...connected.")

    trajectory = JointTrajectory()
    trajectory.joint_names = head_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = head_joint_positions
    trajectory.points[0].velocities = [0.0] * len(head_joint_positions)
    trajectory.points[0].accelerations = [0.0] * len(head_joint_positions)
    trajectory.points[0].time_from_start = rospy.Duration(5.0)

    head_goal = FollowJointTrajectoryGoal()
    head_goal.trajectory = trajectory
    head_goal.goal_time_tolerance = rospy.Duration(0.0)

    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = [0.0] * len(arm_joint_positions)
    trajectory.points[0].velocities = [0.0] * len(arm_joint_positions)
    trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[0].time_from_start = rospy.Duration(1.0)
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[1].positions = arm_intermediate_positions
    trajectory.points[1].velocities = [0.0] * len(arm_joint_positions)
    trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[1].time_from_start = rospy.Duration(4.0)
    trajectory.points.append(JointTrajectoryPoint())
Пример #34
0
    trajectory.points[
        0].positions = arm_joint_positions5  #Asignamos la trayectoria a ejecutar

    trajectory.points[0].velocities = [0.0] * len(
        arm_joint_positions0)  #Asignamos los valores por default = 0

    trajectory.points[0].accelerations = [0.0] * len(
        arm_joint_positions0)  #Asignamos los valores por default = 0

    trajectory.points[0].time_from_start = rospy.Duration(
        1.0)  #Asignamos la duración de 1seg para iniciar

    arm_goal = FollowJointTrajectoryGoal()  #Creamos una variable de tipo Goal

    arm_goal.trajectory = trajectory  #Asignamos nuestra trayectoria hacia la trayectoria de nuestro objetivo(goal)

    arm_goal.goal_time_tolerance = rospy.Duration(
        0.0
    )  #Asignamos la duración de 0seg como tiempo de tolerancia de nuestro objetivo

    rospy.loginfo("Setting positions...")  #Imprimimos en pantalla

    arm_client.send_goal(
        arm_goal
    )  #Enviamos nuestro objetivo hacia el Acción Server de nuestro robot Fetch

    arm_client.wait_for_result(
        rospy.Duration(6.0))  #Esperamos 6 segundos para obtener los resultados

    rospy.loginfo(
    arm_client.wait_for_server()
    rospy.loginfo("...connected.")

    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = [0.0] * len(arm_joint_positions)
    trajectory.points[0].velocities = [0.0] * len(arm_joint_positions)
    trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[0].time_from_start = rospy.Duration(1.0)
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[1].positions = arm_intermediate_positions
    trajectory.points[1].velocities = [0.0] * len(arm_joint_positions)
    trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[1].time_from_start = rospy.Duration(4.0)
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[2].positions = arm_joint_positions
    trajectory.points[2].velocities = [0.0] * len(arm_joint_positions)
    trajectory.points[2].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[2].time_from_start = rospy.Duration(7.5)

    arm_goal = FollowJointTrajectoryGoal()
    arm_goal.trajectory = trajectory
    arm_goal.goal_time_tolerance = rospy.Duration(0.0)

    rospy.loginfo("Setting positions...")
    arm_client.send_goal(arm_goal)
    arm_client.wait_for_result(rospy.Duration(6.0))

    rospy.loginfo("...done")
Пример #36
0
    def execute(self, component_name, as_name, target="", blocking=True):
        ah = ActionHandle("move_joint_trajectory", component_name, target,
                          blocking)

        rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)

        # get joint_names from parameter server
        param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
        if not rospy.has_param(param_string):
            rospy.logerr(
                "parameter %s does not exist on ROS Parameter Server, aborting...",
                param_string)
            ah.set_failed(2)
            return ah
        joint_names = rospy.get_param(param_string)

        # check joint_names parameter
        if not type(joint_names) is list:  # check list
            rospy.logerr(
                "no valid joint_names for %s: not a list, aborting...",
                component_name)
            print "joint_names are:", joint_names
            ah.set_failed(3)
            return ah
        else:
            for i in joint_names:
                #print i,"type1 = ", type(i)
                if not type(i) is str:  # check string
                    rospy.logerr(
                        "no valid joint_names for %s: not a list of strings, aborting...",
                        component_name)
                    print "joint_names are:", param
                    ah.set_failed(3)
                    return ah
                else:
                    rospy.logdebug("accepted joint_names for component %s",
                                   component_name)

        # get joint values from parameter server
        if type(target) is str:
            if not rospy.has_param(self.ns_global_prefix + "/" +
                                   component_name + "/" + target):
                rospy.logerr(
                    "parameter %s does not exist on ROS Parameter Server, aborting...",
                    self.ns_global_prefix + "/" + component_name + "/" +
                    target)
                ah.set_failed(2)
                return ah
            param = rospy.get_param(self.ns_global_prefix + "/" +
                                    component_name + "/" + target)
        else:
            param = target

        # check trajectory parameters
        if not type(param) is list:  # check outer list
            rospy.logerr("no valid parameter for %s: not a list, aborting...",
                         component_name)
            print "parameter is:", param
            ah.set_failed(3)
            return ah

        traj = []

        for point in param:
            #print point,"type1 = ", type(point)
            if type(point) is str:
                if not rospy.has_param(self.ns_global_prefix + "/" +
                                       component_name + "/" + point):
                    rospy.logerr(
                        "parameter %s does not exist on ROS Parameter Server, aborting...",
                        self.ns_global_prefix + "/" + component_name + "/" +
                        point)
                    ah.set_failed(2)
                    return ah
                point = rospy.get_param(self.ns_global_prefix + "/" +
                                        component_name + "/" + point)
                point = point[
                    0]  # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
                #print point
            elif type(point) is list:
                rospy.logdebug("point is a list")
            else:
                rospy.logerr(
                    "no valid parameter for %s: not a list of lists or strings, aborting...",
                    component_name)
                print "parameter is:", param
                ah.set_failed(3)
                return ah

            # here: point should be list of floats/ints
            #print point
            if not len(point) == len(joint_names):  # check dimension
                rospy.logerr(
                    "no valid parameter for %s: dimension should be %d and is %d, aborting...",
                    component_name, len(joint_names), len(point))
                print "parameter is:", param
                ah.set_failed(3)
                return ah

            for value in point:
                #print value,"type2 = ", type(value)
                if not ((type(value) is float) or
                        (type(value) is int)):  # check type
                    #print type(value)
                    rospy.logerr(
                        "no valid parameter for %s: not a list of float or int, aborting...",
                        component_name)
                    print "parameter is:", param
                    ah.set_failed(3)
                    return ah

                rospy.logdebug("accepted value %f for %s", value,
                               component_name)
            traj.append(point)

        rospy.logdebug("accepted trajectory for %s", component_name)

        # convert to ROS trajectory message
        traj_msg = JointTrajectory()
        traj_msg.header.stamp = rospy.Time.now() + rospy.Duration(0.5)
        traj_msg.joint_names = joint_names
        point_nr = 0
        for point in traj:
            point_nr = point_nr + 1
            point_msg = JointTrajectoryPoint()
            point_msg.positions = point
            point_msg.velocities = [0] * len(joint_names)
            point_msg.time_from_start = rospy.Duration(
                3 * point_nr
            )  # this value is set to 3 sec per point. \todo TODO: read from parameter
            traj_msg.points.append(point_msg)

        # call action server
        action_server_name = "/" + component_name + '_controller/follow_joint_trajectory'
        rospy.logdebug("calling %s action server", action_server_name)
        client = actionlib.SimpleActionClient(action_server_name,
                                              FollowJointTrajectoryAction)
        # trying to connect to server
        rospy.logdebug("waiting for %s action server to start",
                       action_server_name)
        if not client.wait_for_server(rospy.Duration(5)):
            # error: server did not respond
            rospy.logerr(
                "%s action server not ready within timeout, aborting...",
                action_server_name)
            ah.set_failed(4)
            return ah
        else:
            rospy.logdebug("%s action server ready", action_server_name)

        # sending goal
        client_goal = FollowJointTrajectoryGoal()
        client_goal.trajectory = traj_msg
        #print client_goal
        client.send_goal(client_goal)
        ah.set_client(client)

        ah.wait_inside()
        return ah
Пример #37
0
    def movel(self, destino, orientation=None, speed=None):
        """
        Move to destination following a line
        args:
            destino: a list of xyz defining a point in space
            orientation: a quaternion defining the orientation
        """
        self.client.cancel_all_goals()
        # to create uniform movements lets define speed to calculate time
        if speed is None:
            speed = 0.4  # m/s

        cur_pose = self.fwd_kin(self.joints_state, i_unit='r', o_unit='p')
        if orientation is None:
            orientation = cur_pose.orientation

        if type(destino) == type(Pose()):
            desired_position = np.array([
                destino.position.x, destino.position.y, destino.position.z,
                destino.orientation.x, destino.orientation.y,
                destino.orientation.z, destino.orientation.w
            ])
            tmp1 = np.array([
                cur_pose.position.x, cur_pose.position.y, cur_pose.position.z
            ])
            tmp2 = np.array(
                [destino.position.x, destino.position.y, destino.position.z])

        else:
            desired_position = np.array([
                *destino, orientation.x, orientation.y, orientation.z,
                orientation.w
            ])
            tmp1 = np.array([
                cur_pose.position.x, cur_pose.position.y, cur_pose.position.z
            ])
            tmp2 = np.array(destino)

        distance = np.linalg.norm(tmp1 - tmp2)
        if distance <= 0:
            nanoseconds = 1_000_000
        else:
            nanoseconds = 1_000_000_000 * distance / speed

        # check if it is possible to reach
        if len(self.check_working_space([tmp2.tolist()])) == 0:
            rospy.logerr('Position %s out of reach' %
                         np.array_str(tmp2, precision=2, suppress_small=True))
            return False

        # cur_pose = np.array([cur_pose.position.x,cur_pose.position.y,cur_pose.position.z])

        lista = self.trajectory(
            desired_position, cur_pose
        )  # returns a list of np array with position and orientation

        pose = Pose()

        g = FollowJointTrajectoryGoal()
        g.trajectory = JointTrajectory()
        g.trajectory.joint_names = self.JOINT_NAMES

        joints_inv_kine = self.joints_state
        g.trajectory.points = []
        n = len(lista)

        if self.debug:
            os.chdir('/home/ubuntu/Documents/debug')
            new_lista = []
            for item in lista:
                new_lista.append(
                    np.array_str(item, precision=2, suppress_small=True))
            with open(time.strftime("%Y%m%d-%H%M%S") + ' lista.yaml',
                      'w') as file:
                yaml.dump(new_lista, file)

        for i, point in enumerate(lista):
            pose.position.x = point[0]
            pose.position.y = point[1]
            pose.position.z = point[2]
            pose.orientation.x = point[3]
            pose.orientation.y = point[4]
            pose.orientation.z = point[5]
            pose.orientation.w = point[6]

            a = self.inv_kine(self.ros2np(pose))
            a = a.transpose().tolist()
            joints_inv_kine = self.select(a, joints_inv_kine)

            t = int(i * nanoseconds / n)
            g.trajectory.points.append(
                JointTrajectoryPoint(positions=joints_inv_kine,
                                     velocities=[0] * 6,
                                     time_from_start=rospy.Duration(0, t)))

        # Callback goal
        self.done = False

        self.client.send_goal(g, done_cb=self.done_callback)
        start_time = time.time()
        while not self.done and time.time(
        ) - start_time < 3:  # 3 seconds timeout
            time.sleep(0.05)

        if not self.done:
            rospy.logerr('Canceling movement')
        self.client.cancel_all_goals()
        return self.done
    def joint_traj_jtas(self, j_traj):
        self.client.wait_for_server()
        goal = FollowJointTrajectoryGoal()

        goal.trajectory = j_traj
        self.client.send_goal(goal)
Пример #39
0
    def __init__(self):

        x = loadmat('cuadrupedo_andar.mat')
        # print x
        piernas_der = x['estruc_patas_rec']
        piernaLF = piernas_der[0, 0]
        piernaLR = piernas_der[0, 1]
        piernaRF = piernas_der[0, 2]
        piernaRR = piernas_der[0, 3]

        rospy.init_node('trajectory_cua')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints_RR = [
            'coxa_joint_RR', 'femur_joint_RR', 'tibia_joint_RR',
            'tarsus_joint_RR'
        ]
        piernas_joints_RF = [
            'coxa_joint_RF', 'femur_joint_RF', 'tibia_joint_RF',
            'tarsus_joint_RF'
        ]

        piernas_joints_LR = [
            'coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR',
            'tarsus_joint_LR'
        ]
        piernas_joints_LF = [
            'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF',
            'tarsus_joint_LF'
        ]

        rospy.loginfo('Waiting for right arm trajectory controller...')

        rr_client = actionlib.SimpleActionClient(
            '/cuadrupedo/pata_rr/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        rr_client.wait_for_server()

        rf_client = actionlib.SimpleActionClient(
            '/cuadrupedo/pata_rf/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        rf_client.wait_for_server()

        lr_client = actionlib.SimpleActionClient(
            '/cuadrupedo/pata_lr/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        lr_client.wait_for_server()

        lf_client = actionlib.SimpleActionClient(
            '/cuadrupedo/pata_lf/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        lf_client.wait_for_server()

        rospy.loginfo('...connected.')

        rr_trajectory1 = JointTrajectory()
        rr_trajectory1.joint_names = piernas_joints_RR

        rf_trajectory1 = JointTrajectory()
        rf_trajectory1.joint_names = piernas_joints_RF
        lr_trajectory1 = JointTrajectory()
        lr_trajectory1.joint_names = piernas_joints_LR

        lf_trajectory1 = JointTrajectory()
        lf_trajectory1.joint_names = piernas_joints_LF

        # piernas_goalF0 = [0, -0.3, -0.4, 0.8, -0.3, -0.4,
        #                   0, -0.4, -0.3, 0.6, +0.4, -0.3]
        # arm_trajectory.points.append(JointTrajectoryPoint())
        # arm_trajectory.points[0].positions = piernas_goalF0
        # arm_trajectory.points[0].velocities = [0.0 for k in piernas_goalF0]
        # arm_trajectory.points[0].accelerations = [0.0 for k in piernas_goalF0]
        # arm_trajectory.points[0].time_from_start = rospy.Duration(2)

        piernasLF_goalF01 = [
            0, piernaLF['nuevo_femur'][0, 0], -piernaLF['nuevo_tibia'][0, 0],
            -piernaLF['nuevo_tarsus'][0, 0]
        ]
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[0].positions = piernasLF_goalF01
        lf_trajectory1.points[0].time_from_start = rospy.Duration(1)

        piernasLR_goalF01 = [
            0, piernaLR['nuevo_femur'][0, 0], -piernaLR['nuevo_tibia'][0, 0],
            -piernaLR['nuevo_tarsus'][0, 0]
        ]
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[0].positions = piernasLR_goalF01
        lr_trajectory1.points[0].time_from_start = rospy.Duration(1)

        piernasRF_goalF01 = [
            0, -piernaRF['nuevo_femur'][0, 0], piernaRF['nuevo_tibia'][0, 0],
            piernaRF['nuevo_tarsus'][0, 0]
        ]
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[0].positions = piernasRF_goalF01
        rf_trajectory1.points[0].time_from_start = rospy.Duration(1)

        piernasRR_goalF01 = [
            0, -piernaRR['nuevo_femur'][0, 0], piernaRR['nuevo_tibia'][0, 0],
            piernaRR['nuevo_tarsus'][0, 0]
        ]
        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[0].positions = piernasRR_goalF01
        rr_trajectory1.points[0].time_from_start = rospy.Duration(1)

        ind = 1
        salto = 5
        for i in range(1, int(piernaRR['nuevo_tarsus'].shape[0] * 0.3), salto):
            piernasLF_goalF01 = [
                0, piernaLF['nuevo_femur'][i, 0],
                piernaLF['nuevo_tibia'][i, 0], piernaLF['nuevo_tarsus'][i, 0]
            ]
            lf_trajectory1.points.append(JointTrajectoryPoint())
            lf_trajectory1.points[ind].positions = piernasLF_goalF01
            # lf_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01]
            # lf_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01]

            piernasLR_goalF01 = [
                0, piernaLR['nuevo_femur'][i, 0],
                piernaLR['nuevo_tibia'][i, 0], piernaLR['nuevo_tarsus'][i, 0]
            ]
            lr_trajectory1.points.append(JointTrajectoryPoint())
            lr_trajectory1.points[ind].positions = piernasLR_goalF01
            # lr_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01]
            # lr_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01]

            piernasRF_goalF01 = [
                0, -piernaRF['nuevo_femur'][i, 0],
                piernaRF['nuevo_tibia'][i, 0], piernaRF['nuevo_tarsus'][i, 0]
            ]
            rf_trajectory1.points.append(JointTrajectoryPoint())
            rf_trajectory1.points[ind].positions = piernasRF_goalF01
            # rf_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01]
            # rf_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01]

            piernasRR_goalF01 = [
                0, -piernaRR['nuevo_femur'][i, 0],
                piernaRR['nuevo_tibia'][i, 0], piernaRR['nuevo_tarsus'][i, 0]
            ]
            rr_trajectory1.points.append(JointTrajectoryPoint())
            rr_trajectory1.points[ind].positions = piernasRR_goalF01
            # rr_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01]
            # rr_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01]
            # print ind
            # print int(art1_der['senialCompleta'].shape[1]*0.2)
            # arm_trajectory.points[ind].velocities = [0.0 for j in piernas_goalF]
            # arm_trajectory.points[ind].accelerations = [0.0 for j in piernas_goalF]
            # art1_der['tiempo_completo'][0, i]
            # 0.01 * (ind + 1)
            lf_trajectory1.points[ind].time_from_start = rospy.Duration(
                1 +
                (piernaLF['tiempo'][i, 0] - piernaLF['tiempo'][0, 0]) * 2.8)
            lr_trajectory1.points[ind].time_from_start = rospy.Duration(
                1 +
                (piernaLR['tiempo'][i, 0] - piernaLR['tiempo'][0, 0]) * 2.8)
            rf_trajectory1.points[ind].time_from_start = rospy.Duration(
                1 +
                (piernaRF['tiempo'][i, 0] - piernaRF['tiempo'][0, 0]) * 2.8)
            rr_trajectory1.points[ind].time_from_start = rospy.Duration(
                1 +
                (piernaRR['tiempo'][i, 0] - piernaRR['tiempo'][0, 0]) * 2.8)
            # print 1+art1_der['tiempo_completo'][0, i]
            ind += 1
            # print art1_der['senialCompleta'][0, i]

        print int(piernaRR['nuevo_tarsus'].shape[0])

        lf_reposo_trajectory1 = JointTrajectory()
        lf_reposo_trajectory1.joint_names = piernas_joints_LF
        lf_reposo_trajectory1.points.append(JointTrajectoryPoint())
        lf_reposo_trajectory1.points[0].positions = [
            0.0 for i in piernas_joints_RF
        ]
        lf_reposo_trajectory1.points[0].velocities = [
            0.0 for i in piernas_joints_RF
        ]
        lf_reposo_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        lf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)

        lr_reposo_trajectory1 = JointTrajectory()
        lr_reposo_trajectory1.joint_names = piernas_joints_LR
        lr_reposo_trajectory1.points.append(JointTrajectoryPoint())
        lr_reposo_trajectory1.points[0].positions = [
            0.0 for i in piernas_joints_RF
        ]
        lr_reposo_trajectory1.points[0].velocities = [
            0.0 for i in piernas_joints_RF
        ]
        lr_reposo_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        lr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)

        rr_reposo_trajectory1 = JointTrajectory()
        rr_reposo_trajectory1.joint_names = piernas_joints_RR
        rr_reposo_trajectory1.points.append(JointTrajectoryPoint())
        rr_reposo_trajectory1.points[0].positions = [
            0.0 for i in piernas_joints_RF
        ]
        rr_reposo_trajectory1.points[0].velocities = [
            0.0 for i in piernas_joints_RF
        ]
        rr_reposo_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        rr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)

        rf_reposo_trajectory1 = JointTrajectory()
        rf_reposo_trajectory1.joint_names = piernas_joints_RF
        rf_reposo_trajectory1.points.append(JointTrajectoryPoint())
        rf_reposo_trajectory1.points[0].positions = [
            0.0 for i in piernas_joints_RF
        ]
        rf_reposo_trajectory1.points[0].velocities = [
            0.0 for i in piernas_joints_RF
        ]
        rf_reposo_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        rf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)

        # Create an empty trajectory goal
        rospy.loginfo('Moving the arm to goal position...')

        # Create an empty trajectory goal
        rr_goal = FollowJointTrajectoryGoal()
        lm_goal = FollowJointTrajectoryGoal()
        rf_goal = FollowJointTrajectoryGoal()

        lr_goal = FollowJointTrajectoryGoal()
        rm_goal = FollowJointTrajectoryGoal()
        lf_goal = FollowJointTrajectoryGoal()
        # Set the trajectory component to the goal trajectory created above
        rr_goal.trajectory = rr_trajectory1
        rf_goal.trajectory = rf_trajectory1
        lr_goal.trajectory = lr_trajectory1
        lf_goal.trajectory = lf_trajectory1
        # Specify zero tolerance for the execution time
        rr_goal.goal_time_tolerance = rospy.Duration(0.01)
        lm_goal.goal_time_tolerance = rospy.Duration(0.01)
        rf_goal.goal_time_tolerance = rospy.Duration(0.01)
        lr_goal.goal_time_tolerance = rospy.Duration(0.01)
        rm_goal.goal_time_tolerance = rospy.Duration(0.01)
        lf_goal.goal_time_tolerance = rospy.Duration(0.01)

        # Send the goal to the action server
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)

        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        rr_client.wait_for_result()
        '''rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)

        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        rr_client.wait_for_result()

        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        rr_client.wait_for_result()

        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)

        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        rr_client.wait_for_result()
        #'''

        rr_goal.trajectory = rr_reposo_trajectory1
        rf_goal.trajectory = rf_reposo_trajectory1
        lr_goal.trajectory = lr_reposo_trajectory1
        lf_goal.trajectory = lf_reposo_trajectory1

        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()

        rospy.loginfo('...done')
Пример #40
0
    def marker_inf_cb(self, data):

        # in order to filter the inamissible (0.0, 0.0, 0.0, -inf) detections
        if data.data[0:3] == (0.0, 0.0, 0.0):
            # print "inadmissible input!"
            return

        vec3 = Vector3()
        vec3.x = data.data[0]
        vec3.y = data.data[1]
        vec3.z = data.data[2]

        ball_pose = np.array([vec3.x, vec3.y, vec3.z])

        for i in range(3):
            if np.isnan(ball_pose[i]):
                return

        cam_object_T = self.set_cam_object_T(ball_pose)

        cam_object_R = tr.identity_matrix()
        cam_object_R[0:3, 0:3] = cam_object_T[0:3, 0:3]
        cam_object_q = tr.quaternion_from_matrix(cam_object_R)
        self.br.sendTransform(
            (cam_object_T[0, 3], cam_object_T[1, 3], cam_object_T[2, 3]),
            cam_object_q, rospy.Time.now(), "object_in_cam_frame",
            "camera_rgb_optical_frame")

        # publishing the object marker to rviz for object_in_cam
        det_marker = Marker()
        det_marker.header.frame_id = "camera_rgb_optical_frame"
        det_marker.header.stamp = rospy.Time.now()
        det_marker.type = Marker.SPHERE
        det_marker.pose.position.x = cam_object_T[0, 3]
        det_marker.pose.position.y = cam_object_T[1, 3]
        det_marker.pose.position.z = cam_object_T[2, 3]
        det_marker.pose.orientation.x = 0
        det_marker.pose.orientation.y = 0
        det_marker.pose.orientation.z = 0
        det_marker.pose.orientation.w = 1
        det_marker.scale.x = 0.05
        det_marker.scale.y = 0.05
        det_marker.scale.z = 0.05
        det_marker.color.a = 1.0
        det_marker.color.r = 1.0
        det_marker.color.g = 0.0
        det_marker.color.b = 0.0
        self.marker_object_in_cam_pub.publish(det_marker)

        base_object_T = np.dot(np.dot(self.base_end_T, self.end_cam_T),
                               cam_object_T)
        # in order to assign object the same orientation as target frame
        base_object_T[0:3, 0:3] = self.base_target_T[0:3, 0:3]

        final_err = self.error_computation(base_object_T, self.base_target_T)

        # target thresholds:
        trans_th = [0.05, 0.05, 0.05]
        rot_th = [15 * np.pi / 180, 15 * np.pi / 180, 15 * np.pi / 180]

        print "timer: ", self.timer

        if self.timer > 5:
            print "goal reached ............................"
            self.gripper_final_action()

        if np.abs(final_err[0]) < trans_th[0] and np.abs(
                final_err[1]) < trans_th[1] and np.abs(
                    final_err[2]) < trans_th[2]:
            if np.abs(final_err[3]) < rot_th[0] and np.abs(
                    final_err[4]) < rot_th[1] and np.abs(
                        final_err[5]) < rot_th[2]:
                self.timer += 1
                return
            else:
                self.timer = 0
                print "error :", final_err
        else:
            self.timer = 0
            print "error :", final_err

        # use of error_gains:
        error_gains = np.reshape(np.array([1, 1, 1, 1, 1, 1]), (6, 1))
        final_err2 = np.multiply(final_err, error_gains)

        # error value type 2
        end_object_T = np.dot(self.end_cam_T, cam_object_T)
        end_target_T = np.dot(self.end_grip_T, self.grip_target_T)
        object_target_T = np.dot(end_object_T, np.linalg.inv(end_target_T))
        # print "error style 2: ", object_target_T

        # publishing the object marker to rviz
        det_marker = Marker()
        det_marker.header.frame_id = "iiwa_base_link"
        det_marker.header.stamp = rospy.Time.now()
        det_marker.type = Marker.SPHERE
        det_marker.pose.position.x = base_object_T[0, 3]
        det_marker.pose.position.y = base_object_T[1, 3]
        det_marker.pose.position.z = base_object_T[2, 3]
        det_marker.pose.orientation.x = 0
        det_marker.pose.orientation.y = 0
        det_marker.pose.orientation.z = 0
        det_marker.pose.orientation.w = 1
        det_marker.scale.x = 0.05
        det_marker.scale.y = 0.05
        det_marker.scale.z = 0.05
        det_marker.color.a = 1.0
        det_marker.color.r = 0.0
        det_marker.color.g = 0.0
        det_marker.color.b = 1.0
        self.marker_object_in_base_pub.publish(det_marker)

        # print "final error is : ", final_err
        # print "base_target_T: ", base_object_T

        # computation of jacobian of the manipulator
        J = self.kdl_kin.jacobian(self.end_eff_current_pose)

        if self.used_method == "DLSM":
            # use of damped_least_squares as matrix inverse
            J_inverse = np.dot(
                J.T,
                np.linalg.inv(
                    np.dot(J, J.T) + ((self.landa**2) * np.identity(6))))

        elif self.used_method == "JPM":
            # use of matrix pseudo_inverse as matrix inverse
            J_inverse = np.linalg.pinv(J)

        elif self.used_method == "JTM":
            # use of matrix transpose as matrix inverse
            J_inverse = np.transpose(J)

        final_vel = np.dot(J_inverse, final_err2)

        # the dereived final speed command for the manipulator
        speed_cmd = list(np.array(final_vel).reshape(-1, ))

        # avoid huge joint velocities:
        joint_vel_th = [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7]
        if speed_cmd[0] > joint_vel_th[0]:
            print "huge velocity for the first joint!"
            return
        elif speed_cmd[1] > joint_vel_th[1]:
            print "huge velocity for the second joint!"
            return
        elif speed_cmd[2] > joint_vel_th[2]:
            print "huge velocity for the third joint!"
            return
        elif speed_cmd[3] > joint_vel_th[3]:
            print "huge velocity for the fourth joint!"
            return
        elif speed_cmd[4] > joint_vel_th[4]:
            print "huge velocity for the fifth joint!"
            return
        elif speed_cmd[5] > joint_vel_th[5]:
            print "huge velocity for the sixth joint!"
            return
        elif speed_cmd[6] > joint_vel_th[6]:
            print "huge velocity for the seventh joint!"
            return

        # block to handle speed publisher using pose publisher
        intergration_time_step = 0.3
        pose_change = [speed * intergration_time_step for speed in speed_cmd]
        final_pose = [
            a + b for a, b in zip(self.end_eff_current_pose, pose_change)
        ]

        # actionlib commands
        self.trajectory.points[0].positions = final_pose
        self.trajectory.points[0].velocities = [0.0] * len(self.joint_names)
        self.trajectory.points[0].accelerations = [0.0] * len(self.joint_names)
        self.trajectory.points[0].time_from_start = rospy.Duration(1.0)

        # print "pose_change", pose_change
        # print "The manipulator is moving..."

        goal = FollowJointTrajectoryGoal()
        goal.trajectory = self.trajectory
        goal.goal_time_tolerance = rospy.Duration(0.0)

        self.manipulator_client.send_goal(goal)
        action_result = self.manipulator_client.wait_for_result()

        if action_result is True:
            # publishing the path of the object in "red" color
            final_object_position = base_object_T[0:3, 3]
            final_object_R = tr.identity_matrix()
            final_object_R[0:3, 0:3] = base_object_T[0:3, 0:3]
            final_object_quaternion = tr.quaternion_from_matrix(final_object_R)
            self.object_path.markers.append(Marker())
            self.object_path.markers[-1].header.frame_id = "iiwa_base_link"
            self.object_path.markers[-1].header.stamp = rospy.Time.now()
            self.object_path.markers[-1].id = len(self.object_path.markers)
            self.object_path.markers[-1].type = Marker.SPHERE
            self.object_path.markers[
                -1].pose.position.x = final_object_position[0]
            self.object_path.markers[
                -1].pose.position.y = final_object_position[1]
            self.object_path.markers[
                -1].pose.position.z = final_object_position[2]
            self.object_path.markers[
                -1].pose.orientation.x = final_object_quaternion[0]
            self.object_path.markers[
                -1].pose.orientation.y = final_object_quaternion[1]
            self.object_path.markers[
                -1].pose.orientation.z = final_object_quaternion[2]
            self.object_path.markers[
                -1].pose.orientation.w = final_object_quaternion[3]
            self.object_path.markers[-1].scale.x = 0.01
            self.object_path.markers[-1].scale.y = 0.01
            self.object_path.markers[-1].scale.z = 0.01
            self.object_path.markers[-1].color.a = 1.0
            self.object_path.markers[-1].color.r = 1.0
            self.object_path.markers[-1].color.g = 0.0
            self.object_path.markers[-1].color.b = 0.0
            self.marker_obj_path_pub.publish(self.object_path)

            # publishing path of the end effector in "green" color
            final_ee_position = self.base_end_T[0:3, 3]
            final_ee_R = tr.identity_matrix()
            final_ee_R[0:3, 0:3] = self.base_end_T[0:3, 0:3]
            final_ee_quaternion = tr.quaternion_from_matrix(final_ee_R)
            self.ee_path.markers.append(Marker())
            self.ee_path.markers[-1].header.frame_id = "iiwa_base_link"
            self.ee_path.markers[-1].header.stamp = rospy.Time.now()
            self.ee_path.markers[-1].id = len(self.ee_path.markers)
            self.ee_path.markers[-1].type = Marker.SPHERE
            self.ee_path.markers[-1].pose.position.x = final_ee_position[0]
            self.ee_path.markers[-1].pose.position.y = final_ee_position[1]
            self.ee_path.markers[-1].pose.position.z = final_ee_position[2]
            self.ee_path.markers[-1].pose.orientation.x = final_ee_quaternion[
                0]
            self.ee_path.markers[-1].pose.orientation.y = final_ee_quaternion[
                1]
            self.ee_path.markers[-1].pose.orientation.z = final_ee_quaternion[
                2]
            self.ee_path.markers[-1].pose.orientation.w = final_ee_quaternion[
                3]
            self.ee_path.markers[-1].scale.x = 0.01
            self.ee_path.markers[-1].scale.y = 0.01
            self.ee_path.markers[-1].scale.z = 0.01
            self.ee_path.markers[-1].color.a = 1.0
            self.ee_path.markers[-1].color.r = 0.0
            self.ee_path.markers[-1].color.g = 1.0
            self.ee_path.markers[-1].color.b = 0.0
            self.marker_eff_path_pub.publish(self.ee_path)
Пример #41
0
    def run(self, joint_pose, duration=2.0):
        """
        The run function for this step

        Args:
            duration (float) : The amount of time within which to execute the
                trajectory
            joint_pose (str, list, tuple, dict) :
                The arm poses to move to. If the type is:

                * str. Then if the string starts with
                    * `joint_poses`, get a ``assistance_msgs/ArmJointPose`` \
                        from :const:`ARM_JOINT_POSES_SERVICE_NAME` and move the \
                        joints to the desired pose
                * list, tuple. Then if the list is of
                    * `floats`, move the joints to the desired pose indicated \
                        with the float values

        .. seealso::

            :meth:`task_executor.abstract_step.AbstractStep.run`
        """
        # Parse out the pose
        parsed_pose = self._parse_pose(joint_pose)
        if parsed_pose is None:
            rospy.logerr("Action {}: FAIL. Unknown Format: {}".format(self.name, joint_pose))
            raise KeyError(self.name, "Unknown Format", joint_pose)

        rospy.loginfo("Action {}: Moving arm to pose {}".format(self.name, parsed_pose))

        # Create and send the goal
        goal = FollowJointTrajectoryGoal()
        trajectory = JointTrajectory()
        trajectory.joint_names = ArmFollowJointTrajAction.JOINT_NAMES
        trajectory.points.append(JointTrajectoryPoint())

        trajectory.points[0].positions = parsed_pose
        trajectory.points[0].velocities = [0.0 for _ in parsed_pose]
        trajectory.points[0].accelerations = [0.0 for _ in parsed_pose]
        trajectory.points[0].time_from_start = rospy.Duration(duration)
        goal.trajectory = trajectory

        self._arm_follow_joint_traj_client.send_goal(goal)

        # Yield an empty dict while we're executing
        while self._arm_follow_joint_traj_client.get_state() in AbstractStep.RUNNING_GOAL_STATES:
            yield self.set_running()

        # Wait for a result and yield based on how we exited
        status = self._arm_follow_joint_traj_client.get_state()
        self._arm_follow_joint_traj_client.wait_for_result()
        result = self._arm_follow_joint_traj_client.get_result()

        if status == GoalStatus.SUCCEEDED:
            yield self.set_succeeded()
        elif status == GoalStatus.PREEMPTED:
            yield self.set_preempted(
                action=self.name,
                status=status,
                result=result,
            )
        else:
            yield self.set_aborted(
                action=self.name,
                status=status,
                result=result,
            )
Пример #42
0
    def __init__(self):

        filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_cuadrupedo_exp.urdf'

        angulo_avance = +0.4  #rad
        altura_pata = +0.06  #cm
        avance_x = 0.03
        # angulo_avance = 0  # +0.40 #rad
        # altura_pata = 0  # -0.04 #cm
        avance_x = 0.11
        robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)

        (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
        cadena_RR = tree.getChain("base_link", "tarsus_RR")
        cadena_RF = tree.getChain("base_link", "tarsus_RF")
        cadena_LR = tree.getChain("base_link", "tarsus_LR")
        cadena_LF = tree.getChain("base_link", "tarsus_LF")

        print cadena_RR.getNrOfSegments()
        fksolver_RR = PyKDL.ChainFkSolverPos_recursive(cadena_RR)
        fksolver_RF = PyKDL.ChainFkSolverPos_recursive(cadena_RF)
        fksolver_LR = PyKDL.ChainFkSolverPos_recursive(cadena_LR)
        fksolver_LF = PyKDL.ChainFkSolverPos_recursive(cadena_LF)

        vik_RR = PyKDL.ChainIkSolverVel_pinv(cadena_RR)
        ik_RR = PyKDL.ChainIkSolverPos_NR(cadena_RR, fksolver_RR, vik_RR)

        vik_RF = PyKDL.ChainIkSolverVel_pinv(cadena_RF)
        ik_RF = PyKDL.ChainIkSolverPos_NR(cadena_RF, fksolver_RF, vik_RF)

        vik_LR = PyKDL.ChainIkSolverVel_pinv(cadena_LR)
        ik_LR = PyKDL.ChainIkSolverPos_NR(cadena_LR, fksolver_LR, vik_LR)

        vik_LF = PyKDL.ChainIkSolverVel_pinv(cadena_LF)
        ik_LF = PyKDL.ChainIkSolverPos_NR(cadena_LF, fksolver_LF, vik_LF)

        nj_izq = cadena_RR.getNrOfJoints()
        posicionInicial_R = PyKDL.JntArray(nj_izq)
        posicionInicial_R[0] = 0
        posicionInicial_R[1] = 0
        posicionInicial_R[2] = 0
        posicionInicial_R[3] = 0

        nj_izq = cadena_LR.getNrOfJoints()
        posicionInicial_L = PyKDL.JntArray(nj_izq)
        posicionInicial_L[0] = 0
        posicionInicial_L[1] = 0
        posicionInicial_L[2] = 0
        posicionInicial_L[3] = 0

        print "Forward kinematics"
        finalFrame_R = PyKDL.Frame()
        finalFrame_L = PyKDL.Frame()

        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints_RR = [
            'coxa_joint_RR', 'femur_joint_RR', 'tibia_joint_RR',
            'tarsus_joint_RR'
        ]
        piernas_joints_RF = [
            'coxa_joint_RF', 'femur_joint_RF', 'tibia_joint_RF',
            'tarsus_joint_RF'
        ]

        piernas_joints_LR = [
            'coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR',
            'tarsus_joint_LR'
        ]
        piernas_joints_LF = [
            'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF',
            'tarsus_joint_LF'
        ]

        rr_goal0 = [0.0, 0.0, 0.0, 0.0]
        rf_goal0 = [0.0, 0.0, 0.0, 0.0]
        rr_goal1 = [0.0, 0.0, 0.0, 0.0]
        rf_goal1 = [0.0, 0.0, 0.0, 0.0]
        lr_goal0 = [0.0, 0.0, 0.0, 0.0]
        lf_goal0 = [0.0, 0.0, 0.0, 0.0]
        lr_goal1 = [0.0, 0.0, 0.0, 0.0]
        lf_goal1 = [0.0, 0.0, 0.0, 0.0]

        #11111111111111111111111111111111111111111111
        print "Inverse Kinematics"
        fksolver_RR.JntToCart(posicionInicial_R, finalFrame_R)
        q_init_RR = posicionInicial_R  # initial angles
        desiredFrameRR = finalFrame_R
        desiredFrameRR.p[0] = finalFrame_R.p[0] + avance_x
        desiredFrameRR.p[1] = finalFrame_R.p[1]
        desiredFrameRR.p[2] = finalFrame_R.p[2] + altura_pata
        print "Desired Position: ", desiredFrameRR.p
        q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints())
        ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR)
        print "Output angles in rads: ", q_out_RR

        rr_goal0[0] = q_out_RR[0]
        rr_goal0[1] = q_out_RR[1]  #+angulo_avance
        rr_goal0[2] = q_out_RR[2]
        rr_goal0[3] = q_out_RR[3]

        print "Inverse Kinematics"
        fksolver_LF.JntToCart(posicionInicial_L, finalFrame_L)
        q_init_LF = posicionInicial_L  # initial angles
        desiredFrameLF = finalFrame_L
        desiredFrameLF.p[0] = finalFrame_L.p[0] + avance_x
        desiredFrameLF.p[1] = finalFrame_L.p[1]
        desiredFrameLF.p[2] = finalFrame_L.p[2] + altura_pata
        print "Desired Position: ", desiredFrameLF.p
        q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints())
        ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF)
        print "Output angles in rads: ", q_out_LF

        lf_goal0[0] = q_out_LF[0]
        lf_goal0[1] = q_out_LF[1]  #- angulo_avance
        lf_goal0[2] = q_out_LF[2]
        lf_goal0[3] = q_out_LF[3]

        # 22222222222222222222222222222222222222222222
        RR_actual = PyKDL.JntArray(nj_izq)
        RR_actual[0] = rr_goal0[0]
        RR_actual[1] = rr_goal0[1]
        RR_actual[2] = rr_goal0[2]
        RR_actual[3] = rr_goal0[3]
        print "Inverse Kinematics"
        fksolver_RR.JntToCart(RR_actual, finalFrame_R)
        q_init_RR = RR_actual  # initial angles
        desiredFrameRR = finalFrame_R
        desiredFrameRR.p[0] = finalFrame_R.p[0] - avance_x
        desiredFrameRR.p[1] = finalFrame_R.p[1]
        desiredFrameRR.p[2] = finalFrame_R.p[2] - altura_pata
        print "Desired Position: ", desiredFrameRR.p
        q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints())
        ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR)
        print "Output angles in rads: ", q_out_RR

        rr_goal1[0] = q_out_RR[0]
        rr_goal1[1] = q_out_RR[1]
        rr_goal1[2] = q_out_RR[2]
        rr_goal1[3] = q_out_RR[3]

        LF_actual = PyKDL.JntArray(nj_izq)
        LF_actual[0] = lf_goal0[0]
        LF_actual[1] = lf_goal0[1]
        LF_actual[2] = lf_goal0[2]
        LF_actual[3] = lf_goal0[3]
        print "Inverse Kinematics"
        fksolver_LF.JntToCart(LF_actual, finalFrame_L)
        q_init_LF = LF_actual  # initial angles
        desiredFrameLF = finalFrame_L
        desiredFrameLF.p[0] = finalFrame_L.p[0] - avance_x
        desiredFrameLF.p[1] = finalFrame_L.p[1]
        desiredFrameLF.p[2] = finalFrame_L.p[2] - altura_pata
        print "Desired Position: ", desiredFrameLF.p
        q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints())
        ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF)
        print "Output angles in rads: ", q_out_LF

        lf_goal1[0] = q_out_LF[0]
        lf_goal1[1] = q_out_LF[1]  # - angulo_avance
        lf_goal1[2] = q_out_LF[2]
        lf_goal1[3] = q_out_LF[3]

        # 11111111111111111111111111111111111111111111
        print "Inverse Kinematics"
        fksolver_LR.JntToCart(posicionInicial_L, finalFrame_L)
        q_init_LR = posicionInicial_L  # initial angles
        desiredFrameLR = finalFrame_L
        desiredFrameLR.p[0] = finalFrame_L.p[0]  #-avance_x
        desiredFrameLR.p[1] = finalFrame_L.p[1]
        desiredFrameLR.p[2] = finalFrame_L.p[2]  # + altura_pata
        print "Desired Position: ", desiredFrameLR.p
        q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints())
        ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR)
        print "Output angles in rads: ", q_out_LR

        lr_goal0[0] = q_out_LR[0]
        lr_goal0[1] = q_out_LR[1] + angulo_avance
        lr_goal0[2] = q_out_LR[2]
        lr_goal0[3] = q_out_LR[3]

        print "Inverse Kinematics"
        fksolver_RF.JntToCart(posicionInicial_R, finalFrame_R)
        q_init_RF = posicionInicial_R  # initial angles
        desiredFrameRF = finalFrame_R
        desiredFrameRF.p[0] = finalFrame_R.p[0]  # -avance_x
        desiredFrameRF.p[1] = finalFrame_R.p[1]
        desiredFrameRF.p[2] = finalFrame_R.p[2]  #+ altura_pata
        print "Desired Position: ", desiredFrameRF.p
        q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints())
        ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF)
        print "Output angles in rads: ", q_out_RF

        rf_goal0[0] = q_out_RF[0]
        rf_goal0[1] = q_out_RF[1] - angulo_avance
        rf_goal0[2] = q_out_RF[2]
        rf_goal0[3] = q_out_RF[3]

        # 2222222222222222222222222222222222222222222222

        LR_actual = PyKDL.JntArray(nj_izq)
        LR_actual[0] = lr_goal0[0]
        LR_actual[1] = lr_goal0[1]
        LR_actual[2] = lr_goal0[2]
        LR_actual[3] = lr_goal0[3]
        print "Inverse Kinematics"
        fksolver_LR.JntToCart(LR_actual, finalFrame_L)
        q_init_LR = LR_actual  # initial angles
        print "Inverse Kinematics"
        desiredFrameLR = finalFrame_L
        desiredFrameLR.p[0] = finalFrame_L.p[0]  #+ avance_x
        desiredFrameLR.p[1] = finalFrame_L.p[1]
        desiredFrameLR.p[2] = finalFrame_L.p[2]  # - altura_pata
        print "Desired Position: ", desiredFrameLR.p
        q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints())
        ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR)
        print "Output angles in rads: ", q_out_LR

        lr_goal1[0] = q_out_LR[0]  #- angulo_avance
        lr_goal1[1] = q_out_LR[1]
        lr_goal1[2] = q_out_LR[2]
        lr_goal1[3] = q_out_LR[3] + angulo_avance

        RF_actual = PyKDL.JntArray(nj_izq)
        RF_actual[0] = rf_goal0[0]
        RF_actual[1] = rf_goal0[1]
        RF_actual[2] = rf_goal0[2]
        RF_actual[3] = rf_goal0[3]
        print "Inverse Kinematics"
        fksolver_RF.JntToCart(RF_actual, finalFrame_R)
        q_init_RF = RF_actual  # initial angles
        desiredFrameRF = finalFrame_R
        desiredFrameRF.p[0] = finalFrame_R.p[0]  # + avance_x
        desiredFrameRF.p[1] = finalFrame_R.p[1]
        desiredFrameRF.p[2] = finalFrame_R.p[2]  #- altura_pata
        print "Desired Position: ", desiredFrameRF.p
        q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints())
        ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF)
        print "Output angles in rads: ", q_out_RF

        rf_goal1[0] = q_out_RF[0]
        rf_goal1[1] = q_out_RF[1]
        rf_goal1[2] = q_out_RF[2]
        rf_goal1[3] = q_out_RF[3] + angulo_avance

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        rr_client = actionlib.SimpleActionClient(
            '/cuadrupedo/pata_rr/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        rr_client.wait_for_server()

        rf_client = actionlib.SimpleActionClient(
            '/cuadrupedo/pata_rf/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        rf_client.wait_for_server()

        lr_client = actionlib.SimpleActionClient(
            '/cuadrupedo/pata_lr/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        lr_client.wait_for_server()

        lf_client = actionlib.SimpleActionClient(
            '/cuadrupedo/pata_lf/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        lf_client.wait_for_server()

        rospy.loginfo('...connected.')

        # Create a single-point arm trajectory with the piernas_goal as the end-point
        rr_trajectory1 = JointTrajectory()
        rr_trajectory1.joint_names = piernas_joints_RR

        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[0].positions = rr_goal0
        rr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RR
        ]
        rr_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[1].positions = rr_goal1
        rr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[1].accelerations = [
            0.0 for i in piernas_joints_RR
        ]
        rr_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[2].positions = rf_goal0
        rr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[2].accelerations = [
            0.0 for i in piernas_joints_RR
        ]
        rr_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[3].positions = rf_goal1
        rr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[3].accelerations = [
            0.0 for i in piernas_joints_RR
        ]
        rr_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
        #'''

        rf_trajectory1 = JointTrajectory()
        rf_trajectory1.joint_names = piernas_joints_RF
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[0].positions = rf_goal0  # [0,0,0,0]
        rf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        rf_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[1].positions = rf_goal1  # [0,0,0,0]
        rf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[1].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        rf_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[2].positions = rr_goal0  # [0,0,0,0]
        rf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[2].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        rf_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[3].positions = rr_goal1  # [0,0,0,0]
        rf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[3].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        rf_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
        # '''
        lr_trajectory1 = JointTrajectory()
        lr_trajectory1.joint_names = piernas_joints_LR
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[0].positions = lr_goal0  #lr_goal0
        lr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RR
        ]
        lr_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[1].positions = lr_goal1
        lr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[1].accelerations = [
            0.0 for i in piernas_joints_RR
        ]
        lr_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[2].positions = lf_goal0
        lr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[2].accelerations = [
            0.0 for i in piernas_joints_RR
        ]
        lr_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[3].positions = lf_goal1  # lr_goal0
        lr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[3].accelerations = [
            0.0 for i in piernas_joints_RR
        ]
        lr_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
        # '''

        lf_trajectory1 = JointTrajectory()
        lf_trajectory1.joint_names = piernas_joints_LF
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[0].positions = lf_goal0  #lf_goal0  # [0,0,0,0]
        lf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        lf_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[1].positions = lf_goal1  # [0,0,0,0]
        lf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[1].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        lf_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[2].positions = lr_goal0  # [0,0,0,0]
        lf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[2].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        lf_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[3].positions = lr_goal1  # lf_goal0  # [0,0,0,0]
        lf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[3].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        lf_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
        # '''
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')

        lf_reposo_trajectory1 = JointTrajectory()
        lf_reposo_trajectory1.joint_names = piernas_joints_LF
        lf_reposo_trajectory1.points.append(JointTrajectoryPoint())
        lf_reposo_trajectory1.points[0].positions = [
            0.0 for i in piernas_joints_RF
        ]
        lf_reposo_trajectory1.points[0].velocities = [
            0.0 for i in piernas_joints_RF
        ]
        lf_reposo_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        lf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)

        lr_reposo_trajectory1 = JointTrajectory()
        lr_reposo_trajectory1.joint_names = piernas_joints_LR
        lr_reposo_trajectory1.points.append(JointTrajectoryPoint())
        lr_reposo_trajectory1.points[0].positions = [
            0.0 for i in piernas_joints_RF
        ]
        lr_reposo_trajectory1.points[0].velocities = [
            0.0 for i in piernas_joints_RF
        ]
        lr_reposo_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        lr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)

        rr_reposo_trajectory1 = JointTrajectory()
        rr_reposo_trajectory1.joint_names = piernas_joints_RR
        rr_reposo_trajectory1.points.append(JointTrajectoryPoint())
        rr_reposo_trajectory1.points[0].positions = [
            0.0 for i in piernas_joints_RF
        ]
        rr_reposo_trajectory1.points[0].velocities = [
            0.0 for i in piernas_joints_RF
        ]
        rr_reposo_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        rr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)

        rf_reposo_trajectory1 = JointTrajectory()
        rf_reposo_trajectory1.joint_names = piernas_joints_RF
        rf_reposo_trajectory1.points.append(JointTrajectoryPoint())
        rf_reposo_trajectory1.points[0].positions = [
            0.0 for i in piernas_joints_RF
        ]
        rf_reposo_trajectory1.points[0].velocities = [
            0.0 for i in piernas_joints_RF
        ]
        rf_reposo_trajectory1.points[0].accelerations = [
            0.0 for i in piernas_joints_RF
        ]
        rf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)

        # Create an empty trajectory goal
        rr_goal = FollowJointTrajectoryGoal()
        lm_goal = FollowJointTrajectoryGoal()
        rf_goal = FollowJointTrajectoryGoal()

        lr_goal = FollowJointTrajectoryGoal()
        rm_goal = FollowJointTrajectoryGoal()
        lf_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        rr_goal.trajectory = rr_trajectory1
        rf_goal.trajectory = rf_trajectory1
        lr_goal.trajectory = lr_trajectory1
        lf_goal.trajectory = lf_trajectory1

        # Specify zero tolerance for the execution time
        rr_goal.goal_time_tolerance = rospy.Duration(0.0)
        lm_goal.goal_time_tolerance = rospy.Duration(0.0)
        rf_goal.goal_time_tolerance = rospy.Duration(0.0)
        lr_goal.goal_time_tolerance = rospy.Duration(0.0)
        rm_goal.goal_time_tolerance = rospy.Duration(0.0)
        lf_goal.goal_time_tolerance = rospy.Duration(0.0)
        # Send the goal to the action server
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        #rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)
        #lr_client.wait_for_result(rospy.Duration(5.0))
        #rr_client.send_goal(rr_goal)
        #lm_client.send_goal(lm_goal)
        #rf_client.send_goal(rf_goal)'''

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            rr_client.wait_for_result(rospy.Duration(5.0))

        rr_client.wait_for_result()
        print rr_client.get_result()

        rr_goal.trajectory = rr_reposo_trajectory1
        rf_goal.trajectory = rf_reposo_trajectory1
        lr_goal.trajectory = lr_reposo_trajectory1
        lf_goal.trajectory = lf_reposo_trajectory1

        rr_client.send_goal(rr_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()

        rospy.loginfo('...done')
Пример #43
0
    def __init__(self):

        x = loadmat('hex_wave.mat'
                    )  #hex_transversal hex_lateral2 hex_lateral_r hex_wave
        # print x
        pierna1 = x['pierna1']
        pierna2 = x['pierna2']
        pierna3 = x['pierna3']
        pierna4 = x['pierna4']
        pierna5 = x['pierna5']
        pierna6 = x['pierna6']

        rospy.init_node('trajectory_hex')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints_RR = [
            'coxa_joint_RR', 'femur_joint_RR', 'tibia_joint_RR',
            'tarsus_joint_RR'
        ]
        piernas_joints_LM = [
            'coxa_joint_LM', 'femur_joint_LM', 'tibia_joint_LM',
            'tarsus_joint_LM'
        ]
        piernas_joints_RF = [
            'coxa_joint_RF', 'femur_joint_RF', 'tibia_joint_RF',
            'tarsus_joint_RF'
        ]

        piernas_joints_LR = [
            'coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR',
            'tarsus_joint_LR'
        ]
        piernas_joints_RM = [
            'coxa_joint_RM', 'femur_joint_RM', 'tibia_joint_RM',
            'tarsus_joint_RM'
        ]
        piernas_joints_LF = [
            'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF',
            'tarsus_joint_LF'
        ]

        rospy.loginfo('Waiting for right arm trajectory controller...')

        rr_client = actionlib.SimpleActionClient(
            '/hexapodo/pata_rr/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        rr_client.wait_for_server()

        lm_client = actionlib.SimpleActionClient(
            '/hexapodo/pata_lm/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        lm_client.wait_for_server()

        rf_client = actionlib.SimpleActionClient(
            '/hexapodo/pata_rf/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        rf_client.wait_for_server()

        lr_client = actionlib.SimpleActionClient(
            '/hexapodo/pata_lr/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        lr_client.wait_for_server()

        rm_client = actionlib.SimpleActionClient(
            '/hexapodo/pata_rm/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        rm_client.wait_for_server()

        lf_client = actionlib.SimpleActionClient(
            '/hexapodo/pata_lf/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        lf_client.wait_for_server()

        rospy.loginfo('...connected.')

        rr_trajectory1 = JointTrajectory()
        rr_trajectory1.joint_names = piernas_joints_RR
        lm_trajectory1 = JointTrajectory()
        lm_trajectory1.joint_names = piernas_joints_LM
        rf_trajectory1 = JointTrajectory()
        rf_trajectory1.joint_names = piernas_joints_RF
        lr_trajectory1 = JointTrajectory()
        lr_trajectory1.joint_names = piernas_joints_LR
        rm_trajectory1 = JointTrajectory()
        rm_trajectory1.joint_names = piernas_joints_RM
        lf_trajectory1 = JointTrajectory()
        lf_trajectory1.joint_names = piernas_joints_LF

        # piernas_goalF0 = [0, -0.3, -0.4, 0.8, -0.3, -0.4,
        #                   0, -0.4, -0.3, 0.6, +0.4, -0.3]
        # arm_trajectory.points.append(JointTrajectoryPoint())
        # arm_trajectory.points[0].positions = piernas_goalF0
        # arm_trajectory.points[0].velocities = [0.0 for k in piernas_goalF0]
        # arm_trajectory.points[0].accelerations = [0.0 for k in piernas_goalF0]
        # arm_trajectory.points[0].time_from_start = rospy.Duration(2)

        piernasLF_goalF01 = [
            pierna6[0, 0], pierna6[1, 0], pierna6[2, 0], -0.4
        ]  #0.35
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[0].positions = piernasLF_goalF01
        lf_trajectory1.points[0].time_from_start = rospy.Duration(1)

        piernasLM_goalF01 = [
            pierna5[0, 0], pierna5[1, 0], pierna5[2, 0], -0.4
        ]  #0.35
        lm_trajectory1.points.append(JointTrajectoryPoint())
        lm_trajectory1.points[0].positions = piernasLM_goalF01
        lm_trajectory1.points[0].time_from_start = rospy.Duration(1)

        piernasLR_goalF01 = [
            pierna4[0, 0], pierna4[1, 0], pierna4[2, 0], -0.4
        ]  #0.35
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[0].positions = piernasLR_goalF01
        lr_trajectory1.points[0].time_from_start = rospy.Duration(1)

        piernasRF_goalF01 = [
            pierna3[0, 0], -pierna3[1, 0], -pierna3[2, 0], 0.4
        ]  #0.35
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[0].positions = piernasRF_goalF01
        rf_trajectory1.points[0].time_from_start = rospy.Duration(1)

        piernasRM_goalF01 = [
            pierna2[0, 0], -pierna2[1, 0], -pierna2[2, 0], 0.4
        ]  #0.35
        rm_trajectory1.points.append(JointTrajectoryPoint())
        rm_trajectory1.points[0].positions = piernasRM_goalF01
        rm_trajectory1.points[0].time_from_start = rospy.Duration(1)

        piernasRR_goalF01 = [
            pierna1[0, 0], -pierna1[1, 0], -pierna1[2, 0], 0.4
        ]  #0.35
        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[0].positions = piernasRR_goalF01
        rr_trajectory1.points[0].time_from_start = rospy.Duration(1)

        ind = 1
        salto = 5
        t_actual = 1
        for i in range(1, int(pierna1.shape[1] * 0.25), salto):
            piernasLF_goalF01 = [
                pierna6[0, i], pierna6[1, i], pierna6[2, i], -0.4
            ]  #0.35
            lf_trajectory1.points.append(JointTrajectoryPoint())
            lf_trajectory1.points[ind].positions = piernasLF_goalF01
            lf_trajectory1.points[0].velocities = [
                0.0 for k in piernasLF_goalF01
            ]
            lf_trajectory1.points[0].accelerations = [
                0.0 for k in piernasLF_goalF01
            ]

            piernasLM_goalF01 = [
                pierna5[0, i], pierna5[1, i], pierna5[2, i], -0.4
            ]  #0.35
            lm_trajectory1.points.append(JointTrajectoryPoint())
            lm_trajectory1.points[ind].positions = piernasLM_goalF01
            lm_trajectory1.points[0].velocities = [
                0.0 for k in piernasLM_goalF01
            ]
            lm_trajectory1.points[0].accelerations = [
                0.0 for k in piernasLM_goalF01
            ]

            piernasLR_goalF01 = [
                pierna4[0, i], pierna4[1, i], pierna4[2, i], -0.4
            ]  #0.35
            lr_trajectory1.points.append(JointTrajectoryPoint())
            lr_trajectory1.points[ind].positions = piernasLR_goalF01
            lr_trajectory1.points[0].velocities = [
                0.0 for k in piernasLR_goalF01
            ]
            lr_trajectory1.points[0].accelerations = [
                0.0 for k in piernasLR_goalF01
            ]

            piernasRF_goalF01 = [
                pierna3[0, i], -pierna3[1, i], -pierna3[2, i], 0.4
            ]  #0.35
            rf_trajectory1.points.append(JointTrajectoryPoint())
            rf_trajectory1.points[ind].positions = piernasRF_goalF01
            rf_trajectory1.points[0].velocities = [
                0.0 for k in piernasRF_goalF01
            ]
            rf_trajectory1.points[0].accelerations = [
                0.0 for k in piernasRF_goalF01
            ]

            piernasRM_goalF01 = [
                pierna2[0, i], -pierna2[1, 0], -pierna2[2, i], 0.4
            ]  #0.35
            rm_trajectory1.points.append(JointTrajectoryPoint())
            rm_trajectory1.points[ind].positions = piernasRM_goalF01
            rm_trajectory1.points[0].velocities = [
                0.0 for k in piernasRM_goalF01
            ]
            rm_trajectory1.points[0].accelerations = [
                0.0 for k in piernasRM_goalF01
            ]

            piernasRR_goalF01 = [
                pierna1[0, i], -pierna1[1, 0], -pierna1[2, i], 0.4
            ]  #0.35
            rr_trajectory1.points.append(JointTrajectoryPoint())
            rr_trajectory1.points[ind].positions = piernasRR_goalF01
            rr_trajectory1.points[0].velocities = [
                0.0 for k in piernasRR_goalF01
            ]
            rr_trajectory1.points[0].accelerations = [
                0.0 for k in piernasRR_goalF01
            ]
            # print ind
            # print int(art1_der['senialCompleta'].shape[1]*0.2)
            # arm_trajectory.points[ind].velocities = [0.0 for j in piernas_goalF]
            # arm_trajectory.points[ind].accelerations = [0.0 for j in piernas_goalF]
            # art1_der['tiempo_completo'][0, i]
            # 0.01 * (ind + 1)
            t_actual += 0.01
            lf_trajectory1.points[ind].time_from_start = rospy.Duration(
                t_actual)
            lm_trajectory1.points[ind].time_from_start = rospy.Duration(
                t_actual)
            lr_trajectory1.points[ind].time_from_start = rospy.Duration(
                t_actual)
            rf_trajectory1.points[ind].time_from_start = rospy.Duration(
                t_actual)
            rm_trajectory1.points[ind].time_from_start = rospy.Duration(
                t_actual)
            rr_trajectory1.points[ind].time_from_start = rospy.Duration(
                t_actual)
            # print 1+art1_der['tiempo_completo'][0, i]
            ind += 1
            # print art1_der['senialCompleta'][0, i]

        # Create an empty trajectory goal
        rospy.loginfo('Moving the arm to goal position...')
        print pierna1.shape[1]

        # Create an empty trajectory goal
        rr_goal = FollowJointTrajectoryGoal()
        lm_goal = FollowJointTrajectoryGoal()
        rf_goal = FollowJointTrajectoryGoal()

        lr_goal = FollowJointTrajectoryGoal()
        rm_goal = FollowJointTrajectoryGoal()
        lf_goal = FollowJointTrajectoryGoal()
        # Set the trajectory component to the goal trajectory created above
        rr_goal.trajectory = rr_trajectory1
        lm_goal.trajectory = lm_trajectory1
        rf_goal.trajectory = rf_trajectory1
        lr_goal.trajectory = lr_trajectory1
        rm_goal.trajectory = rm_trajectory1
        lf_goal.trajectory = lf_trajectory1
        # Specify zero tolerance for the execution time
        rr_goal.goal_time_tolerance = rospy.Duration(0.01)
        lm_goal.goal_time_tolerance = rospy.Duration(0.01)
        rf_goal.goal_time_tolerance = rospy.Duration(0.01)
        lr_goal.goal_time_tolerance = rospy.Duration(0.01)
        rm_goal.goal_time_tolerance = rospy.Duration(0.01)
        lf_goal.goal_time_tolerance = rospy.Duration(0.01)

        # Send the goal to the action server
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)

        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        rr_client.wait_for_result()

        # rr_client.send_goal(rr_goal)
        # lm_client.send_goal(lm_goal)
        # rf_client.send_goal(rf_goal)
        #
        # lr_client.send_goal(lr_goal)
        # rm_client.send_goal(rm_goal)
        # lf_client.send_goal(lf_goal)
        # # rr_client.wait_for_result(rospy.Duration(5.0))
        # rr_client.wait_for_result()
        #
        # lr_client.send_goal(lr_goal)
        # rm_client.send_goal(rm_goal)
        # lf_client.send_goal(lf_goal)
        # # rr_client.wait_for_result(rospy.Duration(5.0))
        # rr_client.wait_for_result()
        #
        # rr_client.send_goal(rr_goal)
        # lm_client.send_goal(lm_goal)
        # rf_client.send_goal(rf_goal)
        #
        # lr_client.send_goal(lr_goal)
        # rm_client.send_goal(rm_goal)
        # lf_client.send_goal(lf_goal)
        # # rr_client.wait_for_result(rospy.Duration(5.0))
        # rr_client.wait_for_result()
        #'''

        rospy.loginfo('...done')
Пример #44
0
def gspline_to_follow_joint_trajectory_goal(_trj, _joint_names, _sample_rate):
    trj_msg = gspline_to_joint_trajectory_message(_trj, _joint_names,
                                                  _sample_rate)
    msg = FollowJointTrajectoryGoal()
    msg.trajectory = trj_msg
    return msg
Пример #45
0
    def __init__(self):
        filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado3.urdf'
        robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)

        (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
        cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
        cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
        cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
        cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")

        print cadena_der_up_down.getNrOfSegments()
        fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(
            cadena_der_up_down)
        fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(
            cadena_der_down_up)
        fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(
            cadena_izq_up_down)
        fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(
            cadena_izq_down_up)

        vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
        ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down,
                                                   fksolver_der_up_down,
                                                   vik_der_up_down)

        vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
        ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up,
                                                   fksolver_der_down_up,
                                                   vik_der_down_up)

        vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
        ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down,
                                                   fksolver_izq_up_down,
                                                   vik_izq_up_down)

        vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
        ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up,
                                                   fksolver_izq_down_up,
                                                   vik_izq_down_up)

        nj_izq = cadena_der_up_down.getNrOfJoints()
        posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_der_up_down[0] = 0.3
        posicionInicial_der_up_down[1] = -0.3
        posicionInicial_der_up_down[2] = 0
        posicionInicial_der_up_down[3] = 0.6
        posicionInicial_der_up_down[4] = -0.3
        posicionInicial_der_up_down[5] = -0.3

        nj_izq = cadena_izq_up_down.getNrOfJoints()
        posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_up_down[0] = 0.3
        posicionInicial_izq_up_down[1] = -0.3
        posicionInicial_izq_up_down[2] = 0
        posicionInicial_izq_up_down[3] = 0.6
        posicionInicial_izq_up_down[4] = -0.3
        posicionInicial_izq_up_down[5] = -0.3

        nj_izq = cadena_der_down_up.getNrOfJoints()
        posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_der_down_up[5] = 0.3
        posicionInicial_der_down_up[4] = -0.3
        posicionInicial_der_down_up[3] = 0
        posicionInicial_der_down_up[2] = 0.6
        posicionInicial_der_down_up[1] = -0.3
        posicionInicial_der_down_up[0] = -0.3

        nj_izq = cadena_izq_down_up.getNrOfJoints()
        posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_down_up[5] = 0.3
        posicionInicial_izq_down_up[4] = -0.3
        posicionInicial_izq_down_up[3] = 0
        posicionInicial_izq_down_up[2] = 0.6
        posicionInicial_izq_down_up[1] = -0.3
        posicionInicial_izq_down_up[0] = -0.3
        print "Forward kinematics"
        finalFrame_izq_up_down = PyKDL.Frame()
        finalFrame_izq_down_up = PyKDL.Frame()
        finalFrame_der_up_down = PyKDL.Frame()
        finalFrame_der_down_up = PyKDL.Frame()

        fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down,
                                       finalFrame_izq_up_down)
        print "Rotational Matrix of the final Frame: "
        print finalFrame_izq_up_down.M
        print "End-effector position: ", finalFrame_izq_up_down.p

        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints = [
            'cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint',
            'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint',
            'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
            'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint',
            'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint',
            'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint'
        ]

        if reset:
            # Set the arm back to the resting position
            arm_goal = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

        else:
            # Set a goal configuration for the arm
            arm_goal = [0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

        print "Inverse Kinematics"
        q_init = posicionInicial_izq_up_down  # initial angles
        #desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame = finalFrame_izq_up_down
        desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
        desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
        desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
        print "Desired Position: ", desiredFrame.p
        q_out = PyKDL.JntArray(6)
        ik_izq_up_down.CartToJnt(q_init, desiredFrame, q_out)
        print "Output angles in rads: ", q_out

        #arm_goal[0] = q_out[0]
        #arm_goal[1] = q_out[1]
        #arm_goal[2] = q_out[2]
        #arm_goal[3] = q_out[3]
        #arm_goal[4] = q_out[4]
        #arm_goal[5] = q_out[5]

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        arm_client = actionlib.SimpleActionClient(
            '/piernas/piernas_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        arm_client.wait_for_server()

        rospy.loginfo('...connected.')

        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = piernas_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(2.0)

        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')

        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)

        # Send the goal to the action server
        arm_client.send_goal(arm_goal)

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            arm_client.wait_for_result(rospy.Duration(5.0))

        rospy.loginfo('...done')
    rospy.loginfo("Waiting for gripper_controller...")
    gripper_client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
    gripper_client.wait_for_server()
    rospy.loginfo("...connected.")

    trajectory = JointTrajectory()
    trajectory.joint_names = head_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = head_joint_positions
    trajectory.points[0].velocities = [0.0] * len(head_joint_positions)
    trajectory.points[0].accelerations = [0.0] * len(head_joint_positions)
    trajectory.points[0].time_from_start = rospy.Duration(5.0)

    head_goal = FollowJointTrajectoryGoal()
    head_goal.trajectory = trajectory
    head_goal.goal_time_tolerance = rospy.Duration(0.0)

    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = [0.0] * len(arm_joint_positions)
    trajectory.points[0].velocities =  [0.0] * len(arm_joint_positions)
    trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[0].time_from_start = rospy.Duration(1.0)
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[1].positions = arm_intermediate_positions
    trajectory.points[1].velocities =  [0.0] * len(arm_joint_positions)
    trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[1].time_from_start = rospy.Duration(4.0)
    trajectory.points.append(JointTrajectoryPoint())
Пример #47
0
    def __init__(self):

        pkl_file = open('datos_articulaciones.pkl', 'rb')

        # data1 = pickle.load(pkl_file)
        # pprint.pprint(data1)

        # data2 = pickle.load(pkl_file)
        # pprint.pprint(data2)

        # print data1['b'][0]
        # for i in data2:
        #    print i

        datos = []
        try:
            while True:  # loop indefinitely
                print "LEYENDO"
                #print pickle.load(pkl_file)
                datos.append(pickle.load(
                    pkl_file))  # add each item from the file to a list
        except EOFError:  # the exception is used to break the loop
            pass  # we don't need to do anything special

        pkl_file.close()
        print len(datos)

        # print datos_trans
        for i in datos:
            pass
            # print i

        filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf'

        robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)

        (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
        cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
        cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
        cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
        cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")

        print cadena_der_up_down.getNrOfSegments()
        fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(
            cadena_der_up_down)
        fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(
            cadena_der_down_up)
        fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(
            cadena_izq_up_down)
        fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(
            cadena_izq_down_up)

        vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
        ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down,
                                                   fksolver_der_up_down,
                                                   vik_der_up_down)

        vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
        ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up,
                                                   fksolver_der_down_up,
                                                   vik_der_down_up)

        vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
        ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down,
                                                   fksolver_izq_up_down,
                                                   vik_izq_up_down)

        vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
        ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up,
                                                   fksolver_izq_down_up,
                                                   vik_izq_down_up)

        nj_izq = cadena_der_up_down.getNrOfJoints()
        posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_der_up_down[0] = 0.3
        posicionInicial_der_up_down[1] = -0.3
        posicionInicial_der_up_down[2] = 0
        posicionInicial_der_up_down[3] = 0.6
        posicionInicial_der_up_down[4] = -0.3
        posicionInicial_der_up_down[5] = -0.3

        nj_izq = cadena_izq_up_down.getNrOfJoints()
        posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_up_down[0] = 0.3
        posicionInicial_izq_up_down[1] = -0.3
        posicionInicial_izq_up_down[2] = 0.0
        posicionInicial_izq_up_down[3] = 0.6
        posicionInicial_izq_up_down[4] = -0.3
        posicionInicial_izq_up_down[5] = -0.3

        nj_izq = cadena_der_down_up.getNrOfJoints()
        posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_der_down_up[5] = 0.3
        posicionInicial_der_down_up[4] = -0.3
        posicionInicial_der_down_up[3] = 0.0
        posicionInicial_der_down_up[2] = 0.6
        posicionInicial_der_down_up[1] = -0.3
        posicionInicial_der_down_up[0] = -0.3

        nj_izq = cadena_izq_down_up.getNrOfJoints()
        posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_down_up[5] = 0.3
        posicionInicial_izq_down_up[4] = -0.3
        posicionInicial_izq_down_up[3] = 0.0
        posicionInicial_izq_down_up[2] = 0.6
        posicionInicial_izq_down_up[1] = -0.3
        posicionInicial_izq_down_up[0] = -0.3
        print "Forward kinematics"
        finalFrame_izq_up_down = PyKDL.Frame()
        finalFrame_izq_down_up = PyKDL.Frame()
        finalFrame_der_up_down = PyKDL.Frame()
        finalFrame_der_down_up = PyKDL.Frame()

        print "Rotational Matrix of the final Frame: "
        print finalFrame_izq_up_down.M
        print "End-effector position: ", finalFrame_izq_up_down.p

        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints = [
            'cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint',
            'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint',
            'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
            'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint',
            'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint',
            'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint'
        ]

        piernas_goal = [
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
        ]

        #11111111111111111111111111111111111111111111
        print "Inverse Kinematics"
        fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down,
                                       finalFrame_izq_up_down)
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up,
                                       finalFrame_izq_down_up)
        q_init_izq_up_down = posicionInicial_izq_up_down  # initial angles
        desiredFrame = finalFrame_izq_up_down
        desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
        desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
        desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
        print "Desired Position: ", desiredFrame.p
        q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame,
                                 q_out_izq_up_down)
        print "Output angles in rads: ", q_out_izq_up_down

        piernas_goal[6] = q_out_izq_up_down[0]
        piernas_goal[7] = q_out_izq_up_down[1]
        piernas_goal[8] = q_out_izq_up_down[2]
        piernas_goal[9] = q_out_izq_up_down[3]
        piernas_goal[10] = q_out_izq_up_down[4]
        piernas_goal[11] = q_out_izq_up_down[5]

        print "Inverse Kinematics"
        fksolver_der_up_down.JntToCart(posicionInicial_der_up_down,
                                       finalFrame_der_up_down)
        fksolver_der_down_up.JntToCart(posicionInicial_der_down_up,
                                       finalFrame_der_down_up)
        q_init_der_up_down = posicionInicial_der_up_down  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame = finalFrame_der_up_down
        desiredFrame.p[0] = finalFrame_der_up_down.p[0]
        desiredFrame.p[1] = finalFrame_der_up_down.p[1]
        desiredFrame.p[2] = finalFrame_der_up_down.p[2] + 0.02  #0.02
        print "Desired Position: ", desiredFrame.p
        q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame,
                                 q_out_der_up_down)
        print "Output angles in rads: ", q_out_der_up_down

        piernas_goal[0] = q_out_der_up_down[0]
        piernas_goal[1] = q_out_der_up_down[1]
        piernas_goal[2] = q_out_der_up_down[2]
        piernas_goal[3] = q_out_der_up_down[3]
        piernas_goal[4] = q_out_der_up_down[4]
        piernas_goal[5] = q_out_der_up_down[5]

        #121212122121212121212121212121212121212121212
        piernas_goal12 = [
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
        ]
        print "Inverse Kinematics"
        desiredFrame = PyKDL.Frame()
        fksolver_izq_up_down.JntToCart(q_out_izq_up_down, desiredFrame)
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up,
                                       finalFrame_izq_down_up)
        q_init_izq_up_down = posicionInicial_izq_up_down  # initial angles
        desiredFrame.p[0] = desiredFrame.p[0]
        desiredFrame.p[1] = desiredFrame.p[1]
        desiredFrame.p[2] = desiredFrame.p[2]
        print "Desired Position: ", desiredFrame.p
        q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame,
                                 q_out_izq_up_down)
        print "Output angles in rads: ", q_out_izq_up_down

        piernas_goal12[6] = q_out_izq_up_down[0]
        piernas_goal12[7] = q_out_izq_up_down[1]
        piernas_goal12[8] = q_out_izq_up_down[2]
        piernas_goal12[9] = q_out_izq_up_down[3]
        piernas_goal12[10] = q_out_izq_up_down[4]
        piernas_goal12[11] = q_out_izq_up_down[5]

        print "Inverse Kinematics"
        desiredFrame0 = PyKDL.Frame()
        fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame0)
        fksolver_der_down_up.JntToCart(posicionInicial_der_down_up,
                                       finalFrame_der_down_up)
        q_init_der_up_down = posicionInicial_der_up_down  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame0.p[0] = desiredFrame0.p[0]
        desiredFrame0.p[1] = desiredFrame0.p[1] - 0.07
        desiredFrame0.p[2] = desiredFrame0.p[2]
        print "Desired Position: ", desiredFrame0.p
        q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame0,
                                 q_out_der_up_down)
        print "Output angles in rads: ", q_out_der_up_down

        piernas_goal12[0] = q_out_der_up_down[0]
        piernas_goal12[1] = q_out_der_up_down[1]
        piernas_goal12[2] = q_out_der_up_down[2]
        piernas_goal12[3] = q_out_der_up_down[3]
        piernas_goal12[4] = q_out_der_up_down[4]
        piernas_goal12[5] = q_out_der_up_down[5]

        # 222222222222222222222222222222222222222222
        piernas_goal2 = [
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
        ]
        print "Inverse Kinematics"
        desiredFrame2 = PyKDL.Frame()
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up,
                                       desiredFrame2)
        q_init_izq_down_up = posicionInicial_izq_down_up  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame2.p[0] = desiredFrame2.p[0] - 0.06  #0.05
        desiredFrame2.p[1] = desiredFrame2.p[1] - 0.06  #0.06
        desiredFrame2.p[2] = desiredFrame2.p[2] - 0.01  #0.02
        print "Desired Position2222aaa: ", desiredFrame2.p
        #print desiredFrame2.M
        q_out_izq_down_up = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_down_up.CartToJnt(q_init_izq_down_up, desiredFrame2,
                                 q_out_izq_down_up)
        print "Output angles in rads2222: ", q_out_izq_down_up

        piernas_goal2[6] = q_out_izq_down_up[5]  #+0.1
        piernas_goal2[7] = q_out_izq_down_up[4]  #-0.05
        piernas_goal2[8] = q_out_izq_down_up[3]
        piernas_goal2[9] = q_out_izq_down_up[2]
        piernas_goal2[10] = q_out_izq_down_up[1]
        piernas_goal2[11] = q_out_izq_down_up[0]

        #q_out_izq_down_up[4] -=0.1

        print "Inverse Kinematics"
        q_init_der_down_up = PyKDL.JntArray(6)
        q_init_der_down_up[0] = q_out_der_up_down[
            5]  # PROBLEMAS CON LA ASIGNACION
        q_init_der_down_up[1] = q_out_der_up_down[4]
        q_init_der_down_up[2] = q_out_der_up_down[3]
        q_init_der_down_up[3] = q_out_der_up_down[2]
        q_init_der_down_up[4] = q_out_der_up_down[1]
        q_init_der_down_up[5] = q_out_der_up_down[0] + 0.08
        fksolver_der_down_up.JntToCart(q_init_der_down_up,
                                       finalFrame_der_down_up)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame3 = finalFrame_der_down_up
        desiredFrame3.p[0] = finalFrame_der_down_up.p[0] - 0.05
        desiredFrame3.p[1] = finalFrame_der_down_up.p[1] - 0.04
        desiredFrame3.p[2] = finalFrame_der_down_up.p[2] - 0.02
        print "Desired Position2222der: ", desiredFrame3.p
        q_out_der_down_up = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints())
        ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3,
                                 q_out_der_down_up)
        print "Output angles in rads22222der: ", q_out_der_down_up
        print "VALOR", q_out_der_up_down[5]
        piernas_goal2[0] = -0.3
        piernas_goal2[1] = -0.3
        piernas_goal2[2] = 0
        piernas_goal2[3] = 0.6
        piernas_goal2[4] = 0.3
        piernas_goal2[5] = -0.3

        # 333333333333333333333333333333333333333333333333
        piernas_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        print "Inverse Kinematics"
        desiredFrame4 = PyKDL.Frame()
        fksolver_izq_down_up.JntToCart(q_out_izq_down_up, desiredFrame4)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame4.p[0] = desiredFrame4.p[0]
        desiredFrame4.p[1] = desiredFrame4.p[1] - 0.02
        desiredFrame4.p[2] = desiredFrame4.p[2]
        ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame4,
                                 q_out_izq_down_up)

        q_init_izq_up_down[0] = q_out_izq_down_up[5]
        q_init_izq_up_down[1] = q_out_izq_down_up[4]
        q_init_izq_up_down[2] = q_out_izq_down_up[3]
        q_init_izq_up_down[3] = q_out_izq_down_up[2]
        q_init_izq_up_down[4] = q_out_izq_down_up[1]
        q_init_izq_up_down[5] = q_out_izq_down_up[0]

        desiredFrame5 = PyKDL.Frame()
        fksolver_izq_up_down.JntToCart(q_init_izq_up_down, desiredFrame5)
        desiredFrame5.p[0] = desiredFrame5.p[0]
        desiredFrame5.p[1] = desiredFrame5.p[1] - 0.1
        desiredFrame5.p[2] = desiredFrame5.p[2] + 0.01
        print "Desired Position: ", desiredFrame5.p
        q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame5,
                                 q_out_izq_up_down2)
        print "Output angles in rads: ", q_out_izq_up_down2
        piernas_goal3[6] = q_out_izq_up_down2[0]
        piernas_goal3[7] = q_out_izq_up_down2[1]
        piernas_goal3[8] = q_out_izq_up_down2[2]
        piernas_goal3[9] = q_out_izq_up_down2[3]
        piernas_goal3[10] = q_out_izq_up_down2[4]  #+0.15
        piernas_goal3[11] = q_out_izq_up_down2[5] - 0.08

        print "Inverse Kinematics"

        piernas_goal3[0] = -0.3
        piernas_goal3[1] = -0.3
        piernas_goal3[2] = 0
        piernas_goal3[3] = 0.6
        piernas_goal3[4] = 0.3
        piernas_goal3[5] = -0.3

        # 121212122121212121212121212121212121212121212
        piernas_goal121 = [
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
        ]
        print "Inverse Kinematics"
        desiredFrame6 = PyKDL.Frame()
        fksolver_izq_up_down.JntToCart(q_out_izq_up_down2, desiredFrame6)
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up,
                                       finalFrame_izq_down_up)
        q_init_izq_up_down = q_out_izq_up_down2  # initial angles  #CUIDADO
        desiredFrame6.p[0] = desiredFrame6.p[0]
        desiredFrame6.p[1] = desiredFrame6.p[1] - 0.05
        desiredFrame6.p[2] = desiredFrame6.p[2]  #+0.01
        print "Desired Position: ", desiredFrame6.p
        q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame6,
                                 q_out_izq_up_down)
        print "Output angles in rads_izq_121: ", q_out_izq_up_down

        piernas_goal121[6] = q_out_izq_up_down[0]
        piernas_goal121[7] = q_out_izq_up_down[1]
        piernas_goal121[8] = q_out_izq_up_down[2]
        piernas_goal121[9] = q_out_izq_up_down[3]
        piernas_goal121[10] = q_out_izq_up_down[4]
        piernas_goal121[11] = q_out_izq_up_down[5]

        print "Inverse Kinematics"
        desiredFrame06 = PyKDL.Frame()
        fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame06)
        fksolver_der_down_up.JntToCart(posicionInicial_der_down_up,
                                       finalFrame_der_down_up)
        q_init_der_up_down = q_out_der_up_down  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame06.p[0] = desiredFrame06.p[0]
        desiredFrame06.p[1] = desiredFrame06.p[1]
        desiredFrame06.p[2] = desiredFrame06.p[2]
        print "Desired Position: ", desiredFrame06.p
        q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame06,
                                 q_out_der_up_down)
        print "Output angles in rads: ", q_out_der_up_down

        q_out_der_up_down21 = PyKDL.JntArray(6)
        q_out_der_up_down21 = [-.3, -.3, 0, .6, .3, -.3]
        piernas_goal121[0] = q_out_der_up_down21[0]
        piernas_goal121[1] = q_out_der_up_down21[1]
        piernas_goal121[2] = q_out_der_up_down21[2]
        piernas_goal121[3] = q_out_der_up_down21[3]
        piernas_goal121[4] = q_out_der_up_down21[4]
        piernas_goal121[5] = q_out_der_up_down21[5]

        # 55555555555555555555555555555555555555555
        piernas_goal25 = [
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
        ]
        print "Inverse Kinematics"
        desiredFrame7 = PyKDL.Frame()
        q_init_izq_down_up3 = PyKDL.JntArray(6)
        q_init_izq_down_up3[0] = q_out_izq_up_down[5] * 1
        q_init_izq_down_up3[1] = q_out_izq_up_down[4] * 1
        q_init_izq_down_up3[2] = q_out_izq_up_down[3] * 1
        q_init_izq_down_up3[3] = q_out_izq_up_down[2] * 1
        q_init_izq_down_up3[4] = q_out_izq_up_down[1] * 1
        q_init_izq_down_up3[5] = q_out_izq_up_down[0] * 1
        fksolver_izq_down_up.JntToCart(q_init_izq_down_up3, desiredFrame7)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame7.p[
            0] = desiredFrame7.p[0] + 0.05  #0.03  # PROBAR A PONERLO MAYOR
        desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06  #0.04
        desiredFrame7.p[2] = desiredFrame7.p[2] + 0.005
        print "Desired Position2222: ", desiredFrame7.p
        q_out_izq_down_up5 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_down_up.CartToJnt(q_init_izq_down_up3, desiredFrame7,
                                 q_out_izq_down_up5)
        print "Output angles in rads2222AAAAA: ", q_out_izq_down_up5

        piernas_goal25[6] = q_out_izq_down_up5[5]
        piernas_goal25[7] = q_out_izq_down_up5[4]
        piernas_goal25[8] = q_out_izq_down_up5[3]
        piernas_goal25[9] = q_out_izq_down_up5[2]
        piernas_goal25[10] = q_out_izq_down_up5[1] - 0.05
        piernas_goal25[11] = q_out_izq_down_up5[0] + 0.05

        print "Inverse Kinematics"
        q_init_der_down_up31 = PyKDL.JntArray(6)
        q_init_der_down_up31[
            0] = q_out_der_up_down21[5] * 1  # PROBLEMAS CON LA ASIGNACION
        q_init_der_down_up31[1] = q_out_der_up_down21[4] * 1
        q_init_der_down_up31[2] = q_out_der_up_down21[3] * 1
        q_init_der_down_up31[3] = q_out_der_up_down21[2] * 1
        q_init_der_down_up31[4] = q_out_der_up_down21[1] * 1
        q_init_der_down_up31[5] = q_out_der_up_down21[0] * 1
        desiredFrame7 = PyKDL.Frame()
        fksolver_der_down_up.JntToCart(q_init_der_down_up31, desiredFrame7)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05
        desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06
        desiredFrame7.p[2] = desiredFrame7.p[2] - 0.02
        print "Desired Position2222der: ", desiredFrame7.p
        q_out_der_down_up71 = PyKDL.JntArray(
            cadena_der_down_up.getNrOfJoints())
        ik_der_down_up.CartToJnt(q_init_der_down_up31, desiredFrame7,
                                 q_out_der_down_up71)
        print "Output angles in rads22222der: ", q_out_der_down_up71
        piernas_goal25[0] = q_out_der_down_up71[5]
        piernas_goal25[1] = q_out_der_down_up71[4]
        piernas_goal25[2] = q_out_der_down_up71[3]
        piernas_goal25[3] = q_out_der_down_up71[2]
        piernas_goal25[4] = q_out_der_down_up71[1]
        piernas_goal25[5] = q_out_der_down_up71[0]

        # 333333333333333333333333333333333333333333333333
        piernas_goal341 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        print "Inverse Kinematics"
        desiredFrame441 = PyKDL.Frame()
        fksolver_der_down_up.JntToCart(q_out_der_down_up71, desiredFrame441)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame441.p[0] = desiredFrame441.p[0]
        desiredFrame441.p[1] = desiredFrame441.p[1] - 0.02
        desiredFrame441.p[2] = desiredFrame441.p[2] - 0.015
        ik_der_down_up.CartToJnt(q_out_der_down_up71, desiredFrame441,
                                 q_out_der_down_up71)

        q_init_der_up_down[0] = q_out_der_down_up71[5]
        q_init_der_up_down[1] = q_out_der_down_up71[4]
        q_init_der_up_down[2] = q_out_der_down_up71[3]
        q_init_der_up_down[3] = q_out_der_down_up71[2]
        q_init_der_up_down[4] = q_out_der_down_up71[1]
        q_init_der_up_down[5] = q_out_der_down_up71[0]

        desiredFrame541 = PyKDL.Frame()
        fksolver_der_up_down.JntToCart(q_init_der_up_down, desiredFrame541)
        desiredFrame541.p[0] = desiredFrame541.p[0] - 0.03
        desiredFrame541.p[1] = desiredFrame541.p[1] - 0.1
        desiredFrame541.p[2] = desiredFrame541.p[2] - 0.01  #nada
        print "Desired Position: ", desiredFrame541.p
        q_out_der_up_down241 = PyKDL.JntArray(
            cadena_izq_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame541,
                                 q_out_der_up_down241)  # con desiredFrame5 va
        print "Output angles in radsaaaaa: ", q_out_der_up_down241

        print "Inverse Kinematics"

        piernas_goal341[6] = 0.3
        piernas_goal341[7] = -0.3
        piernas_goal341[8] = 0
        piernas_goal341[9] = 0.6
        piernas_goal341[10] = -0.3
        piernas_goal341[11] = -0.3

        piernas_goal341[0] = q_out_der_up_down241[0]  #+0.1
        piernas_goal341[1] = q_out_der_up_down241[1]
        piernas_goal341[2] = q_out_der_up_down241[2]
        piernas_goal341[3] = q_out_der_up_down241[3]
        piernas_goal341[4] = q_out_der_up_down241[4]  #-0.1
        piernas_goal341[5] = q_out_der_up_down241[5]  #-0.01

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        arm_client = actionlib.SimpleActionClient(
            '/piernas/piernas_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        #/piernas/piernas_controller/follow_joint_trajectory
        arm_client.wait_for_server()

        rospy.loginfo('...connected.')

        print "Los datos son", len(datos)
        # Create a single-point arm trajectory with the piernas_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = piernas_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = datos[0]
        arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(2.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[1].positions = datos[1]
        arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[1].time_from_start = rospy.Duration(4.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[2].positions = datos[2]
        arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[2].time_from_start = rospy.Duration(6.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[3].positions = datos[3]
        arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[3].time_from_start = rospy.Duration(8.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[4].positions = datos[4]
        arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[4].time_from_start = rospy.Duration(10.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[5].positions = datos[5]
        arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[5].time_from_start = rospy.Duration(12.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[6].positions = datos[6]
        arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[6].time_from_start = rospy.Duration(14.0)
        '''arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[7].positions = piernas_goal12
        arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[7].time_from_start = rospy.Duration(17.5)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[8].positions = piernas_goal2
        arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[8].time_from_start = rospy.Duration(19.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[9].positions = piernas_goal3
        arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[9].time_from_start = rospy.Duration(21.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[10].positions = piernas_goal121
        arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[10].time_from_start = rospy.Duration(23.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[11].positions = piernas_goal25
        arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[11].time_from_start = rospy.Duration(25.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[12].positions = piernas_goal341
        arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[12].time_from_start = rospy.Duration(28.0)'''
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')

        # Create an empty trajectory goal
        piernas_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        piernas_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        piernas_goal.goal_time_tolerance = rospy.Duration(0.01)

        # Send the goal to the action server
        arm_client.send_goal(piernas_goal)

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            arm_client.wait_for_result(rospy.Duration(5.0))

        arm_client.wait_for_result()
        print arm_client.get_result()

        rospy.loginfo('...done')
    rospy.loginfo("Waiting for gripper_controller...")
    gripper_client = actionlib.SimpleActionClient(
        "gripper_controller/gripper_action", GripperCommandAction)
    gripper_client.wait_for_server()
    rospy.loginfo("...connected.")

    trajectory = JointTrajectory()
    trajectory.joint_names = head_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = head_joint_positions
    trajectory.points[0].velocities = [0.0] * len(head_joint_positions)
    trajectory.points[0].accelerations = [0.0] * len(head_joint_positions)
    trajectory.points[0].time_from_start = rospy.Duration(5.0)

    head_goal = FollowJointTrajectoryGoal()
    head_goal.trajectory = trajectory
    head_goal.goal_time_tolerance = rospy.Duration(0.0)

    trajectory = JointTrajectory()
    trajectory.joint_names = torso_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = torso_joint_positions
    trajectory.points[0].velocities = [0.0] * len(torso_joint_positions)
    trajectory.points[0].accelerations = [0.0] * len(torso_joint_positions)
    trajectory.points[0].time_from_start = rospy.Duration(5.0)

    torso_goal = FollowJointTrajectoryGoal()
    torso_goal.trajectory = trajectory
    torso_goal.goal_time_tolerance = rospy.Duration(0.0)

    trajectory = JointTrajectory()
Пример #49
0
import actionlib
import math
from control_msgs.msg import FollowJointTrajectoryAction
client = actionlib.SimpleActionClient('/panda_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
from control_msgs.msg import FollowJointTrajectoryGoal
goal = FollowJointTrajectoryGoal()
from trajectory_msgs.msg import JointTrajectory
joint_traj = JointTrajectory()
joint_traj.joint_names = ["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7"]

from trajectory_msgs.msg import JointTrajectoryPoint
point1 = JointTrajectoryPoint()
point2 = JointTrajectoryPoint()
point1.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795, 0.06015034910227879]
point1.velocities = [0., 0., 0., 0., 0., 0., 0.]
#point2.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879]
point2.positions = [0.33877080806309046, -1.5, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879]
point2.velocities = [0., 0., 0., 0., 0., 0., 0.]
point1.time_from_start = rospy.Duration(2.)
point2.time_from_start = rospy.Duration(4.)

from copy import deepcopy
joint_traj.points = [point1, point2, deepcopy(point1), deepcopy(point2)]
joint_traj.points[2].time_from_start += rospy.Duration(4.)
joint_traj.points[3].time_from_start += rospy.Duration(4.)
goal.trajectory = joint_traj
joint_traj.header.stamp = rospy.Time.now()+rospy.Duration(1.0)
client.send_goal_and_wait(goal)

Пример #50
0
    def sendWaypointTrajectory(self,
                               waypoints,
                               durations=0.,
                               vels=0.,
                               accs=0.,
                               effs=0.):
        if not self.ang_cmd_wait(waypoints[0]):
            print 'Cannot go to the first point in the trajectory'
            return None
#    else:
#      print 'Went to first'

        if not self.traj_connection:
            print 'Action server connection was not established'
            return None
        joint_traj = JointTrajectory()
        joint_traj.joint_names = self.arm_joint_names

        if not durations == 0:
            if not len(durations) == waypoints:
                raise Exception(
                    'The number of duration points is not equal to the number of provided waypoints'
                )
        if not vels == 0:
            if not len(vels) == waypoints:
                raise Exception(
                    'The number velocity points is not equal to the number of provided waypoints'
                )
        if not accs == 0:
            if not len(accs) == waypoints:
                raise Exception(
                    'The number acceleration points is not equal to the number of provided waypoints'
                )
        if not effs == 0:
            if not len(effs) == waypoints:
                raise Exception(
                    'The number effort points is not equal to the number of provided waypoints'
                )

        if not effs == 0:
            if not (vels == 0 and accs == 0):
                raise Exception(
                    'Cannot specify efforts with velocities and accelerations at the same time'
                )
        if (not accs == 0) and vels == 0:
            raise Exception('Cannot specify accelerations without velocities')

        total_time_from_start = 0.5
        for t in range(0, len(waypoints)):
            point = JointTrajectoryPoint()

            waypoint = waypoints[t]
            if not len(waypoint) == len(joint_traj.joint_names):
                raise Exception(
                    'The number of provided joint positions is not equal to the number of available joints for index: '
                    + str(t))
            point.positions = waypoint

            if not vels == 0.:
                velocity = vels[t]
                if not len(velocity) == len(joint_traj.joint_names):
                    raise Exception(
                        'The number of provided joint velocities is not equal to the number of available joints for index: '
                        + str(t))
                point.velocities = velocity

            if not accs == 0.:
                acceleration = accs[t]
                if not len(acceleration) == len(joint_traj.joint_names):
                    raise Exception(
                        'The number of provided joint accelerations is not equal to the number of available joints for index: '
                        + str(t))
                point.accelerations = accelerations

            if not effs == 0.:
                effort = effs[t]
                if not len(effort) == len(joint_traj.joint_names):
                    raise Exception(
                        'The number of provided joint efforts is not equal to the number of available joints for index: '
                        + str(t))
                point.effort = effort

            if not durations == 0.:
                point.duration = duration

            # Deal with increasing time for each trajectory point
            point.time_from_start = rospy.Duration(total_time_from_start)
            total_time_from_start = total_time_from_start + 1.0

            # Set the points
            joint_traj.points.append(point)

        traj_goal = FollowJointTrajectoryGoal()
        traj_goal.trajectory = joint_traj

        self.smooth_joint_trajectory_client.send_goal(traj_goal)
        self.smooth_joint_trajectory_client.wait_for_result()
        return self.smooth_joint_trajectory_client.get_result()
Пример #51
0
    def push_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()

        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name

        current_state = rospy.wait_for_message('l_arm_controller/state',
                                               FollowJointTrajectoryFeedback)
        start_angles = current_state.actual.positions

        full_state = rospy.wait_for_message('/joint_states', JointState)

        req = GetPositionFKRequest()
        req.header.frame_id = 'base_link'
        req.fk_link_names = [GRIPPER_LINK_FRAME]
        req.robot_state.joint_state = full_state

        if not self.set_planning_scene_diff_client():
            rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME)
            self.action_server.set_aborted()
            return

        pose = self.get_fk_srv(req).pose_stamped[0].pose

        frame_id = 'base_link'

        approachpos = [pose.position.x, pose.position.y, pose.position.z]
        approachquat = [
            pose.orientation.x, pose.orientation.y, pose.orientation.z,
            pose.orientation.w
        ]

        push_distance = 0.40
        grasppos = [
            pose.position.x, pose.position.y - push_distance, pose.position.z
        ]
        graspquat = [0.00000, 0.00000, 0.70711, -0.70711]

        # attach object to gripper, they will move together
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = GRIPPER_LINK_FRAME
        obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
        obj.object.id = collision_object_name
        obj.link_name = GRIPPER_LINK_FRAME
        obj.touch_links = GRIPPER_LINKS

        self.attached_object_pub.publish(obj)

        # disable collisions between attached object and table
        collision_operation1 = CollisionOperation()
        collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        collision_operation1.object2 = collision_support_surface_name
        collision_operation1.operation = CollisionOperation.DISABLE

        collision_operation2 = CollisionOperation()
        collision_operation2.object1 = collision_support_surface_name
        collision_operation2.object2 = GRIPPER_GROUP_NAME
        collision_operation2.operation = CollisionOperation.DISABLE
        collision_operation2.penetration_distance = 0.02

        ordered_collision_operations = OrderedCollisionOperations()
        ordered_collision_operations.collision_operations = [
            collision_operation1, collision_operation2
        ]

        res = self.check_cartesian_path_lists(
            approachpos,
            approachquat,
            grasppos,
            graspquat,
            start_angles,
            frame=frame_id,
            ordered_collision_operations=ordered_collision_operations)

        error_code_dict = {
            ArmNavigationErrorCodes.SUCCESS: 0,
            ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
            ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
            ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
            ArmNavigationErrorCodes.PLANNING_FAILED: 4
        }

        trajectory_len = len(res.trajectory.joint_trajectory.points)
        trajectory = [
            res.trajectory.joint_trajectory.points[i].positions
            for i in range(trajectory_len)
        ]
        vels = [
            res.trajectory.joint_trajectory.points[i].velocities
            for i in range(trajectory_len)
        ]
        times = [
            res.trajectory.joint_trajectory.points[i].time_from_start
            for i in range(trajectory_len)
        ]
        error_codes = [
            error_code_dict[error_code.val]
            for error_code in res.trajectory_error_codes
        ]

        # if even one code is not 0, abort
        if sum(error_codes) != 0:
            for ind in range(len(trajectory)):
                rospy.loginfo("error code " + str(error_codes[ind]) +
                              " pos : " + self.pplist(trajectory[ind]))

            rospy.loginfo("")

            for ind in range(len(trajectory)):
                rospy.loginfo("time: " + "%5.3f  " % times[ind].to_sec() +
                              "vels: " + self.pplist(vels[ind]))

            rospy.logerr('%s: attempted push failed' % ACTION_NAME)
            self.action_server.set_aborted()
            return

        req = FilterJointTrajectoryRequest()
        req.trajectory = res.trajectory.joint_trajectory
        req.trajectory.points = req.trajectory.points[
            1:]  # skip zero velocity point
        req.allowed_time = rospy.Duration(2.0)

        filt_res = self.trajectory_filter_srv(req)

        goal = FollowJointTrajectoryGoal()
        goal.trajectory = filt_res.trajectory
        goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)

        self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
        rospy.sleep(0.5)

        self.trajectory_controller.send_goal(goal)
        self.trajectory_controller.wait_for_result()

        state = self.trajectory_controller.get_state()

        if state == GoalStatus.SUCCEEDED:
            rospy.sleep(0.5)
            sound_msg = self.stop_audio_recording_srv(True)
            self.action_server.set_succeeded(
                PushObjectResult(sound_msg.recorded_sound))
            return

        rospy.logerr('%s: attempted push failed' % ACTION_NAME)
        self.stop_audio_recording_srv(False)
        self.action_server.set_aborted()
Пример #52
0
    def __init__(self):

        pkl_file = open('datos_articulacionespkl.pkl', 'rb')

        # data1 = pickle.load(pkl_file)
        # pprint.pprint(data1)

        # data2 = pickle.load(pkl_file)
        # pprint.pprint(data2)

        # print data1['b'][0]
        # for i in data2:
        #    print i

        datos = []
        try:
            while True:  # loop indefinitely
                print "LEYENDO"
                # print pickle.load(pkl_file)
                datos.append(pickle.load(
                    pkl_file))  # add each item from the file to a list
        except EOFError:  # the exception is used to break the loop
            pass  # we don't need to do anything special

        pkl_file.close()
        print len(datos)

        # print datos_trans
        for i in datos:
            pass
            # print i

        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints = [
            'cilinder_blue_box1_der_joint', 'cilinder_blue1_box2_der_joint',
            'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint',
            'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
            'cilinder_blue_box1_izq_joint', 'cilinder_blue1_box2_izq_joint',
            'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint',
            'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint'
        ]

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        arm_client = actionlib.SimpleActionClient(
            '/piernas/piernas_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        #/piernas/piernas_controller/follow_joint_trajectory
        arm_client.wait_for_server()

        rospy.loginfo('...connected.')

        # Create a single-point arm trajectory with the piernas_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = piernas_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = datos[0]
        arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(1.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[1].positions = datos[1]
        arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[1].time_from_start = rospy.Duration(4.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[2].positions = datos[2]
        arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[2].time_from_start = rospy.Duration(6.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[3].positions = datos[3]
        arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[3].time_from_start = rospy.Duration(8.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[4].positions = datos[4]
        arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[4].time_from_start = rospy.Duration(10.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[5].positions = datos[5]
        arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[5].time_from_start = rospy.Duration(12.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[6].positions = datos[6]
        arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[6].time_from_start = rospy.Duration(14.0)

        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[7].positions = datos[1]
        arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[7].time_from_start = rospy.Duration(17.5)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[8].positions = datos[2]
        arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[8].time_from_start = rospy.Duration(19.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[9].positions = datos[3]
        arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[9].time_from_start = rospy.Duration(21.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[10].positions = datos[4]
        arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[10].time_from_start = rospy.Duration(23.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[11].positions = datos[5]
        arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[11].time_from_start = rospy.Duration(25.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[12].positions = datos[6]
        arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[12].time_from_start = rospy.Duration(28.0)
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')

        # Create an empty trajectory goal
        piernas_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        piernas_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        piernas_goal.goal_time_tolerance = rospy.Duration(0.01)

        # Send the goal to the action server
        arm_client.send_goal(piernas_goal)

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            arm_client.wait_for_result(rospy.Duration(5.0))

        arm_client.wait_for_result()
        arm_client.send_goal(piernas_goal)
        print arm_client.get_result()

        rospy.loginfo('...done')
Пример #53
0
    def __init__(self):

        filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf'
        robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)

        (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
        cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
        cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
        cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
        cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")

        print cadena_der_up_down.getNrOfSegments()
        fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down)
        fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up)
        fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down)
        fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up)

        vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
        ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down)

        vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
        ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up)

        vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
        ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down)

        vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
        ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up)

        nj_izq = cadena_der_up_down.getNrOfJoints()
        posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_der_up_down[0] = 0.3
        posicionInicial_der_up_down[1] = -0.3
        posicionInicial_der_up_down[2] = 0
        posicionInicial_der_up_down[3] = 0.6
        posicionInicial_der_up_down[4] = -0.3
        posicionInicial_der_up_down[5] = -0.3

        nj_izq = cadena_izq_up_down.getNrOfJoints()
        posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_up_down[0] = 0.3
        posicionInicial_izq_up_down[1] = -0.3
        posicionInicial_izq_up_down[2] = 0.0
        posicionInicial_izq_up_down[3] = 0.6
        posicionInicial_izq_up_down[4] = -0.3
        posicionInicial_izq_up_down[5] = -0.3

        nj_izq = cadena_der_down_up.getNrOfJoints()
        posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_der_down_up[5] = 0.3
        posicionInicial_der_down_up[4] = -0.3
        posicionInicial_der_down_up[3] = 0.0
        posicionInicial_der_down_up[2] = 0.6
        posicionInicial_der_down_up[1] = -0.3
        posicionInicial_der_down_up[0] = -0.3

        nj_izq = cadena_izq_down_up.getNrOfJoints()
        posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_down_up[5] = 0.3
        posicionInicial_izq_down_up[4] = -0.3
        posicionInicial_izq_down_up[3] = 0.0
        posicionInicial_izq_down_up[2] = 0.6
        posicionInicial_izq_down_up[1] = -0.3
        posicionInicial_izq_down_up[0] = -0.3
        print "Forward kinematics"
        finalFrame_izq_up_down = PyKDL.Frame()
        finalFrame_izq_down_up = PyKDL.Frame()
        finalFrame_der_up_down = PyKDL.Frame()
        finalFrame_der_down_up = PyKDL.Frame()


        print "Rotational Matrix of the final Frame: "
        print  finalFrame_izq_up_down.M
        print "End-effector position: ", finalFrame_izq_up_down.p

        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints = ['cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint', 'cilinder_blue_box2_der_joint',
                        'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
                          'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue_box2_izq_joint',
                          'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ]


        if reset:
            # Set the arm back to the resting position
            arm_goal  = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        else:
            # Set a goal configuration for the arm
            arm_goal  =  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        #11111111111111111111111111111111111111111111
        print "Inverse Kinematics"
        fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down)
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
        q_init_izq_up_down = posicionInicial_izq_up_down  # initial angles
        #desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame = finalFrame_izq_up_down
        desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
        desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
        desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
        print "Desired Position: ", desiredFrame.p
        q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
        print "Output angles in rads: ", q_out_izq_up_down

        arm_goal[6] = q_out_izq_up_down[0]
        arm_goal[7] = q_out_izq_up_down[1]
        arm_goal[8] = q_out_izq_up_down[2]
        arm_goal[9] = q_out_izq_up_down[3]
        arm_goal[10] = q_out_izq_up_down[4]
        arm_goal[11] = q_out_izq_up_down[5]

        print "Inverse Kinematics"
        fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down)
        fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
        q_init_der_up_down = posicionInicial_der_up_down  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame = finalFrame_der_up_down
        desiredFrame.p[0] = finalFrame_der_up_down.p[0]
        desiredFrame.p[1] = finalFrame_der_up_down.p[1]-0.06
        desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02
        print "Desired Position: ", desiredFrame.p
        q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down)
        print "Output angles in rads: ", q_out_der_up_down

        arm_goal[0] = q_out_der_up_down[0]
        arm_goal[1] = q_out_der_up_down[1]
        arm_goal[2] = q_out_der_up_down[2]
        arm_goal[3] = q_out_der_up_down[3]
        arm_goal[4] = q_out_der_up_down[4]
        arm_goal[5] = q_out_der_up_down[5]

        # 222222222222222222222222222222222222222222
        arm_goal2 =  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        print "Inverse Kinematics"
        q_init_izq_down_up = posicionInicial_izq_down_up  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame2 = finalFrame_izq_down_up
        desiredFrame2.p[0] = finalFrame_izq_down_up.p[0] - 0.05
        desiredFrame2.p[1] = finalFrame_izq_down_up.p[1] - 0.06
        desiredFrame2.p[2] = finalFrame_izq_down_up.p[2] - 0.02
        print "Desired Position: ", desiredFrame2.p
        q_out_izq_down_up = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_down_up.CartToJnt(q_init_izq_down_up, desiredFrame2, q_out_izq_down_up)
        print "Output angles in rads: ", q_out_izq_down_up

        arm_goal2[6] = q_out_izq_down_up[5]
        arm_goal2[7] = q_out_izq_down_up[4]
        arm_goal2[8] = q_out_izq_down_up[3]
        arm_goal2[9] = q_out_izq_down_up[2]
        arm_goal2[10] = q_out_izq_down_up[1]
        arm_goal2[11] = q_out_izq_down_up[0]

        print "Inverse Kinematics"
        q_init_der_down_up = q_out_der_up_down  # initial angles
        q_init_der_down_up[0] = q_out_der_up_down[5]
        q_init_der_down_up[1] = q_out_der_up_down[4]
        q_init_der_down_up[2] = q_out_der_up_down[3]
        q_init_der_down_up[3] = q_out_der_up_down[2]
        q_init_der_down_up[4] = q_out_der_up_down[1]
        q_init_der_down_up[5] = q_out_der_up_down[0]
        fksolver_der_down_up.JntToCart(q_init_der_down_up,finalFrame_der_down_up)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame3 = finalFrame_der_down_up
        desiredFrame3.p[0] = finalFrame_der_down_up.p[0] - 0.05
        desiredFrame3.p[1] = finalFrame_der_down_up.p[1] - 0.06
        desiredFrame3.p[2] = finalFrame_der_down_up.p[2] - 0.02
        print "Desired Position: ", desiredFrame3.p
        q_out_der_down_up = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
        ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3, q_out_der_down_up)
        print "Output angles in rads: ", q_out_der_down_up

        arm_goal2[0] = q_out_der_down_up[5]
        arm_goal2[1] = q_out_der_down_up[4]
        arm_goal2[2] = q_out_der_down_up[3]
        arm_goal2[3] = q_out_der_down_up[2]
        arm_goal2[4] = q_out_der_down_up[1]
        arm_goal2[5] = q_out_der_down_up[0]


        # 333333333333333333333333333333333333333333333333
        arm_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        print "Inverse Kinematics"
        fksolver_izq_down_up.JntToCart(q_out_izq_down_up,finalFrame_izq_down_up)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame3 = finalFrame_izq_down_up
        desiredFrame3.p[0] = finalFrame_izq_down_up.p[0]
        desiredFrame3.p[1] = finalFrame_izq_down_up.p[1] - 0.02
        desiredFrame3.p[2] = finalFrame_izq_down_up.p[2]
        ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame3, q_out_izq_down_up)

        q_init_izq_up_down[0] = q_out_izq_down_up[5]
        q_init_izq_up_down[1] = q_out_izq_down_up[4]
        q_init_izq_up_down[2] = q_out_izq_down_up[3]
        q_init_izq_up_down[3] = q_out_izq_down_up[2]
        q_init_izq_up_down[4] = q_out_izq_down_up[1]
        q_init_izq_up_down[5] = q_out_izq_down_up[0]
        fksolver_izq_up_down.JntToCart(q_init_izq_up_down,finalFrame_izq_up_down)

        desiredFrame4 = finalFrame_izq_up_down
        desiredFrame4.p[0] = finalFrame_izq_up_down.p[0]
        desiredFrame4.p[1] = finalFrame_izq_up_down.p[1] - 0.13
        desiredFrame4.p[2] = finalFrame_izq_up_down.p[2] + 0.012
        print "Desired Position: ", desiredFrame4.p
        q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame4, q_out_izq_up_down2)
        print "Output angles in rads: ", q_out_izq_up_down2
        arm_goal3[6] = q_out_izq_up_down2[0]
        arm_goal3[7] = q_out_izq_up_down2[1]
        arm_goal3[8] = q_out_izq_up_down2[2]
        arm_goal3[9] = q_out_izq_up_down2[3]
        arm_goal3[10] = q_out_izq_up_down2[4]
        arm_goal3[11] = q_out_izq_up_down2[5]

        print "Inverse Kinematics"

        arm_goal3[0] = -0.4
        arm_goal3[1] = -0.25
        arm_goal3[2] = 0
        arm_goal3[3] = 0.5
        arm_goal3[4] = 0.4
        arm_goal3[5] = -0.25

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        arm_client = actionlib.SimpleActionClient('/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction)

        arm_client.wait_for_server()

        rospy.loginfo('...connected.')


        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = piernas_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(1.0)
        #arm_trajectory.points.append(JointTrajectoryPoint())
        #arm_trajectory.points[1].positions = arm_goal2
        #arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints]
        #arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints]
        #arm_trajectory.points[1].time_from_start = rospy.Duration(3.0)
        #arm_trajectory.points.append(JointTrajectoryPoint())
        #arm_trajectory.points[2].positions = arm_goal3
        #arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints]
        #arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints]
        #arm_trajectory.points[2].time_from_start = rospy.Duration(4.0)
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')

        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.01)

        # Send the goal to the action server
        arm_client.send_goal(arm_goal)

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            arm_client.wait_for_result(rospy.Duration(5.0))


        rospy.loginfo('...done')
Пример #54
0
    def wipe(self, start, finish):
        dist = int(round(
            200 *
            self.calc_dist(start, finish)))  # 1 point per cm of separation
        print "Steps: %s" % dist
        x_step = finish.pose.position.x - start.pose.position.x
        y_step = finish.pose.position.y - start.pose.position.y
        z_step = finish.pose.position.z - start.pose.position.z
        #print "Increments: %s,%s,%s" %(x_step, y_step, z_step)

        qs = [
            start.pose.orientation.x, start.pose.orientation.y,
            start.pose.orientation.z, start.pose.orientation.w
        ]
        qf = [
            finish.pose.orientation.x, finish.pose.orientation.y,
            finish.pose.orientation.z, finish.pose.orientation.w
        ]
        steps = []
        #print "Start: %s" %start
        #print "Finish: %s" %finish
        for i in range(dist):
            frac = float(i) / float(dist)
            steps.append(PoseStamped())
            steps[i].header.stamp = rospy.Time.now()
            steps[i].header.frame_id = start.header.frame_id
            steps[i].pose.position.x = start.pose.position.x + x_step * frac
            steps[i].pose.position.y = start.pose.position.y + y_step * frac
            steps[i].pose.position.z = start.pose.position.z + z_step * frac
            new_q = transformations.quaternion_slerp(qs, qf, frac)
            steps[i].pose.orientation.x = new_q[0]
            steps[i].pose.orientation.y = new_q[1]
            steps[i].pose.orientation.z = new_q[2]
            steps[i].pose.orientation.w = new_q[3]
        steps.append(finish)
        #print "Steps:"
        #print steps
        #raw_input("Press Enter to continue")
        rospy.loginfo("Planning straight-line path, please wait")
        self.wt_log_out.publish(
            data="Planning straight-line path, please wait")

        rospy.loginfo("Initiating wipe action")
        self.blind_move(finish)
        self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
        rospy.loginfo("At beginning of path")
        pts = []

        for i, p in enumerate(steps):
            frac = float(i) / float(len(steps))
            request = self.form_ik_request(p)
            if not i == 0:
                request.ik_request.ik_seed_state.joint_state.position = seed
            ik_goal = self.ik_pose_proxy(request)
            pts.append(ik_goal.solution.joint_state.position)
            seed = pts[i]

        points = []
        for i in range(len(pts) - 1):
            angs1 = pts[i]
            angs2 = pts[i + 1]
            increm = np.subtract(angs2, angs1)
            for j in range(10):
                points.append(np.add(angs1, np.multiply(0.1 * j, increm)))

        #points = np.unwrap(points,1)
        p1 = points
        traj = JointTrajectory()
        traj.header.frame_id = steps[0].header.frame_id
        traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
        times = []
        for i in range(len(points)):
            frac = float(i) / float(len(points))
            traj.points.append(JointTrajectoryPoint())
            traj.points[i].positions = points[i]
            traj.points[i].velocities = [0] * 7
            times.append(rospy.Duration(frac * dist * 0.2))

        traj_goal = FollowJointTrajectoryGoal()
        traj_goal.trajectory = traj
        tolerance = JointTolerance()
        tolerance.position = 0.05
        tolerance.velocity = 0.1
        traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
        traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
        traj_goal.goal_time_tolerance = rospy.Duration(3)

        #print "Steps: %s" %steps
        count = 0

        while count < 6:
            traj_goal.trajectory.points.reverse()
            for i in range(len(times)):
                traj_goal.trajectory.points[i].time_from_start = times[i]
            if count == 0:
                print traj_goal.trajectory.points
                raw_input("Review Trajectory Goal")
            traj_goal.trajectory.header.stamp = rospy.Time.now()
            self.r_arm_follow_traj_client.send_goal(traj_goal)
            self.r_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
            rospy.sleep(0.5)
            count += 1

        #traj_goal = JointTrajectoryGoal()
        #traj_goal.trajectory = traj
        #print "Steps: %s" %steps
        #count = 0


#
#while count < 6:
#traj_goal.trajectory.points.reverse()
#for i in range(len(times)):
#traj_goal.trajectory.points[i].time_from_start = times[i]
#print traj_goal
#raw_input("Review Trajectory Goal")
##print "Traj goal start:"
##print traj_goal.trajectory.points[0].positions
##print "Traj goal end:"
##print traj_goal.trajectory.points[-1].positions
#traj_goal.trajectory.header.stamp = rospy.Time.now()
#self.r_arm_traj_client.send_goal(traj_goal)
#self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
#rospy.sleep(0.5)
#count += 1
        rospy.loginfo("Done Wiping")
        self.wt_log_out.publish(data="Done Wiping")
        self.linear_move(Float32(-0.15))
    randoms = []
    max_value = max - min
    for i in range(n):
        randoms.append((random.random() * max_value) + min) # this scales the value between min and max
    return randoms

if __name__ == '__main__':
    rospy.init_node('random_left_leg_goals')
    ctl_as = SimpleActionClient(CONTROLLER_AS, FollowJointTrajectoryAction)
    rospy.loginfo("Connecting to " + CONTROLLER_AS)
    ctl_as.wait_for_server()
    rospy.loginfo("Connected.")
    
    while not rospy.is_shutdown():
        fjtg = FollowJointTrajectoryGoal()
        jt = JointTrajectory()
        #jt.joint_names = ['leg_left_1_joint', 'leg_left_2_joint', 'leg_left_3_joint', 'leg_left_4_joint', 'leg_left_5_joint', 'leg_left_6_joint']
        jt.joint_names = JOINT_NAMES
        jtp = JointTrajectoryPoint()
        jtp.positions = get_n_randoms(len(JOINT_NAMES), -4.0, 4.0)
        jtp.velocities = [0.0] * len(JOINT_NAMES)
        #jtp.accelerations = [0.0] * len(JOINT_NAMES)
        #jtp.effort = [0.0] * len(JOINT_NAMES)
        jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s
        jt.points.append(jtp)
        fjtg.trajectory = jt
        ctl_as.send_goal_and_wait(fjtg)
        rospy.loginfo("Sent: " + str(jt))
        rospy.sleep(jtp.time_from_start)

    def run(self, pan_amount=0.0, tilt_amount=0.0, ignore_errors=True):
        """
        The run function for this step

        Args:
            pan_amount (float) : the amount in radians to pan the head by. left
                (+ve) and right (-ve)
            tilt_amount (float) : the amount in radians to tilt the head by.
                down (+ve) and up (-ve)
            ignore_errors (bool) : if ``True``, ignore the trajectory time limit
                exceeded errors that sometimes pop up from the
                :const:`HEAD_ACTION_SERVER`

        .. seealso::

            :meth:`task_executor.abstract_step.AbstractStep.run`
        """
        rospy.loginfo("Action {}: Pan by amount {}, Tilt by amount {}".format(
            self.name, pan_amount, tilt_amount))

        # Update the desired pan and tilt positions based on the joint limits
        desired_pan = max(LookPanTiltAction.HEAD_PAN_MIN,
                          self._current_pan + pan_amount)
        desired_pan = min(LookPanTiltAction.HEAD_PAN_MAX, desired_pan)
        desired_tilt = max(LookPanTiltAction.HEAD_TILT_MIN,
                           self._current_tilt + tilt_amount)
        desired_tilt = min(LookPanTiltAction.HEAD_TILT_MAX, desired_tilt)

        # Create and send the goal height
        trajectory = JointTrajectory()
        trajectory.joint_names = self._joint_names
        trajectory.points.append(JointTrajectoryPoint())
        trajectory.points[0].positions = [desired_pan, desired_tilt]
        trajectory.points[0].velocities = [0.0, 0.0]
        trajectory.points[0].accelerations = [0.0, 0.0]
        trajectory.points[0].time_from_start = rospy.Duration(self._duration)

        follow_goal = FollowJointTrajectoryGoal()
        follow_goal.trajectory = trajectory

        self._head_client.send_goal(follow_goal)
        self.notify_action_send_goal(LookPanTiltAction.HEAD_ACTION_SERVER,
                                     follow_goal)

        # Yield an empty dict while we're executing
        while self._head_client.get_state(
        ) in AbstractStep.RUNNING_GOAL_STATES:
            yield self.set_running()

        # Wait for a result and yield based on how we exited
        status = self._head_client.get_state()
        self._head_client.wait_for_result()
        result = self._head_client.get_result()
        self.notify_action_recv_result(LookPanTiltAction.HEAD_ACTION_SERVER,
                                       status, result)

        if status == GoalStatus.PREEMPTED:
            yield self.set_preempted(action=self.name,
                                     status=status,
                                     goal=[desired_pan, desired_tilt],
                                     result=result)
        elif status == GoalStatus.SUCCEEDED or ignore_errors:
            yield self.set_succeeded()
        else:
            yield self.set_aborted(action=self.name,
                                   status=status,
                                   goal=[desired_pan, desired_tilt],
                                   result=result)
Пример #57
0
    def __init__(self):

        filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf'
        datos_articulaciones = open('datos_articulaciones.pkl', 'wb')

        robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)

        (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
        cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
        cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
        cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
        cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")

        print cadena_der_up_down.getNrOfSegments()
        fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down)
        fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up)
        fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down)
        fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up)

        vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
        ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down)

        vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
        ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up)

        vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
        ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down)

        vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
        ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up)

        nj_izq = cadena_der_up_down.getNrOfJoints()
        posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_der_up_down[0] = 0.3
        posicionInicial_der_up_down[1] = -0.3
        posicionInicial_der_up_down[2] = 0
        posicionInicial_der_up_down[3] = 0.6
        posicionInicial_der_up_down[4] = -0.3
        posicionInicial_der_up_down[5] = -0.3

        nj_izq = cadena_izq_up_down.getNrOfJoints()
        posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_up_down[0] = 0.3
        posicionInicial_izq_up_down[1] = -0.3
        posicionInicial_izq_up_down[2] = 0.0
        posicionInicial_izq_up_down[3] = 0.6
        posicionInicial_izq_up_down[4] = -0.3
        posicionInicial_izq_up_down[5] = -0.3

        nj_izq = cadena_der_down_up.getNrOfJoints()
        posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_der_down_up[5] = 0.3
        posicionInicial_der_down_up[4] = -0.3
        posicionInicial_der_down_up[3] = 0.0
        posicionInicial_der_down_up[2] = 0.6
        posicionInicial_der_down_up[1] = -0.3
        posicionInicial_der_down_up[0] = -0.3

        nj_izq = cadena_izq_down_up.getNrOfJoints()
        posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_down_up[5] = 0.3
        posicionInicial_izq_down_up[4] = -0.3
        posicionInicial_izq_down_up[3] = 0.0
        posicionInicial_izq_down_up[2] = 0.6
        posicionInicial_izq_down_up[1] = -0.3
        posicionInicial_izq_down_up[0] = -0.3
        print "Forward kinematics"
        finalFrame_izq_up_down = PyKDL.Frame()
        finalFrame_izq_down_up = PyKDL.Frame()
        finalFrame_der_up_down = PyKDL.Frame()
        finalFrame_der_down_up = PyKDL.Frame()


        print "Rotational Matrix of the final Frame: "
        print  finalFrame_izq_up_down.M
        print "End-effector position: ", finalFrame_izq_up_down.p

        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints = ['cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint',  'cilinder_blue_box2_der_joint',
                          'cilinder_blue_box4_der_joint',  'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
                          'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint',  'cilinder_blue_box2_izq_joint',
                          'cilinder_blue_box4_izq_joint',  'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ]


        piernas_goal  = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        #11111111111111111111111111111111111111111111
        print "Inverse Kinematics"
        fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down)
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
        q_init_izq_up_down = posicionInicial_izq_up_down  # initial angles
        desiredFrame = finalFrame_izq_up_down
        desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
        desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
        desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
        print "Desired Position: ", desiredFrame.p
        q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
        print "Output angles in rads: ", q_out_izq_up_down

        piernas_goal[6] = q_out_izq_up_down[0]
        piernas_goal[7] = q_out_izq_up_down[1]
        piernas_goal[8] = q_out_izq_up_down[2]
        piernas_goal[9] = q_out_izq_up_down[3]
        piernas_goal[10] = q_out_izq_up_down[4]
        piernas_goal[11] = q_out_izq_up_down[5]

        print "Inverse Kinematics"
        fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down)
        fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
        q_init_der_up_down = posicionInicial_der_up_down  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame = finalFrame_der_up_down
        desiredFrame.p[0] = finalFrame_der_up_down.p[0]
        desiredFrame.p[1] = finalFrame_der_up_down.p[1]
        desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02 #0.02
        print "Desired Position: ", desiredFrame.p
        q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down)
        print "Output angles in rads: ", q_out_der_up_down

        piernas_goal[0] = q_out_der_up_down[0]
        piernas_goal[1] = q_out_der_up_down[1]
        piernas_goal[2] = q_out_der_up_down[2]
        piernas_goal[3] = q_out_der_up_down[3]
        piernas_goal[4] = q_out_der_up_down[4]
        piernas_goal[5] = q_out_der_up_down[5]

        #121212122121212121212121212121212121212121212
        piernas_goal12 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        print "Inverse Kinematics"
        desiredFrame = PyKDL.Frame()
        fksolver_izq_up_down.JntToCart(q_out_izq_up_down, desiredFrame)
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
        q_init_izq_up_down = posicionInicial_izq_up_down  # initial angles
        desiredFrame.p[0] = desiredFrame.p[0]
        desiredFrame.p[1] = desiredFrame.p[1]
        desiredFrame.p[2] = desiredFrame.p[2]
        print "Desired Position: ", desiredFrame.p
        q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
        print "Output angles in rads: ", q_out_izq_up_down

        piernas_goal12[6] = q_out_izq_up_down[0]
        piernas_goal12[7] = q_out_izq_up_down[1]
        piernas_goal12[8] = q_out_izq_up_down[2]
        piernas_goal12[9] = q_out_izq_up_down[3]
        piernas_goal12[10] = q_out_izq_up_down[4]
        piernas_goal12[11] = q_out_izq_up_down[5]

        print "Inverse Kinematics"
        desiredFrame0 = PyKDL.Frame()
        fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame0)
        fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
        q_init_der_up_down = posicionInicial_der_up_down  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame0.p[0] = desiredFrame0.p[0]
        desiredFrame0.p[1] = desiredFrame0.p[1] -0.07
        desiredFrame0.p[2] = desiredFrame0.p[2]
        print "Desired Position: ", desiredFrame0.p
        q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame0, q_out_der_up_down)
        print "Output angles in rads: ", q_out_der_up_down

        piernas_goal12[0] = q_out_der_up_down[0]
        piernas_goal12[1] = q_out_der_up_down[1]
        piernas_goal12[2] = q_out_der_up_down[2]
        piernas_goal12[3] = q_out_der_up_down[3]
        piernas_goal12[4] = q_out_der_up_down[4]
        piernas_goal12[5] = q_out_der_up_down[5]

        # 222222222222222222222222222222222222222222
        piernas_goal2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        print "Inverse Kinematics"
        desiredFrame2 = PyKDL.Frame()
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, desiredFrame2)
        q_init_izq_down_up = posicionInicial_izq_down_up  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame2.p[0] = desiredFrame2.p[0] -0.06 #0.05
        desiredFrame2.p[1] = desiredFrame2.p[1] -0.06#0.06
        desiredFrame2.p[2] = desiredFrame2.p[2] -0.01  #0.02
        print "Desired Position2222aaa: ", desiredFrame2.p
        #print desiredFrame2.M
        q_out_izq_down_up = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_down_up.CartToJnt(q_init_izq_down_up, desiredFrame2, q_out_izq_down_up)
        print "Output angles in rads2222: ", q_out_izq_down_up

        piernas_goal2[6] = q_out_izq_down_up[5]#+0.1
        piernas_goal2[7] = q_out_izq_down_up[4]#-0.05
        piernas_goal2[8] = q_out_izq_down_up[3]
        piernas_goal2[9] = q_out_izq_down_up[2]
        piernas_goal2[10] = q_out_izq_down_up[1]
        piernas_goal2[11] = q_out_izq_down_up[0]

        #q_out_izq_down_up[4] -=0.1

        print "Inverse Kinematics"
        q_init_der_down_up = PyKDL.JntArray(6)
        q_init_der_down_up[0] = q_out_der_up_down[5] # PROBLEMAS CON LA ASIGNACION
        q_init_der_down_up[1] = q_out_der_up_down[4]
        q_init_der_down_up[2] = q_out_der_up_down[3]
        q_init_der_down_up[3] = q_out_der_up_down[2]
        q_init_der_down_up[4] = q_out_der_up_down[1]
        q_init_der_down_up[5] = q_out_der_up_down[0]+0.08
        fksolver_der_down_up.JntToCart(q_init_der_down_up,finalFrame_der_down_up)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame3 = finalFrame_der_down_up
        desiredFrame3.p[0] = finalFrame_der_down_up.p[0]- 0.05
        desiredFrame3.p[1] = finalFrame_der_down_up.p[1]- 0.04
        desiredFrame3.p[2] = finalFrame_der_down_up.p[2]-0.02
        print "Desired Position2222der: ", desiredFrame3.p
        q_out_der_down_up = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints())
        ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3, q_out_der_down_up)
        print "Output angles in rads22222der: ", q_out_der_down_up
        print "VALOR", q_out_der_up_down[5]
        piernas_goal2[0] = -0.3
        piernas_goal2[1] = -0.3
        piernas_goal2[2] = 0
        piernas_goal2[3] = 0.6
        piernas_goal2[4] = 0.3
        piernas_goal2[5] = -0.3


        # 333333333333333333333333333333333333333333333333
        piernas_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        print "Inverse Kinematics"
        desiredFrame4 = PyKDL.Frame()
        fksolver_izq_down_up.JntToCart(q_out_izq_down_up,desiredFrame4)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame4.p[0] = desiredFrame4.p[0]
        desiredFrame4.p[1] = desiredFrame4.p[1] - 0.02
        desiredFrame4.p[2] = desiredFrame4.p[2]
        ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame4, q_out_izq_down_up)

        q_init_izq_up_down[0] = q_out_izq_down_up[5]
        q_init_izq_up_down[1] = q_out_izq_down_up[4]
        q_init_izq_up_down[2] = q_out_izq_down_up[3]
        q_init_izq_up_down[3] = q_out_izq_down_up[2]
        q_init_izq_up_down[4] = q_out_izq_down_up[1]
        q_init_izq_up_down[5] = q_out_izq_down_up[0]

        desiredFrame5 = PyKDL.Frame()
        fksolver_izq_up_down.JntToCart(q_init_izq_up_down,desiredFrame5)
        desiredFrame5.p[0] = desiredFrame5.p[0]
        desiredFrame5.p[1] = desiredFrame5.p[1] - 0.1
        desiredFrame5.p[2] = desiredFrame5.p[2]+0.01
        print "Desired Position: ", desiredFrame5.p
        q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame5, q_out_izq_up_down2)
        print "Output angles in rads: ", q_out_izq_up_down2
        piernas_goal3[6] = q_out_izq_up_down2[0]
        piernas_goal3[7] = q_out_izq_up_down2[1]
        piernas_goal3[8] = q_out_izq_up_down2[2]
        piernas_goal3[9] = q_out_izq_up_down2[3]
        piernas_goal3[10] = q_out_izq_up_down2[4]#+0.15
        piernas_goal3[11] = q_out_izq_up_down2[5]-0.08

        print "Inverse Kinematics"

        piernas_goal3[0] = -0.3
        piernas_goal3[1] = -0.3
        piernas_goal3[2] = 0
        piernas_goal3[3] = 0.6
        piernas_goal3[4] = 0.3
        piernas_goal3[5] = -0.3

        # 121212122121212121212121212121212121212121212
        piernas_goal121 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        print "Inverse Kinematics"
        desiredFrame6 = PyKDL.Frame()
        fksolver_izq_up_down.JntToCart(q_out_izq_up_down2, desiredFrame6)
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
        q_init_izq_up_down = q_out_izq_up_down2  # initial angles  #CUIDADO
        desiredFrame6.p[0] = desiredFrame6.p[0]
        desiredFrame6.p[1] = desiredFrame6.p[1]-0.05
        desiredFrame6.p[2] = desiredFrame6.p[2]#+0.01
        print "Desired Position: ", desiredFrame6.p
        q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame6, q_out_izq_up_down)
        print "Output angles in rads_izq_121: ", q_out_izq_up_down

        piernas_goal121[6] = q_out_izq_up_down[0]
        piernas_goal121[7] = q_out_izq_up_down[1]
        piernas_goal121[8] = q_out_izq_up_down[2]
        piernas_goal121[9] = q_out_izq_up_down[3]
        piernas_goal121[10] = q_out_izq_up_down[4]
        piernas_goal121[11] = q_out_izq_up_down[5]

        print "Inverse Kinematics"
        desiredFrame06 = PyKDL.Frame()
        fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame06)
        fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
        q_init_der_up_down = q_out_der_up_down  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame06.p[0] = desiredFrame06.p[0]
        desiredFrame06.p[1] = desiredFrame06.p[1]
        desiredFrame06.p[2] = desiredFrame06.p[2]
        print "Desired Position: ", desiredFrame06.p
        q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame06, q_out_der_up_down)
        print "Output angles in rads: ", q_out_der_up_down

        q_out_der_up_down21 = PyKDL.JntArray(6)
        q_out_der_up_down21 = [-.3, -.3, 0, .6, .3, -.3 ]
        piernas_goal121[0] = q_out_der_up_down21[0]
        piernas_goal121[1] = q_out_der_up_down21[1]
        piernas_goal121[2] = q_out_der_up_down21[2]
        piernas_goal121[3] = q_out_der_up_down21[3]
        piernas_goal121[4] = q_out_der_up_down21[4]
        piernas_goal121[5] = q_out_der_up_down21[5]

        # 55555555555555555555555555555555555555555
        piernas_goal25 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        print "Inverse Kinematics"
        desiredFrame7= PyKDL.Frame()
        q_init_izq_down_up3 = PyKDL.JntArray(6)
        q_init_izq_down_up3[0] = q_out_izq_up_down[5] * 1
        q_init_izq_down_up3[1] = q_out_izq_up_down[4] * 1
        q_init_izq_down_up3[2] = q_out_izq_up_down[3] * 1
        q_init_izq_down_up3[3] = q_out_izq_up_down[2] * 1
        q_init_izq_down_up3[4] = q_out_izq_up_down[1] * 1
        q_init_izq_down_up3[5] = q_out_izq_up_down[0] * 1
        fksolver_izq_down_up.JntToCart(q_init_izq_down_up3, desiredFrame7)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05#0.03  # PROBAR A PONERLO MAYOR
        desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06#0.04
        desiredFrame7.p[2] = desiredFrame7.p[2] + 0.005
        print "Desired Position2222: ", desiredFrame7.p
        q_out_izq_down_up5 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_down_up.CartToJnt(q_init_izq_down_up3, desiredFrame7, q_out_izq_down_up5)
        print "Output angles in rads2222AAAAA: ", q_out_izq_down_up5

        piernas_goal25[6] = q_out_izq_down_up5[5]
        piernas_goal25[7] = q_out_izq_down_up5[4]
        piernas_goal25[8] = q_out_izq_down_up5[3]
        piernas_goal25[9] = q_out_izq_down_up5[2]
        piernas_goal25[10] = q_out_izq_down_up5[1]-0.05
        piernas_goal25[11] = q_out_izq_down_up5[0]+0.05

        print "Inverse Kinematics"
        q_init_der_down_up31 = PyKDL.JntArray(6)
        q_init_der_down_up31[0] = q_out_der_up_down21[5] *1 # PROBLEMAS CON LA ASIGNACION
        q_init_der_down_up31[1] = q_out_der_up_down21[4] *1
        q_init_der_down_up31[2] = q_out_der_up_down21[3] *1
        q_init_der_down_up31[3] = q_out_der_up_down21[2] *1
        q_init_der_down_up31[4] = q_out_der_up_down21[1] *1
        q_init_der_down_up31[5] = q_out_der_up_down21[0] *1
        desiredFrame7 = PyKDL.Frame()
        fksolver_der_down_up.JntToCart(q_init_der_down_up31, desiredFrame7)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05
        desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06
        desiredFrame7.p[2] = desiredFrame7.p[2] - 0.02
        print "Desired Position2222der: ", desiredFrame7.p
        q_out_der_down_up71 = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints())
        ik_der_down_up.CartToJnt(q_init_der_down_up31, desiredFrame7, q_out_der_down_up71)
        print "Output angles in rads22222der: ", q_out_der_down_up71
        piernas_goal25[0] = q_out_der_down_up71[5]
        piernas_goal25[1] = q_out_der_down_up71[4]
        piernas_goal25[2] = q_out_der_down_up71[3]
        piernas_goal25[3] = q_out_der_down_up71[2]
        piernas_goal25[4] = q_out_der_down_up71[1]
        piernas_goal25[5] = q_out_der_down_up71[0]

        # 333333333333333333333333333333333333333333333333
        piernas_goal341 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        print "Inverse Kinematics"
        desiredFrame441 = PyKDL.Frame()
        fksolver_der_down_up.JntToCart(q_out_der_down_up71, desiredFrame441)
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame441.p[0] = desiredFrame441.p[0]
        desiredFrame441.p[1] = desiredFrame441.p[1] - 0.02
        desiredFrame441.p[2] = desiredFrame441.p[2] - 0.015
        ik_der_down_up.CartToJnt(q_out_der_down_up71, desiredFrame441, q_out_der_down_up71)

        q_init_der_up_down[0] = q_out_der_down_up71[5]
        q_init_der_up_down[1] = q_out_der_down_up71[4]
        q_init_der_up_down[2] = q_out_der_down_up71[3]
        q_init_der_up_down[3] = q_out_der_down_up71[2]
        q_init_der_up_down[4] = q_out_der_down_up71[1]
        q_init_der_up_down[5] = q_out_der_down_up71[0]

        desiredFrame541 = PyKDL.Frame()
        fksolver_der_up_down.JntToCart(q_init_der_up_down, desiredFrame541)
        desiredFrame541.p[0] = desiredFrame541.p[0] - 0.03
        desiredFrame541.p[1] = desiredFrame541.p[1] - 0.1
        desiredFrame541.p[2] = desiredFrame541.p[2] - 0.01 #nada
        print "Desired Position: ", desiredFrame541.p
        q_out_der_up_down241 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame541, q_out_der_up_down241)# con desiredFrame5 va
        print "Output angles in radsaaaaa: ", q_out_der_up_down241

        print "Inverse Kinematics"

        piernas_goal341[6] = 0.3
        piernas_goal341[7] = -0.3
        piernas_goal341[8] = 0
        piernas_goal341[9] = 0.6
        piernas_goal341[10] = -0.3
        piernas_goal341[11] = -0.3

        piernas_goal341[0] = q_out_der_up_down241[0]#+0.1
        piernas_goal341[1] = q_out_der_up_down241[1]
        piernas_goal341[2] = q_out_der_up_down241[2]
        piernas_goal341[3] = q_out_der_up_down241[3]
        piernas_goal341[4] = q_out_der_up_down241[4]#-0.1
        piernas_goal341[5] = q_out_der_up_down241[5]#-0.01

        pickle.dump(piernas_goal, datos_articulaciones, -1)
        pickle.dump(piernas_goal12, datos_articulaciones, -1)
        pickle.dump(piernas_goal2, datos_articulaciones, -1)
        pickle.dump(piernas_goal3, datos_articulaciones, -1)
        pickle.dump(piernas_goal121, datos_articulaciones, -1)
        pickle.dump(piernas_goal25, datos_articulaciones, -1)
        pickle.dump(piernas_goal341, datos_articulaciones, -1)

        datos_articulaciones.close()

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        arm_client = actionlib.SimpleActionClient('/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        #/piernas/piernas_controller/follow_joint_trajectory
        arm_client.wait_for_server()

        rospy.loginfo('...connected.')



        # Create a single-point arm trajectory with the piernas_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = piernas_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = piernas_goal
        arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(2.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[1].positions = piernas_goal12
        arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[1].time_from_start = rospy.Duration(4.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[2].positions = piernas_goal2
        arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[2].time_from_start = rospy.Duration(6.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[3].positions = piernas_goal3
        arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[3].time_from_start = rospy.Duration(8.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[4].positions = piernas_goal121
        arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[4].time_from_start = rospy.Duration(10.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[5].positions = piernas_goal25
        arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[5].time_from_start = rospy.Duration(12.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[6].positions = piernas_goal341
        arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[6].time_from_start = rospy.Duration(14.0)

        '''arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[7].positions = piernas_goal12
        arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[7].time_from_start = rospy.Duration(17.5)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[8].positions = piernas_goal2
        arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[8].time_from_start = rospy.Duration(19.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[9].positions = piernas_goal3
        arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[9].time_from_start = rospy.Duration(21.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[10].positions = piernas_goal121
        arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[10].time_from_start = rospy.Duration(23.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[11].positions = piernas_goal25
        arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[11].time_from_start = rospy.Duration(25.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[12].positions = piernas_goal341
        arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[12].time_from_start = rospy.Duration(28.0)'''
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')



        # Create an empty trajectory goal
        piernas_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        piernas_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        piernas_goal.goal_time_tolerance = rospy.Duration(0.01)

        # Send the goal to the action server
        arm_client.send_goal(piernas_goal)

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            arm_client.wait_for_result(rospy.Duration(5.0))

        arm_client.wait_for_result()
        print arm_client.get_result()


        rospy.loginfo('...done')
Пример #58
0
        mpr.allowed_planning_time = 3.0  # [s]
        # print mpr
        try:
            req = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan)
            res = req(mpr)
            traj = res.motion_plan_response.trajectory
        except rospy.ServiceException, e:
            print "Service call failed: %s" % e

        client = actionlib.SimpleActionClient(self.joint_traj_action_topic,
                                              FollowJointTrajectoryAction)
        client.wait_for_server()

        # http://docs.ros.org/diamondback/api/control_msgs/html/index-msg.html
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = traj.joint_trajectory
        client.send_goal(goal)
        client.wait_for_result(rospy.Duration.from_sec(15.0))
        # http://docs.ros.org/api/actionlib/html/classactionlib_1_1simple__action__client_1_1SimpleActionClient.html
        # print client.get_state()

    def move_from_pregrasp_to_grasp(self, second):
        if self.verbose:
            print 'Moving from pregrasp to grasp'
        # Cartesian path
        header = Header()
        header.frame_id = "/" + self.reference_frame
        start_state = RobotState()
        start_state.joint_state.header.frame_id = "/" + self.reference_frame
        names = []
        vals = []
Пример #59
0
    def __init__(self):

        joint_state_topic = ['joint_states:=/iiwa/joint_states']
        moveit_commander.roscpp_initialize(joint_state_topic)

        rospy.init_node('moveit_ik_demo')

        arm = moveit_commander.MoveGroupCommander('arm')
        gripper = moveit_commander.MoveGroupCommander('gripper')

        end_effector_link = arm.get_end_effector_link()

        reference_frame = 'iiwa_link_0'
        arm.set_pose_reference_frame(reference_frame)

        gripper_joints = ['finger_joint']

        arm.allow_replanning(True)

        # ----set gripper action client----
        gripper_client = actionlib.SimpleActionClient(
            'iiwa/gripper_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction)
        gripper_client.wait_for_server()
        rospy.loginfo('...connected.')

        # ----set accuracy----
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.01)

        gripper.set_goal_position_tolerance(0.01)
        gripper.set_goal_orientation_tolerance(0.05)

        # ----set arm poses----

        # pose1
        target_pose1 = PoseStamped()
        target_pose1.header.frame_id = reference_frame
        target_pose1.header.stamp = rospy.Time.now()

        target_pose1.pose.position.x = 0.6
        target_pose1.pose.position.y = 0.0
        target_pose1.pose.position.z = 0.3
        target_pose1.pose.orientation.x = 0
        target_pose1.pose.orientation.y = 1
        target_pose1.pose.orientation.z = 0
        target_pose1.pose.orientation.w = 0

        # pose1
        target_pose2 = PoseStamped()
        target_pose2.header.frame_id = reference_frame
        target_pose2.header.stamp = rospy.Time.now()

        target_pose2.pose.position.x = 0.0
        target_pose2.pose.position.y = 0.6
        target_pose2.pose.position.z = 0.3
        target_pose2.pose.orientation.x = 0
        target_pose2.pose.orientation.y = 1
        target_pose2.pose.orientation.z = 0
        target_pose2.pose.orientation.w = 0

        # ----set gripepr poses----
        gripper_open = [0.05]
        gripper_close = [0.5]

        gripper_traj_open = JointTrajectory()
        gripper_traj_open.joint_names = gripper_joints
        gripper_traj_open.points.append(JointTrajectoryPoint())
        gripper_traj_open.points[0].positions = gripper_open
        gripper_traj_open.points[0].velocities = [0.0 for i in gripper_joints]
        gripper_traj_open.points[0].accelerations = [
            0.0 for i in gripper_joints
        ]
        gripper_traj_open.points[0].time_from_start = rospy.Duration(3.0)

        gripper_traj_close = JointTrajectory()
        gripper_traj_close.joint_names = gripper_joints
        gripper_traj_close.points.append(JointTrajectoryPoint())
        gripper_traj_close.points[0].positions = gripper_close
        gripper_traj_close.points[0].velocities = [0.0 for i in gripper_joints]
        gripper_traj_close.points[0].accelerations = [
            0.0 for i in gripper_joints
        ]
        gripper_traj_close.points[0].time_from_start = rospy.Duration(3.0)

        gripper_goal = FollowJointTrajectoryGoal()
        gripper_goal.trajectory = gripper_traj_open
        gripper_goal.goal_time_tolerance = rospy.Duration(0.0)
        gripper_client.send_goal(gripper_goal)
        gripper_client.wait_for_result(rospy.Duration(5.0))

        # other setting
        shift_value = -0.05

        while (True):
            arm.set_named_target('home')
            arm.go()
            rospy.sleep(2)

            # arm pose1
            arm.set_start_state_to_current_state()

            arm.set_pose_target(target_pose1, end_effector_link)

            traj = arm.plan()
            arm.execute(traj)
            rospy.sleep(1)

            # move close to target
            arm.shift_pose_target(2, shift_value, end_effector_link)
            arm.go()
            rospy.sleep(1)

            # close griper
            gripper_goal = FollowJointTrajectoryGoal()
            gripper_goal.trajectory = gripper_traj_close
            gripper_goal.goal_time_tolerance = rospy.Duration(0.0)
            gripper_client.send_goal(gripper_goal)
            gripper_client.wait_for_result(rospy.Duration(5.0))

            # arm pose2
            arm.set_start_state_to_current_state()

            arm.set_pose_target(target_pose2, end_effector_link)

            traj = arm.plan()
            arm.execute(traj)
            rospy.sleep(1)

            # move close to target
            arm.shift_pose_target(2, shift_value, end_effector_link)
            arm.go()
            rospy.sleep(1)

            # open griper
            gripper_goal = FollowJointTrajectoryGoal()
            gripper_goal.trajectory = gripper_traj_open
            gripper_goal.goal_time_tolerance = rospy.Duration(0.0)
            gripper_client.send_goal(gripper_goal)
            gripper_client.wait_for_result(rospy.Duration(5.0))

            arm.set_named_target('home')
            arm.go()
            rospy.sleep(2)
        """
        arm.shift_pose_target(1, -0.05, end_effector_link)
        arm.go()
        rospy.sleep(1)
  
        
        arm.shift_pose_target(3, -1.57, end_effector_link)
        arm.go()
        rospy.sleep(1)
           
        
        arm.set_named_target('home')
        arm.go()

        """
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Пример #60
0
print("Generating client...")
client = actionlib.SimpleActionClient('left_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)

print("Waiting for server...")
client.wait_for_server()

# create goal
goal = FollowJointTrajectoryGoal()

# define goal trajectory
waypoint1 = JointTrajectoryPoint()
waypoint1.positions = [0, 1, .75, 0.5]
waypoint1.time_from_start = Duration(0.0)

traj1 = JointTrajectory()
traj1.joint_names = ["l_shoulder_y", "l_shoulder_x", "l_arm_z", "l_elbow_y"]
traj1.points = [waypoint1]

# set goal trajectory
goal.trajectory = traj1

# send goal
client.send_goal(goal, feedback_cb=feedback_cb)
print("Sent goal")
 
client.wait_for_result()
print('[Result] State: %d'%(client.get_state()))

time.sleep(15)