fix sign error

trajectory-invariants · Nov 14, 2024 · 83f29f6 · 83f29f6
1 parent a36fa19
commit 83f29f6
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 2 deletions.
diff --git a/invariants_py/calculate_invariants/opti_calculate_screw_invariants_pose_fatrop.py b/invariants_py/calculate_invariants/opti_calculate_screw_invariants_pose_fatrop.py
@@ -150,7 +150,7 @@ def calculate_invariants(self, T_obj_m, h):
     # Interpolate between R_start and R_end
     T_obj_m = interpT(np.linspace(0,1,N), np.array([0,0.5,1]), np.stack([T_start, T_mid, T_end],0))
 
-    OCP = OCP_calc_pose(N, rms_error_traj_pos = 10e-3, rms_error_traj_rot = 10e-3, bool_unsigned_invariants=True, solver='fatrop')
+    OCP = OCP_calc_pose(N, rms_error_traj_pos = 10e-3, rms_error_traj_rot = 10e-3, bool_unsigned_invariants=True, solver='ipopt')
 
     # Example: calculate invariants for a given trajectory
     h = 0.01 # step size for integration of dynamic equations

diff --git a/invariants_py/dynamics_screw_invariants.py b/invariants_py/dynamics_screw_invariants.py
@@ -29,7 +29,7 @@ def transform_screw_cas(T, twist):
     v = twist[3:]
 
     omega_new = R @ omega
-    v_new = R @ v - cas.cross(p, R @ omega)
+    v_new = R @ v + cas.cross(p, R @ omega)
 
     return cas.vertcat(omega_new, v_new)