Python FollowJointTrajectoryGoal.trajectory方法代码示例

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
    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()

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
    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()

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
    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)

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
 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')

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
 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

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
    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')

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
 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

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
    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")

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
 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

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
    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")

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
    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

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
    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

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
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

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
  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

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import trajectory [as 别名]
    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')