2024-9-23_L/reyuan/lst_robot_r/scripts/check.py

import rospy, sys
import numpy as np
from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply

#####检查关节限位#####
def check_joint_positions(joint_trajectory):
    is_limited = False      #初始化为满足限制条件
    Limit_margin = True     #限制边界，初始化为True，用来检查是否超出限制条件
    
    for i in range(len(joint_trajectory.joint_trajectory.points) - 1):  #遍历除了最后一个关节轨迹点其他所有轨迹点
        if  not is_limited:
            joint_positions1 = joint_trajectory.joint_trajectory.points[i].positions    #选择两个相邻的关节轨迹点
            joint_positions2 = joint_trajectory.joint_trajectory.points[i + 1].positions 

            for j in range(len(joint_positions1)):    #遍历但前轨迹点的每个关节点，这里是joint_positions1
                position_diff = abs(joint_positions1[j] - joint_positions2[j])  #计算相邻关节轨迹点的关节点绝对差值，归一化到[-pi,pi]
                
                if position_diff > 6:   #限位阈值，6rad=343.8度
                    rospy.loginfo("发生大角度翻转: point{}-joint{}:{}".format(i,j+1,joint_positions1))
                    
                    #关节限位值
                    joint_limt =[[],[],[],[-1.6,1.6],[],[-3.838,3.838]] 
                    
                    #设定限位点为当前关节点
                    Limit_point = joint_positions1[j]  
                     
                    #计算限位点(当前点)与起点位置的偏移量的绝对值
                    distance_to_Limit_point = abs(joint_trajectory.joint_trajectory.points[0].positions[j]- Limit_point)
                    Joint_range = abs(joint_limt[j][0] - joint_limt[j][1])  #计算关节限位范围
                    margin = distance_to_Limit_point / Joint_range          #计算限位余量：偏移量的绝对值/关节限位范围
                    if margin > 0.3:        
                        Limit_margin = True  
                    else:
                        Limit_margin = False
                    rospy.loginfo("margin = {}".format(margin))
                    is_limited = True           
                    break
                
                if position_diff > 3:    #限位阈值，3rad=171.9度
                    rospy.loginfo("point{}-joint{}:{}".format(i,j+1,joint_positions1))
      
                    Limit_margin = True
                    is_limited = True
                    break
        if  is_limited:
            break
    if not is_limited:
        rospy.loginfo("Check OK! 轨迹有效")         
    return is_limited,Limit_margin

#姿态调整
def angle_adjustment(pose,axis,angle):      
    if len(pose) == 6:
        q_initial = quaternion_from_euler(pose[3], pose[4], pose[5])    #初始姿态四元数，将欧拉角转换为四元数
    else:
        q_initial = (pose[3], pose[4], pose[5],pose[6])     #len>6，默认为四元数
    if axis == 'z':
        q_rotation = quaternion_from_euler(0, 0, angle)     #绕z轴旋转，转换得到关于z轴的旋转四元数
    if axis == 'y':
        q_rotation = quaternion_from_euler(0, angle, 0)
    if axis == 'x':
        q_rotation = quaternion_from_euler(angle, 0, 0)    
    q_new = quaternion_multiply(q_initial, q_rotation)      #q_new为调整后的姿态四元数，值初始姿态四元数乘以旋转四元数
    pose[3:] = q_new    #将q_new放入pose列表中的3号索引以后的位置
    return pose



# def traj_validity_check(trajectory):
#     if type (trajectory)is moveit_msgs.msg._RobotTrajectory.RobotTrajectory:
#         point_num=len(trajectory.joint_trajectory.point)
#         trajectory.joint_trajectory.point.positions
        
#         # for i
#         trajectory.joint_trajectory.point.velocities

#         trajectory.joint_trajectory.point.accelerations
        
#         return 0
#     # else:
#     #         raise MoveItCommanderException(
#     #             "Expected value in the range from 0 to 1 for scaling factor"
#     #         )
#     def scale_trajectory_speed(traj, scale):
#        # Create a new trajectory object
#        new_traj = RobotTrajectory()
       
#        # Initialize the new trajectory to be the same as the input trajectory
#        new_traj.joint_trajectory = traj.joint_trajectory
       
#        # Get the number of joints involved
#        n_joints = len(traj.joint_trajectory.joint_names)
       
#        # Get the number of points on the trajectory
#        n_points = len(traj.joint_trajectory.points)
        
#        # Store the trajectory points
#        points = list(traj.joint_trajectory.points)
       
#        # Cycle through all points and joints and scale the time from start,
#        # as well as joint speed and acceleration
#        for i in range(n_points):
#            point = JointTrajectoryPoint()
           
#            # The joint positions are not scaled so pull them out first
#            point.positions = list(traj.joint_trajectory.points[i].positions)

#            # Next, scale the time_from_start for this point
#             # point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
#            # Get the joint velocities for this point
#            point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
#            # Get the joint accelerations for this point
#            point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
#            # Scale the velocity and acceleration for each joint at this point
#            for j in range(n_joints):
#             #    if point.positions[j]
#                if has_velocity_limits:
#                 if point.velocities[j] > joint_max_velocity[j]:
#                     vel_exception_point
#                     rospy.loginfo("velocities Test OK") 
#                 else:
#                     raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor")     
#                if has_acceleration_limits:
#                 point.accelerations[j] = point.accelerations[j] * scale * scale
        
#            # Store the scaled trajectory point
#            points[i] = point

#        rospy.loginfo("velocities Check OK") 

#        # Assign the modified points to the new trajectory
#        new_traj.joint_trajectory.points = points

#        # Return the new trajecotry
#        return new_traj

#     else:
#         print("traj type is not std")