Порядок поворота. Постбилдер

[GreyWorm 5]

Всем привет. Вопрос такой:

 

В постбилдере есть процедура PB_CMD_auto_3D_rotation. Мы с ее помощью получаем углы для разворота системы координат в порядке X--> Y--> Z, ну и координаты в развернутой системе координат на выходе. Все это нужно для обработки 3+2, для циклов разворота и т.д.

Вот код процедуры:

Скрытый текст

# This command is used to get auto 3D coordinate rotation by tool axis vector.

  global mom_kin_machine_type
  global save_mom_kin_machine_type
  global mom_tool_axis mom_tool_axis_type
  global mom_siemens_coord_rotation
  global mom_kin_coordinate_system_type
  global mom_siemens_5axis_mode
  global mom_operation_type
  global mom_kin_4th_axis_point save_mom_kin_4th_axis_point
  global mom_kin_5th_axis_point save_mom_kin_5th_axis_point
  global mom_kin_4th_axis_vector save_mom_kin_4th_axis_vector
  global mom_kin_5th_axis_vector save_mom_kin_5th_axis_vector
  global mom_sys_4th_axis_has_limits
  global mom_sys_5th_axis_has_limits
  global save_mom_sys_4th_axis_has_limits
  global save_mom_sys_5th_axis_has_limits
  global mom_mcs_goto mom_pos mom_prev_pos
  global mom_out_angle_pos
  global coord_ang_A coord_ang_B coord_ang_C
  global coord_ref_x coord_ref_y coord_ref_z
  global RAD2DEG DEG2RAD
  global mom_tool_path_type
  global mom_usermachtype

  if { ![string match "*5_axis*" $mom_kin_machine_type] } {
return
  }

if {[string match "MULTIAXIS" $mom_usermachtype]} {
return
}

  if { [info exists mom_kin_coordinate_system_type] && [string match "CSYS" $mom_kin_coordinate_system_type] } {
return
  }

  if { [info exists mom_siemens_5axis_mode] && [string match "TRAFOOF" $mom_siemens_5axis_mode] } {
return
  }

  if { ![info exists mom_tool_axis_type] } {
    set mom_tool_axis_type 0
  }
  if { ![info exists mom_tool_path_type] } {
    set mom_tool_path_type "undefined"
  }

  set mom_siemens_coord_rotation 2

 # Get initial/first point by rotation
  if { [string match "5_axis_dual_head" $mom_kin_machine_type] } {
    set rot0 [expr $mom_out_angle_pos(1)*$DEG2RAD]
    set rot1 [expr $mom_out_angle_pos(0)*$DEG2RAD]
    set mom_pos(3) $mom_out_angle_pos(1)
    set mom_pos(4) $mom_out_angle_pos(0)
  } else {
    set rot0 [expr $mom_out_angle_pos(0)*$DEG2RAD]
    set rot1 [expr $mom_out_angle_pos(1)*$DEG2RAD]
    set mom_pos(3) $mom_out_angle_pos(0)
    set mom_pos(4) $mom_out_angle_pos(1)
  }

 # Reolad kinematics to dual-table machine
  PB_CMD_save_kinematics

  if { [string match "5_axis_dual_head" $mom_kin_machine_type] } {
    PB_CMD_swap_4th_5th_kinematics

    set angle_pos(0)         $mom_out_angle_pos(0)
    set mom_out_angle_pos(0) $mom_out_angle_pos(1)
    set mom_out_angle_pos(1) $angle_pos(0)
  }

  if { ![string match "5_axis_dual_table" $mom_kin_machine_type] } {
    set mom_kin_machine_type "5_axis_dual_table"
  }

  set x 0.0; set y 0.0; set z 0.0;
  VEC3_init x y z mom_kin_4th_axis_point
  VEC3_init x y z mom_kin_5th_axis_point
  MOM_reload_kinematics

  VECTOR_ROTATE mom_kin_5th_axis_vector [expr -1*$rot1] mom_mcs_goto v
  VECTOR_ROTATE mom_kin_4th_axis_vector [expr -1*$rot0] v mom_pos
  MOM_reload_variable -a mom_pos

  global cycle_init_flag mom_current_motion
  global mom_cycle_rapid_to
  if {[info exists cycle_init_flag] && $cycle_init_flag == "TRUE"} {
     if { [string match "initial_move" $mom_current_motion]} {
         set mom_pos(2) [expr $mom_pos(2) + $mom_cycle_rapid_to]
     }
  }

  global mom_prev_out_angle_pos
  set mom_prev_out_angle_pos(0) $mom_out_angle_pos(0)
  set mom_prev_out_angle_pos(1) $mom_out_angle_pos(1)
  MOM_reload_variable -a mom_out_angle_pos
  MOM_reload_variable -a mom_prev_out_angle_pos

 # Initialize coordinate rotation angles
  set coord_ang_A 0.0 ; set coord_ang_B 0.0 ; set coord_ang_C 0.0
  set coord_ref_x 0.0 ; set coord_ref_y 0.0 ; set coord_ref_z 0.0
  set X(0) 1.0; set X(1) 0.0; set X(2) 0.0
  set Y(0) 0.0; set Y(1) 1.0; set Y(2) 0.0
  set Z(0) 0.0; set Z(1) 0.0; set Z(2) 1.0

 # Calculate rotation matrix
  VECTOR_ROTATE mom_kin_4th_axis_vector [expr $rot0] X v1
  VECTOR_ROTATE mom_kin_4th_axis_vector [expr $rot0] Y v2
  VECTOR_ROTATE mom_kin_4th_axis_vector [expr $rot0] Z v3

  VECTOR_ROTATE mom_kin_5th_axis_vector [expr $rot1] v1 X
  VECTOR_ROTATE mom_kin_5th_axis_vector [expr $rot1] v2 Y
  VECTOR_ROTATE mom_kin_5th_axis_vector [expr $rot1] v3 Z

  MTX3_init_x_y_z X Y Z rotation_matrix

 # Calculate euler angles , rotation order is X->Y->Z
  if { [info exists rotation_matrix] } {
    set m0 $rotation_matrix(0)
    set m1 $rotation_matrix(1)
    set m2 $rotation_matrix(2)
    set m3 $rotation_matrix(3)
    set m4 $rotation_matrix(4)
    set m5 $rotation_matrix(5)
    set m6 $rotation_matrix(6)
    set m7 $rotation_matrix(7)
    set m8 $rotation_matrix(8)

    set cos_b_sq [expr $m0*$m0 + $m3*$m3]

    if { [EQ_is_equal $cos_b_sq 0.0] } {

      set cos_b 0.0
      set cos_c 1.0
      set cos_a $m4
      set sin_c 0.0
      set sin_a [expr -1*$m5]

      if { $m6 < 0.0 } {
        set sin_b 1.0
      } else {
        set sin_b -1.0
      }

    } else {

      set cos_b [expr sqrt($cos_b_sq)]
      set sin_b [expr -$m6]

      set cos_a [expr $m8/$cos_b]
      set sin_a [expr $m7/$cos_b]

      set cos_c [expr $m0/$cos_b]
      set sin_c [expr $m3/$cos_b]

    }

    set coord_ang_A [expr -atan2($sin_a,$cos_a)*$RAD2DEG]
    set coord_ang_B [expr -atan2($sin_b,$cos_b)*$RAD2DEG]
    set coord_ang_C [expr -atan2($sin_c,$cos_c)*$RAD2DEG]
  }

 

ней есть такой момент:

MTX3_init_x_y_z X Y Z rotation_matrix

 # Calculate euler angles , rotation order is X->Y->Z

.....

.....

Т.е. понятно, что порядок разворота X->Y->Z.

 

Можно ли как-то этот порядок изменить, чтобы расчеты велись для порядка Z-->Y-->X или Y-->Z-->X и т.д.???

[mr_Voden 148]

@GreyWorm Вот код функции MTX3_init_x_y_z

MTX3_init_x_y_z  

#  MTX3_init_x_y_z (u, v, w, r) r = (u, v, w)      Initialize a matrix from
#                                                  given x, y and z vectors
proc  MTX3_init_x_y_z { u v w r } {
      upvar $u u1 ; upvar $v v1 ; upvar $w w1 ; upvar $r r1
      set status 0

#   Unitize the input vectors and proceed if neither vector is zero.

    if {[VEC3_unitize u1 xxxxx] && \
        [VEC3_unitize v1 yyyyy] && \
        [VEC3_unitize w1 zzzzz]} {

#       Proceed if the input vectors are orthogonal

        if {[EQ_is_zero [VEC3_dot xxxxx yyyyy]] && \
            [EQ_is_zero [VEC3_dot xxxxx zzzzz]] && \
            [EQ_is_zero [VEC3_dot yyyyy zzzzz]]} {

#           Cross the unitized input vectors and initialize the matrix
#           Orthonormal test is stricter than EQ_ask_systol, so
#           recalculate y and z.

            set status 1
            VEC3_cross xxxxx yyyyy zzzzz
            set len [VEC3_unitize zzzzz zzzzz]
            VEC3_cross zzzzz xxxxx yyyyy

            set r1(0) $xxxxx(0)
            set r1(1) $xxxxx(1)
            set r1(2) $xxxxx(2)
            set r1(3) $yyyyy(0)
            set r1(4) $yyyyy(1)
            set r1(5) $yyyyy(2)
            set r1(6) $zzzzz(0)
            set r1(7) $zzzzz(1)
            set r1(8) $zzzzz(2)

        }
    }

    return $status
}

Hide  

В PB_CMD_auto_3D_rotation выше определены единичные вектора X Y Z и с помощью VECTOR_ROTATE развернуты на углы mom_out_angle_pos (переведенные в радианы) вокруг 4 и 5й оси, исходя из кинематической схемы станка.

VECTOR_ROTATE  

#=============================================================
proc VECTOR_ROTATE { axis angle input_vector output_vector } {
#=============================================================
# This proc is used to rotating a vector about arbitrary axis
#
   upvar $axis r; upvar $input_vector input ; upvar $output_vector output
   #set up matrix to rotate about an arbitrary axis
   set m(0) [expr $r(0)*$r(0)*(1-cos($angle))+cos($angle)]
   set m(1) [expr $r(0)*$r(1)*(1-cos($angle))-$r(2)*sin($angle)]
   set m(2) [expr $r(0)*$r(2)*(1-cos($angle))+$r(1)*sin($angle)]
   set m(3) [expr $r(0)*$r(1)*(1-cos($angle))+$r(2)*sin($angle)]
   set m(4) [expr $r(1)*$r(1)*(1-cos($angle))+cos($angle)]
   set m(5) [expr $r(1)*$r(2)*(1-cos($angle))-$r(0)*sin($angle)]
   set m(6) [expr $r(0)*$r(2)*(1-cos($angle))-$r(1)*sin($angle)]
   set m(7) [expr $r(1)*$r(2)*(1-cos($angle))+$r(0)*sin($angle)]
   set m(8) [expr $r(2)*$r(2)*(1-cos($angle))+cos($angle)]
   MTX3_vec_multiply input m output
}

 

Hide  

В дальнейшем, как я понял, с помощью функции MTX3_init_x_y_z создается rotation_matrix. За порядок расчета ( X->Y->Z ) отвечает фрагмент кода "if { [info exists rotation_matrix] } {....}".