Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 13 additions & 10 deletions src/roboticstoolbox/robot/Dynamics.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,8 @@
from __future__ import annotations

import warnings
from typing import Any, Callable, NamedTuple
from collections.abc import Callable
from typing import Any, NamedTuple
from typing import Literal as L

import numpy as np
Expand Down Expand Up @@ -207,10 +208,10 @@ def fdyn(
T: float,
q0: ArrayLike,
Q: Callable[[Any, float, NDArray, NDArray], NDArray] | None = None,
Q_args: dict = {},
Q_args: dict | None = None,
qd0: ArrayLike | None = None,
solver: str = "RK45",
solver_args: dict = {},
solver_args: dict | None = None,
dt: float | None = None,
progress: bool = False,
) -> Any:
Expand Down Expand Up @@ -320,9 +321,11 @@ def fdyn(
qd0 = np.zeros((n,))
else:
qd0 = getvector(qd0, n)
if Q is not None:
if not callable(Q):
raise ValueError("generalized joint torque function must be callable")
if Q is not None and not callable(Q):
raise ValueError("generalized joint torque function must be callable")
# not a default argument, which would be one dict shared by every call
Q_args = {} if Q_args is None else Q_args
solver_args = {} if solver_args is None else solver_args

# concatenate q and qd into the initial state vector
x0 = np.r_[q0, qd0]
Expand Down Expand Up @@ -613,7 +616,7 @@ def pay(

return tau

def payload(self: RobotProto, m: float, p: Any = np.zeros(3)) -> None:
def payload(self: RobotProto, m: float, p: ArrayLike | None = None) -> None:
"""
Add a payload to the end-effector

Expand Down Expand Up @@ -661,7 +664,7 @@ def payload(self: RobotProto, m: float, p: Any = np.zeros(3)) -> None:
link.I = I0.copy()
return

p = getvector(p, 3)
p = np.zeros(3) if p is None else getvector(p, 3)
mc = m0 + m
rc = (m0 * r0 + m * p) / mc

Expand Down Expand Up @@ -704,8 +707,8 @@ def jointdynamics(self: RobotProto, q: Any, qd: Any = None) -> Any:
tf = []
for j, link in enumerate(self.links):
# compute inertia for this joint
zero = np.zeros((self.n))
qdd = np.zeros((self.n))
zero = np.zeros(self.n)
qdd = np.zeros(self.n)
qdd[j] = 1
M = self.rne(q, zero, qdd, gravity=[0, 0, 0])
J = link.Jm + M[j] / abs(link.G) ** 2
Expand Down
3 changes: 2 additions & 1 deletion src/roboticstoolbox/robot/RobotProto.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
from typing import Callable, Literal
from collections.abc import Callable
from typing import Literal

from spatialmath import SE3
from typing_extensions import Protocol, Self
Expand Down
48 changes: 48 additions & 0 deletions tests/test_dynamics_defaults.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
"""
Default arguments of the dynamics methods are None, not a mutable object that is
shared by every call.
"""

import inspect

import numpy as np
import numpy.testing as nt
import pytest

import roboticstoolbox as rtb
from roboticstoolbox.robot.Dynamics import DynamicsMixin


@pytest.mark.parametrize(
"method, name",
[("fdyn", "Q_args"), ("fdyn", "solver_args"), ("payload", "p")],
)
def test_default_is_none(method, name):
default = inspect.signature(getattr(DynamicsMixin, method)).parameters[name].default
assert default is None

Check warning on line 22 in tests/test_dynamics_defaults.py

View check run for this annotation

Codacy Production / Codacy Static Code Analysis

tests/test_dynamics_defaults.py#L22

Use of assert detected. The enclosed code will be removed when compiling to optimised byte code.


def test_payload_without_a_position_is_at_the_origin():
puma = rtb.models.DH.Puma560()
q = puma.qn
puma.payload(2.0)
default = puma.gravload(q)
puma.payload(0)
puma.payload(2.0, [0, 0, 0])
nt.assert_allclose(default, puma.gravload(q))
puma.payload(0)


def test_payload_zero_mass_is_a_no_op():
puma = rtb.models.DH.Puma560()
before = puma.gravload(puma.qn)
puma.payload(0)
nt.assert_allclose(puma.gravload(puma.qn), before)


def test_fdyn_without_optional_arguments():
puma = rtb.models.DH.Puma560()
for _ in range(2):
t, q, qd = puma.fdyn(0.1, puma.qn, dt=0.05)
assert q.shape[1] == 6

Check warning on line 47 in tests/test_dynamics_defaults.py

View check run for this annotation

Codacy Production / Codacy Static Code Analysis

tests/test_dynamics_defaults.py#L47

Use of assert detected. The enclosed code will be removed when compiling to optimised byte code.
assert np.all(np.isfinite(q))

Check warning on line 48 in tests/test_dynamics_defaults.py

View check run for this annotation

Codacy Production / Codacy Static Code Analysis

tests/test_dynamics_defaults.py#L48

Use of assert detected. The enclosed code will be removed when compiling to optimised byte code.
Loading