- 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])
+ 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