nphysics3d/joint/
pin_slot_joint.rs

1use na::{DVectorSliceMut, Isometry3, RealField, Unit, Vector3};
2
3use crate::joint::{Joint, PrismaticJoint, RevoluteJoint};
4use crate::math::{JacobianSliceMut, Velocity};
5use crate::object::{BodyPartHandle, Multibody, MultibodyLink};
6use crate::solver::{ConstraintSet, GenericNonlinearConstraint, IntegrationParameters};
7
8/// A joint that allows one translational and one rotational degrees of freedom.
9///
10/// Both are not required to be along the same direction.
11#[derive(Copy, Clone, Debug)]
12pub struct PinSlotJoint<N: RealField + Copy> {
13    prism: PrismaticJoint<N>,
14    revo: RevoluteJoint<N>,
15}
16
17impl<N: RealField + Copy> PinSlotJoint<N> {
18    /// Create a new pin-slot joint with axii expressed in the local coordinate frame of the attached bodies, and
19    /// with initial linear position and angle.
20    pub fn new(axis_v: Unit<Vector3<N>>, axis_w: Unit<Vector3<N>>, position: N, angle: N) -> Self {
21        let prism = PrismaticJoint::new(axis_v, position);
22        let revo = RevoluteJoint::new(axis_w, angle);
23
24        PinSlotJoint { prism, revo }
25    }
26
27    /// The linear displacement.
28    pub fn offset(&self) -> N {
29        self.prism.offset()
30    }
31
32    /// The angular displacement.
33    pub fn angle(&self) -> N {
34        self.revo.angle()
35    }
36}
37
38impl<N: RealField + Copy> Joint<N> for PinSlotJoint<N> {
39    #[inline]
40    fn ndofs(&self) -> usize {
41        2
42    }
43
44    fn body_to_parent(&self, parent_shift: &Vector3<N>, body_shift: &Vector3<N>) -> Isometry3<N> {
45        self.prism.translation() * self.revo.body_to_parent(parent_shift, body_shift)
46    }
47
48    fn update_jacobians(&mut self, body_shift: &Vector3<N>, vels: &[N]) {
49        self.prism.update_jacobians(body_shift, vels);
50        self.revo.update_jacobians(body_shift, &[vels[1]]);
51    }
52
53    fn jacobian(&self, transform: &Isometry3<N>, out: &mut JacobianSliceMut<N>) {
54        self.prism.jacobian(transform, &mut out.columns_mut(0, 1));
55        self.revo.jacobian(transform, &mut out.columns_mut(1, 1));
56    }
57
58    fn jacobian_dot(&self, transform: &Isometry3<N>, out: &mut JacobianSliceMut<N>) {
59        self.prism
60            .jacobian_dot(transform, &mut out.columns_mut(0, 1));
61        self.revo
62            .jacobian_dot(transform, &mut out.columns_mut(1, 1));
63    }
64
65    fn jacobian_dot_veldiff_mul_coordinates(
66        &self,
67        transform: &Isometry3<N>,
68        vels: &[N],
69        out: &mut JacobianSliceMut<N>,
70    ) {
71        self.prism.jacobian_dot_veldiff_mul_coordinates(
72            transform,
73            vels,
74            &mut out.columns_mut(0, 1),
75        );
76        self.revo.jacobian_dot_veldiff_mul_coordinates(
77            transform,
78            &[vels[1]],
79            &mut out.columns_mut(1, 1),
80        );
81    }
82
83    fn jacobian_mul_coordinates(&self, vels: &[N]) -> Velocity<N> {
84        self.prism.jacobian_mul_coordinates(vels) + self.revo.jacobian_mul_coordinates(&[vels[1]])
85    }
86
87    fn jacobian_dot_mul_coordinates(&self, vels: &[N]) -> Velocity<N> {
88        // NOTE: The following is zero.
89        // self.prism.jacobian_dot_mul_coordinates(vels) +
90        self.revo.jacobian_dot_mul_coordinates(&[vels[1]])
91    }
92
93    fn default_damping(&self, out: &mut DVectorSliceMut<N>) {
94        self.prism.default_damping(&mut out.rows_mut(0, 1));
95        self.revo.default_damping(&mut out.rows_mut(1, 1));
96    }
97
98    fn integrate(&mut self, parameters: &IntegrationParameters<N>, vels: &[N]) {
99        self.prism.integrate(parameters, vels);
100        self.revo.integrate(parameters, &[vels[1]]);
101    }
102
103    fn apply_displacement(&mut self, disp: &[N]) {
104        self.prism.apply_displacement(disp);
105        self.revo.apply_displacement(&[disp[1]]);
106    }
107
108    #[inline]
109    fn clone(&self) -> Box<dyn Joint<N>> {
110        Box::new(*self)
111    }
112
113    fn num_velocity_constraints(&self) -> usize {
114        self.prism.num_velocity_constraints() + self.revo.num_velocity_constraints()
115    }
116
117    fn velocity_constraints(
118        &self,
119        parameters: &IntegrationParameters<N>,
120        multibody: &Multibody<N>,
121        link: &MultibodyLink<N>,
122        assembly_id: usize,
123        dof_id: usize,
124        ext_vels: &[N],
125        ground_j_id: &mut usize,
126        jacobians: &mut [N],
127        constraints: &mut ConstraintSet<N, (), (), usize>,
128    ) {
129        self.prism.velocity_constraints(
130            parameters,
131            multibody,
132            link,
133            assembly_id,
134            dof_id,
135            ext_vels,
136            ground_j_id,
137            jacobians,
138            constraints,
139        );
140        self.revo.velocity_constraints(
141            parameters,
142            multibody,
143            link,
144            assembly_id,
145            dof_id + 1,
146            ext_vels,
147            ground_j_id,
148            jacobians,
149            constraints,
150        );
151    }
152
153    fn num_position_constraints(&self) -> usize {
154        // NOTE: we don't test if constraints exist to simplify indexing.
155        2
156    }
157
158    fn position_constraint(
159        &self,
160        i: usize,
161        multibody: &Multibody<N>,
162        link: &MultibodyLink<N>,
163        handle: BodyPartHandle<()>,
164        dof_id: usize,
165        jacobians: &mut [N],
166    ) -> Option<GenericNonlinearConstraint<N, ()>> {
167        if i == 0 {
168            self.prism
169                .position_constraint(0, multibody, link, handle, dof_id, jacobians)
170        } else {
171            self.revo
172                .position_constraint(0, multibody, link, handle, dof_id + 1, jacobians)
173        }
174    }
175}
176
177prismatic_motor_limit_methods!(PinSlotJoint, prism);
178revolute_motor_limit_methods!(PinSlotJoint, revo);
nphysics3d/joint/pin_slot_joint.rs

nphysics3d/joint/
pin_slot_joint.rs
