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.")