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