From 0e9aa3a92c802757fe26fefff9d2ddb457833052 Mon Sep 17 00:00:00 2001 From: jk-ethz Date: Mon, 28 Aug 2023 10:28:03 +0200 Subject: [PATCH 1/4] Add speedup of robot motions --- moma_mission/config/state_machine/piloting.yaml | 8 +++++--- moma_mission/demo/piloting.py | 3 ++- moma_mission/src/moma_mission/states/gripper.py | 2 +- moma_robot/config/controllers.yaml | 8 ++++---- panda_control/config/controllers.yaml | 8 ++++---- 5 files changed, 16 insertions(+), 13 deletions(-) diff --git a/moma_mission/config/state_machine/piloting.yaml b/moma_mission/config/state_machine/piloting.yaml index f9368c265..0ed4cd1c2 100644 --- a/moma_mission/config/state_machine/piloting.yaml +++ b/moma_mission/config/state_machine/piloting.yaml @@ -236,13 +236,15 @@ GRASP_VALVE: section: "first" timeout: 15.0 delay: 1.0 - linear_speed: 0.01 + linear_speed: 0.05 MANIPULATE_VALVE: default_outcome: None mode: pose - linear_speed: 0.1 - angular_speed: 1.0 + linear_speed: 0.4 + angular_speed: 1.5 + linear_tolerance: 0.05 + angular_tolerance: 0.18 control_frame: panda_link0 pose_topic: "/panda/cartesian_impedance_controller/equilibrium_pose" manager_namespace: panda diff --git a/moma_mission/demo/piloting.py b/moma_mission/demo/piloting.py index 9c839e3d6..15fcdd349 100755 --- a/moma_mission/demo/piloting.py +++ b/moma_mission/demo/piloting.py @@ -314,7 +314,8 @@ valve_sequence.add( "STORE_FINAL_POSE", TransformRecorderState, - transitions={"Completed": "APPROACH_FINAL_POSE", "Failure": "Failure"}, + # transitions={"Completed": "APPROACH_FINAL_POSE", "Failure": "Failure"}, + transitions={"Completed": "OPEN_GRIPPER", "Failure": "Failure"}, ) rospy.loginfo("Approach final pose") diff --git a/moma_mission/src/moma_mission/states/gripper.py b/moma_mission/src/moma_mission/states/gripper.py index 2e8adb367..0ef5dae05 100644 --- a/moma_mission/src/moma_mission/states/gripper.py +++ b/moma_mission/src/moma_mission/states/gripper.py @@ -167,7 +167,7 @@ def _set_goal(self): self.gripper_goal.epsilon.outer = ( 1.0 # how much fingers can open more than specified ) - self.gripper_goal.speed = 0.01 + self.gripper_goal.speed = 0.1 self.gripper_goal.width = self.position self.gripper_goal.force = self.max_effort diff --git a/moma_robot/config/controllers.yaml b/moma_robot/config/controllers.yaml index f4da3db36..68da6b439 100644 --- a/moma_robot/config/controllers.yaml +++ b/moma_robot/config/controllers.yaml @@ -35,8 +35,8 @@ joint_space_controller: - panda_joint7 lower_limit: [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973] upper_limit: [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973] - max_velocity: 0.3 - max_acceleration: 0.1 + max_velocity: 0.6 + max_acceleration: 0.15 gain: 1.0 tolerance: 0.017 # 1 deg pid_gains: @@ -69,8 +69,8 @@ joint_velocity_controller: lower_limit: [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973] upper_limit: [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973] safety_margin: 0.1 - max_velocity: 0.3 - max_acceleration: 0.1 + max_velocity: 0.6 + max_acceleration: 0.15 max_deceleration: 0.3 gain: 1.0 diff --git a/panda_control/config/controllers.yaml b/panda_control/config/controllers.yaml index ce834fd55..fdc16c905 100644 --- a/panda_control/config/controllers.yaml +++ b/panda_control/config/controllers.yaml @@ -25,8 +25,8 @@ joint_space_controller: - panda_joint7 lower_limit: [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973] upper_limit: [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973] - max_velocity: 0.3 - max_acceleration: 0.1 + max_velocity: 0.6 + max_acceleration: 0.15 gain: 1.0 tolerance: 0.017 # 1 deg pid_gains: @@ -59,8 +59,8 @@ joint_velocity_controller: lower_limit: [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973] upper_limit: [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973] safety_margin: 0.1 - max_velocity: 0.3 - max_acceleration: 0.1 + max_velocity: 0.6 + max_acceleration: 0.15 max_deceleration: 0.3 gain: 1.0 From 8990b113ca7d36716c40adae67ceedccd9f02028 Mon Sep 17 00:00:00 2001 From: jk-ethz Date: Mon, 28 Aug 2023 10:43:27 +0200 Subject: [PATCH 2/4] FIx delay when switching to PathVisitorState --- moma_mission/src/moma_mission/states/path_visitor.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/moma_mission/src/moma_mission/states/path_visitor.py b/moma_mission/src/moma_mission/states/path_visitor.py index 45e32608a..94c79ae48 100644 --- a/moma_mission/src/moma_mission/states/path_visitor.py +++ b/moma_mission/src/moma_mission/states/path_visitor.py @@ -38,8 +38,8 @@ def __init__(self, ns): self.timeout = self.get_scoped_param("timeout", 0) self.timeout_factor = self.get_scoped_param("timeout_factor", 2) self.delay = self.get_scoped_param("delay", 2.0) - self.linear_speed = self.get_scoped_param("linear_speed", 0.1) # m/s - self.angular_speed = self.get_scoped_param("angular_speed", 0.5) # rad/s + self.linear_speed = self.get_scoped_param("linear_speed", 0.3) # m/s + self.angular_speed = self.get_scoped_param("angular_speed", 0.8) # rad/s self.linear_tolerance = self.get_scoped_param("linear_tolerance", 0.02) self.angular_tolerance = self.get_scoped_param("angular_tolerance", 0.1) self.section = self.get_scoped_param("section", "all") # "first", "last" @@ -71,8 +71,12 @@ def _poses_msg(self, msg: PoseArray): self.poses = msg def run(self): - # Wait for poses callback to be triggered in case of latching - rospy.sleep(2.0) + for i in range(200): + if self.poses is not None: + break + # Wait for poses callback to be triggered in case of latching + rospy.sleep(0.01) + if self.poses is None: rospy.logerr(f"No poses received yet, can't publish path") return "Failure" From 0b6087d26fd6710f6e0ece8f95961998c15d81c8 Mon Sep 17 00:00:00 2001 From: jk-ethz Date: Wed, 29 Nov 2023 12:40:01 +0100 Subject: [PATCH 3/4] Decrease grasping speed --- moma_mission/config/state_machine/piloting.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moma_mission/config/state_machine/piloting.yaml b/moma_mission/config/state_machine/piloting.yaml index 0ed4cd1c2..bd4bb42dd 100644 --- a/moma_mission/config/state_machine/piloting.yaml +++ b/moma_mission/config/state_machine/piloting.yaml @@ -236,7 +236,7 @@ GRASP_VALVE: section: "first" timeout: 15.0 delay: 1.0 - linear_speed: 0.05 + linear_speed: 0.02 MANIPULATE_VALVE: default_outcome: None From 59dde680a535c23f127a1f5872e33f9c697fdad0 Mon Sep 17 00:00:00 2001 From: jk-ethz Date: Wed, 29 Nov 2023 12:52:01 +0100 Subject: [PATCH 4/4] Add object keypoints speedup --- moma_piloting.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moma_piloting.repos b/moma_piloting.repos index 56ca5dece..f74dca9b5 100644 --- a/moma_piloting.repos +++ b/moma_piloting.repos @@ -12,7 +12,7 @@ repositories: object_keypoints_ros: type: git url: git@github.com:ethz-asl/object_keypoints_ros.git - version: 4862b6fae5df4f18cec0973f1844f79f1c10572f + version: ccf068fa88066ba7dad11ee27ec885e9c291daed cartographer: type: git