@@ -1016,6 +1016,14 @@ def update_joint_transforms_kernel(
10161016
10171017 child_xform = joint_X_c [tid ]
10181018 parent_xform = joint_X_p [tid ]
1019+
1020+ # update body pos and quat from parent joint transform
1021+ child = joint_child [joint_in_world ] # Newton body id
1022+ body_id = body_mapping [child ] # MuJoCo body id
1023+ tf = parent_xform * wp .transform_inverse (child_xform )
1024+ body_pos [worldid , body_id ] = tf .p
1025+ body_quat [worldid , body_id ] = wp .quat (tf .q .w , tf .q .x , tf .q .y , tf .q .z )
1026+
10191027 lin_axis_count = joint_dof_dim [tid , 0 ]
10201028 ang_axis_count = joint_dof_dim [tid , 1 ]
10211029
@@ -1042,13 +1050,6 @@ def update_joint_transforms_kernel(
10421050 joint_axis [worldid , ai ] = wp .quat_rotate (child_xform .q , axis )
10431051 joint_pos [worldid , ai ] = child_xform .p
10441052
1045- # update body pos and quat from parent joint transform
1046- child = joint_child [joint_in_world ] # Newton body id
1047- body_id = body_mapping [child ] # MuJoCo body id
1048- tf = parent_xform * wp .transform_inverse (child_xform )
1049- body_pos [worldid , body_id ] = tf .p
1050- body_quat [worldid , body_id ] = wp .quat (tf .q .w , tf .q .x , tf .q .y , tf .q .z )
1051-
10521053
10531054@wp .kernel (enable_backward = False )
10541055def update_shape_mappings_kernel (
0 commit comments