1use std::{sync::Arc, time::Duration};
2
3use arci::{
4 gamepad::{Axis, Button, GamepadEvent},
5 JointTrajectoryClient, Speaker,
6};
7use async_trait::async_trait;
8use k::{Translation3, Vector3};
9use openrr_client::IkSolverWithChain;
10use parking_lot::Mutex;
11use schemars::JsonSchema;
12use serde::{Deserialize, Serialize};
13
14use super::control_mode::ControlMode;
15
16const IK_POSITION_TURBO_GAIN: f64 = 2.0;
17
18struct IkModeInner {
19 linear_velocity: Vector3<f64>,
20 angular_velocity: Vector3<f64>,
21 move_step_linear: [f64; 3],
22 move_step_angular: [f64; 3],
23 is_turbo: bool,
24 is_sending: bool,
25}
26
27impl IkModeInner {
28 fn new(move_step_linear: [f64; 3], move_step_angular: [f64; 3]) -> Self {
29 Self {
30 linear_velocity: Vector3::new(0.0, 0.0, 0.0),
31 angular_velocity: Vector3::new(0.0, 0.0, 0.0),
32 move_step_linear,
33 move_step_angular,
34 is_turbo: false,
35 is_sending: false,
36 }
37 }
38
39 fn handle_event(&mut self, event: GamepadEvent) {
40 match event {
41 GamepadEvent::ButtonPressed(Button::LeftTrigger2) => {
42 self.is_turbo = true;
43 }
44 GamepadEvent::ButtonReleased(Button::LeftTrigger2) => {
45 self.is_turbo = false;
46 }
47 GamepadEvent::ButtonPressed(Button::RightTrigger2) => {
48 self.is_sending = true;
49 }
50 GamepadEvent::ButtonReleased(Button::RightTrigger2) => {
51 self.is_sending = false;
52 self.clear_velocity();
53 }
54 GamepadEvent::ButtonPressed(Button::South) => {
55 self.linear_velocity.z = -self.move_step_linear[2];
56 }
57 GamepadEvent::ButtonReleased(Button::South) => {
58 self.linear_velocity.z = 0.0;
59 }
60 GamepadEvent::ButtonPressed(Button::West) => {
61 self.linear_velocity.z = self.move_step_linear[2];
62 }
63 GamepadEvent::ButtonReleased(Button::West) => {
64 self.linear_velocity.z = 0.0;
65 }
66 GamepadEvent::AxisChanged(Axis::RightStickY, v) => {
67 self.linear_velocity.x = self.move_step_linear[0] * v;
68 }
69 GamepadEvent::AxisChanged(Axis::RightStickX, v) => {
70 self.linear_velocity.y = self.move_step_linear[1] * v;
71 }
72 GamepadEvent::AxisChanged(Axis::LeftStickX, v) => {
73 self.angular_velocity.x = -self.move_step_angular[0] * v;
74 }
75 GamepadEvent::AxisChanged(Axis::LeftStickY, v) => {
76 self.angular_velocity.y = self.move_step_angular[1] * v;
77 }
78 GamepadEvent::ButtonPressed(Button::DPadRight) => {
79 self.angular_velocity.z = -self.move_step_angular[2];
80 }
81 GamepadEvent::ButtonReleased(Button::DPadRight) => {
82 self.angular_velocity.z = 0.0;
83 }
84 GamepadEvent::ButtonPressed(Button::DPadLeft) => {
85 self.angular_velocity.z = self.move_step_angular[2];
86 }
87 GamepadEvent::ButtonReleased(Button::DPadLeft) => {
88 self.angular_velocity.z = 0.0;
89 }
90 GamepadEvent::Disconnected => {
91 self.is_sending = false;
92 self.is_turbo = false;
93 self.clear_velocity();
94 }
95 _ => {}
96 }
97 }
98
99 fn clear_velocity(&mut self) {
100 self.linear_velocity.x = 0.0;
101 self.linear_velocity.y = 0.0;
102 self.linear_velocity.z = 0.0;
103 self.angular_velocity.x = 0.0;
104 self.angular_velocity.y = 0.0;
105 self.angular_velocity.z = 0.0;
106 }
107
108 fn get_linear_velocity(&self) -> Vector3<f64> {
109 self.linear_velocity
110 * if self.is_turbo {
111 IK_POSITION_TURBO_GAIN
112 } else {
113 1.0
114 }
115 }
116}
117
118pub struct IkMode<J, S>
119where
120 J: JointTrajectoryClient,
121 S: Speaker,
122{
123 joint_trajectory_client: J,
124 speaker: S,
125 mode: String,
126 submode: String,
127 step_duration: Duration,
128 ik_solver_with_chain: Arc<IkSolverWithChain>,
129 inner: Mutex<IkModeInner>,
130}
131
132impl<J, S> IkMode<J, S>
133where
134 J: JointTrajectoryClient,
135 S: Speaker,
136{
137 pub fn new(
138 mode: String,
139 joint_trajectory_client: J,
140 move_step_linear: [f64; 3],
141 move_step_angular: [f64; 3],
142 step_duration: Duration,
143 speaker: S,
144 ik_solver_with_chain: Arc<IkSolverWithChain>,
145 ) -> Self {
146 Self {
147 joint_trajectory_client,
148 speaker,
149 mode,
150 submode: "".to_string(),
151 step_duration,
152 ik_solver_with_chain,
153 inner: Mutex::new(IkModeInner::new(move_step_linear, move_step_angular)),
154 }
155 }
156
157 pub fn new_from_config(
158 config: IkModeConfig,
159 joint_trajectory_client: J,
160 speaker: S,
161 ik_solver_with_chain: Arc<IkSolverWithChain>,
162 ) -> Self {
163 Self::new(
164 config.mode,
165 joint_trajectory_client,
166 config.move_step_linear,
167 config.move_step_angular,
168 Duration::from_secs_f64(config.step_duration_secs),
169 speaker,
170 ik_solver_with_chain,
171 )
172 }
173}
174
175#[async_trait]
176impl<N, S> ControlMode for IkMode<N, S>
177where
178 N: JointTrajectoryClient,
179 S: Speaker,
180{
181 fn handle_event(&self, event: GamepadEvent) {
182 self.inner.lock().handle_event(event);
183 }
184
185 async fn proc(&self) {
186 let (is_sending, angular_velocity, linear_velocity) = {
187 let inner = self.inner.lock();
188 (
189 inner.is_sending,
190 inner.angular_velocity,
191 inner.get_linear_velocity(),
192 )
193 };
194 if is_sending {
195 let current_positions = self
196 .joint_trajectory_client
197 .current_joint_positions()
198 .unwrap();
199 self.ik_solver_with_chain
200 .set_joint_positions_clamped(¤t_positions);
201 let current_pose = self.ik_solver_with_chain.end_transform();
202 let rotated = current_pose
203 * k::UnitQuaternion::from_euler_angles(
204 angular_velocity.x,
205 angular_velocity.y,
206 angular_velocity.z,
207 );
208 let target_pose = rotated * Translation3::from(linear_velocity);
209 if self.ik_solver_with_chain.solve(&target_pose).is_ok() {
210 let pos = self.ik_solver_with_chain.joint_positions();
211 self.joint_trajectory_client
212 .send_joint_positions(pos, self.step_duration)
213 .unwrap()
214 .await
215 .unwrap();
216 } else {
217 self.speaker.speak("ik fail").unwrap().await.unwrap();
218 }
219 }
220 }
221
222 fn mode(&self) -> &str {
223 &self.mode
224 }
225
226 fn submode(&self) -> String {
227 self.submode.to_owned()
228 }
229}
230
231#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
232#[serde(deny_unknown_fields)]
233pub struct IkModeConfig {
234 pub mode: String,
235 #[serde(default = "default_move_step_angular")]
236 pub move_step_angular: [f64; 3],
237 #[serde(default = "default_move_step_linear")]
238 pub move_step_linear: [f64; 3],
239 #[serde(default = "default_step_duration_secs")]
240 pub step_duration_secs: f64,
241}
242
243const fn default_move_step_angular() -> [f64; 3] {
244 [0.05, 0.05, 0.17]
245}
246
247const fn default_move_step_linear() -> [f64; 3] {
248 [0.01, 0.01, 0.01]
249}
250
251const fn default_step_duration_secs() -> f64 {
252 0.1
253}
254
255#[cfg(test)]
256mod tests {
257 use assert_approx_eq::*;
258
259 use super::*;
260
261 #[test]
262 fn test_default_move_step_angular() {
263 let def = default_move_step_angular();
264
265 assert_eq!(def.len(), 3_usize);
266 assert_approx_eq!(def[0], 0.05_f64);
267 assert_approx_eq!(def[1], 0.05_f64);
268 assert_approx_eq!(def[2], 0.17_f64);
269 }
270
271 #[test]
272 fn test_default_move_step_linear() {
273 let def = default_move_step_linear();
274
275 assert_eq!(def.len(), 3_usize);
276 assert_approx_eq!(def[0], 0.01_f64);
277 assert_approx_eq!(def[1], 0.01_f64);
278 assert_approx_eq!(def[2], 0.01_f64);
279 }
280
281 #[test]
282 fn test_default_step_duration_secs() {
283 let def = default_step_duration_secs();
284
285 assert_approx_eq!(def, 0.1_f64);
286 }
287}