2 # -*- coding: iso-8859-15 -*-
6 import arm # interface to hardware
9 from dynamic_reconfigure.server import Server
10 from arm_ros_conn.cfg import ArmConfig
11 from sensor_msgs.msg import JointState
12 from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryActionResult, FollowJointTrajectoryActionFeedback, FollowJointTrajectoryResult
13 from control_msgs.msg import GripperCommandAction, GripperCommandActionResult, GripperCommandFeedback
14 from actionlib_msgs.msg import GoalStatus
15 from time import sleep
18 lJointNames = ["link1_joint", "link2_joint", "link3_joint", "link4_joint", "link5_joint", "left_gripper_joint", "right_gripper_joint"]
22 _feedback = FollowJointTrajectoryActionFeedback()
23 _result = FollowJointTrajectoryActionResult()
24 _gripper_feedback = GripperCommandFeedback()
25 _gripper_result = GripperCommandActionResult()
28 rospy.init_node('arm')
30 self.lSpeeds = [220] * 6 # default pwm speed for all 6 motors
31 self.lAngles = [0] * 6 # current angle for all 6 motors, needed to calculate current error
32 # switch forward/reverse direction on motors
35 # these motors do not have anquadrature encoder
36 arm.set_hall_mode(3, 0)
37 arm.set_hall_mode(5, 0)
38 # lower position tolerance of these motors
39 arm.set_tolerance(3, 0)
40 arm.set_tolerance(5, 0)
42 self._as_arm = actionlib.SimpleActionServer("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction, execute_cb=self.execute_joint_trajectory, auto_start = False)
44 self._as_gripper = actionlib.SimpleActionServer("gripper_controller/gripper_action", GripperCommandAction, execute_cb=self.execute_gripper_action, auto_start = False)
45 self._as_gripper.start()
46 self.pub_joint_states = rospy.Publisher("joint_states", JointState, queue_size=16)
47 self.dyn_conf = Server(ArmConfig, self.execute_dyn_reconf)
52 while not rospy.is_shutdown():
53 self.publish_joint_states()
56 def execute_dyn_reconf(self, config, level):
57 self.lSpeeds = [config["speed_1"], config["speed_2"], config["speed_3"], config["speed_4"], config["speed_5"], config["speed_6"]]
60 def publish_joint_states(self):
61 joint_state = JointState()
62 joint_state.header.stamp = rospy.Time.now()
63 joint_state.name = lJointNames
64 # position of last motor (gripper) needs to be adjusted by 0.35, also value is expected two times: For right and left gripper joint.
65 self.lAngles = [arm.get_angle(0), arm.get_angle(1), arm.get_angle(2), arm.get_angle(3), arm.get_angle(4), 0.35-arm.get_angle(5)]
66 joint_state.position = self.lAngles[:-1] + [self.lAngles[-1], self.lAngles[-1]]
67 self.pub_joint_states.publish(joint_state)
69 def execute_joint_trajectory(self, goal):
70 self._result.status = FollowJointTrajectoryResult.SUCCESSFUL
71 for point in goal.trajectory.points:
72 print goal.trajectory.joint_names
74 # get new desired joint values in order from link1_joint to link5_joint
76 point.positions[goal.trajectory.joint_names.index(lJointNames[0])],
77 point.positions[goal.trajectory.joint_names.index(lJointNames[1])],
78 point.positions[goal.trajectory.joint_names.index(lJointNames[2])],
79 point.positions[goal.trajectory.joint_names.index(lJointNames[3])],
80 point.positions[goal.trajectory.joint_names.index(lJointNames[4])],
83 # commit new values to hardware
84 arm.to_angle(0, self.lSpeeds[0], lGoalPosOrdered[0])
85 arm.to_angle(1, self.lSpeeds[1], lGoalPosOrdered[1])
86 arm.to_angle(2, self.lSpeeds[2], lGoalPosOrdered[2])
87 arm.to_angle(3, self.lSpeeds[3], lGoalPosOrdered[3])
88 arm.to_angle(4, self.lSpeeds[4], lGoalPosOrdered[4])
89 except arm.RangeError as e:
90 # reject if position is impossible for the hardware to reach
91 print >> sys.stderr, e.message
92 self._feedback.status = GoalStatus.REJECTED
93 self._as_arm.publish_feedback(self._feedback.feedback)
94 self._result.status = FollowJointTrajectoryResult.INVALID_GOAL
97 # publish current position error while driving to position
100 error = np.array(lGoalPosOrdered) - np.array(self.lAngles[:-1]) # error = desired - current
102 # Position reached, done
103 if all(abs(f) < 0.15 for f in error): # allow a tiny tolerance and continue with next goal
104 print "Position reached"
105 self._feedback.status = GoalStatus.SUCCEEDED
108 # Cancel if requested
109 if self._as_arm.is_preempt_requested():
110 self._feedback.status = GoalStatus.PREEMPTING
111 self._as_arm.set_preempted()
115 # Give feedback to either canceled or current position reached
116 self._feedback.feedback.joint_names = lJointNames[:-1]
117 self._feedback.feedback.desired.positions = lGoalPosOrdered
118 self._feedback.feedback.actual.positions = self.lAngles[:-1]
119 self._feedback.feedback.error.positions = error
120 self._as_arm.publish_feedback(self._feedback.feedback)
122 self._as_arm.set_succeeded(self._result.result)
125 def execute_gripper_action(self, goal):
126 # adjust position and commit to hardware
127 arm.to_angle(5, self.lSpeeds[5], 0.35-goal.command.position)
129 error = goal.command.position - self.lAngles[-1] # error = desired - current
130 if abs(error) < 0.02: # tolerance for position
134 self._gripper_feedback.position = self.lAngles[-1]
135 self._gripper_feedback.stalled = False
136 self._gripper_feedback.reached_goal = False
138 # Cancel if requested
139 if self._as_gripper.is_preempt_requested():
140 self._as_gripper.set_preempted()
144 # Done, either canceled or position reached
145 self._gripper_result.status = GoalStatus.SUCCEEDED
146 self._gripper_result.result.position = goal.command.position
147 self._gripper_result.result.stalled = False
148 self._gripper_result.result.reached_goal = True
149 self._as_gripper.set_succeeded(self._gripper_result.result)
151 if __name__ == '__main__':