+ for point in goal.trajectory.points:
+ print goal.trajectory.joint_names
+ print point.positions
+ arm.to_angle(0, self.speed, -point.positions[goal.trajectory.joint_names.index(lJointNames[0])])
+ arm.to_angle(1, self.speed, point.positions[goal.trajectory.joint_names.index(lJointNames[1])])
+ arm.to_angle(2, self.speed, -point.positions[goal.trajectory.joint_names.index(lJointNames[2])])
+ arm.to_angle(3, self.speed, -point.positions[goal.trajectory.joint_names.index(lJointNames[3])])
+ arm.to_angle(4, self.speed, point.positions[goal.trajectory.joint_names.index(lJointNames[4])])
+ self._feedback.status = GoalStatus.SUCCEEDED
+ self._feedback.feedback.joint_names = goal.trajectory.joint_names
+ self._feedback.feedback.desired = point
+ self._as.publish_feedback(self._feedback.feedback)
+ self._result.status = FollowJointTrajectoryResult.SUCCESSFUL
+ self._as.set_succeeded(self._result.result)