@@ -3265,6 +3265,29 @@ impl MultibodyLink {
32653265 fn joint_rot ( & self ) -> Rotation3 {
32663266 Rotation3 ( self . 0 . joint ( ) . joint_rot ( ) . into ( ) )
32673267 }
3268+ /// Index of this link's first degree of freedom in the
3269+ /// articulation-wide generalized vectors.
3270+ ///
3271+ /// The link owns the :py:attr:`ndofs` entries starting at this
3272+ /// index in :py:meth:`Multibody.generalized_position`,
3273+ /// :py:meth:`Multibody.generalized_velocity`, and friends.
3274+ #[ getter]
3275+ fn assembly_id ( & self ) -> usize {
3276+ self . 0 . assembly_id ( )
3277+ }
3278+ /// Number of degrees of freedom of this link's joint.
3279+ #[ getter]
3280+ fn ndofs ( & self ) -> usize {
3281+ self . 0 . joint ( ) . ndofs ( )
3282+ }
3283+ }
3284+
3285+ /// Indices, in the 6-entry spatial layout of
3286+ /// `MultibodyJoint::coords`, of the joint's unlocked axes, in the
3287+ /// order the solver assigns its degrees of freedom.
3288+ fn free_dof_axes ( joint : & rapier:: dynamics:: MultibodyJoint ) -> impl Iterator < Item = usize > {
3289+ let locked = joint. data . locked_axes . bits ( ) ;
3290+ ( 0 ..rapier:: math:: SPATIAL_DIM ) . filter ( move |i| locked & ( 1u8 << * i) == 0 )
32683291}
32693292
32703293// =================================================================
@@ -3330,6 +3353,27 @@ impl Multibody {
33303353 Ok ( f ( mb) )
33313354 } )
33323355 }
3356+ /// Run `f` on link `link_id`, or raise `IndexError` if there is no
3357+ /// such link.
3358+ fn with_link_mut (
3359+ & mut self ,
3360+ link_id : usize ,
3361+ f : impl FnOnce ( & mut rapier:: dynamics:: MultibodyLink ) ,
3362+ ) -> PyResult < ( ) > {
3363+ self . with_mut ( |mb| match mb. link_mut ( link_id) {
3364+ Some ( link) => {
3365+ f ( link) ;
3366+ Ok ( ( ) )
3367+ }
3368+ None => Err ( link_index_error ( link_id) ) ,
3369+ } ) ?
3370+ }
3371+ }
3372+
3373+ /// The `IndexError` raised when a link id does not name a link of the
3374+ /// articulation.
3375+ fn link_index_error ( link_id : usize ) -> PyErr {
3376+ crate :: pyo3:: exceptions:: PyIndexError :: new_err ( format ! ( "no multibody link with id {link_id}" ) )
33333377}
33343378
33353379#[ pymethods]
@@ -3395,6 +3439,119 @@ impl Multibody {
33953439 Ok ( ( ) )
33963440 } ) ?
33973441 }
3442+ /// Per-degree-of-freedom armature (reflected rotor inertia,
3443+ /// length :attr:`ndofs`).
3444+ ///
3445+ /// Armature is added straight to the generalized mass-matrix
3446+ /// diagonal, which is what makes a stiff position motor stable at
3447+ /// large gains.
3448+ fn armature ( & self ) -> PyResult < Vec < Real > > {
3449+ self . with_ref ( |mb| mb. armature ( ) . iter ( ) . copied ( ) . collect ( ) )
3450+ }
3451+ /// Set the per-DOF armature values.
3452+ ///
3453+ /// :raises ValueError: If ``values`` length differs from `ndofs`.
3454+ fn set_armature ( & mut self , values : Vec < Real > ) -> PyResult < ( ) > {
3455+ self . with_mut ( |mb| {
3456+ let a = mb. armature_mut ( ) ;
3457+ if values. len ( ) != a. len ( ) {
3458+ return Err ( PyValueError :: new_err ( format ! (
3459+ "expected {} armature values (ndofs), got {}" ,
3460+ a. len( ) ,
3461+ values. len( )
3462+ ) ) ) ;
3463+ }
3464+ for ( i, v) in values. iter ( ) . enumerate ( ) {
3465+ a[ i] = * v;
3466+ }
3467+ Ok ( ( ) )
3468+ } ) ?
3469+ }
3470+ /// Generalized position vector (one entry per DOF).
3471+ ///
3472+ /// Entry ``link.assembly_id + k`` is the ``k``-th unlocked
3473+ /// coordinate of that link's joint, so the layout matches
3474+ /// :py:meth:`generalized_velocity` and the vectors accepted by
3475+ /// :py:meth:`apply_displacements`.
3476+ fn generalized_position ( & self ) -> PyResult < Vec < Real > > {
3477+ self . with_ref ( |mb| {
3478+ let mut out = vec ! [ 0.0 ; mb. ndofs( ) ] ;
3479+ for link in mb. links ( ) {
3480+ let joint = link. joint ( ) ;
3481+ let coords = joint. coords ( ) ;
3482+ for ( k, axis) in free_dof_axes ( joint) . enumerate ( ) {
3483+ out[ link. assembly_id ( ) + k] = coords[ axis] ;
3484+ }
3485+ }
3486+ out
3487+ } )
3488+ }
3489+ /// Fully configure the motor on ``axis`` of link ``link_id``'s
3490+ /// joint (the reduced-coordinate counterpart of
3491+ /// :py:meth:`GenericJoint.set_motor`).
3492+ ///
3493+ /// :raises IndexError: If ``link_id`` is out of range.
3494+ fn set_link_motor (
3495+ & mut self ,
3496+ link_id : usize ,
3497+ axis : JointAxis ,
3498+ target_pos : Real ,
3499+ target_vel : Real ,
3500+ stiffness : Real ,
3501+ damping : Real ,
3502+ ) -> PyResult < ( ) > {
3503+ self . with_link_mut ( link_id, |link| {
3504+ link. joint
3505+ . data
3506+ . set_motor ( axis. to_rapier ( ) , target_pos, target_vel, stiffness, damping) ;
3507+ } )
3508+ }
3509+ /// Clamp the maximum force the motor on ``axis`` of link
3510+ /// ``link_id`` can apply.
3511+ ///
3512+ /// :raises IndexError: If ``link_id`` is out of range.
3513+ fn set_link_motor_max_force (
3514+ & mut self ,
3515+ link_id : usize ,
3516+ axis : JointAxis ,
3517+ max_force : Real ,
3518+ ) -> PyResult < ( ) > {
3519+ self . with_link_mut ( link_id, |link| {
3520+ link. joint
3521+ . data
3522+ . set_motor_max_force ( axis. to_rapier ( ) , max_force) ;
3523+ } )
3524+ }
3525+ /// Select the motor model on ``axis`` of link ``link_id`` (see
3526+ /// :class:`MotorModel`).
3527+ ///
3528+ /// :raises IndexError: If ``link_id`` is out of range.
3529+ fn set_link_motor_model (
3530+ & mut self ,
3531+ link_id : usize ,
3532+ axis : JointAxis ,
3533+ model : MotorModel ,
3534+ ) -> PyResult < ( ) > {
3535+ self . with_link_mut ( link_id, |link| {
3536+ link. joint
3537+ . data
3538+ . set_motor_model ( axis. to_rapier ( ) , model. to_rapier ( ) ) ;
3539+ } )
3540+ }
3541+ /// Return the motor configured on ``axis`` of link ``link_id``,
3542+ /// if any.
3543+ ///
3544+ /// :raises IndexError: If ``link_id`` is out of range.
3545+ fn link_motor ( & self , link_id : usize , axis : JointAxis ) -> PyResult < Option < JointMotor > > {
3546+ self . with_ref ( |mb| {
3547+ let link = mb. link ( link_id) . ok_or_else ( || link_index_error ( link_id) ) ?;
3548+ Ok ( link
3549+ . joint ( )
3550+ . data
3551+ . motor ( axis. to_rapier ( ) )
3552+ . map ( JointMotor :: from_rapier) )
3553+ } ) ?
3554+ }
33983555 /// Generalized velocity vector (one entry per DOF).
33993556 fn generalized_velocity ( & self ) -> PyResult < Vec < Real > > {
34003557 self . with_ref ( |mb| mb. generalized_velocity ( ) . iter ( ) . copied ( ) . collect ( ) )
0 commit comments