"""Linearize rigid-body robot dynamics from a URDF into a state-space model.
The manipulator equation
M(q) q_ddot + C(q, q_dot) q_dot + g(q) = S^T u
is linearized about an equilibrium (q_eq, q_dot = 0, u_eq = gravity
compensation), giving the LTI system
x_dot = A x + B u_delta, y = C x + D u_delta
with state x = [q - q_eq, q_dot] (tangent space) and input
u_delta = u - u_eq. The partial derivatives of the forward dynamics are
computed analytically by Pinocchio (computeABADerivatives), not by finite
differences.
"""
from dataclasses import dataclass, field
from typing import List, Mapping, Optional, Sequence, Union
import numpy as np
try:
import pinocchio as pin
except ImportError as exc: # pragma: no cover
raise ImportError(
"urdf_state_space needs the Pinocchio dynamics library. "
"Install it with 'pip install pin' (NOT 'pip install pinocchio', "
"which is an unrelated package) or 'apt install ros-$ROS_DISTRO-pinocchio'."
) from exc
[docs]
@dataclass
class StateSpaceModel:
"""LTI model of a robot linearized about an equilibrium point.
State x = [q - q_eq (tangent), q_dot], input u = tau_actuated - u_eq.
"""
A: np.ndarray
B: np.ndarray
C: np.ndarray
D: np.ndarray
q_eq: np.ndarray
u_eq: np.ndarray
joint_names: List[str]
actuated_joint_names: List[str]
output_names: List[str] = field(default_factory=list)
@property
def n_states(self) -> int:
return self.A.shape[0]
@property
def n_inputs(self) -> int:
return self.B.shape[1]
@property
def n_outputs(self) -> int:
return self.C.shape[0]
[docs]
def poles(self) -> np.ndarray:
return np.linalg.eigvals(self.A)
[docs]
def controllability_matrix(self) -> np.ndarray:
n = self.n_states
blocks = [self.B]
for _ in range(n - 1):
blocks.append(self.A @ blocks[-1])
return np.hstack(blocks)
[docs]
def observability_matrix(self) -> np.ndarray:
n = self.n_states
blocks = [self.C]
for _ in range(n - 1):
blocks.append(blocks[-1] @ self.A)
return np.vstack(blocks)
[docs]
def is_controllable(self, tol: Optional[float] = None) -> bool:
return np.linalg.matrix_rank(
self.controllability_matrix(), tol=tol) == self.n_states
[docs]
def is_observable(self, tol: Optional[float] = None) -> bool:
return np.linalg.matrix_rank(
self.observability_matrix(), tol=tol) == self.n_states
[docs]
def to_control(self):
"""Return a python-control StateSpace object (pip install control)."""
import control
sys = control.ss(self.A, self.B, self.C, self.D)
sys.set_inputs(self.actuated_joint_names)
sys.set_outputs(self.output_names)
return sys
[docs]
def c2d(self, dt: float) -> 'StateSpaceModel':
"""Discretize with zero-order hold (exact, via matrix exponential)."""
from scipy.linalg import expm
n, m = self.n_states, self.n_inputs
aug = np.zeros((n + m, n + m))
aug[:n, :n] = self.A * dt
aug[:n, n:] = self.B * dt
phi = expm(aug)
return StateSpaceModel(
A=phi[:n, :n], B=phi[:n, n:], C=self.C.copy(), D=self.D.copy(),
q_eq=self.q_eq.copy(), u_eq=self.u_eq.copy(),
joint_names=list(self.joint_names),
actuated_joint_names=list(self.actuated_joint_names),
output_names=list(self.output_names),
)
[docs]
def save_npz(self, path: str) -> None:
np.savez(
path, A=self.A, B=self.B, C=self.C, D=self.D,
q_eq=self.q_eq, u_eq=self.u_eq,
joint_names=np.array(self.joint_names),
actuated_joint_names=np.array(self.actuated_joint_names),
output_names=np.array(self.output_names),
)
[docs]
def save_mat(self, path: str) -> None:
"""Save to MATLAB .mat (usable with hinfsyn/mixsyn in MATLAB)."""
from scipy.io import savemat
savemat(path, {
'A': self.A, 'B': self.B, 'C': self.C, 'D': self.D,
'q_eq': self.q_eq, 'u_eq': self.u_eq,
'joint_names': self.joint_names,
'actuated_joint_names': self.actuated_joint_names,
'output_names': self.output_names,
})
[docs]
@classmethod
def load_npz(cls, path: str) -> 'StateSpaceModel':
d = np.load(path, allow_pickle=False)
return cls(
A=d['A'], B=d['B'], C=d['C'], D=d['D'],
q_eq=d['q_eq'], u_eq=d['u_eq'],
joint_names=[str(s) for s in d['joint_names']],
actuated_joint_names=[str(s) for s in d['actuated_joint_names']],
output_names=[str(s) for s in d['output_names']],
)
[docs]
def summary(self) -> str:
lines = [
f'states : {self.n_states} (x = [q - q_eq, q_dot])',
f'inputs : {self.n_inputs} {self.actuated_joint_names}',
f'outputs: {self.n_outputs} {self.output_names}',
f'q_eq : {np.array2string(self.q_eq, precision=4)}',
f'u_eq : {np.array2string(self.u_eq, precision=4)} (gravity comp.)',
f'poles : {np.array2string(np.sort_complex(self.poles()), precision=4)}',
f'controllable: {self.is_controllable()} '
f'observable: {self.is_observable()}',
]
return '\n'.join(lines)
def _load_model(urdf: str, floating_base: bool = False) -> 'pin.Model':
"""Build a pinocchio model from a URDF path or URDF XML string."""
root = pin.JointModelFreeFlyer() if floating_base else None
if urdf.lstrip().startswith('<'):
if root is not None:
return pin.buildModelFromXML(urdf, root)
return pin.buildModelFromXML(urdf)
if root is not None:
return pin.buildModelFromUrdf(urdf, root)
return pin.buildModelFromUrdf(urdf)
JointValues = Union[Sequence[float], Mapping[str, float]]
def _named_q(model, mapping: Mapping[str, float]) -> np.ndarray:
"""Build a full q vector from {joint_name: position}, others neutral."""
q = pin.neutral(model)
for name, val in mapping.items():
if not model.existJointName(name):
raise ValueError(f'Unknown joint {name!r} in q_eq')
j = model.joints[model.getJointId(name)]
vals = np.atleast_1d(np.asarray(val, dtype=float))
if vals.shape != (j.nq,):
raise ValueError(
f'q_eq[{name!r}] needs {j.nq} value(s), got {vals.shape[0]}')
q[j.idx_q:j.idx_q + j.nq] = vals
return q
def _named_damping(model, mapping: Mapping[str, float]) -> np.ndarray:
"""Build a full nv damping vector from {joint_name: damping}, others 0."""
d = np.zeros(model.nv)
for name, val in mapping.items():
if not model.existJointName(name):
raise ValueError(f'Unknown joint {name!r} in joint_damping')
j = model.joints[model.getJointId(name)]
d[j.idx_v:j.idx_v + j.nv] = val
return d
[docs]
def build_state_space(
urdf: str,
q_eq: Optional[JointValues] = None,
actuated_joints: Optional[Sequence[str]] = None,
output_joints: Optional[Sequence[str]] = None,
velocity_outputs: bool = False,
joint_damping: Optional[JointValues] = None,
floating_base: bool = False,
gravity_tol: float = 1e-6,
) -> StateSpaceModel:
"""Linearize the robot in ``urdf`` about (q_eq, q_dot = 0).
Parameters
----------
urdf:
Path to a URDF file, or the URDF XML itself.
q_eq:
Equilibrium configuration: either a full pinocchio q vector, or a
{joint_name: position} mapping (unnamed joints stay neutral).
Defaults to the neutral configuration.
actuated_joints:
Names of actuated joints (columns of B). Default: all movable joints.
output_joints:
Joints whose positions appear in y. Default: the actuated joints.
velocity_outputs:
Also include the velocities of ``output_joints`` in y.
joint_damping:
Viscous damping added to the model (URDF <damping> values are NOT
read by pinocchio): either a per-DoF sequence of length nv, or a
{joint_name: damping} mapping (unnamed joints get 0). N*m*s/rad.
floating_base:
Model the base as a free-flyer joint instead of fixed to the world.
gravity_tol:
Warn if unactuated joints need more torque than this to hold q_eq
(the point is then not a true equilibrium for the given actuation).
"""
model = _load_model(urdf, floating_base)
data = model.createData()
nq, nv = model.nq, model.nv
if q_eq is None:
q = pin.neutral(model)
elif isinstance(q_eq, Mapping):
q = _named_q(model, q_eq)
else:
q = np.asarray(q_eq, dtype=float)
if q.shape != (nq,):
raise ValueError(f'q_eq must have length nq={nq}, got {q.shape}')
v = np.zeros(nv)
if isinstance(joint_damping, Mapping):
joint_damping = _named_damping(model, joint_damping)
# Movable joints (skip the "universe" joint at index 0).
movable = [model.names[i] for i in range(1, model.njoints)]
if actuated_joints is None:
actuated_joints = list(movable)
idx_v = {}
for name in movable:
jid = model.getJointId(name)
idx_v[name] = (model.joints[jid].idx_v, model.joints[jid].nv)
unknown = [j for j in actuated_joints if j not in idx_v]
if unknown:
raise ValueError(f'Unknown actuated joints {unknown}; '
f'movable joints are {movable}')
# Actuation selection matrix S (nv x nu): tau_full = S @ u.
cols = []
for name in actuated_joints:
i0, jnv = idx_v[name]
cols.extend(range(i0, i0 + jnv))
nu = len(cols)
S = np.zeros((nv, nu))
for k, i in enumerate(cols):
S[i, k] = 1.0
# Equilibrium torque: full gravity/bias torque at (q, v=0, a=0).
tau_full = pin.rnea(model, data, q, v, np.zeros(nv))
residual = tau_full - S @ (S.T @ tau_full)
if np.linalg.norm(residual, np.inf) > gravity_tol:
import warnings
warnings.warn(
'q_eq is not an equilibrium for the chosen actuated joints: '
f'unactuated DoFs need torque {residual} to hold it. '
'The linearization is still computed about this point.',
stacklevel=2)
u_eq = S.T @ tau_full
# Analytic derivatives of the forward dynamics a = ABA(q, v, tau).
pin.computeABADerivatives(model, data, q, v, tau_full)
ddq_dq = np.asarray(data.ddq_dq)
ddq_dv = np.asarray(data.ddq_dv)
Minv = np.asarray(data.Minv)
if joint_damping is not None:
d = np.asarray(joint_damping, dtype=float)
if d.shape != (nv,):
raise ValueError(f'joint_damping must have length nv={nv}')
# tau -> tau - D q_dot adds -Minv @ D to d(q_ddot)/d(q_dot).
ddq_dv = ddq_dv - Minv @ np.diag(d)
A = np.block([
[np.zeros((nv, nv)), np.eye(nv)],
[ddq_dq, ddq_dv],
])
B = np.vstack([np.zeros((nv, nu)), Minv @ S])
# Output selection.
if output_joints is None:
output_joints = list(actuated_joints)
unknown = [j for j in output_joints if j not in idx_v]
if unknown:
raise ValueError(f'Unknown output joints {unknown}')
out_rows = []
output_names = []
for name in output_joints:
i0, jnv = idx_v[name]
for k in range(jnv):
out_rows.append(i0 + k)
output_names.append(f'{name}.q' if jnv == 1 else f'{name}.q[{k}]')
ny = len(out_rows)
C = np.zeros((ny, 2 * nv))
for r, i in enumerate(out_rows):
C[r, i] = 1.0
if velocity_outputs:
Cv = np.zeros((ny, 2 * nv))
for r, i in enumerate(out_rows):
Cv[r, nv + i] = 1.0
C = np.vstack([C, Cv])
output_names += [n.replace('.q', '.qd') for n in output_names[:ny]]
D = np.zeros((C.shape[0], nu))
return StateSpaceModel(
A=A, B=B, C=C, D=D, q_eq=q, u_eq=u_eq,
joint_names=movable,
actuated_joint_names=list(actuated_joints),
output_names=output_names,
)