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