diff --git a/.pylintrc b/.pylintrc index 8038d3cb..f606fc6b 100644 --- a/.pylintrc +++ b/.pylintrc @@ -138,7 +138,7 @@ disable=print-statement, superfluous-parens, no-member, too-few-public-methods, - fixme + fixme,anomalous-backslash-in-string # Enable the message, report, category or checker with the given id(s). You can @@ -342,7 +342,8 @@ good-names=i, _, logger, dx, dy, dz, dq, q, - x, y, z, s, dt + x, y, z, s, dt, F, g, a, b, c + # Include a hint for the correct naming format with invalid-name include-naming-hint=no diff --git a/docs/source/modules.rst b/docs/source/modules.rst index 0528d358..b16995f0 100644 --- a/docs/source/modules.rst +++ b/docs/source/modules.rst @@ -6,51 +6,53 @@ Module references Interpolators ------------- +Interpolator base class +^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.Interpolator + :members: -SplineInterplator +Spline Interplator ^^^^^^^^^^^^^^^^^^^ .. autoclass:: toppra.SplineInterpolator :members: -RaveTrajectoryWrapper -^^^^^^^^^^^^^^^^^^^^^^ +Rave Trajectory Wrapper +^^^^^^^^^^^^^^^^^^^^^^^^^^ .. autoclass:: toppra.RaveTrajectoryWrapper :members: -Interpolator (base class) -^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.Interpolator - :members: - Constraints ------------ -JointAccelerationConstraint +Joint Acceleration Constraint ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. autoclass:: toppra.constraint.JointAccelerationConstraint :members: + :special-members: -JointVelocityConstraint +Joint Velocity Constraint ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. autoclass:: toppra.constraint.JointVelocityConstraint :members: + :special-members: -SecondOrderConstraint +Second Order Constraint ^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autoclass:: toppra.constraint.CanonicalLinearSecondOrderConstraint - :members: - -CanonicalLinearConstraint (base class) -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -.. autoclass:: toppra.constraint.CanonicalLinearConstraint +.. autoclass:: toppra.constraint.SecondOrderConstraint :members: + :special-members: -RobustLinearConstraint +Robust Linear Constraint ^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.constraint.RobustLinearConstraint + :members: -.. autoclass:: toppra.constraint.RobustCanonicalLinearConstraint +Canonical Linear Constraint (base class) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autoclass:: toppra.constraint.LinearConstraint :members: + :special-members: + Constraints (base class) ^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/tests/constraint/test_joint_acceleration.py b/tests/constraint/test_joint_acceleration.py index eb45f34a..c0621f52 100644 --- a/tests/constraint/test_joint_acceleration.py +++ b/tests/constraint/test_joint_acceleration.py @@ -6,7 +6,7 @@ from toppra.constants import JACC_MAXU -@pytest.fixture(scope="class", params=[1, 2, 6], name='acceleration_pc_data') +@pytest.fixture(params=[1, 2, 6], name='acceleration_pc_data') def create_acceleration_pc_fixtures(request): """ Parameterized Acceleration path constraint. @@ -45,59 +45,49 @@ def create_acceleration_pc_fixtures(request): return data, pc_vel -class TestClass_JointAccelerationConstraint(object): +def test_constraint_type(acceleration_pc_data): + """ Syntactic correct. """ + data, pc = acceleration_pc_data + assert pc.get_constraint_type() == constraint.ConstraintType.CanonicalLinear - Tests: - ------ - 1. syntactic: the object return should have correct dimension. +def test_constraint_params(acceleration_pc_data): + """ Test constraint satisfaction with cvxpy. + """ + data, constraint = acceleration_pc_data + path, ss, alim = data - 2. constraint satisfaction: the `PathConstraint` returned should - be consistent with the data. + # An user of the class + a, b, c, F, g, ubound, xbound = constraint.compute_constraint_params(path, ss, 1.0) + assert xbound is None - """ - def test_constraint_type(self, acceleration_pc_data): - """ Syntactic correct. - """ - data, pc = acceleration_pc_data - assert pc.get_constraint_type() == constraint.ConstraintType.CanonicalLinear - - def test_constraint_params(self, acceleration_pc_data): - """ Test constraint satisfaction with cvxpy. - """ - data, constraint = acceleration_pc_data - path, ss, alim = data - - # An user of the class - a, b, c, F, g, ubound, xbound = constraint.compute_constraint_params(path, ss, 1.0) + N = ss.shape[0] - 1 + dof = path.dof + + ps = path.evald(ss) + pss = path.evaldd(ss) + + F_actual = np.vstack((np.eye(dof), - np.eye(dof))) + g_actual = np.hstack((alim[:, 1], - alim[:, 0])) + + npt.assert_allclose(F, F_actual) + npt.assert_allclose(g, g_actual) + for i in range(0, N + 1): + npt.assert_allclose(a[i], ps[i]) + npt.assert_allclose(b[i], pss[i]) + npt.assert_allclose(c[i], np.zeros_like(ps[i])) + assert ubound is None assert xbound is None - N = ss.shape[0] - 1 - dof = path.get_dof() - - ps = path.evald(ss) - pss = path.evaldd(ss) - - F_actual = np.vstack((np.eye(dof), - np.eye(dof))) - g_actual = np.hstack((alim[:, 1], - alim[:, 0])) - - npt.assert_allclose(F, F_actual) - npt.assert_allclose(g, g_actual) - for i in range(0, N + 1): - npt.assert_allclose(a[i], ps[i]) - npt.assert_allclose(b[i], pss[i]) - npt.assert_allclose(c[i], np.zeros_like(ps[i])) - assert ubound is None - assert xbound is None - - def test_wrong_dimension(self, acceleration_pc_data): - data, pc = acceleration_pc_data - path_wrongdim = ta.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10)) - with pytest.raises(ValueError) as e_info: - pc.compute_constraint_params(path_wrongdim, np.r_[0, 0.5, 1], 1.0) - assert e_info.value.args[0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( - pc.get_dof(), 10 - ) + +def test_wrong_dimension(acceleration_pc_data): + data, pc = acceleration_pc_data + path_wrongdim = ta.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10)) + with pytest.raises(ValueError) as e_info: + pc.compute_constraint_params(path_wrongdim, np.r_[0, 0.5, 1], 1.0) + assert e_info.value.args[0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( + pc.get_dof(), 10 + ) diff --git a/tests/constraint/test_second_order.py b/tests/constraint/test_second_order.py index a25b18c2..ffab6959 100644 --- a/tests/constraint/test_second_order.py +++ b/tests/constraint/test_second_order.py @@ -34,19 +34,20 @@ def g(q): def test_wrong_dimension(coefficients_functions): + """If the given path has wrong dimension, raise error.""" A, B, C, cnst_F, cnst_g, path = coefficients_functions def inv_dyn(q, qd, qdd): return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q) constraint = toppra.constraint.SecondOrderConstraint(inv_dyn, cnst_F, cnst_g, dof=2) path_wrongdim = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10)) - with pytest.raises(AssertionError) as e_info: + with pytest.raises(ValueError) as e_info: constraint.compute_constraint_params(path_wrongdim, np.r_[0, 0.5, 1], 1.0) assert e_info.value.args[0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( - constraint.get_dof(), 10 + constraint.dof, 10 ) -def test_assemble_ABCFg(coefficients_functions): +def test_correctness(coefficients_functions): """ For randomly set A, B, C, F, g functions. The generated parameters must equal those given by equations. """ @@ -54,15 +55,16 @@ def test_assemble_ABCFg(coefficients_functions): def inv_dyn(q, qd, qdd): return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q) - constraint = toppra.constraint.SecondOrderConstraint(inv_dyn, cnst_F, cnst_g, dof=2) - constraint.set_discretization_type(0) + constraint = toppra.constraint.SecondOrderConstraint( + inv_dyn, cnst_F, cnst_g, dof=2, + discretization_scheme=toppra.constraint.DiscretizationType.Collocation) a, b, c, F, g, _, _ = constraint.compute_constraint_params( - path, np.linspace(0, path.get_duration(), 10), 1.0) + path, np.linspace(0, path.duration, 10), 1.0) # Correct params - q_vec = path.eval(np.linspace(0, path.get_duration(), 10)) - qs_vec = path.evald(np.linspace(0, path.get_duration(), 10)) - qss_vec = path.evaldd(np.linspace(0, path.get_duration(), 10)) + q_vec = path.eval(np.linspace(0, path.duration, 10)) + qs_vec = path.evald(np.linspace(0, path.duration, 10)) + qss_vec = path.evaldd(np.linspace(0, path.duration, 10)) for i in range(10): ai_ = A(q_vec[i]).dot(qs_vec[i]) @@ -73,3 +75,74 @@ def inv_dyn(q, qd, qdd): np.testing.assert_allclose(ci_, c[i]) np.testing.assert_allclose(cnst_F(q_vec[i]), F[i]) np.testing.assert_allclose(cnst_g(q_vec[i]), g[i]) + + +def test_correctness_friction(coefficients_functions): + """ Same as the above test, but has frictional effect. + """ + # setup + A, B, C, cnst_F, cnst_g, path = coefficients_functions + def inv_dyn(q, qd, qdd): + return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q) + def friction(q): + """Randomize with fixed input/output.""" + np.random.seed(int(np.sum(q) * 1000)) + return 2 + np.sin(q) + np.random.rand(len(q)) + + constraint = toppra.constraint.SecondOrderConstraint(inv_dyn, cnst_F, cnst_g, 2, friction=friction) + constraint.set_discretization_type(0) + a, b, c, F, g, _, _ = constraint.compute_constraint_params( + path, np.linspace(0, path.duration, 10), 1.0) + + # Correct params + p_vec = path.eval(np.linspace(0, path.duration, 10)) + ps_vec = path.evald(np.linspace(0, path.duration, 10)) + pss_vec = path.evaldd(np.linspace(0, path.duration, 10)) + + for i in range(10): + ai_ = A(p_vec[i]).dot(ps_vec[i]) + bi_ = A(p_vec[i]).dot(pss_vec[i]) + np.dot(ps_vec[i].T, B(p_vec[i]).dot(ps_vec[i])) + ci_ = C(p_vec[i]) + np.sign(ps_vec[i]) * friction(p_vec[i]) + np.testing.assert_allclose(ai_, a[i]) + np.testing.assert_allclose(bi_, b[i]) + np.testing.assert_allclose(ci_, c[i]) + np.testing.assert_allclose(cnst_F(p_vec[i]), F[i]) + + +def test_joint_force(coefficients_functions): + """ Same as the above test, but has frictional effect. + """ + # setup + A, B, C, cnst_F, cnst_g, path = coefficients_functions + def inv_dyn(q, qd, qdd): + return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q) + def friction(q): + """Randomize with fixed input/output.""" + np.random.seed(int(np.sum(q) * 1000)) + return 2 + np.sin(q) + np.random.rand(len(q)) + taulim = np.random.randn(2, 2) + + constraint = toppra.constraint.SecondOrderConstraint.joint_torque_constraint( + inv_dyn, taulim, friction=friction) + constraint.set_discretization_type(0) + a, b, c, F, g, _, _ = constraint.compute_constraint_params( + path, np.linspace(0, path.duration, 10), 1.0) + + # Correct params + p_vec = path.eval(np.linspace(0, path.duration, 10)) + ps_vec = path.evald(np.linspace(0, path.duration, 10)) + pss_vec = path.evaldd(np.linspace(0, path.duration, 10)) + + dof = 2 + F_actual = np.vstack((np.eye(dof), - np.eye(dof))) + g_actual = np.hstack((taulim[:, 1], - taulim[:, 0])) + + for i in range(10): + ai_ = A(p_vec[i]).dot(ps_vec[i]) + bi_ = A(p_vec[i]).dot(pss_vec[i]) + np.dot(ps_vec[i].T, B(p_vec[i]).dot(ps_vec[i])) + ci_ = C(p_vec[i]) + np.sign(ps_vec[i]) * friction(p_vec[i]) + np.testing.assert_allclose(ai_, a[i]) + np.testing.assert_allclose(bi_, b[i]) + np.testing.assert_allclose(ci_, c[i]) + np.testing.assert_allclose(F_actual, F[i]) + np.testing.assert_allclose(g_actual, g[i]) diff --git a/toppra/__init__.py b/toppra/__init__.py index c1d780e3..b0bf70b9 100644 --- a/toppra/__init__.py +++ b/toppra/__init__.py @@ -6,14 +6,12 @@ This package produces routines for creation and handling path constraints using the algorithm `TOPP-RA`. """ -# import constraint -# from .TOPP import * -from .interpolator import * -# from .constraints import * +from .interpolator import RaveTrajectoryWrapper, SplineInterpolator,\ + UnivariateSplineInterpolator, PolynomialPath, Interpolator from .utils import smooth_singularities, setup_logging -# from . import postprocess -# from .postprocess import compute_trajectory_gridpoints, compute_trajectory_uniform -from .planning_utils import retime_active_joints_kinematics, create_rave_torque_path_constraint -from . import constraint as constraint -from . import algorithm as algorithm -from . import solverwrapper as solverwrapper +from .planning_utils import retime_active_joints_kinematics,\ + create_rave_torque_path_constraint + +from . import constraint +from . import algorithm +from . import solverwrapper diff --git a/toppra/constants.py b/toppra/constants.py index 9c25f57f..bacb30f2 100644 --- a/toppra/constants.py +++ b/toppra/constants.py @@ -13,13 +13,12 @@ # TODO: What are these constant used for? MAXU = 10000 # Max limit for `u` MAXX = 10000 # Max limit for `x` -MAXSD = 100 # square root of maxx +MAXSD = 100 # square root of maxx # constraint creation -JVEL_MAXSD = 1e8 # max sd when creating joint velocity constraints +JVEL_MAXSD = 1e8 # max sd when creating joint velocity constraints JACC_MAXU = 1e16 # max u when creating joint acceleration constraint - # solver wrapper related constants. # NOTE: Read the wrapper's documentation for more details. diff --git a/toppra/constraint/__init__.py b/toppra/constraint/__init__.py index b88c88e6..106bf6f0 100644 --- a/toppra/constraint/__init__.py +++ b/toppra/constraint/__init__.py @@ -1,4 +1,4 @@ -"""toppra modules.""" +"""This module defines different dynamic constraints.""" from .constraint import ConstraintType, DiscretizationType, Constraint from .joint_torque import JointTorqueConstraint from .linear_joint_acceleration import JointAccelerationConstraint diff --git a/toppra/constraint/constraint.py b/toppra/constraint/constraint.py index c45c9906..7e93eb18 100644 --- a/toppra/constraint/constraint.py +++ b/toppra/constraint/constraint.py @@ -25,22 +25,7 @@ class DiscretizationType(Enum): class Constraint(object): - """Base class for all parameterization constraints. - - This class has two main functions: first, to tell its type and second, to produce - the parameters given the geometric path and the gridpoints. - - A derived class should implement the following method - - compute_constraint_params(): tuple - - Attributes - ---------- - constraint_type: ConstraintType - discretization_type: DiscretizationType - n_extra_vars: int - dof: int - Degree-of-freedom of the input path. - """ + """The base constraint class.""" def __repr__(self): string = self.__class__.__name__ + '(\n' string += ' Type: {:}'.format(self.constraint_type) + '\n' @@ -50,34 +35,52 @@ def __repr__(self): return string def get_dof(self): + """Return the degree of freedom of the constraint. + + TODO: It is unclear what is a dof of a constraint. Perharps remove this. + """ return self.dof def get_no_extra_vars(self): + """Return the number of extra variable required. + + TODO: This is not a property of a constraint. Rather it is specific to kinds of constraints. To be removed. + """ return self.n_extra_vars def get_constraint_type(self): + """Return the constraint type. + + TODO: Use property instead. + + """ return self.constraint_type def get_discretization_type(self): + """Return the discretization type. + + TODO: Use property instead. + + """ return self.discretization_type - def set_discretization_type(self, t): - """ Discretization type: Collocation or Interpolation. + def set_discretization_type(self, discretization_type): + """Discretization type: Collocation or Interpolation. Parameters ---------- - t: int, or DiscretizationType - If is 1, set to Interpolation. - If is 0, set to Collocation. + discretization_type: int or :class:`DiscretizationType` + Method to discretize this constraint. """ - if t == 0: + if discretization_type == 0: self.discretization_type = DiscretizationType.Collocation - elif t == 1: + elif discretization_type == 1: self.discretization_type = DiscretizationType.Interpolation - elif t == DiscretizationType.Collocation or t == DiscretizationType.Interpolation: - self.discretization_type = t + elif discretization_type == DiscretizationType.Collocation or discretization_type == DiscretizationType.Interpolation: + self.discretization_type = discretization_type else: - raise "Discretization type: {:} not implemented!".format(t) + raise "Discretization type: {:} not implemented!".format(discretization_type) - def compute_constraint_params(self, path, gridpoints, scaling): + def compute_constraint_params(self, path, gridpoints, scaling=1): + """Evaluate parameters of the constraint.""" raise NotImplementedError diff --git a/toppra/constraint/linear_constraint.py b/toppra/constraint/linear_constraint.py index 1af6b567..ba6bf434 100644 --- a/toppra/constraint/linear_constraint.py +++ b/toppra/constraint/linear_constraint.py @@ -14,7 +14,7 @@ class LinearConstraint(Constraint): .. math:: \mathbf a_i u + \mathbf b_i x + \mathbf c_i &= v, \\\\ - \mathbf F_i v &\\leq \mathbf h_i, \\\\ + \mathbf F_i v &\\leq \mathbf g_i, \\\\ x^{bound}_{i, 0} \\leq x &\\leq x^{bound}_{i, 1}, \\\\ u^{bound}_{i, 0} \\leq u &\\leq u^{bound}_{i, 1}. @@ -22,16 +22,16 @@ class LinearConstraint(Constraint): of :math:`i`, then we can consider the simpler constraint: .. math:: - \mathbf F v &\\leq \mathbf h, \\\\ + \mathbf F v &\\leq \mathbf w, \\\\ In this case, the returned value of :math:`F` by `compute_constraint_params` has shape (k, m) instead of (N, k, m), - :math:`h` shape (k) instead of (N, k) and the class attribute + :math:`w` shape (k) instead of (N, k) and the class attribute `identical` will be True. .. note:: - Derived classes of :class:`CanonicalLinearConstraint` should at + Derived classes of :class:`LinearConstraint` should at least implement the method :func:`compute_constraint_params`. @@ -48,33 +48,38 @@ def __init__(self): self.n_extra_vars = 0 self.identical = False - def compute_constraint_params(self, path, gridpoints, scaling): + def compute_constraint_params(self, path, gridpoints, scaling=1): """Compute numerical coefficients of the given constraint. Parameters ---------- - path: `Interpolator` + path: :class:`Interpolator` The geometric path. gridpoints: np.ndarray Shape (N+1,). Gridpoint use for discretizing path. scaling: float Numerical scaling. If is 1, no scaling is performed. + NOTE: This parameter is deprecated. For numerical + stability it is better to scale at the solver level, as + then one can perform solver-specific optimization, as well + as strictly more general scaling strategy. Another + advantage of scaling at the solver level is a cleaner API. Returns ------- - a: array, or None + a: np.ndarray or None Shape (N + 1, m). See notes. - b: array, or None + b: np.ndarray, or None Shape (N + 1, m). See notes. - c: array, or None + c: np.ndarray or None Shape (N + 1, m). See notes. - F: array, or None + F: np.ndarray or None Shape (N + 1, k, m). See notes. - g: array, or None + g: np.ndarray or None Shape (N + 1, k,). See notes - ubound: array, or None + ubound: np.ndarray, or None Shape (N + 1, 2). See notes. - xbound: array, or None + xbound: np.ndarray or None Shape (N + 1, 2). See notes. """ @@ -88,38 +93,40 @@ def canlinear_colloc_to_interpolate(a, b, c, F, g, xbound, ubound, gridpoints, i Parameters ---------- - a: array, or None + a: np.ndarray or None Shape (N + 1, m). See notes. - b: array, or None + b: np.ndarray or None Shape (N + 1, m). See notes. - c: array, or None + c: np.ndarray or None Shape (N + 1, m). See notes. - F: array, or None + F: np.ndarray or None Shape (N + 1, k, m). See notes. - g: array, or None + g: np.ndarray or None Shape (N + 1, k,). See notes - ubound: array, or None + ubound: np.ndarray, or None Shape (N + 1, 2). See notes. - xbound: array, or None + xbound: np.ndarray or None Shape (N + 1, 2). See notes. - gridpoints: array - (N+1,) array. The path discretization. + gridpoints: np.ndarray + Shape (N+1,). The path discretization. + identical: bool, optional + If True, matrices F and g are identical at all gridpoint. Returns ------- - a_intp: array, or None + a_intp: np.ndarray, or None Shape (N + 1, m). See notes. - b_intp: array, or None + b_intp: np.ndarray, or None Shape (N + 1, m). See notes. - c_intp: array, or None + c_intp: np.ndarray, or None Shape (N + 1, m). See notes. - F_intp: array, or None + F_intp: np.ndarray, or None Shape (N + 1, k, m). See notes. - g_intp: array, or None + g_intp: np.ndarray, or None Shape (N + 1, k,). See notes - ubound: array, or None + ubound: np.ndarray, or None Shape (N + 1, 2). See notes. - xbound: array, or None + xbound: np.ndarray, or None Shape (N + 1, 2). See notes. """ diff --git a/toppra/constraint/linear_joint_acceleration.py b/toppra/constraint/linear_joint_acceleration.py index a311a9c8..b3a2361b 100644 --- a/toppra/constraint/linear_joint_acceleration.py +++ b/toppra/constraint/linear_joint_acceleration.py @@ -1,42 +1,47 @@ +"""Joint acceleration constraint.""" +import numpy as np from .linear_constraint import LinearConstraint, canlinear_colloc_to_interpolate from ..constraint import DiscretizationType -import numpy as np class JointAccelerationConstraint(LinearConstraint): - """Joint Acceleration Constraint. + """The Joint Acceleration Constraint class. A joint acceleration constraint is given by .. math :: - \ddot{\mathbf{q}}_{min} & \leq \ddot{\mathbf q} &\leq \ddot{\mathbf{q}}_{max} \\\\ - \ddot{\mathbf{q}}_{min} & \leq \mathbf{q}'(s_i) u_i + \mathbf{q}''(s_i) x_i &\leq \ddot{\mathbf{q}}_{max} + \ddot{\mathbf{q}}_{min} & \leq \ddot{\mathbf q} + &\leq \ddot{\mathbf{q}}_{max} \\\\ + \ddot{\mathbf{q}}_{min} & \leq \mathbf{q}'(s_i) u_i + \mathbf{q}''(s_i) x_i + &\leq \ddot{\mathbf{q}}_{max} where :math:`u_i, x_i` are respectively the path acceleration and path velocity square at :math:`s_i`. For more detail see :ref:`derivationKinematics`. Rearranging the above pair of vector inequalities into the form - required by :class:`CanonicalLinearConstraint`, we have: + required by :class:`LinearConstraint`, we have: - :code:`a[i]` := :math:`\mathbf q'(s_i)` - :code:`b[i]` := :math:`\mathbf q''(s_i)` - :code:`F` := :math:`[\mathbf{I}, -\mathbf I]^T` - :code:`h` := :math:`[\ddot{\mathbf{q}}_{max}^T, -\ddot{\mathbf{q}}_{min}^T]^T` + """ - Parameters - ---------- - alim: array - Shape (dof, 2). The lower and upper acceleration bounds of the - j-th joint are alim[j, 0] and alim[j, 1] respectively. + def __init__(self, alim, discretization_scheme=DiscretizationType.Collocation): + """Initialize the joint acceleration class. - discretization_scheme: :class:`.DiscretizationType` - Can be either Collocation (0) or Interpolation - (1). Interpolation gives more accurate results with slightly - higher computational cost. + Parameters + ---------- + alim: array + Shape (dof, 2). The lower and upper acceleration bounds of the + j-th joint are alim[j, 0] and alim[j, 1] respectively. - """ - def __init__(self, alim, discretization_scheme=DiscretizationType.Collocation): + discretization_scheme: :class:`.DiscretizationType` + Can be either Collocation (0) or Interpolation + (1). Interpolation gives more accurate results with slightly + higher computational cost. + """ super(JointAccelerationConstraint, self).__init__() self.alim = np.array(alim, dtype=float) self.dof = self.alim.shape[0] @@ -48,27 +53,23 @@ def __init__(self, alim, discretization_scheme=DiscretizationType.Collocation): self.identical = True def compute_constraint_params(self, path, gridpoints, scaling): - if path.get_dof() != self.get_dof(): + if path.dof != self.dof: raise ValueError("Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( - self.get_dof(), path.get_dof() + self.dof, path.dof )) - ps = path.evald(gridpoints / scaling) / scaling - pss = path.evaldd(gridpoints / scaling) / scaling ** 2 - N = gridpoints.shape[0] - 1 - dof = path.get_dof() - I_dof = np.eye(dof) - F = np.zeros((dof * 2, dof)) - g = np.zeros(dof * 2) - ubound = np.zeros((N + 1, 2)) - g[0:dof] = self.alim[:, 1] - g[dof:] = - self.alim[:, 0] - F[0:dof, :] = I_dof - F[dof:, :] = -I_dof + ps_vec = path.evald(gridpoints / scaling) / scaling + pss_vec = path.evaldd(gridpoints / scaling) / scaling ** 2 + dof = path.dof + F_single = np.zeros((dof * 2, dof)) + g_single = np.zeros(dof * 2) + g_single[0:dof] = self.alim[:, 1] + g_single[dof:] = - self.alim[:, 0] + F_single[0:dof, :] = np.eye(dof) + F_single[dof:, :] = -np.eye(dof) if self.discretization_type == DiscretizationType.Collocation: - return ps, pss, np.zeros_like(ps), F, g, None, None + return ps_vec, pss_vec, np.zeros_like(ps_vec), F_single, g_single, None, None elif self.discretization_type == DiscretizationType.Interpolation: - return canlinear_colloc_to_interpolate(ps, pss, np.zeros_like(ps), F, g, None, None, + return canlinear_colloc_to_interpolate(ps_vec, pss_vec, np.zeros_like(ps_vec), F_single, g_single, None, None, gridpoints, identical=True) else: raise NotImplementedError("Other form of discretization not supported!") - diff --git a/toppra/constraint/linear_second_order.py b/toppra/constraint/linear_second_order.py index 18d8e02b..09327a39 100644 --- a/toppra/constraint/linear_second_order.py +++ b/toppra/constraint/linear_second_order.py @@ -1,59 +1,68 @@ +"""This module implements the general Second-Order constraints.""" +import numpy as np from .linear_constraint import LinearConstraint, canlinear_colloc_to_interpolate from .constraint import DiscretizationType -import numpy as np class SecondOrderConstraint(LinearConstraint): - """A class to represent Canonical Linear Generalized Second-order constraints. + """This class represents the linear generalized Second-order constraints. - Notes - ----- - A Second Order Constraint can be given by the following formula: + A `SecondOrderConstraint` is given by the following formula: .. math:: - A(q) \ddot q + \dot q^\\top B(q) \dot q + C(q) + sign(qdot) * D(q) = w, + A(\mathbf{q}) \ddot {\mathbf{q}} + \dot + {\mathbf{q}}^\\top B(\mathbf{q}) \dot {\mathbf{q}} + + C(\mathbf{q}) + sign(\dot {\mathbf{q}}) * D(\mathbf{q}) = w, where w is a vector that satisfies the polyhedral constraint: .. math:: - F(q) w \\leq g(q). + F(\mathbf{q}) w \\leq g(\mathbf{q}). - The functions `A, B, C, D` can represent respectively the - inertial, Corriolis, gravitational and dry friction term for robot - torque bound constraint. + The functions :math:`A, B, C, D` represent respectively the inertial, + Corriolis, gravitational and dry friction terms in a robot torque + bound constraint. - To evaluate the constraint on a geometric path `p(s)`, multiple - calls to `inv_dyn` and `const_coeff` are made as follows: + To evaluate the constraint on a geometric path :math:`\mathbf{p}(s)`: .. math:: - A(q) p'(s) \ddot s + [A(q) p''(s) + p'(s)^\\top B(q) p'(s)] \dot s^2 + C(q) + sign(p'(s)) * D(p(s)) = w, + + A(\mathbf{q}) \mathbf{p}'(s) \ddot s + [A(\mathbf{q}) \mathbf{p}''(s) + \mathbf{p}'(s)^\\top B(\mathbf{q}) + \mathbf{p}'(s)] \dot s^2 + C(\mathbf{q}) + sign(\mathbf{p}'(s)) * D(\mathbf{p}(s)) = w, \\\\ a(s) \ddot s + b(s) \dot s ^2 + c(s) = w. - To evaluate the coefficients a(s), b(s), c(s), inv_dyn is called - repeatedly with appropriate arguments. + where :math:`\mathbf{p}', \mathbf{p}''` denote respectively the + first and second derivatives of the path. + + + It is important to note that to evaluate the coefficients + :math:`a(s), b(s), c(s)`, it is not necessary to have the + functions :math:`A, B, C`. Rather, only the sum of the these 3 + functions--the inverse dynamic function--is necessary. """ - def __init__(self, inv_dyn, cnst_F, cnst_g, dof, discretization_scheme=DiscretizationType.Interpolation): - # type: ((np.array, np.array, np.array) -> np.ndarray) -> None + def __init__(self, inv_dyn, cnst_F, cnst_g, dof, friction=None, discretization_scheme=1): """Initialize the constraint. Parameters ---------- - inv_dyn: (array, array, array) -> array + inv_dyn: [np.ndarray, np.ndarray, np.ndarray] -> np.ndarray The "inverse dynamics" function that receives joint position, velocity and acceleration as inputs and ouputs the "joint torque". It is not necessary to supply each individual component functions such as gravitational, Coriolis, etc. - - cnst_F: array -> array + cnst_F: [np.ndarray] -> np.ndarray Coefficient function. See notes for more details. - cnst_g: array -> array + cnst_g: [np.ndarray] -> np.ndarray Coefficient function. See notes for more details. - dof: int, optional - Dimension of joint position vectors. Required. - + dof: int + The dimension of the joint position. + discretization_scheme: DiscretizationType + Type of discretization. + friction: [np.ndarray] -> np.ndarray + Dry friction function. """ super(SecondOrderConstraint, self).__init__() self.set_discretization_type(discretization_scheme) @@ -61,35 +70,57 @@ def __init__(self, inv_dyn, cnst_F, cnst_g, dof, discretization_scheme=Discretiz self.cnst_F = cnst_F self.cnst_g = cnst_g self.dof = dof + if friction is None: + self.friction = lambda s: np.zeros(self.dof) + else: + self.friction = friction self._format_string = " Kind: Generalized Second-order constraint\n" self._format_string = " Dimension:\n" - F_ = cnst_F(np.zeros(dof)) - self._format_string += " F in R^({:d}, {:d})\n".format(*F_.shape) + self._format_string += " F in R^({:d}, {:d})\n".format(*cnst_F(np.zeros(dof)).shape) + + @staticmethod + def joint_torque_constraint(inv_dyn, taulim, **kwargs): + """Initialize a Joint Torque constraint. + + Parameters + ---------- + inv_dyn: [np.ndarray, np.ndarray, np.ndarray] -> np.ndarray + Inverse dynamic function of the robot. + taulim: np.ndarray + Shape (N, 2). The i-th element contains the minimum and maximum joint torque limits + respectively. + + """ + dof = np.shape(taulim)[0] + F_aug = np.vstack((np.eye(dof), - np.eye(dof))) + g_aug = np.zeros(2 * dof) + g_aug[:dof] = taulim[:, 1] + g_aug[dof:] = - taulim[:, 0] + cnst_F = lambda _: F_aug + cnst_g = lambda _: g_aug + return SecondOrderConstraint(inv_dyn, cnst_F, cnst_g, dof, **kwargs) def compute_constraint_params(self, path, gridpoints, scaling): - assert path.get_dof() == self.get_dof(), ("Wrong dimension: constraint dof ({:d}) " - "not equal to path dof ({:d})".format( - self.get_dof(), path.get_dof())) - v_zero = np.zeros(path.get_dof()) - p = path.eval(gridpoints / scaling) - ps = path.evald(gridpoints / scaling) / scaling - pss = path.evaldd(gridpoints / scaling) / scaling ** 2 - - F = np.array(list(map(self.cnst_F, p))) - g = np.array(list(map(self.cnst_g, p))) - c = np.array( - [self.inv_dyn(p_, v_zero, v_zero) for p_ in p] - ) - a = np.array( - [self.inv_dyn(p_, v_zero, ps_) for p_, ps_ in zip(p, ps)] - ) - c - b = np.array( - [self.inv_dyn(p_, ps_, pss_) for p_, ps_, pss_ in zip(p, ps, pss)] - ) - c + if path.dof != self.dof: + raise ValueError("Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( + self.dof, path.dof)) + v_zero = np.zeros(path.dof) + p_vec = path.eval(gridpoints / scaling) + ps_vec = path.evald(gridpoints / scaling) / scaling + pss_vec = path.evaldd(gridpoints / scaling) / scaling ** 2 + + F_vec = np.array(list(map(self.cnst_F, p_vec))) + g_vec = np.array(list(map(self.cnst_g, p_vec))) + c_vec = np.array([self.inv_dyn(_p, v_zero, v_zero) for _p in p_vec]) + a_vec = np.array([self.inv_dyn(_p, v_zero, _ps) for _p, _ps in zip(p_vec, ps_vec)]) - c_vec + b_vec = np.array([self.inv_dyn(_p, _ps, pss_) for _p, _ps, pss_ in zip(p_vec, ps_vec, pss_vec)]) - c_vec + + for i, (_p, _ps) in enumerate(zip(p_vec, ps_vec)): + c_vec[i] = c_vec[i] + np.sign(_ps) * self.friction(_p) if self.discretization_type == DiscretizationType.Collocation: - return a, b, c, F, g, None, None + return a_vec, b_vec, c_vec, F_vec, g_vec, None, None elif self.discretization_type == DiscretizationType.Interpolation: - return canlinear_colloc_to_interpolate(a, b, c, F, g, None, None, gridpoints) + return canlinear_colloc_to_interpolate(a_vec, b_vec, c_vec, F_vec, g_vec, None, None, gridpoints) else: raise NotImplementedError("Other form of discretization not supported!") diff --git a/toppra/interpolator.py b/toppra/interpolator.py index ea946c6b..867fc70b 100644 --- a/toppra/interpolator.py +++ b/toppra/interpolator.py @@ -49,13 +49,14 @@ def _find_left_index(gridpoints, s): """ for i in range(1, len(gridpoints)): - if gridpoints[i - 1] <= s and s < gridpoints[i]: + if gridpoints[i - 1] <= s < gridpoints[i]: return i - 1 return len(gridpoints) - 2 class Interpolator(object): """Abstract class for interpolators.""" + def __init__(self): pass @@ -155,12 +156,14 @@ def compute_ros_trajectory(self): class RaveTrajectoryWrapper(Interpolator): """An interpolator that wraps OpenRAVE's :class:`GenericTrajectory`. - Only trajectories using quadratic interpolation or cubic interpolation are supported. - The trajectory is represented as a piecewise polynomial. The polynomial could be - quadratic or cubic depending the interpolation method used by the input trajectory object. - + Only trajectories using quadratic interpolation or cubic + interpolation are supported. The trajectory is represented as a + piecewise polynomial. The polynomial could be quadratic or cubic + depending the interpolation method used by the input trajectory + object. """ + def __init__(self, traj, robot): # type: (orpy.RaveTrajectory, orpy.Robot) -> None """Initialize the Trajectory Wrapper. @@ -179,9 +182,12 @@ def __init__(self, traj, robot): self._interpolation = self.spec.GetGroupFromName('joint').interpolation if self._interpolation not in ['quadratic', 'cubic']: - raise ValueError("This class only handles trajectories with quadratic or cubic interpolation") + raise ValueError( + "This class only handles trajectories with quadratic or cubic interpolation" + ) self._duration = traj.GetDuration() - all_waypoints = traj.GetWaypoints(0, traj.GetNumWaypoints()).reshape(traj.GetNumWaypoints(), -1) + all_waypoints = traj.GetWaypoints(0, traj.GetNumWaypoints()).reshape( + traj.GetNumWaypoints(), -1) valid_wp_indices = [0] self.ss_waypoints = [0.0] for i in range(1, traj.GetNumWaypoints()): @@ -195,12 +201,16 @@ def __init__(self, traj, robot): self.s_start = self.ss_waypoints[0] self.s_end = self.ss_waypoints[-1] - self.waypoints = np.array( - [self.spec.ExtractJointValues(all_waypoints[i], robot, robot.GetActiveDOFIndices()) - for i in valid_wp_indices]) - self.waypoints_d = np.array( - [self.spec.ExtractJointValues(all_waypoints[i], robot, robot.GetActiveDOFIndices(), 1) - for i in valid_wp_indices]) + self.waypoints = np.array([ + self.spec.ExtractJointValues(all_waypoints[i], robot, + robot.GetActiveDOFIndices()) + for i in valid_wp_indices + ]) + self.waypoints_d = np.array([ + self.spec.ExtractJointValues(all_waypoints[i], robot, + robot.GetActiveDOFIndices(), 1) + for i in valid_wp_indices + ]) # Degenerate case: there is only one waypoint. if self.n_waypoints == 1: @@ -213,8 +223,8 @@ def __init__(self, traj, robot): elif self._interpolation == "quadratic": self.waypoints_dd = [] for i in range(self.n_waypoints - 1): - qdd = ((self.waypoints_d[i + 1] - self.waypoints_d[i]) - / (self.ss_waypoints[i + 1] - self.ss_waypoints[i])) + qdd = ((self.waypoints_d[i + 1] - self.waypoints_d[i]) / + (self.ss_waypoints[i + 1] - self.ss_waypoints[i])) self.waypoints_dd.append(qdd) self.waypoints_dd = np.array(self.waypoints_dd) @@ -222,19 +232,23 @@ def __init__(self, traj, robot): pp_coeffs = np.zeros((3, self.n_waypoints - 1, self.dof)) for idof in range(self.dof): for iseg in range(self.n_waypoints - 1): - pp_coeffs[:, iseg, idof] = [self.waypoints_dd[iseg, idof] / 2, - self.waypoints_d[iseg, idof], - self.waypoints[iseg, idof]] + pp_coeffs[:, iseg, idof] = [ + self.waypoints_dd[iseg, idof] / 2, + self.waypoints_d[iseg, idof], + self.waypoints[iseg, idof] + ] self.ppoly = PPoly(pp_coeffs, self.ss_waypoints) elif self._interpolation == "cubic": - self.waypoints_dd = np.array( - [self.spec.ExtractJointValues(all_waypoints[i], robot, robot.GetActiveDOFIndices(), 2) - for i in valid_wp_indices]) + self.waypoints_dd = np.array([ + self.spec.ExtractJointValues(all_waypoints[i], robot, + robot.GetActiveDOFIndices(), 2) + for i in valid_wp_indices + ]) self.waypoints_ddd = [] for i in range(self.n_waypoints - 1): - qddd = ((self.waypoints_dd[i + 1] - self.waypoints_dd[i]) - / (self.ss_waypoints[i + 1] - self.ss_waypoints[i])) + qddd = ((self.waypoints_dd[i + 1] - self.waypoints_dd[i]) / + (self.ss_waypoints[i + 1] - self.ss_waypoints[i])) self.waypoints_ddd.append(qddd) self.waypoints_ddd = np.array(self.waypoints_ddd) @@ -242,21 +256,26 @@ def __init__(self, traj, robot): pp_coeffs = np.zeros((4, self.n_waypoints - 1, self.dof)) for idof in range(self.dof): for iseg in range(self.n_waypoints - 1): - pp_coeffs[:, iseg, idof] = [self.waypoints_ddd[iseg, idof] / 6, - self.waypoints_dd[iseg, idof] / 2, - self.waypoints_d[iseg, idof], - self.waypoints[iseg, idof]] + pp_coeffs[:, iseg, idof] = [ + self.waypoints_ddd[iseg, idof] / 6, + self.waypoints_dd[iseg, idof] / 2, + self.waypoints_d[iseg, idof], + self.waypoints[iseg, idof] + ] self.ppoly = PPoly(pp_coeffs, self.ss_waypoints) self.ppoly_d = self.ppoly.derivative() self.ppoly_dd = self.ppoly.derivative(2) def get_duration(self): - warnings.warn("`get_duration` method is deprecated, use `duration` property instead", PendingDeprecationWarning) + warnings.warn( + "`get_duration` method is deprecated, use `duration` property instead", + PendingDeprecationWarning) return self.duration def get_dof(self): # type: () -> int - warnings.warn("This method is deprecated, use the property instead", PendingDeprecationWarning) + warnings.warn("This method is deprecated, use the property instead", + PendingDeprecationWarning) return self.dof @property @@ -305,6 +324,7 @@ class SplineInterpolator(Interpolator): The path 2nd derivative. """ + def __init__(self, ss_waypoints, waypoints, bc_type='clamped'): super(SplineInterpolator, self).__init__() assert ss_waypoints[0] == 0, "First index must equals zero." @@ -317,6 +337,7 @@ def __init__(self, ss_waypoints, waypoints, bc_type='clamped'): self.s_end = self.ss_waypoints[-1] if len(ss_waypoints) == 1: + def _1dof_cspl(s): try: ret = np.zeros((len(s), self.dof)) @@ -345,7 +366,9 @@ def get_waypoints(self): return self.ss_waypoints, self.waypoints def get_duration(self): - warnings.warn("use duration property instead", PendingDeprecationWarning) + warnings.warn( + "get_duration is deprecated, use duration (property) instead", + PendingDeprecationWarning) return self.duration @property @@ -359,7 +382,8 @@ def dof(self): return self.waypoints[0].shape[0] def get_dof(self): # type: () -> int - warnings.warn("This method is deprecated, use the property instead", PendingDeprecationWarning) + warnings.warn("get_dof is deprecated, use dof (property) instead", + PendingDeprecationWarning) return self.dof def eval(self, ss_sam): @@ -398,7 +422,8 @@ def compute_rave_trajectory(self, robot): q = self.eval(0) qd = self.evald(0) qdd = self.evaldd(0) - traj.Insert(traj.GetNumWaypoints(), list(q) + list(qd) + list(qdd) + [0]) + traj.Insert(traj.GetNumWaypoints(), + list(q) + list(qd) + list(qdd) + [0]) else: qs = self.eval(self.ss_waypoints) qds = self.evald(self.ss_waypoints) @@ -422,6 +447,7 @@ class UnivariateSplineInterpolator(Interpolator): waypoints: ndarray The waypoints. """ + def __init__(self, ss_waypoints, waypoints): super(UnivariateSplineInterpolator, self).__init__() assert ss_waypoints[0] == 0, "First index must equals zero." @@ -438,7 +464,8 @@ def __init__(self, ss_waypoints, waypoints): self.uspl = [] for i in range(self.dof): - self.uspl.append(UnivariateSpline(self.ss_waypoints, self.waypoints[:, i])) + self.uspl.append( + UnivariateSpline(self.ss_waypoints, self.waypoints[:, i])) self.uspld = [spl.derivative() for spl in self.uspl] self.uspldd = [spl.derivative() for spl in self.uspld] @@ -479,6 +506,7 @@ class PolynomialPath(Interpolator): coeff[i, 0] + coeff[i, 1] s + coeff[i, 2] s^2 + ... """ + def __init__(self, coeff, s_start=0.0, s_end=1.0): # type: (np.ndarray, float, float) -> None """Initialize the polynomial path. @@ -502,7 +530,8 @@ def __init__(self, coeff, s_start=0.0, s_end=1.0): else: self.poly = [ np.polynomial.Polynomial(self.coeff[i]) - for i in range(self.dof)] + for i in range(self.dof) + ] self.polyd = [poly.deriv() for poly in self.poly] self.polydd = [poly.deriv() for poly in self.polyd] @@ -516,11 +545,13 @@ def duration(self): return self.s_end - self.s_start def get_duration(self): - warnings.warn("", PendingDeprecationWarning) + warnings.warn("get_duration is deprecated, use duration", + PendingDeprecationWarning) return self.duration def get_dof(self): - warnings.warn("", PendingDeprecationWarning) + warnings.warn("get_dof is deprecated, use dof", + PendingDeprecationWarning) return self.dof def eval(self, ss_sam): diff --git a/toppra/planning_utils.py b/toppra/planning_utils.py index b1ce364b..5c11b431 100644 --- a/toppra/planning_utils.py +++ b/toppra/planning_utils.py @@ -9,8 +9,15 @@ logger = logging.getLogger(__name__) -def retime_active_joints_kinematics(traj, robot, output_interpolator=False, vmult=1.0, amult=1.0, - N=100, use_ravewrapper=False, additional_constraints=[], solver_wrapper='hotqpoases'): +def retime_active_joints_kinematics(traj, + robot, + output_interpolator=False, + vmult=1.0, + amult=1.0, + N=100, + use_ravewrapper=False, + additional_constraints=[], + solver_wrapper='hotqpoases'): """ Retime a trajectory wrt velocity and acceleration constraints. Parameters @@ -50,7 +57,8 @@ def retime_active_joints_kinematics(traj, robot, output_interpolator=False, vmul ss_waypoints = np.linspace(0, 1, len(traj)) path = SplineInterpolator(ss_waypoints, traj, bc_type='natural') elif use_ravewrapper: - logger.warning("Use RaveTrajectoryWrapper. This might not work properly!") + logger.warning( + "Use RaveTrajectoryWrapper. This might not work properly!") path = RaveTrajectoryWrapper(traj, robot) elif isinstance(traj, SplineInterpolator): path = traj @@ -69,8 +77,8 @@ def retime_active_joints_kinematics(traj, robot, output_interpolator=False, vmul else: ss_waypoints.append(ss_waypoints[-1] + dt) waypoints.append( - spec.ExtractJointValues( - data, robot, robot.GetActiveDOFIndices())) + spec.ExtractJointValues(data, robot, + robot.GetActiveDOFIndices())) path = SplineInterpolator(ss_waypoints, waypoints) vmax = robot.GetActiveDOFMaxVel() * vmult @@ -82,8 +90,7 @@ def retime_active_joints_kinematics(traj, robot, output_interpolator=False, vmul pc_acc = JointAccelerationConstraint( alim, discretization_scheme=DiscretizationType.Interpolation) logger.debug( - "Number of constraints {:d}".format( - 2 + len(additional_constraints))) + "Number of constraints %d", 2 + len(additional_constraints)) logger.debug(str(pc_vel)) logger.debug(str(pc_acc)) for _c in additional_constraints: @@ -95,18 +102,19 @@ def retime_active_joints_kinematics(traj, robot, output_interpolator=False, vmul for i in range(len(path.ss_waypoints) - 1): Ni = int( np.ceil((path.ss_waypoints[i + 1] - path.ss_waypoints[i]) / ds)) - gridpoints.extend( - path.ss_waypoints[i] - + np.linspace(0, 1, Ni + 1)[1:] * (path.ss_waypoints[i + 1] - path.ss_waypoints[i])) - instance = TOPPRA([pc_vel, pc_acc] + additional_constraints, path, - gridpoints=gridpoints, solver_wrapper=solver_wrapper) + gridpoints.extend(path.ss_waypoints[i] + + np.linspace(0, 1, Ni + 1)[1:] * + (path.ss_waypoints[i + 1] - path.ss_waypoints[i])) + instance = TOPPRA([pc_vel, pc_acc] + additional_constraints, + path, + gridpoints=gridpoints, + solver_wrapper=solver_wrapper) _t1 = time.time() traj_ra, aux_traj = instance.compute_trajectory(0, 0) _t2 = time.time() logger.debug("t_setup={:.5f}ms, t_solve={:.5f}ms, t_total={:.5f}ms".format( - (_t1 - _t0) * 1e3, (_t2 - _t1) * 1e3, (_t2 - _t0) * 1e3 - )) + (_t1 - _t0) * 1e3, (_t2 - _t1) * 1e3, (_t2 - _t0) * 1e3)) if traj_ra is None: logger.warning("Retime fails.") traj_rave = None @@ -155,15 +163,21 @@ def inv_dyn(q, qd, qdd): robot.SetDOFVelocityLimits(vlim) robot.SetDOFAccelerationLimits(alim) return res[active_dofs] + tau_max = robot.GetDOFTorqueLimits()[robot.GetActiveDOFIndices()] - F = np.vstack((np.eye(robot.GetActiveDOF()), - - np.eye(robot.GetActiveDOF()))) + F = np.vstack( + (np.eye(robot.GetActiveDOF()), -np.eye(robot.GetActiveDOF()))) g = np.hstack((tau_max, tau_max)) - def cnst_F(q): return F + def cnst_F(q): + return F - def cnst_g(q): return g + def cnst_g(q): + return g - cnst = SecondOrderConstraint(inv_dyn, cnst_F, cnst_g, dof=robot.GetActiveDOF(), + cnst = SecondOrderConstraint(inv_dyn, + cnst_F, + cnst_g, + dof=robot.GetActiveDOF(), discretization_scheme=discretization_scheme) return cnst