def joint_state_publisher(self):
 
 
     try:
 
         joint_state_msg = JointState()
         joint_fb_msg = FollowJointTrajectoryFeedback()
         time = rospy.Time.now()
         
         with self.lock:
             
             #Joint states
                 
             joint_state_msg.header.stamp = time
             joint_state_msg.name = self.joint_names
             joint_state_msg.position = self.motion_ctrl.get_joint_positions()
             
             #self.joint_state_pub.publish(joint_state_msg)
             
             #Joint feedback
             joint_fb_msg.header.stamp = time
             joint_fb_msg.joint_names = self.joint_names
             joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions()
             
             #self.joint_feedback_pub.publish(joint_fb_msg)
                      
     except Exception as e:
         rospy.logerr('Unexpected exception in joint state publisher: %s', e)
Exemple #2
0
def feedback_states_publisher(rate):

	global current_joints
	global current_velocity
	
	publisher = rospy.Publisher('feedback_states', FollowJointTrajectoryFeedback, queue_size = conf_rate)
	sleeper = rospy.Rate(rate)

	while not rospy.is_shutdown():

		msg = FollowJointTrajectoryFeedback()
		msg.header.stamp = rospy.Time.now()
		msg.header.frame_id = conf_base_name
		msg.joint_names = conf_joint_names
		msg.actual = JointTrajectoryPoint()
		# -----------------------------------------------------------
		# Critical Section
		# -----------------------------------------------------------
		with lock:
			msg.actual.positions = current_joints
			msg.actual.velocities = current_velocity
			msg.actual.effort = current_effort
			msg.actual.time_from_start = rospy.get_rostime() - trajectory_start
		# -----------------------------------------------------------

		publisher.publish(msg)
		sleeper.sleep()
Exemple #3
0
    def _state_publisher(self):
        try:
            joint_state_msg = JointState()
            joint_fb_msg = FollowJointTrajectoryFeedback()

            with self._lock:

                time = rospy.Time.now()

                q_actual, q_desired, q_error = self._rsi_connection.get_joint_states(
                )

                #Joint states
                joint_state_msg.header.stamp = time
                joint_state_msg.name = self._joint_names
                joint_state_msg.position = q_actual

                self._joint_state_pub.publish(joint_state_msg)

                #Joint feedback
                joint_fb_msg.header.stamp = time
                joint_fb_msg.joint_names = self._joint_names
                joint_fb_msg.actual.positions = q_actual
                joint_fb_msg.desired.positions = q_desired
                joint_fb_msg.error.positions = q_error

                self._joint_feedback_pub.publish(joint_fb_msg)

        except Exception as e:
            rospy.logerr('Unexpected exception in joint state publisher: %s',
                         e)
    def joint_state_publisher(self):

        joint_state_msg = JointState()
        joint_state_msg.header.stamp = rospy.Time.now()
        joint_state_msg.name = self.ABB_joint_name_list + self.gripper3F_joint_name_list
        joint_state_msg.position = list(self.ABB_joint_states_list) + list(
            self.gripper3F_joint_states_list)
        joint_state_msg.velocity = []
        joint_state_msg.effort = []

        control_state_msg = FollowJointTrajectoryFeedback()
        control_state_msg.header.stamp = rospy.Time.now()
        control_state_msg.joint_names = self.ABB_joint_name_list + self.gripper3F_joint_name_list
        control_state_msg.actual.positions = list(
            self.ABB_feedback_states_list) + list(
                self.gripper3F_feedback_states_list)
        control_state_msg.desired.positions = list(
            self.ABB_feedback_states_list) + list(
                self.gripper3F_feedback_states_list)
        control_state_msg.error.positions = [
            0 for i in control_state_msg.joint_names
        ]

        self.joint_state_pub.publish(joint_state_msg)
        self.control_state_pub.publish(control_state_msg)
    def joint_state_publisher(self):
        if self.EnableFlag == 1 and self.motion_ctrl.positionUpdatedFlag == '1':
            try:
                joint_state_msg = JointState()
                joint_fb_msg = FollowJointTrajectoryFeedback()
                time = rospy.Time.now()

                with self.lock:
                    #Joint states
                    joint_state_msg.header.stamp = time
                    joint_state_msg.name = self.joint_names
                    joint_state_msg.position = self.motion_ctrl.get_joint_positions(
                    )

                    # self.joint_state_pub.publish(joint_state_msg)

                    #Joint feedback
                    joint_fb_msg.header.stamp = time
                    joint_fb_msg.joint_names = self.joint_names
                    joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions(
                    )

                    self.joint_feedback_pub.publish(joint_fb_msg)

            except Exception as e:
                rospy.loginfo(
                    'Unexpected exception in joint state publisher: %s', e)
Exemple #6
0
    def execute_cb(self, goal):
        rospy.loginfo("Received new goal:\n%s"%goal)
        success = True
        start_time = rospy.Time.now()

        i = 1
        previouspoint = None
        for point in goal.trajectory.points:
            
            time_to_wait = start_time + point.time_from_start - rospy.Time.now()
            
           
            	#previouspoint=point
            
            if time_to_wait > rospy.Duration(0):
                #rospy.loginfo("Sleeping for %s seconds..."%time_to_wait.to_sec())
                rospy.loginfo("Sleeping for 0.3 second")
                rospy.sleep(0.3)

            
            # only publish feedback if we have received some position recently
            if previouspoint and self.latestpositions:
                fb = FollowJointTrajectoryFeedback()
                fb.desired = previouspoint
                fb.actual = JointTrajectoryPoint()
                fb.actual.positions = self.latestpositions
                fb.error = JointTrajectoryPoint()
                fb.error.positions = map(sub, fb.desired.positions, fb.actual.positions)
                self.action_server.publish_feedback(fb)
            
            # only use positions and velocities (velocities are always positive)
            point.velocities = map(abs, point.velocities)
            point.accelerations = []
            point.effort = []
            
            rospy.loginfo("Go to point %d:\n%s"%(i, point))
            serial_command=Float64MultiArray()
            previous_position=Float64MultiArray()

            #serial_command.data= tuple(numpy.subtract(point.positions,previouspoint.positions))
             
            serial_command.data=point.positions
            
            self.pub_command.publish(serial_command) 
            self.latestpositions = None
            self.pub_trajpoint.publish(point)
            
            previouspoint = point
            i += 1

        if success:
            rospy.loginfo('%s: Succeeded' % self.action_name)
            self.result = FollowJointTrajectoryResult()
            self.result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
    	    self.action_server.set_succeeded(self.result)
            self.traj_complete.publish(1)
    def read_joint_state_data(self, msg):

        joint_fb_msg = FollowJointTrajectoryFeedback()

        joint_fb_msg.header.stamp = rospy.Time.now()
        joint_fb_msg.joint_names = msg.name 
        joint_fb_msg.actual.positions = msg.position
        joint_fb_msg.actual.velocities = msg.velocity
                
        self.joint_feedback_pub.publish(joint_fb_msg)
Exemple #8
0
    def on_goal(self, goal_handle):
        """Handle a new goal trajectory command."""
        # Checks if the joints are just incorrect
        if set(goal_handle.get_goal().trajectory.joint_names) != set(
                self.prefixedJointNames):
            rospy.logerr(
                "Received a goal with incorrect joint names: (%s)" %
                ', '.join(goal_handle.get_goal().trajectory.joint_names))
            goal_handle.set_rejected()
            return

        if not trajectory_is_finite(goal_handle.get_goal().trajectory):
            rospy.logerr("Received a goal with infinites or NaNs")
            goal_handle.set_rejected(
                text="Received a goal with infinites or NaNs")
            return

        # Checks that the trajectory has velocities
        if not has_velocities(goal_handle.get_goal().trajectory):
            rospy.logerr("Received a goal without velocities")
            goal_handle.set_rejected(text="Received a goal without velocities")
            return

        feedback = FollowJointTrajectoryFeedback()
        result = FollowJointTrajectoryResult()
        feedback.joint_names = goal_handle.get_goal().trajectory.joint_names
        # Orders the joints of the trajectory according to joint_names
        reorder_trajectory_joints(goal_handle.get_goal().trajectory,
                                  self.prefixedJointNames)

        # Inserts the current setpoint at the head of the trajectory
        now = self.robot.getTime()
        print 'trajectory_t0: {}'.format(self.trajectory_t0)
        point0 = sample_trajectory(self.trajectory, now - self.trajectory_t0)
        point0.time_from_start = rospy.Duration(0.0)
        goal_handle.get_goal().trajectory.points.insert(0, point0)

        feedback.desired.positions = point0

        self.trajectory_t0 = now
        print "i am here"
        print 'point0: {}'.format(point0.positions)

        # Replaces the goal
        self.goal_handle = goal_handle
        self.trajectory = goal_handle.get_goal().trajectory

        self.feedback = feedback
        self.result = result

        goal_handle.set_accepted()
Exemple #9
0
    def __init__(self, limb, reconfig_server, rate=100.0):
        self._dyn = reconfig_server
        self._ns = 'robot/limb/' + limb + '/follow_joint_trajectory'
        self._server = actionlib.SimpleActionServer(
            self._ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)
        self._action_name = rospy.get_name()
        self._server.start()
        self._limb = baxter_interface.Limb(limb)

        # Action Feedback/Result
        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()

        # Controller parameters from arguments and dynamic reconfigure
        self._control_rate = rate  # Hz
        self._control_joints = []
        self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
        self._goal_time = 0.0
        self._goal_error = dict()
        self._error_threshold = dict()
        self._dflt_vel = dict()

        # Create our PID controllers
        self._pid = dict()
        for joint in self._limb.joint_names():
            self._pid[joint] = baxter_control.PID()

        # Set joint state publishing to specified control rate
        self._pub_rate = rospy.Publisher(
            '/robot/joint_state_publish_rate', UInt16)
        self._pub_rate.publish(self._control_rate)
Exemple #10
0
    def initialize(self):
	# subscribe to joint states to check if the arm is moving, based on velocities of each joint
        rospy.Subscriber("/joint_states", JointState, self.jointStatesCallback, queue_size=1)
        ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
        self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)
        self.stopped_velocity_tolerance = rospy.get_param(ns + '/stopped_velocity_tolerance', 0.01)
        self.goal_constraints = []
        self.trajectory_constraints = []
        self.min_velocity = rospy.get_param(self.controller_namespace + '/joint_trajectory_action_node/min_velocity', 0.1)
        
        for joint in self.joint_names:
            self.goal_constraints.append(rospy.get_param(ns + '/' + joint + '/goal', -1.0))
            self.trajectory_constraints.append(rospy.get_param(ns + '/' + joint + '/trajectory', -1.0))
            
        # Message containing current state for all controlled joints
        self.msg = FollowJointTrajectoryFeedback()
        self.msg.joint_names = self.joint_names
        self.msg.desired.positions = [0.0] * self.num_joints
        self.msg.desired.velocities = [0.0] * self.num_joints
        self.msg.desired.accelerations = [0.0] * self.num_joints
        self.msg.actual.positions = [0.0] * self.num_joints
        self.msg.actual.velocities = [0.0] * self.num_joints
        self.msg.error.positions = [0.0] * self.num_joints
        self.msg.error.velocities = [0.0] * self.num_joints
        
        return True
    def __init__(self,
                 limb,
                 reconfig_server,
                 rate=100.0,
                 mode='position_w_id',
                 interpolation='minjerk'):
        self._dyn = reconfig_server
        self._ns = 'robot/limb/' + limb
        self._fjt_ns = self._ns + '/follow_joint_trajectory'
        self._server = actionlib.SimpleActionServer(
            self._fjt_ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)
        self._action_name = rospy.get_name()
        self._limb = intera_interface.Limb(limb, synchronous_pub=True)
        self._enable = intera_interface.RobotEnable()
        self._name = limb
        self._interpolation = interpolation
        self._cuff = intera_interface.Cuff(limb=limb)
        # Verify joint control mode
        self._mode = mode
        if (self._mode != 'position' and self._mode != 'position_w_id'
                and self._mode != 'velocity'):
            rospy.logerr("%s: Action Server Creation Failed - "
                         "Provided Invalid Joint Control Mode '%s' (Options: "
                         "'position_w_id', 'position', 'velocity')" % (
                             self._action_name,
                             self._mode,
                         ))
            return
        self._server.start()
        self._alive = True
        # Action Feedback/Result
        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()

        # Controller parameters from arguments, messages, and dynamic
        # reconfigure
        self._control_rate = rate  # Hz
        self._control_joints = []
        self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
        self._goal_time = 0.0
        self._stopped_velocity = 0.0
        self._goal_error = dict()
        self._path_thresh = dict()

        # Create our PID controllers
        self._pid = dict()
        for joint in self._limb.joint_names():
            self._pid[joint] = intera_control.PID()

        # Create our spline coefficients
        self._coeff = [None] * len(self._limb.joint_names())

        # Set joint state publishing to specified control rate
        self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate',
                                         UInt16,
                                         queue_size=10)
        self._pub_rate.publish(self._control_rate)
    def initialize(self):
        ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
        self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)
        self.stopped_velocity_tolerance = rospy.get_param(
            ns + '/stopped_velocity_tolerance', 0.01)
        self.goal_constraints = []
        self.trajectory_constraints = []
        self.min_velocity = rospy.get_param(
            self.controller_namespace +
            '/joint_trajectory_action_node/min_velocity', 0.1)
        # Topic name for publish joint_states
        self.joint_states_topic = rospy.get_param(
            self.controller_namespace +
            '/joint_trajectory_action_node/joint_states_topic',
            '/joint_states')

        for joint in self.joint_names:
            self.goal_constraints.append(
                rospy.get_param(ns + '/' + joint + '/goal', -1.0))
            self.trajectory_constraints.append(
                rospy.get_param(ns + '/' + joint + '/trajectory', -1.0))

        # Message containing current state for all controlled joints
        self.msg = FollowJointTrajectoryFeedback()
        self.msg.joint_names = self.joint_names
        self.msg.desired.positions = [0.0] * self.num_joints
        self.msg.desired.velocities = [0.0] * self.num_joints
        self.msg.desired.accelerations = [0.0] * self.num_joints
        self.msg.actual.positions = [0.0] * self.num_joints
        self.msg.actual.velocities = [0.0] * self.num_joints
        self.msg.error.positions = [0.0] * self.num_joints
        self.msg.error.velocities = [0.0] * self.num_joints

        return True
    def state_msg(self, ref_state, type_msg):
        if type_msg == 'state':
            st = JointTrajectoryControllerState()
        elif type_msg == 'feedback':
            st = FollowJointTrajectoryFeedback()
        st.joint_names = [self.gripper_joint_name]
        st.header = ref_state.header

        st.desired.positions.extend(
            [p * 2.0 for p in ref_state.desired.positions[:1]])
        st.desired.velocities.extend(ref_state.desired.velocities[:1])
        st.desired.accelerations.extend(ref_state.desired.accelerations[:1])
        st.desired.effort.extend(ref_state.desired.effort[:1])
        st.desired.time_from_start = ref_state.desired.time_from_start

        st.actual.positions.extend(
            [p * 2.0 for p in ref_state.actual.positions[:1]])
        st.actual.velocities.extend(ref_state.actual.velocities[:1])
        st.actual.accelerations.extend(ref_state.actual.accelerations[:1])
        st.actual.effort.extend(ref_state.actual.effort[:1])
        st.actual.time_from_start = ref_state.actual.time_from_start

        st.error.positions.extend(
            [p * 2.0 for p in ref_state.error.positions[:1]])
        st.error.velocities.extend(ref_state.error.velocities[:1])
        st.error.accelerations.extend(ref_state.error.accelerations[:1])
        st.error.effort.extend(ref_state.error.effort[:1])

        return st
    def __init__(self, reconfig_server, rate=100.0, interpolation='minjerk'):
        self._dyn = reconfig_server
        self._fjt_ns = "position_joint_trajectory_controller/follow_joint_trajectory"
        self._server = actionlib.SimpleActionServer(
            self._fjt_ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)
        self._arm = franka_interface.ArmInterface(synchronous_pub=True)
        self._enable = franka_interface.RobotEnable()
        self._interpolation = interpolation
        # Verify joint control mode
        self._server.start()
        self._alive = True
        # Action Feedback/Result
        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()

        # Controller parameters from arguments, messages, and dynamic
        # reconfigure
        self._control_rate = rate  # Hz
        self._control_joints = []
        self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
        self._goal_time = 0.0
        self._stopped_velocity = 0.0001
        self._goal_error = dict()
        self._path_thresh = dict()

        # Create our PID controllers
        self._pid = dict()
        for joint in self._arm.joint_names():
            self._pid[joint] = pid.PID()

        # Create our spline coefficients
        self._coeff = [None] * len(self._arm.joint_names())
Exemple #15
0
 def __init__(self, topic_in):
     pub_topic = rospy.get_param(node_name + '/pub_topic',
                                 '/feedback_states')
     rospy.Subscriber(topic_in, JointState, self._callBack)
     self.joint_trajectory_pub = rospy.Publisher(
         pub_topic, FollowJointTrajectoryFeedback, queue_size=1)
     self.follow_joint_trajectory_feedback_ = FollowJointTrajectoryFeedback(
     )
Exemple #16
0
    def joint_state_publisher(self):
        LOG.info(
            "QWERQWERQWERQWEQWRQWERQWERWQERQWERQWERQWERQWERQWERERQWQREWQWREQWERQWER!"
        )
        print('comecomecomecomecomecomecomecomecomecomecome')
        joint_state_msg = JointState()
        joint_state_msg.header.stamp = rospy.Time.now()

        if self.robot_name == 'NRMK-IndyRP2':
            joint_state_msg.name = [
                'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5',
                'joint6'
            ]
        else:
            joint_state_msg.name = [
                'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5'
            ]

        cur_joint_pos = self.indy.get_joint_pos()

        joint_state_msg.position = utils_transf.degs2rads(cur_joint_pos)
        joint_state_msg.velocity = [self.vel]
        joint_state_msg.effort = []

        control_state_msg = FollowJointTrajectoryFeedback()
        control_state_msg.header.stamp = rospy.Time.now()
        if self.robot_name == 'NRMK-IndyRP2':
            control_state_msg.joint_names = [
                'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5',
                'joint6'
            ]
        else:
            control_state_msg.joint_names = [
                'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5'
            ]

        control_state_msg.actual.positions = utils_transf.degs2rads(
            cur_joint_pos)
        control_state_msg.desired.positions = utils_transf.degs2rads(
            cur_joint_pos)
        control_state_msg.error.positions = [
            0 for i in control_state_msg.joint_names
        ]

        self.joint_state_pub.publish(joint_state_msg)
        self.control_state_pub.publish(control_state_msg)
Exemple #17
0
    def __init__(self, node):
        self.node = node
        self.server = actionlib.SimpleActionServer(
            '/stretch_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction,
            execute_cb=self.execute_cb,
            auto_start=False)
        self.feedback = FollowJointTrajectoryFeedback()
        self.result = FollowJointTrajectoryResult()

        r = self.node.robot
        head_pan_range_ticks = r.head.motors['head_pan'].params['range_t']
        head_pan_range_rad = (r.head.motors['head_pan'].ticks_to_world_rad(
            head_pan_range_ticks[1]),
                              r.head.motors['head_pan'].ticks_to_world_rad(
                                  head_pan_range_ticks[0]))
        head_tilt_range_ticks = r.head.motors['head_tilt'].params['range_t']
        head_tilt_range_rad = (r.head.motors['head_tilt'].ticks_to_world_rad(
            head_tilt_range_ticks[1]),
                               r.head.motors['head_tilt'].ticks_to_world_rad(
                                   head_tilt_range_ticks[0]))
        wrist_yaw_range_ticks = r.end_of_arm.motors['wrist_yaw'].params[
            'range_t']
        wrist_yaw_range_rad = (
            r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(
                wrist_yaw_range_ticks[1]),
            r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(
                wrist_yaw_range_ticks[0]))
        gripper_range_ticks = r.end_of_arm.motors['stretch_gripper'].params[
            'range_t']
        gripper_range_rad = (
            r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(
                gripper_range_ticks[0]),
            r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(
                gripper_range_ticks[1]))
        gripper_range_robotis = (
            r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(
                gripper_range_rad[0]),
            r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(
                gripper_range_rad[1]))

        self.head_pan_cg = HeadPanCommandGroup(
            head_pan_range_rad, self.node.head_pan_calibrated_offset_rad,
            self.node.head_pan_calibrated_looked_left_offset_rad)
        self.head_tilt_cg = HeadTiltCommandGroup(
            head_tilt_range_rad, self.node.head_tilt_calibrated_offset_rad,
            self.node.head_tilt_calibrated_looking_up_offset_rad,
            self.node.head_tilt_backlash_transition_angle_rad)
        self.wrist_yaw_cg = WristYawCommandGroup(wrist_yaw_range_rad)
        self.gripper_cg = GripperCommandGroup(gripper_range_robotis)
        self.telescoping_cg = TelescopingCommandGroup(
            tuple(r.arm.params['range_m']),
            self.node.wrist_extension_calibrated_retracted_offset_m)
        self.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m']))
        self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5,
                                                                      0.5))
Exemple #18
0
    def execute_cb_arm(self, goal):
        # create feedback msg
        self.arm_feedback = FollowJointTrajectoryFeedback()
        self.arm_feedback.joint_names = goal.trajectory.joint_names
        ## notice that the order in goal.trajectory.joint_names is different
        ## from the default order in self.joint_names!!!!
        #~ print goal.trajectory.joint_names
        print "arm"
        #~ print len(goal.trajectory.points)
        print goal.trajectory.points

        # we execute the trajectory at 5Hz, which means we execute 5
        # points per second
        action_rate = rospy.Rate(5)

        ## for each point in the planned trajectory, we excute it through
        ## SPI and send feedback
        for trajectory_point in goal.trajectory.points:
            # the actual jointstate is same as the required jointstate
            self.arm_feedback.actual = trajectory_point
            # we send the feedback infomation back to the client
            self._as_arm.publish_feedback(self.arm_feedback)

            # send the jointstates through SPI in the correct order !!!!
            for i in range(len(self.jointStates) - 1):
                self.jointStates[i] = trajectory_point.positions[
                    goal.trajectory.joint_names.index(self.joint_names[i])]
            print self.jointStates
            ########################## IMPORTANT #######################
            ### add your own linear mapping between self.jointStates and self.jointPWMs
            ### self.jointPWWs = f(self.jointStates)
            self.jointPWMs = np.array(self.k) * np.array(
                self.jointStates) + np.array(self.b)
            print self.jointPWMs
            ############################################################

            # we publish the updated joint states, with the correct time stamps
            self.joint_state_msg.header.stamp = rospy.Time.now()
            self.joint_state_msg.position = self.jointStates
            self.pub_state.publish(self.joint_state_msg)
            self.joint_PWM_msg.position = self.jointPWMs
            self.pub_PWM.publish(self.joint_PWM_msg)

            # send PWMs to SPI
            # uncomment this if you are using real hardware and sending
            # data to Arduino through SPI
            self.spi_send_PWM()

            action_rate.sleep()

        # After executing every trajectory point sent by the client
        # we notify the client that the action is being completed.
        self.arm_result = FollowJointTrajectoryResult()
        self.arm_result.error_code = 0  # error code 0 means successful
        self._as_arm.set_succeeded(self.arm_result)
Exemple #19
0
def tr2_arm_follow_joint_trajectory(goal):
    global tr2_arm_action_server, tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state, tr2_a4_state, tr2_a0_pub, tr2_a1_pub, tr2_a2_pub, tr2_a3_pub, tr2_a4_pub, tr2_mode_servo_pub, tr2_stop_pub

    tr2_mode_servo_pub.publish(1)
    tr2_stop_pub.publish(0)

    time.sleep(0.050)

    success = True
    feedback = FollowJointTrajectoryFeedback()
    result = FollowJointTrajectoryResult()

    joint_names = goal.trajectory.joint_names
    feedback.joint_names = joint_names

    print len(goal.trajectory.points)

    t_start = datetime.datetime.now()
    for point in goal.trajectory.points:
        tr2_a0_pub.publish(point.positions[0])
        tr2_a1_pub.publish(point.positions[1])
        tr2_a2_pub.publish(point.positions[2])
        tr2_a3_pub.publish(point.positions[3])
        tr2_a4_pub.publish(point.positions[4])
        '''if tr2_arm_action_server.is_preempt_requested():
			tr2_arm_action_server.set_preempted()
			success = False
			break'''

        while (datetime.datetime.now() - t_start
               ).total_seconds() < point.time_from_start.nsecs / 1000000000.0:
            feedback.desired = point
            feedback.actual.positions = [
                tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state,
                tr2_a4_state
            ]
            #feedback.error.positions
            tr2_arm_action_server.publish_feedback(feedback)

    tr2_arm_action_server.set_succeeded(result)
    time.sleep(5)
    tr2_stop_pub.publish(1)
    def execute_cb(self, goal):
        rospy.loginfo("Received new goal:\n%s"%goal)
        success = True
        start_time = rospy.Time.now()

        i = 1
        previouspoint = None
        for point in goal.trajectory.points:
            
            time_to_wait = start_time + point.time_from_start - rospy.Time.now()
            if time_to_wait > rospy.Duration(0):
                rospy.loginfo("Sleeping for %s seconds..."%time_to_wait.to_sec())
                rospy.sleep(time_to_wait)
            
            # only publish feedback if we have received some position recently
            if previouspoint and self.latestpositions:
                fb = FollowJointTrajectoryFeedback()
                fb.desired = previouspoint
                fb.actual = JointTrajectoryPoint()
                fb.actual.positions = self.latestpositions
                fb.error = JointTrajectoryPoint()
                fb.error.positions = map(sub, fb.desired.positions, fb.actual.positions)
                self.action_server.publish_feedback(fb)
            
            # only use positions and velocities (velocities are always positive)
            point.velocities = map(abs, point.velocities)
            point.accelerations = []
            point.effort = []
            
            rospy.loginfo("Go to point %d:\n%s"%(i, point))
            self.latestpositions = None
            self.pub_trajpoint.publish(point)
            
            previouspoint = point
            i += 1

        if success:
            rospy.loginfo('%s: Succeeded' % self.action_name)
            self.result = FollowJointTrajectoryResult()
            self.result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
    	    self.action_server.set_succeeded(self.result)
 def __init__(self, controller_name):
     self._action_ns = controller_name + '/follow_joint_trajectory'
     self._as = actionlib.SimpleActionServer(self._action_ns,
                                             FollowJointTrajectoryAction,
                                             execute_cb=self._execute_cb,
                                             auto_start=False)
     self._action_name = rospy.get_name()
     self._as.start()
     self._feedback = FollowJointTrajectoryFeedback()
     self._result = FollowJointTrajectoryResult()
     self._rate = rospy.Rate(20)
     rospy.loginfo('Successful init')
    def __init__(self, reconfig_server, rate=100.0):
        self._dyn = reconfig_server
        self.continuous = self._dyn.config['continuous']
        self._fjt = '/follow_joint_trajectory'
        self._server = actionlib.SimpleActionServer(
            self._fjt,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)
        self._action_name = rospy.get_name()

        # Current joint states
        self._positions = {}
        self._velocities = {}

        # Actual robot control
        self.states = EdoStates()
        rospy.Subscriber("joint_states", JointState, self._js_callback)

        # Action Feedback/Result
        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()

        # Controller parameters from arguments, messages, and dynamic reconfigure
        self._control_rate = rate  # Hz
        self._update_rate_spinner = rospy.Rate(self._control_rate)
        self._control_joints = []
        self._goal_time = 0.0
        self._stopped_velocity = 0.0
        self._goal_error = dict()
        self._path_thresh = dict()

        # Start the action server
        rospy.sleep(0.5)
        while True:
            if self.states.edo_current_state in [
                    self.states.CS_CALIBRATED, self.states.CS_BRAKED
            ]:
                self._server.start()
                self._alive = True
                rospy.loginfo("Trayectory action server started")
                break
            else:
                rospy.logerr(
                    "Joint Trajectory Action Server cannot be started when robot is in state %s"
                    % self.states.get_current_code_string())
                rospy.logerr(
                    "Make sure your robot is started and calibrated properly with calibrate.launch"
                )
                rospy.logerr("Trying again in 1 second... ")
                rospy.sleep(1)
    def __init__(self, limb, reconfig_server, rate=100.0,
                 mode='joint_impedance', sim = False, interpolation='minjerk'):

        self._mode = mode
        self._dyn = reconfig_server
        self._ns = limb + '/joint_trajectory_controller'
        self._fjt_ns = self._ns + '/follow_joint_trajectory2'
        self._server = actionlib.SimpleActionServer(
            self._fjt_ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)
        self._action_name = rospy.get_name()
        self._limb = BorisRobot()

        

        self._name = limb
        self._interpolation = interpolation

        self._server.start()
        self._alive = True
        # Action Feedback/Result
        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()

        # Controller parameters from arguments, messages, and dynamic
        # reconfigure
        self._control_rate = rate  # Hz
        self._control_joints = []
        self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
        self._goal_time = 0.0
        self._stopped_velocity = 0.0
        self._goal_error = dict()
        self._path_thresh = dict()

        self._limb.set_control_mode(mode=self._mode)
        if sim:
            rospy.loginfo("Setting gains for simulated robot")
            self._limb.set_joint_impedance(np.array([2000,2000,2000,2000,2000,2000,2000]),np.array([100,100,100,100,100,100,100]))

        # Create our spline coefficients
        self._coeff = [None] * len(self._limb.joint_names(self._name))

        # Set joint state publishing to specified control rate
        self._pub_rate = rospy.Publisher(
            '/robot/joint_state_publish_rate',
             UInt16,
             queue_size=10)
        self._pub_rate.publish(self._control_rate)
    def _set_current_joint_angles(self, joint_angles):
        self._current_joint_angles = joint_angles

        if self._trajectory_valid:
            #If the specified trajectory is too far away from current joint angles, abort it
            trajectory_angles = self._get_trajectory_joint_angles(
                self._trajectory_t)
            #TODO: improve trajectory error tracking
            if (np.any(
                    np.abs(trajectory_angles -
                           joint_angles) > np.deg2rad(45))):
                rospy.logerr("Trajectory aborted due to tracking error: %s",
                             str(np.rad2deg(trajectory_angles - joint_angles)))
                self._abort_trajectory()
            else:
                fb = FollowJointTrajectoryFeedback()
                fb.header.stamp = rospy.Time.now()
                fb.desired.positions = self._get_trajectory_joint_angles(
                    self._trajectory_max_t)
                fb.desired.time_from_start = rospy.Duration(
                    secs=self._trajectory_t)
                fb.actual = fb.desired
                gh = self._trajectory_gh
                gh.publish_feedback(fb)
    def __init__(self, rate=100.0):
        self._alive = False
        self.init_success = False

        self._action_name = rospy.get_name()
        # Action Feedback/Result
        """
        Define the joint names
        """
        self._joint_names = ['linear_joint']
        """
        Controller parameters from arguments, messages, and dynamic
        reconfigure
        """
        self._trajectory_control_rate = rate  # Hz
        self._goal_time = 0.0
        self._stopped_velocity = 0.0
        self._goal_error = dict()
        self._path_thresh = dict()
        self._estop_delay = 0

        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()
        #self._dyn = reconfig_server
        self._ns = '/movo/torso_controller'
        self._fjt_ns = self._ns + '/follow_joint_trajectory'
        self._server = actionlib.SimpleActionServer(
            self._fjt_ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)
        self._alive = True
        self.estop = False
        self._movo_status_sub = rospy.Subscriber("/movo/feedback/status",
                                                 Status,
                                                 self._update_movo_status)
        self._js = rospy.wait_for_message("/movo/linear_actuator/joint_states",
                                          JointState)
        self.pos_targets = self._get_current_position(self._joint_names)
        self._sub = rospy.Subscriber("/movo/linear_actuator/joint_states",
                                     JointState,
                                     self._update_movo_joint_states)
        self._cmd_pub = rospy.Publisher("/movo/linear_actuator_cmd",
                                        LinearActuatorCmd,
                                        queue_size=10)
        self._server.start()

        self.init_success = True
Exemple #26
0
    def __init__(self, arm, serial_number, rate=100.0):
        self._alive = False
        self.init_success = True

        self._action_name = rospy.get_name()
        self._name = arm
        # Action Feedback/Result

        """
        Define the joint names
        """
        self._joint_names = ['%s_shoulder_pan_joint'%arm,
                             '%s_shoulder_lift_joint'%arm,
                             '%s_elbow_joint'%arm,
                             '%s_wrist_1_joint'%arm,
                             '%s_wrist_2_joint'%arm,
                             '%s_wrist_3_joint'%arm]

        """
        Controller parameters from arguments, messages, and dynamic
        reconfigure
        """
        self._trajectory_control_rate = rate  # Hz
        self._goal_time = 0.0
        self._stopped_velocity = 0.0
        self._goal_error = dict()
        self._path_thresh = dict()
        self._traj_smoother = TrajectorySmoother(rospy.get_name(),arm)
        self._ctl = SIArmController(self._joint_names,arm,serial_number)

        if not self._ctl.init_success:
            rospy.logerr("Failed to initialize controller, make sure the serial number exists")
            self.clean_shutdown()
            self.init_success = False
            return

        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()        
        #self._dyn = reconfig_server
        self._ns = '/vector/%s_arm'%arm
        self._fjt_ns = self._ns + '/follow_joint_trajectory'
        self._server = actionlib.SimpleActionServer(
            self._fjt_ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)            
        self._alive = True
        self._server.start()
    def initialize(self):
        ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'

        # rospy.get_param will try to fetch the information from the parameter server,
        # but if the parameter is not available, it will use the default value.

        # let motors roll for specified amount of time
        self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)

        self.goal_constraints = []
        self.trajectory_constraints = []

        # The controller will look for goals / trajectory contrains on the parameter server
        # The values should be set on top of the controller namespace + '/joint_trajectory_action_node/constraints'
        for joint in self.joint_names:
            self.goal_constraints.append(
                rospy.get_param(ns + '/' + joint + '/goal', 0.1))  #0.04
            self.trajectory_constraints.append(
                rospy.get_param(ns + '/' + joint + '/trajectory', 1.5))

        rospy.loginfo("Goal constrains: " + ",".join([
            ji + "=" + str(ci)
            for ji, ci in zip(self.joint_names, self.goal_constraints)
        ]))
        rospy.loginfo("Trajectory constrains: " + ",".join([
            ji + "=" + str(ci)
            for ji, ci in zip(self.joint_names, self.trajectory_constraints)
        ]))

        # Message containing current state for all controlled joints
        # control_msgs/FollowJointTrajectoryFeedback
        self.feedback = FollowJointTrajectoryFeedback()
        self.feedback.joint_names = self.joint_names
        self.feedback.desired.positions = [0.0] * self.num_joints
        self.feedback.desired.velocities = [0.0] * self.num_joints
        self.feedback.desired.accelerations = [0.0] * self.num_joints
        self.feedback.actual.positions = [0.0] * self.num_joints
        self.feedback.actual.velocities = [0.0] * self.num_joints
        self.feedback.error.positions = [0.0] * self.num_joints
        self.feedback.error.velocities = [0.0] * self.num_joints

        self.running = dict()

        return 0
Exemple #28
0
    def send_goal(self, goal):
        # send goals to all controllers
        for controller in self.controllers:
            controller.send_goal(goal)

        # publish feedback until they are done
        done_goals = [False] * len(self.controllers)
        while not rospy.is_shutdown() and not all(done_goals):
            # construct feedback message
            curr_fed = FollowJointTrajectoryFeedback()
            for cidx, controller in enumerate(self.controllers):
                if controller.is_goal_done():
                    done_goals[cidx] = True
                # add to feedback message this controller stuff
                fed = controller.get_feedback()
                # curr_fed add stuff

        # compose global result and publish
        for controller in self.controllers:
            res = controller.get_result()
Exemple #29
0
    def __init__(self, name):
        '''
			\brief 		Constructor for class moveit_action
			\param[in] 	name Name of subscribed topic. 
		'''
        # Action variables
        ## Action service result
        self.ac_result = FollowJointTrajectoryResult()
        ## Action service feedback
        self.ac_feedback = FollowJointTrajectoryFeedback()

        # Action Initialization
        ## Topic name
        self.action_name = name
        ## Action server
        self.action_server = actionlib.SimpleActionServer(
            self.action_name,
            FollowJointTrajectoryAction,
            execute_cb=self.action_callback,
            auto_start=False)
        self.action_server.start()
Exemple #30
0
    def __init__(self, name):
        self._feedback = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()

        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name,
                                                FollowJointTrajectoryAction,
                                                execute_cb=self.execute_cb,
                                                auto_start=False)
        self._as.start()

        # Getting publishers for all joints
        self._poppy = rospy.get_param("/poppy")
        self._joints_pub = {}
        for k, d in self._poppy.iteritems():
            if k != 'joint_state_controller':  #just ignore this one
                self._joints_pub[k.replace('_position_controller',
                                           '')] = rospy.Publisher(
                                               "/poppy/" + k + "/command",
                                               Float64)

        rospy.loginfo('%s: is ready' % self._action_name)
Exemple #31
0
class MoveItAction(object):

    _feedback = FollowJointTrajectoryFeedback()
    _result = FollowJointTrajectoryResult()

    def __init__(self, name):
        self.publisher = rospy.Publisher("joint_path_command", JointTrajectory)
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name,
                                                FollowJointTrajectoryAction,
                                                execute_cb=self.execute_cb,
                                                auto_start=False)
        self._as.start()

    def execute_cb(self, goal):
        self.publisher.publish(goal.trajectory)

        try:
            rospy.sleep(goal.trajectory.points[-1].time_from_start)
            self._result.error_code = 0
        except:
            self._result.error_code = self._result.OLD_HEADER_TIMESTAMP

        self._as.set_succeeded(self._result)
def do_trajectory(goal):
    joint_names = goal.trajectory.joint_names
    trajectory_points = goal.trajectory.points
    print("GOAL TRAJECTORY:")
    print(goal.trajectory)

    # publish normally???
    left_arm_pub = rospy.Publisher('r2d2_left_arm_controller/command',
                                   JointTrajectory,
                                   queue_size=10)
    left_arm_pub.publish(goal.trajectory)
    # print("Published")

    start_time = time.time()
    print(time.time() - start_time)
    while (time.time() - start_time < 10):
        print(time.time() - start_time)
        feedback = FollowJointTrajectoryFeedback()

        feedback.desired = JointTrajectoryPoint()
        feedback.desired = goal.trajectory.points[0]

        # get actual positions from joint state or robot publisher ...
        feedback.actual = JointTrajectoryPoint()
        feedback.actual.positions = [0, 0, 0]

        feedback.error = JointTrajectoryPoint()
        feedback.error.positions = numpy.subtract(feedback.desired.positions,
                                                  feedback.actual.positions)

        server.publish_feedback(feedback)

        time.sleep(1.0)

    result = FollowJointTrajectoryResult()
    server.set_succeeded(result, "Trajectory completed successfully")
    print(result.SUCCESSFUL)
Exemple #33
0
class JointContoller():
    _feedback = FollowJointTrajectoryFeedback()
    _result = FollowJointTrajectoryResult()

    def __init__(self):
        self.name = rospy.get_name()

        self.publisher = rospy.Publisher('joint_states',
                                         JointState,
                                         queue_size=10)
        # Set up action client
        self.action_server = actionlib.SimpleActionServer(
            '%s/follow_joint_trajectory' % self.name,
            FollowJointTrajectoryAction, self.do_action_callback, False)
        self.action_server.start()

        self.rate = rospy.Rate(10)  # 10hz
        self.joint_state = JointState()
        self.joint_state.header = Header()
        self.joint_state.name = ['hip', 'shoulder', 'elbow', 'wrist']
        self.joint_state.position = [
            1.57, 1.57, 1.57, 1.57
        ]  # will be updated in home() before publication in the loop
        self.joint_state.velocity = []
        self.joint_state.effort = []

        # Initialise the PCA9685 using the default address (0x40) and the bus
        self.pwm = Adafruit_PWM_Servo_Driver.PWM(address=0x40, busnum=1)
        self.pwm.setPWMFreq(cal.PWM_FREQUENCY)
        self.home()

    def do_action_callback(self, goal):
        print 'Received goal %d' % len(goal.trajectory.points[0].positions)
        timePassed = rospy.Duration(0)
        self.joint_state.name = goal.trajectory.joint_names

        for point in goal.trajectory.points:
            self.joint_state.header.stamp = rospy.Time.now()
            self.move_arm(goal.trajectory.joint_names, point.positions)
            self.publisher.publish(self.joint_state)
            rospy.sleep(point.time_from_start - timePassed)
            timePassed = point.time_from_start
            print('step')

        self._result.error_code = 0
        self.action_server.set_succeeded(self._result)
        print('move complete')

    def get_joint_goal(self, joint_name, joint_list, goal_list):
        if joint_name in joint_list:
            index = joint_list.index(joint_name)
            return goal_list[index]
        print 'ERROR!! Could not find %s goal' % joint_name

    def set_joint_state(self, joint_name, pulse):
        index = self.joint_state.name.index(joint_name)
        print '%s at %d is %d (%0.2f)' % (joint_name, index, pulse,
                                          math.radians(pulse))
        self.joint_state.position[index] = math.radians(pulse)

    def move_arm(self, names, goal):
        base = self.get_joint_goal('hip', names, goal)
        shoulder = self.get_joint_goal('shoulder', names, goal)
        elbow = self.get_joint_goal('elbow', names, goal)
        claw = self.get_joint_goal('wrist', names, goal)
        self.setJointAngles(base, shoulder, elbow, claw)

    def check_min_max(self, minVal, maxVal, test):
        if (test < minVal):
            test = minVal
            print "min %d" % (minVal)
        if (test > maxVal):
            test = maxVal
            print "min %d" % (maxVal)
        return test

    # Helper function to make setting a servo pulse width simpler.
    def set_servo_pulse(self, channel, pulse):
        pwm_val = 150 + int(
            pulse * 450.0 / 180.0)  # magic incantation to make the timing work
        self.pwm.setPWM(channel, 0, pwm_val)

    def b(self, pulse):
        pulse = self.check_min_max(cal.BASE_MIN_PWM, cal.BASE_MAX_PWM, pulse)
        self.set_joint_state('hip', pulse)
        self.set_servo_pulse(cal.BASE_SERVO_CHANNEL, pulse)

    def s(self, pulse):
        pulse = self.check_min_max(cal.SHOULDER_MIN_PWM, cal.SHOULDER_MAX_PWM,
                                   pulse)
        self.set_joint_state('shoulder', pulse)
        self.set_servo_pulse(cal.SHOULDER_SERVO_CHANNEL, pulse)

    def e(self, pulse):
        pulse = self.check_min_max(cal.ELBOW_MIN_PWM, cal.ELBOW_MAX_PWM, pulse)
        self.set_joint_state('elbow', pulse)
        self.set_servo_pulse(cal.ELBOW_SERVO_CHANNEL, pulse)

    def c(self, pulse):
        pulse = self.check_min_max(cal.CLAW_MIN_PWM, cal.CLAW_MAX_PWM, pulse)
        self.set_joint_state('wrist', pulse)
        self.set_servo_pulse(cal.CLAW_SERVO_CHANNEL, pulse)

    def setJointAngles(self, base, shoulder, elbow, claw):
        self.b(int(math.degrees(base)))
        self.s(int(math.degrees(shoulder)))
        self.e(int(math.degrees(elbow)))
        self.c(int(math.degrees(claw)))

    def home(self):
        self.b(cal.BASE_HOME_PWM)
        self.s(cal.SHOULDER_HOME_PWM)
        self.e(cal.ELBOW_HOME_PWM)
        self.c(cal.CLAW_HOME_PWM)

    def off(self):
        self.pwm.setPWM(cal.BASE_SERVO_CHANNEL, 0, 0)
        self.pwm.setPWM(cal.SHOULDER_SERVO_CHANNEL, 0, 0)
        self.pwm.setPWM(cal.ELBOW_SERVO_CHANNEL, 0, 0)
        self.pwm.setPWM(cal.CLAW_SERVO_CHANNEL, 0, 0)

    def loop(self):
        # publish states regularly though this is also done on commands
        while not rospy.is_shutdown():
            self.joint_state.header.stamp = rospy.Time.now()
            self.publisher.publish(self.joint_state)
            self.rate.sleep()
        self.off()