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

Joint API and joint motors improvements

Browse Source
This commit is contained in:
Sébastien Crozet
2022-02-20 12:55:00 +01:00
committed by Sébastien Crozet
parent e740493b98
commit fb20d72ee2
108 changed files with 2650 additions and 1854 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,10 +1,11 @@
use crate::dynamics::{JointAxesMask, JointData};
use crate::dynamics::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::math::{Isometry, Point, Real};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
pub struct FixedJoint {
data: JointData,
data: GenericJoint,
}
impl Default for FixedJoint {
@@ -14,48 +15,100 @@ impl Default for FixedJoint {
}
impl FixedJoint {
#[must_use]
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);
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build();
Self { data }
}
#[must_use]
pub fn local_frame1(&self) -> &Isometry<Real> {
&self.data.local_frame1
}
pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.data.set_local_frame1(local_frame);
self
}
#[must_use]
pub fn local_frame2(&self) -> &Isometry<Real> {
&self.data.local_frame2
}
pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.data.set_local_frame2(local_frame);
self
}
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
}
impl Into<GenericJoint> for FixedJoint {
fn into(self) -> GenericJoint {
self.data
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq, Default)]
pub struct FixedJointBuilder(FixedJoint);
impl FixedJointBuilder {
pub fn new() -> Self {
Self(FixedJoint::new())
}
#[must_use]
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
self.data = self.data.local_frame1(local_frame);
self.0.set_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.0.set_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.0.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2);
self.0.set_local_anchor2(anchor2);
self
}
#[must_use]
pub fn build(self) -> FixedJoint {
self.0
}
}
impl Into<JointData> for FixedJoint {
fn into(self) -> JointData {
self.data
impl Into<GenericJoint> for FixedJointBuilder {
fn into(self) -> GenericJoint {
self.0.into()
}
}

501
src/dynamics/joint/generic_joint.rs Normal file
View File

@@ -0,0 +1,501 @@
use na::SimdRealField;
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint};
use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM};
use crate::utils::WBasis;
#[cfg(feature = "dim3")]
use crate::dynamics::SphericalJoint;
#[cfg(feature = "dim3")]
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct JointAxesMask: u8 {
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;
const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
const LOCKED_SPHERICAL_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits;
const FREE_REVOLUTE_AXES = Self::ANG_X.bits;
const FREE_PRISMATIC_AXES = Self::X.bits;
const FREE_FIXED_AXES = 0;
const FREE_SPHERICAL_AXES = Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
}
}
#[cfg(feature = "dim2")]
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct JointAxesMask: u8 {
const X = 1 << 0;
const Y = 1 << 1;
const ANG_X = 1 << 2;
const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits;
const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::ANG_X.bits;
const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::ANG_X.bits;
const FREE_REVOLUTE_AXES = Self::ANG_X.bits;
const FREE_PRISMATIC_AXES = Self::X.bits;
const FREE_FIXED_AXES = 0;
}
}
#[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<N> {
pub min: N,
pub max: N,
pub impulse: N,
}
impl<N: SimdRealField<Element = Real>> Default for JointLimits<N> {
fn default() -> Self {
Self {
min: -N::splat(Real::MAX),
max: N::splat(Real::MAX),
impulse: N::splat(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_force: 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_force: Real::MAX,
impulse: 0.0,
model: MotorModel::AccelerationBased, // VelocityBased,
}
}
}
impl JointMotor {
pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters<Real> {
let (erp_inv_dt, cfm_coeff, cfm_gain) =
self.model
.combine_coefficients(dt, self.stiffness, self.damping);
MotorParameters {
erp_inv_dt,
cfm_coeff,
cfm_gain,
// keep_lhs,
target_pos: self.target_pos,
target_vel: self.target_vel,
max_impulse: self.max_force * dt,
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct GenericJoint {
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<Real>; SPATIAL_DIM],
pub motors: [JointMotor; SPATIAL_DIM],
}
impl Default for GenericJoint {
fn default() -> Self {
Self {
local_frame1: Isometry::identity(),
local_frame2: Isometry::identity(),
locked_axes: JointAxesMask::empty(),
limit_axes: JointAxesMask::empty(),
motor_axes: JointAxesMask::empty(),
limits: [JointLimits::default(); SPATIAL_DIM],
motors: [JointMotor::default(); SPATIAL_DIM],
}
}
}
impl GenericJoint {
#[must_use]
pub fn new(locked_axes: JointAxesMask) -> Self {
*Self::default().lock_axes(locked_axes)
}
/// Can this joint use SIMD-accelerated constraint formulations?
pub(crate) fn supports_simd_constraints(&self) -> bool {
self.limit_axes.is_empty() && self.motor_axes.is_empty()
}
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)
}
}
pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self {
self.locked_axes |= axes;
self
}
pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.local_frame1 = local_frame;
self
}
pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.local_frame2 = local_frame;
self
}
#[must_use]
pub fn local_axis1(&self) -> UnitVector<Real> {
self.local_frame1 * Vector::x_axis()
}
pub fn set_local_axis1(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
self
}
#[must_use]
pub fn local_axis2(&self) -> UnitVector<Real> {
self.local_frame2 * Vector::x_axis()
}
pub fn set_local_axis2(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
self
}
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.local_frame1.translation.vector.into()
}
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.local_frame1.translation.vector = anchor1.coords;
self
}
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.local_frame2.translation.vector.into()
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.local_frame2.translation.vector = anchor2.coords;
self
}
#[must_use]
pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
let i = axis as usize;
if self.limit_axes.contains(axis.into()) {
Some(&self.limits[i])
} else {
None
}
}
pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
let i = axis as usize;
self.limit_axes |= axis.into();
self.limits[i].min = limits[0];
self.limits[i].max = limits[1];
self
}
#[must_use]
pub fn motor_model(&self, axis: JointAxis) -> Option<MotorModel> {
let i = axis as usize;
if self.motor_axes.contains(axis.into()) {
Some(self.motors[i].model)
} else {
None
}
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self {
self.motors[axis as usize].model = model;
self
}
/// Sets the target velocity this motor needs to reach.
pub fn set_motor_velocity(
&mut self,
axis: JointAxis,
target_vel: Real,
factor: Real,
) -> &mut Self {
self.set_motor(
axis,
self.motors[axis as usize].target_pos,
target_vel,
0.0,
factor,
)
}
/// Sets the target angle this motor needs to reach.
pub fn set_motor_position(
&mut self,
axis: JointAxis,
target_pos: Real,
stiffness: Real,
damping: Real,
) -> &mut Self {
self.set_motor(axis, target_pos, 0.0, stiffness, damping)
}
pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self {
self.motors[axis as usize].max_force = max_force;
self
}
#[must_use]
pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
let i = axis as usize;
if self.motor_axes.contains(axis.into()) {
Some(&self.motors[i])
} else {
None
}
}
/// Configure both the target angle and target velocity of the motor.
pub fn set_motor(
&mut self,
axis: JointAxis,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) -> &mut 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
}
}
macro_rules! joint_conversion_methods(
($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => {
#[must_use]
pub fn $as_joint(&self) -> Option<&$Joint> {
if self.locked_axes == $axes {
// SAFETY: this is OK because the target joint type is
// a `repr(transparent)` newtype of `Joint`.
Some(unsafe { std::mem::transmute(self) })
} else {
None
}
}
#[must_use]
pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> {
if self.locked_axes == $axes {
// SAFETY: this is OK because the target joint type is
// a `repr(transparent)` newtype of `Joint`.
Some(unsafe { std::mem::transmute(self) })
} else {
None
}
}
}
);
impl GenericJoint {
joint_conversion_methods!(
as_revolute,
as_revolute_mut,
RevoluteJoint,
JointAxesMask::LOCKED_REVOLUTE_AXES
);
joint_conversion_methods!(
as_fixed,
as_fixed_mut,
FixedJoint,
JointAxesMask::LOCKED_FIXED_AXES
);
joint_conversion_methods!(
as_prismatic,
as_prismatic_mut,
PrismaticJoint,
JointAxesMask::LOCKED_PRISMATIC_AXES
);
#[cfg(feature = "dim3")]
joint_conversion_methods!(
as_spherical,
as_spherical_mut,
SphericalJoint,
JointAxesMask::LOCKED_SPHERICAL_AXES
);
}
#[derive(Copy, Clone, Debug)]
pub struct GenericJointBuilder(GenericJoint);
impl GenericJointBuilder {
#[must_use]
pub fn new(locked_axes: JointAxesMask) -> Self {
Self(GenericJoint::new(locked_axes))
}
#[must_use]
pub fn lock_axes(mut self, axes: JointAxesMask) -> Self {
self.0.lock_axes(axes);
self
}
#[must_use]
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
self.0.set_local_frame1(local_frame);
self
}
#[must_use]
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
self.0.set_local_frame2(local_frame);
self
}
#[must_use]
pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
self.0.set_local_axis1(local_axis);
self
}
#[must_use]
pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
self.0.set_local_axis2(local_axis);
self
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.0.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.0.set_local_anchor2(anchor2);
self
}
#[must_use]
pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
self.0.set_limits(axis, limits);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
#[must_use]
pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
self.0.set_motor_model(axis, model);
self
}
/// Sets the target velocity this motor needs to reach.
#[must_use]
pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
self.0.set_motor_velocity(axis, target_vel, factor);
self
}
/// Sets the target angle this motor needs to reach.
#[must_use]
pub fn motor_position(
mut self,
axis: JointAxis,
target_pos: Real,
stiffness: Real,
damping: Real,
) -> Self {
self.0
.set_motor_position(axis, target_pos, stiffness, damping);
self
}
/// Configure both the target angle and target velocity of the motor.
#[must_use]
pub fn set_motor(
mut self,
axis: JointAxis,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) -> Self {
self.0
.set_motor(axis, target_pos, target_vel, stiffness, damping);
self
}
#[must_use]
pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
self.0.set_motor_max_force(axis, max_force);
self
}
#[must_use]
pub fn build(self) -> GenericJoint {
self.0
}
}

4
src/dynamics/joint/impulse_joint/impulse_joint.rs
View File

@@ -1,4 +1,4 @@
use crate::dynamics::{ImpulseJointHandle, JointData, RigidBodyHandle};
use crate::dynamics::{GenericJoint, ImpulseJointHandle, RigidBodyHandle};
use crate::math::{Real, SpacialVector};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -10,7 +10,7 @@ pub struct ImpulseJoint {
/// Handle to the second body attached to this joint.
pub body2: RigidBodyHandle,
pub data: JointData,
pub data: GenericJoint,
pub impulses: SpacialVector<Real>,
// A joint needs to know its handle to simplify its removal.

4
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -3,8 +3,8 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio
use crate::data::arena::Arena;
use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut};
use crate::dynamics::{GenericJoint, RigidBodyHandle};
use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
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.
@@ -177,7 +177,7 @@ impl ImpulseJointSet {
&mut self,
body1: RigidBodyHandle,
body2: RigidBodyHandle,
data: impl Into<JointData>,
data: impl Into<GenericJoint>,
) -> ImpulseJointHandle {
let data = data.into();
let handle = self.joint_ids.insert(0.into());

275
src/dynamics/joint/joint_data.rs
View File

@@ -1,275 +0,0 @@
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.
#[must_use]
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.
#[must_use]
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.
#[must_use]
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.
#[must_use]
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
}
#[must_use]
pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self {
self.motors[axis as usize].max_impulse = max_impulse;
self
}
}

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

@@ -1,17 +1,17 @@
pub use self::fixed_joint::FixedJoint;
pub use self::fixed_joint::*;
pub use self::impulse_joint::*;
pub use self::joint_data::*;
pub use self::generic_joint::*;
pub use self::motor_model::MotorModel;
pub use self::multibody_joint::*;
pub use self::prismatic_joint::PrismaticJoint;
pub use self::revolute_joint::RevoluteJoint;
pub use self::prismatic_joint::*;
pub use self::revolute_joint::*;
#[cfg(feature = "dim3")]
pub use self::spherical_joint::SphericalJoint;
pub use self::spherical_joint::*;
mod fixed_joint;
mod impulse_joint;
mod joint_data;
mod generic_joint;
mod motor_model;
mod multibody_joint;
mod prismatic_joint;

49
src/dynamics/joint/motor_model.rs
View File

@@ -5,57 +5,40 @@ use crate::math::Real;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub enum MotorModel {
/// The solved spring-like equation is:
/// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
///
/// Here the `stiffness` is the ratio of position error to be solved at each timestep (like
/// a velocity-based ERP), and the `damping` is the ratio of velocity error to be solved at
/// each timestep.
VelocityBased,
/// The solved spring-like equation is:
/// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
/// `acceleration = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
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 = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
ForceBased,
}
impl Default for MotorModel {
fn default() -> Self {
MotorModel::VelocityBased
MotorModel::AccelerationBased
}
}
impl MotorModel {
/// Combines the coefficients used for solving the spring equation.
///
/// 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`.
/// - `new_inv_lhs = gamma * if keep_inv_lhs { inv_lhs } else { 1.0 }`.
/// Note that the returned `gamma` will be zero if both `stiffness` and `damping` are zero.
/// Returns the coefficients (erp_inv_dt, cfm_coeff, cfm_gain).
pub fn combine_coefficients(
self,
dt: Real,
stiffness: Real,
damping: Real,
) -> (Real, Real, Real, bool) {
) -> (Real, Real, Real) {
match self {
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.
// Maybe I got the formulation wrong, so let's keep it to 1.0 for now,
// and get back to this later.
// let gamma = effective_stiffness * dt + effective_damping;
(effective_stiffness, effective_damping, 1.0, true)
} // 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)
// }
let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
let cfm_coeff = crate::utils::inv(dt * dt * stiffness + dt * damping);
(erp_inv_dt, cfm_coeff, 0.0)
}
MotorModel::ForceBased => {
let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
let cfm_gain = crate::utils::inv(dt * dt * stiffness + dt * damping);
(erp_inv_dt, 0.0, cfm_gain)
}
}
}
}

54
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -1,6 +1,6 @@
use crate::dynamics::solver::AnyJointVelocityConstraint;
use crate::dynamics::{
joint, FixedJoint, IntegrationParameters, JointData, Multibody, MultibodyLink,
joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
RigidBodyVelocity,
};
use crate::math::{
@@ -14,13 +14,13 @@ use na::{UnitQuaternion, Vector3};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug)]
pub struct MultibodyJoint {
pub data: JointData,
pub data: GenericJoint,
pub(crate) coords: SpacialVector<Real>,
pub(crate) joint_rot: Rotation<Real>,
}
impl MultibodyJoint {
pub fn new(data: JointData) -> Self {
pub fn new(data: GenericJoint) -> Self {
Self {
data,
coords: na::zero(),
@@ -29,13 +29,13 @@ impl MultibodyJoint {
}
pub(crate) fn free(pos: Isometry<Real>) -> Self {
let mut result = Self::new(JointData::default());
let mut result = Self::new(GenericJoint::default());
result.set_free_pos(pos);
result
}
pub(crate) fn fixed(pos: Isometry<Real>) -> Self {
Self::new(FixedJoint::new().local_frame1(pos).into())
Self::new(FixedJointBuilder::new().local_frame1(pos).build().into())
}
pub(crate) fn set_free_pos(&mut self, pos: Isometry<Real>) {
@@ -263,19 +263,11 @@ impl MultibodyJoint {
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,
);
}
let limits = if (limit_bits & (1 << i)) != 0 {
Some([self.data.limits[i].min, self.data.limits[i].max])
} else {
None
};
if (motor_bits & (1 << i)) != 0 {
joint::unit_joint_motor_constraint(
@@ -284,6 +276,21 @@ impl MultibodyJoint {
link,
&self.data.motors[i],
self.coords[i],
limits,
dof_id + curr_free_dof,
j_id,
jacobians,
constraints,
);
}
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,
@@ -310,19 +317,23 @@ impl MultibodyJoint {
// 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 {
let limits = if (limit_bits & (1 << i)) != 0 {
let limits = [self.data.limits[i].min, self.data.limits[i].max];
joint::unit_joint_limit_constraint(
params,
multibody,
link,
[self.data.limits[i].min, self.data.limits[i].max],
limits,
self.coords[i],
dof_id + curr_free_dof,
j_id,
jacobians,
constraints,
);
}
Some(limits)
} else {
None
};
if (motor_bits & (1 << i)) != 0 {
joint::unit_joint_motor_constraint(
@@ -331,6 +342,7 @@ impl MultibodyJoint {
link,
&self.data.motors[i],
self.coords[i],
limits,
dof_id + curr_free_dof,
j_id,
jacobians,

4
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -1,7 +1,7 @@
use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index};
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::{
IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
RigidBodyIds, RigidBodyType,
};
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
@@ -112,7 +112,7 @@ impl MultibodyJointSet {
&mut self,
body1: RigidBodyHandle,
body2: RigidBodyHandle,
data: impl Into<JointData>,
data: impl Into<GenericJoint>,
) -> Option<MultibodyJointHandle> {
let data = data.into();
let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {

27
src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
View File

@@ -26,7 +26,8 @@ pub fn unit_joint_limit_constraint(
let min_enabled = curr_pos < limits[0];
let max_enabled = limits[1] < curr_pos;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let cfm_coeff = params.joint_cfm_coeff();
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];
@@ -54,6 +55,8 @@ pub fn unit_joint_limit_constraint(
inv_lhs: crate::utils::inv(lhs),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
cfm_coeff,
cfm_gain: 0.0,
writeback_id: WritebackId::Limit(dof_id),
};
@@ -71,11 +74,13 @@ pub fn unit_joint_motor_constraint(
link: &MultibodyLink,
motor: &JointMotor,
curr_pos: Real,
limits: Option<[Real; 2]>,
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
) {
let inv_dt = params.inv_dt();
let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link);
@@ -93,14 +98,20 @@ pub fn unit_joint_motor_constraint(
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.erp_inv_dt != 0.0 {
rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.erp_inv_dt;
}
if motor_params.damping != 0.0 {
let dvel = joint_velocity[dof_id];
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
}
let mut target_vel = motor_params.target_vel;
if let Some(limits) = limits {
target_vel = target_vel.clamp(
(limits[0] - curr_pos) * inv_dt,
(limits[1] - curr_pos) * inv_dt,
);
};
let dvel = joint_velocity[dof_id];
rhs_wo_bias += dvel - target_vel;
let constraint = JointGenericVelocityGroundConstraint {
mj_lambda2: multibody.solver_id,
@@ -109,6 +120,8 @@ pub fn unit_joint_motor_constraint(
joint_id: usize::MAX,
impulse: 0.0,
impulse_bounds,
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
inv_lhs: crate::utils::inv(lhs),
rhs: rhs_wo_bias,
rhs_wo_bias,

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

@@ -1,91 +1,215 @@
use crate::dynamics::joint::{JointAxesMask, JointData};
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real, UnitVector};
use super::{JointLimits, JointMotor};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
pub struct PrismaticJoint {
data: JointData,
data: GenericJoint,
}
impl PrismaticJoint {
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)
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES)
.local_axis1(axis)
.local_axis2(axis);
.local_axis2(axis)
.build();
Self { data }
}
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
#[must_use]
pub fn local_axis1(&self) -> UnitVector<Real> {
self.data.local_axis1()
}
pub fn set_local_axis1(&mut self, axis1: UnitVector<Real>) -> &mut Self {
self.data.set_local_axis1(axis1);
self
}
#[must_use]
pub fn local_axis2(&self) -> UnitVector<Real> {
self.data.local_axis2()
}
pub fn set_local_axis2(&mut self, axis2: UnitVector<Real>) -> &mut Self {
self.data.set_local_axis2(axis2);
self
}
#[must_use]
pub fn motor(&self) -> Option<&JointMotor> {
self.data.motor(JointAxis::X)
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self {
self.data.set_motor_model(JointAxis::X, model);
self
}
/// Sets the target velocity this motor needs to reach.
pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self {
self.data
.set_motor_velocity(JointAxis::X, target_vel, factor);
self
}
/// Sets the target angle this motor needs to reach.
pub fn set_motor_position(
&mut self,
target_pos: Real,
stiffness: Real,
damping: Real,
) -> &mut Self {
self.data
.set_motor_position(JointAxis::X, target_pos, stiffness, damping);
self
}
/// Configure both the target angle and target velocity of the motor.
pub fn set_motor(
&mut self,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) -> &mut Self {
self.data
.set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping);
self
}
pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
self.data.set_motor_max_force(JointAxis::X, max_force);
self
}
#[must_use]
pub fn limits(&self) -> Option<&JointLimits<Real>> {
self.data.limits(JointAxis::X)
}
pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
self.data.set_limits(JointAxis::X, limits);
self
}
}
impl Into<GenericJoint> for PrismaticJoint {
fn into(self) -> GenericJoint {
self.data
}
}
pub struct PrismaticJointBuilder(PrismaticJoint);
impl PrismaticJointBuilder {
pub fn new(axis: UnitVector<Real>) -> Self {
Self(PrismaticJoint::new(axis))
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1);
self.0.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2);
self.0.set_local_anchor2(anchor2);
self
}
#[must_use]
pub fn local_axis1(mut self, axis1: UnitVector<Real>) -> Self {
self.0.set_local_axis1(axis1);
self
}
#[must_use]
pub fn local_axis2(mut self, axis2: UnitVector<Real>) -> Self {
self.0.set_local_axis2(axis2);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
#[must_use]
pub fn motor_model(mut self, model: MotorModel) -> Self {
self.data = self.data.motor_model(JointAxis::X, model);
self.0.set_motor_model(model);
self
}
/// Sets the target velocity this motor needs to reach.
#[must_use]
pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
self.data = self.data.motor_velocity(JointAxis::X, target_vel, factor);
self.0.set_motor_velocity(target_vel, factor);
self
}
/// Sets the target angle this motor needs to reach.
#[must_use]
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.0.set_motor_position(target_pos, stiffness, damping);
self
}
/// Configure both the target angle and target velocity of the motor.
pub fn motor_axis(
#[must_use]
pub fn set_motor(
mut self,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) -> 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.0.set_motor(target_pos, target_vel, stiffness, damping);
self
}
#[must_use]
pub fn limit_axis(mut self, limits: [Real; 2]) -> Self {
self.data = self.data.limit_axis(JointAxis::X, limits);
pub fn motor_max_force(mut self, max_force: Real) -> Self {
self.0.set_motor_max_force(max_force);
self
}
#[must_use]
pub fn limits(mut self, limits: [Real; 2]) -> Self {
self.0.set_limits(limits);
self
}
#[must_use]
pub fn build(self) -> PrismaticJoint {
self.0
}
}
impl Into<JointData> for PrismaticJoint {
fn into(self) -> JointData {
self.data
impl Into<GenericJoint> for PrismaticJointBuilder {
fn into(self) -> GenericJoint {
self.0.into()
}
}

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

@@ -1,5 +1,5 @@
use crate::dynamics::joint::{JointAxesMask, JointData};
use crate::dynamics::{JointAxis, MotorModel};
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, JointLimits, JointMotor, MotorModel};
use crate::math::{Point, Real};
#[cfg(feature = "dim3")]
@@ -7,100 +7,197 @@ use crate::math::UnitVector;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
pub struct RevoluteJoint {
data: JointData,
data: GenericJoint,
}
impl RevoluteJoint {
#[cfg(feature = "dim2")]
pub fn new() -> Self {
let mask = JointAxesMask::X | JointAxesMask::Y;
let data = JointData::default().lock_axes(mask);
Self { data }
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES);
Self { data: data.build() }
}
#[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)
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES)
.local_axis1(axis)
.local_axis2(axis);
.local_axis2(axis)
.build();
Self { data }
}
pub fn data(&self) -> &JointData {
pub fn data(&self) -> &GenericJoint {
&self.data
}
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
#[must_use]
pub fn motor(&self) -> Option<&JointMotor> {
self.data.motor(JointAxis::AngX)
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self {
self.data.set_motor_model(JointAxis::AngX, model);
self
}
/// Sets the target velocity this motor needs to reach.
pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self {
self.data
.set_motor_velocity(JointAxis::AngX, target_vel, factor);
self
}
/// Sets the target angle this motor needs to reach.
pub fn set_motor_position(
&mut self,
target_pos: Real,
stiffness: Real,
damping: Real,
) -> &mut Self {
self.data
.set_motor_position(JointAxis::AngX, target_pos, stiffness, damping);
self
}
/// Configure both the target angle and target velocity of the motor.
pub fn set_motor(
&mut self,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) -> &mut Self {
self.data
.set_motor(JointAxis::AngX, target_pos, target_vel, stiffness, damping);
self
}
pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
self.data.set_motor_max_force(JointAxis::AngX, max_force);
self
}
#[must_use]
pub fn limits(&self) -> Option<&JointLimits<Real>> {
self.data.limits(JointAxis::AngX)
}
pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
self.data.set_limits(JointAxis::AngX, limits);
self
}
}
impl Into<GenericJoint> for RevoluteJoint {
fn into(self) -> GenericJoint {
self.data
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct RevoluteJointBuilder(RevoluteJoint);
impl RevoluteJointBuilder {
#[cfg(feature = "dim2")]
pub fn new() -> Self {
Self(RevoluteJoint::new())
}
#[cfg(feature = "dim3")]
pub fn new(axis: UnitVector<Real>) -> Self {
Self(RevoluteJoint::new(axis))
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1);
self.0.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2);
self.0.set_local_anchor2(anchor2);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
#[must_use]
pub fn motor_model(mut self, model: MotorModel) -> Self {
self.data = self.data.motor_model(JointAxis::AngX, model);
self.0.set_motor_model(model);
self
}
/// Sets the target velocity this motor needs to reach.
#[must_use]
pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
self.data = self
.data
.motor_velocity(JointAxis::AngX, target_vel, factor);
self.0.set_motor_velocity(target_vel, factor);
self
}
/// Sets the target angle this motor needs to reach.
#[must_use]
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.0.set_motor_position(target_pos, stiffness, damping);
self
}
/// Configure both the target angle and target velocity of the motor.
pub fn motor_axis(
#[must_use]
pub fn motor(
mut self,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) -> Self {
self.data =
self.data
.motor_axis(JointAxis::AngX, 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::AngX, max_impulse);
self.0.set_motor(target_pos, target_vel, stiffness, damping);
self
}
#[must_use]
pub fn limit_axis(mut self, limits: [Real; 2]) -> Self {
self.data = self.data.limit_axis(JointAxis::AngX, limits);
pub fn motor_max_force(mut self, max_force: Real) -> Self {
self.0.set_motor_max_force(max_force);
self
}
#[must_use]
pub fn limits(mut self, limits: [Real; 2]) -> Self {
self.0.set_limits(limits);
self
}
#[must_use]
pub fn build(self) -> RevoluteJoint {
self.0
}
}
impl Into<JointData> for RevoluteJoint {
fn into(self) -> JointData {
self.data
impl Into<GenericJoint> for RevoluteJointBuilder {
fn into(self) -> GenericJoint {
self.0.into()
}
}

146
src/dynamics/joint/spherical_joint.rs
View File

@@ -1,11 +1,12 @@
use crate::dynamics::joint::{JointAxesMask, JointData};
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
pub struct SphericalJoint {
data: JointData,
data: GenericJoint,
}
impl Default for SphericalJoint {
@@ -16,40 +17,128 @@ impl Default for SphericalJoint {
impl SphericalJoint {
pub fn new() -> Self {
let data =
JointData::default().lock_axes(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z);
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_SPHERICAL_AXES).build();
Self { data }
}
pub fn data(&self) -> &JointData {
pub fn data(&self) -> &GenericJoint {
&self.data
}
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self {
self.data.set_motor_model(axis, model);
self
}
/// Sets the target velocity this motor needs to reach.
pub fn set_motor_velocity(
&mut self,
axis: JointAxis,
target_vel: Real,
factor: Real,
) -> &mut Self {
self.data.set_motor_velocity(axis, target_vel, factor);
self
}
/// Sets the target angle this motor needs to reach.
pub fn set_motor_position(
&mut self,
axis: JointAxis,
target_pos: Real,
stiffness: Real,
damping: Real,
) -> &mut Self {
self.data
.set_motor_position(axis, target_pos, stiffness, damping);
self
}
/// Configure both the target angle and target velocity of the motor.
pub fn set_motor(
&mut self,
axis: JointAxis,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) -> &mut Self {
self.data
.set_motor(axis, target_pos, target_vel, stiffness, damping);
self
}
pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self {
self.data.set_motor_max_force(axis, max_force);
self
}
pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
self.data.set_limits(axis, limits);
self
}
}
impl Into<GenericJoint> for SphericalJoint {
fn into(self) -> GenericJoint {
self.data
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct SphericalJointBuilder(SphericalJoint);
impl Default for SphericalJointBuilder {
fn default() -> Self {
Self(SphericalJoint::new())
}
}
impl SphericalJointBuilder {
pub fn new() -> Self {
Self(SphericalJoint::new())
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1);
self.0.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2);
self.0.set_local_anchor2(anchor2);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
#[must_use]
pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
self.data = self.data.motor_model(axis, model);
self.0.set_motor_model(axis, model);
self
}
/// Sets the target velocity this motor needs to reach.
#[must_use]
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.0.set_motor_velocity(axis, target_vel, factor);
self
}
/// Sets the target angle this motor needs to reach.
#[must_use]
pub fn motor_position(
mut self,
axis: JointAxis,
@@ -57,14 +146,14 @@ impl SphericalJoint {
stiffness: Real,
damping: Real,
) -> Self {
self.data = self
.data
.motor_position(axis, target_pos, stiffness, damping);
self.0
.set_motor_position(axis, target_pos, stiffness, damping);
self
}
/// Configure both the target angle and target velocity of the motor.
pub fn motor_axis(
#[must_use]
pub fn motor(
mut self,
axis: JointAxis,
target_pos: Real,
@@ -72,26 +161,31 @@ impl SphericalJoint {
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.0
.set_motor(axis, target_pos, target_vel, stiffness, damping);
self
}
#[must_use]
pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
self.data = self.data.limit_axis(axis, limits);
pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
self.0.set_motor_max_force(axis, max_force);
self
}
#[must_use]
pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
self.0.set_limits(axis, limits);
self
}
#[must_use]
pub fn build(self) -> SphericalJoint {
self.0
}
}
impl Into<JointData> for SphericalJoint {
fn into(self) -> JointData {
self.data
impl Into<GenericJoint> for SphericalJointBuilder {
fn into(self) -> GenericJoint {
self.0.into()
}
}