compile to webassembly for javascript wrappers

.gitignore

@@ -10,4 +10,7 @@ Cargo.lock
 # These are backup files generated by rustfmt
 **/*.rs.bk
 
-wrappers/__pycache__/
+wrappers/__pycache__/
+
+# webassembly files
+/pkg/

Cargo.toml

@@ -7,14 +7,13 @@ edition = "2018"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
-nalgebra = "0.30"
+nalgebra = {version = "0.30", features = ["serde-serialize"]}
 yaml-rust = "0.4"
-optimization_engine = "0.6.0"
+optimization_engine =  { version = "0.7.7", features = ["wasm"] }
 ndarray = "0.13.0"
 num = "0.2.0"
 time = "0.1"
 rand = "0.7.2"
-# ncollide3d = "0.21.0"
 parry3d-f64 = "0.8.0"
 path-slash = "0.1.2"
 lazy_static = "1.4.0"
@@ -25,13 +24,22 @@ dirs = "4.0.0"
 urdf-rs =  "0.6.7"
 k = "0.29"
 
+# Below are for webassembly
+wasm-bindgen = { version = "0.2", features = ["serde-serialize"] }
+js-sys = "0.3"
+serde_json = "1.0"
+console_error_panic_hook = "0.1"
+
+
 [profile.dev]
 opt-level = 3
 
 [profile.release]
 opt-level = 3
 
 [lib]
-name = "relaxed_ik_lib"
+# name = "relaxed_ik_lib"
 path = "src/lib.rs"
-crate-type = ["rlib", "dylib"]
+# crate-type = ["rlib", "dylib", "cdylib"]
+# path = "src/relaxed_ik_web.rs"
+crate-type = ["cdylib"]

configs/example_settings/baxter.yaml

@@ -7,4 +7,5 @@ ee_links:
   - right_hand
   - left_hand
 starting_config: [0.0, -0.0003125999999995521, 1.5704593245206757, 0.8793927999999999, 0.0, -6.462112780614149e-05, 0.0, 0.0, -0.0003125999999995521, -1.5704593245206757, 0.8793927999999999, 0.0, -6.462112780614149e-05, 0.0]
+joint_ordering: [ "right_s0", "right_s1", "right_e0", "right_e1", "right_w0", "right_w1", "right_w2", "left_s0", "left_s1", "left_e0", "left_e1", "left_w0", "left_w1", "left_w2" ]
 obstacles:

configs/example_settings/sawyer.yaml

@@ -5,4 +5,5 @@ base_links:
 ee_links:
   - right_hand
 starting_config: [ 0.0, 0.0, -1.5708, 1.5708, 0.0, -1.5708, 0.0 ]
+joint_ordering: [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ]
 obstacles:

configs/example_settings/spot_arm.yaml

@@ -4,5 +4,6 @@ base_links:
   - body
 ee_links:
   - tool_tip
-starting_config: [ 0.0, -1.10, 2.00, 0.0, -0.90, 0.0 ]
+starting_config: [ 0.4, -2.6, 2.9, -2.3, -0.30, -0.3 ]
+joint_ordering: ["arm_joint1", "arm_joint2", "arm_joint3", "arm_joint4", "arm_joint5", "arm_joint6"]
 obstacles:

configs/example_settings/ur5.yaml

@@ -5,4 +5,5 @@ base_links:
 ee_links:
   - tool0
 starting_config: [3.14, -1.95, -1.2, -3.14, -1.57, -1.57]
+joint_ordering: [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ]
 obstacles:

src/groove/objective_master.rs

@@ -30,17 +30,17 @@ impl ObjectiveMaster {
         let mut num_dofs = 0;
         for i in 0..num_chains {
             objectives.push(Box::new(MatchEEPosiDoF::new(i, 0)));
-            weight_priors.push(1.0);
+            weight_priors.push(50.0);
             objectives.push(Box::new(MatchEEPosiDoF::new(i, 1)));
-            weight_priors.push(1.0);
+            weight_priors.push(50.0);
             objectives.push(Box::new(MatchEEPosiDoF::new(i, 2)));
-            weight_priors.push(1.0);
+            weight_priors.push(50.0);
             objectives.push(Box::new(MatchEERotaDoF::new(i, 0)));
-            weight_priors.push(1.0);
+            weight_priors.push(10.0);
             objectives.push(Box::new(MatchEERotaDoF::new(i, 1)));
-            weight_priors.push(1.0);
+            weight_priors.push(10.0);
             objectives.push(Box::new(MatchEERotaDoF::new(i, 2)));
-            weight_priors.push(1.0);
+            weight_priors.push(10.0);
             // objectives.push(Box::new(EnvCollision::new(i)));
             // weight_priors.push(1.0);
             num_dofs += chain_lengths[i];

src/groove/vars.rs

@@ -7,29 +7,18 @@ use yaml_rust::{YamlLoader, Yaml};
 use std::fs::File;
 use std::io::prelude::*;
 
-#[derive(Clone, Debug)]
-pub struct Vars {
-    pub init_state: Vec<f64>,
-    pub xopt: Vec<f64>,
-    pub prev_state: Vec<f64>,
-    pub prev_state2: Vec<f64>,
-    pub prev_state3: Vec<f64>
-}
-impl Vars {
-    pub fn new(init_state: Vec<f64>) -> Self {
-        Vars{init_state: init_state.clone(), xopt: init_state.clone(), prev_state: init_state.clone(),
-            prev_state2: init_state.clone(), prev_state3: init_state.clone()}
-    }
+use wasm_bindgen::prelude::*;
+use serde::{Serialize, Deserialize};
 
-    pub fn update(&mut self, xopt: Vec<f64>) {
-        self.prev_state3 = self.prev_state2.clone();
-        self.prev_state2 = self.prev_state.clone();
-        self.prev_state = self.xopt.clone();
-        self.xopt = xopt.clone();
-    }
+#[derive(Serialize, Deserialize)]
+pub struct VarsConstructorData {
+    pub urdf: String,
+    pub link_radius:f64,
+    pub base_links: Vec<String>,
+    pub ee_links: Vec<String>,
+    starting_config: Vec<f64>
 }
 
-
 pub struct RelaxedIKVars {
     pub robot: Robot,
     pub init_state: Vec<f64>,
@@ -39,10 +28,12 @@ pub struct RelaxedIKVars {
     pub prev_state3: Vec<f64>,
     pub goal_positions: Vec<Vector3<f64>>,
     pub goal_quats: Vec<UnitQuaternion<f64>>,
-    pub tolerances: Vec<Vector6<f64>>
+    pub tolerances: Vec<Vector6<f64>>,
+    pub init_ee_positions: Vec<Vector3<f64>>,
+    pub init_ee_quats: Vec<UnitQuaternion<f64>>
 }
 impl RelaxedIKVars {
-    pub fn from_settings(path_to_setting: &str) -> Self {
+    pub fn from_local_settings(path_to_setting: &str) -> Self {
         let path_to_src = get_path_to_src();
         let mut file = File::open(path_to_setting).unwrap();
         let mut contents = String::new();
@@ -54,20 +45,21 @@ impl RelaxedIKVars {
         println!("RelaxedIK is using below URDF file: {}", path_to_urdf);
         let chain = k::Chain::<f64>::from_urdf_file(path_to_urdf.clone()).unwrap();
 
-        let num_chains = settings["base_links"].as_vec().unwrap().len();
-
         let base_links_arr = settings["base_links"].as_vec().unwrap();
         let ee_links_arr = settings["ee_links"].as_vec().unwrap();
+        let num_chains = base_links_arr.len();
+
         let mut base_links = Vec::new();
         let mut ee_links = Vec::new();
         let mut tolerances: Vec<Vector6<f64>> = Vec::new();
-        for i in 0..base_links_arr.len() {
+        for i in 0..num_chains {
             base_links.push(base_links_arr[i].as_str().unwrap().to_string());
             ee_links.push(ee_links_arr[i].as_str().unwrap().to_string());
             tolerances.push(Vector6::new(0., 0., 0., 0., 0., 0.));
         }
 
-        let robot = Robot::from_urdf(path_to_urdf.clone(), &base_links, &ee_links);
+        let urdf = &std::fs::read_to_string(path_to_urdf).unwrap();
+        let robot = Robot::from_urdf(urdf, &base_links, &ee_links);
 
         let mut starting_config = Vec::new();
         if settings["starting_config"].is_badvalue() {
@@ -82,19 +74,98 @@ impl RelaxedIKVars {
             }
         }
 
-        let mut goal_positions: Vec<Vector3<f64>> = Vec::new();
-        let mut goal_quats: Vec<UnitQuaternion<f64>> = Vec::new();
+        let mut init_ee_positions: Vec<Vector3<f64>> = Vec::new();
+        let mut init_ee_quats: Vec<UnitQuaternion<f64>> = Vec::new();
         let pose = robot.get_ee_pos_and_quat_immutable(&starting_config);
+        assert!(pose.len() == num_chains);
+
         for i in 0..pose.len() {
-            goal_positions.push(pose[i].0);
-            goal_quats.push(pose[i].1);
+            init_ee_positions.push(pose[i].0);
+            init_ee_quats.push(pose[i].1);
         }
 
         RelaxedIKVars{robot, init_state: starting_config.clone(), xopt: starting_config.clone(),
             prev_state: starting_config.clone(), prev_state2: starting_config.clone(), prev_state3: starting_config.clone(),
-            goal_positions, goal_quats, tolerances}
+            goal_positions: init_ee_positions.clone(), goal_quats: init_ee_quats.clone(), tolerances, init_ee_positions, init_ee_quats}
     }
 
+    // for webassembly
+    pub fn from_jsvalue( configs: VarsConstructorData, urdf: &str) -> Self  {
+
+        let num_chains = configs.base_links.len();
+
+        let mut tolerances: Vec<Vector6<f64>> = Vec::new();
+        for i in 0..num_chains {
+            tolerances.push(Vector6::new(0., 0., 0., 0., 0., 0.));
+        }
+
+        let robot = Robot::from_urdf(urdf, &configs.base_links, &configs.ee_links);
+
+        let mut init_ee_positions: Vec<Vector3<f64>> = Vec::new();
+        let mut init_ee_quats: Vec<UnitQuaternion<f64>> = Vec::new();
+        let pose = robot.get_ee_pos_and_quat_immutable(&configs.starting_config);
+        assert!(pose.len() == num_chains);
+
+        for i in 0..pose.len() {
+            init_ee_positions.push(pose[i].0);
+            init_ee_quats.push(pose[i].1);
+        }
+
+        RelaxedIKVars{robot, init_state: configs.starting_config.clone(), xopt: configs.starting_config.clone(),
+            prev_state: configs.starting_config.clone(), prev_state2: configs.starting_config.clone(), prev_state3: configs.starting_config.clone(),
+            goal_positions: init_ee_positions.clone(), goal_quats: init_ee_quats.clone(), tolerances, init_ee_positions, init_ee_quats}
+
+    }
+
+    // pub fn new( config: VarsConstructorData) -> Self {
+
+    //     let robotConfig = RobotConstructorData {
+    //         num_chains: config.joint_names.len(),
+    //         num_dof: config.joint_ordering.len(),
+    //         joint_names: config.joint_names,
+    //         joint_ordering: config.joint_ordering,
+    //         collision_file_name: config.collision_file_name,
+    //         collision_nn_file: config.collision_nn_file,
+    //         axis_types: config.axis_types,
+    //         velocity_limits: config.velocity_limits,
+    //         joint_limits: config.joint_limits,
+    //         displacements: config.displacements,
+    //         disp_offsets: config.disp_offsets,
+    //         rot_offsets: config.rot_offsets,
+    //         joint_types: config.joint_types
+    //     };
+    //     let mut robot = Robot::new(robotConfig);
+    //     let num_chains = robot.num_chains;
+    //     let position_mode_relative = true;
+    //     let rotation_mode_relative = true; 
+    //     let starting_config = config.starting_config;
+
+    //     let mut goal_positions: Vec<Vector3<f64>> = Vec::new();
+    //     let mut goal_quats: Vec<UnitQuaternion<f64>> = Vec::new();
+    //     let mut goal_quat_vectors: Vec<Vec<Vector3<f64>>> = Vec::new();
+    //     let mut goal_quat_constraints: Vec<Vec<u32>> = Vec::new();
+
+    //     let init_ee_positions = robot.get_ee_positions(starting_config.as_slice());
+    //     let init_ee_quats = robot.get_ee_quats(starting_config.as_slice());
+
+    //     for i in 0..num_chains {
+    //         goal_positions.push(init_ee_positions[i]);
+    //         goal_quats.push(init_ee_quats[i]);
+    //     }
+
+    //     let collision_nn = CollisionNN::new(nn_config);
+
+    //     let env_collision_file = EnvCollisionFileParser::new(env_config);
+    //     let frames = robot.get_frames_immutable(&starting_config.clone());
+    //     let env_collision = RelaxedIKEnvCollision::init_collision_world(env_collision_file, &frames);
+
+    //     RelaxedIKVars{robot, init_state: starting_config.clone(), xopt: starting_config.clone(),
+    //         prev_state: starting_config.clone(), prev_state2: starting_config.clone(), prev_state3: starting_config.clone(),
+    //         goal_positions, goal_quats,
+    //         init_ee_positions, init_ee_quats, position_mode_relative, rotation_mode_relative,
+    //         collision_nn, env_collision}
+    // }
+
     pub fn update(&mut self, xopt: Vec<f64>) {
         self.prev_state3 = self.prev_state2.clone();
         self.prev_state2 = self.prev_state.clone();

src/lib.rs

@@ -2,4 +2,5 @@ pub mod utils_rust;
 pub mod spacetime;
 pub mod groove;
 pub mod relaxed_ik;
-pub mod relaxed_ik_wrapper;
+pub mod relaxed_ik_wrapper;
+pub mod relaxed_ik_web;

src/relaxed_ik.rs

@@ -22,7 +22,7 @@ impl RelaxedIK {
     pub fn load_settings( path_to_setting: &str) -> Self {
         println!("RelaxedIK is using below setting file {}", path_to_setting);
 
-        let vars = RelaxedIKVars::from_settings(path_to_setting);
+        let vars = RelaxedIKVars::from_local_settings(path_to_setting);
         let om = ObjectiveMaster::relaxed_ik(&vars.robot.chain_lengths);
 
         let groove = OptimizationEngineOpen::new(vars.robot.num_dofs.clone());