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
