1use robot_behavior::ArmParam;
2use std::f64::consts::FRAC_PI_2;
3
4struct ExRobot {}
5
6impl ArmParam<7> for ExRobot {
7 const DH: [[f64; 4]; 7] = [
8 [0., 0.333, 0., 0.],
9 [0., 0., 0., -FRAC_PI_2],
10 [0., 0.316, 0., FRAC_PI_2],
11 [0., 0., 0.0825, FRAC_PI_2],
12 [0., 0.384, -0.0825, -FRAC_PI_2],
13 [0., 0., 0., FRAC_PI_2],
14 [0., 0., 0.088, FRAC_PI_2],
15 ];
16 const JOINT_MIN: [f64; 7] = [
17 -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973,
18 ];
19 const JOINT_MAX: [f64; 7] = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973];
20}
21
22fn main() {
23 let q = [1.; 7];
24 let pose = ExRobot::forward_kinematics(&q);
25 println!("pose: {:?}", pose.axis_angle());
26 println!("pose: {:?}", pose.euler());
27 println!("pose: {:?}", pose.homo());
28 println!("pose: {:?}", pose.position());
29 println!("pose: {:?}", pose.quat());
30}