"""Command-line tool: linearize a URDF into state-space matrices.
Preferred: describe everything in a YAML config (joint-name keyed, no
positional pitfalls) and run
urdf2ss model.yaml
Quick one-offs can still pass a URDF and flags directly:
urdf2ss robot.urdf
urdf2ss robot.urdf --q-eq 0 0.5 0 --actuated shoulder_joint elbow_joint
urdf2ss robot.xacro -o model.npz --mat model.mat
"""
import argparse
import sys
import numpy as np
from .config import ExportSpec, _read_urdf, build_from_yaml
from .state_space import build_state_space
[docs]
def main(argv=None) -> int:
parser = argparse.ArgumentParser(
prog='urdf2ss',
description='Linearize a robot URDF about an equilibrium into a '
'state-space model (A, B, C, D).')
parser.add_argument('input',
help='YAML config file (recommended), or a URDF/xacro '
'file combined with the flags below')
flags = parser.add_argument_group('URDF-input flags (ignored with YAML)')
flags.add_argument('--q-eq', type=float, nargs='+', default=None,
help='equilibrium joint positions (default: zeros)')
flags.add_argument('--actuated', nargs='+', default=None, metavar='JOINT',
help='actuated joint names (default: all)')
flags.add_argument('--outputs', nargs='+', default=None, metavar='JOINT',
help='measured joints (default: the actuated ones)')
flags.add_argument('--velocity-outputs', action='store_true',
help='also output joint velocities')
flags.add_argument('--damping', type=float, nargs='+', default=None,
help='viscous damping, one value per DoF of the WHOLE '
'model (URDF damping tags are ignored by the '
'dynamics backend); prefer per-joint values in a '
'YAML config')
flags.add_argument('--floating-base', action='store_true',
help='model the base as a free-flyer')
parser.add_argument('-o', '--npz', default=None,
help='save model to a .npz file')
parser.add_argument('--mat', default=None,
help='save model to a MATLAB .mat file')
parser.add_argument('--dt', type=float, default=None,
help='also print/save the ZOH-discretized model')
args = parser.parse_args(argv)
if args.input.endswith(('.yaml', '.yml')):
for name in ('q_eq', 'actuated', 'outputs', 'damping'):
if getattr(args, name) is not None:
parser.error(
f'--{name.replace("_", "-")} cannot be combined with a '
'YAML config; set it in the config file instead')
model, export = build_from_yaml(args.input)
else:
model = build_state_space(
_read_urdf(args.input),
q_eq=args.q_eq,
actuated_joints=args.actuated,
output_joints=args.outputs,
velocity_outputs=args.velocity_outputs,
joint_damping=args.damping,
floating_base=args.floating_base,
)
export = ExportSpec()
# Command-line export flags override the config's export section.
if args.npz is not None:
export.npz = args.npz
if args.mat is not None:
export.mat = args.mat
if args.dt is not None:
export.dt = args.dt
if export.dt is not None:
model = model.c2d(export.dt)
print(f'--- discrete-time model (ZOH, dt={export.dt}) ---')
np.set_printoptions(precision=5, suppress=True, linewidth=120)
print(model.summary())
print('\nA =\n', model.A)
print('\nB =\n', model.B)
print('\nC =\n', model.C)
print('\nD =\n', model.D)
if export.npz:
model.save_npz(export.npz)
print(f'\nsaved {export.npz}')
if export.mat:
model.save_mat(export.mat)
print(f'saved {export.mat}')
return 0
if __name__ == '__main__':
sys.exit(main())