diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 5006920858..026e59b422 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -36,7 +36,7 @@ TEST(TestLoadAdmittanceController, load_controller) cm.set_parameter( {"load_admittance_controller.type", "admittance_controller/AdmittanceController"}); - ASSERT_EQ(cm.load_controller("load_admittance_controller"), nullptr); + ASSERT_NE(cm.load_controller("load_admittance_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 22cbda2df9..0aa568dec4 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -16,6 +16,77 @@ load_admittance_controller: - position - velocity + kinematics: + plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL + plugin_package: kinematics_interface + base: base_link # Assumed to be stationary + tip: tool0 + group_name: arm + alpha: 0.0005 + + ft_sensor: + name: ft_sensor_name + frame: + id: link_6 # tool0 Wrench measurements are in this frame + external: false # force torque frame exists within URDF kinematic chain + filter_coefficient: 0.005 + + control: + frame: + id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: + id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + gravity_compensation: + frame: + id: tool0 + external: false + + CoG: # specifies the center of gravity of the end effector + pos: + - 0.1 # x + - 0.0 # y + - 0.0 # z + force: 23.0 # mass * 9.81 + + admittance: + selected_axes: + - true # x + - true # y + - true # z + - true # rx + - true # ry + - true # rz + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: + - 5.5 + - 6.6 + - 7.7 + - 8.8 + - 9.9 + - 10.10 + + damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) + - 2.828427 # x + - 2.828427 # y + - 2.828427 # z + - 2.828427 # rx + - 2.828427 # ry + - 2.828427 # rz + + stiffness: + - 214.1 + - 214.2 + - 214.3 + - 214.4 + - 214.5 + - 214.6 test_admittance_controller: # contains minimal needed parameters for kuka_kr6