How do I manual control robot motors?

asked 2019-06-23 04:29:33 -0500

madlink306 gravatar image

Hi, everyone! I'm new to ROS. Currently, I'm trying to create a joint_trajectory_action_server.py to manually control a robot arm. I can send the position to the arm one at a time. Supposed, I got the trajectory message from moveit, and it is a list of positions.I google around and people suggest using FollowJointTrajectoryAction, but I don't get how each position is sent to the arm. Most example people just send the whole list to the arm. Here is my code for the execute_callback:

def _execute_cb(self, goal):
   '''Initialises and runs route, and updates feedback until complete or time duration violated'''
   rospy.loginfo('Started cb')
   start_time = rospy.get_time()
   joint_names = goal.trajectory.joint_names
   trajectory_points = goal.trajectory.points
   if joint_to_angle(trajectory_points[0].positions) == joint_to_angle(trajectory_points[1].positions):
        trajectory_points = trajectory_points[1:]

   joint_values = [0]*6 
   for point in trajectory_points:
        joint_values = angles_to_joint(point.positions)
        arm.go_to_joint_pos(joint_values) #Is this where I should send the joint position???
   end_time = trajectory_points[-1].time_from_start.to_sec()   
   time_elapsed = rospy.get_time() - start_time
   pos = trajectory_points[0].positions    
   rospy.loginfo('Started route')
   while (time_elapsed < end_time and not rospy.is_shutdown()):    
        self._rate.sleep()  #Or this is
        if self._as.is_preempt_requested():
            rospy.loginfo("%s: Joint trajectory action preempted, stopping arm" % (self._action_name))

            pos = self._update_feedback(joint_names, pos, time_elapsed)  
            self._as.set_preempted()   
            break                
        result = self._check_goal(pos, trajectory_points[-1].positions, GOAL_TOLERANCE)
        pos = self._update_feedback(joint_names, pos, time_elapsed)    
        if result:
            rospy.loginfo("%s: Joint trajectory action succeeded" % (self._action_name))
            self._result.error_code = self._result.SUCCESSFUL
            self._as.set_succeeded(self._result)
            break

Can anyone give me any pointer? Thank you in advance!

edit retag flag offensive close merge delete