def diff_calc_func(start_mat, end_mat, corrected_avg_interval): temp = start_mat.M.Inverse() * end_mat.M ang, temp_axis = Rotation.GetRotAngle(temp) o = start_mat.M * temp_axis p_start_vec = start_mat.p p_end_vec = end_mat.p # print "p vectors ", p_end_vec, p_start_vec # Finding difference in position values between start and end delta_x = p_end_vec[0] - p_start_vec[0] delta_y = p_end_vec[1] - p_start_vec[1] delta_z = p_end_vec[2] - p_start_vec[2] # Calculation of differentiated values such as velocity/acceleration twist_val = geometry_msgs.msg.Vector3() twist_val.x = delta_x / corrected_avg_interval.to_sec() twist_val.y = delta_y / corrected_avg_interval.to_sec() twist_val.z = delta_z / corrected_avg_interval.to_sec() # twist_rot = geometry_msgs.msg.Vector3() # Calculation of angular velocity/acceleration twist_rot = o * (ang / corrected_avg_interval.to_sec()) # print "accel is ", twist_vel # print "accel rot is ", twist_rot[2] return twist_val, twist_rot
def vel_calc_func(start_mat, end_mat, corrected_average_interval): # Create rotation matrices start_r = start_mat.M end_r = end_mat.M # Transform matrices using inverse for correct orientation temp = start_mat.M.Inverse() * end_mat.M temp_mat = Frame(Rotation(temp)) ang, temp_axis = Rotation.GetRotAngle(temp) o = start_mat.M * temp_axis p_start_vec = start_mat.p p_end_vec = end_mat.p # print "p vectors ", p_end_vec, p_start_vec delta_x = p_end_vec[0] - p_start_vec[0] print "delta x is ", delta_x delta_y = p_end_vec[1] - p_start_vec[1] delta_z = p_end_vec[2] - p_start_vec[2] # Assign values to a Vector3 element twist_vel = geometry_msgs.msg.Vector3() twist_vel.x = delta_x / corrected_average_interval.to_sec() twist_vel.y = delta_y / corrected_average_interval.to_sec() twist_vel.z = delta_z / corrected_average_interval.to_sec() # twist_rot = geometry_msgs.msg.Vector3() twist_rot = o * (ang / corrected_average_interval.to_sec()) print "twist vel is ", twist_vel print "twist rot is ", twist_rot[2] return twist_vel, twist_rot