robot_description_builder/joint/smartjointbuilder/smartparams/
axis.rs

1use crate::joint::{
2	jointbuilder::JointBuilder,
3	smartjointbuilder::{smart_joint_datatraits, SmartJointBuilder},
4};
5
6/// A trait to label a `SmartJointType` that is allowed to have an axis.
7pub trait AxisAllowed {}
8
9/// A type to significy that no [`axis`](JointBuilder::axis) was specified.
10#[derive(Debug, Default, Clone)]
11pub struct NoAxis;
12impl smart_joint_datatraits::AxisDataType for NoAxis {}
13
14#[derive(Debug, Default, Clone)]
15pub struct WithAxis(f32, f32, f32);
16impl smart_joint_datatraits::AxisDataType for WithAxis {
17	fn simplify(&self, joint_builder: &mut JointBuilder) {
18		joint_builder.with_axis((self.0, self.1, self.2));
19	}
20}
21
22impl<Type, Axis, Calibration, Dynamics, Limit, Mimic, SafetyController>
23	SmartJointBuilder<Type, Axis, Calibration, Dynamics, Limit, Mimic, SafetyController>
24where
25	Type: AxisAllowed,
26	Axis: smart_joint_datatraits::AxisDataType,
27	Calibration: smart_joint_datatraits::CalibrationDataType,
28	Dynamics: smart_joint_datatraits::DynamicsDataType,
29	Limit: smart_joint_datatraits::LimitDataType,
30	Mimic: smart_joint_datatraits::MimicDataType,
31	SafetyController: smart_joint_datatraits::SafetyControllerDataType,
32{
33	pub fn with_axis(
34		self,
35		axis: (f32, f32, f32),
36	) -> SmartJointBuilder<Type, WithAxis, Calibration, Dynamics, Limit, Mimic, SafetyController> {
37		let length = f32::sqrt(axis.0 * axis.0 + axis.1 * axis.1 + axis.2 * axis.2);
38		SmartJointBuilder {
39			name: self.name,
40			joint_type: self.joint_type,
41			transform: self.transform,
42			axis: WithAxis(axis.0 / length, axis.1 / length, axis.2 / length),
43			calibration: self.calibration,
44			dynamics: self.dynamics,
45			limit: self.limit,
46			mimic: self.mimic,
47			safety_controller: self.safety_controller,
48		}
49	}
50}
51
52impl<Type, Calibration, Dynamics, Limit, Mimic, SafetyController>
53	SmartJointBuilder<Type, WithAxis, Calibration, Dynamics, Limit, Mimic, SafetyController>
54where
55	Type: AxisAllowed,
56	Calibration: smart_joint_datatraits::CalibrationDataType,
57	Dynamics: smart_joint_datatraits::DynamicsDataType,
58	Limit: smart_joint_datatraits::LimitDataType,
59	Mimic: smart_joint_datatraits::MimicDataType,
60	SafetyController: smart_joint_datatraits::SafetyControllerDataType,
61{
62	pub fn axis(&self) -> (f32, f32, f32) {
63		(self.axis.0, self.axis.1, self.axis.2)
64	}
65}
robot_description_builder/joint/smartjointbuilder/smartparams/axis.rs

robot_description_builder/joint/smartjointbuilder/smartparams/
axis.rs
