Using the trace in the rotation error.

ami-iit · Oct 16, 2023 · 065423e · 065423e
1 parent b856e8d
commit 065423e
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 2 deletions.
diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py
@@ -403,7 +403,7 @@ def add_kinematics_regularization(
             qd=variables.references.frame_quaternion_xyzw,
         )["rotation_error"]
         problem.add_cost(
-            expression=cs.sumsqr(cs.DM.eye(3) - rotation_error_kinematics),
+            expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3),
             name="frame_rotation_error",
             scaling=self.settings.desired_frame_quaternion_cost_multiplier,
         )

diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py
@@ -698,7 +698,7 @@ def add_kinematics_regularization(
             qd=sym.references.desired_frame_quaternion_xyzw,
         )["rotation_error"]
         problem.add_expression_to_horizon(
-            expression=cs.sumsqr(cs.DM.eye(3) - rotation_error_kinematics),
+            expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3),
             apply_to_first_elements=False,
             name="frame_quaternion_error",
             mode=hp.ExpressionType.minimize,