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

Start experimenting with a generic joint implementation for joint drives.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-02-10 11:56:51 +01:00
parent 3be8669206
commit 5b80c4efbf
14 changed files with 1350 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -7,12 +7,16 @@ use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint};
#[cfg(feature = "simd-is-enabled")]
use super::{
WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint,
WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint,
WFixedVelocityGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint,
WGenericVelocityConstraint, WGenericVelocityGroundConstraint, WPrismaticVelocityConstraint,
WPrismaticVelocityGroundConstraint,
};
#[cfg(feature = "dim3")]
#[cfg(feature = "simd-is-enabled")]
use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
GenericVelocityConstraint, GenericVelocityGroundConstraint,
};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
@@ -34,6 +38,12 @@ pub(crate) enum AnyJointVelocityConstraint {
WFixedConstraint(WFixedVelocityConstraint),
#[cfg(feature = "simd-is-enabled")]
WFixedGroundConstraint(WFixedVelocityGroundConstraint),
GenericConstraint(GenericVelocityConstraint),
GenericGroundConstraint(GenericVelocityGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericConstraint(WGenericVelocityConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericGroundConstraint(WGenericVelocityGroundConstraint),
PrismaticConstraint(PrismaticVelocityConstraint),
PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
@@ -79,6 +89,9 @@ impl AnyJointVelocityConstraint {
JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
),
JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
),
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
@@ -109,6 +122,12 @@ impl AnyJointVelocityConstraint {
params, joint_id, rbs1, rbs2, joints,
))
}
JointParams::GenericJoint(_) => {
let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
AnyJointVelocityConstraint::WGenericConstraint(
WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
)
}
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -148,6 +167,11 @@ impl AnyJointVelocityConstraint {
JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
),
JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
GenericVelocityGroundConstraint::from_params(
params, joint_id, rb1, rb2, p, flipped,
),
),
JointParams::PrismaticJoint(p) => {
AnyJointVelocityConstraint::PrismaticGroundConstraint(
PrismaticVelocityGroundConstraint::from_params(
@@ -199,6 +223,14 @@ impl AnyJointVelocityConstraint {
),
)
}
JointParams::GenericJoint(_) => {
let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
AnyJointVelocityConstraint::WGenericGroundConstraint(
WGenericVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
),
)
}
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -235,6 +267,12 @@ impl AnyJointVelocityConstraint {
AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
@@ -269,6 +307,12 @@ impl AnyJointVelocityConstraint {
AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
@@ -311,6 +355,16 @@ impl AnyJointVelocityConstraint {
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
c.writeback_impulses(joints_all)