diff --git a/example/experiment1.py b/example/experiment1.py index f17a52f6..82488438 100644 --- a/example/experiment1.py +++ b/example/experiment1.py @@ -23,9 +23,9 @@ def __init__(self, robot, eff_link, base_link=None): self.qd = self.builder.get_model_state(self.robot_name, 0, time_deriv=1) self.qc = self.builder.add_parameter("qc", robot.ndof) if base_link is None: - self.J = robot.get_global_geometric_jacobian(eff_link, self.qc) + self.J = robot.get_global_link_geometric_jacobian(eff_link, self.qc) else: - self.J = robot.get_geometric_jacobian(eff_link, self.qc, base_link) + self.J = robot.get_link_geometric_jacobian(eff_link, self.qc, base_link) self.veff = self.J @ self.qd self._solver_duration = None diff --git a/example/experiment2.py b/example/experiment2.py index b2e040ae..f4d6cb19 100644 --- a/example/experiment2.py +++ b/example/experiment2.py @@ -74,10 +74,10 @@ def __init__(self): qF = builder.get_model_state(self.name, t=0) qd = (qF - qc) / dt pg = builder.add_parameter("pg", 3) # goal position - Jp = robot.get_global_linear_jacobian(ee_link, qc) + Jp = robot.get_global_link_linear_jacobian(ee_link, qc) pc = pos(qc) p = pc + dt * Jp @ qd - Ja = robot.get_global_angular_geometric_jacobian(ee_link, qc) + Ja = robot.get_global_link_angular_geometric_jacobian(ee_link, qc) va = Ja @ qd Jg = robot.get_global_link_geometric_jacobian(ee_link, qF) manip = cs.sqrt(cs.det(Jg @ Jg.T)) diff --git a/example/pushing.py b/example/pushing.py index cb5a0405..31805138 100644 --- a/example/pushing.py +++ b/example/pushing.py @@ -251,7 +251,7 @@ def __init__(self, dt, thresh_angle): q = qc + dt * dq # Get jacobian - Jl = kuka.get_global_linear_jacobian(link_ee, qc) + Jl = kuka.get_global_link_linear_jacobian(link_ee, qc) # Get end-effector velocity dp = Jl @ dq diff --git a/optas/models.py b/optas/models.py index ae372505..cb12a1e9 100644 --- a/optas/models.py +++ b/optas/models.py @@ -1,5 +1,4 @@ """! @brief Several Model classes are defined.""" - import os import warnings import functools @@ -1197,11 +1196,6 @@ def get_link_rpy_function( numpy_output=numpy_output, ) - @deprecation_warning("get_global_link_geometric_jacobian") - def get_global_geometric_jacobian(self, link, q): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_global_link_geometric_jacobian( @@ -1224,7 +1218,6 @@ def get_global_link_geometric_jacobian( jacobian_columns = [] for joint in self.urdf.joints: - if joint.type == "fixed": continue @@ -1260,7 +1253,6 @@ def get_global_link_geometric_jacobian( jcol = cs.DM.zeros(6) jacobian_columns.append(jcol) - # Sort columns of jacobian jacobian_columns_ordered = [jacobian_columns[idx] for idx in joint_index_order] @@ -1271,11 +1263,6 @@ def get_global_link_geometric_jacobian( return J - @deprecation_warning("get_global_link_geometric_jacobian_function") - def get_global_geometric_jacobian_function(self, link, n=1): - """! Deprecated function.""" - pass - def get_global_link_geometric_jacobian_function( self, link: str, @@ -1293,11 +1280,6 @@ def get_global_link_geometric_jacobian_function( "J", link, self.get_global_link_geometric_jacobian, n=n ) - @deprecation_warning("get_global_link_analytical_jacobian") - def get_global_analytical_jacobian(self, link, q): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_global_link_analytical_jacobian( @@ -1314,11 +1296,6 @@ def get_global_link_analytical_jacobian( self.get_global_link_angular_analytical_jacobian(link, q), ) - @deprecation_warning("get_global_link_analytical_jacobian_function") - def get_global_analytical_jacobian_function(self, link): - """! Deprecated function.""" - pass - def get_global_link_analytical_jacobian_function( self, link: str, @@ -1340,11 +1317,6 @@ def get_global_link_analytical_jacobian_function( numpy_output=numpy_output, ) - @deprecation_warning("get_link_geometric_jacobian") - def get_geometric_jacobian(self): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_link_geometric_jacobian( @@ -1371,11 +1343,6 @@ def get_link_geometric_jacobian( return J - @deprecation_warning("get_link_geometric_jacobian_function") - def get_geometric_jacobian_function(self, link, base_link, n=1): - """! Deprecated function.""" - pass - def get_link_geometric_jacobian_function( self, link: str, @@ -1400,11 +1367,6 @@ def get_link_geometric_jacobian_function( numpy_output=numpy_output, ) - @deprecation_warning("get_link_analytical_jacobian") - def get_analytical_jacobian(self): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_link_analytical_jacobian( @@ -1422,11 +1384,6 @@ def get_link_analytical_jacobian( self.get_link_angular_analytical_jacobian(link, q, base_link), ) - @deprecation_warning("get_link_analytical_jacobian_function") - def get_analytical_jacobian_function(self, link, base_link): - """! Deprecated function.""" - pass - def get_link_analytical_jacobian_function( self, link: str, @@ -1451,11 +1408,6 @@ def get_link_analytical_jacobian_function( numpy_output=numpy_output, ) - @deprecation_warning("get_global_link_linear_jacobian") - def get_global_linear_jacobian(self, link, q): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_global_link_linear_jacobian( @@ -1470,11 +1422,6 @@ def get_global_link_linear_jacobian( J = self.get_global_link_geometric_jacobian(link, q) return J[:3, :] - @deprecation_warning("get_global_link_linear_jacobian_function") - def get_global_linear_jacobian_function(self, link, n=1): - """! Deprecated function.""" - pass - def get_global_link_linear_jacobian_function( self, link: str, @@ -1496,11 +1443,6 @@ def get_global_link_linear_jacobian_function( numpy_output=numpy_output, ) - @deprecation_warning("get_link_linear_jacobian") - def get_linear_jacobian(self, link, q, base_link): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_link_linear_jacobian( @@ -1516,11 +1458,6 @@ def get_link_linear_jacobian( J = self.get_link_geometric_jacobian(link, q, base_link) return J[:3, :] - @deprecation_warning("get_link_linear_jacobian_function") - def get_linear_jacobian_function(self, link, base_link, n=1): - """! Deprecated function.""" - pass - def get_link_linear_jacobian_function( self, link: str, base_link: str, n: int = 1, numpy_output: bool = False ) -> cs.Function: @@ -1541,11 +1478,6 @@ def get_link_linear_jacobian_function( numpy_output=numpy_output, ) - @deprecation_warning("get_global_link_angular_geometric_jacobian") - def get_global_angular_geometric_jacobian(self, link, q): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_global_link_angular_geometric_jacobian( @@ -1561,11 +1493,6 @@ def get_global_link_angular_geometric_jacobian( J = self.get_global_link_geometric_jacobian(link, q) return J[3:, :] - @deprecation_warning("get_global_link_angular_geometric_jacobian_function") - def get_global_angular_geometric_jacobian_function(self, link, n=1): - """! Deprecated function.""" - pass - def get_global_link_angular_geometric_jacobian_function( self, link: str, @@ -1587,11 +1514,6 @@ def get_global_link_angular_geometric_jacobian_function( numpy_output=numpy_output, ) - @deprecation_warning("get_global_link_angular_analytical_jacobian") - def get_global_angular_analytical_jacobian(self, link, q): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_global_link_angular_analytical_jacobian( @@ -1605,11 +1527,6 @@ def get_global_link_angular_analytical_jacobian( """ return self.get_link_angular_analytical_jacobian(link, q, self.get_root_link()) - @deprecation_warning("get_global_link_angular_analytical_jacobian_function") - def get_global_angular_analytical_jacobian_function(self, link): - """! Deprecated function.""" - pass - def get_global_link_angular_analytical_jacobian_function( self, link: str, @@ -1631,11 +1548,6 @@ def get_global_link_angular_analytical_jacobian_function( numpy_output=numpy_output, ) - @deprecation_warning("get_link_angular_geometric_jacobian") - def get_angular_geometric_jacobian(self, link, q, base_link): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_link_angular_geometric_jacobian( @@ -1651,11 +1563,6 @@ def get_link_angular_geometric_jacobian( J = self.get_link_geometric_jacobian(link, q, base_link) return J[3:, :] - @deprecation_warning("get_link_angular_geometric_jacobian_function") - def get_angular_geometric_jacobian_function(self, link, base_link, n=1): - """! Deprecated function.""" - pass - def get_link_angular_geometric_jacobian_function( self, link: str, @@ -1680,11 +1587,6 @@ def get_link_angular_geometric_jacobian_function( numpy_output=numpy_output, ) - @deprecation_warning("get_link_angular_analytical_jacobian") - def get_angular_analytical_jacobian(self, link, q, base_link): - """! Deprecated function.""" - pass - @arrayify_args @listify_output def get_link_angular_analytical_jacobian( @@ -1708,11 +1610,6 @@ def get_link_angular_analytical_jacobian( return Ja(q) - @deprecation_warning("get_link_angular_analytical_jacobian_function") - def get_angular_analytical_jacobian_function(self, link, base_link): - """! Deprecated function.""" - pass - def get_link_angular_analytical_jacobian_function( self, link: str,