+ def execute_joint_trajectory(self, goal):
+ self._result.status = FollowJointTrajectoryResult.SUCCESSFUL
+ for point in goal.trajectory.points:
+ print goal.trajectory.joint_names
+ print point.positions
+ lGoalPosOrdered = [
+ point.positions[goal.trajectory.joint_names.index(lJointNames[0])],
+ point.positions[goal.trajectory.joint_names.index(lJointNames[1])],
+ point.positions[goal.trajectory.joint_names.index(lJointNames[2])],
+ point.positions[goal.trajectory.joint_names.index(lJointNames[3])],
+ point.positions[goal.trajectory.joint_names.index(lJointNames[4])],
+ ]
+ try:
+ arm.to_angle(0, self.speed, -lGoalPosOrdered[0])
+ arm.to_angle(1, self.speed, lGoalPosOrdered[1])
+ arm.to_angle(2, self.speed, -lGoalPosOrdered[2])
+ arm.to_angle(3, self.speed, -lGoalPosOrdered[3])
+ arm.to_angle(4, self.speed, lGoalPosOrdered[4])
+ except arm.RangeError as e:
+ print >> sys.stderr, e.message
+ self._feedback.status = GoalStatus.REJECTED
+ self._as_arm.publish_feedback(self._feedback.feedback)
+ self._result.status = FollowJointTrajectoryResult.INVALID_GOAL
+ break
+
+ error = 0
+ while True:
+ error = np.array(lGoalPosOrdered) - np.array(self.lAngles[:-1])
+ print "Error", error
+ if all(f < 0.02 for f in error):
+ break
+
+ if self._as_arm.is_preempt_requested():
+ self._as_arm.set_preempted()
+ break
+ sleep(0.001)
+
+ self._feedback.status = GoalStatus.SUCCEEDED
+ self._feedback.feedback.joint_names = lJointNames[:-1]
+ self._feedback.feedback.desired.positions = lGoalPosOrdered
+ self._feedback.feedback.actual.positions = self.lAngles[:-1]
+ self._feedback.feedback.error.positions = error
+ self._as_arm.publish_feedback(self._feedback.feedback)
+ self._as_arm.set_succeeded(self._result.result)
+
+
+ def execute_gripper_action(self, goal):
+ print goal