def __check_joint_validity_moveit(self, joints):
"""
Check target joint validity (no collision) with MoveIt.
:return: The target joint validity in a boolean, and the two links colliding if available
"""
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.__arm_state.joints_name
group_name = self.__arm.get_name()
null_constraints = Constraints()
try:
response = self.__parameters_validator.check_state_validity(
robot_state_target, group_name, null_constraints)
if not response.valid:
if len(response.contacts) > 0:
rospy.logwarn(
'Jog Controller - Joints target unreachable because of collision between %s and %s',
response.contacts[0].contact_body_1,
response.contacts[0].contact_body_2)
return False, 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_throttle(
1,
'Jog Controller - Joints target unreachable because of '
'collision between two parts of Ned')
return False, None, None
else:
return True, None, None
except AttributeError as _e: # maybe delete later, useful for the test on real robot when using the service
return True, None, None
def get_forward_kinematics(self, joints):
"""
Get forward kinematic from joints
:param joints: list of joints value
:type joints: Pose
:return: A RobotState object
"""
try:
rospy.wait_for_service('compute_fk', 2)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("Arm commander - Impossible to connect to FK service : " + str(e))
return RobotState()
try:
moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
fk_link = ['base_link', 'tool_link']
header = Header(0, rospy.Time.now(), "world")
rs = RobotStateMoveIt()
rs.joint_state.name = self.__arm_state.joints_name
rs.joint_state.position = joints
response = moveit_fk(header, fk_link, rs)
except rospy.ServiceException as e:
rospy.logerr("Arm commander - Failed to get FK : " + str(e))
return RobotState()
pose = self.__transform_handler.ee_link_to_tcp_pose_target(response.pose_stamped[1].pose, "tool_link")
quaternion = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
rpy = euler_from_quaternion(quaternion)
quaternion = (round(quaternion[0], 3), round(quaternion[1], 3), round(quaternion[2], 3),
round(quaternion[3], 3))
point = (round(pose.position.x, 3),
round(pose.position.y, 3),
round(pose.position.z, 3))
return RobotState(position=Point(*point), rpy=RPY(*rpy), orientation=Quaternion(*quaternion))
def __get_plan_start_robot_state(self, plan):
start_state = RobotStateMoveIt()
start_state.joint_state.header.frame_id = "world"
start_state.joint_state.position = plan.joint_trajectory.points[
0].positions
start_state.joint_state.name = self.__joints_name
return start_state
def __compute_and_execute_trajectory(self,
list_poses,
dist_smoothing=0.01):
"""
Hard try to to smooth Trajectories, not really working at the moment
:param list_poses: list of Pose object
:param dist_smoothing: smoothing distance
:return: None
"""
# rospy.logerr(list_poses)
list_plans = []
new_state = RobotStateMoveIt()
new_state.joint_state.header.stamp = rospy.Time.now()
new_state.joint_state.name = self.__joints_name
# rospy.logerr(list_poses)
for i, pose_target in enumerate(list_poses):
# Getting plan to target
self.__arm.set_pose_target(pose_target, self.__end_effector_link)
list_plans.append(self.__arm.plan())
# Generating new start state for future iteration
success, new_state.joint_state.position = self.get_inverse_kinematics(
pose=pose_target)
if not success:
self.__arm.set_start_state_to_current_state()
raise ArmCommanderException(
CommandStatus.INVERT_KINEMATICS_FAILURE, "IK Fail")
self.__arm.set_start_state(new_state)
self.__arm.set_start_state_to_current_state()
plan = self.__link_plans(dist_smoothing, *list_plans)
# self.__display_traj(plan, id_=int(1000 * dist_smoothing))
return self.__execute_plan(plan)
def __compute_trajectory(self, list_poses, dist_smoothing=0.01):
"""
Hard try to to smooth Trajectories, not really working at the moment
:param list_poses: list of Pose object
:param dist_smoothing: smoothing distance
:return: None
"""
self.__arm.set_start_state_to_current_state()
list_plans = []
new_state = RobotStateMoveIt()
new_state.joint_state.header.stamp = rospy.Time.now()
new_state.joint_state.name = self.__joints_name
new_state.joint_state.position = self.__arm.get_current_joint_values()
for i, pose_target in enumerate(list_poses):
# Getting plan to target
# Try a linear path first
partial_plan = self.__traj_executor.compute_cartesian_plan(
[pose_target])
if not partial_plan: # Try a PTP path if linear path failed
rospy.loginfo(
"Linear path not found between {} joints and {} pose".
format(new_state.joint_state.position,
pose_to_list(pose_target)))
self.__arm.set_pose_target(pose_target,
self.__end_effector_link)
try:
partial_plan = self.__traj_executor.plan()
except ArmCommanderException as e:
self.__arm.set_start_state_to_current_state()
raise e
list_plans.append(copy.deepcopy(partial_plan))
# Generating new start state for future iteration
new_state.joint_state.position = list_plans[
-1].joint_trajectory.points[-1].positions
self.__arm.set_start_state(new_state)
self.__arm.set_start_state_to_current_state()
if dist_smoothing > 0:
plan = self.__link_plans_with_smoothing(dist_smoothing,
*list_plans)
else:
plan = self.__link_plans(*list_plans)
self.display_traj(plan, id_=int(1000 * dist_smoothing))
return 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))