changed frames and speed

sequenceplanner · Apr 22, 2022 · 62bf239 · 62bf239
1 parent 1d3c69e
commit 62bf239
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 19 deletions.
diff --git a/src/bin/scenario_1.rs b/src/bin/scenario_1.rs
@@ -18,7 +18,7 @@ pub fn make_model() -> (Model, SPState) {
     let ur = m.add_resource("ur");
     let frames: Vec<SPValue> = ["at_conv", "above_conv",
                                 "pose_1", "pose_2"].iter().map(|f|f.to_spvalue()).collect();
-    let tool_frames: Vec<SPValue> = ["tool0", "tool1"].iter().map(|f|f.to_spvalue()).collect();
+    let tool_frames: Vec<SPValue> = ["robotiq_2f_tcp", "tool1"].iter().map(|f|f.to_spvalue()).collect();
     let mut ur = UrRobotResource::new(m.get_resource(&ur), frames, tool_frames);
 
     let gripper = m.add_resource("gripper");
@@ -34,32 +34,32 @@ pub fn make_model() -> (Model, SPState) {
     let guard2 = p!(est_pos == "pose_1");
     // let runner_guard = p!(plc.bool_from_plc_1);
     let runner_guard = Predicate::TRUE;
-    ur.run_transition(&mut m, guard1, runner_guard.clone(),
-                      "tool0", "pose_1", "move_j", 0.8, 0.5, vec![], vec![]);
-    ur.run_transition(&mut m, guard2, runner_guard.clone(),
-                      "tool0", "pose_2", "move_j", 0.4, 0.3, vec![], vec![]);
+    ur.create_transition(&mut m, guard1, runner_guard.clone(),
+                      "robotiq_2f_tcp", "pose_1", "move_j", 0.1, 0.3, vec![], vec![]);
+    ur.create_transition(&mut m, guard2, runner_guard.clone(),
+                      "robotiq_2f_tcp", "pose_2", "move_j", 0.1, 0.3, vec![], vec![]);
 
 
-    ur.run_transition(&mut m,
+    ur.create_transition(&mut m,
                       p!(est_pos == "pose_1"),
                       Predicate::TRUE,
-                      "tool0", "above_conv", "move_j", 0.8, 0.5, vec![], vec![]);
+                      "robotiq_2f_tcp", "above_conv", "move_j", 0.1, 0.3, vec![], vec![]);
 
-    ur.run_transition(&mut m,
+    ur.create_transition(&mut m,
                       p!(est_pos == "above_conv"),
                       Predicate::TRUE,
-                      "tool0", "pose_1", "move_j", 0.8, 0.5, vec![], vec![]);
+                      "robotiq_2f_tcp", "pose_1", "move_j", 0.1, 0.3, vec![], vec![]);
 
-    ur.run_transition(&mut m,
+    ur.create_transition(&mut m,
                       p!([est_pos == "above_conv"] &&
                          [[(gripper.measured) == "opened"] || [(gripper.measured) == "gripping"]]),
                       Predicate::TRUE,
-                      "tool0", "at_conv", "move_j", 0.8, 0.5, vec![], vec![]);
+                      "robotiq_2f_tcp", "at_conv", "move_j", 0.1, 0.3, vec![], vec![]);
 
-    ur.run_transition(&mut m,
+    ur.create_transition(&mut m,
                       p!([est_pos == "at_conv"]),
                       Predicate::TRUE,
-                      "tool0", "above_conv", "move_j", 0.8, 0.5, vec![], vec![]);
+                      "robotiq_2f_tcp", "above_conv", "move_j", 0.1, 0.3, vec![], vec![]);
 
     // can only grip in certain positions.
     m.add_invar(
@@ -137,7 +137,8 @@ pub fn make_model() -> (Model, SPState) {
     m.add_intention(
         "back",
         true,
-        &p!(!op_done),
+        // &p!(!op_done),
+        &Predicate::FALSE,
         &p!(op_done),
         &[],
     );

diff --git a/src/resources/ur.rs b/src/resources/ur.rs
@@ -49,10 +49,10 @@ impl UrRobotResource {
         ));
 
         let acceleration= resource.add_variable(Variable::new(
-            "request/acceleration",VariableType::Runner,SPValueType::Float32,vec!(1.0.to_spvalue()),
+            "request/acceleration",VariableType::Runner,SPValueType::Float32,vec!(0.1.to_spvalue()),
         ));
         let velocity= resource.add_variable(Variable::new(
-            "request/velocity",VariableType::Runner,SPValueType::Float32,vec!(1.0.to_spvalue()),
+            "request/velocity",VariableType::Runner,SPValueType::Float32,vec!(0.1.to_spvalue()),
         ));
         let goal_feature_name= resource.add_variable(Variable::new(
             "request/goal_feature_name",VariableType::Command,SPValueType::String, frame_domain.clone(),
@@ -201,9 +201,9 @@ impl UrRobotResource {
                 (tcp_name.clone(), tool_frame_domain[1].clone()),
                 (last_visited_frame.clone(), frame_domain[0].clone()),
                 (last_visited_with_tcp.clone(), tool_frame_domain[0].clone()),
-                (velocity_scaling, 0.5.to_spvalue()),
+                (velocity_scaling, 0.1.to_spvalue()),
                 (velocity.clone(), 0.1.to_spvalue()),
-                (acceleration_scaling, 0.5.to_spvalue()),
+                (acceleration_scaling, 0.1.to_spvalue()),
                 (acceleration.clone(), 0.1.to_spvalue()),
                 (trigger.clone(), false.to_spvalue()),
                 (done.clone(), false.to_spvalue()),
@@ -231,7 +231,7 @@ impl UrRobotResource {
         }
     }
 
-    pub fn run_transition(
+    pub fn create_transition(
         &mut self,
         model: &mut Model,
         guard: Predicate,