From 327ac0af070494632919685fd4e73d1446908607 Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 27 Oct 2023 15:06:05 +0200 Subject: [PATCH 01/24] Throw exception for a negative damping. --- src/cartesian_impedance_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/cartesian_impedance_controller.cpp b/src/cartesian_impedance_controller.cpp index cde3070..846c742 100644 --- a/src/cartesian_impedance_controller.cpp +++ b/src/cartesian_impedance_controller.cpp @@ -153,7 +153,8 @@ namespace cartesian_impedance_controller if (damping_new(i) < 0) { damping_new(i) = this->damping_factors_(i); - } + throw std::invalid_argument("Damping factor should not be negative."); + } } this->damping_factors_ = damping_new; this->applyDamping(); From e3353e15febaa355d3e610b514089463ba41f5c1 Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 27 Oct 2023 15:06:28 +0200 Subject: [PATCH 02/24] Added tests --- CMakeLists.txt | 30 ++++ config/panda.yaml | 29 ++++ .../cartesian_impedance_controller.h | 2 +- launch/panda_real.launch | 10 ++ launch/panda_sim.launch | 91 ++++++++++ launch/panda_total.launch | 36 ++++ package.xml | 48 ++++-- test/rostest.test | 3 + test/rostest2.test | 15 ++ test/test1.cpp | 124 ++++++++++++++ test/test1_ros.cpp | 126 ++++++++++++++ test/test2_ros.cpp | 162 ++++++++++++++++++ 12 files changed, 656 insertions(+), 20 deletions(-) create mode 100644 config/panda.yaml create mode 100644 launch/panda_real.launch create mode 100755 launch/panda_sim.launch create mode 100644 launch/panda_total.launch create mode 100755 test/rostest.test create mode 100644 test/rostest2.test create mode 100755 test/test1.cpp create mode 100644 test/test1_ros.cpp create mode 100644 test/test2_ros.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 25eb88e..4af7064 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,3 +98,33 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) + +## Testing + +#Cpp level +if (CATKIN_ENABLE_TESTING) + catkin_add_gtest(${PROJECT_NAME}-test test/test1.cpp) + if(TARGET ${PROJECT_NAME}-test) + target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${Eigen3_LIBRARIES}) + endif() +endif() + +#Cpp level but in ROS +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test1_ros test/rostest.test test/test1_ros.cpp) + if(TARGET test1_ros) + target_link_libraries(test1_ros ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) + endif() +endif() + +#ROS tests +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test2_ros test/rostest2.test test/test2_ros.cpp) + if(TARGET test2_ros) + target_link_libraries(test2_ros ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) + endif() +endif() + + diff --git a/config/panda.yaml b/config/panda.yaml new file mode 100644 index 0000000..c888900 --- /dev/null +++ b/config/panda.yaml @@ -0,0 +1,29 @@ +CartesianImpedance_trajectory_controller: + type: cartesian_impedance_controller/CartesianImpedanceController + arm_id: $(arg arm_id) + joints: # Joints to control + - $(arg arm_id)_joint1 + - $(arg arm_id)_joint2 + - $(arg arm_id)_joint3 + - $(arg arm_id)_joint4 + - $(arg arm_id)_joint5 + - $(arg arm_id)_joint6 + - $(arg arm_id)_joint7 + end_effector: $(arg arm_id)_hand_tcp # Link to control arm in + update_frequency: 1000 # Controller update frequency in Hz + dynamic_reconfigure: true # Starts dynamic reconfigure server + handle_trajectories: false # Accept traj., e.g. from MoveIt + wrench_ee_frame: $(arg arm_id)_hand_tcp # Default frame for wrench commands. Replace for $(arg arm_id)_link0 if you want to command the forces in the frame of the base of the robot. + delta_tau_max: 1.0 # Max. commanded torque diff between steps in Nm + filtering: # Update existing values (0.0 1.0] per s + nullspace_config: 1.0 # Nullspace configuration filtering + pose: 1.0 # Reference pose filtering + stiffness: 1.0 # Cartesian and nullspace stiffness + wrench: 1.0 # Commanded torque + verbosity: + verbose_print: false # Enables additional prints + state_msgs: false # Messages of controller state + tf_frames: false # Extra tf frames + + + diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller.h b/include/cartesian_impedance_controller/cartesian_impedance_controller.h index 8e100b3..969333b 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller.h +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller.h @@ -284,4 +284,4 @@ namespace cartesian_impedance_controller void updateFilteredWrench(); }; -} // namespace cartesian_impedance_controller \ No newline at end of file +} // namespace cartesian_impedance_controller diff --git a/launch/panda_real.launch b/launch/panda_real.launch new file mode 100644 index 0000000..6c21231 --- /dev/null +++ b/launch/panda_real.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/panda_sim.launch b/launch/panda_sim.launch new file mode 100755 index 0000000..bd69262 --- /dev/null +++ b/launch/panda_sim.launch @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [franka_state_controller/joint_states, franka_gripper/joint_states] + + + + diff --git a/launch/panda_total.launch b/launch/panda_total.launch new file mode 100644 index 0000000..847f34f --- /dev/null +++ b/launch/panda_total.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 2b225e2..5bd78d8 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,4 @@ - + cartesian_impedance_controller 1.0.0 @@ -32,27 +32,37 @@ tf_conversions trajectory_msgs - actionlib - actionlib_msgs - control_msgs - controller_interface - controller_manager - dynamic_reconfigure - eigen_conversions - geometry_msgs - hardware_interface - message_runtime - pluginlib - realtime_tools - roscpp - sensor_msgs - std_msgs - tf - tf_conversions - trajectory_msgs + actionlib + actionlib_msgs + control_msgs + controller_interface + controller_manager + dynamic_reconfigure + eigen_conversions + geometry_msgs + hardware_interface + message_runtime + pluginlib + realtime_tools + roscpp + sensor_msgs + std_msgs + tf + tf_conversions + trajectory_msgs + + gtest + eigen + gtest_conversions + roscpp + rostest + geometry_msgs + std_msgs + dynamic_reconfigure + diff --git a/test/rostest.test b/test/rostest.test new file mode 100755 index 0000000..eeb24f2 --- /dev/null +++ b/test/rostest.test @@ -0,0 +1,3 @@ + + + diff --git a/test/rostest2.test b/test/rostest2.test new file mode 100644 index 0000000..a702408 --- /dev/null +++ b/test/rostest2.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/test/test1.cpp b/test/test1.cpp new file mode 100755 index 0000000..857b505 --- /dev/null +++ b/test/test1.cpp @@ -0,0 +1,124 @@ +#include "cartesian_impedance_controller/cartesian_impedance_controller.h" +#include +#include + + +using namespace cartesian_impedance_controller; + + + +CartesianImpedanceController cont; // Create an instance of the controller + + +TEST(ControllerTests, initializationTests){ + Eigen::Vector3d position_d_target(1.0, 2.0, 3.0); + Eigen::Quaterniond orientation_d_target(1.0, 0.0, 0.0, 0.0); + + EXPECT_NO_THROW(cont.initDesiredPose(position_d_target, orientation_d_target)); + + + Eigen::VectorXd q_d_nullspace_target(7); + q_d_nullspace_target << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0; + int n = 7; + cont.setNumberOfJoints(n); + EXPECT_NO_THROW(cont.initNullspaceConfig(q_d_nullspace_target)); //Should not fail since we have the same number of joints as q_d_nullspace_target elements + + cont.setNumberOfJoints(n+1); + EXPECT_DEBUG_DEATH(cont.initNullspaceConfig(q_d_nullspace_target), "Nullspace target needs to same size as n_joints_"); //Should fail since we have differences between the number of joints and the number of q_d_nullspace_target elements + +} + + +TEST(ControllerTests, setjointnumberTests){ + int n = 7; + EXPECT_NO_THROW(cont.setNumberOfJoints(n)); //This value of n shouldn't throw an exception. + EXPECT_ANY_THROW(cont.setNumberOfJoints(-n)); //For a negative number of joints, we expect to catch the error. +} + +TEST(ControllerTests, dampingTests){ + double d = 3.2; + EXPECT_NO_THROW(cont.setDampingFactors(d,d,d,d,d,d,d)); //This should work. + EXPECT_ANY_THROW(cont.setDampingFactors(-d,d,d,d,d,d,d)); //This should not work since damping should be positive. +} + +TEST(ControllerTests, stiffnessTests) { + Eigen::Matrix stiffness_good, stiffness_bad; + stiffness_good << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + stiffness_bad << -1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + double t_x = 1.0, t_y = 1.0, t_z = 1.0, r_x = 1.0, r_y = 1.0, r_z = 1.0, n = 1.0; + + EXPECT_NO_THROW(cont.setStiffness(stiffness_good, false)); + EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, false), "Stiffness values need to be positive."); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, false)); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, false)); + + EXPECT_NO_THROW(cont.setStiffness(stiffness_good, true)); + EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, true), "Stiffness values need to be positive."); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, true)); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, true)); +} + + +TEST(ControllerTests, filteringTests){ + EXPECT_NO_THROW(cont.setFiltering(100, 0.5, 0.6, 0.7, 0.8)); + EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 0.7, 0.8), "Update frequency needs to be greater or equal to zero"); + EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 1.7, 0.8), "Filter params need to be between 0 and 1."); +} + +TEST(ControllerTests, saturationTests){ + EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0)); + EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0,100.0)); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0), "Allowed torque change must be positive"); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0,100.0), "Allowed torque change must be positive"); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(2.0,-100.0), "Update frequency needs to be greater or equal to zero"); +} + +TEST(ControllerTests, wrenchTest){ + Eigen::Matrix wrench; + wrench << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + EXPECT_NO_THROW(cont.applyWrench(wrench)); +} + +TEST(ControllerTests, getterTests){ + Eigen::VectorXd q(7); + q << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::VectorXd dq(7); + dq << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::Vector3d position(1.0, 2.0, 3.0); + Eigen::Quaterniond orientation(1.0, 0.0, 0.0, 0.0); + Eigen::Vector3d position_d(2.0, 2.0, 3.0); + Eigen::Quaterniond orientation_d(1.0, 0.0, 0.0, 0.0); + Eigen::Matrix cartesian_stiffness; + double nullspace_stiffness = 1.0; + Eigen::VectorXd q_d_nullspace(7); + q_d_nullspace << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::Matrix cartesian_damping; + + EXPECT_NO_THROW(cont.getState(&q, &dq, &position, &orientation, &position_d, &orientation_d, + &cartesian_stiffness, &nullspace_stiffness, &q_d_nullspace, &cartesian_damping)); + EXPECT_NO_THROW(cont.getAppliedWrench()); + EXPECT_NO_THROW(cont.getLastCommands()); + EXPECT_NO_THROW(cont.getPoseError()); + + cont.setNumberOfJoints(7); + Eigen::MatrixXd jacobian(6,7); + jacobian.setZero(); + EXPECT_NO_THROW(cont.calculateCommandedTorques(q, dq, position, orientation, jacobian)); + + + Eigen::Matrix error_getter, error_state; + error_getter << cont.getPoseError(); + error_state.head(3) << position - position_d; + + EXPECT_DOUBLE_EQ(error_getter(0),error_state(0)); + EXPECT_DOUBLE_EQ(error_getter(1),error_state(1)); + EXPECT_DOUBLE_EQ(error_getter(2),error_state(2)); + +} + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/test/test1_ros.cpp b/test/test1_ros.cpp new file mode 100644 index 0000000..4fce25d --- /dev/null +++ b/test/test1_ros.cpp @@ -0,0 +1,126 @@ +#include "cartesian_impedance_controller/cartesian_impedance_controller.h" +#include +#include +#include + +using namespace cartesian_impedance_controller; + + + +CartesianImpedanceController cont; // Create an instance of the controller + + +TEST(ControllerTests, initializationTests){ + Eigen::Vector3d position_d_target(1.0, 2.0, 3.0); + Eigen::Quaterniond orientation_d_target(1.0, 0.0, 0.0, 0.0); + + EXPECT_NO_THROW(cont.initDesiredPose(position_d_target, orientation_d_target)); + + + Eigen::VectorXd q_d_nullspace_target(7); + q_d_nullspace_target << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0; + int n = 7; + cont.setNumberOfJoints(n); + EXPECT_NO_THROW(cont.initNullspaceConfig(q_d_nullspace_target)); //Should not fail since we have the same number of joints as q_d_nullspace_target elements + + cont.setNumberOfJoints(n+1); + EXPECT_DEBUG_DEATH(cont.initNullspaceConfig(q_d_nullspace_target), "Nullspace target needs to same size as n_joints_"); //Should fail since we have differences between the number of joints and the number of q_d_nullspace_target elements + +} + + +TEST(ControllerTests, setjointnumberTests){ + int n = 7; + EXPECT_NO_THROW(cont.setNumberOfJoints(n)); //This value of n shouldn't throw an exception. + EXPECT_ANY_THROW(cont.setNumberOfJoints(-n)); //For a negative number of joints, we expect to catch the error. +} + +TEST(ControllerTests, dampingTests){ + double d = 3.2; + EXPECT_NO_THROW(cont.setDampingFactors(d,d,d,d,d,d,d)); //This should work. + EXPECT_ANY_THROW(cont.setDampingFactors(-d,d,d,d,d,d,d)); //This should not work since damping should be positive. +} + +TEST(ControllerTests, stiffnessTests) { + Eigen::Matrix stiffness_good, stiffness_bad; + stiffness_good << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + stiffness_bad << -1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + double t_x = 1.0, t_y = 1.0, t_z = 1.0, r_x = 1.0, r_y = 1.0, r_z = 1.0, n = 1.0; + + EXPECT_NO_THROW(cont.setStiffness(stiffness_good, false)); + EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, false), "Stiffness values need to be positive."); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, false)); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, false)); + + EXPECT_NO_THROW(cont.setStiffness(stiffness_good, true)); + EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, true), "Stiffness values need to be positive."); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, true)); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, true)); +} + + +TEST(ControllerTests, filteringTests){ + EXPECT_NO_THROW(cont.setFiltering(100, 0.5, 0.6, 0.7, 0.8)); + EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 0.7, 0.8), "Update frequency needs to be greater or equal to zero"); + EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 1.7, 0.8), "Filter params need to be between 0 and 1."); +} + +TEST(ControllerTests, saturationTests){ + EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0)); + EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0,100.0)); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0), "Allowed torque change must be positive"); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0,100.0), "Allowed torque change must be positive"); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(2.0,-100.0), "Update frequency needs to be greater or equal to zero"); +} + +TEST(ControllerTests, wrenchTest){ + Eigen::Matrix wrench; + wrench << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + EXPECT_NO_THROW(cont.applyWrench(wrench)); +} + +TEST(ControllerTests, getterTests){ + Eigen::VectorXd q(7); + q << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::VectorXd dq(7); + dq << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::Vector3d position(1.0, 2.0, 3.0); + Eigen::Quaterniond orientation(1.0, 0.0, 0.0, 0.0); + Eigen::Vector3d position_d(2.0, 2.0, 3.0); + Eigen::Quaterniond orientation_d(1.0, 0.0, 0.0, 0.0); + Eigen::Matrix cartesian_stiffness; + double nullspace_stiffness = 1.0; + Eigen::VectorXd q_d_nullspace(7); + q_d_nullspace << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::Matrix cartesian_damping; + + EXPECT_NO_THROW(cont.getState(&q, &dq, &position, &orientation, &position_d, &orientation_d, + &cartesian_stiffness, &nullspace_stiffness, &q_d_nullspace, &cartesian_damping)); + EXPECT_NO_THROW(cont.getAppliedWrench()); + EXPECT_NO_THROW(cont.getLastCommands()); + EXPECT_NO_THROW(cont.getPoseError()); + + cont.setNumberOfJoints(7); + Eigen::MatrixXd jacobian(6,7); + jacobian.setZero(); + EXPECT_NO_THROW(cont.calculateCommandedTorques(q, dq, position, orientation, jacobian)); + + + Eigen::Matrix error_getter, error_state; + error_getter << cont.getPoseError(); + error_state.head(3) << position - position_d; + + EXPECT_DOUBLE_EQ(error_getter(0),error_state(0)); + EXPECT_DOUBLE_EQ(error_getter(1),error_state(1)); + EXPECT_DOUBLE_EQ(error_getter(2),error_state(2)); + +} + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "tester"); + ros::NodeHandle nh; + return RUN_ALL_TESTS(); +} + diff --git a/test/test2_ros.cpp b/test/test2_ros.cpp new file mode 100644 index 0000000..811e097 --- /dev/null +++ b/test/test2_ros.cpp @@ -0,0 +1,162 @@ +#include +#include +#include +#include +#include +#include +#include + + +//ROSBaseTests tested that key topics in the library are being written. +TEST(ROSBaseTests, referenceposeTests) +{ + ros::NodeHandle n; + ros::Duration timeout(10); + for (int i = 0; i < 5; i++) { + auto msg_pose = ros::topic::waitForMessage( + "/CartesianImpedance_trajectory_controller/reference_pose", n, timeout); + + ASSERT_TRUE(msg_pose != nullptr) << "Did not receive message on iteration " << i + 1; + + EXPECT_LE(std::abs(msg_pose->pose.orientation.x), 11); + } + +} + +TEST(ROSBaseTests, commandedtorqueTests) +{ + ros::NodeHandle n; + ros::Duration timeout(5); + for (int i = 0; i < 5; i++) { + auto msg_torque = ros::topic::waitForMessage( + "/CartesianImpedance_trajectory_controller/commanded_torques", n, timeout); + + ASSERT_TRUE(msg_torque != nullptr) << "Did not receive message on iteration " << i + 1; + + EXPECT_LE(std::abs(msg_torque->data[0]), 1000.0); + } + +} + +//ROSReconfigureTests tested that we are able to modify external wrench, virtual stiffness, and damping online. +TEST(ROSReconfigureTests, wrenchTest) { + ros::NodeHandle n; + ros::Duration timeout(10); + for (int i = 0; i < 5; i++) { + auto msg_wup = ros::topic::waitForMessage( + "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/parameter_updates", n, timeout); + + ASSERT_TRUE(msg_wup != nullptr) << "Did not receive message on iteration " << i + 1; + + for(const dynamic_reconfigure::DoubleParameter& double_param : msg_wup->doubles) { + if(double_param.name == "f_x") { + EXPECT_EQ(double_param.value, 5.0); + break; + } + } + } +} + +TEST(ROSReconfigureTests, stiffnessTest) { + ros::NodeHandle n; + ros::Duration timeout(5); + for (int i = 0; i < 5; i++) { + auto msg_stup = ros::topic::waitForMessage( + "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/parameter_updates", n, timeout); + + ASSERT_TRUE(msg_stup != nullptr) << "Did not receive message on iteration " << i + 1; + + for(const dynamic_reconfigure::DoubleParameter& double_param : msg_stup->doubles) { + if(double_param.name == "translation_x") { + EXPECT_EQ(double_param.value, 100.0); + break; + } + } + } +} + +TEST(ROSReconfigureTests, dampingTest) { + ros::NodeHandle n; + ros::Duration timeout(5); + for (int i = 0; i < 5; i++) { + auto msg_dup = ros::topic::waitForMessage( + "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/parameter_updates", n, timeout); + + ASSERT_TRUE(msg_dup != nullptr) << "Did not receive message on iteration " << i + 1; + + for(const dynamic_reconfigure::DoubleParameter& double_param : msg_dup->doubles) { + if(double_param.name == "translation_x") { + EXPECT_EQ(double_param.value, 0.75); + break; + } + } + } +} + +//ROSParameterTests make sure that all the necessary ROS parameters have been created. +TEST(ROSParameterTests, paramTest) { + ros::NodeHandle n; + ros::Duration timeout(5); + std::vector params_to_check = { + "/CartesianImpedance_trajectory_controller/arm_id", + "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/apply_wrench", + "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/f_x", + "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/f_y", + "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/f_z", + "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/tau_x", + "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/tau_y", + "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/tau_z", + "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/nullspace_damping", + "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/rotation_x", + "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/rotation_y", + "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/rotation_z", + "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/translation_x", + "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/translation_y", + "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/translation_z", + "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/update_damping_factors", + "/CartesianImpedance_trajectory_controller/delta_tau_max", + "/CartesianImpedance_trajectory_controller/dynamic_reconfigure", + "/CartesianImpedance_trajectory_controller/end_effector", + "/CartesianImpedance_trajectory_controller/filtering/nullspace_config", + "/CartesianImpedance_trajectory_controller/filtering/pose", + "/CartesianImpedance_trajectory_controller/filtering/stiffness", + "/CartesianImpedance_trajectory_controller/filtering/wrench", + "/CartesianImpedance_trajectory_controller/handle_trajectories", + "/CartesianImpedance_trajectory_controller/joints", + "/CartesianImpedance_trajectory_controller/root_frame", + "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/nullspace_stiffness", + "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/rotation_x", + "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/rotation_y", + "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/rotation_z", + "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/translation_x", + "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/translation_y", + "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/translation_z", + "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/update_stiffness", + "/CartesianImpedance_trajectory_controller/type", + "/CartesianImpedance_trajectory_controller/update_frequency", + "/CartesianImpedance_trajectory_controller/verbosity/state_msgs", + "/CartesianImpedance_trajectory_controller/verbosity/tf_frames", + "/CartesianImpedance_trajectory_controller/verbosity/verbose_print", + "/CartesianImpedance_trajectory_controller/wrench_ee_frame" + }; + + for (const auto ¶m : params_to_check) { + EXPECT_TRUE(ros::param::has(param)) << "Parameter [" << param << "] does NOT exist."; + } +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_ros"); + return RUN_ALL_TESTS(); +} + + + + + + + + + + From 67a98610513b5205139289fcbb02a83c0fcb648f Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 27 Oct 2023 15:17:51 +0200 Subject: [PATCH 03/24] File names now more self-explanatory --- CMakeLists.txt | 23 +++++++---------------- test/base.test | 3 +++ test/{test1_ros.cpp => base_tests.cpp} | 0 test/{test1.cpp => base_tests_old.cpp} | 0 test/{rostest2.test => ros.test} | 2 +- test/{test2_ros.cpp => ros_tests.cpp} | 0 test/rostest.test | 3 --- 7 files changed, 11 insertions(+), 20 deletions(-) create mode 100755 test/base.test rename test/{test1_ros.cpp => base_tests.cpp} (100%) rename test/{test1.cpp => base_tests_old.cpp} (100%) rename test/{rostest2.test => ros.test} (88%) rename test/{test2_ros.cpp => ros_tests.cpp} (100%) delete mode 100755 test/rostest.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 4af7064..a896b1c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,30 +100,21 @@ install(DIRECTORY include/${PROJECT_NAME}/ PATTERN ".svn" EXCLUDE) ## Testing - -#Cpp level -if (CATKIN_ENABLE_TESTING) - catkin_add_gtest(${PROJECT_NAME}-test test/test1.cpp) - if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${Eigen3_LIBRARIES}) - endif() -endif() - -#Cpp level but in ROS +#Cpp tests if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest_gtest(test1_ros test/rostest.test test/test1_ros.cpp) - if(TARGET test1_ros) - target_link_libraries(test1_ros ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) + add_rostest_gtest(base_tests test/base.test test/base_tests.cpp) + if(TARGET base_tests) + target_link_libraries(base_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) endif() endif() #ROS tests if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest_gtest(test2_ros test/rostest2.test test/test2_ros.cpp) - if(TARGET test2_ros) - target_link_libraries(test2_ros ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) + add_rostest_gtest(ros_tests test/ros.test test/ros_tests.cpp) + if(TARGET ros_tests) + target_link_libraries(ros_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) endif() endif() diff --git a/test/base.test b/test/base.test new file mode 100755 index 0000000..954f11c --- /dev/null +++ b/test/base.test @@ -0,0 +1,3 @@ + + + diff --git a/test/test1_ros.cpp b/test/base_tests.cpp similarity index 100% rename from test/test1_ros.cpp rename to test/base_tests.cpp diff --git a/test/test1.cpp b/test/base_tests_old.cpp similarity index 100% rename from test/test1.cpp rename to test/base_tests_old.cpp diff --git a/test/rostest2.test b/test/ros.test similarity index 88% rename from test/rostest2.test rename to test/ros.test index a702408..21f7ec3 100644 --- a/test/rostest2.test +++ b/test/ros.test @@ -11,5 +11,5 @@ - + diff --git a/test/test2_ros.cpp b/test/ros_tests.cpp similarity index 100% rename from test/test2_ros.cpp rename to test/ros_tests.cpp diff --git a/test/rostest.test b/test/rostest.test deleted file mode 100755 index eeb24f2..0000000 --- a/test/rostest.test +++ /dev/null @@ -1,3 +0,0 @@ - - - From c0dc044fe2f058cd7e829cdad17251eea3a1f5f8 Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 27 Oct 2023 15:21:19 +0200 Subject: [PATCH 04/24] Making the tests slightly prettier --- test/base_tests.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/base_tests.cpp b/test/base_tests.cpp index 4fce25d..d76f059 100644 --- a/test/base_tests.cpp +++ b/test/base_tests.cpp @@ -7,7 +7,7 @@ using namespace cartesian_impedance_controller; -CartesianImpedanceController cont; // Create an instance of the controller +CartesianImpedanceController cont; // Created an instance of the controller TEST(ControllerTests, initializationTests){ @@ -24,7 +24,7 @@ TEST(ControllerTests, initializationTests){ EXPECT_NO_THROW(cont.initNullspaceConfig(q_d_nullspace_target)); //Should not fail since we have the same number of joints as q_d_nullspace_target elements cont.setNumberOfJoints(n+1); - EXPECT_DEBUG_DEATH(cont.initNullspaceConfig(q_d_nullspace_target), "Nullspace target needs to same size as n_joints_"); //Should fail since we have differences between the number of joints and the number of q_d_nullspace_target elements + EXPECT_DEBUG_DEATH(cont.initNullspaceConfig(q_d_nullspace_target), "Nullspace target needs to same size as n_joints_"); } @@ -79,7 +79,7 @@ TEST(ControllerTests, wrenchTest){ EXPECT_NO_THROW(cont.applyWrench(wrench)); } -TEST(ControllerTests, getterTests){ +TEST(ControllerTests, getterTests){ Eigen::VectorXd q(7); q << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; Eigen::VectorXd dq(7); From e534201b215ac7be9e8d1333369f92ba5dfebab4 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 31 Oct 2023 15:09:44 +0000 Subject: [PATCH 05/24] New: Adds example robot and minimal launch file --- res/config/example_controller.yaml | 40 ++++ res/launch/examples.launch | 41 ++++ res/urdf/robot.urdf.xacro | 372 +++++++++++++++++++++++++++++ 3 files changed, 453 insertions(+) create mode 100644 res/config/example_controller.yaml create mode 100644 res/launch/examples.launch create mode 100644 res/urdf/robot.urdf.xacro diff --git a/res/config/example_controller.yaml b/res/config/example_controller.yaml new file mode 100644 index 0000000..3ea1c09 --- /dev/null +++ b/res/config/example_controller.yaml @@ -0,0 +1,40 @@ +CartesianImpedance_trajectory_controller: + type: cartesian_impedance_controller/CartesianImpedanceController + joints: # Joints to control + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + end_effector: tool0 # Link to control arm in + update_frequency: 500 # Controller update frequency in Hz + dynamic_reconfigure: true # Starts dynamic reconfigure server + handle_trajectories: true # Accept traj., e.g. from MoveIt + robot_description: /robot_description # In case of a varying name + wrench_ee_frame: tool0 # Default frame for wrench commands + delta_tau_max: 1.0 # Max. commanded torque diff between steps in Nm + filtering: # Update existing values (0.0 1.0] per s + nullspace_config: 0.1 # Nullspace configuration filtering + pose: 0.1 # Reference pose filtering + stiffness: 0.1 # Cartesian and nullspace stiffness + wrench: 0.1 # Commanded torque + verbosity: + verbose_print: true # Enables additional prints + state_msgs: true # Messages of controller state + tf_frames: true # Extra tf frames + +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 500 + +# Settings for ros_control hardware interface +hardware_interface: + control_freq: 500 # in Hz + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/res/launch/examples.launch b/res/launch/examples.launch new file mode 100644 index 0000000..5bae475 --- /dev/null +++ b/res/launch/examples.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/res/urdf/robot.urdf.xacro b/res/urdf/robot.urdf.xacro new file mode 100644 index 0000000..b4b1610 --- /dev/null +++ b/res/urdf/robot.urdf.xacro @@ -0,0 +1,372 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + true + + + true + + + true + + + true + + + true + + + true + + From 93025a4c52097f44e09a24922fd717a2b8a4fbfe Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 31 Oct 2023 15:51:32 +0000 Subject: [PATCH 06/24] Fix: package.xml removes "gtest_conversions" + other edits --- package.xml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/package.xml b/package.xml index 5bd78d8..5c0b299 100644 --- a/package.xml +++ b/package.xml @@ -9,6 +9,7 @@ Matthias Mayr Oussama Chouman + Julian Salt Ducaju catkin @@ -51,18 +52,16 @@ tf_conversions trajectory_msgs - + gtest eigen - gtest_conversions roscpp rostest geometry_msgs std_msgs dynamic_reconfigure - From 37966595b4de0a7a54e6f0cc156e1d0edbd83b48 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 31 Oct 2023 14:35:09 +0000 Subject: [PATCH 07/24] Fix: Set number of joints in ROS based on YAML --- src/cartesian_impedance_controller_ros.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index a535231..6354ff0 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -54,7 +54,7 @@ namespace cartesian_impedance_controller std::vector joint_names; if (!nh.getParam("joints", joint_names)) { - ROS_ERROR("Invalid or no joint_names parameters provided, aborting controller init!"); + ROS_ERROR("Invalid or no 'joints' parameter provided, aborting controller init!"); return false; } for (size_t i = 0; i < joint_names.size(); ++i) @@ -70,6 +70,7 @@ namespace cartesian_impedance_controller } } ROS_INFO_STREAM("Number of joints specified in parameters: " << joint_names.size()); + this->setNumberOfJoints(joint_names.size()); return true; } From de434c20e1b8c199a3fd008728440062a85477f4 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 31 Oct 2023 15:09:44 +0000 Subject: [PATCH 08/24] New: Adds example robot and minimal launch file --- res/config/example_controller.yaml | 40 ++++ res/launch/examples.launch | 41 ++++ res/urdf/robot.urdf.xacro | 372 +++++++++++++++++++++++++++++ 3 files changed, 453 insertions(+) create mode 100644 res/config/example_controller.yaml create mode 100644 res/launch/examples.launch create mode 100644 res/urdf/robot.urdf.xacro diff --git a/res/config/example_controller.yaml b/res/config/example_controller.yaml new file mode 100644 index 0000000..3ea1c09 --- /dev/null +++ b/res/config/example_controller.yaml @@ -0,0 +1,40 @@ +CartesianImpedance_trajectory_controller: + type: cartesian_impedance_controller/CartesianImpedanceController + joints: # Joints to control + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + end_effector: tool0 # Link to control arm in + update_frequency: 500 # Controller update frequency in Hz + dynamic_reconfigure: true # Starts dynamic reconfigure server + handle_trajectories: true # Accept traj., e.g. from MoveIt + robot_description: /robot_description # In case of a varying name + wrench_ee_frame: tool0 # Default frame for wrench commands + delta_tau_max: 1.0 # Max. commanded torque diff between steps in Nm + filtering: # Update existing values (0.0 1.0] per s + nullspace_config: 0.1 # Nullspace configuration filtering + pose: 0.1 # Reference pose filtering + stiffness: 0.1 # Cartesian and nullspace stiffness + wrench: 0.1 # Commanded torque + verbosity: + verbose_print: true # Enables additional prints + state_msgs: true # Messages of controller state + tf_frames: true # Extra tf frames + +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 500 + +# Settings for ros_control hardware interface +hardware_interface: + control_freq: 500 # in Hz + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/res/launch/examples.launch b/res/launch/examples.launch new file mode 100644 index 0000000..5bae475 --- /dev/null +++ b/res/launch/examples.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/res/urdf/robot.urdf.xacro b/res/urdf/robot.urdf.xacro new file mode 100644 index 0000000..b4b1610 --- /dev/null +++ b/res/urdf/robot.urdf.xacro @@ -0,0 +1,372 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + true + + + true + + + true + + + true + + + true + + + true + + From 49e145db60b472cec75078fd6474cecf48e037f0 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 31 Oct 2023 18:53:54 +0000 Subject: [PATCH 09/24] Tests: Improves base library tests Also defines more consistent behavior in the library --- src/cartesian_impedance_controller.cpp | 31 +++++-- test/base_tests.cpp | 50 +++++----- test/base_tests_old.cpp | 124 ------------------------- 3 files changed, 48 insertions(+), 157 deletions(-) mode change 100644 => 100755 test/base_tests.cpp delete mode 100755 test/base_tests_old.cpp diff --git a/src/cartesian_impedance_controller.cpp b/src/cartesian_impedance_controller.cpp index 846c742..14615b0 100644 --- a/src/cartesian_impedance_controller.cpp +++ b/src/cartesian_impedance_controller.cpp @@ -103,6 +103,10 @@ namespace cartesian_impedance_controller void CartesianImpedanceController::setNumberOfJoints(size_t n_joints) { + if (n_joints < 0) + { + throw std::invalid_argument("Number of joints must be positive"); + } this->n_joints_ = n_joints; this->q_ = Eigen::VectorXd::Zero(this->n_joints_); this->dq_ = Eigen::VectorXd::Zero(this->n_joints_); @@ -116,12 +120,25 @@ namespace cartesian_impedance_controller { for (int i = 0; i < 6; i++) { - assert(stiffness(i) >= 0.0 && "Stiffness values need to be positive."); // Set diagonal values of stiffness matrix - this->cartesian_stiffness_target_(i, i) = stiffness(i); + if (stiffness(i) < 0.0) + { + assert(stiffness(i) >= 0 && "Stiffness values need to be positive."); + this->cartesian_stiffness_target_(i, i) = 0.0; + } + else + { + this->cartesian_stiffness_target_(i, i) = stiffness(i); + } + } + if (stiffness(6) < 0.0) { + assert(stiffness(6) >= 0.0 && "Stiffness values need to be positive."); + this->nullspace_stiffness_target_ = 0.0; + } + else + { + this->nullspace_stiffness_target_ = stiffness(6); } - assert(stiffness(6) >= 0.0 && "Stiffness values need to be positive."); - this->nullspace_stiffness_target_ = stiffness(6); if (auto_damping) { this->applyDamping(); @@ -152,8 +169,8 @@ namespace cartesian_impedance_controller { if (damping_new(i) < 0) { + assert(damping_new(i) >= 0 && "Damping factor must not be negative."); damping_new(i) = this->damping_factors_(i); - throw std::invalid_argument("Damping factor should not be negative."); } } this->damping_factors_ = damping_new; @@ -238,8 +255,8 @@ namespace cartesian_impedance_controller updateFilteredWrench(); // Compute error term - error_.head(3) << this->position_ - this->position_d_; - error_.tail(3) << calculateOrientationError(this->orientation_d_, this->orientation_); + this->error_.head(3) << this->position_ - this->position_d_; + this->error_.tail(3) << calculateOrientationError(this->orientation_d_, this->orientation_); // Kinematic pseuoinverse Eigen::MatrixXd jacobian_transpose_pinv; diff --git a/test/base_tests.cpp b/test/base_tests.cpp old mode 100644 new mode 100755 index d76f059..46865cb --- a/test/base_tests.cpp +++ b/test/base_tests.cpp @@ -1,44 +1,45 @@ #include "cartesian_impedance_controller/cartesian_impedance_controller.h" #include #include -#include + using namespace cartesian_impedance_controller; -CartesianImpedanceController cont; // Created an instance of the controller +CartesianImpedanceController cont; // Create an instance of the controller TEST(ControllerTests, initializationTests){ - Eigen::Vector3d position_d_target(1.0, 2.0, 3.0); - Eigen::Quaterniond orientation_d_target(1.0, 0.0, 0.0, 0.0); - - EXPECT_NO_THROW(cont.initDesiredPose(position_d_target, orientation_d_target)); - - + const Eigen::Vector3d position_d_target(1.0, 2.0, 3.0); + const Eigen::Quaterniond orientation_d_target(1.0, 0.0, 0.0, 0.0); + + EXPECT_NO_THROW(cont.initDesiredPose(position_d_target, orientation_d_target)); +} + +TEST(ControllerTests, nullspaceConfigTests){ Eigen::VectorXd q_d_nullspace_target(7); - q_d_nullspace_target << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0; - int n = 7; + q_d_nullspace_target << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0; + const int n = 7; cont.setNumberOfJoints(n); EXPECT_NO_THROW(cont.initNullspaceConfig(q_d_nullspace_target)); //Should not fail since we have the same number of joints as q_d_nullspace_target elements - + cont.setNumberOfJoints(n+1); - EXPECT_DEBUG_DEATH(cont.initNullspaceConfig(q_d_nullspace_target), "Nullspace target needs to same size as n_joints_"); - + EXPECT_DEBUG_DEATH(cont.initNullspaceConfig(q_d_nullspace_target), "Nullspace target needs to same size as n_joints_"); //Should fail since we have differences between the number of joints and the number of q_d_nullspace_target elements } - TEST(ControllerTests, setjointnumberTests){ - int n = 7; + const int n = 7; EXPECT_NO_THROW(cont.setNumberOfJoints(n)); //This value of n shouldn't throw an exception. EXPECT_ANY_THROW(cont.setNumberOfJoints(-n)); //For a negative number of joints, we expect to catch the error. } TEST(ControllerTests, dampingTests){ - double d = 3.2; + const double d = 3.2; EXPECT_NO_THROW(cont.setDampingFactors(d,d,d,d,d,d,d)); //This should work. - EXPECT_ANY_THROW(cont.setDampingFactors(-d,d,d,d,d,d,d)); //This should not work since damping should be positive. + EXPECT_NO_THROW(cont.setDampingFactors(-d,d,d,d,d,d,d)); //Negative damping factors are ignored outside debug builds + + EXPECT_DEBUG_DEATH(cont.setDampingFactors(-d,d,d,d,d,d,d), "Negative damping factors are tested by asserts"); //This should not work since damping should be positive } TEST(ControllerTests, stiffnessTests) { @@ -46,12 +47,13 @@ TEST(ControllerTests, stiffnessTests) { stiffness_good << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; stiffness_bad << -1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; double t_x = 1.0, t_y = 1.0, t_z = 1.0, r_x = 1.0, r_y = 1.0, r_z = 1.0, n = 1.0; - + + // Without auto-damping EXPECT_NO_THROW(cont.setStiffness(stiffness_good, false)); EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, false), "Stiffness values need to be positive."); EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, false)); EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, false)); - + // With auto-damping EXPECT_NO_THROW(cont.setStiffness(stiffness_good, true)); EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, true), "Stiffness values need to be positive."); EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, true)); @@ -79,7 +81,7 @@ TEST(ControllerTests, wrenchTest){ EXPECT_NO_THROW(cont.applyWrench(wrench)); } -TEST(ControllerTests, getterTests){ +TEST(ControllerTests, getterTests){ Eigen::VectorXd q(7); q << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; Eigen::VectorXd dq(7); @@ -93,7 +95,7 @@ TEST(ControllerTests, getterTests){ Eigen::VectorXd q_d_nullspace(7); q_d_nullspace << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; Eigen::Matrix cartesian_damping; - + EXPECT_NO_THROW(cont.getState(&q, &dq, &position, &orientation, &position_d, &orientation_d, &cartesian_stiffness, &nullspace_stiffness, &q_d_nullspace, &cartesian_damping)); EXPECT_NO_THROW(cont.getAppliedWrench()); @@ -105,22 +107,18 @@ TEST(ControllerTests, getterTests){ jacobian.setZero(); EXPECT_NO_THROW(cont.calculateCommandedTorques(q, dq, position, orientation, jacobian)); - Eigen::Matrix error_getter, error_state; error_getter << cont.getPoseError(); error_state.head(3) << position - position_d; - + EXPECT_DOUBLE_EQ(error_getter(0),error_state(0)); EXPECT_DOUBLE_EQ(error_getter(1),error_state(1)); EXPECT_DOUBLE_EQ(error_getter(2),error_state(2)); - } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "tester"); - ros::NodeHandle nh; return RUN_ALL_TESTS(); } diff --git a/test/base_tests_old.cpp b/test/base_tests_old.cpp deleted file mode 100755 index 857b505..0000000 --- a/test/base_tests_old.cpp +++ /dev/null @@ -1,124 +0,0 @@ -#include "cartesian_impedance_controller/cartesian_impedance_controller.h" -#include -#include - - -using namespace cartesian_impedance_controller; - - - -CartesianImpedanceController cont; // Create an instance of the controller - - -TEST(ControllerTests, initializationTests){ - Eigen::Vector3d position_d_target(1.0, 2.0, 3.0); - Eigen::Quaterniond orientation_d_target(1.0, 0.0, 0.0, 0.0); - - EXPECT_NO_THROW(cont.initDesiredPose(position_d_target, orientation_d_target)); - - - Eigen::VectorXd q_d_nullspace_target(7); - q_d_nullspace_target << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0; - int n = 7; - cont.setNumberOfJoints(n); - EXPECT_NO_THROW(cont.initNullspaceConfig(q_d_nullspace_target)); //Should not fail since we have the same number of joints as q_d_nullspace_target elements - - cont.setNumberOfJoints(n+1); - EXPECT_DEBUG_DEATH(cont.initNullspaceConfig(q_d_nullspace_target), "Nullspace target needs to same size as n_joints_"); //Should fail since we have differences between the number of joints and the number of q_d_nullspace_target elements - -} - - -TEST(ControllerTests, setjointnumberTests){ - int n = 7; - EXPECT_NO_THROW(cont.setNumberOfJoints(n)); //This value of n shouldn't throw an exception. - EXPECT_ANY_THROW(cont.setNumberOfJoints(-n)); //For a negative number of joints, we expect to catch the error. -} - -TEST(ControllerTests, dampingTests){ - double d = 3.2; - EXPECT_NO_THROW(cont.setDampingFactors(d,d,d,d,d,d,d)); //This should work. - EXPECT_ANY_THROW(cont.setDampingFactors(-d,d,d,d,d,d,d)); //This should not work since damping should be positive. -} - -TEST(ControllerTests, stiffnessTests) { - Eigen::Matrix stiffness_good, stiffness_bad; - stiffness_good << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; - stiffness_bad << -1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; - double t_x = 1.0, t_y = 1.0, t_z = 1.0, r_x = 1.0, r_y = 1.0, r_z = 1.0, n = 1.0; - - EXPECT_NO_THROW(cont.setStiffness(stiffness_good, false)); - EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, false), "Stiffness values need to be positive."); - EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, false)); - EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, false)); - - EXPECT_NO_THROW(cont.setStiffness(stiffness_good, true)); - EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, true), "Stiffness values need to be positive."); - EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, true)); - EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, true)); -} - - -TEST(ControllerTests, filteringTests){ - EXPECT_NO_THROW(cont.setFiltering(100, 0.5, 0.6, 0.7, 0.8)); - EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 0.7, 0.8), "Update frequency needs to be greater or equal to zero"); - EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 1.7, 0.8), "Filter params need to be between 0 and 1."); -} - -TEST(ControllerTests, saturationTests){ - EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0)); - EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0,100.0)); - EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0), "Allowed torque change must be positive"); - EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0,100.0), "Allowed torque change must be positive"); - EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(2.0,-100.0), "Update frequency needs to be greater or equal to zero"); -} - -TEST(ControllerTests, wrenchTest){ - Eigen::Matrix wrench; - wrench << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; - EXPECT_NO_THROW(cont.applyWrench(wrench)); -} - -TEST(ControllerTests, getterTests){ - Eigen::VectorXd q(7); - q << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; - Eigen::VectorXd dq(7); - dq << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; - Eigen::Vector3d position(1.0, 2.0, 3.0); - Eigen::Quaterniond orientation(1.0, 0.0, 0.0, 0.0); - Eigen::Vector3d position_d(2.0, 2.0, 3.0); - Eigen::Quaterniond orientation_d(1.0, 0.0, 0.0, 0.0); - Eigen::Matrix cartesian_stiffness; - double nullspace_stiffness = 1.0; - Eigen::VectorXd q_d_nullspace(7); - q_d_nullspace << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; - Eigen::Matrix cartesian_damping; - - EXPECT_NO_THROW(cont.getState(&q, &dq, &position, &orientation, &position_d, &orientation_d, - &cartesian_stiffness, &nullspace_stiffness, &q_d_nullspace, &cartesian_damping)); - EXPECT_NO_THROW(cont.getAppliedWrench()); - EXPECT_NO_THROW(cont.getLastCommands()); - EXPECT_NO_THROW(cont.getPoseError()); - - cont.setNumberOfJoints(7); - Eigen::MatrixXd jacobian(6,7); - jacobian.setZero(); - EXPECT_NO_THROW(cont.calculateCommandedTorques(q, dq, position, orientation, jacobian)); - - - Eigen::Matrix error_getter, error_state; - error_getter << cont.getPoseError(); - error_state.head(3) << position - position_d; - - EXPECT_DOUBLE_EQ(error_getter(0),error_state(0)); - EXPECT_DOUBLE_EQ(error_getter(1),error_state(1)); - EXPECT_DOUBLE_EQ(error_getter(2),error_state(2)); - -} - - -int main(int argc, char **argv){ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - From 50b9e3423d50de9079dcc617dca46de2f17cefc4 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 31 Oct 2023 19:49:23 +0000 Subject: [PATCH 10/24] New: Clean up ROS tests, switch to example robot - Minor modifications to the CMakeLists - Bumps package version to 1.1 - Switches to the example controller configuration --- CMakeLists.txt | 16 ++-- package.xml | 2 +- res/config/example_controller.yaml | 6 +- test/ros.test | 12 +-- test/ros_tests.cpp | 125 +++++++++++++---------------- 5 files changed, 71 insertions(+), 90 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a896b1c..9669d24 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) +cmake_policy(SET CMP0048 NEW) # Version not in project() command project(cartesian_impedance_controller) find_package(Boost 1.49 REQUIRED) @@ -100,22 +101,15 @@ install(DIRECTORY include/${PROJECT_NAME}/ PATTERN ".svn" EXCLUDE) ## Testing -#Cpp tests if (CATKIN_ENABLE_TESTING) + # Base library tests find_package(rostest REQUIRED) add_rostest_gtest(base_tests test/base.test test/base_tests.cpp) - if(TARGET base_tests) - target_link_libraries(base_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) - endif() -endif() + target_link_libraries(base_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) -#ROS tests -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) + # ROS tests add_rostest_gtest(ros_tests test/ros.test test/ros_tests.cpp) - if(TARGET ros_tests) - target_link_libraries(ros_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) - endif() + target_link_libraries(ros_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) endif() diff --git a/package.xml b/package.xml index 5c0b299..ed409af 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ cartesian_impedance_controller - 1.0.0 + 1.1.0 A Cartesian Impedance controller implementation diff --git a/res/config/example_controller.yaml b/res/config/example_controller.yaml index 3ea1c09..9d31ec0 100644 --- a/res/config/example_controller.yaml +++ b/res/config/example_controller.yaml @@ -1,3 +1,7 @@ +# An example configuration that works with the example +# +# It is also used in the integration tests + CartesianImpedance_trajectory_controller: type: cartesian_impedance_controller/CartesianImpedanceController joints: # Joints to control @@ -20,7 +24,7 @@ CartesianImpedance_trajectory_controller: stiffness: 0.1 # Cartesian and nullspace stiffness wrench: 0.1 # Commanded torque verbosity: - verbose_print: true # Enables additional prints + verbose_print: false # Enables additional prints state_msgs: true # Messages of controller state tf_frames: true # Extra tf frames diff --git a/test/ros.test b/test/ros.test index 21f7ec3..ad0d969 100644 --- a/test/ros.test +++ b/test/ros.test @@ -1,15 +1,11 @@ - - - + + - - - + - - + \ No newline at end of file diff --git a/test/ros_tests.cpp b/test/ros_tests.cpp index 811e097..b8d0b9b 100644 --- a/test/ros_tests.cpp +++ b/test/ros_tests.cpp @@ -6,15 +6,16 @@ #include #include +const std::string ctrl_name = "/CartesianImpedance_trajectory_controller"; //ROSBaseTests tested that key topics in the library are being written. TEST(ROSBaseTests, referenceposeTests) { - ros::NodeHandle n; - ros::Duration timeout(10); + ros::NodeHandle n; + ros::Duration timeout(1); for (int i = 0; i < 5; i++) { auto msg_pose = ros::topic::waitForMessage( - "/CartesianImpedance_trajectory_controller/reference_pose", n, timeout); + ctrl_name + "/reference_pose", n, timeout); ASSERT_TRUE(msg_pose != nullptr) << "Did not receive message on iteration " << i + 1; @@ -25,14 +26,13 @@ TEST(ROSBaseTests, referenceposeTests) TEST(ROSBaseTests, commandedtorqueTests) { - ros::NodeHandle n; - ros::Duration timeout(5); + ros::NodeHandle n; + ros::Duration timeout(5); + const std::string torque_topic {ctrl_name + "/commanded_torques"}; for (int i = 0; i < 5; i++) { - auto msg_torque = ros::topic::waitForMessage( - "/CartesianImpedance_trajectory_controller/commanded_torques", n, timeout); + auto msg_torque = ros::topic::waitForMessage(torque_topic, n, timeout); ASSERT_TRUE(msg_torque != nullptr) << "Did not receive message on iteration " << i + 1; - EXPECT_LE(std::abs(msg_torque->data[0]), 1000.0); } @@ -40,11 +40,11 @@ TEST(ROSBaseTests, commandedtorqueTests) //ROSReconfigureTests tested that we are able to modify external wrench, virtual stiffness, and damping online. TEST(ROSReconfigureTests, wrenchTest) { - ros::NodeHandle n; - ros::Duration timeout(10); + ros::NodeHandle n; + ros::Duration timeout(10); for (int i = 0; i < 5; i++) { auto msg_wup = ros::topic::waitForMessage( - "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/parameter_updates", n, timeout); + ctrl_name + "/cartesian_wrench_reconfigure/parameter_updates", n, timeout); ASSERT_TRUE(msg_wup != nullptr) << "Did not receive message on iteration " << i + 1; @@ -58,11 +58,11 @@ TEST(ROSReconfigureTests, wrenchTest) { } TEST(ROSReconfigureTests, stiffnessTest) { - ros::NodeHandle n; - ros::Duration timeout(5); + ros::NodeHandle n; + ros::Duration timeout(5); for (int i = 0; i < 5; i++) { auto msg_stup = ros::topic::waitForMessage( - "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/parameter_updates", n, timeout); + ctrl_name + "/stiffness_reconfigure/parameter_updates", n, timeout); ASSERT_TRUE(msg_stup != nullptr) << "Did not receive message on iteration " << i + 1; @@ -80,7 +80,7 @@ TEST(ROSReconfigureTests, dampingTest) { ros::Duration timeout(5); for (int i = 0; i < 5; i++) { auto msg_dup = ros::topic::waitForMessage( - "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/parameter_updates", n, timeout); + ctrl_name + "/damping_factors_reconfigure/parameter_updates", n, timeout); ASSERT_TRUE(msg_dup != nullptr) << "Did not receive message on iteration " << i + 1; @@ -95,53 +95,50 @@ TEST(ROSReconfigureTests, dampingTest) { //ROSParameterTests make sure that all the necessary ROS parameters have been created. TEST(ROSParameterTests, paramTest) { - ros::NodeHandle n; - ros::Duration timeout(5); - std::vector params_to_check = { - "/CartesianImpedance_trajectory_controller/arm_id", - "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/apply_wrench", - "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/f_x", - "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/f_y", - "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/f_z", - "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/tau_x", - "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/tau_y", - "/CartesianImpedance_trajectory_controller/cartesian_wrench_reconfigure/tau_z", - "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/nullspace_damping", - "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/rotation_x", - "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/rotation_y", - "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/rotation_z", - "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/translation_x", - "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/translation_y", - "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/translation_z", - "/CartesianImpedance_trajectory_controller/damping_factors_reconfigure/update_damping_factors", - "/CartesianImpedance_trajectory_controller/delta_tau_max", - "/CartesianImpedance_trajectory_controller/dynamic_reconfigure", - "/CartesianImpedance_trajectory_controller/end_effector", - "/CartesianImpedance_trajectory_controller/filtering/nullspace_config", - "/CartesianImpedance_trajectory_controller/filtering/pose", - "/CartesianImpedance_trajectory_controller/filtering/stiffness", - "/CartesianImpedance_trajectory_controller/filtering/wrench", - "/CartesianImpedance_trajectory_controller/handle_trajectories", - "/CartesianImpedance_trajectory_controller/joints", - "/CartesianImpedance_trajectory_controller/root_frame", - "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/nullspace_stiffness", - "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/rotation_x", - "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/rotation_y", - "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/rotation_z", - "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/translation_x", - "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/translation_y", - "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/translation_z", - "/CartesianImpedance_trajectory_controller/stiffness_reconfigure/update_stiffness", - "/CartesianImpedance_trajectory_controller/type", - "/CartesianImpedance_trajectory_controller/update_frequency", - "/CartesianImpedance_trajectory_controller/verbosity/state_msgs", - "/CartesianImpedance_trajectory_controller/verbosity/tf_frames", - "/CartesianImpedance_trajectory_controller/verbosity/verbose_print", - "/CartesianImpedance_trajectory_controller/wrench_ee_frame" + const std::vector params_to_check = { + "cartesian_wrench_reconfigure/apply_wrench", + "cartesian_wrench_reconfigure/f_x", + "cartesian_wrench_reconfigure/f_y", + "cartesian_wrench_reconfigure/f_z", + "cartesian_wrench_reconfigure/tau_x", + "cartesian_wrench_reconfigure/tau_y", + "cartesian_wrench_reconfigure/tau_z", + "damping_factors_reconfigure/nullspace_damping", + "damping_factors_reconfigure/rotation_x", + "damping_factors_reconfigure/rotation_y", + "damping_factors_reconfigure/rotation_z", + "damping_factors_reconfigure/translation_x", + "damping_factors_reconfigure/translation_y", + "damping_factors_reconfigure/translation_z", + "damping_factors_reconfigure/update_damping_factors", + "delta_tau_max", + "dynamic_reconfigure", + "end_effector", + "filtering/nullspace_config", + "filtering/pose", + "filtering/stiffness", + "filtering/wrench", + "handle_trajectories", + "joints", + "root_frame", + "stiffness_reconfigure/nullspace_stiffness", + "stiffness_reconfigure/rotation_x", + "stiffness_reconfigure/rotation_y", + "stiffness_reconfigure/rotation_z", + "stiffness_reconfigure/translation_x", + "stiffness_reconfigure/translation_y", + "stiffness_reconfigure/translation_z", + "stiffness_reconfigure/update_stiffness", + "type", + "update_frequency", + "verbosity/state_msgs", + "verbosity/tf_frames", + "verbosity/verbose_print", + "wrench_ee_frame" }; for (const auto ¶m : params_to_check) { - EXPECT_TRUE(ros::param::has(param)) << "Parameter [" << param << "] does NOT exist."; + EXPECT_TRUE(ros::param::has("/" + ctrl_name + "/" + param)) << "Parameter [" << param << "] does NOT exist."; } } @@ -150,13 +147,3 @@ int main(int argc, char **argv){ ros::init(argc, argv, "test_ros"); return RUN_ALL_TESTS(); } - - - - - - - - - - From 11118c8732247962726b25ab8bbfa0b7a261e274 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 31 Oct 2023 20:02:08 +0000 Subject: [PATCH 11/24] Chore: Removes Panda-specific files --- config/panda.yaml | 29 ------------- launch/panda_real.launch | 10 ----- launch/panda_sim.launch | 91 --------------------------------------- launch/panda_total.launch | 36 ---------------- 4 files changed, 166 deletions(-) delete mode 100644 config/panda.yaml delete mode 100644 launch/panda_real.launch delete mode 100755 launch/panda_sim.launch delete mode 100644 launch/panda_total.launch diff --git a/config/panda.yaml b/config/panda.yaml deleted file mode 100644 index c888900..0000000 --- a/config/panda.yaml +++ /dev/null @@ -1,29 +0,0 @@ -CartesianImpedance_trajectory_controller: - type: cartesian_impedance_controller/CartesianImpedanceController - arm_id: $(arg arm_id) - joints: # Joints to control - - $(arg arm_id)_joint1 - - $(arg arm_id)_joint2 - - $(arg arm_id)_joint3 - - $(arg arm_id)_joint4 - - $(arg arm_id)_joint5 - - $(arg arm_id)_joint6 - - $(arg arm_id)_joint7 - end_effector: $(arg arm_id)_hand_tcp # Link to control arm in - update_frequency: 1000 # Controller update frequency in Hz - dynamic_reconfigure: true # Starts dynamic reconfigure server - handle_trajectories: false # Accept traj., e.g. from MoveIt - wrench_ee_frame: $(arg arm_id)_hand_tcp # Default frame for wrench commands. Replace for $(arg arm_id)_link0 if you want to command the forces in the frame of the base of the robot. - delta_tau_max: 1.0 # Max. commanded torque diff between steps in Nm - filtering: # Update existing values (0.0 1.0] per s - nullspace_config: 1.0 # Nullspace configuration filtering - pose: 1.0 # Reference pose filtering - stiffness: 1.0 # Cartesian and nullspace stiffness - wrench: 1.0 # Commanded torque - verbosity: - verbose_print: false # Enables additional prints - state_msgs: false # Messages of controller state - tf_frames: false # Extra tf frames - - - diff --git a/launch/panda_real.launch b/launch/panda_real.launch deleted file mode 100644 index 6c21231..0000000 --- a/launch/panda_real.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/launch/panda_sim.launch b/launch/panda_sim.launch deleted file mode 100755 index bd69262..0000000 --- a/launch/panda_sim.launch +++ /dev/null @@ -1,91 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [franka_state_controller/joint_states, franka_gripper/joint_states] - - - - diff --git a/launch/panda_total.launch b/launch/panda_total.launch deleted file mode 100644 index 847f34f..0000000 --- a/launch/panda_total.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 7757e4d369b3c8902e50850ec0a490196799dcd4 Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Thu, 2 Nov 2023 10:39:47 +0100 Subject: [PATCH 12/24] Testing the minimal example --- config/example_controller.yaml | 40 +++ launch/examples.launch | 41 +++ src/cartesian_impedance_controller_ros.cpp | 3 +- urdf/robot.urdf.xacro | 377 +++++++++++++++++++++ 4 files changed, 460 insertions(+), 1 deletion(-) create mode 100755 config/example_controller.yaml create mode 100755 launch/examples.launch create mode 100755 urdf/robot.urdf.xacro diff --git a/config/example_controller.yaml b/config/example_controller.yaml new file mode 100755 index 0000000..3ea1c09 --- /dev/null +++ b/config/example_controller.yaml @@ -0,0 +1,40 @@ +CartesianImpedance_trajectory_controller: + type: cartesian_impedance_controller/CartesianImpedanceController + joints: # Joints to control + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + end_effector: tool0 # Link to control arm in + update_frequency: 500 # Controller update frequency in Hz + dynamic_reconfigure: true # Starts dynamic reconfigure server + handle_trajectories: true # Accept traj., e.g. from MoveIt + robot_description: /robot_description # In case of a varying name + wrench_ee_frame: tool0 # Default frame for wrench commands + delta_tau_max: 1.0 # Max. commanded torque diff between steps in Nm + filtering: # Update existing values (0.0 1.0] per s + nullspace_config: 0.1 # Nullspace configuration filtering + pose: 0.1 # Reference pose filtering + stiffness: 0.1 # Cartesian and nullspace stiffness + wrench: 0.1 # Commanded torque + verbosity: + verbose_print: true # Enables additional prints + state_msgs: true # Messages of controller state + tf_frames: true # Extra tf frames + +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 500 + +# Settings for ros_control hardware interface +hardware_interface: + control_freq: 500 # in Hz + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/launch/examples.launch b/launch/examples.launch new file mode 100755 index 0000000..1c80d4a --- /dev/null +++ b/launch/examples.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index a535231..6354ff0 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -54,7 +54,7 @@ namespace cartesian_impedance_controller std::vector joint_names; if (!nh.getParam("joints", joint_names)) { - ROS_ERROR("Invalid or no joint_names parameters provided, aborting controller init!"); + ROS_ERROR("Invalid or no 'joints' parameter provided, aborting controller init!"); return false; } for (size_t i = 0; i < joint_names.size(); ++i) @@ -70,6 +70,7 @@ namespace cartesian_impedance_controller } } ROS_INFO_STREAM("Number of joints specified in parameters: " << joint_names.size()); + this->setNumberOfJoints(joint_names.size()); return true; } diff --git a/urdf/robot.urdf.xacro b/urdf/robot.urdf.xacro new file mode 100755 index 0000000..4fe419d --- /dev/null +++ b/urdf/robot.urdf.xacro @@ -0,0 +1,377 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + true + + + true + + + true + + + true + + + true + + + true + + From 97cbfe161a90c426a9c18a96d4bd8b0a4b6a67be Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 3 Nov 2023 11:16:00 +0100 Subject: [PATCH 13/24] New minimal example, now with 7 joints --- res/config/example_controller.yaml | 2 + res/launch/examples.launch | 2 +- res/urdf/robot.urdf.xacro | 473 +++++++---------------------- res/urdf/robot_old.urdf.xacro | 372 +++++++++++++++++++++++ 4 files changed, 483 insertions(+), 366 deletions(-) create mode 100644 res/urdf/robot_old.urdf.xacro diff --git a/res/config/example_controller.yaml b/res/config/example_controller.yaml index 3ea1c09..d367ec4 100644 --- a/res/config/example_controller.yaml +++ b/res/config/example_controller.yaml @@ -7,6 +7,7 @@ CartesianImpedance_trajectory_controller: - joint4 - joint5 - joint6 + - joint7 end_effector: tool0 # Link to control arm in update_frequency: 500 # Controller update frequency in Hz dynamic_reconfigure: true # Starts dynamic reconfigure server @@ -38,3 +39,4 @@ hardware_interface: - joint4 - joint5 - joint6 + - joint7 diff --git a/res/launch/examples.launch b/res/launch/examples.launch index 5bae475..7f85fbb 100644 --- a/res/launch/examples.launch +++ b/res/launch/examples.launch @@ -38,4 +38,4 @@ - \ No newline at end of file + diff --git a/res/urdf/robot.urdf.xacro b/res/urdf/robot.urdf.xacro index b4b1610..dcf04ec 100644 --- a/res/urdf/robot.urdf.xacro +++ b/res/urdf/robot.urdf.xacro @@ -1,372 +1,115 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - true - - - true - - - true - - - true - - - true - - - true - + + + + diff --git a/res/urdf/robot_old.urdf.xacro b/res/urdf/robot_old.urdf.xacro new file mode 100644 index 0000000..b4b1610 --- /dev/null +++ b/res/urdf/robot_old.urdf.xacro @@ -0,0 +1,372 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + true + + + true + + + true + + + true + + + true + + + true + + From 12bd6c5e2979561f70d2f70e99254f67ecdb5512 Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 3 Nov 2023 13:27:03 +0100 Subject: [PATCH 14/24] Manually added the previously written tests --- CMakeLists.txt | 15 ++ .../cartesian_impedance_controller.h | 2 +- package.xml | 51 +++--- res/urdf/robot.urdf.xacro | 33 ++-- src/cartesian_impedance_controller.cpp | 32 +++- test/base.test | 3 + test/base_tests.cpp | 124 +++++++++++++++ test/ros.test | 11 ++ test/ros_tests.cpp | 149 ++++++++++++++++++ 9 files changed, 380 insertions(+), 40 deletions(-) create mode 100755 test/base.test create mode 100755 test/base_tests.cpp create mode 100644 test/ros.test create mode 100644 test/ros_tests.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 25eb88e..9669d24 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) +cmake_policy(SET CMP0048 NEW) # Version not in project() command project(cartesian_impedance_controller) find_package(Boost 1.49 REQUIRED) @@ -98,3 +99,17 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) + +## Testing +if (CATKIN_ENABLE_TESTING) + # Base library tests + find_package(rostest REQUIRED) + add_rostest_gtest(base_tests test/base.test test/base_tests.cpp) + target_link_libraries(base_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) + + # ROS tests + add_rostest_gtest(ros_tests test/ros.test test/ros_tests.cpp) + target_link_libraries(ros_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) +endif() + + diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller.h b/include/cartesian_impedance_controller/cartesian_impedance_controller.h index 8e100b3..969333b 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller.h +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller.h @@ -284,4 +284,4 @@ namespace cartesian_impedance_controller void updateFilteredWrench(); }; -} // namespace cartesian_impedance_controller \ No newline at end of file +} // namespace cartesian_impedance_controller diff --git a/package.xml b/package.xml index 2b225e2..ed409af 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ - + cartesian_impedance_controller - 1.0.0 + 1.1.0 A Cartesian Impedance controller implementation @@ -9,6 +9,7 @@ Matthias Mayr Oussama Chouman + Julian Salt Ducaju catkin @@ -32,27 +33,35 @@ tf_conversions trajectory_msgs - actionlib - actionlib_msgs - control_msgs - controller_interface - controller_manager - dynamic_reconfigure - eigen_conversions - geometry_msgs - hardware_interface - message_runtime - pluginlib - realtime_tools - roscpp - sensor_msgs - std_msgs - tf - tf_conversions - trajectory_msgs + actionlib + actionlib_msgs + control_msgs + controller_interface + controller_manager + dynamic_reconfigure + eigen_conversions + geometry_msgs + hardware_interface + message_runtime + pluginlib + realtime_tools + roscpp + sensor_msgs + std_msgs + tf + tf_conversions + trajectory_msgs - + + + gtest + eigen + roscpp + rostest + geometry_msgs + std_msgs + dynamic_reconfigure diff --git a/res/urdf/robot.urdf.xacro b/res/urdf/robot.urdf.xacro index dcf04ec..c0f2da0 100644 --- a/res/urdf/robot.urdf.xacro +++ b/res/urdf/robot.urdf.xacro @@ -5,6 +5,9 @@ + + + @@ -30,7 +33,7 @@ - + @@ -40,9 +43,17 @@ - + + + + + + + + + - + @@ -55,7 +66,7 @@ - + transmission_interface/SimpleTransmission hardware_interface/EffortJointInterface @@ -77,13 +88,13 @@ - - - - - - - + + + + + + + diff --git a/src/cartesian_impedance_controller.cpp b/src/cartesian_impedance_controller.cpp index cde3070..14615b0 100644 --- a/src/cartesian_impedance_controller.cpp +++ b/src/cartesian_impedance_controller.cpp @@ -103,6 +103,10 @@ namespace cartesian_impedance_controller void CartesianImpedanceController::setNumberOfJoints(size_t n_joints) { + if (n_joints < 0) + { + throw std::invalid_argument("Number of joints must be positive"); + } this->n_joints_ = n_joints; this->q_ = Eigen::VectorXd::Zero(this->n_joints_); this->dq_ = Eigen::VectorXd::Zero(this->n_joints_); @@ -116,12 +120,25 @@ namespace cartesian_impedance_controller { for (int i = 0; i < 6; i++) { - assert(stiffness(i) >= 0.0 && "Stiffness values need to be positive."); // Set diagonal values of stiffness matrix - this->cartesian_stiffness_target_(i, i) = stiffness(i); + if (stiffness(i) < 0.0) + { + assert(stiffness(i) >= 0 && "Stiffness values need to be positive."); + this->cartesian_stiffness_target_(i, i) = 0.0; + } + else + { + this->cartesian_stiffness_target_(i, i) = stiffness(i); + } + } + if (stiffness(6) < 0.0) { + assert(stiffness(6) >= 0.0 && "Stiffness values need to be positive."); + this->nullspace_stiffness_target_ = 0.0; + } + else + { + this->nullspace_stiffness_target_ = stiffness(6); } - assert(stiffness(6) >= 0.0 && "Stiffness values need to be positive."); - this->nullspace_stiffness_target_ = stiffness(6); if (auto_damping) { this->applyDamping(); @@ -152,8 +169,9 @@ namespace cartesian_impedance_controller { if (damping_new(i) < 0) { + assert(damping_new(i) >= 0 && "Damping factor must not be negative."); damping_new(i) = this->damping_factors_(i); - } + } } this->damping_factors_ = damping_new; this->applyDamping(); @@ -237,8 +255,8 @@ namespace cartesian_impedance_controller updateFilteredWrench(); // Compute error term - error_.head(3) << this->position_ - this->position_d_; - error_.tail(3) << calculateOrientationError(this->orientation_d_, this->orientation_); + this->error_.head(3) << this->position_ - this->position_d_; + this->error_.tail(3) << calculateOrientationError(this->orientation_d_, this->orientation_); // Kinematic pseuoinverse Eigen::MatrixXd jacobian_transpose_pinv; diff --git a/test/base.test b/test/base.test new file mode 100755 index 0000000..954f11c --- /dev/null +++ b/test/base.test @@ -0,0 +1,3 @@ + + + diff --git a/test/base_tests.cpp b/test/base_tests.cpp new file mode 100755 index 0000000..46865cb --- /dev/null +++ b/test/base_tests.cpp @@ -0,0 +1,124 @@ +#include "cartesian_impedance_controller/cartesian_impedance_controller.h" +#include +#include + + +using namespace cartesian_impedance_controller; + + + +CartesianImpedanceController cont; // Create an instance of the controller + + +TEST(ControllerTests, initializationTests){ + const Eigen::Vector3d position_d_target(1.0, 2.0, 3.0); + const Eigen::Quaterniond orientation_d_target(1.0, 0.0, 0.0, 0.0); + + EXPECT_NO_THROW(cont.initDesiredPose(position_d_target, orientation_d_target)); +} + +TEST(ControllerTests, nullspaceConfigTests){ + Eigen::VectorXd q_d_nullspace_target(7); + q_d_nullspace_target << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0; + const int n = 7; + cont.setNumberOfJoints(n); + EXPECT_NO_THROW(cont.initNullspaceConfig(q_d_nullspace_target)); //Should not fail since we have the same number of joints as q_d_nullspace_target elements + + cont.setNumberOfJoints(n+1); + EXPECT_DEBUG_DEATH(cont.initNullspaceConfig(q_d_nullspace_target), "Nullspace target needs to same size as n_joints_"); //Should fail since we have differences between the number of joints and the number of q_d_nullspace_target elements +} + +TEST(ControllerTests, setjointnumberTests){ + const int n = 7; + EXPECT_NO_THROW(cont.setNumberOfJoints(n)); //This value of n shouldn't throw an exception. + EXPECT_ANY_THROW(cont.setNumberOfJoints(-n)); //For a negative number of joints, we expect to catch the error. +} + +TEST(ControllerTests, dampingTests){ + const double d = 3.2; + EXPECT_NO_THROW(cont.setDampingFactors(d,d,d,d,d,d,d)); //This should work. + EXPECT_NO_THROW(cont.setDampingFactors(-d,d,d,d,d,d,d)); //Negative damping factors are ignored outside debug builds + + EXPECT_DEBUG_DEATH(cont.setDampingFactors(-d,d,d,d,d,d,d), "Negative damping factors are tested by asserts"); //This should not work since damping should be positive +} + +TEST(ControllerTests, stiffnessTests) { + Eigen::Matrix stiffness_good, stiffness_bad; + stiffness_good << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + stiffness_bad << -1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + double t_x = 1.0, t_y = 1.0, t_z = 1.0, r_x = 1.0, r_y = 1.0, r_z = 1.0, n = 1.0; + + // Without auto-damping + EXPECT_NO_THROW(cont.setStiffness(stiffness_good, false)); + EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, false), "Stiffness values need to be positive."); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, false)); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, false)); + // With auto-damping + EXPECT_NO_THROW(cont.setStiffness(stiffness_good, true)); + EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, true), "Stiffness values need to be positive."); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, true)); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, true)); +} + + +TEST(ControllerTests, filteringTests){ + EXPECT_NO_THROW(cont.setFiltering(100, 0.5, 0.6, 0.7, 0.8)); + EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 0.7, 0.8), "Update frequency needs to be greater or equal to zero"); + EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 1.7, 0.8), "Filter params need to be between 0 and 1."); +} + +TEST(ControllerTests, saturationTests){ + EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0)); + EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0,100.0)); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0), "Allowed torque change must be positive"); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0,100.0), "Allowed torque change must be positive"); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(2.0,-100.0), "Update frequency needs to be greater or equal to zero"); +} + +TEST(ControllerTests, wrenchTest){ + Eigen::Matrix wrench; + wrench << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + EXPECT_NO_THROW(cont.applyWrench(wrench)); +} + +TEST(ControllerTests, getterTests){ + Eigen::VectorXd q(7); + q << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::VectorXd dq(7); + dq << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::Vector3d position(1.0, 2.0, 3.0); + Eigen::Quaterniond orientation(1.0, 0.0, 0.0, 0.0); + Eigen::Vector3d position_d(2.0, 2.0, 3.0); + Eigen::Quaterniond orientation_d(1.0, 0.0, 0.0, 0.0); + Eigen::Matrix cartesian_stiffness; + double nullspace_stiffness = 1.0; + Eigen::VectorXd q_d_nullspace(7); + q_d_nullspace << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::Matrix cartesian_damping; + + EXPECT_NO_THROW(cont.getState(&q, &dq, &position, &orientation, &position_d, &orientation_d, + &cartesian_stiffness, &nullspace_stiffness, &q_d_nullspace, &cartesian_damping)); + EXPECT_NO_THROW(cont.getAppliedWrench()); + EXPECT_NO_THROW(cont.getLastCommands()); + EXPECT_NO_THROW(cont.getPoseError()); + + cont.setNumberOfJoints(7); + Eigen::MatrixXd jacobian(6,7); + jacobian.setZero(); + EXPECT_NO_THROW(cont.calculateCommandedTorques(q, dq, position, orientation, jacobian)); + + Eigen::Matrix error_getter, error_state; + error_getter << cont.getPoseError(); + error_state.head(3) << position - position_d; + + EXPECT_DOUBLE_EQ(error_getter(0),error_state(0)); + EXPECT_DOUBLE_EQ(error_getter(1),error_state(1)); + EXPECT_DOUBLE_EQ(error_getter(2),error_state(2)); +} + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/test/ros.test b/test/ros.test new file mode 100644 index 0000000..ad0d969 --- /dev/null +++ b/test/ros.test @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/test/ros_tests.cpp b/test/ros_tests.cpp new file mode 100644 index 0000000..b8d0b9b --- /dev/null +++ b/test/ros_tests.cpp @@ -0,0 +1,149 @@ +#include +#include +#include +#include +#include +#include +#include + +const std::string ctrl_name = "/CartesianImpedance_trajectory_controller"; + +//ROSBaseTests tested that key topics in the library are being written. +TEST(ROSBaseTests, referenceposeTests) +{ + ros::NodeHandle n; + ros::Duration timeout(1); + for (int i = 0; i < 5; i++) { + auto msg_pose = ros::topic::waitForMessage( + ctrl_name + "/reference_pose", n, timeout); + + ASSERT_TRUE(msg_pose != nullptr) << "Did not receive message on iteration " << i + 1; + + EXPECT_LE(std::abs(msg_pose->pose.orientation.x), 11); + } + +} + +TEST(ROSBaseTests, commandedtorqueTests) +{ + ros::NodeHandle n; + ros::Duration timeout(5); + const std::string torque_topic {ctrl_name + "/commanded_torques"}; + for (int i = 0; i < 5; i++) { + auto msg_torque = ros::topic::waitForMessage(torque_topic, n, timeout); + + ASSERT_TRUE(msg_torque != nullptr) << "Did not receive message on iteration " << i + 1; + EXPECT_LE(std::abs(msg_torque->data[0]), 1000.0); + } + +} + +//ROSReconfigureTests tested that we are able to modify external wrench, virtual stiffness, and damping online. +TEST(ROSReconfigureTests, wrenchTest) { + ros::NodeHandle n; + ros::Duration timeout(10); + for (int i = 0; i < 5; i++) { + auto msg_wup = ros::topic::waitForMessage( + ctrl_name + "/cartesian_wrench_reconfigure/parameter_updates", n, timeout); + + ASSERT_TRUE(msg_wup != nullptr) << "Did not receive message on iteration " << i + 1; + + for(const dynamic_reconfigure::DoubleParameter& double_param : msg_wup->doubles) { + if(double_param.name == "f_x") { + EXPECT_EQ(double_param.value, 5.0); + break; + } + } + } +} + +TEST(ROSReconfigureTests, stiffnessTest) { + ros::NodeHandle n; + ros::Duration timeout(5); + for (int i = 0; i < 5; i++) { + auto msg_stup = ros::topic::waitForMessage( + ctrl_name + "/stiffness_reconfigure/parameter_updates", n, timeout); + + ASSERT_TRUE(msg_stup != nullptr) << "Did not receive message on iteration " << i + 1; + + for(const dynamic_reconfigure::DoubleParameter& double_param : msg_stup->doubles) { + if(double_param.name == "translation_x") { + EXPECT_EQ(double_param.value, 100.0); + break; + } + } + } +} + +TEST(ROSReconfigureTests, dampingTest) { + ros::NodeHandle n; + ros::Duration timeout(5); + for (int i = 0; i < 5; i++) { + auto msg_dup = ros::topic::waitForMessage( + ctrl_name + "/damping_factors_reconfigure/parameter_updates", n, timeout); + + ASSERT_TRUE(msg_dup != nullptr) << "Did not receive message on iteration " << i + 1; + + for(const dynamic_reconfigure::DoubleParameter& double_param : msg_dup->doubles) { + if(double_param.name == "translation_x") { + EXPECT_EQ(double_param.value, 0.75); + break; + } + } + } +} + +//ROSParameterTests make sure that all the necessary ROS parameters have been created. +TEST(ROSParameterTests, paramTest) { + const std::vector params_to_check = { + "cartesian_wrench_reconfigure/apply_wrench", + "cartesian_wrench_reconfigure/f_x", + "cartesian_wrench_reconfigure/f_y", + "cartesian_wrench_reconfigure/f_z", + "cartesian_wrench_reconfigure/tau_x", + "cartesian_wrench_reconfigure/tau_y", + "cartesian_wrench_reconfigure/tau_z", + "damping_factors_reconfigure/nullspace_damping", + "damping_factors_reconfigure/rotation_x", + "damping_factors_reconfigure/rotation_y", + "damping_factors_reconfigure/rotation_z", + "damping_factors_reconfigure/translation_x", + "damping_factors_reconfigure/translation_y", + "damping_factors_reconfigure/translation_z", + "damping_factors_reconfigure/update_damping_factors", + "delta_tau_max", + "dynamic_reconfigure", + "end_effector", + "filtering/nullspace_config", + "filtering/pose", + "filtering/stiffness", + "filtering/wrench", + "handle_trajectories", + "joints", + "root_frame", + "stiffness_reconfigure/nullspace_stiffness", + "stiffness_reconfigure/rotation_x", + "stiffness_reconfigure/rotation_y", + "stiffness_reconfigure/rotation_z", + "stiffness_reconfigure/translation_x", + "stiffness_reconfigure/translation_y", + "stiffness_reconfigure/translation_z", + "stiffness_reconfigure/update_stiffness", + "type", + "update_frequency", + "verbosity/state_msgs", + "verbosity/tf_frames", + "verbosity/verbose_print", + "wrench_ee_frame" + }; + + for (const auto ¶m : params_to_check) { + EXPECT_TRUE(ros::param::has("/" + ctrl_name + "/" + param)) << "Parameter [" << param << "] does NOT exist."; + } +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_ros"); + return RUN_ALL_TESTS(); +} From 8ee9df3005e75bae541f5f4cf1b98c40aefe2c4f Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 3 Nov 2023 13:36:48 +0100 Subject: [PATCH 15/24] Added reference node sender to tests. All tests succeed! --- test/ros.test | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/test/ros.test b/test/ros.test index ad0d969..a536ac5 100644 --- a/test/ros.test +++ b/test/ros.test @@ -3,9 +3,12 @@ + + + - \ No newline at end of file + From 4e43f8c59cd2f2f9a8fef7d0e0f9d143f2cefea3 Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 3 Nov 2023 13:43:31 +0100 Subject: [PATCH 16/24] Turned off verbose --- res/config/example_controller.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/res/config/example_controller.yaml b/res/config/example_controller.yaml index d367ec4..29a65a7 100644 --- a/res/config/example_controller.yaml +++ b/res/config/example_controller.yaml @@ -21,7 +21,7 @@ CartesianImpedance_trajectory_controller: stiffness: 0.1 # Cartesian and nullspace stiffness wrench: 0.1 # Commanded torque verbosity: - verbose_print: true # Enables additional prints + verbose_print: false # Enables additional prints state_msgs: true # Messages of controller state tf_frames: true # Extra tf frames From 2968e68c99d35497f67cc424af976883d0d5b2cf Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 3 Nov 2023 15:25:16 +0100 Subject: [PATCH 17/24] Prettier urdf --- res/urdf/robot.urdf.xacro | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/res/urdf/robot.urdf.xacro b/res/urdf/robot.urdf.xacro index c0f2da0..185b701 100644 --- a/res/urdf/robot.urdf.xacro +++ b/res/urdf/robot.urdf.xacro @@ -33,7 +33,7 @@ - + @@ -51,7 +51,7 @@ - + @@ -59,7 +59,7 @@ - + @@ -88,13 +88,13 @@ - - - - - - - + + + + + + + From 991161cc7b113938c9d5ee1ac4d2df71ca794ffb Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 3 Nov 2023 15:25:48 +0100 Subject: [PATCH 18/24] Removed reference_pose test --- test/ros.test | 5 +---- test/ros_tests.cpp | 14 -------------- 2 files changed, 1 insertion(+), 18 deletions(-) diff --git a/test/ros.test b/test/ros.test index a536ac5..3ad951d 100644 --- a/test/ros.test +++ b/test/ros.test @@ -2,10 +2,7 @@ - - - - + diff --git a/test/ros_tests.cpp b/test/ros_tests.cpp index b8d0b9b..7aa09af 100644 --- a/test/ros_tests.cpp +++ b/test/ros_tests.cpp @@ -9,20 +9,6 @@ const std::string ctrl_name = "/CartesianImpedance_trajectory_controller"; //ROSBaseTests tested that key topics in the library are being written. -TEST(ROSBaseTests, referenceposeTests) -{ - ros::NodeHandle n; - ros::Duration timeout(1); - for (int i = 0; i < 5; i++) { - auto msg_pose = ros::topic::waitForMessage( - ctrl_name + "/reference_pose", n, timeout); - - ASSERT_TRUE(msg_pose != nullptr) << "Did not receive message on iteration " << i + 1; - - EXPECT_LE(std::abs(msg_pose->pose.orientation.x), 11); - } - -} TEST(ROSBaseTests, commandedtorqueTests) { From 2a78bc9d7b1d3792331575478fa82366c191223d Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Fri, 3 Nov 2023 15:45:15 +0000 Subject: [PATCH 19/24] Chore: Removes wrongly commited copy of robot_old.urdf.xacro --- res/urdf/robot_old.urdf.xacro | 372 ---------------------------------- 1 file changed, 372 deletions(-) delete mode 100644 res/urdf/robot_old.urdf.xacro diff --git a/res/urdf/robot_old.urdf.xacro b/res/urdf/robot_old.urdf.xacro deleted file mode 100644 index b4b1610..0000000 --- a/res/urdf/robot_old.urdf.xacro +++ /dev/null @@ -1,372 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - - - - - true - - - true - - - true - - - true - - - true - - - true - - From 85f32cad191738ea923d96d273e0170f6a4213af Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Fri, 3 Nov 2023 15:46:05 +0000 Subject: [PATCH 20/24] New: Run unit tests after building the code --- .github/workflows/build_code.yml | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build_code.yml b/.github/workflows/build_code.yml index 76a71c3..ac55877 100644 --- a/.github/workflows/build_code.yml +++ b/.github/workflows/build_code.yml @@ -1,6 +1,6 @@ name: CI -on: [push] +on: [push, pull_request] jobs: build-code: @@ -29,4 +29,10 @@ jobs: catkin init catkin config --extend /opt/ros/$ROS_DISTRO catkin build - echo "Compile complete." \ No newline at end of file + echo "Compile complete." + - name: Run tests + shell: bash + run: | + source /opt/ros/$ROS_DISTRO/setup.bash + cd catkin_ws + catkin test \ No newline at end of file From 103c919f782b3de4d7fca5be60628f94f0dbf4ed Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 3 Nov 2023 16:48:43 +0100 Subject: [PATCH 21/24] URDF fixed visual representation of links --- res/urdf/robot.urdf.xacro | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/res/urdf/robot.urdf.xacro b/res/urdf/robot.urdf.xacro index 185b701..0f24866 100644 --- a/res/urdf/robot.urdf.xacro +++ b/res/urdf/robot.urdf.xacro @@ -88,13 +88,13 @@ - - - - - - - + + + + + + + From 36100381d240429fdb80962c3c6819cc6c973dec Mon Sep 17 00:00:00 2001 From: jsaltducaju Date: Fri, 3 Nov 2023 18:26:48 +0100 Subject: [PATCH 22/24] Testing: Introduces new ROS functionality tests --- CMakeLists.txt | 10 +++-- test/ros_func.test | 7 ++++ test/ros_func_tests.cpp | 90 +++++++++++++++++++++++++++++++++++++++++ 3 files changed, 104 insertions(+), 3 deletions(-) create mode 100644 test/ros_func.test create mode 100644 test/ros_func_tests.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9669d24..47ce961 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,12 +104,16 @@ install(DIRECTORY include/${PROJECT_NAME}/ if (CATKIN_ENABLE_TESTING) # Base library tests find_package(rostest REQUIRED) - add_rostest_gtest(base_tests test/base.test test/base_tests.cpp) + add_rostest_gtest(base_tests test/base.test test/base_tests.cpp) target_link_libraries(base_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) - # ROS tests - add_rostest_gtest(ros_tests test/ros.test test/ros_tests.cpp) + # ROS basic tests + add_rostest_gtest(ros_tests test/ros.test test/ros_tests.cpp) target_link_libraries(ros_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) + + # ROS functionality tests + add_rostest_gtest(ros_func_tests test/ros_func.test test/ros_func_tests.cpp) + target_link_libraries(ros_func_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) endif() diff --git a/test/ros_func.test b/test/ros_func.test new file mode 100644 index 0000000..2143ede --- /dev/null +++ b/test/ros_func.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/test/ros_func_tests.cpp b/test/ros_func_tests.cpp new file mode 100644 index 0000000..e223ff4 --- /dev/null +++ b/test/ros_func_tests.cpp @@ -0,0 +1,90 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +const std::string ctrl_name = "/CartesianImpedance_trajectory_controller"; + +//ROSFunctionalityTests tested the controller functionality + +TEST(ROSFunctionalityTests, forceTests) // Tested that a desired external force is being applied. +{ + ros::NodeHandle n; + ros::Duration timeout(5); + ros::Publisher wrench_publisher = n.advertise(ctrl_name +"/set_cartesian_wrench", 500); + + const std::string controller_state {ctrl_name + "/controller_state"}; + auto msg_controller_up = ros::topic::waitForMessage(controller_state, n, timeout); + ASSERT_TRUE(msg_controller_up != nullptr) << "Controller is not up."; + EXPECT_DOUBLE_EQ(std::abs(msg_controller_up->commanded_wrench.force.x), 0.0); + + geometry_msgs::WrenchStamped wrench_msg; + wrench_msg.header.frame_id = "world"; + wrench_msg.header.stamp = ros::Time::now(); + wrench_msg.wrench.force.x = 5.0; + wrench_msg.wrench.force.y = 0.0; + wrench_msg.wrench.force.z = 0.0; + wrench_msg.wrench.torque.x = 0.0; + wrench_msg.wrench.torque.y = 0.0; + wrench_msg.wrench.torque.z = 0.0; + wrench_publisher.publish(wrench_msg); + ros::Duration(1).sleep(); + + for (int i = 0; i < 5; i++) { + auto msg_state = ros::topic::waitForMessage(controller_state, n, timeout); + + ASSERT_TRUE(msg_state != nullptr) << "Did not receive message on iteration " << i + 1; + EXPECT_GE(std::abs(msg_state->commanded_wrench.force.x), 1.0); + } + + // Cancel the wrench + wrench_msg.header.stamp = ros::Time::now(); + wrench_msg.wrench.force.x = 0.0; + wrench_publisher.publish(wrench_msg); + ros::Duration(1).sleep(); +} + +TEST(ROSFunctionalityTests, motionTests) // Tested that a new desired reference pose is sought +{ + ros::NodeHandle n; + ros::Duration timeout(5); + ros::Publisher pose_publisher = n.advertise(ctrl_name +"/reference_pose", 500); + ros::Duration(0.5).sleep(); // C++ publisher need some time to start up + + const std::string controller_state {ctrl_name + "/controller_state"}; + auto msg_controller_up = ros::topic::waitForMessage(controller_state, n, timeout); + ASSERT_TRUE(msg_controller_up != nullptr) << "Controller is not up."; + EXPECT_NEAR(std::abs(msg_controller_up->current_pose.position.y), 0.3, 0.05); + + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = "world"; + pose_msg.header.stamp = ros::Time::now(); + pose_msg.pose.position.x = 0.4; + pose_msg.pose.position.y = 0.0; + pose_msg.pose.position.z = 0.14; + pose_msg.pose.orientation.x = 0.0; + pose_msg.pose.orientation.y = 1.0; + pose_msg.pose.orientation.z = 0.0; + pose_msg.pose.orientation.w = 0.0; + pose_publisher.publish(pose_msg); + ros::Duration(5).sleep(); + + for (int i = 0; i < 5; i++) { + auto msg_state = ros::topic::waitForMessage(controller_state, n, timeout); + ASSERT_TRUE(msg_state != nullptr) << "Did not receive message on iteration " << i + 1; + EXPECT_NEAR(std::abs(msg_state->reference_pose.position.y), 0.0, 0.11); + EXPECT_NEAR(std::abs(msg_state->current_pose.position.y), 0.0, 0.15); + ros::Duration(0.1).sleep(); + } +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_ros"); + return RUN_ALL_TESTS(); +} + From 4c92f24f08f60ff91587708d8e523506c6fefc16 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 7 Nov 2023 08:40:43 +0000 Subject: [PATCH 23/24] Chore: Removes duplicate launch, yaml and urdf --- config/example_controller.yaml | 40 ---- launch/examples.launch | 41 ---- urdf/robot.urdf.xacro | 377 --------------------------------- 3 files changed, 458 deletions(-) delete mode 100755 config/example_controller.yaml delete mode 100755 launch/examples.launch delete mode 100755 urdf/robot.urdf.xacro diff --git a/config/example_controller.yaml b/config/example_controller.yaml deleted file mode 100755 index 3ea1c09..0000000 --- a/config/example_controller.yaml +++ /dev/null @@ -1,40 +0,0 @@ -CartesianImpedance_trajectory_controller: - type: cartesian_impedance_controller/CartesianImpedanceController - joints: # Joints to control - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - end_effector: tool0 # Link to control arm in - update_frequency: 500 # Controller update frequency in Hz - dynamic_reconfigure: true # Starts dynamic reconfigure server - handle_trajectories: true # Accept traj., e.g. from MoveIt - robot_description: /robot_description # In case of a varying name - wrench_ee_frame: tool0 # Default frame for wrench commands - delta_tau_max: 1.0 # Max. commanded torque diff between steps in Nm - filtering: # Update existing values (0.0 1.0] per s - nullspace_config: 0.1 # Nullspace configuration filtering - pose: 0.1 # Reference pose filtering - stiffness: 0.1 # Cartesian and nullspace stiffness - wrench: 0.1 # Commanded torque - verbosity: - verbose_print: true # Enables additional prints - state_msgs: true # Messages of controller state - tf_frames: true # Extra tf frames - -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 500 - -# Settings for ros_control hardware interface -hardware_interface: - control_freq: 500 # in Hz - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 diff --git a/launch/examples.launch b/launch/examples.launch deleted file mode 100755 index 1c80d4a..0000000 --- a/launch/examples.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf/robot.urdf.xacro b/urdf/robot.urdf.xacro deleted file mode 100755 index 4fe419d..0000000 --- a/urdf/robot.urdf.xacro +++ /dev/null @@ -1,377 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - - - - - true - - - true - - - true - - - true - - - true - - - true - - From 4558a04ddcf4db6605404c6b132d4e12851c0bc4 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 7 Nov 2023 08:44:02 +0000 Subject: [PATCH 24/24] Chore: Removes execute right from text files --- cfg/stiffness.cfg | 0 test/base.test | 0 test/base_tests.cpp | 0 3 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 cfg/stiffness.cfg mode change 100755 => 100644 test/base.test mode change 100755 => 100644 test/base_tests.cpp diff --git a/cfg/stiffness.cfg b/cfg/stiffness.cfg old mode 100755 new mode 100644 diff --git a/test/base.test b/test/base.test old mode 100755 new mode 100644 diff --git a/test/base_tests.cpp b/test/base_tests.cpp old mode 100755 new mode 100644