robot_description_builder/joint/smartjointbuilder/smartparams/
safety_controller.rs

1use crate::joint::{
2	joint_data,
3	jointbuilder::JointBuilder,
4	smartjointbuilder::{smart_joint_datatraits, SmartJointBuilder},
5};
6
7/// A trait to signify if the `SafetyController` element is allowed on a specific `JointType` for the `SmartJointBuilder`.
8// TODO: EXPAND
9pub trait SafetyControllerAllowed {}
10
11/// A type to significy that no [`SafetyController`](joint_data::SafetyControllerData) was specified.
12#[derive(Debug, Default, Clone)]
13pub struct NoSafetyController;
14impl smart_joint_datatraits::SafetyControllerDataType for NoSafetyController {}
15
16#[derive(Debug, Default, Clone)]
17pub struct WithSafetyController {
18	// (optional, defaults to 0)
19	//
20	// An attribute specifying the lower joint boundary where the safety controller starts limiting the position of the joint. This limit needs to be larger than the lower joint limit (see above). See See safety limits for more details.
21	// TODO: FIX DOCUMENTATION
22	soft_lower_limit: Option<f32>,
23	// (optional, defaults to 0)
24	//
25	// An attribute specifying the upper joint boundary where the safety controller starts limiting the position of the joint. This limit needs to be smaller than the upper joint limit (see above). See See safety limits for more details.
26	// TODO: FIX DOCUMENTATION
27	soft_upper_limit: Option<f32>,
28	//  (optional, defaults to 0)
29	//
30	// An attribute specifying the relation between position and velocity limits. See See safety limits for more details.
31	// TODO: FIX DOCUMENTATION
32	k_position: Option<f32>,
33	// An attribute specifying the relation between effort and velocity limits. See See safety limits for more details.
34	k_velocity: f32,
35}
36
37impl From<WithSafetyController> for joint_data::SafetyControllerData {
38	fn from(value: WithSafetyController) -> Self {
39		Self {
40			soft_lower_limit: value.soft_lower_limit,
41			soft_upper_limit: value.soft_upper_limit,
42			k_position: value.k_position,
43			k_velocity: value.k_velocity,
44		}
45	}
46}
47
48impl smart_joint_datatraits::SafetyControllerDataType for WithSafetyController {
49	fn simplify(&self, joint_builder: &mut JointBuilder) {
50		joint_builder.with_safety_controller(self.clone().into());
51	}
52}
53
54impl<Type, Axis, Calibration, Dynamics, Limit, Mimic>
55	SmartJointBuilder<Type, Axis, Calibration, Dynamics, Limit, Mimic, NoSafetyController>
56where
57	Type: SafetyControllerAllowed,
58	Axis: smart_joint_datatraits::AxisDataType,
59	Calibration: smart_joint_datatraits::CalibrationDataType,
60	Dynamics: smart_joint_datatraits::DynamicsDataType,
61	Limit: smart_joint_datatraits::LimitDataType,
62	Mimic: smart_joint_datatraits::MimicDataType,
63{
64	pub fn with_safety_controller(
65		self,
66		k_velocity: f32,
67	) -> SmartJointBuilder<Type, Axis, Calibration, Dynamics, Limit, Mimic, WithSafetyController> {
68		SmartJointBuilder {
69			name: self.name,
70			joint_type: self.joint_type,
71			transform: self.transform,
72
73			axis: self.axis,
74			calibration: self.calibration,
75			dynamics: self.dynamics,
76			limit: self.limit,
77			mimic: self.mimic,
78			safety_controller: WithSafetyController {
79				k_velocity,
80				..Default::default()
81			},
82		}
83	}
84}
85
86impl<Type, Axis, Calibration, Dynamics, Limit, Mimic>
87	SmartJointBuilder<Type, Axis, Calibration, Dynamics, Limit, Mimic, WithSafetyController>
88where
89	Type: SafetyControllerAllowed,
90	Axis: smart_joint_datatraits::AxisDataType,
91	Calibration: smart_joint_datatraits::CalibrationDataType,
92	Dynamics: smart_joint_datatraits::DynamicsDataType,
93	Limit: smart_joint_datatraits::LimitDataType,
94	Mimic: smart_joint_datatraits::MimicDataType,
95{
96	// Defaults 0
97	pub fn set_k_position(mut self, k_position: f32) -> Self {
98		self.safety_controller.k_position = Some(k_position);
99		self
100	}
101
102	// Defaults 0
103	pub fn k_position(&self) -> Option<f32> {
104		self.safety_controller.k_position
105	}
106
107	/// Sets the k_velocity limit to the specified value in m/s or rad/s ([`k_velocity`](crate::joint::joint_data::SafetyControllerData::k_velocity)).
108	pub fn set_k_velocity(mut self, k_velocity: f32) -> Self {
109		self.safety_controller.k_velocity = k_velocity;
110		self
111	}
112
113	/// Retrieves the set k_velocity limit in m/s or rad/s ([`k_velocity`](crate::joint::joint_data::SafetyControllerData::k_velocity)).
114	pub fn k_velocity(&self) -> f32 {
115		self.safety_controller.k_velocity
116	}
117}
118
119/// The (soft) limits are only available on non continuous `JointType`s.
120impl<Type, Axis, Calibration, Dynamics, Limit, Mimic>
121	SmartJointBuilder<Type, Axis, Calibration, Dynamics, Limit, Mimic, WithSafetyController>
122where
123	Type: SafetyControllerAllowed + smart_joint_datatraits::SmartJointTypeTrait<false>,
124	Axis: smart_joint_datatraits::AxisDataType,
125	Calibration: smart_joint_datatraits::CalibrationDataType,
126	Dynamics: smart_joint_datatraits::DynamicsDataType,
127	Limit: smart_joint_datatraits::LimitDataType,
128	Mimic: smart_joint_datatraits::MimicDataType,
129{
130	/// Sets the soft lower limit ([`soft_lower_limit`](crate::joint::joint_data::SafetyControllerData::soft_lower_limit)).
131	pub fn set_soft_lower_limit(mut self, soft_lower_limit: f32) -> Self {
132		self.safety_controller.soft_lower_limit = Some(soft_lower_limit);
133		self
134	}
135
136	/// Retrieve the specified soft lower limit ([`soft_lower_limit`](crate::joint::joint_data::SafetyControllerData::soft_lower_limit)).
137	pub fn soft_lower_limit(&self) -> Option<f32> {
138		self.safety_controller.soft_lower_limit
139	}
140
141	/// Sets the soft upper limit ([`soft_upper_limit`](crate::joint::joint_data::SafetyControllerData::soft_upper_limit)).
142	pub fn set_soft_upper_limit(mut self, soft_upper_limit: f32) -> Self {
143		self.safety_controller.soft_upper_limit = Some(soft_upper_limit);
144		self
145	}
146
147	/// Retrieve the specified soft upper limit ([`soft_upper_limit`](crate::joint::joint_data::SafetyControllerData::soft_upper_limit)).
148	pub fn soft_upper_limit(&self) -> Option<f32> {
149		self.safety_controller.soft_upper_limit
150	}
151}
robot_description_builder/joint/smartjointbuilder/smartparams/safety_controller.rs

robot_description_builder/joint/smartjointbuilder/smartparams/
safety_controller.rs
