diff --git a/src/bin/scenario_1.rs b/src/bin/scenario_1.rs index cbdba7c..f9450c6 100644 --- 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 = ["at_conv", "above_conv", "pose_1", "pose_2"].iter().map(|f|f.to_spvalue()).collect(); - let tool_frames: Vec = ["tool0", "tool1"].iter().map(|f|f.to_spvalue()).collect(); + let tool_frames: Vec = ["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 index a49bc1e..3ee3f88 100644 --- 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,