Add backbone for F/T sensor payload ID

Author: nepfaff

robot_payload_id/data/__init__.py

@@ -1,5 +1,6 @@
 from .data_matrix_numeric import (
     compute_base_param_mapping,
+    construct_ft_data_matrix,
     extract_numeric_data_matrix_autodiff,
 )
 from .data_matrix_symbolic import (
@@ -15,5 +16,6 @@
 from .joint_data import (
     compute_autodiff_joint_data_from_fourier_series_traj_params1,
     compute_autodiff_joint_data_from_simple_sinusoidal_traj_params,
+    compute_ft_sensor_measurements,
     generate_random_joint_data,
 )

robot_payload_id/data/data_matrix_numeric.py

@@ -16,9 +16,12 @@
     DecomposeLumpedParameters,
     Evaluate,
     Expression,
+    JacobianWrtVariable,
     MathematicalProgram,
     MultibodyForces_,
     MultibodyPlant_,
+    SpatialAcceleration,
+    SpatialForce,
 )
 from tqdm import tqdm
 
@@ -317,6 +320,179 @@ def extract_numeric_data_matrix_autodiff(
     return W_data, w0, tau_data
 
 
+def construct_ft_matrix(
+    linear_gravity: np.ndarray,
+    angular_velocity: np.ndarray,
+    linear_acceleration: np.ndarray,
+    angular_acceleration: np.ndarray,
+) -> np.ndarray:
+    """
+    Constructs the 6x10 matrix that maps the inertial parameters to the force-torque
+    sensor measurements. The equations take the form [f, tau] = mat * alpha, where f
+    and tau are the force and torque measurements, and alpha are the lumped inertial
+    parameters of form [m, hx, hy, hz, Ixx, Ixy, Ixz, Iyy, Iyz, Izz].
+    All parameters and measurements should be expressed in the sensor frame S.
+
+    See equation 5 in "Improving Force Control Performance by Computational Elimination
+    of Non-Contact Forces/Torques" by Kubus et al.
+
+    Args:
+        linear_gravity (np.ndarray): The linear gravity vector of shape (3,). Expressed
+            in the sensor frame. g_W_S.
+        angular_velocity (np.ndarray): The angular velocity vector of shape (3,).
+            Expressed in the sensor frame. omega_WS_S.
+        linear_acceleration (np.ndarray): The linear acceleration vector of shape (3,).
+            Expressed in the sensor frame. a_WS_S.
+        angular_acceleration (np.ndarray): The angular acceleration vector of shape
+            (3,). Expressed in the sensor frame. alpha_WS_S.
+
+    Returns:
+        np.ndarray: The 6x10 matrix that maps the inertial parameters to the
+            force-torque sensor measurements.
+    """
+    mat = np.zeros((6, 10))
+    mat[0, 0:4] = [
+        linear_acceleration[0] - linear_gravity[0],
+        -angular_velocity[1] ** 2 - angular_velocity[2] ** 2,
+        angular_velocity[0] * angular_velocity[1] - angular_acceleration[2],
+        angular_velocity[0] * angular_velocity[2] + angular_acceleration[1],
+    ]
+    mat[1, 0:4] = [
+        linear_acceleration[1] - linear_gravity[1],
+        angular_velocity[0] * angular_velocity[1] + angular_acceleration[2],
+        -angular_velocity[0] ** 2 - angular_velocity[2] ** 2,
+        angular_velocity[1] * angular_velocity[2] - angular_acceleration[0],
+    ]
+    mat[2, 0:4] = [
+        linear_acceleration[2] - linear_gravity[2],
+        angular_velocity[0] * angular_velocity[2] - angular_acceleration[1],
+        angular_velocity[1] * angular_velocity[2] + angular_acceleration[0],
+        -angular_velocity[1] ** 2 - angular_velocity[0] ** 2,
+    ]
+    mat[3, 0:4] = [
+        0.0,
+        0.0,
+        linear_acceleration[2] - linear_gravity[2],
+        linear_gravity[1] - linear_acceleration[1],
+    ]
+    mat[4, 0:4] = [
+        0.0,
+        linear_gravity[2] - linear_acceleration[2],
+        0.0,
+        linear_acceleration[0] - linear_gravity[0],
+    ]
+    mat[5, 0:4] = [
+        0.0,
+        linear_acceleration[1] - linear_gravity[1],
+        linear_gravity[0] - linear_acceleration[0],
+        0.0,
+    ]
+    mat[3, 4:10] = [
+        angular_acceleration[0],
+        angular_acceleration[1] - angular_velocity[0] * angular_velocity[2],
+        angular_acceleration[2] + angular_velocity[0] * angular_velocity[1],
+        -angular_velocity[1] * angular_velocity[2],
+        angular_velocity[1] ** 2 - angular_velocity[2] ** 2,
+        angular_velocity[1] * angular_velocity[2],
+    ]
+    mat[4, 4:10] = [
+        angular_velocity[0] * angular_velocity[2],
+        angular_acceleration[0] + angular_velocity[1] * angular_velocity[2],
+        angular_velocity[2] ** 2 - angular_velocity[0] ** 2,
+        angular_acceleration[1],
+        angular_acceleration[2] - angular_velocity[0] * angular_velocity[1],
+        -angular_velocity[0] * angular_velocity[2],
+    ]
+    mat[5, 4:10] = [
+        -angular_velocity[0] * angular_velocity[1],
+        angular_velocity[0] ** 2 - angular_velocity[1] ** 2,
+        angular_acceleration[0] - angular_velocity[1] * angular_velocity[2],
+        angular_velocity[0] * angular_velocity[1],
+        angular_acceleration[1] + angular_velocity[0] * angular_velocity[2],
+        angular_acceleration[2],
+    ]
+    return mat
+
+
+def construct_ft_data_matrix(
+    plant_components: ArmPlantComponents,
+    ft_body_name: str,
+    ft_sensor_frame_name: str,
+    joint_data: JointData,
+    use_progress_bar: bool = True,
+) -> np.ndarray:
+    """Constructs the numeric data matrix for the force-torque sensor measurements.
+    See `construct_ft_matrix` for more details.
+
+    Args:
+        plant_components (ArmPlantComponents): This must contain the plant. Optionally,
+            it can also contain the plant's context.
+        body_name: The name of the body to which the force-torque sensor is welded.
+            This could be the F/T sensor body if it is modelled as such.
+        ft_sensor_frame_name (str): The name of the frame that the F/T sensor is
+            measuring in.
+        joint_data (JointData): The joint data. Only the joint positions, velocities,
+            and accelerations are used for the data matrix computation.
+        use_progress_bar (bool, optional): Whether to use a progress bar.
+
+    Returns:
+        np.ndarray: The numeric data matrix of shape (num_joints * num_timesteps, 10).
+    """
+    plant = plant_components.plant
+    context = plant_components.plant_context
+
+    ft_sensor_body = plant.GetBodyByName(ft_body_name)
+    ft_sensor_body_frame = ft_sensor_body.body_frame()
+    # Transform from sensor body frame S to measurement frame F
+    measurement_frame = plant.GetFrameByName(ft_sensor_frame_name)
+    X_SF = measurement_frame.CalcPose(context, ft_sensor_body_frame)
+
+    # Transform from the world frame to the sensor frame S
+    X_WS = plant.EvalBodyPoseInWorld(context=context, body=ft_sensor_body)
+    X_WF = X_WS @ X_SF
+    X_FW = X_WF.inverse()
+    R_FW = X_FW.rotation()
+
+    num_timesteps = len(joint_data.sample_times_s)
+    data_matrix = np.zeros((num_timesteps * 6, 10))
+    for i in tqdm(
+        range(num_timesteps),
+        desc="Extracting data matrix",
+        disable=not use_progress_bar,
+    ):
+        # Set joint data
+        plant.SetPositions(context, joint_data.joint_positions[i])
+        plant.SetVelocities(context, joint_data.joint_velocities[i])
+
+        # Compute forces due to gravity
+        g_W = [0.0, 0.0, -9.81]
+        g_W_F = R_FW @ g_W
+
+        # Compute spatial velocity
+        V_WS = plant.EvalBodySpatialVelocityInWorld(
+            context=context,
+            body=ft_sensor_body,
+        )
+        omega_WS_F = R_FW @ V_WS.rotational()
+
+        # Compute spatial accelerations
+        A_WS = plant.EvalBodySpatialAccelerationInWorld(
+            context=context,
+            body=ft_sensor_body,
+        )
+        a_WS_F = R_FW @ A_WS.translational()
+        alpha_WS_F = R_FW @ A_WS.rotational()
+
+        data_matrix[i * 6 : (i + 1) * 6, :] = construct_ft_matrix(
+            linear_gravity=g_W_F,
+            angular_velocity=omega_WS_F,
+            linear_acceleration=a_WS_F,
+            angular_acceleration=alpha_WS_F,
+        )
+
+    return data_matrix
+
+
 def compute_base_param_mapping(
     W_data: np.ndarray, scale_by_singular_values: bool = False, tol: float = 1e-6
 ) -> np.ndarray:

robot_payload_id/data/joint_data.py

@@ -2,10 +2,20 @@
 
 import numpy as np
 
-from pydrake.all import AutoDiffXd, MultibodyForces_, MultibodyPlant
+from pydrake.all import (
+    AutoDiffXd,
+    JacobianWrtVariable,
+    MultibodyForces_,
+    MultibodyPlant,
+    SpatialForce,
+)
 from tqdm import tqdm
 
-from robot_payload_id.utils import FourierSeriesTrajectoryAttributes, JointData
+from robot_payload_id.utils import (
+    ArmPlantComponents,
+    FourierSeriesTrajectoryAttributes,
+    JointData,
+)
 
 
 def generate_random_joint_data(
@@ -332,3 +342,79 @@ def compute_autodiff_joint_data_from_fourier_series_traj_params1(
         sample_times_s=np.arange(num_timesteps) * sample_delta,
     )
     return joint_data
+
+
+def compute_ft_sensor_measurements(
+    arm_plant_components: ArmPlantComponents,
+    joint_data: JointData,
+    ft_sensor_frame_name: str,
+) -> JointData:
+    """"""
+    plant = arm_plant_components.plant
+    context = arm_plant_components.plant_context
+
+    measurement_frame = plant.GetFrameByName(ft_sensor_frame_name)
+    X_WF = measurement_frame.CalcPoseInWorld(context)
+    X_FW = X_WF.inverse()
+    R_FW = X_FW.rotation()
+
+    # spatial_jacobian = plant.CalcJacobianSpatialVelocity(
+    #     context=context,
+    #     with_respect_to=JacobianWrtVariable.kQDot,
+    #     frame_B=plant.GetFrameByName(ft_sensor_frame_name),
+    #     p_BoBp_B=np.zeros(3),
+    #     frame_A=plant.world_frame(),
+    #     frame_E=plant.world_frame(),
+    # )
+    # spatial_jacobian_transpose_inv = np.linalg.pinv(spatial_jacobian.T)
+
+    ft_sensor_measurements = np.empty((joint_data.joint_positions.shape[0], 6))
+    for i in range(joint_data.joint_positions.shape[0]):
+        # Set joint data
+        plant.SetPositions(context, joint_data.joint_positions[i])
+        plant.SetVelocities(context, joint_data.joint_velocities[i])
+
+        # # Compute inverse dynamics
+        # forces = MultibodyForces_(plant)
+        # plant.CalcForceElementsContribution(context, forces)
+        # joint_torques = plant.CalcInverseDynamics(
+        #     context=context,
+        #     known_vdot=joint_data.joint_accelerations[i],
+        #     external_forces=forces,
+        # )
+
+        # # Convert generalized forces into spatial forces
+        # spatial_force = spatial_jacobian_transpose_inv @ joint_torques
+
+        # ft_sensor_measurement = np.concatenate(
+        #     [spatial_force.translational(), spatial_force.rotational()]
+        # )
+
+        # TODO: Figure out who to ask about this. Doesn't make sense without this
+        # taking accelerations but inverting Jacobian bad as not full rank.
+
+        # Compute the reaction force F_CJc_Jc on the child body C at the last joint's
+        # child frame Jc. For the iiwa, Jc is 'iiwa_link_7'.
+        # TODO: See https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1710888837360519?thread_ts=1710863581.014389&cid=C2WBPQDB7
+        reaction_force: SpatialForce = plant.get_reaction_forces_output_port().Eval(
+            context
+        )[-1]
+        # Invert using action-reaction principle.
+        ft_sensor_measurement = -np.concatenate(
+            [reaction_force.translational(), reaction_force.rotational()]
+        )
+
+        # Transform into measurement frame. This works as there is no lever arm effect.
+        ft_sensor_measurement_in_F = np.concatenate(
+            [R_FW @ ft_sensor_measurement[:3], R_FW @ ft_sensor_measurement[3:]]
+        )
+        ft_sensor_measurements[i] = ft_sensor_measurement_in_F
+
+    return JointData(
+        joint_positions=joint_data.joint_positions,
+        joint_velocities=joint_data.joint_velocities,
+        joint_accelerations=joint_data.joint_accelerations,
+        joint_torques=joint_data.joint_torques,
+        ft_sensor_measurements=ft_sensor_measurements,
+        sample_times_s=joint_data.sample_times_s,
+    )

robot_payload_id/environment/arm.py

@@ -50,31 +50,31 @@ def create_arm(
     )
 
     # Add Controller
-    controller_plant = MultibodyPlant(time_step)
-    controller_parser = get_parser(controller_plant)
-    controller_parser.AddModels(arm_file_path)
-    controller_plant.Finalize()
-    arm_controller = builder.AddSystem(
-        InverseDynamicsController(
-            controller_plant,
-            kp=[100] * num_joints,
-            kd=[10] * num_joints,
-            ki=[1] * num_joints,
-            has_reference_acceleration=False,
-        )
-    )
-    arm_controller.set_name("arm_controller")
-    builder.Connect(
-        plant.get_state_output_port(arm),
-        arm_controller.get_input_port_estimated_state(),
-    )
-    builder.Connect(
-        arm_controller.get_output_port_control(), plant.get_actuation_input_port(arm)
-    )
-    builder.Connect(
-        trajectory_source.get_output_port(),
-        arm_controller.get_input_port_desired_state(),
-    )
+    # controller_plant = MultibodyPlant(time_step)
+    # controller_parser = get_parser(controller_plant)
+    # controller_parser.AddModels(arm_file_path)
+    # controller_plant.Finalize()
+    # arm_controller = builder.AddSystem(
+    #     InverseDynamicsController(
+    #         controller_plant,
+    #         kp=[100] * num_joints,
+    #         kd=[10] * num_joints,
+    #         ki=[1] * num_joints,
+    #         has_reference_acceleration=False,
+    #     )
+    # )
+    # arm_controller.set_name("arm_controller")
+    # builder.Connect(
+    #     plant.get_state_output_port(arm),
+    #     arm_controller.get_input_port_estimated_state(),
+    # )
+    # builder.Connect(
+    #     arm_controller.get_output_port_control(), plant.get_actuation_input_port(arm)
+    # )
+    # builder.Connect(
+    #     trajectory_source.get_output_port(),
+    #     arm_controller.get_input_port_desired_state(),
+    # )
 
     # Meshcat
     if use_meshcat:
@@ -89,9 +89,9 @@ def create_arm(
         meshcat_visualizer = None
 
     state_logger = LogVectorOutput(plant.get_state_output_port(), builder)
-    commanded_torque_logger = LogVectorOutput(
-        arm_controller.get_output_port_control(), builder
-    )
+    # commanded_torque_logger = LogVectorOutput(
+    #     arm_controller.get_output_port_control(), builder
+    # )
 
     diagram = builder.Build()
 
@@ -101,7 +101,7 @@ def create_arm(
         plant=plant,
         trajectory_source=trajectory_source,
         state_logger=state_logger,
-        commanded_torque_logger=commanded_torque_logger,
+        commanded_torque_logger=None,
         meshcat=meshcat,
         meshcat_visualizer=meshcat_visualizer,
     )

robot_payload_id/optimization/__init__.py

@@ -1,4 +1,4 @@
-from .inertial_param_sdp import solve_inertial_param_sdp
+from .inertial_param_sdp import solve_ft_payload_sdp, solve_inertial_param_sdp
 from .nevergrad_augmented_lagrangian import NevergradAugmentedLagrangian
 from .optimal_experiment_design_b_spline import (
     ExcitationTrajectoryOptimizerBsplineBlackBoxALNumeric,