diff --git a/c_transpiler/assets/sim_api.rs b/c_transpiler/assets/sim_api.rs index 61e86f6..22bf90f 100644 --- a/c_transpiler/assets/sim_api.rs +++ b/c_transpiler/assets/sim_api.rs @@ -381,4 +381,4 @@ pub trait Sim : RemoteApiClientInterface { (sim_yaw_pitch_roll_to_alpha_beta_gamma,"yawPitchRollToAlphaBetaGamma",(yaw_angle:f64,pitch_angle:f64,roll_angle:f64)->(f64,f64,f64)), (sim_yield,"yield"->()) } -} +} \ No newline at end of file diff --git a/c_transpiler/assets/sim_ik_api.py b/c_transpiler/assets/sim_ik_api.py index 13b1b74..19748e2 100644 --- a/c_transpiler/assets/sim_ik_api.py +++ b/c_transpiler/assets/sim_ik_api.py @@ -1,7 +1,7 @@ from typing import Optional, Protocol -class SimIk(Protocol): +class SimIK(Protocol): def addElement(self,environmentHandle:int, ikGroupHandle:int, tipDummyHandle:int)->int: ... diff --git a/c_transpiler/assets/sim_ik_api.rs b/c_transpiler/assets/sim_ik_api.rs index b1b5023..f9a9ee3 100644 --- a/c_transpiler/assets/sim_ik_api.rs +++ b/c_transpiler/assets/sim_ik_api.rs @@ -1,7 +1,7 @@ use crate::RemoteApiClientInterface; -pub trait SimIk : RemoteApiClientInterface { +pub trait SimIK : RemoteApiClientInterface { requests!{ -"sim_ik", +"simIK", (sim_ik_add_element,"addElement",(environment_handle:i64,ik_group_handle:i64,tip_dummy_handle:i64)->i64), (sim_ik_add_element_from_scene,"addElementFromScene",(environment_handle:i64,ik_group:i64,base_handle:i64,tip_handle:i64,target_handle:i64,constraints:i64)->(i64,serde_json::Value,serde_json::Value)), (sim_ik_compute_group_jacobian,"computeGroupJacobian",(environment_handle:i64,ik_group_handle:i64)->(Vec,Vec)), diff --git a/c_transpiler/convert.py b/c_transpiler/convert.py index 2773225..cec90ca 100644 --- a/c_transpiler/convert.py +++ b/c_transpiler/convert.py @@ -60,7 +60,7 @@ def main(): sim_h = assets / Path('sim_api_header.h') apis = [ - API(sim_ik_h, 'sim_ik', 'sim_ik_api'), + API(sim_ik_h, 'simIK', 'sim_ik_api'), API(sim_h, 'sim', 'sim_api'), ] diff --git a/c_transpiler/ir_transpiler/ir_to_macro_request_rust.py b/c_transpiler/ir_transpiler/ir_to_macro_request_rust.py index 78d6984..e9d6d73 100644 --- a/c_transpiler/ir_transpiler/ir_to_macro_request_rust.py +++ b/c_transpiler/ir_transpiler/ir_to_macro_request_rust.py @@ -3,10 +3,9 @@ import inflection -def ir_to_macro_request_rust(assign: FunctionAssign, file_name: str) -> str: +def ir_to_macro_request_rust(assign: FunctionAssign, api_name: str) -> str: return_type = type_node_to_rust(assign.return_type) - - rust_func_name = f'{file_name}_{inflection.underscore(assign.function_name)}' + rust_func_name = f'{inflection.underscore(api_name)}_{inflection.underscore(assign.function_name)}' required_args = [] option_args = [] diff --git a/c_transpiler/tests/test_ir.py b/c_transpiler/tests/test_ir.py index 2e08608..cf46fb3 100644 --- a/c_transpiler/tests/test_ir.py +++ b/c_transpiler/tests/test_ir.py @@ -127,20 +127,6 @@ def test_ir_to_cpp(self): result = [ir_to_cpp(ir) for ir in inputs] self.assertEqualStrings(result, expected_strings) - def test_ir_parser_remote_api_header(self): - assets = Path('assets') - header = assets / Path('sim_api_header.h') - expected_h = assets / Path('expected.h') - - content = header.read_text() - stream = StringStream(content) - scanner = Scanner(stream) - - assigns = parser(scanner, stream) - ir = [ir_to_cpp(assign) for assign in assigns] - result_content = "\n".join(ir) - - self.assertEqual(result_content + '\n', expected_h.read_text()) def test_ir_to_string(self): vec_u8_ir = TypeNode(TokenType.VEC, [TypeNode(TokenType.U8, [])]) @@ -242,7 +228,7 @@ def test_ir_to_macro_request_rust(self): '(test_get_vision_sensor_depth_buffer,"getVisionSensorDepthBuffer",(sensor_handle:i64),opt(pos:Vec,size:Vec)->(Vec,Vec))' ] result = [ir_to_macro_request_rust( - ir, file_name='test') for ir in inputs] + ir, api_name='test') for ir in inputs] self.assertEqualStrings(result, expected_strings) def assertEqualStrings(self, result: list[str], expected: list[str]): diff --git a/examples/sim_ik_example.rs b/examples/sim_ik_example.rs new file mode 100644 index 0000000..6d1a152 --- /dev/null +++ b/examples/sim_ik_example.rs @@ -0,0 +1,108 @@ +use serde_json::json; +use zmq_remote_api::{ + sim, + sim::{Sim, SimIK}, + RemoteAPIError, RemoteApiClientInterface, RemoteApiClientParams, +}; + +/* +# +# Example based in scenes/tutorials/InverseKinematics/redundantRobot.ttt +# Make sure to have CoppeliaSim running, with following scene loaded: +# +# scenes/redundantRobot.ttt +# +# Do not launch simulation, but run this script +*/ + +fn main() -> Result<(), RemoteAPIError> { + // use the env variable RUST_LOG="trace" or RUST_LOG="debug" to observe the zmq communication + env_logger::init(); + + println!("Program started"); + + let client = zmq_remote_api::RemoteApiClient::new(RemoteApiClientParams { + host: "localhost".to_string(), + ..RemoteApiClientParams::default() + })?; + + client.require(sim::Module::SimIK)?; + + // When simulation is not running, ZMQ message handling could be a bit + // slow, since the idle loop runs at 8 Hz by default. So let's make + // sure that the idle loop runs at full speed for this program: + + let default_idle_fps = client.sim_get_int32_param(sim::INTPARAM_IDLE_FPS)?; + client.sim_set_int32_param(sim::INTPARAM_IDLE_FPS, 0)?; + + let tip = client.sim_get_object("./redundantRob_tip".into(), None)?; + let target = client.sim_get_object("./redundantRob_target".into(), None)?; + let base = client.sim_get_object("./redundantRobot".into(), None)?; + + // Create an IK enviroment + let ik_env = client.sim_ik_create_environment(None)?; + + // create an IK group + let ik_group = client.sim_ik_create_group(ik_env, None)?; + + // set its resolution method to undamped + client.sim_ik_set_group_calculation(ik_env, ik_group, sim::IK_PSEUDO_INVERSE_METHOD, 0.0, 6)?; + + let constraint_pos = sim::IK_X_CONSTRAINT | sim::IK_Y_CONSTRAINT | sim::IK_Z_CONSTRAINT; + let constraint_ori = sim::IK_ALPHA_BETA_CONSTRAINT | sim::IK_GAMMA_CONSTRAINT; + let constraint_pose = constraint_pos | constraint_ori; + // create an IK element based on the scene content + client.sim_ik_add_element_from_scene(ik_env, ik_group, base, tip, target, constraint_pose)?; + + // create another IK group + let ik_group_damped = client.sim_ik_create_group(ik_env, None)?; + client.sim_ik_set_group_calculation( + ik_env, + ik_group_damped, + sim::IK_DAMPED_LEAST_SQUARES_METHOD, + 1.0, + 99, + )?; + + // create an IK element based on the scene content: + client.sim_ik_add_element_from_scene( + ik_env, + ik_group_damped, + base, + tip, + target, + constraint_pose, + )?; + + client.sim_start_simulation()?; + + while client.sim_get_simulation_state()? != sim::SIMULATION_ADVANCING_RUNNING { + std::thread::sleep(std::time::Duration::from_secs_f64(0.1)) + } + + while client.sim_get_simulation_state()? == sim::SIMULATION_ADVANCING_RUNNING { + // try to solve with the undamped method: + let (status, _, _) = + client.sim_ik_handle_group(ik_env, ik_group, Some(json!({"syncWorlds" : true})))?; + + if status != sim::IKRESULT_SUCCESS { + // try to solve with the damped method: + client.sim_ik_handle_group( + ik_env, + ik_group_damped, + Some(json!({"syncWorlds" : true})), + )?; + } + } + + // if you need to make sure we really stopped: + while client.sim_get_simulation_state()? != sim::SIMULATION_STOPPED { + std::thread::sleep(std::time::Duration::from_secs_f64(0.1)) + } + + client.sim_ik_erase_environment(ik_env)?; + client.sim_set_int32_param(sim::INTPARAM_IDLE_FPS, default_idle_fps)?; + println!("Program ended"); + + Ok(()) +} diff --git a/scenes/redundantRobot.ttt b/scenes/redundantRobot.ttt index a7b0afc..a4e339c 100644 Binary files a/scenes/redundantRobot.ttt and b/scenes/redundantRobot.ttt differ diff --git a/src/remote_api_client/client.rs b/src/remote_api_client/client.rs index f7ab657..87ea8dc 100644 --- a/src/remote_api_client/client.rs +++ b/src/remote_api_client/client.rs @@ -4,7 +4,7 @@ use uuid::Uuid; use crate::remote_api_client::RemoteApiClientInterface; use crate::sim::Sim; -use crate::sim::SimIk; +use crate::sim::SimIK; use crate::zmq_requests::{RawRequest, ZmqRequest}; use crate::{log_utils, RemoteAPIError, RemoteApiClientParams}; @@ -116,5 +116,5 @@ impl Drop for RemoteApiClient { impl Sim for RemoteApiClient {} impl Sim for &RemoteApiClient {} -impl SimIk for RemoteApiClient {} -impl SimIk for &RemoteApiClient {} +impl SimIK for RemoteApiClient {} +impl SimIK for &RemoteApiClient {} diff --git a/src/remote_api_client/remote_api_client_trait.rs b/src/remote_api_client/remote_api_client_trait.rs index acbda38..97da196 100644 --- a/src/remote_api_client/remote_api_client_trait.rs +++ b/src/remote_api_client/remote_api_client_trait.rs @@ -1,11 +1,22 @@ use serde_json::Value as JsonValue; -use crate::{RawRequest, RemoteAPIError, ZmqRequest}; +use crate::{sim::Module, RawRequest, RemoteAPIError, ZmqRequest}; pub trait RemoteApiClientInterface { fn send_raw_request(&self, request: Vec) -> Result; fn get_uuid(&self) -> String; + fn require(&self, module: Module) -> Result<(), RemoteAPIError> { + let name = match module { + Module::SimIK => "simIK".into(), + }; + + let require_req = ZmqRequest::require_request(name, self.get_uuid()); + self.send_request(require_req)?; + + Ok(()) + } + fn send_request( &self, request: ZmqRequest, diff --git a/src/remote_api_objects/sim/mod.rs b/src/remote_api_objects/sim/mod.rs index b072d61..e17c869 100644 --- a/src/remote_api_objects/sim/mod.rs +++ b/src/remote_api_objects/sim/mod.rs @@ -4,7 +4,11 @@ mod sim_ik_api; pub use sim_api::Sim; pub use sim_const::*; -pub use sim_ik_api::SimIk; +pub use sim_ik_api::SimIK; + +pub enum Module { + SimIK, +} /* this suite test mocks the zmqClient, so it's possible diff --git a/src/remote_api_objects/sim/sim_api.rs b/src/remote_api_objects/sim/sim_api.rs index 1f019b6..97ba155 100644 --- a/src/remote_api_objects/sim/sim_api.rs +++ b/src/remote_api_objects/sim/sim_api.rs @@ -381,4 +381,4 @@ pub trait Sim: RemoteApiClientInterface { (sim_yaw_pitch_roll_to_alpha_beta_gamma,"yawPitchRollToAlphaBetaGamma",(yaw_angle:f64,pitch_angle:f64,roll_angle:f64)->(f64,f64,f64)), (sim_yield,"yield"->()) } -} // test +} diff --git a/src/remote_api_objects/sim/sim_ik_api.rs b/src/remote_api_objects/sim/sim_ik_api.rs index ef27988..f39411b 100644 --- a/src/remote_api_objects/sim/sim_ik_api.rs +++ b/src/remote_api_objects/sim/sim_ik_api.rs @@ -1,7 +1,7 @@ use crate::RemoteApiClientInterface; -pub trait SimIk: RemoteApiClientInterface { +pub trait SimIK: RemoteApiClientInterface { requests! { - "sim_ik", + "simIK", (sim_ik_add_element,"addElement",(environment_handle:i64,ik_group_handle:i64,tip_dummy_handle:i64)->i64), (sim_ik_add_element_from_scene,"addElementFromScene",(environment_handle:i64,ik_group:i64,base_handle:i64,tip_handle:i64,target_handle:i64,constraints:i64)->(i64,serde_json::Value,serde_json::Value)), (sim_ik_compute_group_jacobian,"computeGroupJacobian",(environment_handle:i64,ik_group_handle:i64)->(Vec,Vec)), diff --git a/src/zmq_requests/zmq_request.rs b/src/zmq_requests/zmq_request.rs index c0caaea..2abdb29 100644 --- a/src/zmq_requests/zmq_request.rs +++ b/src/zmq_requests/zmq_request.rs @@ -46,6 +46,16 @@ impl ZmqRequest { lang: LANG.into(), } } + + pub fn require_request(name: String, uuid: String) -> ZmqRequest { + ZmqRequest { + uuid, + func: "zmqRemoteApi.require".into(), + args: vec![cbor!(name).unwrap()], + ver: VERSION, + lang: LANG.into(), + } + } } impl RawRequest for ZmqRequest {