<s> """<STR_LIT>""" <EOL> from sympy import zeros , sign <EOL> from . . utils import identity <EOL> _frictionterms = set ( [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ) <EOL> def frictionforce ( rbtdef , ifunc = None ) : <EOL> '''<STR_LIT>''' <EOL> if not ifunc : <EOL> ifunc = identity <EOL> fric = zeros ( rbtdef . dof , <NUM_LIT:1> ) <EOL> if rbtdef . frictionmodel is None or len ( rbtdef . frictionmodel ) == <NUM_LIT:0> : <EOL> pass <EOL> else : <EOL> askedterms = set ( rbtdef . frictionmodel ) <EOL> if askedterms . issubset ( _frictionterms ) : <EOL> if '<STR_LIT>' in askedterms : <EOL> for i in range ( rbtdef . dof ) : <EOL> fric [ i ] += rbtdef . fv [ i ] * rbtdef . dq [ i ] <EOL> if '<STR_LIT>' in askedterms : <EOL> for i in range ( rbtdef . dof ) : <EOL> fric [ i ] += rbtdef . fc [ i ] * sign ( rbtdef . dq [ i ] ) <EOL> if '<STR_LIT>' in askedterms : <EOL> for i in range ( rbtdef . dof ) : <EOL> fric [ i ] += rbtdef . fo [ i ] <EOL> fric [ i ] = ifunc ( fric [ i ] ) <EOL> else : <EOL> raise Exception ( <EOL> '<STR_LIT>' <EOL> '<STR_LIT>' % <EOL> ( str ( askedterms - _frictionterms ) , _frictionterms ) ) <EOL> return fric <EOL> def driveinertiaterm ( rbtdef , ifunc = None ) : <EOL> '''<STR_LIT>''' <EOL> if not ifunc : <EOL> ifunc = identity <EOL> driveinertia = zeros ( rbtdef . dof , <NUM_LIT:1> ) <EOL> if rbtdef . driveinertiamodel is None : <EOL> pass <EOL> elif rbtdef . driveinertiamodel == '<STR_LIT>' : <EOL> for i in range ( rbtdef . dof ) : <EOL> driveinertia [ i ] = rbtdef . Ia [ i ] * rbtdef . ddq [ i ] <EOL> driveinertia [ i ] = ifunc ( driveinertia [ i ] ) <EOL> else : <EOL> raise Exception ( '<STR_LIT>' <EOL> '<STR_LIT>' % rbtdef . driveinertiamodel ) <EOL> return driveinertia </s>
