def validate_position(self, position): if isinstance(position, Point): x = position.x y = position.y z = position.z else: x, y, z = position v = self.validation_consts['position_limits'] if x < v['x']['min'] or x > v['x']['max']: raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "x not in range ( {} , {} )".format(v['x']['min'], v['x']['max'])) if y < v['y']['min'] or y > v['y']['max']: raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "y not in range ( {} , {} )".format(v['y']['min'], v['y']['max'])) if z < v['z']['min'] or z > v['z']['max']: raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "z not in range ( {} , {} )".format(v['z']['min'], v['z']['max']))
def validate_orientation(self, orientation): if isinstance(orientation, RPY): roll = orientation.roll pitch = orientation.pitch yaw = orientation.yaw else: roll = orientation[0] pitch = orientation[1] yaw = orientation[2] v = self.validation_consts['rpy_limits'] if roll < v['roll']['min'] or roll > v['roll']['max']: raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "roll not in range ( {} , {} )".format(v['roll']['min'], v['roll']['max'])) if pitch < v['pitch']['min'] or pitch > v['pitch']['max']: raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "pitch not in range ( {} , {} )".format( v['pitch']['min'], v['pitch']['max'])) if yaw < v['yaw']['min'] or yaw > v['yaw']['max']: raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "yaw not in range ( {} , {} )".format(v['yaw']['min'], v['yaw']['max']))
def validate_shift_pose(shift): if shift.axis_number not in [0, 1, 2, 3, 4, 5]: raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "shift axis number not in [0,1,2,3,4,5]") if shift.value < -1.0 or shift.value > 1.0: raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "shift value can't be null and < -1 or > 1")
def validate_joints(self, joint_array): if len(joint_array) != len(self.joints_limits): raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "Joint array must have {} joints".format( len(self.joints_limits))) for joint_index, joint_cmd in enumerate(joint_array): if self.joints_limits[joint_index] and not self.joints_limits[joint_index].lower <= joint_cmd <= \ self.joints_limits[joint_index].upper: raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "joint_{} not in range ({}, {})".format( joint_index + 1, self.joints_limits[joint_index].lower, self.joints_limits[joint_index].upper))
def __validate_params_move(self, command_type, *args): """ Validate parameters according to command_type The function will raise an error if command is not valid :param command_type: :type command_type: ArmMoveCommand :param args: parameters :return: None """ if command_type == ArmMoveCommand.JOINTS: self.__parameters_validator.validate_joints(args[0]) elif command_type == ArmMoveCommand.POSE: self.__parameters_validator.validate_position(args[0]) self.__parameters_validator.validate_orientation(args[1]) elif command_type == ArmMoveCommand.POSITION: self.__parameters_validator.validate_position(args[0]) elif command_type == ArmMoveCommand.RPY: self.__parameters_validator.validate_orientation(args[0]) elif command_type == ArmMoveCommand.EXECUTE_TRAJ: self.__parameters_validator.validate_trajectory(args[0]) elif command_type == ArmMoveCommand.POSE_QUAT: self.__parameters_validator.validate_position(args[0]) self.__parameters_validator.validate_orientation_quaternion( args[1]) elif command_type == ArmMoveCommand.SHIFT_POSE: self.__parameters_validator.validate_shift_pose(args[0]) elif command_type == ArmMoveCommand.SHIFT_LINEAR_POSE: self.__parameters_validator.validate_shift_pose(args[0]) else: raise ArmCommanderException(CommandStatus.UNKNOWN_COMMAND, "Wrong command type")
def plan(self): for _ in range(self.__compute_plan_max_tries): partial_plan = self.__arm.plan() if len(partial_plan.joint_trajectory.points) != 0: return partial_plan else: raise ArmCommanderException(CommandStatus.NO_PLAN_AVAILABLE, "No trajectory found.")
def execute_plan(self, plan): """ Execute the plan given Firstly, calculate a timeout which is : 1.5 times *the estimated time of the plan* Then send execute command to MoveIt and wait until the execution finished or the timeout happens :param plan: Computed plan :type plan: RobotTrajectory :return: CommandStatus, message """ if not plan: raise ArmCommanderException( CommandStatus.NO_PLAN_AVAILABLE, "You are trying to execute a plan which doesn't exist") # Reset self.__traj_finished_event.clear() self.__current_goal_id = None self.__current_goal_result = GoalStatus.LOST trajectory_time_out = max(1.5 * self.__get_plan_time(plan), self.__trajectory_minimum_timeout) # Send trajectory and wait self.__collision_detected = False self.__arm.execute(plan, wait=False) if self.__traj_finished_event.wait(trajectory_time_out): if self.__current_goal_result == GoalStatus.SUCCEEDED: return CommandStatus.SUCCESS, "Command has been successfully processed" elif self.__current_goal_result == GoalStatus.PREEMPTED: if self.__collision_detected: self.__collision_detected = False self.__collision_detected_publisher.publish(True) self.__set_learning_mode(True) return ( CommandStatus.STOPPED, "Command has been aborted due to a collision " "or a motor not able to follow the given trajectory") else: return CommandStatus.STOPPED, "Command has been successfully stopped" elif self.__current_goal_result == GoalStatus.ABORTED: # if joint_trajectory_controller aborts the goal, it will still try to # finish executing the trajectory --> so we ask it to stop from here # http://wiki.ros.org/joint_trajectory_controller -> preemption policy # Send an empty trajectory from the topic interface self.__set_position_hold_mode() abort_str = "Command has been aborted due to a collision or " \ "a motor not able to follow the given trajectory" self.__set_learning_mode(True) self.__collision_detected_publisher.publish(True) return CommandStatus.CONTROLLER_PROBLEMS, abort_str else: # problem from ros_control itself self.__current_goal_id = None return CommandStatus.SHOULD_RESTART, "" else: # This timeout will happen if something fails in ros_control # It is not related to a trajectory failure self.__current_goal_id = None return CommandStatus.SHOULD_RESTART, ""
def __get_plan_time(plan): """ Extract duration from a plan. If it cannot be done, raise ArmCommanderException :param plan: plan given by MoveIt :type plan: RobotTrajectory :return: duration in seconds """ if plan and plan.joint_trajectory.points: return plan.joint_trajectory.points[-1].time_from_start.to_sec() else: raise ArmCommanderException(CommandStatus.NO_PLAN_AVAILABLE, "No current plan found")
def compute_and_execute_cartesian_plan(self, list_poses, velocity_factor=1.0, acceleration_factor=1.0): """ Compute a cartesian plan according to list_poses and then, execute it The robot will follow a straight line between each points If the plan is not validate, raise ArmCommanderException :param list_poses: :param velocity_factor: :param acceleration_factor: :return: status, message """ if len(list_poses) == 0: return GoalStatus.REJECTED, "No Waypoints" try: plan = self.compute_cartesian_plan(list_poses) except Exception: raise ArmCommanderException(CommandStatus.ARM_COMMANDER_FAILURE, "IK Fail") if plan is None: raise ArmCommanderException( CommandStatus.NO_PLAN_AVAILABLE, "The goal cannot be reached with a linear trajectory") # Apply robot speeds plan = self.retime_plan( plan, velocity_scaling_factor=velocity_factor, acceleration_scaling_factor=acceleration_factor, optimize=False) if plan is None: raise ArmCommanderException( CommandStatus.NO_PLAN_AVAILABLE, "The goal cannot be reached with a linear trajectory") return self.execute_plan(plan)
def __check_collision_in_joints_target(self, joints): """ Check if the joints target implies a self collision. Raise an exception is a collision is detected. :param joints: list of joints value """ try: robot_state_target = RobotStateMoveIt() robot_state_target.joint_state.header.frame_id = "world" robot_state_target.joint_state.position = joints robot_state_target.joint_state.name = self.__joints_name group_name = self.__arm.get_name() response = self.__parameters_validator.check_state_validity( robot_state_target, group_name, None) if not response.valid: if len(response.contacts) > 0: rospy.logwarn( 'Arm commander - Joints target unreachable because of collision between %s and %s', response.contacts[0].contact_body_1, response.contacts[0].contact_body_2) raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "Target joints would lead to a collision between links {} and {} " .format(response.contacts[0].contact_body_1, response.contacts[0].contact_body_2)) else: # didn't succeed to get the contacts on the real robot rospy.logwarn( 'Arm commander - Joints target unreachable because of collision between two parts of Ned' ) raise ArmCommanderException( CommandStatus.INVALID_PARAMETERS, "Target joints would lead to a collision between two parts of Ned" ) except rospy.ServiceException as e: rospy.logwarn("Arm commander - Failed to check state validity : " + str(e))
def compute_waypointed_trajectory(self, arm_cmd): """ Version 1 : Go to first pose using "classic" trajectory then compute cartesian path between all points Version 2 : Going to all poses one by one using "classical way" Version 3 : Generate all plans, merge them & smooth transitions """ list_tcp_poses = arm_cmd.list_poses if len(list_tcp_poses) == 0: return CommandStatus.NO_PLAN_AVAILABLE, "Can't generate plan from a list of length 0", None list_ee_poses = [ self.__transform_handler.tcp_to_ee_link_pose_target( tcp_pose, self.__end_effector_link) for tcp_pose in list_tcp_poses ] dist_smoothing = arm_cmd.dist_smoothing if dist_smoothing == 0.0: # We are going to the initial pose using "classical" method try: plan = self.__traj_executor.compute_cartesian_plan( list_ee_poses) if plan is None: raise ArmCommanderException( CommandStatus.NO_PLAN_AVAILABLE, "") plan = self.__traj_executor.retime_plan( plan, self.__arm_state.velocity_scaling_factor, self.__arm_state.acceleration_scaling_factor, optimize=False) except ArmCommanderException as e: if e.status == CommandStatus.NO_PLAN_AVAILABLE: rospy.loginfo( "Cartesian path computation failed, let's try another way!" ) plan = self.__compute_trajectory( list_ee_poses, dist_smoothing=dist_smoothing) else: raise e else: plan = self.__compute_trajectory(list_ee_poses, dist_smoothing=dist_smoothing) return CommandStatus.SUCCESS, "Trajectory is Good!", plan
def compute_and_execute_plan_to_target(self): """ Firstly try to compute the plan to the set target. If fails a first time, will try ComputePMaxTries times to stop the arm and recompute the plan Then, execute the plan :return: status, message """ for tries in range(self.__compute_plan_max_tries + 1): # We are going to try 3 times # if we are re-trying, first stop current plan if tries > 0: self.stop_current_plan() rospy.sleep(0.1) plan = self.__get_computed_plan() if not plan: raise ArmCommanderException( CommandStatus.PLAN_FAILED, "MoveIt failed to compute the plan.") if self.__hardware_version == 'ned': self.__reset_controller() rospy.loginfo( "Arm commander - Send MoveIt trajectory to controller.") status, message = self.execute_plan(plan) if status != CommandStatus.SHOULD_RESTART: return status, message if tries >= self.__compute_plan_max_tries: rospy.logerr( "Arm commander - Big failure from the controller. Try to restart the robot" ) return CommandStatus.SHOULD_RESTART, "Please restart the robot and try again." rospy.logwarn("Arm commander - Will retry to compute " "& execute trajectory {} time(s)".format( self.__compute_plan_max_tries - tries))
def validate_orientation_quaternion(quat): norm = quat.x**2 + quat.y**2 + quat.z**2 + quat.w**2 norm = sqrt(norm) if abs(norm - 1.0) > 0.001: raise ArmCommanderException(CommandStatus.INVALID_PARAMETERS, "Quaternion is not normalised.")