nphysics3d/joint/
planar_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 1 rotational and 2 translational degrees of freedom.
9#[derive(Copy, Clone, Debug)]
10pub struct PlanarJoint<N: RealField + Copy> {
11    prism1: PrismaticJoint<N>,
12    prism2: PrismaticJoint<N>,
13    revo: RevoluteJoint<N>,
14}
15
16impl<N: RealField + Copy> PlanarJoint<N> {
17    /// Create a new planar joint where both translational degrees of freedoms are along the provide axii.
18    ///
19    /// The rotational degree of freedom is along an axis orthogonal to `axis1` and `axis2`. Idealy, the two
20    /// provided axii should be orthogonal. All axis are in the local coordinate space of the attached multibody links.
21    ///
22    /// Panics if `axis1` and `axis2` are near-colinear.
23    pub fn new(
24        axis1: Unit<Vector3<N>>,
25        axis2: Unit<Vector3<N>>,
26        pos1: N,
27        pos2: N,
28        angle: N,
29    ) -> Self {
30        let cross = axis1.cross(&*axis2);
31        let normal = Unit::try_new(cross, N::default_epsilon())
32            .expect("A planar joint cannot be defined from two collinear axis.");
33        let prism1 = PrismaticJoint::new(axis1, pos1);
34        let prism2 = PrismaticJoint::new(axis2, pos2);
35        let revo = RevoluteJoint::new(normal, angle);
36
37        PlanarJoint {
38            prism1,
39            prism2,
40            revo,
41        }
42    }
43}
44
45impl<N: RealField + Copy> Joint<N> for PlanarJoint<N> {
46    #[inline]
47    fn ndofs(&self) -> usize {
48        3
49    }
50
51    fn body_to_parent(&self, parent_shift: &Vector3<N>, body_shift: &Vector3<N>) -> Isometry3<N> {
52        self.prism1.translation()
53            * self.prism2.translation()
54            * self.revo.body_to_parent(parent_shift, body_shift)
55    }
56
57    fn update_jacobians(&mut self, body_shift: &Vector3<N>, vels: &[N]) {
58        self.prism1.update_jacobians(body_shift, vels);
59        self.prism2.update_jacobians(body_shift, &vels[1..]);
60        self.revo.update_jacobians(body_shift, &vels[2..]);
61    }
62
63    fn jacobian(&self, transform: &Isometry3<N>, out: &mut JacobianSliceMut<N>) {
64        self.prism1.jacobian(transform, &mut out.columns_mut(0, 1));
65        self.prism2.jacobian(transform, &mut out.columns_mut(1, 1));
66        self.revo.jacobian(transform, &mut out.columns_mut(2, 1));
67    }
68
69    fn jacobian_dot(&self, transform: &Isometry3<N>, out: &mut JacobianSliceMut<N>) {
70        self.prism1
71            .jacobian_dot(transform, &mut out.columns_mut(0, 1));
72        self.prism2
73            .jacobian_dot(transform, &mut out.columns_mut(1, 1));
74        self.revo
75            .jacobian_dot(transform, &mut out.columns_mut(2, 1));
76    }
77
78    fn jacobian_dot_veldiff_mul_coordinates(
79        &self,
80        transform: &Isometry3<N>,
81        vels: &[N],
82        out: &mut JacobianSliceMut<N>,
83    ) {
84        self.prism1.jacobian_dot_veldiff_mul_coordinates(
85            transform,
86            vels,
87            &mut out.columns_mut(0, 1),
88        );
89        self.prism2.jacobian_dot_veldiff_mul_coordinates(
90            transform,
91            &[vels[1]],
92            &mut out.columns_mut(1, 1),
93        );
94        self.revo.jacobian_dot_veldiff_mul_coordinates(
95            transform,
96            &[vels[2]],
97            &mut out.columns_mut(2, 1),
98        );
99    }
100
101    fn jacobian_mul_coordinates(&self, vels: &[N]) -> Velocity<N> {
102        self.prism1.jacobian_mul_coordinates(vels)
103            + self.prism2.jacobian_mul_coordinates(&[vels[1]])
104            + self.revo.jacobian_mul_coordinates(&[vels[2]])
105    }
106
107    fn jacobian_dot_mul_coordinates(&self, vels: &[N]) -> Velocity<N> {
108        // NOTE: The two folowing are zero.
109        // self.prism1.jacobian_dot_mul_coordinates(vels)       +
110        // self.prism2.jacobian_dot_mul_coordinates(&[vels[1]]) +
111        self.revo.jacobian_dot_mul_coordinates(&[vels[2]])
112    }
113
114    fn default_damping(&self, out: &mut DVectorSliceMut<N>) {
115        self.prism1.default_damping(&mut out.rows_mut(0, 1));
116        self.prism2.default_damping(&mut out.rows_mut(1, 1));
117        self.revo.default_damping(&mut out.rows_mut(2, 1));
118    }
119
120    fn integrate(&mut self, parameters: &IntegrationParameters<N>, vels: &[N]) {
121        self.prism1.integrate(parameters, vels);
122        self.prism2.integrate(parameters, &[vels[1]]);
123        self.revo.integrate(parameters, &[vels[2]]);
124    }
125
126    fn apply_displacement(&mut self, disp: &[N]) {
127        self.prism1.apply_displacement(disp);
128        self.prism2.apply_displacement(&[disp[1]]);
129        self.revo.apply_displacement(&[disp[2]]);
130    }
131
132    #[inline]
133    fn clone(&self) -> Box<dyn Joint<N>> {
134        Box::new(*self)
135    }
136
137    fn num_velocity_constraints(&self) -> usize {
138        self.prism1.num_velocity_constraints()
139            + self.prism2.num_velocity_constraints()
140            + self.revo.num_velocity_constraints()
141    }
142
143    fn velocity_constraints(
144        &self,
145        parameters: &IntegrationParameters<N>,
146        multibody: &Multibody<N>,
147        link: &MultibodyLink<N>,
148        assembly_id: usize,
149        dof_id: usize,
150        ext_vels: &[N],
151        ground_j_id: &mut usize,
152        jacobians: &mut [N],
153        constraints: &mut ConstraintSet<N, (), (), usize>,
154    ) {
155        self.prism1.velocity_constraints(
156            parameters,
157            multibody,
158            link,
159            assembly_id,
160            dof_id,
161            ext_vels,
162            ground_j_id,
163            jacobians,
164            constraints,
165        );
166        self.prism2.velocity_constraints(
167            parameters,
168            multibody,
169            link,
170            assembly_id,
171            dof_id + 1,
172            ext_vels,
173            ground_j_id,
174            jacobians,
175            constraints,
176        );
177        self.revo.velocity_constraints(
178            parameters,
179            multibody,
180            link,
181            assembly_id,
182            dof_id + 2,
183            ext_vels,
184            ground_j_id,
185            jacobians,
186            constraints,
187        );
188    }
189
190    fn num_position_constraints(&self) -> usize {
191        // NOTE: we don't test if constraints exist to simplify indexing.
192        3
193    }
194
195    fn position_constraint(
196        &self,
197        i: usize,
198        multibody: &Multibody<N>,
199        link: &MultibodyLink<N>,
200        handle: BodyPartHandle<()>,
201        dof_id: usize,
202        jacobians: &mut [N],
203    ) -> Option<GenericNonlinearConstraint<N, ()>> {
204        if i == 0 {
205            self.prism1
206                .position_constraint(0, multibody, link, handle, dof_id, jacobians)
207        } else if i == 1 {
208            self.prism2
209                .position_constraint(0, multibody, link, handle, dof_id + 1, jacobians)
210        } else {
211            self.revo
212                .position_constraint(0, multibody, link, handle, dof_id + 2, jacobians)
213        }
214    }
215}
216
217prismatic_motor_limit_methods_1!(PlanarJoint, prism1);
218prismatic_motor_limit_methods_2!(PlanarJoint, prism2);
219revolute_motor_limit_methods!(PlanarJoint, revo);
nphysics3d/joint/planar_joint.rs

nphysics3d/joint/
planar_joint.rs
