Merge branch 'main' into optistack-fatrop

.github/workflows/install_and_test.yml

@@ -2,8 +2,8 @@ name: CI
 
 on:
   push:
-    branches:
-      - main
+    # branches:
+    #   - main
     paths-ignore:
       - '**/*.md'  # Do not run workflow if only markdown files are changed
   pull_request:

examples/calculate_screw_invariants_pose.py

@@ -159,7 +159,7 @@ def main():
     plot_trajectory_kettle(T, 'Input Trajectory')
 
     # Initialize OCP object and calculate pose
-    OCP = OCP_calc_pose(N, rms_error_traj_pos = 1e-3, rms_error_traj_rot= 1e-2, solver='fatrop')
+    OCP = OCP_calc_pose(N, rms_error_traj_pos = 1e-3, rms_error_traj_rot= 1e-2, solver='fatrop', bool_unsigned_invariants=False)
 
     # Calculate screw invariants and other outputs
     U, T_sol, T_isa = OCP.calculate_invariants(T, dt)

invariants_py/calculate_invariants/opti_calculate_screw_invariants_pose.py

@@ -117,15 +117,15 @@ def calculate_invariants(self, T_obj_m, h):
 if __name__ == "__main__":
 
     from invariants_py.reparameterization import interpT
-    import invariants_py.kinematics.orientation_kinematics as SE3
+    import invariants_py.kinematics.orientation_kinematics as SO3
 
     # Test data    
     N = 100
     T_start = np.eye(4)  # Rotation matrix 1
     T_mid = np.eye(4)
-    T_mid[:3, :3] = SE3.rotate_z(np.pi)  # Rotation matrix 3
+    T_mid[:3, :3] = SO3.rotate_z(np.pi)  # Rotation matrix 3
     T_end = np.eye(4)
-    T_end[:3, :3] = SE3.RPY(np.pi/2, 0, np.pi/2)  # Rotation matrix 2
+    T_end[:3, :3] = SO3.RPY(np.pi/2, 0, np.pi/2)  # Rotation matrix 2
 
     # 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))

invariants_py/calculate_invariants/opti_calculate_screw_invariants_pose_fatrop.py

@@ -137,15 +137,15 @@ def calculate_invariants(self, T_obj_m, h):
 if __name__ == "__main__":
 
     from invariants_py.reparameterization import interpT
-    import invariants_py.kinematics.orientation_kinematics as SE3
+    import invariants_py.kinematics.orientation_kinematics as SO3
 
     # Test data    
     N = 100
     T_start = np.eye(4)  # Rotation matrix 1
     T_mid = np.eye(4)
-    T_mid[:3, :3] = SE3.rotate_z(np.pi)  # Rotation matrix 3
+    T_mid[:3, :3] = SO3.rotate_z(np.pi)  # Rotation matrix 3
     T_end = np.eye(4)
-    T_end[:3, :3] = SE3.RPY(np.pi/2, 0, np.pi/2)  # Rotation matrix 2
+    T_end[:3, :3] = SO3.RPY(np.pi/2, 0, np.pi/2)  # Rotation matrix 2
 
     # 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))

invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py

@@ -50,6 +50,8 @@ def __init__(self, window_len = 100,
 
         # Lower bounds on controls
         if bool_unsigned_invariants:
+            # positive vs negative velocities
+            # false means both pos and neg values are allowed
             opti.subject_to(U[0,:]>=0) # lower bounds on control
             #opti.subject_to(U[1,:]>=0) # lower bounds on control
 
@@ -60,14 +62,17 @@ def __init__(self, window_len = 100,
 
         # Additional constraint: First invariant remains constant throughout the window
         if geometric:
+            # enforce a fixed velocity for the entire trajectory
             for k in range(window_len-2):
                 opti.subject_to(U[0,k+1] == U[0,k])
 
         # Measurement fitting constraint
         trajectory_error = 0
         for k in range(window_len):
             err_pos = p_obj[k] - p_obj_m[k] # position error
-            trajectory_error = trajectory_error + cas.dot(err_pos,err_pos)    
+            trajectory_error = trajectory_error + cas.dot(err_pos,err_pos) 
+
+        # r.24 in paper [Maxim 2023]   
         opti.subject_to(trajectory_error  < window_len*rms_error_traj**2)
 
         # Boundary constraints

invariants_py/dynamics_screw_invariants.py

@@ -29,7 +29,6 @@ def transform_screw_cas(T, twist):
     v = twist[3:]
 
     omega_new = R @ omega
-    #v_new = R @ (v - cas.cross(omega, p))
     v_new = R @ v - cas.cross(p, R @ omega)
 
     return cas.vertcat(omega_new, v_new)

invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars.py

@@ -159,7 +159,7 @@ def generate_trajectory_global(self,U_demo,initial_values,boundary_constraints,s
 
         return invariants, calculated_trajectory, calculated_movingframe
 
-def generate_trajectory_translation(invariant_model, boundary_constraints, N=40):
+def generate_trajectory_translation(invariant_model, boundary_constraints, N=100):
 
     # Specify optimization problem symbolically
     OCP = OCP_gen_pos(N = N)

invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py

@@ -134,12 +134,12 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False
         if include_robot_model:
             objective_invariants = ocp.sum(1/window_len*cas.dot(w_invars*(invars - invars_demo),w_invars*(invars - invars_demo)),include_last=True)
             # Objective for joint limits
-            e_pos = cas.dot(p_obj_fwkin - p_obj,p_obj_fwkin - p_obj)
-            e_rot = cas.dot(R_obj.T @ R_obj_fwkin - np.eye(3),R_obj.T @ R_obj_fwkin - np.eye(3))
-            objective_inverse_kin = ocp.sum(e_pos + e_rot + 0.001*cas.dot(qdot,qdot),include_last = True)
-            # ocp.subject_to(p_obj == p_obj_fwkin)
-            # ocp.subject_to(tril_vec_no_diag(R_obj.T @ R_obj_fwkin - np.eye(3)) == 0.)
-            # objective_inverse_kin = ocp.sum(0.001*cas.dot(qdot,qdot),include_last = True)
+            # e_pos = cas.dot(p_obj_fwkin - p_obj,p_obj_fwkin - p_obj)
+            # e_rot = cas.dot(R_obj.T @ R_obj_fwkin - np.eye(3),R_obj.T @ R_obj_fwkin - np.eye(3))
+            # objective_inverse_kin = ocp.sum(e_pos + e_rot + 0.001*cas.dot(qdot,qdot),include_last = True)
+            ocp.subject_to(p_obj - p_obj_fwkin == 0)
+            ocp.subject_to(tril_vec_no_diag(R_obj.T @ R_obj_fwkin - np.eye(3)) == 0.)
+            objective_inverse_kin = ocp.sum(0.001*cas.dot(qdot,qdot),include_last = True)
             objective = ocp.sum(objective_invariants + objective_inverse_kin, include_last = True)
         else:
             objective = ocp.sum(1/window_len*cas.dot(w_invars*(invars - invars_demo),w_invars*(invars - invars_demo)),include_last=True)
@@ -152,7 +152,7 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False
             ocp._method.set_name("/codegen/generation_pose")
         else:
             ocp.method(rockit.MultipleShooting(N=window_len-1))
-            ocp.solver('ipopt', {'expand':True})
+            ocp.solver('ipopt', {'expand':True, 'ipopt.max_iter': max_iters})
             # ocp.solver('ipopt',{"print_time":True,"expand":True},{'tol':1e-4,'print_level':0,'ma57_automatic_scaling':'no','linear_solver':'mumps','max_iter':100})
 
         # Solve already once with dummy values for code generation (TODO: can this step be avoided somehow?)
@@ -176,7 +176,7 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False
         if "position" in boundary_constraints and "initial" in boundary_constraints["position"]:
             ocp.set_value(p_obj_start, p_obj_dummy)
         if "position" in boundary_constraints and "final" in boundary_constraints["position"]:
-            ocp.set_value(p_obj_end, p_obj_dummy + np.array([0.01,0,0]))
+            ocp.set_value(p_obj_end, p_obj_dummy + np.array([0.5,0,0]))
         if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]:
             ocp.set_value(R_obj_start, np.eye(3))
         if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]:

invariants_py/initialization.py

@@ -311,7 +311,7 @@ def estimate_initial_frames(vector_traj):
 
     #TODO  this is not correct yet, ex not perpendicular to ey + not robust for singularities, these parts must still be transferred from Matlab
 
-    ex = vector_traj / np.linalg.norm(vector_traj,axis=1).reshape(N,1)
+    ex = vector_traj / (np.linalg.norm(vector_traj,axis=1).reshape(N,1)+0.000001)
     ez = np.tile( np.array((0,0,1)), (N,1) )
     ey = np.array([ np.cross(ez[i,:],ex[i,:]) for i in range(N) ])
 

invariants_py/kinematics/orientation_kinematics.py

@@ -242,14 +242,17 @@ def logm(R):
     -------
     A (3x3) skew-symmetric matrix corresponding to the displacement rotation
     """
-    # Cosine of the rotation angle
+
+    assert R.shape == (3, 3), "Input must be a 3x3 matrix"
+
+    # Calculate cosine of the rotation angle
     ca = (trace(R)-1)/2.0
 
-    # Special case rotation angle = 0
+    # Special case: rotation angle = 0
     if np.isclose(ca,1):
         return np.zeros((3,3))
 
-    # Special case rotation angle = pi or -pi
+    # Special case: rotation angle = pi or -pi
     if np.isclose(ca,-1):
         _,_,VT = np.linalg.svd(R - np.eye(3)) # R*v = v --> (R-I)*v = 0
         rotation_vec = VT[-1,:]

invariants_py/kinematics/rigidbody_kinematics.py

@@ -263,6 +263,8 @@ def logm_T(T):
     This function calculates the matrix logarithm corresponding to the displacement twist
     of a given homogeneous transformation matrix.
 
+    Note: this implementation is more efficient than scipy.linalg.logm for screws.
+
     Parameters
     ----------
     T : numpy.ndarray

invariants_py/reparameterization.py

@@ -182,6 +182,11 @@ def reparameterize_positiontrajectory_arclength(trajectory, N=0):
         Sequence of positions with arc length parameterization
     s : array
         Arc length as a function of original parameterization
+    
+    1/len(s) : dimensionaless arclength
+    or 1/N is the dimensionaless arclength
+    s[-1]/N : can be h for arclength
+     
 
     """
 

pyproject.toml

@@ -9,9 +9,9 @@ authors = [ {name = "Maxim Vochten"},
 readme = "README.md"
 requires-python = ">=3.8"
 keywords = ["trajectory analysis", "trajectory generation", "invariant", "coordinate-invariant", "trajectory representation", "trajectory optimization", "trajectory planning", "robotics", "dynamics", "kinematics", "screw theory", "optimal control", "geometric optimal control", "differential geometry", "optimization", "casadi", "python"]
-dependencies = [ # it is possible to have optional dependencies
+dependencies = [ # keep dependencies limited if possible, (you can also have optional dependencies)
     "scipy",
-    "casadi>=3.6.7",
+    "casadi>=3.6.7", # to use the fatrop solver we need casadi>=3.6.7
     "PyQt5",
     "matplotlib",
     "numpy",
@@ -22,7 +22,7 @@ dependencies = [ # it is possible to have optional dependencies
     "jupyter",
     "yourdfpy"
 ]
-classifiers = [ # only relevant/used for PyPI
+classifiers = [ # these classifiers are used to add labels to the project on PyPI: https://pypi.org/project/invariants-py/
     "Development Status :: 3 - Alpha",
     "Intended Audience :: Science/Research",
     "License :: OSI Approved :: MIT License",