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

nphysics3d/joint/
cylindrical_joint.rs
