lambdas.py
Go to the documentation of this file.
1 #
2 # Copyright (c) 2016 CNRS
3 #
4 
5 import numpy as np
6 from pinocchio import Motion, Force, skew
7 from pinocchio.utils import zero
8 
9 def jFromIdx(idxv,robot):
10  '''Return the joint index from the velocity index'''
11  for j in range(1,robot.model.njoint):
12  if idxv in range(robot.model.joints[j].idx_v,
13  robot.model.joints[j].idx_v+robot.model.joints[j].nv):
14  return j
15 
16 parent = lambda i,robot: robot.model.parents[i]
17 iv = lambda i,robot: list(range(robot.model.joints[i].idx_v,
18  robot.model.joints[i].idx_v+robot.model.joints[i].nv))
19 ancestors = lambda j,robot,res=[]: res if j==0 else ancestors(robot.model.parents[j],robot,[j,]+res)
20 
21 class ancestorOf:
22  def __init__(self,i,robot): self.dec=i; self.robot=robot
23  def __contains__(self,anc):
24  dec = self.dec
25  while(dec>0):
26  if anc==dec: return True
27  else: dec = self.robot.model.parents[dec]
28 #descendants = lambda root,robot: filter( lambda i: root in ancestorOf(i,robot), range(root,robot.model.njoints) )
29 descendants = lambda root,robot: robot.model.subtrees[root]
30 
31 
32 def setRobotArgs(robot):
33  ancestors.__defaults__ = (robot,)+ancestors.__defaults__
34  descendants.__defaults__ = (robot,)
35  #ancestorsOf.__init__.__defaults__ = (robot,)
36  iv.__defaults__ = (robot,)
37  parent.__defaults__ = (robot,)
38  jFromIdx.__defaults__ = (robot,)
39 
40 # --- SE3 operators
41 Mcross = lambda x,y: Motion(x).cross(Motion(y)).vector
42 Mcross.__doc__ = "Motion cross product"
43 
44 Fcross = lambda x,y: Motion(x).cross(Force(y)).vector
45 Fcross.__doc__ = "Force cross product"
46 
47 MCross = lambda V,v: np.bmat([ Mcross(V[:,i],v) for i in range(V.shape[1]) ])
48 FCross = lambda V,f: np.bmat([ Fcross(V[:,i],f) for i in range(V.shape[1]) ])
49 
50 
51 adj = lambda nu: np.bmat([[ skew(nu[3:]),skew(nu[:3])],[zero([3,3]),skew(nu[3:])]])
52 adj.__doc__ = "Motion pre-cross product (ie adjoint, lie bracket operator)"
53 
54 adjdual = lambda nu: np.bmat([[ skew(nu[3:]),zero([3,3])],[skew(nu[:3]),skew(nu[3:])]])
55 adjdual.__doc__ = "Force pre-cross product adjdual(a) = -adj(a)' "
56 
57 td = np.tensordot
58 quad = lambda H,v: np.array(td(td(H,v,[2,0]),v,[1,0]))
59 quad.__doc__ = '''Tensor product v'*H*v, with H.shape = [ nop, nv, nv ]'''
60 
61 def np_prettyprint(sarg = '{: 0.5f}',eps=5e-7):
62  mformat = lambda x,sarg = sarg,eps=eps: sarg.format(x) if abs(x)>eps else ' 0. '
63  np.set_printoptions(formatter={'float': mformat})
64 
def __contains__(self, anc)
Definition: lambdas.py:23
ForceTpl< double, 0 > Force
def __init__(self, i, robot)
Definition: lambdas.py:22
def np_prettyprint(sarg='{:0.5f}', eps=5e-7)
Definition: lambdas.py:61
Fcross
Definition: lambdas.py:44
MotionTpl< double, 0 > Motion
def setRobotArgs(robot)
Definition: lambdas.py:32
Mcross
Definition: lambdas.py:41
list ancestors
Definition: lambdas.py:19
def jFromIdx(idxv, robot)
Definition: lambdas.py:9


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:31