6 from pinocchio
import Motion, Force, skew
11 """Return the joint index from the velocity index"""
12 for j
in range(1, robot.model.njoint):
14 robot.model.joints[j].idx_v,
15 robot.model.joints[j].idx_v + robot.model.joints[j].nv,
20 parent =
lambda i, robot: robot.model.parents[i]
21 iv =
lambda i, robot: list(
23 robot.model.joints[i].idx_v,
24 robot.model.joints[i].idx_v + robot.model.joints[i].nv,
28 lambda j, robot, res=[]: res
31 robot.model.parents[j],
52 dec = self.
robot.model.parents[dec]
56 descendants =
lambda root, robot: robot.model.subtrees[root]
60 ancestors.__defaults__ = (robot,) + ancestors.__defaults__
61 descendants.__defaults__ = (robot,)
63 iv.__defaults__ = (robot,)
64 parent.__defaults__ = (robot,)
65 jFromIdx.__defaults__ = (robot,)
70 Mcross.__doc__ =
"Motion cross product"
73 Fcross.__doc__ =
"Force cross product"
75 MCross =
lambda V, v: np.bmat([
Mcross(V[:, i], v)
for i
in range(V.shape[1])])
76 FCross =
lambda V, f: np.bmat([
Fcross(V[:, i], f)
for i
in range(V.shape[1])])
80 adj.__doc__ =
"Motion pre-cross product (ie adjoint, lie bracket operator)"
82 adjdual =
lambda nu: np.bmat(
85 adjdual.__doc__ =
"Force pre-cross product adjdual(a) = -adj(a)' "
88 quad =
lambda H, v: np.array(
td(
td(H, v, [2, 0]), v, [1, 0]))
89 quad.__doc__ =
"""Tensor product v'*H*v, with H.shape = [ nop, nv, nv ]"""
94 lambda x, sarg=sarg, eps=eps: sarg.format(x)
if abs(x) > eps
else " 0. "
96 np.set_printoptions(formatter={
"float": mformat})