diff options
Diffstat (limited to 'src/dynamics')
30 files changed, 2041 insertions, 458 deletions
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 719f3c5..426cbb1 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -129,7 +129,7 @@ impl TOIEntry { // .ok(); let res_toi = query_dispatcher - .nonlinear_time_of_impact( + .cast_shapes_nonlinear( &motion_c1, co1.shape.as_ref(), &motion_c2, @@ -143,7 +143,7 @@ impl TOIEntry { let toi = res_toi??; Some(Self::new( - toi.toi, + toi.time_of_impact, ch1, co1.parent.map(|p| p.handle), ch2, diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 13b3fde..2de58ae 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,13 +1,18 @@ use crate::math::Real; +use na::RealField; use std::num::NonZeroUsize; +// TODO: enabling the block solver in 3d introduces a lot of jitters in +// the 3D domino demo. So for now we dont enable it in 3D. +pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2"); + /// Parameters for a time-step of the physics engine. #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct IntegrationParameters { - /// The timestep length (default: `1.0 / 60.0`) + /// The timestep length (default: `1.0 / 60.0`). pub dt: Real, - /// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`) + /// Minimum timestep size when using CCD with multiple substeps (default: `1.0 / 60.0 / 100.0`). /// /// When CCD with multiple substeps is enabled, the timestep is subdivided /// into smaller pieces. This timestep subdivision won't generate timestep @@ -19,37 +24,78 @@ pub struct IntegrationParameters { /// to numerical instabilities. pub min_ccd_dt: Real, - /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) - /// will be compensated for during the velocity solve. - /// (default `0.8`). - pub erp: Real, - /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. - /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations - /// before stabilization). - /// (default `0.25`). - pub damping_ratio: Real, - - /// 0-1: multiplier for how much of the joint violation - /// will be compensated for during the velocity solve. - /// (default `1.0`). - pub joint_erp: Real, + /// > 0: the damping ratio used by the springs for contact constraint stabilization. + /// + /// Larger values make the constraints more compliant (allowing more visible + /// penetrations before stabilization). + /// (default `5.0`). + pub contact_damping_ratio: Real, + + /// > 0: the natural frequency used by the springs for contact constraint regularization. + /// + /// Increasing this value will make it so that penetrations get fixed more quickly at the + /// expense of potential jitter effects due to overshooting. In order to make the simulation + /// look stiffer, it is recommended to increase the [`Self::contact_damping_ratio`] instead of this + /// value. + /// (default: `30.0`). + pub contact_natural_frequency: Real, + + /// > 0: the natural frequency used by the springs for joint constraint regularization. + /// + /// Increasing this value will make it so that penetrations get fixed more quickly. + /// (default: `1.0e6`). + pub joint_natural_frequency: Real, /// The fraction of critical damping applied to the joint for constraints regularization. - /// (default `0.25`). + /// + /// Larger values make the constraints more compliant (allowing more joint + /// drift before stabilization). + /// (default `1.0`). pub joint_damping_ratio: Real, - /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). - pub allowed_linear_error: Real, - /// Maximum amount of penetration the solver will attempt to resolve in one timestep. - pub max_penetration_correction: Real, - /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). - pub prediction_distance: Real, + /// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the + /// initial solution (instead of 0) at the next simulation step. + /// + /// This should generally be set to 1. + /// + /// (default `1.0`). + pub warmstart_coefficient: Real, + + /// The approximate size of most dynamic objects in the scene. + /// + /// This value is used internally to estimate some length-based tolerance. In particular, the + /// values [`IntegrationParameters::allowed_linear_error`], + /// [`IntegrationParameters::max_corrective_velocity`], + /// [`IntegrationParameters::prediction_distance`], [`RigidBodyActivation::linear_threshold`] + /// are scaled by this value implicitly. + /// + /// This value can be understood as the number of units-per-meter in your physical world compared + /// to a human-sized world in meter. For example, in a 2d game, if your typical object size is 100 + /// pixels, set the [`Self::length_unit`] parameter to 100.0. The physics engine will interpret + /// it as if 100 pixels is equivalent to 1 meter in its various internal threshold. + /// (default `1.0`). + pub length_unit: Real, + + /// Amount of penetration the engine won’t attempt to correct (default: `0.001m`). + /// + /// This value is implicitly scaled by [`IntegrationParameters::length_unit`]. + pub normalized_allowed_linear_error: Real, + /// Maximum amount of penetration the solver will attempt to resolve in one timestep (default: `10.0`). + /// + /// This value is implicitly scaled by [`IntegrationParameters::length_unit`]. + pub normalized_max_corrective_velocity: Real, + /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`). + /// + /// This value is implicitly scaled by [`IntegrationParameters::length_unit`]. + pub normalized_prediction_distance: Real, /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`). pub num_solver_iterations: NonZeroUsize, - /// Number of addition friction resolution iteration run during the last solver sub-step (default: `4`). + /// Number of addition friction resolution iteration run during the last solver sub-step (default: `0`). pub num_additional_friction_iterations: usize, /// Number of internal Project Gauss Seidel (PGS) iterations run at each solver iteration (default: `1`). pub num_internal_pgs_iterations: usize, + /// The number of stabilization iterations run at each solver iterations (default: `2`). + pub num_internal_stabilization_iterations: usize, /// Minimum number of dynamic bodies in each active island (default: `128`). pub min_island_size: usize, /// Maximum number of substeps performed by the solver (default: `1`). @@ -57,51 +103,6 @@ pub struct IntegrationParameters { } impl IntegrationParameters { - /// Configures the integration parameters to match the old PGS solver - /// from Rapier version <= 0.17. - /// - /// This solver was slightly faster than the new one but resulted - /// in less stable joints and worse convergence rates. - /// - /// This should only be used for comparison purpose or if you are - /// experiencing problems with the new solver. - /// - /// NOTE: this does not affect any [`RigidBody::additional_solver_iterations`] that will - /// still create solver iterations based on the new "small-steps" PGS solver. - /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`], - /// [`Self::joint_damping_ratio`] to their former default values. - pub fn switch_to_standard_pgs_solver(&mut self) { - self.num_internal_pgs_iterations *= self.num_solver_iterations.get(); - self.num_solver_iterations = NonZeroUsize::new(1).unwrap(); - self.erp = 0.8; - self.damping_ratio = 0.25; - self.joint_erp = 1.0; - self.joint_damping_ratio = 1.0; - } - - /// Configures the integration parameters to match the new "small-steps" PGS solver - /// from Rapier version >= 0.18. - /// - /// The "small-steps" PGS solver is the default one given by [`Self::default()`] so - /// calling this function is generally not needed unless - /// [`Self::switch_to_standard_pgs_solver()`] was called. - /// - /// This solver results in more stable joints and significantly better convergence - /// rates but is slightly slower in its default settings. - /// - /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`], - /// [`Self::joint_damping_ratio`] to their default values. - pub fn switch_to_small_steps_pgs_solver(&mut self) { - self.num_solver_iterations = NonZeroUsize::new(self.num_internal_pgs_iterations).unwrap(); - self.num_internal_pgs_iterations = 1; - - let default = Self::default(); - self.erp = default.erp; - self.damping_ratio = default.damping_ratio; - self.joint_erp = default.joint_erp; - self.joint_damping_ratio = default.joint_damping_ratio; - } - /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz). /// /// This is zero if `self.dt` is zero. @@ -134,29 +135,65 @@ impl IntegrationParameters { } } - /// The ERP coefficient, multiplied by the inverse timestep length. - pub fn erp_inv_dt(&self) -> Real { - self.erp * self.inv_dt() + /// The contact’s spring angular frequency for constraints regularization. + pub fn contact_angular_frequency(&self) -> Real { + self.contact_natural_frequency * Real::two_pi() + } + + /// The [`Self::contact_erp`] coefficient, multiplied by the inverse timestep length. + pub fn contact_erp_inv_dt(&self) -> Real { + let ang_freq = self.contact_angular_frequency(); + ang_freq / (self.dt * ang_freq + 2.0 * self.contact_damping_ratio) + } + + /// The effective Error Reduction Parameter applied for calculating regularization forces + /// on contacts. + /// + /// This parameter is computed automatically from [`Self::contact_natural_frequency`], + /// [`Self::contact_damping_ratio`] and the substep length. + pub fn contact_erp(&self) -> Real { + self.dt * self.contact_erp_inv_dt() + } + + /// The joint’s spring angular frequency for constraint regularization. + pub fn joint_angular_frequency(&self) -> Real { + self.joint_natural_frequency * Real::two_pi() } - /// The joint ERP coefficient, multiplied by the inverse timestep length. + /// The [`Self::joint_erp`] coefficient, multiplied by the inverse timestep length. pub fn joint_erp_inv_dt(&self) -> Real { - self.joint_erp * self.inv_dt() + let ang_freq = self.joint_angular_frequency(); + ang_freq / (self.dt * ang_freq + 2.0 * self.joint_damping_ratio) } - /// The CFM factor to be used in the constraints resolution. - pub fn cfm_factor(&self) -> Real { + /// The effective Error Reduction Parameter applied for calculating regularization forces + /// on joints. + /// + /// This parameter is computed automatically from [`Self::joint_natural_frequency`], + /// [`Self::joint_damping_ratio`] and the substep length. + pub fn joint_erp(&self) -> Real { + self.dt * self.joint_erp_inv_dt() + } + + /// The CFM factor to be used in the constraint resolution. + /// + /// This parameter is computed automatically from [`Self::contact_natural_frequency`], + /// [`Self::contact_damping_ratio`] and the substep length. + pub fn contact_cfm_factor(&self) -> Real { // Compute CFM assuming a critically damped spring multiplied by the damping ratio. - let inv_erp_minus_one = 1.0 / self.erp - 1.0; + let inv_erp_minus_one = 1.0 / self.contact_erp() - 1.0; // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass // / (dt * dt * inv_erp_minus_one * inv_erp_minus_one); // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass // / (dt * inv_erp_minus_one); // let cfm = 1.0 / (dt * dt * stiffness + dt * damping); - // NOTE: This simplies to cfm = cfm_coefff / projected_mass: + // NOTE: This simplifies to cfm = cfm_coeff / projected_mass: let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one - / ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio); + / ((1.0 + inv_erp_minus_one) + * 4.0 + * self.contact_damping_ratio + * self.contact_damping_ratio); // Furthermore, we use this coefficient inside of the impulse resolution. // Surprisingly, several simplifications happen there. @@ -173,36 +210,65 @@ impl IntegrationParameters { // new_impulse = cfm_factor * (old_impulse - m * delta_vel) // // The value returned by this function is this cfm_factor that can be used directly - // in the constraints solver. + // in the constraint solver. 1.0 / (1.0 + cfm_coeff) } - /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization + /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization. + /// + /// This parameter is computed automatically from [`Self::joint_natural_frequency`], + /// [`Self::joint_damping_ratio`] and the substep length. pub fn joint_cfm_coeff(&self) -> Real { // Compute CFM assuming a critically damped spring multiplied by the damping ratio. - let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0; + // The logic is similar to `Self::cfm_factor`. + let inv_erp_minus_one = 1.0 / self.joint_erp() - 1.0; inv_erp_minus_one * inv_erp_minus_one / ((1.0 + inv_erp_minus_one) * 4.0 * self.joint_damping_ratio * self.joint_damping_ratio) } -} -impl Default for IntegrationParameters { - fn default() -> Self { + /// Amount of penetration the engine won’t attempt to correct (default: `0.001` multiplied by + /// [`Self::length_unit`]). + pub fn allowed_linear_error(&self) -> Real { + self.normalized_allowed_linear_error * self.length_unit + } + + /// Maximum amount of penetration the solver will attempt to resolve in one timestep. + /// + /// This is equal to [`Self::normalized_max_corrective_velocity`] multiplied by + /// [`Self::length_unit`]. + pub fn max_corrective_velocity(&self) -> Real { + if self.normalized_max_corrective_velocity != Real::MAX { + self.normalized_max_corrective_velocity * self.length_unit + } else { + Real::MAX + } + } + + /// The maximal distance separating two objects that will generate predictive contacts + /// (default: `0.002m` multiped by [`Self::length_unit`]). + pub fn prediction_distance(&self) -> Real { + self.normalized_prediction_distance * self.length_unit + } + + /// Initialize the simulation parameters with settings matching the TGS-soft solver + /// with warmstarting. + /// + /// This is the default configuration, equivalent to [`IntegrationParameters::default()`]. + pub fn tgs_soft() -> Self { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - erp: 0.6, - damping_ratio: 1.0, - joint_erp: 1.0, + contact_natural_frequency: 30.0, + contact_damping_ratio: 5.0, + joint_natural_frequency: 1.0e6, joint_damping_ratio: 1.0, - allowed_linear_error: 0.001, - max_penetration_correction: Real::MAX, - prediction_distance: 0.002, + warmstart_coefficient: 1.0, num_internal_pgs_iterations: 1, - num_additional_friction_iterations: 4, + num_internal_stabilization_iterations: 2, + num_additional_friction_iterations: 0, num_solver_iterations: NonZeroUsize::new(4).unwrap(), // TODO: what is the optimal value for min_island_size? // It should not be too big so that we don't end up with @@ -210,7 +276,42 @@ impl Default for IntegrationParameters { // However we don't want it to be too small and end up with // tons of islands, reducing SIMD parallelism opportunities. min_island_size: 128, + normalized_allowed_linear_error: 0.001, + normalized_max_corrective_velocity: 10.0, + normalized_prediction_distance: 0.002, max_ccd_substeps: 1, + length_unit: 1.0, + } + } + + /// Initialize the simulation parameters with settings matching the TGS-soft solver + /// **without** warmstarting. + /// + /// The [`IntegrationParameters::tgs_soft()`] configuration should be preferred unless + /// warmstarting proves to be undesirable for your use-case. + pub fn tgs_soft_without_warmstart() -> Self { + Self { + contact_damping_ratio: 0.25, + warmstart_coefficient: 0.0, + num_additional_friction_iterations: 4, + ..Self::tgs_soft() } } + + /// Initializes the integration parameters to match the legacy PGS solver from Rapier version <= 0.17. + /// + /// This exists mainly for testing and comparison purpose. + pub fn pgs_legacy() -> Self { + Self { + num_solver_iterations: NonZeroUsize::new(1).unwrap(), + num_internal_pgs_iterations: 4, + ..Self::tgs_soft_without_warmstart() + } + } +} + +impl Default for IntegrationParameters { + fn default() -> Self { + Self::tgs_soft() + } } diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 8f18941..a576946 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -150,6 +150,7 @@ impl IslandManager { pub(crate) fn update_active_set_with_contacts( &mut self, dt: Real, + length_unit: Real, bodies: &mut RigidBodySet, colliders: &ColliderSet, narrow_phase: &NarrowPhase, @@ -181,7 +182,7 @@ impl IslandManager { let sq_linvel = rb.vels.linvel.norm_squared(); let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); - update_energy(&mut rb.activation, sq_linvel, sq_angvel, dt); + update_energy(length_unit, &mut rb.activation, sq_linvel, sq_angvel, dt); if rb.activation.time_since_can_sleep >= rb.activation.time_until_sleep { // Mark them as sleeping for now. This will @@ -324,8 +325,15 @@ impl IslandManager { } } -fn update_energy(activation: &mut RigidBodyActivation, sq_linvel: Real, sq_angvel: Real, dt: Real) { - if sq_linvel < activation.linear_threshold * activation.linear_threshold.abs() +fn update_energy( + length_unit: Real, + activation: &mut RigidBodyActivation, + sq_linvel: Real, + sq_angvel: Real, + dt: Real, +) { + let linear_threshold = activation.normalized_linear_threshold * length_unit; + if sq_linvel < linear_threshold * linear_threshold.abs() && sq_angvel < activation.angular_threshold * activation.angular_threshold.abs() { activation.time_since_can_sleep += dt; diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 76a7fe1..8c8a4aa 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -496,6 +496,24 @@ impl GenericJoint { self.motors[i].damping = damping; self } + + /// Flips the orientation of the joint, including limits and motors. + pub fn flip(&mut self) { + std::mem::swap(&mut self.local_frame1, &mut self.local_frame2); + + let coupled_bits = self.coupled_axes.bits(); + + for dim in 0..SPATIAL_DIM { + if coupled_bits & (1 << dim) == 0 { + let limit = self.limits[dim]; + self.limits[dim].min = -limit.max; + self.limits[dim].max = -limit.min; + } + + self.motors[dim].target_vel = -self.motors[dim].target_vel; + self.motors[dim].target_pos = -self.motors[dim].target_pos; + } + } } macro_rules! joint_conversion_methods( diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index c789004..dc1fa75 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -1,6 +1,7 @@ //! MultibodyJoints using the reduced-coordinates formalism or using constraints. pub use self::multibody::Multibody; +pub use self::multibody_ik::InverseKinematicsOption; pub use self::multibody_joint::MultibodyJoint; pub use self::multibody_joint_set::{ MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId, @@ -13,5 +14,6 @@ mod multibody_joint_set; mod multibody_link; mod multibody_workspace; +mod multibody_ik; mod multibody_joint; mod unit_multibody_joint; diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 617d447..fb56087 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -89,6 +89,7 @@ impl Default for Multibody { Multibody::new() } } + impl Multibody { /// Creates a new multibody with no link. pub fn new() -> Self { @@ -115,6 +116,8 @@ impl Multibody { pub(crate) fn with_root(handle: RigidBodyHandle) -> Self { let mut mb = Multibody::new(); + // NOTE: we have no way of knowing if the root in fixed at this point, so + // we mark it as dynamic and will fixe later with `Self::update_root_type`. mb.root_is_dynamic = true; let joint = MultibodyJoint::free(Isometry::identity()); mb.add_link(None, joint, handle); @@ -747,6 +750,12 @@ impl Multibody { self.velocities.rows(0, self.ndofs) } + /// The body jacobian for link `link_id` calculated by the last call to [`Multibody::forward_kinematics`]. + #[inline] + pub fn body_jacobian(&self, link_id: usize) -> &Jacobian<Real> { + &self.body_jacobians[link_id] + } + /// The mutable generalized velocities of this multibodies. #[inline] pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<Real> { @@ -762,17 +771,27 @@ impl Multibody { } /// Apply displacements, in generalized coordinates, to this multibody. + /// + /// Note this does **not** updates the link poses, only their generalized coordinates. + /// To update the link poses and associated rigid-bodies, call [`Self::forward_kinematics`] + /// or [`Self::finalize`]. pub fn apply_displacements(&mut self, disp: &[Real]) { for link in self.links.iter_mut() { link.joint.apply_displacement(&disp[link.assembly_id..]) } } - pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) { + pub(crate) fn update_root_type(&mut self, bodies: &RigidBodySet, take_body_pose: bool) { if let Some(rb) = bodies.get(self.links[0].rigid_body) { if rb.is_dynamic() != self.root_is_dynamic { + let root_pose = if take_body_pose { + *rb.position() + } else { + self.links[0].local_to_world + }; + if rb.is_dynamic() { - let free_joint = MultibodyJoint::free(*rb.position()); + let free_joint = MultibodyJoint::free(root_pose); let prev_root_ndofs = self.links[0].joint().ndofs(); self.links[0].joint = free_joint; self.links[0].assembly_id = 0; @@ -791,7 +810,7 @@ impl Multibody { assert!(self.damping.len() >= SPATIAL_DIM); assert!(self.accelerations.len() >= SPATIAL_DIM); - let fixed_joint = MultibodyJoint::fixed(*rb.position()); + let fixed_joint = MultibodyJoint::fixed(root_pose); let prev_root_ndofs = self.links[0].joint().ndofs(); self.links[0].joint = fixed_joint; self.links[0].assembly_id = 0; @@ -820,30 +839,86 @@ impl Multibody { } // Make sure the positions are properly set to match the rigid-body’s. - if self.links[0].joint.data.locked_axes.is_empty() { - self.links[0].joint.set_free_pos(*rb.position()); + if take_body_pose { + if self.links[0].joint.data.locked_axes.is_empty() { + self.links[0].joint.set_free_pos(*rb.position()); + } else { + self.links[0].joint.data.local_frame1 = *rb.position(); + } + } + } + } + + /// Update the rigid-body poses based on this multibody joint poses. + /// + /// This is typically called after [`Self::forward_kinematics`] to apply the new joint poses + /// to the rigid-bodies. + pub fn update_rigid_bodies(&self, bodies: &mut RigidBodySet, update_mass_properties: bool) { + self.update_rigid_bodies_internal(bodies, update_mass_properties, false, true) + } + + pub(crate) fn update_rigid_bodies_internal( + &self, + bodies: &mut RigidBodySet, + update_mass_properties: bool, + update_next_positions_only: bool, + change_tracking: bool, + ) { + // Handle the children. They all have a parent within this multibody. + for link in self.links.iter() { + let rb = if change_tracking { + bodies.get_mut_internal_with_modification_tracking(link.rigid_body) } else { - self.links[0].joint.data.local_frame1 = *rb.position(); + bodies.get_mut_internal(link.rigid_body) + }; + + if let Some(rb) = rb { + rb.pos.next_position = link.local_to_world; + + if !update_next_positions_only { + rb.pos.position = link.local_to_world; + } + + if update_mass_properties { + rb.mprops.update_world_mass_properties(&link.local_to_world); + } } } } // TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians // (i.e. just something used by the velocity solver’s small steps). - /// Apply forward-kinematics to this multibody and its related rigid-bodies. - pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) { + /// Apply forward-kinematics to this multibody. + /// + /// This will update the [`MultibodyLink`] pose information as wall as the body jacobians. + /// This will also ensure that the multibody has the proper number of degrees of freedom if + /// its root node changed between dynamic and non-dynamic. + /// + /// Note that this does **not** update the poses of the [`RigidBody`] attached to the joints. + /// Run [`Self::update_rigid_bodies`] to trigger that update. + /// + /// This method updates `self` with the result of the forward-kinematics operation. + /// For a non-mutable version running forward kinematics on a single link, see + /// [`Self::forward_kinematics_single_link`]. + /// + /// ## Parameters + /// - `bodies`: the set of rigid-bodies. + /// - `read_root_pose_from_rigid_body`: if set to `true`, the root joint (either a fixed joint, + /// or a free joint) will have its pose set to its associated-rigid-body pose. Set this to `true` + /// when the root rigid-body pose has been modified and needs to affect the multibody. + pub fn forward_kinematics( + &mut self, + bodies: &RigidBodySet, + read_root_pose_from_rigid_body: bool, + ) { + // Be sure the degrees of freedom match and take the root position if needed. + self.update_root_type(bodies, read_root_pose_from_rigid_body); + // Special case for the root, which has no parent. { let link = &mut self.links[0]; link.local_to_parent = link.joint.body_to_parent(); link.local_to_world = link.local_to_parent; - - if let Some(rb) = bodies.get_mut_internal(link.rigid_body) { - rb.pos.next_position = link.local_to_world; - if update_rb_mass_props { - rb.mprops.update_world_mass_properties(&link.local_to_world); - } - } } // Handle the children. They all have a parent within this multibody. @@ -865,20 +940,11 @@ impl Multibody { link.shift23 = c3 - c2; } - let link_rb = bodies.index_mut_internal(link.rigid_body); - link_rb.pos.next_position = link.local_to_world; - assert_eq!( - link_rb.body_type, + bodies[link.rigid_body].body_type, RigidBodyType::Dynamic, "A rigid-body that is not at the root of a multibody must b |