From 0402a25c7d8823c0d54c4cac9bf207fd27f67457 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Thu, 6 May 2021 10:32:28 +0200 Subject: [PATCH 1/6] Add new state message for Admittance controller --- control_msgs/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/control_msgs/CMakeLists.txt b/control_msgs/CMakeLists.txt index 7bdd72b..97daf06 100644 --- a/control_msgs/CMakeLists.txt +++ b/control_msgs/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(std_msgs REQUIRED) find_package(trajectory_msgs REQUIRED) set(msg_files + msg/AdmittanceControllerState.msg msg/DynamicJointState.msg msg/GripperCommand.msg msg/InterfaceValue.msg From be4a4e27c56689ddb397bac60211c6cd5f6ac57b Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Sat, 8 May 2021 01:35:27 +0200 Subject: [PATCH 2/6] Updated/extended messages for better debugging --- .../msg/AdmittanceControllerState.msg | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 control_msgs/msg/AdmittanceControllerState.msg diff --git a/control_msgs/msg/AdmittanceControllerState.msg b/control_msgs/msg/AdmittanceControllerState.msg new file mode 100644 index 0000000..d461957 --- /dev/null +++ b/control_msgs/msg/AdmittanceControllerState.msg @@ -0,0 +1,19 @@ +geometry_msgs/WrenchStamped input_force_command #commanded input force for the controller +geometry_msgs/PoseStamped input_pose_command #commanded input pose for the controller +trajectory_msgs/JointTrajectory input_joint_command # commanded input JointTrajectory (used only first point) + +geometry_msgs/WrenchStamped input_force_control_frame # input force transformed into control frame +geometry_msgs/PoseStamped input_pose_control_frame # input pose transformed into control frame + +geometry_msgs/WrenchStamped measured_force # measured force from the sensor (sensor frame) +geometry_msgs/WrenchStamped measured_force_filtered # measured force after low-pass and gravity compensation filters in sensor frame +geometry_msgs/WrenchStamped measured_force_control_frame # transformed measured force to control frame +geometry_msgs/WrenchStamped measured_force_endeffector_frame # measured force transformed to endeffector + +geometry_msgs/PoseStamped desired_pose # goal pose to send to the robot +geometry_msgs/TransformStamped relative_desired_pose # relative pose from the actual pose as result of the admittance rule + +string[] joint_names +trajectory_msgs/JointTrajectoryPoint desired_joint_states # result of IK from goal_pose_command +trajectory_msgs/JointTrajectoryPoint actual_joint_states # read from the hardware +trajectory_msgs/JointTrajectoryPoint error_joint_state From a43ef7127af238ca0e07598a235b068542ec880c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 19 May 2021 16:37:37 -0500 Subject: [PATCH 3/6] Rename force->wrench (#1) --- control_msgs/msg/AdmittanceControllerState.msg | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/control_msgs/msg/AdmittanceControllerState.msg b/control_msgs/msg/AdmittanceControllerState.msg index d461957..c7ad1d2 100644 --- a/control_msgs/msg/AdmittanceControllerState.msg +++ b/control_msgs/msg/AdmittanceControllerState.msg @@ -1,14 +1,14 @@ -geometry_msgs/WrenchStamped input_force_command #commanded input force for the controller +geometry_msgs/WrenchStamped input_wrench_command #commanded input wrench for the controller geometry_msgs/PoseStamped input_pose_command #commanded input pose for the controller trajectory_msgs/JointTrajectory input_joint_command # commanded input JointTrajectory (used only first point) -geometry_msgs/WrenchStamped input_force_control_frame # input force transformed into control frame +geometry_msgs/WrenchStamped input_wrench_control_frame # input wrench transformed into control frame geometry_msgs/PoseStamped input_pose_control_frame # input pose transformed into control frame -geometry_msgs/WrenchStamped measured_force # measured force from the sensor (sensor frame) -geometry_msgs/WrenchStamped measured_force_filtered # measured force after low-pass and gravity compensation filters in sensor frame -geometry_msgs/WrenchStamped measured_force_control_frame # transformed measured force to control frame -geometry_msgs/WrenchStamped measured_force_endeffector_frame # measured force transformed to endeffector +geometry_msgs/WrenchStamped measured_wrench # measured wrench from the sensor (sensor frame) +geometry_msgs/WrenchStamped measured_wrench_filtered # measured wrench after low-pass and gravity compensation filters in sensor frame +geometry_msgs/WrenchStamped measured_wrench_control_frame # transformed measured wrench to control frame +geometry_msgs/WrenchStamped measured_wrench_endeffector_frame # measured wrench transformed to endeffector geometry_msgs/PoseStamped desired_pose # goal pose to send to the robot geometry_msgs/TransformStamped relative_desired_pose # relative pose from the actual pose as result of the admittance rule From 8555691fcf0f4426a5008d2253ef30f86f54b361 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 30 May 2021 12:35:14 +0200 Subject: [PATCH 4/6] Add current position in the state message --- control_msgs/msg/AdmittanceControllerState.msg | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/control_msgs/msg/AdmittanceControllerState.msg b/control_msgs/msg/AdmittanceControllerState.msg index c7ad1d2..551acac 100644 --- a/control_msgs/msg/AdmittanceControllerState.msg +++ b/control_msgs/msg/AdmittanceControllerState.msg @@ -10,8 +10,9 @@ geometry_msgs/WrenchStamped measured_wrench_filtered # measured wrench after lo geometry_msgs/WrenchStamped measured_wrench_control_frame # transformed measured wrench to control frame geometry_msgs/WrenchStamped measured_wrench_endeffector_frame # measured wrench transformed to endeffector -geometry_msgs/PoseStamped desired_pose # goal pose to send to the robot -geometry_msgs/TransformStamped relative_desired_pose # relative pose from the actual pose as result of the admittance rule +geometry_msgs/PoseStamped current_pose # Current pose from FK +geometry_msgs/PoseStamped desired_pose # goal pose to send to the robot +geometry_msgs/TransformStamped relative_desired_pose # relative pose from the actual pose as result of the admittance rule string[] joint_names trajectory_msgs/JointTrajectoryPoint desired_joint_states # result of IK from goal_pose_command From 854f48bcc9ae7b2e9a5446e751c83f73ca9ada68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 1 Jun 2021 06:24:23 +0200 Subject: [PATCH 5/6] Unify field names --- control_msgs/msg/AdmittanceControllerState.msg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control_msgs/msg/AdmittanceControllerState.msg b/control_msgs/msg/AdmittanceControllerState.msg index 551acac..ee31afa 100644 --- a/control_msgs/msg/AdmittanceControllerState.msg +++ b/control_msgs/msg/AdmittanceControllerState.msg @@ -15,6 +15,6 @@ geometry_msgs/PoseStamped desired_pose # goal pose to send to the geometry_msgs/TransformStamped relative_desired_pose # relative pose from the actual pose as result of the admittance rule string[] joint_names -trajectory_msgs/JointTrajectoryPoint desired_joint_states # result of IK from goal_pose_command -trajectory_msgs/JointTrajectoryPoint actual_joint_states # read from the hardware +trajectory_msgs/JointTrajectoryPoint desired_joint_state # result of IK from goal_pose_command +trajectory_msgs/JointTrajectoryPoint actual_joint_state # read from the hardware trajectory_msgs/JointTrajectoryPoint error_joint_state From dc9154ae2450cf68ba88c40639565885d22d49f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 1 Jun 2021 21:09:41 +0200 Subject: [PATCH 6/6] Add calculation values of the admittance rule --- control_msgs/msg/AdmittanceControllerState.msg | 2 ++ 1 file changed, 2 insertions(+) diff --git a/control_msgs/msg/AdmittanceControllerState.msg b/control_msgs/msg/AdmittanceControllerState.msg index ee31afa..c71cd6f 100644 --- a/control_msgs/msg/AdmittanceControllerState.msg +++ b/control_msgs/msg/AdmittanceControllerState.msg @@ -10,6 +10,8 @@ geometry_msgs/WrenchStamped measured_wrench_filtered # measured wrench after lo geometry_msgs/WrenchStamped measured_wrench_control_frame # transformed measured wrench to control frame geometry_msgs/WrenchStamped measured_wrench_endeffector_frame # measured wrench transformed to endeffector +trajectory_msgs/JointTrajectoryPoint admittance_rule_calculated_values # Values calculated by admittance rule (Cartesian space: [x, y, z, rx, ry, rz]) - position = pose_error; effort = measured_wrench + geometry_msgs/PoseStamped current_pose # Current pose from FK geometry_msgs/PoseStamped desired_pose # goal pose to send to the robot geometry_msgs/TransformStamped relative_desired_pose # relative pose from the actual pose as result of the admittance rule