nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implement multibody joints and the new solver

Browse Source
This commit is contained in:
Sébastien Crozet
2022-01-02 14:47:40 +01:00
parent b45d4b5ac2
commit f74b8401ad
182 changed files with 9871 additions and 12645 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
src/dynamics/joint/ball_joint.rs
View File

@@ -1,148 +0,0 @@
use crate::dynamics::SpringModel;
use crate::math::{Point, Real, Rotation, UnitVector, Vector};
#[derive(Copy, Clone, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A joint that removes all relative linear motion between a pair of points on two bodies.
pub struct BallJoint {
/// Where the ball joint is attached on the first body, expressed in the first body local frame.
pub local_anchor1: Point<Real>,
/// Where the ball joint is attached on the second body, expressed in the second body local frame.
pub local_anchor2: Point<Real>,
/// The impulse applied by this joint on the first body.
///
/// The impulse applied to the second body is given by `-impulse`.
pub impulse: Vector<Real>,
/// The target relative angular velocity the motor will attempt to reach.
#[cfg(feature = "dim2")]
pub motor_target_vel: Real,
/// The target relative angular velocity the motor will attempt to reach.
#[cfg(feature = "dim3")]
pub motor_target_vel: Vector<Real>,
/// The target angular position of this joint, expressed as an axis-angle.
pub motor_target_pos: Rotation<Real>,
/// The motor's stiffness.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_stiffness: Real,
/// The motor's damping.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_damping: Real,
/// The maximal impulse the motor is able to deliver.
pub motor_max_impulse: Real,
/// The angular impulse applied by the motor.
#[cfg(feature = "dim2")]
pub motor_impulse: Real,
/// The angular impulse applied by the motor.
#[cfg(feature = "dim3")]
pub motor_impulse: Vector<Real>,
/// The spring-like model used by the motor to reach the target velocity and .
pub motor_model: SpringModel,
/// Are the limits enabled for this joint?
pub limits_enabled: bool,
/// The axis of the limit cone for this joint, if the local-space of the first body.
pub limits_local_axis1: UnitVector<Real>,
/// The axis of the limit cone for this joint, if the local-space of the first body.
pub limits_local_axis2: UnitVector<Real>,
/// The maximum angle allowed between the two limit axes in world-space.
pub limits_angle: Real,
/// The impulse applied to enforce joint limits.
pub limits_impulse: Real,
}
impl BallJoint {
/// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies.
pub fn new(local_anchor1: Point<Real>, local_anchor2: Point<Real>) -> Self {
Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
}
pub(crate) fn with_impulse(
local_anchor1: Point<Real>,
local_anchor2: Point<Real>,
impulse: Vector<Real>,
) -> Self {
Self {
local_anchor1,
local_anchor2,
impulse,
motor_target_vel: na::zero(),
motor_target_pos: Rotation::identity(),
motor_stiffness: 0.0,
motor_damping: 0.0,
motor_impulse: na::zero(),
motor_max_impulse: Real::MAX,
motor_model: SpringModel::default(),
limits_enabled: false,
limits_local_axis1: Vector::x_axis(),
limits_local_axis2: Vector::x_axis(),
limits_angle: Real::MAX,
limits_impulse: 0.0,
}
}
/// Can a SIMD constraint be used for resolving this joint?
pub fn supports_simd_constraints(&self) -> bool {
// SIMD ball constraints don't support motors and limits right now.
!self.limits_enabled
&& (self.motor_max_impulse == 0.0
|| (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn configure_motor_model(&mut self, model: SpringModel) {
self.motor_model = model;
}
/// Sets the target velocity and velocity correction factor this motor.
#[cfg(feature = "dim2")]
pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
}
/// Sets the target velocity and velocity correction factor this motor.
#[cfg(feature = "dim3")]
pub fn configure_motor_velocity(&mut self, target_vel: Vector<Real>, factor: Real) {
self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
}
/// Sets the target orientation this motor needs to reach.
pub fn configure_motor_position(
&mut self,
target_pos: Rotation<Real>,
stiffness: Real,
damping: Real,
) {
self.configure_motor(target_pos, na::zero(), stiffness, damping)
}
/// Sets the target orientation this motor needs to reach.
#[cfg(feature = "dim2")]
pub fn configure_motor(
&mut self,
target_pos: Rotation<Real>,
target_vel: Real,
stiffness: Real,
damping: Real,
) {
self.motor_target_vel = target_vel;
self.motor_target_pos = target_pos;
self.motor_stiffness = stiffness;
self.motor_damping = damping;
}
/// Configure both the target orientation and target velocity of the motor.
#[cfg(feature = "dim3")]
pub fn configure_motor(
&mut self,
target_pos: Rotation<Real>,
target_vel: Vector<Real>,
stiffness: Real,
damping: Real,
) {
self.motor_target_vel = target_vel;
self.motor_target_pos = target_pos;
self.motor_stiffness = stiffness;
self.motor_damping = damping;
}
}

73
src/dynamics/joint/fixed_joint.rs
View File

@@ -1,38 +1,55 @@
use crate::math::{Isometry, Real, SpacialVector};
use crate::dynamics::{JointAxesMask, JointData};
use crate::math::{Isometry, Point, Real};
#[derive(Copy, Clone, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A joint that prevents all relative movement between two bodies.
///
/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct FixedJoint {
/// The frame of reference for the first body affected by this joint, expressed in the local frame
/// of the first body.
pub local_frame1: Isometry<Real>,
/// The frame of reference for the second body affected by this joint, expressed in the local frame
/// of the first body.
pub local_frame2: Isometry<Real>,
/// The impulse applied to the first body affected by this joint.
///
/// The impulse applied to the second body affected by this joint is given by `-impulse`.
/// This combines both linear and angular impulses:
/// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
/// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
pub impulse: SpacialVector<Real>,
data: JointData,
}
impl FixedJoint {
/// Creates a new fixed joint from the frames of reference of both bodies.
pub fn new(local_frame1: Isometry<Real>, local_frame2: Isometry<Real>) -> Self {
Self {
local_frame1,
local_frame2,
impulse: SpacialVector::zeros(),
}
pub fn new() -> Self {
#[cfg(feature = "dim2")]
let mask = JointAxesMask::X | JointAxesMask::Y | JointAxesMask::ANG_X;
#[cfg(feature = "dim3")]
let mask = JointAxesMask::X
| JointAxesMask::Y
| JointAxesMask::Z
| JointAxesMask::ANG_X
| JointAxesMask::ANG_Y
| JointAxesMask::ANG_Z;
let data = JointData::default().lock_axes(mask);
Self { data }
}
/// Can a SIMD constraint be used for resolving this joint?
pub fn supports_simd_constraints(&self) -> bool {
true
#[must_use]
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
self.data = self.data.local_frame1(local_frame);
self
}
#[must_use]
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
self.data = self.data.local_frame2(local_frame);
self
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2);
self
}
}
impl Into<JointData> for FixedJoint {
fn into(self) -> JointData {
self.data
}
}

144
src/dynamics/joint/generic_joint.rs
View File

@@ -1,144 +0,0 @@
use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint};
use crate::math::{Isometry, Real, SpacialVector};
use crate::na::{Rotation3, UnitQuaternion};
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A joint that prevents all relative movement between two bodies.
///
/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
pub struct GenericJoint {
/// The frame of reference for the first body affected by this joint, expressed in the local frame
/// of the first body.
pub local_anchor1: Isometry<Real>,
/// The frame of reference for the second body affected by this joint, expressed in the local frame
/// of the first body.
pub local_anchor2: Isometry<Real>,
/// The impulse applied to the first body affected by this joint.
///
/// The impulse applied to the second body affected by this joint is given by `-impulse`.
/// This combines both linear and angular impulses:
/// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
/// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
pub impulse: SpacialVector<Real>,
pub min_position: SpacialVector<Real>,
pub max_position: SpacialVector<Real>,
pub min_velocity: SpacialVector<Real>,
pub max_velocity: SpacialVector<Real>,
/// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0
pub min_impulse: SpacialVector<Real>,
/// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0
pub max_impulse: SpacialVector<Real>,
/// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0
pub min_pos_impulse: SpacialVector<Real>,
/// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0
pub max_pos_impulse: SpacialVector<Real>,
}
impl GenericJoint {
/// Creates a new fixed joint from the frames of reference of both bodies.
pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self {
Self {
local_anchor1,
local_anchor2,
impulse: SpacialVector::zeros(),
min_position: SpacialVector::zeros(),
max_position: SpacialVector::zeros(),
min_velocity: SpacialVector::zeros(),
max_velocity: SpacialVector::zeros(),
min_impulse: SpacialVector::repeat(-Real::MAX),
max_impulse: SpacialVector::repeat(Real::MAX),
min_pos_impulse: SpacialVector::repeat(-Real::MAX),
max_pos_impulse: SpacialVector::repeat(Real::MAX),
}
}
pub fn set_dof_vel(&mut self, dof: u8, target_vel: Real, max_force: Real) {
self.min_velocity[dof as usize] = target_vel;
self.max_velocity[dof as usize] = target_vel;
self.min_impulse[dof as usize] = -max_force;
self.max_impulse[dof as usize] = max_force;
}
pub fn free_dof(&mut self, dof: u8) {
self.min_position[dof as usize] = -Real::MAX;
self.max_position[dof as usize] = Real::MAX;
self.min_velocity[dof as usize] = -Real::MAX;
self.max_velocity[dof as usize] = Real::MAX;
self.min_impulse[dof as usize] = 0.0;
self.max_impulse[dof as usize] = 0.0;
self.min_pos_impulse[dof as usize] = 0.0;
self.max_pos_impulse[dof as usize] = 0.0;
}
pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) {
self.min_position[dof as usize] = min;
self.max_position[dof as usize] = max;
}
}
impl From<RevoluteJoint> for GenericJoint {
fn from(joint: RevoluteJoint) -> Self {
let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
let mut result = Self::new(local_anchor1, local_anchor2);
result.free_dof(3);
if joint.motor_damping != 0.0 {
result.set_dof_vel(3, joint.motor_target_vel, joint.motor_max_impulse);
}
result.impulse[0] = joint.impulse[0];
result.impulse[1] = joint.impulse[1];
result.impulse[2] = joint.impulse[2];
result.impulse[3] = joint.motor_impulse;
result.impulse[4] = joint.impulse[3];
result.impulse[5] = joint.impulse[4];
result
}
}
impl From<BallJoint> for GenericJoint {
fn from(joint: BallJoint) -> Self {
let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero());
let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero());
let mut result = Self::new(local_anchor1, local_anchor2);
result.impulse[0] = joint.impulse[0];
result.impulse[1] = joint.impulse[1];
result.impulse[2] = joint.impulse[2];
result.free_dof(3);
result.free_dof(4);
result.free_dof(5);
result
}
}
impl From<PrismaticJoint> for GenericJoint {
fn from(joint: PrismaticJoint) -> Self {
let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
let mut result = Self::new(local_anchor1, local_anchor2);
result.free_dof(0);
result.set_dof_limits(0, joint.limits[0], joint.limits[1]);
result
}
}
impl From<FixedJoint> for GenericJoint {
fn from(joint: FixedJoint) -> Self {
Self::new(joint.local_anchor1, joint.local_anchor2)
}
}

20
src/dynamics/joint/impulse_joint/impulse_joint.rs Normal file
View File

@@ -0,0 +1,20 @@
use crate::dynamics::{JointData, JointHandle, RigidBodyHandle};
use crate::math::{Real, SpacialVector};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, PartialEq)]
/// A joint attached to two bodies.
pub struct ImpulseJoint {
/// Handle to the first body attached to this joint.
pub body1: RigidBodyHandle,
/// Handle to the second body attached to this joint.
pub body2: RigidBodyHandle,
pub data: JointData,
pub impulses: SpacialVector<Real>,
// A joint needs to know its handle to simplify its removal.
pub(crate) handle: JointHandle,
#[cfg(feature = "parallel")]
pub(crate) constraint_index: usize,
}

73
src/dynamics/joint/joint_set.rs → src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -1,10 +1,10 @@
use super::Joint;
use super::ImpulseJoint;
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
use crate::data::arena::Arena;
use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut};
use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
use crate::dynamics::{JointParams, RigidBodyHandle};
use crate::dynamics::{JointData, RigidBodyHandle};
/// The unique identifier of a joint added to the joint set.
/// The unique identifier of a collider added to a collider set.
@@ -34,19 +34,19 @@ impl JointHandle {
}
pub(crate) type JointIndex = usize;
pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
pub(crate) type JointGraphEdge = crate::data::graph::Edge<ImpulseJoint>;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Default)]
/// A set of joints that can be handled by a physics `World`.
pub struct JointSet {
/// A set of impulse_joints that can be handled by a physics `World`.
pub struct ImpulseJointSet {
rb_graph_ids: Coarena<RigidBodyGraphIndex>,
joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
joint_graph: InteractionGraph<RigidBodyHandle, Joint>,
joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>,
}
impl JointSet {
/// Creates a new empty set of joints.
impl ImpulseJointSet {
/// Creates a new empty set of impulse_joints.
pub fn new() -> Self {
Self {
rb_graph_ids: Coarena::new(),
@@ -55,26 +55,26 @@ impl JointSet {
}
}
/// The number of joints on this set.
/// The number of impulse_joints on this set.
pub fn len(&self) -> usize {
self.joint_graph.graph.edges.len()
}
/// `true` if there are no joints in this set.
/// `true` if there are no impulse_joints in this set.
pub fn is_empty(&self) -> bool {
self.joint_graph.graph.edges.is_empty()
}
/// Retrieve the joint graph where edges are joints and nodes are rigid body handles.
pub fn joint_graph(&self) -> &InteractionGraph<RigidBodyHandle, Joint> {
/// Retrieve the joint graph where edges are impulse_joints and nodes are rigid body handles.
pub fn joint_graph(&self) -> &InteractionGraph<RigidBodyHandle, ImpulseJoint> {
&self.joint_graph
}
/// Iterates through all the joitns attached to the given rigid-body.
/// Iterates through all the impulse_joints attached to the given rigid-body.
pub fn joints_with<'a>(
&'a self,
body: RigidBodyHandle,
) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyHandle, &'a Joint)> {
) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyHandle, &'a ImpulseJoint)> {
self.rb_graph_ids
.get(body.0)
.into_iter()
@@ -87,13 +87,13 @@ impl JointSet {
}
/// Gets the joint with the given handle.
pub fn get(&self, handle: JointHandle) -> Option<&Joint> {
pub fn get(&self, handle: JointHandle) -> Option<&ImpulseJoint> {
let id = self.joint_ids.get(handle.0)?;
self.joint_graph.graph.edge_weight(*id)
}
/// Gets a mutable reference to the joint with the given handle.
pub fn get_mut(&mut self, handle: JointHandle) -> Option<&mut Joint> {
pub fn get_mut(&mut self, handle: JointHandle) -> Option<&mut ImpulseJoint> {
let id = self.joint_ids.get(handle.0)?;
self.joint_graph.graph.edge_weight_mut(*id)
}
@@ -107,7 +107,7 @@ impl JointSet {
///
/// Using this is discouraged in favor of `self.get(handle)` which does not
/// suffer form the ABA problem.
pub fn get_unknown_gen(&self, i: u32) -> Option<(&Joint, JointHandle)> {
pub fn get_unknown_gen(&self, i: u32) -> Option<(&ImpulseJoint, JointHandle)> {
let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
Some((
self.joint_graph.graph.edge_weight(*id)?,
@@ -124,7 +124,7 @@ impl JointSet {
///
/// Using this is discouraged in favor of `self.get_mut(handle)` which does not
/// suffer form the ABA problem.
pub fn get_unknown_gen_mut(&mut self, i: u32) -> Option<(&mut Joint, JointHandle)> {
pub fn get_unknown_gen_mut(&mut self, i: u32) -> Option<(&mut ImpulseJoint, JointHandle)> {
let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
Some((
self.joint_graph.graph.edge_weight_mut(*id)?,
@@ -133,7 +133,7 @@ impl JointSet {
}
/// Iterates through all the joint on this set.
pub fn iter(&self) -> impl Iterator<Item = (JointHandle, &Joint)> {
pub fn iter(&self) -> impl Iterator<Item = (JointHandle, &ImpulseJoint)> {
self.joint_graph
.graph
.edges
@@ -142,7 +142,7 @@ impl JointSet {
}
/// Iterates mutably through all the joint on this set.
pub fn iter_mut(&mut self) -> impl Iterator<Item = (JointHandle, &mut Joint)> {
pub fn iter_mut(&mut self) -> impl Iterator<Item = (JointHandle, &mut ImpulseJoint)> {
self.joint_graph
.graph
.edges
@@ -150,8 +150,8 @@ impl JointSet {
.map(|e| (e.weight.handle, &mut e.weight))
}
// /// The set of joints as an array.
// pub(crate) fn joints(&self) -> &[JointGraphEdge] {
// /// The set of impulse_joints as an array.
// pub(crate) fn impulse_joints(&self) -> &[JointGraphEdge] {
// // self.joint_graph
// // .graph
// // .edges
@@ -170,25 +170,22 @@ impl JointSet {
}
/// Inserts a new joint into this set and retrieve its handle.
pub fn insert<J>(
pub fn insert(
&mut self,
body1: RigidBodyHandle,
body2: RigidBodyHandle,
joint_params: J,
) -> JointHandle
where
J: Into<JointParams>,
{
data: impl Into<JointData>,
) -> JointHandle {
let data = data.into();
let handle = self.joint_ids.insert(0.into());
let joint = Joint {
let joint = ImpulseJoint {
body1,
body2,
data,
impulses: na::zero(),
handle: JointHandle(handle),
#[cfg(feature = "parallel")]
constraint_index: 0,
#[cfg(feature = "parallel")]
position_constraint_index: 0,
params: joint_params.into(),
};
let default_id = InteractionGraph::<(), ()>::invalid_graph_index();
@@ -201,12 +198,12 @@ impl JointSet {
// NOTE: the body won't have a graph index if it does not
// have any joint attached.
if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index1) {
if !InteractionGraph::<RigidBodyHandle, ImpulseJoint>::is_graph_index_valid(graph_index1) {
graph_index1 = self.joint_graph.graph.add_node(joint.body1);
self.rb_graph_ids.insert(joint.body1.0, graph_index1);
}
if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index2) {
if !InteractionGraph::<RigidBodyHandle, ImpulseJoint>::is_graph_index_valid(graph_index2) {
graph_index2 = self.joint_graph.graph.add_node(joint.body2);
self.rb_graph_ids.insert(joint.body2.0, graph_index2);
}
@@ -215,7 +212,7 @@ impl JointSet {
JointHandle(handle)
}
/// Retrieve all the joints happening between two active bodies.
/// Retrieve all the impulse_joints happening between two active bodies.
// NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
pub(crate) fn select_active_interactions<Bodies>(
&self,
@@ -271,7 +268,7 @@ impl JointSet {
islands: &mut IslandManager,
bodies: &mut Bodies,
wake_up: bool,
) -> Option<Joint>
) -> Option<ImpulseJoint>
where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
@@ -299,11 +296,11 @@ impl JointSet {
removed_joint
}
/// Deletes all the joints attached to the given rigid-body.
/// Deletes all the impulse_joints attached to the given rigid-body.
///
/// The provided rigid-body handle is not required to identify a rigid-body that
/// is still contained by the `bodies` component set.
/// Returns the (now invalid) handles of the removed joints.
/// Returns the (now invalid) handles of the removed impulse_joints.
pub fn remove_joints_attached_to_rigid_body<Bodies>(
&mut self,
handle: RigidBodyHandle,

6
src/dynamics/joint/impulse_joint/mod.rs Normal file
View File

@@ -0,0 +1,6 @@
pub use self::impulse_joint::ImpulseJoint;
pub use self::impulse_joint_set::{ImpulseJointSet, JointHandle};
pub(crate) use self::impulse_joint_set::{JointGraphEdge, JointIndex};
mod impulse_joint;
mod impulse_joint_set;

143
src/dynamics/joint/joint.rs
View File

@@ -1,143 +0,0 @@
#[cfg(feature = "dim3")]
use crate::dynamics::RevoluteJoint;
use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
#[derive(Copy, Clone, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// An enum grouping all possible types of joints.
pub enum JointParams {
/// A Ball joint that removes all relative linear degrees of freedom between the affected bodies.
BallJoint(BallJoint),
/// A fixed joint that removes all relative degrees of freedom between the affected bodies.
FixedJoint(FixedJoint),
/// A prismatic joint that removes all degrees of degrees of freedom between the affected
/// bodies except for the translation along one axis.
PrismaticJoint(PrismaticJoint),
#[cfg(feature = "dim3")]
/// A revolute joint that removes all degrees of degrees of freedom between the affected
/// bodies except for the translation along one axis.
RevoluteJoint(RevoluteJoint),
// GenericJoint(GenericJoint),
}
impl JointParams {
/// An integer identifier for each type of joint.
pub fn type_id(&self) -> usize {
match self {
JointParams::BallJoint(_) => 0,
JointParams::FixedJoint(_) => 1,
JointParams::PrismaticJoint(_) => 2,
// JointParams::GenericJoint(_) => 3,
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => 4,
}
}
/// Gets a reference to the underlying ball joint, if `self` is one.
pub fn as_ball_joint(&self) -> Option<&BallJoint> {
if let JointParams::BallJoint(j) = self {
Some(j)
} else {
None
}
}
/// Gets a reference to the underlying fixed joint, if `self` is one.
pub fn as_fixed_joint(&self) -> Option<&FixedJoint> {
if let JointParams::FixedJoint(j) = self {
Some(j)
} else {
None
}
}
// /// Gets a reference to the underlying generic joint, if `self` is one.
// pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
// if let JointParams::GenericJoint(j) = self {
// Some(j)
// } else {
// None
// }
// }
/// Gets a reference to the underlying prismatic joint, if `self` is one.
pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
if let JointParams::PrismaticJoint(j) = self {
Some(j)
} else {
None
}
}
/// Gets a reference to the underlying revolute joint, if `self` is one.
#[cfg(feature = "dim3")]
pub fn as_revolute_joint(&self) -> Option<&RevoluteJoint> {
if let JointParams::RevoluteJoint(j) = self {
Some(j)
} else {
None
}
}
}
impl From<BallJoint> for JointParams {
fn from(j: BallJoint) -> Self {
JointParams::BallJoint(j)
}
}
impl From<FixedJoint> for JointParams {
fn from(j: FixedJoint) -> Self {
JointParams::FixedJoint(j)
}
}
// impl From<GenericJoint> for JointParams {
// fn from(j: GenericJoint) -> Self {
// JointParams::GenericJoint(j)
// }
// }
#[cfg(feature = "dim3")]
impl From<RevoluteJoint> for JointParams {
fn from(j: RevoluteJoint) -> Self {
JointParams::RevoluteJoint(j)
}
}
impl From<PrismaticJoint> for JointParams {
fn from(j: PrismaticJoint) -> Self {
JointParams::PrismaticJoint(j)
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
/// A joint attached to two bodies.
pub struct Joint {
/// Handle to the first body attached to this joint.
pub body1: RigidBodyHandle,
/// Handle to the second body attached to this joint.
pub body2: RigidBodyHandle,
// A joint needs to know its handle to simplify its removal.
pub(crate) handle: JointHandle,
#[cfg(feature = "parallel")]
pub(crate) constraint_index: usize,
#[cfg(feature = "parallel")]
pub(crate) position_constraint_index: usize,
/// The joint geometric parameters and impulse.
pub params: JointParams,
}
impl Joint {
/// Can this joint use SIMD-accelerated constraint formulations?
pub fn supports_simd_constraints(&self) -> bool {
match &self.params {
JointParams::PrismaticJoint(joint) => joint.supports_simd_constraints(),
JointParams::FixedJoint(joint) => joint.supports_simd_constraints(),
JointParams::BallJoint(joint) => joint.supports_simd_constraints(),
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(),
}
}
}

270
src/dynamics/joint/joint_data.rs Normal file
View File

@@ -0,0 +1,270 @@
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::MotorModel;
use crate::math::{Isometry, Point, Real, Rotation, UnitVector, SPATIAL_DIM};
use crate::utils::WBasis;
#[cfg(feature = "dim3")]
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct JointAxesMask: u8 {
const FREE = 0;
const X = 1 << 0;
const Y = 1 << 1;
const Z = 1 << 2;
const ANG_X = 1 << 3;
const ANG_Y = 1 << 4;
const ANG_Z = 1 << 5;
}
}
#[cfg(feature = "dim2")]
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct JointAxesMask: u8 {
const FREE = 0;
const X = 1 << 0;
const Y = 1 << 1;
const ANG_X = 1 << 2;
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub enum JointAxis {
X = 0,
Y,
#[cfg(feature = "dim3")]
Z,
AngX,
#[cfg(feature = "dim3")]
AngY,
#[cfg(feature = "dim3")]
AngZ,
}
impl From<JointAxis> for JointAxesMask {
fn from(axis: JointAxis) -> Self {
JointAxesMask::from_bits(1 << axis as usize).unwrap()
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct JointLimits {
pub min: Real,
pub max: Real,
pub impulse: Real,
}
impl Default for JointLimits {
fn default() -> Self {
Self {
min: -Real::MAX,
max: Real::MAX,
impulse: 0.0,
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct JointMotor {
pub target_vel: Real,
pub target_pos: Real,
pub stiffness: Real,
pub damping: Real,
pub max_impulse: Real,
pub impulse: Real,
pub model: MotorModel,
}
impl Default for JointMotor {
fn default() -> Self {
Self {
target_pos: 0.0,
target_vel: 0.0,
stiffness: 0.0,
damping: 0.0,
max_impulse: Real::MAX,
impulse: 0.0,
model: MotorModel::VelocityBased,
}
}
}
impl JointMotor {
pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters<Real> {
let (stiffness, damping, gamma, _keep_lhs) =
self.model
.combine_coefficients(dt, self.stiffness, self.damping);
MotorParameters {
stiffness,
damping,
gamma,
// keep_lhs,
target_pos: self.target_pos,
target_vel: self.target_vel,
max_impulse: self.max_impulse,
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct JointData {
pub local_frame1: Isometry<Real>,
pub local_frame2: Isometry<Real>,
pub locked_axes: JointAxesMask,
pub limit_axes: JointAxesMask,
pub motor_axes: JointAxesMask,
pub limits: [JointLimits; SPATIAL_DIM],
pub motors: [JointMotor; SPATIAL_DIM],
}
impl Default for JointData {
fn default() -> Self {
Self {
local_frame1: Isometry::identity(),
local_frame2: Isometry::identity(),
locked_axes: JointAxesMask::FREE,
limit_axes: JointAxesMask::FREE,
motor_axes: JointAxesMask::FREE,
limits: [JointLimits::default(); SPATIAL_DIM],
motors: [JointMotor::default(); SPATIAL_DIM],
}
}
}
impl JointData {
#[must_use]
pub fn new(locked_axes: JointAxesMask) -> Self {
Self::default().lock_axes(locked_axes)
}
/// Can this joint use SIMD-accelerated constraint formulations?
pub fn supports_simd_constraints(&self) -> bool {
self.limit_axes.is_empty() && self.motor_axes.is_empty()
}
#[must_use]
pub fn lock_axes(mut self, axes: JointAxesMask) -> Self {
self.locked_axes |= axes;
self
}
fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
let basis = axis.orthonormal_basis();
#[cfg(feature = "dim2")]
{
use na::{Matrix2, Rotation2, UnitComplex};
let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]);
let rotmat = Rotation2::from_matrix_unchecked(mat);
UnitComplex::from_rotation_matrix(&rotmat)
}
#[cfg(feature = "dim3")]
{
use na::{Matrix3, Rotation3, UnitQuaternion};
let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]);
let rotmat = Rotation3::from_matrix_unchecked(mat);
UnitQuaternion::from_rotation_matrix(&rotmat)
}
}
#[must_use]
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
self.local_frame1 = local_frame;
self
}
#[must_use]
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
self.local_frame2 = local_frame;
self
}
#[must_use]
pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
self
}
#[must_use]
pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
self
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.local_frame1.translation.vector = anchor1.coords;
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.local_frame2.translation.vector = anchor2.coords;
self
}
#[must_use]
pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
let i = axis as usize;
self.limit_axes |= axis.into();
self.limits[i].min = limits[0];
self.limits[i].max = limits[1];
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
self.motors[axis as usize].model = model;
self
}
/// Sets the target velocity this motor needs to reach.
pub fn motor_velocity(self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
self.motor_axis(
axis,
self.motors[axis as usize].target_pos,
target_vel,
0.0,
factor,
)
}
/// Sets the target angle this motor needs to reach.
pub fn motor_position(
self,
axis: JointAxis,
target_pos: Real,
stiffness: Real,
damping: Real,
) -> Self {
self.motor_axis(axis, target_pos, 0.0, stiffness, damping)
}
/// Configure both the target angle and target velocity of the motor.
pub fn motor_axis(
mut self,
axis: JointAxis,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) -> Self {
self.motor_axes |= axis.into();
let i = axis as usize;
self.motors[i].target_vel = target_vel;
self.motors[i].target_pos = target_pos;
self.motors[i].stiffness = stiffness;
self.motors[i].damping = damping;
self
}
pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self {
self.motors[axis as usize].max_impulse = max_impulse;
self
}
}

29
src/dynamics/joint/mod.rs
View File

@@ -1,20 +1,21 @@
pub use self::ball_joint::BallJoint;
pub use self::fixed_joint::FixedJoint;
// pub use self::generic_joint::GenericJoint;
pub use self::joint::{Joint, JointParams};
pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
pub use self::joint_set::{JointHandle, JointSet};
pub use self::impulse_joint::*;
pub use self::joint_data::*;
pub use self::motor_model::MotorModel;
pub use self::multibody_joint::*;
pub use self::prismatic_joint::PrismaticJoint;
#[cfg(feature = "dim3")]
pub use self::revolute_joint::RevoluteJoint;
pub use self::spring_model::SpringModel;
mod ball_joint;
mod fixed_joint;
// mod generic_joint;
mod joint;
mod joint_set;
mod prismatic_joint;
#[cfg(feature = "dim3")]
pub use self::spherical_joint::SphericalJoint;
mod fixed_joint;
mod impulse_joint;
mod joint_data;
mod motor_model;
mod multibody_joint;
mod prismatic_joint;
mod revolute_joint;
mod spring_model;
#[cfg(feature = "dim3")]
mod spherical_joint;

36
src/dynamics/joint/spring_model.rs → src/dynamics/joint/motor_model.rs
View File

@@ -3,9 +3,7 @@ use crate::math::Real;
/// The spring-like model used for constraints resolution.
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub enum SpringModel {
/// No equation is solved.
Disabled,
pub enum MotorModel {
/// The solved spring-like equation is:
/// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
///
@@ -16,21 +14,21 @@ pub enum SpringModel {
/// The solved spring-like equation is:
/// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
AccelerationBased,
/// The solved spring-like equation is:
/// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))`
ForceBased,
// /// The solved spring-like equation is:
// /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))`
// ForceBased,
}
impl Default for SpringModel {
impl Default for MotorModel {
fn default() -> Self {
SpringModel::VelocityBased
MotorModel::VelocityBased
}
}
impl SpringModel {
impl MotorModel {
/// Combines the coefficients used for solving the spring equation.
///
/// Returns the new coefficients (stiffness, damping, inv_lhs_scale, keep_inv_lhs)
/// Returns the new coefficients (stiffness, damping, gamma, keep_inv_lhs)
/// coefficients for the equivalent impulse-based equation. These new
/// coefficients must be used in the following way:
/// - `rhs = (stiffness * pos_err + damping * vel_err) / gamma`.
@@ -43,8 +41,8 @@ impl SpringModel {
damping: Real,
) -> (Real, Real, Real, bool) {
match self {
SpringModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
SpringModel::AccelerationBased => {
MotorModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
MotorModel::AccelerationBased => {
let effective_stiffness = stiffness * dt;
let effective_damping = damping * dt;
// TODO: Using gamma behaves very badly for some reasons.
@@ -52,14 +50,12 @@ impl SpringModel {
// and get back to this later.
// let gamma = effective_stiffness * dt + effective_damping;
(effective_stiffness, effective_damping, 1.0, true)
}
SpringModel::ForceBased => {
let effective_stiffness = stiffness * dt;
let effective_damping = damping * dt;
let gamma = effective_stiffness * dt + effective_damping;
(effective_stiffness, effective_damping, gamma, false)
}
SpringModel::Disabled => return (0.0, 0.0, 0.0, false),
} // MotorModel::ForceBased => {
// let effective_stiffness = stiffness * dt;
// let effective_damping = damping * dt;
// let gamma = effective_stiffness * dt + effective_damping;
// (effective_stiffness, effective_damping, gamma, false)
// }
}
}
}

15
src/dynamics/joint/multibody_joint/mod.rs Normal file
View File

@@ -0,0 +1,15 @@
//! MultibodyJoints using the reduced-coordinates formalism or using constraints.
pub use self::multibody::Multibody;
pub use self::multibody_joint::MultibodyJoint;
pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet};
pub use self::multibody_link::MultibodyLink;
pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint};
mod multibody;
mod multibody_joint_set;
mod multibody_link;
mod multibody_workspace;
mod multibody_joint;
mod unit_multibody_joint;

1021
src/dynamics/joint/multibody_joint/multibody.rs Normal file
View File

File diff suppressed because it is too large Load Diff

571
src/dynamics/joint/multibody_joint/multibody_joint.rs Normal file
View File

@@ -0,0 +1,571 @@
use crate::dynamics::solver::AnyJointVelocityConstraint;
use crate::dynamics::{
joint, FixedJoint, IntegrationParameters, JointAxesMask, JointData, Multibody, MultibodyLink,
RigidBodyVelocity,
};
use crate::math::{
Isometry, JacobianSliceMut, Matrix, Real, Rotation, SpacialVector, Translation, Vector,
ANG_DIM, DIM, SPATIAL_DIM,
};
use crate::utils::WCross;
use na::{DVector, DVectorSliceMut};
#[cfg(feature = "dim3")]
use {
crate::utils::WCrossMatrix,
na::{UnitQuaternion, Vector3, VectorSlice3},
};
#[derive(Copy, Clone, Debug)]
pub struct MultibodyJoint {
pub data: JointData,
pub(crate) coords: SpacialVector<Real>,
pub(crate) joint_rot: Rotation<Real>,
jacobian_v: Matrix<Real>,
jacobian_dot_v: Matrix<Real>,
jacobian_dot_veldiff_v: Matrix<Real>,
}
#[cfg(feature = "dim2")]
fn revolute_locked_axes() -> JointAxesMask {
JointAxesMask::X | JointAxesMask::Y
}
#[cfg(feature = "dim3")]
fn revolute_locked_axes() -> JointAxesMask {
JointAxesMask::X
| JointAxesMask::Y
| JointAxesMask::Z
| JointAxesMask::ANG_Y
| JointAxesMask::ANG_Z
}
impl MultibodyJoint {
pub fn new(data: JointData) -> Self {
Self {
data,
coords: na::zero(),
joint_rot: Rotation::identity(),
jacobian_v: na::zero(),
jacobian_dot_v: na::zero(),
jacobian_dot_veldiff_v: na::zero(),
}
}
pub(crate) fn free(pos: Isometry<Real>) -> Self {
let mut result = Self::new(JointData::default());
result.set_free_pos(pos);
result
}
pub(crate) fn fixed(pos: Isometry<Real>) -> Self {
Self::new(FixedJoint::new().local_frame1(pos).into())
}
pub(crate) fn set_free_pos(&mut self, pos: Isometry<Real>) {
self.coords
.fixed_rows_mut::<DIM>(0)
.copy_from(&pos.translation.vector);
self.joint_rot = pos.rotation;
}
pub fn local_joint_rot(&self) -> &Rotation<Real> {
&self.joint_rot
}
fn num_free_lin_dofs(&self) -> usize {
let locked_bits = self.data.locked_axes.bits();
DIM - (locked_bits & ((1 << DIM) - 1)).count_ones() as usize
}
/// The number of degrees of freedom allowed by the multibody_joint.
pub fn ndofs(&self) -> usize {
SPATIAL_DIM - self.data.locked_axes.bits().count_ones() as usize
}
/// The position of the multibody link containing this multibody_joint relative to its parent.
pub fn body_to_parent(&self) -> Isometry<Real> {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
self.data.local_frame1.translation
* self.joint_rot
* self.data.local_frame2.translation.inverse()
} else {
let locked_bits = self.data.locked_axes.bits();
let mut transform = self.joint_rot * self.data.local_frame2.inverse();
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
transform = Translation::from(Vector::ith(i, self.coords[i])) * transform;
}
}
self.data.local_frame1 * transform
}
}
/// Integrate the position of this multibody_joint.
pub fn integrate(&mut self, dt: Real, vels: &[Real]) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
#[cfg(feature = "dim3")]
let axis = self.data.local_frame1 * Vector::x_axis();
self.coords[DIM] += vels[0] * dt;
#[cfg(feature = "dim2")]
{
self.joint_rot = Rotation::from_angle(self.coords[DIM]);
}
#[cfg(feature = "dim3")]
{
self.joint_rot = Rotation::from_axis_angle(&axis, self.coords[DIM]);
}
} else {
let locked_bits = self.data.locked_axes.bits();
let mut curr_free_dof = 0;
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
self.coords[i] += vels[curr_free_dof] * dt;
curr_free_dof += 1;
}
}
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]);
let disp = UnitQuaternion::new_eps(angvel * dt, 0.0);
self.joint_rot = disp * self.joint_rot;
}
_ => unreachable!(),
}
}
}
/// Apply a displacement to the multibody_joint.
pub fn apply_displacement(&mut self, disp: &[Real]) {
self.integrate(1.0, disp);
}
/// Update the jacobians of this multibody_joint.
pub fn update_jacobians(&mut self, vels: &[Real]) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
#[cfg(feature = "dim2")]
let axis = 1.0;
#[cfg(feature = "dim3")]
let axis = self.data.local_frame1 * Vector::x_axis();
let body_shift = self.data.local_frame2.translation.vector;
let shift = self.joint_rot * -body_shift;
let shift_dot_veldiff = axis.gcross(shift);
#[cfg(feature = "dim2")]
{
self.jacobian_v.column_mut(0).copy_from(&axis.gcross(shift));
}
#[cfg(feature = "dim3")]
{
self.jacobian_v.column_mut(0).copy_from(&axis.gcross(shift));
}
self.jacobian_dot_veldiff_v
.column_mut(0)
.copy_from(&axis.gcross(shift_dot_veldiff));
self.jacobian_dot_v
.column_mut(0)
.copy_from(&(axis.gcross(shift_dot_veldiff) * vels[0]));
} else {
let locked_bits = self.data.locked_axes.bits();
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let num_free_lin_dofs = self.num_free_lin_dofs();
let inv_frame2 = self.data.local_frame2.inverse();
let shift = self.joint_rot * inv_frame2.translation.vector;
let angvel =
VectorSlice3::from_slice(&vels[num_free_lin_dofs..num_free_lin_dofs + 3]);
let inv_rotmat2 = inv_frame2.rotation.to_rotation_matrix().into_inner();
self.jacobian_v = inv_rotmat2 * shift.gcross_matrix().transpose();
self.jacobian_dot_v =
inv_rotmat2 * angvel.cross(&shift).gcross_matrix().transpose();
}
_ => unreachable!(),
}
}
}
/// Sets in `out` the non-zero entries of the multibody_joint jacobian transformed by `transform`.
pub fn jacobian(&self, transform: &Isometry<Real>, out: &mut JacobianSliceMut<Real>) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
#[cfg(feature = "dim2")]
let axis = 1.0;
#[cfg(feature = "dim3")]
let axis = self.data.local_frame1 * Vector::x();
let jacobian = RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis);
out.copy_from(jacobian.transformed(transform).as_vector())
} else {
let locked_bits = self.data.locked_axes.bits();
let mut curr_free_dof = 0;
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
let transformed_axis = transform * self.data.local_frame1 * Vector::ith(i, 1.0);
out.fixed_slice_mut::<DIM, 1>(0, curr_free_dof)
.copy_from(&transformed_axis);
curr_free_dof += 1;
}
}
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let rotmat = transform.rotation.to_rotation_matrix();
out.fixed_slice_mut::<3, 3>(0, curr_free_dof)
.copy_from(&(rotmat * self.jacobian_v));
out.fixed_slice_mut::<3, 3>(3, curr_free_dof)
.copy_from(rotmat.matrix());
}
_ => unreachable!(),
}
}
}
/// Sets in `out` the non-zero entries of the time-derivative of the multibody_joint jacobian transformed by `transform`.
pub fn jacobian_dot(&self, transform: &Isometry<Real>, out: &mut JacobianSliceMut<Real>) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
let jacobian = RigidBodyVelocity::from_vectors(
self.jacobian_dot_v.column(0).into_owned(),
na::zero(),
);
out.copy_from(jacobian.transformed(transform).as_vector())
} else {
let locked_bits = self.data.locked_axes.bits();
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let num_free_lin_dofs = self.num_free_lin_dofs();
let rotmat = transform.rotation.to_rotation_matrix();
out.fixed_slice_mut::<3, 3>(0, num_free_lin_dofs)
.copy_from(&(rotmat * self.jacobian_dot_v));
}
_ => unreachable!(),
}
}
}
/// Sets in `out` the non-zero entries of the velocity-derivative of the time-derivative of the multibody_joint jacobian transformed by `transform`.
pub fn jacobian_dot_veldiff_mul_coordinates(
&self,
transform: &Isometry<Real>,
acc: &[Real],
out: &mut JacobianSliceMut<Real>,
) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
let jacobian = RigidBodyVelocity::from_vectors(
self.jacobian_dot_veldiff_v.column(0).into_owned(),
na::zero(),
);
out.copy_from((jacobian.transformed(transform) * acc[0]).as_vector())
} else {
let locked_bits = self.data.locked_axes.bits();
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let num_free_lin_dofs = self.num_free_lin_dofs();
let angvel =
Vector3::from_row_slice(&acc[num_free_lin_dofs..num_free_lin_dofs + 3]);
let rotmat = transform.rotation.to_rotation_matrix();
let res = rotmat * angvel.gcross_matrix() * self.jacobian_v;
out.fixed_slice_mut::<3, 3>(0, num_free_lin_dofs)
.copy_from(&res);
}
_ => unreachable!(),
}
}
}
/// Multiply the multibody_joint jacobian by generalized velocities to obtain the
/// relative velocity of the multibody link containing this multibody_joint.
pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
#[cfg(feature = "dim2")]
let axis = 1.0;
#[cfg(feature = "dim3")]
let axis = self.data.local_frame1 * Vector::x();
RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis) * acc[0]
} else {
let locked_bits = self.data.locked_axes.bits();
let mut result = RigidBodyVelocity::zero();
let mut curr_free_dof = 0;
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
result.linvel += self.data.local_frame1 * Vector::ith(i, acc[curr_free_dof]);
curr_free_dof += 1;
}
}
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let angvel = Vector3::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]);
let linvel = self.jacobian_v * angvel;
result += RigidBodyVelocity::new(linvel, angvel);
}
_ => unreachable!(),
}
result
}
}
/// Multiply the multibody_joint jacobian by generalized accelerations to obtain the
/// relative acceleration of the multibody link containing this multibody_joint.
pub fn jacobian_dot_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
RigidBodyVelocity::from_vectors(self.jacobian_dot_v.column(0).into_owned(), na::zero())
* acc[0]
} else {
let locked_bits = self.data.locked_axes.bits();
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => {
/* No free dofs. */
RigidBodyVelocity::zero()
}
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let num_free_lin_dofs = self.num_free_lin_dofs();
let angvel =
Vector3::from_row_slice(&acc[num_free_lin_dofs..num_free_lin_dofs + 3]);
let linvel = self.jacobian_dot_v * angvel;
RigidBodyVelocity::new(linvel, na::zero())
}
_ => unreachable!(),
}
}
}
/// Fill `out` with the non-zero entries of a damping that can be applied by default to ensure a good stability of the multibody_joint.
pub fn default_damping(&self, out: &mut DVectorSliceMut<Real>) {
let locked_bits = self.data.locked_axes.bits();
let mut curr_free_dof = self.num_free_lin_dofs();
// A default damping only for the angular dofs
for i in DIM..SPATIAL_DIM {
if locked_bits & (1 << i) == 0 {
// This is a free angular DOF.
out[curr_free_dof] = 0.2;
curr_free_dof += 1;
}
}
}
/// Maximum number of velocity constrains that can be generated by this multibody_joint.
pub fn num_velocity_constraints(&self) -> usize {
let locked_bits = self.data.locked_axes.bits();
let limit_bits = self.data.limit_axes.bits();
let motor_bits = self.data.motor_axes.bits();
let mut num_constraints = 0;
for i in 0..SPATIAL_DIM {
if (locked_bits & (1 << i)) == 0 {
if (limit_bits & (1 << i)) != 0 {
num_constraints += 1;
}
if (motor_bits & (1 << i)) != 0 {
num_constraints += 1;
}
}
}
num_constraints
}
/// Initialize and generate velocity constraints to enforce, e.g., multibody_joint limits and motors.
pub fn velocity_constraints(
&self,
params: &IntegrationParameters,
multibody: &Multibody,
link: &MultibodyLink,
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
) {
let locked_bits = self.data.locked_axes.bits();
let limit_bits = self.data.limit_axes.bits();
let motor_bits = self.data.motor_axes.bits();
let mut curr_free_dof = 0;
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
if (limit_bits & (1 << i)) != 0 {
joint::unit_joint_limit_constraint(
params,
multibody,
link,
[self.data.limits[i].min, self.data.limits[i].max],
self.coords[i],
dof_id + curr_free_dof,
j_id,
jacobians,
constraints,
);
}
if (motor_bits & (1 << i)) != 0 {
joint::unit_joint_motor_constraint(
params,
multibody,
link,
&self.data.motors[i],
self.coords[i],
dof_id + curr_free_dof,
j_id,
jacobians,
constraints,
);
}
curr_free_dof += 1;
}
}
/*
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {}
2 => {
todo!()
}
3 => {}
_ => unreachable!(),
}
*/
// TODO: we should make special cases for multi-angular-dofs limits/motors
for i in DIM..SPATIAL_DIM {
if (locked_bits & (1 << i)) == 0 {
if (limit_bits & (1 << i)) != 0 {
joint::unit_joint_limit_constraint(
params,
multibody,
link,
[self.data.limits[i].min, self.data.limits[i].max],
self.coords[i],
dof_id + curr_free_dof,
j_id,
jacobians,
constraints,
);
}
if (motor_bits & (1 << i)) != 0 {
joint::unit_joint_motor_constraint(
params,
multibody,
link,
&self.data.motors[i],
self.coords[i],
dof_id + curr_free_dof,
j_id,
jacobians,
constraints,
);
}
curr_free_dof += 1;
}
}
}
}

352
src/dynamics/joint/multibody_joint/multibody_joint_set.rs Normal file
View File

@@ -0,0 +1,352 @@
use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut};
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::{
IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
RigidBodyIds, RigidBodyType,
};
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
use crate::parry::partitioning::IndexedData;
use std::ops::Index;
/// The unique handle of an multibody_joint added to a `MultibodyJointSet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct MultibodyJointHandle(pub crate::data::arena::Index);
/// The temporary index of a multibody added to a `MultibodyJointSet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct MultibodyIndex(pub crate::data::arena::Index);
impl MultibodyJointHandle {
/// Converts this handle into its (index, generation) components.
pub fn into_raw_parts(self) -> (u32, u32) {
self.0.into_raw_parts()
}
/// Reconstructs an handle from its (index, generation) components.
pub fn from_raw_parts(id: u32, generation: u32) -> Self {
Self(crate::data::arena::Index::from_raw_parts(id, generation))
}
/// An always-invalid rigid-body handle.
pub fn invalid() -> Self {
Self(crate::data::arena::Index::from_raw_parts(
crate::INVALID_U32,
crate::INVALID_U32,
))
}
}
impl Default for MultibodyJointHandle {
fn default() -> Self {
Self::invalid()
}
}
impl IndexedData for MultibodyJointHandle {
fn default() -> Self {
Self(IndexedData::default())
}
fn index(&self) -> usize {
self.0.index()
}
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct MultibodyJointLink {
pub graph_id: RigidBodyGraphIndex,
pub multibody: MultibodyIndex,
pub id: usize,
}
impl Default for MultibodyJointLink {
fn default() -> Self {
Self {
graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32),
multibody: MultibodyIndex(crate::data::arena::Index::from_raw_parts(
crate::INVALID_U32,
crate::INVALID_U32,
)),
id: 0,
}
}
}
/// A set of rigid bodies that can be handled by a physics pipeline.
pub struct MultibodyJointSet {
pub(crate) multibodies: Arena<Multibody>, // NOTE: a Slab would be sufficient.
pub(crate) rb2mb: Coarena<MultibodyJointLink>,
// NOTE: this is mostly for the island extraction. So perhaps we won’t need
// that any more in the future when we improve our island builder.
pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
}
impl MultibodyJointSet {
/// Create a new empty set of multibodies.
pub fn new() -> Self {
Self {
multibodies: Arena::new(),
rb2mb: Coarena::new(),
connectivity_graph: InteractionGraph::new(),
}
}
pub fn iter(&self) -> impl Iterator<Item = (MultibodyJointHandle, &Multibody, &MultibodyLink)> {
self.rb2mb
.iter()
.filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user.
.map(|(h, link)| {
let mb = &self.multibodies[link.multibody.0];
(MultibodyJointHandle(h), mb, mb.link(link.id).unwrap())
})
}
/// Inserts a new multibody_joint into this set.
pub fn insert(
&mut self,
body1: RigidBodyHandle,
body2: RigidBodyHandle,
data: impl Into<JointData>,
) -> Option<MultibodyJointHandle> {
let data = data.into();
let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {
let mb_handle = self.multibodies.insert(Multibody::with_root(body1));
MultibodyJointLink {
graph_id: self.connectivity_graph.graph.add_node(body1),
multibody: MultibodyIndex(mb_handle),
id: 0,
}
});
let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| {
let mb_handle = self.multibodies.insert(Multibody::with_root(body2));
MultibodyJointLink {
graph_id: self.connectivity_graph.graph.add_node(body2),
multibody: MultibodyIndex(mb_handle),
id: 0,
}
});
if link1.multibody == link2.multibody || link2.id != 0 {
// This would introduce an invalid configuration.
return None;
}
self.connectivity_graph
.graph
.add_edge(link1.graph_id, link2.graph_id, ());
self.rb2mb.insert(body1.0, link1);
self.rb2mb.insert(body2.0, link2);
let mb2 = self.multibodies.remove(link2.multibody.0).unwrap();
let multibody1 = &mut self.multibodies[link1.multibody.0];
for mb_link2 in mb2.links() {
let link = self.rb2mb.get_mut(mb_link2.rigid_body.0).unwrap();
link.multibody = link1.multibody;
link.id += multibody1.num_links();
}
multibody1.append(mb2, link1.id, MultibodyJoint::new(data));
// Because each rigid-body can only have one parent link,
// we can use the second rigid-body’s handle as the multibody_joint’s
// handle.
Some(MultibodyJointHandle(body2.0))
}
/// Removes an multibody_joint from this set.
pub fn remove<Bodies>(
&mut self,
handle: MultibodyJointHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
wake_up: bool,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
if let Some(removed) = self.rb2mb.get(handle.0).copied() {
let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
// Remove the edge from the connectivity graph.
if let Some(parent_link) = multibody.link(removed.id).unwrap().parent_id() {
let parent_rb = multibody.link(parent_link).unwrap().rigid_body;
self.connectivity_graph.remove_edge(
self.rb2mb.get(parent_rb.0).unwrap().graph_id,
removed.graph_id,
);
if wake_up {
islands.wake_up(bodies, RigidBodyHandle(handle.0), true);
islands.wake_up(bodies, parent_rb, true);
}
// TODO: remove the node if it no longer has any attached edges?
// Extract the individual sub-trees generated by this removal.
let multibodies = multibody.remove_link(removed.id, true);
// Update the rb2mb mapping.
for multibody in multibodies {
if multibody.num_links() == 1 {
// We don’t have any multibody_joint attached to this body, remove it.
if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) {
self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id;
}
} else {
let mb_id = self.multibodies.insert(multibody);
for link in self.multibodies[mb_id].links() {
let ids = self.rb2mb.get_mut(link.rigid_body.0).unwrap();
ids.multibody = MultibodyIndex(mb_id);
ids.id = link.internal_id;
}
}
}
}
}
}
/// Removes all the multibody_joints from the multibody the given rigid-body is part of.
pub fn remove_multibody_articulations<Bodies>(
&mut self,
handle: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
wake_up: bool,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
if let Some(removed) = self.rb2mb.get(handle.0).copied() {
// Remove the multibody.
let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
for link in multibody.links() {
let rb_handle = link.rigid_body;
if wake_up {
islands.wake_up(bodies, rb_handle, true);
}
// Remove the rigid-body <-> multibody mapping for this link.
let removed = self.rb2mb.remove(rb_handle.0, Default::default()).unwrap();
// Remove the node (and all it’s edges) from the connectivity graph.
if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) {
self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id;
}
}
}
}
pub fn remove_articulations_attached_to_rigid_body<Bodies>(
&mut self,
rb_to_remove: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
// TODO: optimize this.
if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
let mut articulations_to_remove = vec![];
for (rb1, rb2, _) in self
.connectivity_graph
.interactions_with(link_to_remove.graph_id)
{
// There is an multibody_joint that we need to remove between these two bodies.
// If this is an outbound edge, then the multibody_joint’s handle is equal to the
// second body handle.
if rb1 == rb_to_remove {
articulations_to_remove.push(MultibodyJointHandle(rb2.0));
} else {
articulations_to_remove.push(MultibodyJointHandle(rb1.0));
}
islands.wake_up(bodies, rb1, true);
islands.wake_up(bodies, rb2, true);
}
for articulation_handle in articulations_to_remove {
self.remove(articulation_handle, islands, bodies, true);
}
}
}
/// Returns the link of this multibody attached to the given rigid-body.
///
/// Returns `None` if `rb` isn’t part of any rigid-body.
pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyJointLink> {
self.rb2mb.get(rb.0)
}
/// Gets a reference to a multibody, based on its temporary index.
pub fn get_multibody(&self, index: MultibodyIndex) -> Option<&Multibody> {
self.multibodies.get(index.0)
}
/// Gets a mutable reference to a multibody, based on its temporary index.
///
/// This method will bypass any modification-detection automatically done by the
/// `MultibodyJointSet`.
pub fn get_multibody_mut_internal(&mut self, index: MultibodyIndex) -> Option<&mut Multibody> {
self.multibodies.get_mut(index.0)
}
/// Gets a reference to the multibody identified by its `handle`.
pub fn get(&self, handle: MultibodyJointHandle) -> Option<(&Multibody, usize)> {
let link = self.rb2mb.get(handle.0)?;
let multibody = self.multibodies.get(link.multibody.0)?;
Some((multibody, link.id))
}
/// Gets a mutable reference to the multibody identified by its `handle`.
pub fn get_mut_internal(
&mut self,
handle: MultibodyJointHandle,
) -> Option<(&mut Multibody, usize)> {
let link = self.rb2mb.get(handle.0)?;
let multibody = self.multibodies.get_mut(link.multibody.0)?;
Some((multibody, link.id))
}
/// Iterate through the handles of all the rigid-bodies attached to this rigid-body
/// by an multibody_joint.
pub fn attached_bodies<'a>(
&'a self,
body: RigidBodyHandle,
) -> impl Iterator<Item = RigidBodyHandle> + 'a {
self.rb2mb
.get(body.0)
.into_iter()
.flat_map(move |id| self.connectivity_graph.interactions_with(id.graph_id))
.map(move |inter| crate::utils::select_other((inter.0, inter.1), body))
}
/// Iterates through all the multibodies on this set.
pub fn multibodies(&self) -> impl Iterator<Item = &Multibody> {
self.multibodies.iter().map(|e| e.1)
}
}
impl Index<MultibodyIndex> for MultibodyJointSet {
type Output = Multibody;
fn index(&self, index: MultibodyIndex) -> &Multibody {
&self.multibodies[index.0]
}
}
// impl Index<MultibodyJointHandle> for MultibodyJointSet {
// type Output = Multibody;
//
// fn index(&self, index: MultibodyJointHandle) -> &Multibody {
// &self.multibodies[index.0]
// }
// }

173
src/dynamics/joint/multibody_joint/multibody_link.rs Normal file
View File

@@ -0,0 +1,173 @@
use std::ops::{Deref, DerefMut};
use crate::dynamics::{MultibodyJoint, RigidBodyHandle};
use crate::math::{Isometry, Real};
use crate::prelude::RigidBodyVelocity;
pub(crate) struct KinematicState {
pub joint: MultibodyJoint,
pub parent_to_world: Isometry<Real>,
// TODO: should this be removed in favor of the rigid-body position?
pub local_to_world: Isometry<Real>,
pub local_to_parent: Isometry<Real>,
}
impl Clone for KinematicState {
fn clone(&self) -> Self {
Self {
joint: self.joint.clone(),
parent_to_world: self.parent_to_world.clone(),
local_to_world: self.local_to_world.clone(),
local_to_parent: self.local_to_parent.clone(),
}
}
}
/// One link of a multibody.
pub struct MultibodyLink {
pub(crate) name: String,
// FIXME: make all those private.
pub(crate) internal_id: usize,
pub(crate) assembly_id: usize,
pub(crate) is_leaf: bool,
pub(crate) parent_internal_id: usize,
pub(crate) rigid_body: RigidBodyHandle,
/*
* Change at each time step.
*/
pub(crate) state: KinematicState,
// FIXME: put this on a workspace buffer instead ?
pub(crate) velocity_dot_wrt_joint: RigidBodyVelocity,
pub(crate) velocity_wrt_joint: RigidBodyVelocity,
}
impl MultibodyLink {
/// Creates a new multibody link.
pub fn new(
rigid_body: RigidBodyHandle,
internal_id: usize,
assembly_id: usize,
parent_internal_id: usize,
joint: MultibodyJoint,
parent_to_world: Isometry<Real>,
local_to_world: Isometry<Real>,
local_to_parent: Isometry<Real>,
) -> Self {
let is_leaf = true;
let velocity_dot_wrt_joint = RigidBodyVelocity::zero();
let velocity_wrt_joint = RigidBodyVelocity::zero();
let kinematic_state = KinematicState {
joint,
parent_to_world,
local_to_world,
local_to_parent,
};
MultibodyLink {
name: String::new(),
internal_id,
assembly_id,
is_leaf,
parent_internal_id,
state: kinematic_state,
velocity_dot_wrt_joint,
velocity_wrt_joint,
rigid_body,
}
}
pub fn joint(&self) -> &MultibodyJoint {
&self.state.joint
}
pub fn rigid_body_handle(&self) -> RigidBodyHandle {
self.rigid_body
}
/// Checks if this link is the root of the multibody.
#[inline]
pub fn is_root(&self) -> bool {
self.internal_id == 0
}
/// This link's name.
#[inline]
pub fn name(&self) -> &str {
&self.name
}
/// Sets this link's name.
#[inline]
pub fn set_name(&mut self, name: String) {
self.name = name
}
/// The handle of this multibody link.
#[inline]
pub fn link_id(&self) -> usize {
self.internal_id
}
/// The handle of the parent link.
#[inline]
pub fn parent_id(&self) -> Option<usize> {
if self.internal_id != 0 {
Some(self.parent_internal_id)
} else {
None
}
}
#[inline]
pub fn local_to_world(&self) -> &Isometry<Real> {
&self.state.local_to_world
}
#[inline]
pub fn local_to_parent(&self) -> &Isometry<Real> {
&self.state.local_to_parent
}
}
// FIXME: keep this even if we already have the Index2 traits?
pub(crate) struct MultibodyLinkVec(pub Vec<MultibodyLink>);
impl MultibodyLinkVec {
#[inline]
pub fn get_mut_with_parent(&mut self, i: usize) -> (&mut MultibodyLink, &MultibodyLink) {
let parent_id = self[i].parent_internal_id;
assert!(
parent_id != i,
"Internal error: circular rigid body dependency."
);
assert!(parent_id < self.len(), "Invalid parent index.");
unsafe {
let rb = &mut *(self.get_unchecked_mut(i) as *mut _);
let parent_rb = &*(self.get_unchecked(parent_id) as *const _);
(rb, parent_rb)
}
}
}
impl Deref for MultibodyLinkVec {
type Target = Vec<MultibodyLink>;
#[inline]
fn deref(&self) -> &Vec<MultibodyLink> {
let MultibodyLinkVec(ref me) = *self;
me
}
}
impl DerefMut for MultibodyLinkVec {
#[inline]
fn deref_mut(&mut self) -> &mut Vec<MultibodyLink> {
let MultibodyLinkVec(ref mut me) = *self;
me
}
}

27
src/dynamics/joint/multibody_joint/multibody_workspace.rs Normal file
View File

@@ -0,0 +1,27 @@
use crate::dynamics::RigidBodyVelocity;
use crate::math::Real;
use na::DVector;
/// A temporary workspace for various updates of the multibody.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub(crate) struct MultibodyWorkspace {
pub accs: Vec<RigidBodyVelocity>,
pub ndofs_vec: DVector<Real>,
}
impl MultibodyWorkspace {
/// Create an empty workspace.
pub fn new() -> Self {
MultibodyWorkspace {
accs: Vec::new(),
ndofs_vec: DVector::zeros(0),
}
}
/// Resize the workspace so it is enough for `nlinks` links.
pub fn resize(&mut self, nlinks: usize, ndofs: usize) {
self.accs.resize(nlinks, RigidBodyVelocity::zero());
self.ndofs_vec = DVector::zeros(ndofs)
}
}

122
src/dynamics/joint/multibody_joint/unit_multibody_joint.rs Normal file
View File

@@ -0,0 +1,122 @@
#![allow(missing_docs)] // For downcast.
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::solver::{
AnyJointVelocityConstraint, JointGenericVelocityGroundConstraint, WritebackId,
};
use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
use crate::math::Real;
use na::DVector;
/// Initializes and generate the velocity constraints applicable to the multibody links attached
/// to this multibody_joint.
pub fn unit_joint_limit_constraint(
params: &IntegrationParameters,
multibody: &Multibody,
link: &MultibodyLink,
limits: [Real; 2],
curr_pos: Real,
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
) {
let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link);
let min_enabled = curr_pos < limits[0];
let max_enabled = limits[1] < curr_pos;
let erp_inv_dt = params.erp_inv_dt();
let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
let rhs_wo_bias = joint_velocity[dof_id];
let dof_j_id = *j_id + dof_id + link.assembly_id;
jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
jacobians[dof_j_id] = 1.0;
jacobians[dof_j_id + ndofs] = 1.0;
multibody
.inv_augmented_mass()
.solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
let impulse_bounds = [
min_enabled as u32 as Real * -Real::MAX,
max_enabled as u32 as Real * Real::MAX,
];
let constraint = JointGenericVelocityGroundConstraint {
mj_lambda2: multibody.solver_id,
ndofs2: ndofs,
j_id2: *j_id,
joint_id: usize::MAX,
impulse: 0.0,
impulse_bounds,
inv_lhs: crate::utils::inv(lhs),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id: WritebackId::Limit(dof_id),
};
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
*j_id += 2 * ndofs;
}
/// Initializes and generate the velocity constraints applicable to the multibody links attached
/// to this multibody_joint.
pub fn unit_joint_motor_constraint(
params: &IntegrationParameters,
multibody: &Multibody,
link: &MultibodyLink,
motor: &JointMotor,
curr_pos: Real,
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
) {
let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link);
let motor_params = motor.motor_params(params.dt);
let dof_j_id = *j_id + dof_id + link.assembly_id;
jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
jacobians[dof_j_id] = 1.0;
jacobians[dof_j_id + ndofs] = 1.0;
multibody
.inv_augmented_mass()
.solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 {
rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.stiffness;
}
if motor_params.damping != 0.0 {
let dvel = joint_velocity[dof_id];
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
}
let constraint = JointGenericVelocityGroundConstraint {
mj_lambda2: multibody.solver_id,
ndofs2: ndofs,
j_id2: *j_id,
joint_id: usize::MAX,
impulse: 0.0,
impulse_bounds,
inv_lhs: crate::utils::inv(lhs),
rhs: rhs_wo_bias,
rhs_wo_bias,
writeback_id: WritebackId::Limit(dof_id),
};
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
*j_id += 2 * ndofs;
}

289
src/dynamics/joint/prismatic_joint.rs
View File

@@ -1,250 +1,91 @@
use crate::dynamics::SpringModel;
use crate::math::{Isometry, Point, Real, Vector, DIM};
use crate::utils::WBasis;
use na::Unit;
#[cfg(feature = "dim2")]
use na::Vector2;
#[cfg(feature = "dim3")]
use na::Vector5;
use crate::dynamics::joint::{JointAxesMask, JointData};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real, UnitVector};
#[derive(Copy, Clone, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A joint that removes all relative motion between two bodies, except for the translations along one axis.
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct PrismaticJoint {
/// Where the prismatic joint is attached on the first body, expressed in the local space of the first attached body.
pub local_anchor1: Point<Real>,
/// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body.
pub local_anchor2: Point<Real>,
pub(crate) local_axis1: Unit<Vector<Real>>,
pub(crate) local_axis2: Unit<Vector<Real>>,
pub(crate) basis1: [Vector<Real>; DIM - 1],
pub(crate) basis2: [Vector<Real>; DIM - 1],
/// The impulse applied by this joint on the first body.
///
/// The impulse applied to the second body is given by `-impulse`.
#[cfg(feature = "dim3")]
pub impulse: Vector5<Real>,
/// The impulse applied by this joint on the first body.
///
/// The impulse applied to the second body is given by `-impulse`.
#[cfg(feature = "dim2")]
pub impulse: Vector2<Real>,
/// Whether or not this joint should enforce translational limits along its axis.
pub limits_enabled: bool,
/// The min an max relative position of the attached bodies along this joint's axis.
pub limits: [Real; 2],
/// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis.
///
/// The impulse applied to the second body is given by `-impulse`.
pub limits_impulse: Real,
/// The target relative angular velocity the motor will attempt to reach.
pub motor_target_vel: Real,
/// The target relative angle along the joint axis the motor will attempt to reach.
pub motor_target_pos: Real,
/// The motor's stiffness.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_stiffness: Real,
/// The motor's damping.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_damping: Real,
/// The maximal impulse the motor is able to deliver.
pub motor_max_impulse: Real,
/// The angular impulse applied by the motor.
pub motor_impulse: Real,
/// The spring-like model used by the motor to reach the target velocity and .
pub motor_model: SpringModel,
data: JointData,
}
impl PrismaticJoint {
/// Creates a new prismatic joint with the given point of applications and axis, all expressed
/// in the local-space of the affected bodies.
#[cfg(feature = "dim2")]
pub fn new(
local_anchor1: Point<Real>,
local_axis1: Unit<Vector<Real>>,
local_anchor2: Point<Real>,
local_axis2: Unit<Vector<Real>>,
) -> Self {
Self {
local_anchor1,
local_anchor2,
local_axis1,
local_axis2,
basis1: local_axis1.orthonormal_basis(),
basis2: local_axis2.orthonormal_basis(),
impulse: na::zero(),
limits_enabled: false,
limits: [-Real::MAX, Real::MAX],
limits_impulse: 0.0,
motor_target_vel: 0.0,
motor_target_pos: 0.0,
motor_stiffness: 0.0,
motor_damping: 0.0,
motor_max_impulse: Real::MAX,
motor_impulse: 0.0,
motor_model: SpringModel::VelocityBased,
}
pub fn new(axis: UnitVector<Real>) -> Self {
#[cfg(feature = "dim2")]
let mask = JointAxesMask::Y | JointAxesMask::ANG_X;
#[cfg(feature = "dim3")]
let mask = JointAxesMask::Y
| JointAxesMask::Z
| JointAxesMask::ANG_X
| JointAxesMask::ANG_Y
| JointAxesMask::ANG_Z;
let data = JointData::default()
.lock_axes(mask)
.local_axis1(axis)
.local_axis2(axis);
Self { data }
}
/// Creates a new prismatic joint with the given point of applications and axis, all expressed
/// in the local-space of the affected bodies.
///
/// The local tangent are vector orthogonal to the local axis. It is used to compute a basis orthonormal
/// to the joint's axis. If this tangent is set to zero, te orthonormal basis will be automatically
/// computed arbitrarily.
#[cfg(feature = "dim3")]
pub fn new(
local_anchor1: Point<Real>,
local_axis1: Unit<Vector<Real>>,
local_tangent1: Vector<Real>,
local_anchor2: Point<Real>,
local_axis2: Unit<Vector<Real>>,
local_tangent2: Vector<Real>,
) -> Self {
let basis1 = if let Some(local_bitangent1) =
Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3)
{
[
local_bitangent1.cross(&local_axis1),
local_bitangent1.into_inner(),
]
} else {
local_axis1.orthonormal_basis()
};
let basis2 = if let Some(local_bitangent2) =
Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3)
{
[
local_bitangent2.cross(&local_axis2),
local_bitangent2.into_inner(),
]
} else {
local_axis2.orthonormal_basis()
};
Self {
local_anchor1,
local_anchor2,
local_axis1,
local_axis2,
basis1,
basis2,
impulse: na::zero(),
limits_enabled: false,
limits: [-Real::MAX, Real::MAX],
limits_impulse: 0.0,
motor_target_vel: 0.0,
motor_target_pos: 0.0,
motor_stiffness: 0.0,
motor_damping: 0.0,
motor_max_impulse: Real::MAX,
motor_impulse: 0.0,
motor_model: SpringModel::VelocityBased,
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1);
self
}
/// The local axis of this joint, expressed in the local-space of the first attached body.
pub fn local_axis1(&self) -> Unit<Vector<Real>> {
self.local_axis1
}
/// The local axis of this joint, expressed in the local-space of the second attached body.
pub fn local_axis2(&self) -> Unit<Vector<Real>> {
self.local_axis2
}
/// Can a SIMD constraint be used for resolving this joint?
pub fn supports_simd_constraints(&self) -> bool {
// SIMD revolute constraints don't support motors right now.
self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0)
}
// FIXME: precompute this?
#[cfg(feature = "dim2")]
pub(crate) fn local_frame1(&self) -> Isometry<Real> {
use na::{Matrix2, Rotation2, UnitComplex};
let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]);
let rotmat = Rotation2::from_matrix_unchecked(mat);
let rotation = UnitComplex::from_rotation_matrix(&rotmat);
let translation = self.local_anchor1.coords.into();
Isometry::from_parts(translation, rotation)
}
// FIXME: precompute this?
#[cfg(feature = "dim2")]
pub(crate) fn local_frame2(&self) -> Isometry<Real> {
use na::{Matrix2, Rotation2, UnitComplex};
let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]);
let rotmat = Rotation2::from_matrix_unchecked(mat);
let rotation = UnitComplex::from_rotation_matrix(&rotmat);
let translation = self.local_anchor2.coords.into();
Isometry::from_parts(translation, rotation)
}
// FIXME: precompute this?
#[cfg(feature = "dim3")]
pub(crate) fn local_frame1(&self) -> Isometry<Real> {
use na::{Matrix3, Rotation3, UnitQuaternion};
let mat = Matrix3::from_columns(&[
self.local_axis1.into_inner(),
self.basis1[0],
self.basis1[1],
]);
let rotmat = Rotation3::from_matrix_unchecked(mat);
let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
let translation = self.local_anchor1.coords.into();
Isometry::from_parts(translation, rotation)
}
// FIXME: precompute this?
#[cfg(feature = "dim3")]
pub(crate) fn local_frame2(&self) -> Isometry<Real> {
use na::{Matrix3, Rotation3, UnitQuaternion};
let mat = Matrix3::from_columns(&[
self.local_axis2.into_inner(),
self.basis2[0],
self.basis2[1],
]);
let rotmat = Rotation3::from_matrix_unchecked(mat);
let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
let translation = self.local_anchor2.coords.into();
Isometry::from_parts(translation, rotation)
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn configure_motor_model(&mut self, model: SpringModel) {
self.motor_model = model;
pub fn motor_model(mut self, model: MotorModel) -> Self {
self.data = self.data.motor_model(JointAxis::X, model);
self
}
/// Sets the target velocity this motor needs to reach.
pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
self.data = self.data.motor_velocity(JointAxis::X, target_vel, factor);
self
}
/// Sets the target position this motor needs to reach.
pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
self.configure_motor(target_pos, 0.0, stiffness, damping)
/// Sets the target angle this motor needs to reach.
pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
self.data = self
.data
.motor_position(JointAxis::X, target_pos, stiffness, damping);
self
}
/// Configure both the target position and target velocity of the motor.
pub fn configure_motor(
&mut self,
/// Configure both the target angle and target velocity of the motor.
pub fn motor_axis(
mut self,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) {
self.motor_target_vel = target_vel;
self.motor_target_pos = target_pos;
self.motor_stiffness = stiffness;
self.motor_damping = damping;
) -> Self {
self.data = self
.data
.motor_axis(JointAxis::X, target_pos, target_vel, stiffness, damping);
self
}
pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self {
self.data = self.data.motor_max_impulse(JointAxis::X, max_impulse);
self
}
#[must_use]
pub fn limit_axis(mut self, limits: [Real; 2]) -> Self {
self.data = self.data.limit_axis(JointAxis::X, limits);
self
}
}
impl Into<JointData> for PrismaticJoint {
fn into(self) -> JointData {
self.data
}
}

220
src/dynamics/joint/revolute_joint.rs
View File

@@ -1,174 +1,106 @@
use crate::dynamics::SpringModel;
use crate::math::{Isometry, Point, Real, Vector};
use crate::utils::WBasis;
use na::{RealField, Unit, Vector5};
use crate::dynamics::joint::{JointAxesMask, JointData};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real};
#[cfg(feature = "dim3")]
use crate::math::UnitVector;
#[derive(Copy, Clone, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A joint that removes all relative motion between two bodies, except for the rotations along one axis.
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct RevoluteJoint {
/// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body.
pub local_anchor1: Point<Real>,
/// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body.
pub local_anchor2: Point<Real>,
/// The rotation axis of this revolute joint expressed in the local space of the first attached body.
pub local_axis1: Unit<Vector<Real>>,
/// The rotation axis of this revolute joint expressed in the local space of the second attached body.
pub local_axis2: Unit<Vector<Real>>,
/// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
pub basis1: [Vector<Real>; 2],
/// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
pub basis2: [Vector<Real>; 2],
/// The impulse applied by this joint on the first body.
///
/// The impulse applied to the second body is given by `-impulse`.
pub impulse: Vector5<Real>,
/// Whether or not this joint should enforce translational limits along its axis.
pub limits_enabled: bool,
/// The min an max relative position of the attached bodies along this joint's axis.
pub limits: [Real; 2],
/// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis.
///
/// The impulse applied to the second body is given by `-impulse`.
pub limits_impulse: Real,
/// The target relative angular velocity the motor will attempt to reach.
pub motor_target_vel: Real,
/// The target relative angle along the joint axis the motor will attempt to reach.
pub motor_target_pos: Real,
/// The motor's stiffness.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_stiffness: Real,
/// The motor's damping.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_damping: Real,
/// The maximal impulse the motor is able to deliver.
pub motor_max_impulse: Real,
/// The angular impulse applied by the motor.
pub motor_impulse: Real,
/// The spring-like model used by the motor to reach the target velocity and .
pub motor_model: SpringModel,
// Used to handle cases where the position target ends up being more than pi radians away.
pub(crate) motor_last_angle: Real,
// The angular impulse expressed in world-space.
pub(crate) world_ang_impulse: Vector<Real>,
// The world-space orientation of the free axis of the first attached body.
pub(crate) prev_axis1: Vector<Real>,
data: JointData,
}
impl RevoluteJoint {
/// Creates a new revolute joint with the given point of applications and axis, all expressed
/// in the local-space of the affected bodies.
pub fn new(
local_anchor1: Point<Real>,
local_axis1: Unit<Vector<Real>>,
local_anchor2: Point<Real>,
local_axis2: Unit<Vector<Real>>,
) -> Self {
Self {
local_anchor1,
local_anchor2,
local_axis1,
local_axis2,
basis1: local_axis1.orthonormal_basis(),
basis2: local_axis2.orthonormal_basis(),
impulse: na::zero(),
world_ang_impulse: na::zero(),
limits_enabled: false,
limits: [-Real::MAX, Real::MAX],
limits_impulse: 0.0,
motor_target_vel: 0.0,
motor_target_pos: 0.0,
motor_stiffness: 0.0,
motor_damping: 0.0,
motor_max_impulse: Real::MAX,
motor_impulse: 0.0,
prev_axis1: *local_axis1,
motor_model: SpringModel::default(),
motor_last_angle: 0.0,
}
#[cfg(feature = "dim2")]
pub fn new() -> Self {
let mask = JointAxesMask::X | JointAxesMask::Y;
let data = JointData::default().lock_axes(mask);
Self { data }
}
/// Can a SIMD constraint be used for resolving this joint?
pub fn supports_simd_constraints(&self) -> bool {
// SIMD revolute constraints don't support motors and limits right now.
!self.limits_enabled
&& (self.motor_max_impulse == 0.0
|| (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
#[cfg(feature = "dim3")]
pub fn new(axis: UnitVector<Real>) -> Self {
let mask = JointAxesMask::X
| JointAxesMask::Y
| JointAxesMask::Z
| JointAxesMask::ANG_Y
| JointAxesMask::ANG_Z;
let data = JointData::default()
.lock_axes(mask)
.local_axis1(axis)
.local_axis2(axis);
Self { data }
}
pub fn data(&self) -> &JointData {
&self.data
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn configure_motor_model(&mut self, model: SpringModel) {
self.motor_model = model;
pub fn motor_model(mut self, model: MotorModel) -> Self {
self.data = self.data.motor_model(JointAxis::AngX, model);
self
}
/// Sets the target velocity this motor needs to reach.
pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
self.data = self
.data
.motor_velocity(JointAxis::AngX, target_vel, factor);
self
}
/// Sets the target angle this motor needs to reach.
pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
self.configure_motor(target_pos, 0.0, stiffness, damping)
pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
self.data = self
.data
.motor_position(JointAxis::AngX, target_pos, stiffness, damping);
self
}
/// Configure both the target angle and target velocity of the motor.
pub fn configure_motor(
&mut self,
pub fn motor_axis(
mut self,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) {
self.motor_target_vel = target_vel;
self.motor_target_pos = target_pos;
self.motor_stiffness = stiffness;
self.motor_damping = damping;
) -> Self {
self.data =
self.data
.motor_axis(JointAxis::AngX, target_pos, target_vel, stiffness, damping);
self
}
/// Estimates the current position of the motor angle.
pub fn estimate_motor_angle(
&self,
body_pos1: &Isometry<Real>,
body_pos2: &Isometry<Real>,
) -> Real {
Self::estimate_motor_angle_from_params(
&(body_pos1 * self.local_axis1),
&(body_pos1 * self.basis1[0]),
&(body_pos2 * self.basis2[0]),
self.motor_last_angle,
)
pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self {
self.data = self.data.motor_max_impulse(JointAxis::AngX, max_impulse);
self
}
/// Estimates the current position of the motor angle given the joint parameters.
pub fn estimate_motor_angle_from_params(
axis1: &Unit<Vector<Real>>,
tangent1: &Vector<Real>,
tangent2: &Vector<Real>,
motor_last_angle: Real,
) -> Real {
let last_angle_cycles = (motor_last_angle / Real::two_pi()).trunc() * Real::two_pi();
// Measure the position between 0 and 2-pi
let new_angle = if tangent1.cross(&tangent2).dot(&axis1) < 0.0 {
Real::two_pi() - tangent1.angle(&tangent2)
} else {
tangent1.angle(&tangent2)
};
// The last angle between 0 and 2-pi
let last_angle_zero_two_pi = motor_last_angle - last_angle_cycles;
// Figure out the smallest angle differance.
let mut angle_diff = new_angle - last_angle_zero_two_pi;
if angle_diff > Real::pi() {
angle_diff -= Real::two_pi()
} else if angle_diff < -Real::pi() {
angle_diff += Real::two_pi()
}
motor_last_angle + angle_diff
#[must_use]
pub fn limit_axis(mut self, limits: [Real; 2]) -> Self {
self.data = self.data.limit_axis(JointAxis::AngX, limits);
self
}
}
impl Into<JointData> for RevoluteJoint {
fn into(self) -> JointData {
self.data
}
}

91
src/dynamics/joint/spherical_joint.rs Normal file
View File

@@ -0,0 +1,91 @@
use crate::dynamics::joint::{JointAxesMask, JointData};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct SphericalJoint {
data: JointData,
}
impl SphericalJoint {
pub fn new() -> Self {
let data =
JointData::default().lock_axes(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z);
Self { data }
}
pub fn data(&self) -> &JointData {
&self.data
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
self.data = self.data.motor_model(axis, model);
self
}
/// Sets the target velocity this motor needs to reach.
pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
self.data = self.data.motor_velocity(axis, target_vel, factor);
self
}
/// Sets the target angle this motor needs to reach.
pub fn motor_position(
mut self,
axis: JointAxis,
target_pos: Real,
stiffness: Real,
damping: Real,
) -> Self {
self.data = self
.data
.motor_position(axis, target_pos, stiffness, damping);
self
}
/// Configure both the target angle and target velocity of the motor.
pub fn motor_axis(
mut self,
axis: JointAxis,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) -> Self {
self.data = self
.data
.motor_axis(axis, target_pos, target_vel, stiffness, damping);
self
}
pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self {
self.data = self.data.motor_max_impulse(axis, max_impulse);
self
}
#[must_use]
pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
self.data = self.data.limit_axis(axis, limits);
self
}
}
impl Into<JointData> for SphericalJoint {
fn into(self) -> JointData {
self.data
}
}