Add option to constrain max total mass

Author: nepfaff

robot_payload_id/optimization/inertial_param_sdp.py

@@ -68,6 +68,7 @@ def solve_inertial_param_sdp(
     identify_viscous_friction: bool = True,
     identify_dynamic_dry_friction: bool = True,
     payload_only=False,
+    known_max_mass: Optional[float] = None,
     solver_kPrintToConsole: bool = False,
 ) -> Tuple[
     MathematicalProgram, MathematicalProgramResult, np.ndarray, np.ndarray, np.ndarray
@@ -102,6 +103,8 @@ def solve_inertial_param_sdp(
         payload_only (bool, optional): Whether to only include the 10 inertial
             parameters of the last link. These are the parameters that we care about
             for payload identification.
+        known_max_mass (float, optional): The known maximum mass of the robot. This is
+            used as an upper bound on the sum of the identified masses.
         solver_kPrintToConsole (bool, optional): Whether to print solver output.
 
     Returns:
@@ -291,6 +294,11 @@ def solve_inertial_param_sdp(
         for dynamic_dry_friction in dynamic_dry_frictions:
             prog.AddConstraint(dynamic_dry_friction >= 0)
 
+    # Maximum total mass constraint
+    if known_max_mass is not None:
+        masses = np.array([var.m for var in variables])
+        prog.AddConstraint(masses.sum() <= known_max_mass)
+
     options = None
     if solver_kPrintToConsole:
         options = SolverOptions()

scripts/solve_inertial_param_sdp.py

@@ -361,6 +361,12 @@ def main():
         + "idea if the initial parameters contain random values which can occur when "
         + "some of the parameters are unidentifiable.",
     )
+    parser.add_argument(
+        "--known_max_mass",
+        type=float,
+        help="The known maximum mass of the robot. This is used to set the upper bound "
+        + "for the combined mass parameters.",
+    )
     parser.add_argument(
         "--process_joint_data",
         action="store_true",
@@ -481,6 +487,7 @@ def main():
     initial_param_path = args.initial_param_path
     output_param_path = args.output_param_path
     use_initial_params_for_regularization = args.use_initial_params_for_regularization
+    known_max_mass = args.known_max_mass
     do_process_joint_data = args.process_joint_data
     num_endpoints_to_remove = args.num_endpoints_to_remove
     compute_velocities = not args.not_compute_velocities
@@ -775,6 +782,7 @@ def main():
         identify_viscous_friction=identify_viscous_friction,
         identify_dynamic_dry_friction=identify_dynamic_dry_friction,
         payload_only=payload_only,
+        known_max_mass=known_max_mass,
         solver_kPrintToConsole=args.kPrintToConsole,
     )
     if result.is_success():