How do I manual control robot motors?
Hi, everyone! I'm new to ROS. Currently, I'm trying to create a jointtrajectoryactionserver.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 executecallback:
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!
Asked by madlink306 on 2019-06-23 04:29:33 UTC
Comments