Skip to content

Commit 846c463

Browse files
authored
feat(python): add more control other multibodies throug the python bindings (#1025)
* feat(python): multibody state writing, MJCF names and actuator parameters * fix link to the soft-body blog-post * chore: clippy fixes
1 parent b716d37 commit 846c463

6 files changed

Lines changed: 553 additions & 27 deletions

File tree

‎bindings/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -758,6 +758,10 @@ class MultibodyLink:
758758
def coords(self) -> list[float]: ...
759759
@property
760760
def joint_rot(self) -> Rotation3: ...
761+
@property
762+
def assembly_id(self) -> int: ...
763+
@property
764+
def ndofs(self) -> int: ...
761765

762766
class Multibody:
763767
@property
@@ -772,6 +776,16 @@ class Multibody:
772776
def set_self_contacts_enabled(self, v: bool) -> None: ...
773777
def damping(self) -> list[float]: ...
774778
def set_damping(self, values: list[float]) -> None: ...
779+
def armature(self) -> list[float]: ...
780+
def set_armature(self, values: list[float]) -> None: ...
781+
def generalized_position(self) -> list[float]: ...
782+
def set_link_motor(
783+
self, link_id: int, axis: JointAxis, target_pos: float,
784+
target_vel: float, stiffness: float, damping: float,
785+
) -> None: ...
786+
def set_link_motor_max_force(self, link_id: int, axis: JointAxis, max_force: float) -> None: ...
787+
def set_link_motor_model(self, link_id: int, axis: JointAxis, model: MotorModel) -> None: ...
788+
def link_motor(self, link_id: int, axis: JointAxis) -> JointMotor | None: ...
775789
def generalized_velocity(self) -> list[float]: ...
776790
def set_generalized_velocity(self, values: list[float]) -> None: ...
777791
def apply_displacements(self, displacements: Sequence[float]) -> None: ...
@@ -1052,7 +1066,21 @@ class MjcfActuatorHandle:
10521066
@property
10531067
def name(self) -> str | None: ...
10541068
@property
1069+
def kind(self) -> str: ...
1070+
@property
1071+
def joint_name(self) -> str | None: ...
1072+
@property
10551073
def joint(self) -> ImpulseJointHandle | MultibodyJointHandle | None: ...
1074+
@property
1075+
def gear(self) -> list[float]: ...
1076+
@property
1077+
def ctrl_range(self) -> list[float] | None: ...
1078+
@property
1079+
def force_range(self) -> list[float] | None: ...
1080+
@property
1081+
def kp(self) -> float | None: ...
1082+
@property
1083+
def kv(self) -> float | None: ...
10561084

10571085
class MjcfContactHooks:
10581086
def filter_contact_pair(self, ctx: PairFilterContext) -> SolverFlags | None: ...
@@ -1068,6 +1096,14 @@ class MjcfRobotHandles:
10681096
@property
10691097
def actuators(self) -> list[MjcfActuatorHandle]: ...
10701098
@property
1099+
def body_names(self) -> list[str | None]: ...
1100+
@property
1101+
def joint_names(self) -> list[str | None]: ...
1102+
@property
1103+
def body_name_to_idx(self) -> dict[str, int]: ...
1104+
@property
1105+
def joint_name_to_idx(self) -> dict[str, int]: ...
1106+
@property
10711107
def keyframe_names(self) -> list[str | None]: ...
10721108
def apply_controls(
10731109
self,
@@ -1096,6 +1132,14 @@ class MjcfRobot:
10961132
def gravity(self) -> Vec3: ...
10971133
@property
10981134
def keyframe_names(self) -> list[str | None]: ...
1135+
@property
1136+
def body_names(self) -> list[str | None]: ...
1137+
@property
1138+
def joint_names(self) -> list[str | None]: ...
1139+
@property
1140+
def body_name_to_idx(self) -> dict[str, int]: ...
1141+
@property
1142+
def joint_name_to_idx(self) -> dict[str, int]: ...
10991143
def append_transform(self, transform: IsometryLike) -> None: ...
11001144
def insert_using_impulse_joints(
11011145
self, bodies: RigidBodySet, colliders: ColliderSet, impulse_joints: ImpulseJointSet

‎bindings/python/rapier-py-3d/src/joints.rs‎

Lines changed: 157 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -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

Comments
 (0)