use crate::{JAKA_FREQUENCY, JAKA_VERSION, network::NetWork, robot_impl::RobotImpl, types::*};
use robot_behavior::{
ArmDOF, ArmPreplannedPath, ArmState, ControlType, Coord, LoadState, MotionType, OverrideOnce,
Pose, Realtime, RobotException, RobotResult, behavior::*, utils::rad_to_deg,
};
use rsruckig::{
error::ThrowErrorHandler,
prelude::{InputParameter, OutputParameter},
result::RuckigResult,
ruckig::Ruckig,
util::DataArrayOrVec,
};
use serde::{Deserialize, Serialize};
use std::{
marker::PhantomData,
sync::{Arc, Mutex, RwLock},
thread::{self, sleep},
time::Duration,
};
pub trait JakaType {
const N: usize;
}
pub struct JakaRobot<T: JakaType, const N: usize> {
pub(crate) marker: PhantomData<T>,
pub robot_impl: RobotImpl<N>,
pub(crate) robot_state: Arc<RwLock<RobotState>>,
pub(crate) streaming_handle: thread::JoinHandle<()>,
pub(crate) is_moving: bool,
pub(crate) coord: OverrideOnce<Coord>,
pub(crate) max_vel: OverrideOnce<[f64; N]>,
pub(crate) max_acc: OverrideOnce<[f64; N]>,
pub(crate) max_cartesian_vel: OverrideOnce<f64>,
pub(crate) max_cartesian_acc: OverrideOnce<f64>,
pub(crate) max_rotation_vel: OverrideOnce<f64>,
pub(crate) max_rotation_acc: OverrideOnce<f64>,
pub path: Option<Vec<MotionType<N>>>,
}
impl<T: JakaType, const N: usize> ArmDOF for JakaRobot<T, N> {
const N: usize = N;
}
impl<T: JakaType, const N: usize> JakaRobot<T, N>
where
[f64; N]: Serialize + for<'a> Deserialize<'a>,
{
pub fn set_tio_vout(&mut self, tio: TioVout) -> RobotResult<()> {
let data = match tio {
TioVout::Enable(mode) => match mode {
TioVoutMode::V12V => SetTioVoutParamData { tio_vout_ena: 1, tio_vout_vol: 0 },
TioVoutMode::V24V => SetTioVoutParamData { tio_vout_ena: 1, tio_vout_vol: 1 },
},
TioVout::Disable => SetTioVoutParamData { tio_vout_ena: 0, tio_vout_vol: 0 },
};
self.robot_impl._set_tio_vout_param(data)?.into()
}
pub fn get_tio_vout(&mut self) -> RobotResult<TioVout> {
let state = self.robot_impl._get_tio_vout_param()?;
let tio = match state.tio_vout_ena {
0 => TioVout::Disable,
1 => match state.tio_vout_vol {
0 => TioVout::Enable(TioVoutMode::V12V),
1 => TioVout::Enable(TioVoutMode::V24V),
_ => {
return Err(RobotException::CommandException(
"Invalid TIO VOUT voltage".to_string(),
));
}
},
_ => {
return Err(RobotException::CommandException(
"Invalid TIO VOUT enable status".to_string(),
));
}
};
Ok(tio)
}
}
impl<T: JakaType, const N: usize> JakaRobot<T, N>
where
JakaRobot<T, N>: ArmParam<N> + Arm<N>,
{
pub fn new(ip: &str) -> Self {
let robot_state = NetWork::state_connect(ip);
let mut robot = Self {
marker: PhantomData,
robot_impl: RobotImpl::new(ip),
robot_state,
streaming_handle: thread::spawn(|| {}),
is_moving: false,
coord: OverrideOnce::new(Coord::OCS),
max_vel: OverrideOnce::new(Self::JOINT_VEL_BOUND),
max_acc: OverrideOnce::new(Self::JOINT_ACC_BOUND),
max_cartesian_vel: OverrideOnce::new(Self::CARTESIAN_VEL_BOUND),
max_cartesian_acc: OverrideOnce::new(Self::CARTESIAN_ACC_BOUND),
max_rotation_vel: OverrideOnce::new(Self::ROTATION_VEL_BOUND),
max_rotation_acc: OverrideOnce::new(Self::ROTATION_ACC_BOUND),
path: None,
};
let _ = robot.set_scale(0.05);
robot
}
}
impl<T: JakaType, const N: usize> Robot for JakaRobot<T, N>
where
[f64; N]: Serialize + for<'a> Deserialize<'a>,
{
type State = RobotState;
fn version() -> String {
format!("JAKA Robot v{JAKA_VERSION}")
}
fn init(&mut self) -> RobotResult<()> {
self.robot_impl._power_on()?.into()
}
fn shutdown(&mut self) -> RobotResult<()> {
self.robot_impl._power_off()?.into()
}
fn enable(&mut self) -> RobotResult<()> {
let _ = self.robot_impl._power_on()?;
self.robot_impl._enable()?.into()
}
fn disable(&mut self) -> RobotResult<()> {
self.robot_impl._disable()?.into()
}
fn is_moving(&mut self) -> RobotResult<bool> {
if self.is_moving {
self.is_moving = self.robot_impl._get_data().unwrap().curr_tcp_trans_vel > 0.1;
}
Ok(self.is_moving)
}
fn waiting_for_finish(&mut self) -> RobotResult<()> {
while self.is_moving()? {
sleep(Duration::from_millis(100));
}
Ok(())
}
fn reset(&mut self) -> RobotResult<()> {
unimplemented!()
}
fn pause(&mut self) -> RobotResult<()> {
unimplemented!()
}
fn resume(&mut self) -> RobotResult<()> {
unimplemented!()
}
fn stop(&mut self) -> RobotResult<()> {
self.robot_impl._stop_program()?.into()
}
fn emergency_stop(&mut self) -> RobotResult<()> {
unimplemented!()
}
fn clear_emergency_stop(&mut self) -> RobotResult<()> {
self.robot_impl._clear_error()?.into()
}
fn read_state(&mut self) -> RobotResult<Self::State> {
let state = self.robot_state.read().unwrap();
Ok(state.clone())
}
}
impl<T: JakaType, const N: usize> Arm<N> for JakaRobot<T, N>
where
JakaRobot<T, N>: ArmParam<N>,
[f64; N]: Serialize + for<'a> Deserialize<'a>,
{
fn state(&mut self) -> RobotResult<ArmState<N>> {
let data = self.robot_impl._get_data()?;
let joint: [f64; N] = data.joint_actual_position[..N].try_into().unwrap();
let joint = joint.map(|f| f.to_radians());
let cartesian_tran = data.actual_position[0..3].try_into().unwrap();
let cartesian_rot: [f64; 3] = data.actual_position[3..6].try_into().unwrap();
let cartesian_rot = cartesian_rot.map(|f| f.to_radians());
let pose_o_to_ee = Pose::Euler(cartesian_tran, cartesian_rot);
let arm_state = ArmState {
joint: Some(joint),
joint_vel: None,
joint_acc: None,
pose_o_to_ee: Some(pose_o_to_ee),
pose_ee_to_k: None,
cartesian_vel: None,
load: None,
torque: None,
};
Ok(arm_state)
}
fn set_load(&mut self, load: LoadState) -> RobotResult<()> {
let set_load_data = SetPayloadData { mass: load.m, centroid: load.x };
self.robot_impl._set_payload(set_load_data)?.into()
}
fn set_coord(&mut self, coord: Coord) -> RobotResult<()> {
self.coord.set(coord);
Ok(())
}
fn set_scale(&mut self, scale: f64) -> RobotResult<()> {
self.max_vel.set(Self::JOINT_VEL_BOUND.map(|v| v * scale));
self.max_acc.set(Self::JOINT_ACC_BOUND.map(|v| v * scale));
self.max_cartesian_vel
.set(Self::CARTESIAN_VEL_BOUND * scale);
self.max_cartesian_acc
.set(Self::CARTESIAN_ACC_BOUND * scale);
Ok(())
}
fn with_coord(&mut self, coord: Coord) -> &mut Self {
self.coord.once(coord);
self
}
fn with_scale(&mut self, scale: f64) -> &mut Self {
self.max_vel.once(Self::JOINT_VEL_BOUND.map(|v| v * scale));
self.max_acc.once(Self::JOINT_ACC_BOUND.map(|v| v * scale));
self.max_cartesian_vel
.once(Self::CARTESIAN_VEL_BOUND * scale);
self.max_cartesian_acc
.once(Self::CARTESIAN_ACC_BOUND * scale);
self
}
fn with_velocity(&mut self, joint_vel: &[f64; N]) -> &mut Self {
self.max_vel.once(*joint_vel);
self
}
fn with_acceleration(&mut self, joint_acc: &[f64; N]) -> &mut Self {
self.max_acc.once(*joint_acc);
self
}
fn with_jerk(&mut self, _joint_jerk: &[f64; N]) -> &mut Self {
self
}
fn with_cartesian_velocity(&mut self, cartesian_vel: f64) -> &mut Self {
self.max_cartesian_vel.once(cartesian_vel);
self
}
fn with_cartesian_acceleration(&mut self, cartesian_acc: f64) -> &mut Self {
self.max_cartesian_acc.once(cartesian_acc);
self
}
fn with_cartesian_jerk(&mut self, _cartesian_jerk: f64) -> &mut Self {
self
}
fn with_rotation_velocity(&mut self, rotation_vel: f64) -> &mut Self {
self.max_rotation_vel.once(rotation_vel);
self
}
fn with_rotation_acceleration(&mut self, rotation_acc: f64) -> &mut Self {
self.max_rotation_acc.once(rotation_acc);
self
}
fn with_rotation_jerk(&mut self, _rotation_jerk: f64) -> &mut Self {
self
}
}
impl<T: JakaType, const N: usize> ArmPreplannedMotion<N> for JakaRobot<T, N>
where
JakaRobot<T, N>: Arm<N>,
[f64; N]: Serialize + for<'a> Deserialize<'a>,
{
fn move_joint(&mut self, target: &[f64; N]) -> RobotResult<()> {
self.move_joint_async(target)?;
self.waiting_for_finish()?;
Ok(())
}
fn move_joint_async(&mut self, target: &[f64; N]) -> RobotResult<()> {
if self.is_moving {
return Err(RobotException::CommandException(
"Robot is moving".to_string(),
));
}
self.is_moving = true;
let coord = self.coord.get();
let move_data = JointMoveData::<N> {
joint_position: rad_to_deg(*target),
speed: self.max_vel.get()[0].to_degrees(),
accel: self.max_acc.get()[0].to_degrees(),
relflag: u8::from(coord != Coord::OCS),
};
self.robot_impl._joint_move(move_data)?;
Ok(())
}
fn move_cartesian(&mut self, target: &Pose) -> RobotResult<()> {
self.move_cartesian_async(target)?;
self.waiting_for_finish()?;
Ok(())
}
fn move_cartesian_async(&mut self, target: &Pose) -> RobotResult<()> {
if self.is_moving {
return Err(RobotException::CommandException(
"Robot is moving".to_string(),
));
}
self.is_moving = true;
let mut pose: [f64; 6] = (*target).into();
for i in 0..3 {
pose[i] = pose[i] * 1000.0; pose[i + 3] = pose[i + 3].to_degrees();
}
let coord = self.coord.get();
let move_data = MoveLData {
cart_position: pose,
speed: self.max_cartesian_vel.get() * 1000.0, accel: self.max_cartesian_acc.get() * 1000.0, relflag: u8::from(coord != Coord::OCS),
};
self.robot_impl._move_l(move_data)?;
self.is_moving = false;
Ok(())
}
}
impl<T: JakaType, const N: usize> ArmPreplannedPath<N> for JakaRobot<T, N>
where
JakaRobot<T, N>: Arm<N>,
JakaRobot<T, N>: ArmParam<N>,
[f64; N]: Serialize + for<'a> Deserialize<'a>,
{
fn move_traj(&mut self, path: Vec<MotionType<N>>) -> RobotResult<()> {
self.move_traj_async(path)?;
self.waiting_for_finish()?;
Ok(())
}
fn move_traj_async(&mut self, path: Vec<MotionType<N>>) -> RobotResult<()> {
let mut path_iter = path.into_iter();
self.move_with_closure(move |_, _| {
if let Some(motion) = path_iter.next() {
(motion, false)
} else {
(MotionType::Joint([0.0; N]), true)
}
})
}
fn move_waypoints(&mut self, path: Vec<MotionType<N>>) -> RobotResult<()> {
self.move_waypoints_async(path)?;
self.waiting_for_finish()?;
Ok(())
}
fn move_waypoints_async(&mut self, path: Vec<MotionType<N>>) -> RobotResult<()> {
match path.first() {
Some(MotionType::Joint(first)) => {
let mut ruckig = Ruckig::<N, ThrowErrorHandler>::new(None, 1. / JAKA_FREQUENCY);
let mut input = InputParameter::new(None);
let mut output = OutputParameter::new(None);
input.max_velocity = DataArrayOrVec::Stack(Self::JOINT_VEL_BOUND);
input.max_acceleration = DataArrayOrVec::Stack(Self::JOINT_ACC_BOUND);
input.current_position = DataArrayOrVec::Stack(*first);
let mut new_path: Vec<MotionType<N>> = Vec::new();
for target in path {
if let MotionType::Joint(joint) = target {
input.target_position = DataArrayOrVec::Stack(joint);
loop {
let result = ruckig.update(&input, &mut output);
input.current_position = output.new_position.clone();
input.current_velocity = output.new_velocity.clone();
input.current_acceleration = output.new_acceleration.clone();
new_path
.push(MotionType::Joint(*output.new_position.as_array().unwrap()));
if let Ok(RuckigResult::Finished) = result {
break; }
}
} else {
return Err(RobotException::CommandException(
"Only joint waypoints are supported".to_string(),
));
}
}
self.move_traj_async(new_path)?;
}
_ => {
return Err(RobotException::CommandException(
"Only joint waypoints are supported".to_string(),
));
}
}
Ok(())
}
fn move_waypoints_prepare(&mut self, path: Vec<MotionType<N>>) -> RobotResult<()> {
self.path = Some(path);
Ok(())
}
fn move_waypoints_start(&mut self, _: MotionType<N>) -> RobotResult<()> {
if let Some(path) = self.path.take() {
if path.is_empty() {
return Err(RobotException::CommandException(
"Path is empty".to_string(),
));
}
self.move_waypoints_async(path)?;
} else {
return Err(RobotException::CommandException(
"Path is not prepared".to_string(),
));
}
Ok(())
}
}
pub struct JakaStreamingHandle<const N: usize> {
motion: Arc<Mutex<Option<MotionType<N>>>>,
}
impl<const N: usize> ArmStreamingHandle<N> for JakaStreamingHandle<N> {
fn move_to(&mut self, target: MotionType<N>) -> RobotResult<()> {
*self.motion.lock().unwrap() = Some(target);
Ok(())
}
fn last_motion(&self) -> Option<MotionType<N>> {
unimplemented!()
}
fn control_with(&mut self, _control: ControlType<N>) -> RobotResult<()> {
unimplemented!()
}
fn last_control(&self) -> Option<ControlType<N>> {
unimplemented!()
}
}
impl<T: JakaType, const N: usize> ArmStreamingMotion<N> for JakaRobot<T, N>
where
JakaRobot<T, N>: Arm<N>,
[f64; N]: Serialize + for<'a> Deserialize<'a>,
{
type Handle = JakaStreamingHandle<N>;
fn start_streaming(&mut self) -> RobotResult<Self::Handle> {
self.robot_impl._servo_move(ServoMoveData { relflag: 1 })?;
let motion = Arc::new(Mutex::new(None));
Ok(JakaStreamingHandle { motion })
}
fn end_streaming(&mut self) -> RobotResult<()> {
self.streaming_handle.thread().unpark();
self.robot_impl._servo_move(ServoMoveData { relflag: 0 })?;
Ok(())
}
fn move_to_target(&mut self) -> Arc<Mutex<Option<MotionType<N>>>> {
unimplemented!()
}
fn control_with_target(&mut self) -> Arc<Mutex<Option<ControlType<N>>>> {
unimplemented!()
}
}
impl<T: JakaType, const N: usize> Realtime for JakaRobot<T, N> {}
impl<T: JakaType, const N: usize> ArmRealtimeControl<N> for JakaRobot<T, N>
where
JakaRobot<T, N>: Arm<N>,
[f64; N]: Serialize + for<'a> Deserialize<'a>,
{
fn move_with_closure<FM>(&mut self, mut closure: FM) -> RobotResult<()>
where
FM: FnMut(ArmState<N>, std::time::Duration) -> (MotionType<N>, bool) + Send + 'static,
{
if self.is_moving {
return Err(RobotException::CommandException(
"Robot is moving".to_string(),
));
}
self.is_moving = true;
let mut robot = self.robot_impl.clone();
thread::spawn(move || {
robot._servo_move(ServoMoveData { relflag: 1 })?;
loop {
let state = ArmState::<N>::default();
let (motion, finished) =
closure(state.clone(), Duration::from_secs_f64(1. / JAKA_FREQUENCY));
if finished {
robot._servo_move(ServoMoveData { relflag: 0 })?;
break;
}
match motion {
MotionType::Joint(joint) => {
let data = ServoJData::<N> { joint_angles: rad_to_deg(joint), relflag: 0 };
robot._servo_j(data)?;
}
MotionType::Cartesian(pose) => {
let mut pose: [f64; 6] = pose.into();
for i in 0..3 {
pose[i] = pose[i] * 1000.0; pose[i + 3] = pose[i + 3].to_degrees();
}
let data = ServoPData { cat_position: pose, relflag: 0 };
robot._servo_p(data)?;
}
_ => {
return Err(RobotException::CommandException(
"Invalid motion type".to_string(),
));
}
}
}
Ok(())
});
Ok(())
}
fn control_with_closure<FC>(&mut self, mut _closure: FC) -> RobotResult<()>
where
FC: FnMut(ArmState<N>, std::time::Duration) -> (ControlType<N>, bool) + Send + 'static,
{
unimplemented!()
}
}
impl<T: JakaType, const N: usize> ArmRealtimeControlExt<N> for JakaRobot<T, N>
where
JakaRobot<T, N>: Arm<N>,
[f64; N]: Serialize + for<'a> Deserialize<'a>,
{
}