rotate Z+90 and few other improvements

Pose2Sim_Blender/cameras.py

@@ -373,10 +373,12 @@ def setup_cams(calib_params, collection=''):
 
         # rotation and translation
         r, t = world_to_camera_persp(R[c], T[c])
+        t = np.array([t[1], -t[0], t[2]])
         homog_matrix = np.block([[r,t.reshape(3,1)], 
                                 [np.zeros(3), 1 ]])
         camera_obj.matrix_world = mathutils.Matrix(homog_matrix)
         set_loc_rotation(camera_obj, np.radians([180,0,0]))
+        camera_obj.rotation_euler += np.radians([0, 0, -90])
 
         # principal point # see https://blender.stackexchange.com/a/58236/174689
         principal_point =  K[c][0,2],  K[c][1,2]
@@ -442,10 +444,10 @@ def show_images(camera, img_vid_path, single_image=False):
         if img.data.source == 'MOVIE':
             # BUG: if select single image, delete, and then reload as movie, does not update source as movie
             img.image_user.frame_duration =  img.data.frame_duration
-            img.image_user.frame_start =  0
+            img.image_user.frame_start =  1
         elif img.data.source == 'FILE': 
             img.data.source = 'SEQUENCE'
-            img.image_user.frame_start =  0
+            img.image_user.frame_start =  1
     else: 
         img.data.source = 'FILE'
 

Pose2Sim_Blender/forces.py

@@ -141,7 +141,8 @@ def import_forces(grf_path, direction='zup', target_framerate=30):
 
     # create arrows
     x_unit_arrow=Vector([1,0,0])
-    H_zup = np.array([[0,0,1,0], [1,0,0,0], [0,1,0,0], [0,0,0,1]])
+    # H_zup = np.array([[0,0,1,0], [1,0,0,0], [0,1,0,0], [0,0,0,1]])
+    H_zup = np.array([[1,0,0,0], [0,0,-1,0], [0,1,0,0], [0,0,0,1]])
     for n in range(0, len(times), conv_fac_frame_rate):
         for i, f in enumerate(grfNames):
             T = grf_data_np[n, 1+3+9*i : 1+3+9*i+3]

Pose2Sim_Blender/markers.py

@@ -137,8 +137,11 @@ def import_trc(trc_path, direction='zup', target_framerate=30):
             for n in range(0, len(times), conv_fac_frame_rate):
                 # y-up to z-up
                 if direction=='zup':
-                    loc_x = trc_data_np[n,3*i+3]
-                    loc_y = trc_data_np[n,3*i+1]
+                    # loc_x = trc_data_np[n,3*i+3]
+                    # loc_y = trc_data_np[n,3*i+1]
+                    # loc_z = trc_data_np[n,3*i+2]
+                    loc_x = trc_data_np[n,3*i+1]
+                    loc_y = -trc_data_np[n,3*i+3]
                     loc_z = trc_data_np[n,3*i+2]
                 else:
                     loc_x = trc_data_np[n,3*i+1]
@@ -147,11 +150,14 @@ def import_trc(trc_path, direction='zup', target_framerate=30):
                 obj=marker_collection.objects[m]
                 obj.location=loc_x,loc_y,loc_z
                 obj.keyframe_insert('location',frame=int(n/conv_fac_frame_rate)+1)
+        [ob.select_set(True) for ob in marker_collection.objects]
 
     elif trc_path.endswith('.c3d'):
         bpy.ops.preferences.addon_enable(module='io_anim_c3d')
         from io_anim_c3d import c3d_importer
         operator = bpy.types.Operator
-        c3d_importer.load(operator, bpy.context, filepath = trc_path)
+        c3d_importer.load(operator, bpy.context, \
+                          filepath = trc_path, \
+                          use_manual_orientation=True, axis_forward='Y', axis_up='Z')
 
     print(f'Marker data imported from {trc_path}')

Pose2Sim_Blender/model.py

@@ -191,6 +191,8 @@ def import_model(osim_path, modelRoot='',stlRoot='.',collection='', color = COLO
     empties = bpy.context.selected_objects
     [empt.hide_set(True) for empt in empties]
 
+    bpy.context.view_layer.active_layer_collection = bpy.context.view_layer.layer_collection.children[collection.name]
+
     print(f'OpenSim model imported from {osim_path}')
 
 

Pose2Sim_Blender/motion.py

@@ -120,7 +120,9 @@ def apply_mot_to_model(mot_path, osim_path, direction='zup', target_framerate=30
         # animate model
         state = model.initSystem()
         loc_rot_frame_all = []
-        H_zup = np.array([[0,0,1,0], [1,0,0,0], [0,1,0,0], [0,0,0,1]])
+        # H_zup = np.array([[0,0,1,0], [1,0,0,0], [0,1,0,0], [0,0,0,1]])
+        H_zup = np.array([[1,0,0,0], [0,0,-1,0], [0,1,0,0], [0,0,0,1]])
+
         print('Time frame:')
         for n in range(0, len(times), conv_fac_frame_rate):
             print(times[n], 's')