Automatic extraction of robot parameters from urdf using 'yourdf' pac…

…kage

examples/fatrop-tests/trajectory_generation_fullpose_coupled_rockit.py

@@ -155,14 +155,13 @@ def interpolate_model_invariants(demo_invariants, progress_values):
 }
 
 # Define robot parameters
-urdf_file_name = 'ur10.urdf' # use None if do not want to include robot model
 robot_params = {
-    "urdf_file_name": urdf_file_name,
-    "joint_number": 6, # Number of joints
-    "home": [-pi, -2.27, 2.27, -pi/2, -pi/2, pi/4] * np.ones((number_samples,6)), # Home position joint values
-    "q_lim": [2*pi, 2*pi, pi, 2*pi, 2*pi, 2*pi], # Join limits
-    "root": 'base_link', # Name of the robot root
-    "tip": 'TCP_frame' # Name of the robot tip
+    "urdf_file_name": 'ur10.urdf', # use None if do not want to include robot model
+    "q_init": [-pi, -2.27, 2.27, -pi/2, -pi/2, pi/4] * np.ones((number_samples,6)), # Initial joint values
+    "tip": 'TCP_frame' # Name of the robot tip (if empty standard 'tool0' is used)
+    # "joint_number": 6, # Number of joints (if empty it is automatically taken from urdf file)
+    # "q_lim": [2*pi, 2*pi, pi, 2*pi, 2*pi, 2*pi], # Join limits (if empty it is automatically taken from urdf file)
+    # "root": 'world', # Name of the robot root (if empty it is automatically taken from urdf file)
 }
 
 # specify optimization problem symbolically
@@ -178,7 +177,7 @@ def interpolate_model_invariants(demo_invariants, progress_values):
         "rotational": R_r_init_array,
     },
     "invariants": model_invariants,
-    "joint-values": robot_params["home"] if urdf_file_name is not None else {}
+    "joint-values": robot_params["q_init"] if robot_params["urdf_file_name"] is not None else {}
 }
 
 # Define OCP weights

invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py

@@ -14,6 +14,7 @@
 from invariants_py.kinematics.orientation_kinematics import rotate_x
 from invariants_py.initialization import generate_initvals_from_constraints
 import invariants_py.data_handler as dh
+import yourdfpy as urdf
 
 class OCP_gen_pose:
 
@@ -26,11 +27,12 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False
         path_to_urdf = dh.find_robot_path(urdf_file_name) 
         include_robot_model = True if path_to_urdf is not None else False
         if include_robot_model:
-            nb_joints = robot_params.get('join_number', 6)
-            home = robot_params.get('home', np.zeros(nb_joints))
-            q_limits = robot_params.get('q_lim', 2*np.pi*np.ones(nb_joints))
-            root = robot_params.get('root', 'base_link')
+            robot = urdf.URDF.load(path_to_urdf)
+            nb_joints = robot_params.get('joint_number', robot.num_actuated_joints)
+            q_limits = robot_params.get('q_lim', [robot._actuated_joints[i].limit.upper for i in range(robot.num_actuated_joints)])
+            root = robot_params.get('root', robot.base_link)
             tip = robot_params.get('tip', 'tool0')
+            q_init = robot_params.get('q_init', np.zeros(nb_joints))
 
         #%% Create decision variables and parameters for the optimization problem
 
@@ -169,7 +171,7 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False
         ocp.set_value(w_invars, 0.001+np.zeros((6,window_len)))
         ocp.set_value(h, 0.01)
         if include_robot_model:
-            ocp.set_initial(q,home) 
+            ocp.set_initial(q,q_init) 
             ocp.set_initial(qdot, 0.001*np.ones((nb_joints,window_len-1)))
             ocp.set_value(q_lim,q_limits)
         # Boundary constraints
@@ -293,7 +295,7 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False
             self.nb_joints = nb_joints
             self.q = q
             self.qdot = qdot
-            self.home = home
+            self.q_init = q_init
             self.q_lim = q_limits
 
     def generate_trajectory(self, invariant_model, boundary_constraints, step_size, weights_params = {}, initial_values = {}):
@@ -328,7 +330,7 @@ def generate_trajectory(self, invariant_model, boundary_constraints, step_size,
                         boundary_values_list.append(value)
 
         if self.first_window and not initial_values:
-            self.solution = generate_initvals_from_constraints(boundary_constraints, np.size(invariant_model,0), q_init = self.home if self.include_robot_model else None)
+            self.solution = generate_initvals_from_constraints(boundary_constraints, np.size(invariant_model,0), q_init = self.q_init if self.include_robot_model else None)
             self.first_window = False
         elif self.first_window:
             self.solution = [initial_values["invariants"][:N-1,:].T, initial_values["trajectory"]["position"][:N,:].T, initial_values["moving-frame"]["translational"][:N].T.transpose(1,2,0).reshape(3,3*N), initial_values["moving-frame"]["rotational"][:N].T.transpose(1,2,0).reshape(3,3*N), initial_values["trajectory"]["orientation"][:N].T.transpose(1,2,0).reshape(3,3*N)]

pyproject.toml

@@ -19,7 +19,8 @@ dependencies = [ # it is possible to have optional dependencies
     "numpy-stl",
     "pandas",
     "urdf2casadi",
-    "jupyter"
+    "jupyter",
+    "yourdfpy"
 ]
 classifiers = [ # only relevant/used for PyPI
     "Development Status :: 3 - Alpha",