debugging_example.py

    if args.enable_torques:
        print("ASSUMING THE WITNESS POINTS ARE DEFINED WITH RESPECT TO THE CENTER OF MASS!!! CHANGE BODY FRAME ORIGIN IF THIS IS NOT TRUE")
        # for each contact point, setup the product of binary-continuous for w*b
        # sum all the torques for a single object

        # setup obj-obj first
        for i in range(config.num_internal_bodies):
            net_torque_to_object_i = np.zeros(3)
            for j in range(config.num_internal_bodies):
                net_torque_to_i_from_j = np.zeros(3)

                if i!= j:
                    witness_i_top_set = witness_points_dict[(i, selected_facets[i][1])]
                    witness_j_bottom_set = witness_points_dict[(j, selected_facets[j][0])]

                    witness_j_top_set = witness_points_dict[(j, selected_facets[j][1])]
                    witness_i_bottom_set = witness_points_dict[(i, selected_facets[i][0])]

                    for ik_idx, ik in enumerate(witness_i_top_set):
                        for jk_idx, jk in enumerate(witness_j_bottom_set):
                            if np.all(ik[:2] == np.zeros(2)):
                                print("ln284 skipping, ik is under COM")
                            else:
                                # any coincident pair of contact points can generate a force
                                force_as_normal_vector = np.append(ik[:2], force_var_dict[f"force_top,i{i}_bot,j{j}_ik{ik_idx}_jk{jk_idx}"].item())

                                # I don't need product of binary here. If the binary shuts off the force
                                # the force will already be 0, and the cross product will be 0.

                                # we take the witness point from ik because it is in the COM frame of object i
                                # net_torque_to_i_from_j += np.cross(ik, force_as_normal_vector)

                                net_torque_to_i_from_j = net_torque_to_i_from_j + cross_prod(ik, force_as_normal_vector)


                    for ik_idx, ik in enumerate(witness_i_bottom_set):
                        for jk_idx, jk in enumerate(witness_j_top_set):
                            if np.all(ik[:2] == np.zeros(2)):
                                print("ln301 skipping, ik is under COM")
                            else:
                                force_as_normal_vector = np.append(ik[:2], force_var_dict[f"force_bot,i{i}_top,j{j}_ik{ik_idx}_jk{jk_idx}"].item())

                                # net_torque_to_i_from_j += np.cross(ik, force_as_normal_vector)
                                net_torque_to_i_from_j = net_torque_to_i_from_j + cross_prod(ik, force_as_normal_vector)

                net_torque_to_object_i = net_torque_to_object_i + net_torque_to_i_from_j

            # add torque from ground
            witness_i_bottom_set = witness_points_dict[(i, selected_facets[i][0])]
            # technically, we only need one force here... except for torque?
            for ik_idx, ik in enumerate(witness_i_bottom_set):
                if np.all(ik[:2] == np.zeros(2)):
                    print("ln315 skipping, ik is under COM")
                else:
                    force_as_normal_vector = np.append(ik[:2], force_var_dict[f"force_i{i}_ground_ik{ik_idx}"].item())
                    # net_torque_to_object_i += np.cross(ik, force_as_normal_vector)
                    net_torque_to_object_i = net_torque_to_object_i + cross_prod(ik, force_as_normal_vector)

            def check_bool_vector(vec):
                if type(vec[0]) == np.bool_ and type(vec[1]) == np.bool_ and type(vec[2]) == np.bool_:
                    return True
                else:
                    return False
            if check_bool_vector(eq(net_torque_to_object_i, np.zeros(3))) and np.all(eq(net_torque_to_object_i, np.zeros(3))):
                print("only have COMs... not adding constraint")
            else:
                # Note: since the last dimension is zero, we have to remove it before adding the constraint
                prog.AddLinearConstraint(eq(net_torque_to_object_i[:2], np.ones(2) * TORQUE_BALANCE_EPSILON)
                                         )