diff --git a/linear_feedback_controller/CMakeLists.txt b/CMakeLists.txt similarity index 100% rename from linear_feedback_controller/CMakeLists.txt rename to CMakeLists.txt diff --git a/README.md b/README.md index bd3e3167..d0d9b149 100644 --- a/README.md +++ b/README.md @@ -1,162 +1,9 @@ -[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/loco-3d/linear-feedback-controller/main.svg)](https://results.pre-commit.ci/latest/github/loco-3d/linear-feedback-controller/main) +## linear_feedback_controller -# linear feedback controller +ros controller based on the pal_base_ros_controller package. -This repository contains two packages: -- the [linear_feedback_controller](./linear_feedback_controller/README.md) -- the [linear_feedback_controller_msgs](./linear_feedback_controller_msgs/README.md) +### Test -These two package are under the license [BSD-2 license](./LICENSE). - -The current versions of this 2 package can be found in their respective `package.xml` file. - -Follows a quick description of these package. - -## The linear_feedback_controller - -In this package we implement a [roscontrol](http://wiki.ros.org/ros_control) -controller. It is based on the [pal_base_ros_controller](https://gitlab.com/pal-robotics/LAAS/pal_base_ros_controller_tutorials) -package. - -We implement a RosTopic publisher that sends the state of the robot: -- base configuration -- base velocity -- joint positions -- joint velocities -- joint efforts (torques applied to the joints) - -We then subscribe to a RosTopic that sends us controls for the robot: -- A feedback gain matrix -- A feedforward term in torque -- The state which was used to linearize the control. - -This allows us, for example,to use it on the Talos (1 and 3) robots with a remote controller -using a whole body model predictive control based on [croccodyl](/~https://github.com/loco-3d/crocoddyl) - -Please check the [README.md](./linear_feedback_controller/README.md) of the package for more details. - -## The linear_feedback_controller_msgs - -This package contains the external user interface to the linear_feedback_controller -package. It describes the sensor data exchanged in the previously cited RosTopics. - -And in particular it offers a very simple ROS/[Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) -conversion. This is made to facilitate further computations with the Sensor -data. And ease to fill in the Control message. - -Please check the [README.md](./linear_feedback_controller_msgs/README.md) of the package for more details. - -## Example of usage - -### Requirements - -For this example you need to have access to the dockers in https://gitlab.laas.fr/gsaurel/docker-pal/. -This is a private repository so one need to be part of the LAAS-CNRS french laboratory. -Further development will provide a new public image in order to use this code in simulation. - -### Build the repository using ROS and docker. - -First of all build or pull the docker from https://gitlab.laas.fr/gsaurel/docker-pal/ -using the branch `gsaurel`. - -``` -docker pull gitlab.laas.fr:4567/gsaurel/docker-pal:gsaurel -``` - -Then run the docker and mount (`-v` docker option) the `src` and `build` folder -of your workspace in order to get some cache between to run of the container: - -``` - chown -R :gepetto $(YOUR_WS) # you might need sudo if you get errors here - chmod -R g+rwX $(YOUR_WS) # you might need sudo if you get errors here - xhost +local: - docker run --rm -v $(YOUR_WS)/src:/ws/src -v $(PWD)/Makefile:/ws/Makefile -v /home/$(USER)/devel:/home/user/devel -v $(YOUR_WS)/build:/ws/build --gpus all --net host -e DISPLAY -it gitlab.laas.fr:4567/gsaurel/docker-pal:gsaurel -``` - -If you need another terminal to connect to the last docker container ran you can use: -``` -docker exec --workdir=/ws -u user -it `docker ps --latest --quiet` bash -``` - -Once connected to docker with multiple terminal (5) please run in order: - -- Terminal 1: Start the simulation: -``` -reset && catkin build linear_feedback_controller && source install/setup.bash && roslaunch talos_pal_physics_simulator talos_pal_physics_simulator_with_actuators.launch robot:=full_v2 -``` - -- Terminal 2: Spawn the default controller: -``` -reset && source install/setup.bash && roslaunch talos_controller_configuration default_controllers.launch -``` -in the same terminal, once the robot is in the default pose, kill (`ctrl+C`) the roslaunch - -- Terminal 3: Spawn the linear feedback controller: -``` -reset && source install/setup.bash && roslaunch linear_feedback_controller talos_linear_feedback_controller.launch simulation:=true default_params:=true -``` - -- Terminal 4: For recording logs you can use `rosbag`: -``` -reset && source install/setup.bash && rosbag record -o src/lfc /linear_feedback_controller/sensor_state /linear_feedback_controller/desired_control -``` - -- Terminal 5: For starting a very simple PD controller via Feedback gains run: -``` -reset && source install/setup.bash && rosrun linear_feedback_controller pd_controller -``` - -The next paragraph is the same procedure except one can use the Makefile that is -in the root of this repos to do so. - -### Build the repository using make - -There is also [Makefile](Makefile) to ease the usage of docker and ROS instructions -in order to execute the demo. - -- Create the workspace and clone the repository -``` -mkdir -p ~/devel/workspace/src -cd ~/devel/workspace/src -git clone --recursive git@github.com:loco-3d/linear-feedback-controller.git -``` - -- Run the docker and build in terminal(1) -``` -cd ~/devel/workspace/src/linear_feedback_controller -make docker_pull_and_run -make build -``` - -- Run the simulation, terminal(1) -``` -make simu -``` - -- Run the default controller and kill it when the robot is in half-sitting terminal(2) -``` -make default_ctrl -``` - -- Run the linear feedback controller terminal (2) -``` -make lf_ctrl -``` - -- Run the PD controller using the lfc terminal (3) -``` -make pd_ctrl -``` - -### Copyrights and License - -See the BSD-2 LICENSE file. -For the main authors see the github repository for the main recent contributors. - -Here is the list of the historical main authors: -- Côme Perrot -- Maximilien Naveau - -Main maintainers -- Guilhem Saurel (gsaurel@laas.fr) -- Maximilien Naveau (maximilien.naveau@gmail.com) +- start docker +- `catkin config -DCMAKE_BUILD_TYPE=Debug -DBUILD_TESTING=ON -DCATKIN_ENABLE_TESTING=ON --install` +- `reset && catkin build linear_feedback_controller && catkin run_tests --verbose linear_feedback_controller` diff --git a/linear_feedback_controller/config/talos_linear_feedback_controller_params.yaml b/config/talos_linear_feedback_controller_params.yaml similarity index 100% rename from linear_feedback_controller/config/talos_linear_feedback_controller_params.yaml rename to config/talos_linear_feedback_controller_params.yaml diff --git a/linear_feedback_controller/controller_plugins.xml b/controller_plugins.xml similarity index 100% rename from linear_feedback_controller/controller_plugins.xml rename to controller_plugins.xml diff --git a/doc/README.md b/doc/README.md new file mode 100644 index 00000000..bd3e3167 --- /dev/null +++ b/doc/README.md @@ -0,0 +1,162 @@ +[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/loco-3d/linear-feedback-controller/main.svg)](https://results.pre-commit.ci/latest/github/loco-3d/linear-feedback-controller/main) + +# linear feedback controller + +This repository contains two packages: +- the [linear_feedback_controller](./linear_feedback_controller/README.md) +- the [linear_feedback_controller_msgs](./linear_feedback_controller_msgs/README.md) + +These two package are under the license [BSD-2 license](./LICENSE). + +The current versions of this 2 package can be found in their respective `package.xml` file. + +Follows a quick description of these package. + +## The linear_feedback_controller + +In this package we implement a [roscontrol](http://wiki.ros.org/ros_control) +controller. It is based on the [pal_base_ros_controller](https://gitlab.com/pal-robotics/LAAS/pal_base_ros_controller_tutorials) +package. + +We implement a RosTopic publisher that sends the state of the robot: +- base configuration +- base velocity +- joint positions +- joint velocities +- joint efforts (torques applied to the joints) + +We then subscribe to a RosTopic that sends us controls for the robot: +- A feedback gain matrix +- A feedforward term in torque +- The state which was used to linearize the control. + +This allows us, for example,to use it on the Talos (1 and 3) robots with a remote controller +using a whole body model predictive control based on [croccodyl](/~https://github.com/loco-3d/crocoddyl) + +Please check the [README.md](./linear_feedback_controller/README.md) of the package for more details. + +## The linear_feedback_controller_msgs + +This package contains the external user interface to the linear_feedback_controller +package. It describes the sensor data exchanged in the previously cited RosTopics. + +And in particular it offers a very simple ROS/[Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) +conversion. This is made to facilitate further computations with the Sensor +data. And ease to fill in the Control message. + +Please check the [README.md](./linear_feedback_controller_msgs/README.md) of the package for more details. + +## Example of usage + +### Requirements + +For this example you need to have access to the dockers in https://gitlab.laas.fr/gsaurel/docker-pal/. +This is a private repository so one need to be part of the LAAS-CNRS french laboratory. +Further development will provide a new public image in order to use this code in simulation. + +### Build the repository using ROS and docker. + +First of all build or pull the docker from https://gitlab.laas.fr/gsaurel/docker-pal/ +using the branch `gsaurel`. + +``` +docker pull gitlab.laas.fr:4567/gsaurel/docker-pal:gsaurel +``` + +Then run the docker and mount (`-v` docker option) the `src` and `build` folder +of your workspace in order to get some cache between to run of the container: + +``` + chown -R :gepetto $(YOUR_WS) # you might need sudo if you get errors here + chmod -R g+rwX $(YOUR_WS) # you might need sudo if you get errors here + xhost +local: + docker run --rm -v $(YOUR_WS)/src:/ws/src -v $(PWD)/Makefile:/ws/Makefile -v /home/$(USER)/devel:/home/user/devel -v $(YOUR_WS)/build:/ws/build --gpus all --net host -e DISPLAY -it gitlab.laas.fr:4567/gsaurel/docker-pal:gsaurel +``` + +If you need another terminal to connect to the last docker container ran you can use: +``` +docker exec --workdir=/ws -u user -it `docker ps --latest --quiet` bash +``` + +Once connected to docker with multiple terminal (5) please run in order: + +- Terminal 1: Start the simulation: +``` +reset && catkin build linear_feedback_controller && source install/setup.bash && roslaunch talos_pal_physics_simulator talos_pal_physics_simulator_with_actuators.launch robot:=full_v2 +``` + +- Terminal 2: Spawn the default controller: +``` +reset && source install/setup.bash && roslaunch talos_controller_configuration default_controllers.launch +``` +in the same terminal, once the robot is in the default pose, kill (`ctrl+C`) the roslaunch + +- Terminal 3: Spawn the linear feedback controller: +``` +reset && source install/setup.bash && roslaunch linear_feedback_controller talos_linear_feedback_controller.launch simulation:=true default_params:=true +``` + +- Terminal 4: For recording logs you can use `rosbag`: +``` +reset && source install/setup.bash && rosbag record -o src/lfc /linear_feedback_controller/sensor_state /linear_feedback_controller/desired_control +``` + +- Terminal 5: For starting a very simple PD controller via Feedback gains run: +``` +reset && source install/setup.bash && rosrun linear_feedback_controller pd_controller +``` + +The next paragraph is the same procedure except one can use the Makefile that is +in the root of this repos to do so. + +### Build the repository using make + +There is also [Makefile](Makefile) to ease the usage of docker and ROS instructions +in order to execute the demo. + +- Create the workspace and clone the repository +``` +mkdir -p ~/devel/workspace/src +cd ~/devel/workspace/src +git clone --recursive git@github.com:loco-3d/linear-feedback-controller.git +``` + +- Run the docker and build in terminal(1) +``` +cd ~/devel/workspace/src/linear_feedback_controller +make docker_pull_and_run +make build +``` + +- Run the simulation, terminal(1) +``` +make simu +``` + +- Run the default controller and kill it when the robot is in half-sitting terminal(2) +``` +make default_ctrl +``` + +- Run the linear feedback controller terminal (2) +``` +make lf_ctrl +``` + +- Run the PD controller using the lfc terminal (3) +``` +make pd_ctrl +``` + +### Copyrights and License + +See the BSD-2 LICENSE file. +For the main authors see the github repository for the main recent contributors. + +Here is the list of the historical main authors: +- Côme Perrot +- Maximilien Naveau + +Main maintainers +- Guilhem Saurel (gsaurel@laas.fr) +- Maximilien Naveau (maximilien.naveau@gmail.com) diff --git a/linear_feedback_controller/include/linear_feedback_controller/averaging_filter.hpp b/include/linear_feedback_controller/averaging_filter.hpp similarity index 100% rename from linear_feedback_controller/include/linear_feedback_controller/averaging_filter.hpp rename to include/linear_feedback_controller/averaging_filter.hpp diff --git a/linear_feedback_controller/include/linear_feedback_controller/averaging_filter.hxx b/include/linear_feedback_controller/averaging_filter.hxx similarity index 100% rename from linear_feedback_controller/include/linear_feedback_controller/averaging_filter.hxx rename to include/linear_feedback_controller/averaging_filter.hxx diff --git a/linear_feedback_controller/include/linear_feedback_controller/contact_detector.hpp b/include/linear_feedback_controller/contact_detector.hpp similarity index 100% rename from linear_feedback_controller/include/linear_feedback_controller/contact_detector.hpp rename to include/linear_feedback_controller/contact_detector.hpp diff --git a/linear_feedback_controller/include/linear_feedback_controller/linear_feedback_controller.hpp b/include/linear_feedback_controller/linear_feedback_controller.hpp similarity index 100% rename from linear_feedback_controller/include/linear_feedback_controller/linear_feedback_controller.hpp rename to include/linear_feedback_controller/linear_feedback_controller.hpp diff --git a/linear_feedback_controller/include/linear_feedback_controller/min_jerk.hpp b/include/linear_feedback_controller/min_jerk.hpp similarity index 100% rename from linear_feedback_controller/include/linear_feedback_controller/min_jerk.hpp rename to include/linear_feedback_controller/min_jerk.hpp diff --git a/linear_feedback_controller/launch/talos_linear_feedback_controller.launch b/launch/talos_linear_feedback_controller.launch similarity index 100% rename from linear_feedback_controller/launch/talos_linear_feedback_controller.launch rename to launch/talos_linear_feedback_controller.launch diff --git a/linear_feedback_controller/launch/talos_params.launch b/launch/talos_params.launch similarity index 100% rename from linear_feedback_controller/launch/talos_params.launch rename to launch/talos_params.launch diff --git a/linear_feedback_controller/README.md b/linear_feedback_controller/README.md deleted file mode 100644 index d0d9b149..00000000 --- a/linear_feedback_controller/README.md +++ /dev/null @@ -1,9 +0,0 @@ -## linear_feedback_controller - -ros controller based on the pal_base_ros_controller package. - -### Test - -- start docker -- `catkin config -DCMAKE_BUILD_TYPE=Debug -DBUILD_TESTING=ON -DCATKIN_ENABLE_TESTING=ON --install` -- `reset && catkin build linear_feedback_controller && catkin run_tests --verbose linear_feedback_controller` diff --git a/linear_feedback_controller_msgs/CMakeLists.txt b/linear_feedback_controller_msgs/CMakeLists.txt deleted file mode 100644 index c8cbcbe3..00000000 --- a/linear_feedback_controller_msgs/CMakeLists.txt +++ /dev/null @@ -1,79 +0,0 @@ -cmake_minimum_required(VERSION 3.10) - -# -# Choosing to build the unittest (Catkin vs Modern CMake vs Ament). -# -if(DEFINED CATKIN_ENABLE_TESTING) - set(BUILD_TESTING ${CATKIN_ENABLE_TESTING}) -endif() - -# -# Project definition -# -project(linear_feedback_controller_msgs LANGUAGES CXX) - -# -# Dependencies -# -find_package(Eigen3 REQUIRED) -find_package(catkin REQUIRED COMPONENTS std_msgs sensor_msgs geometry_msgs - eigen_conversions message_generation) - -# -# Generate the messages -# -add_message_files(DIRECTORY msg FILES Contact.msg Control.msg Sensor.msg) -generate_messages(DEPENDENCIES std_msgs sensor_msgs geometry_msgs) - -# -# Export as catkin package (For ROS1 compatibility...). -# -# cmake-format: off -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS std_msgs sensor_msgs geometry_msgs message_runtime eigen_conversions - DEPENDS EIGEN3) -# cmake-format: on - -# -# Main lib. -# -set(${PROJECT_NAME}_HEADERS include/${PROJECT_NAME}/eigen_conversions.hpp) -set(${PROJECT_NAME}_SOURCES "") -add_library(${PROJECT_NAME} INTERFACE) -target_link_libraries( - ${PROJECT_NAME} - INTERFACE Eigen3::Eigen ${std_msgs_LIBRARIES} ${sensor_msgs_LIBRARIES} - ${eigen_conversions_LIBRARIES} ${geometry_msgs_LIBRARIES} - ${message_generation_LIBRARIES}) -target_include_directories( - ${PROJECT_NAME} SYSTEM - INTERFACE ${std_msgs_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS} - ${eigen_conversions_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} - ${message_generation_INCLUDE_DIRS}) -target_include_directories( - ${PROJECT_NAME} - INTERFACE $ - $) - -# -# Unit tests -# -if(BUILD_TESTING) - catkin_add_gtest(test_eigen_conversions tests/test_eigen_conversions.cpp - tests/gtest_main.cpp) - target_link_libraries(test_eigen_conversions ${PROJECT_NAME}) -endif() - -# -# Install -# -install( - TARGETS ${PROJECT_NAME} - EXPORT ${TARGETS_EXPORT_NAME} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(FILES package.xml DESTINATION share/${PROJECT_NAME}) -install( - DIRECTORY include/${PROJECT_NAME} - DESTINATION include - PATTERN "third_party" EXCLUDE) diff --git a/linear_feedback_controller_msgs/README.md b/linear_feedback_controller_msgs/README.md deleted file mode 100644 index ed6ced55..00000000 --- a/linear_feedback_controller_msgs/README.md +++ /dev/null @@ -1 +0,0 @@ -ROS msgs that interface the linear_feedback_controller package. diff --git a/linear_feedback_controller_msgs/include/linear_feedback_controller_msgs/eigen_conversions.hpp b/linear_feedback_controller_msgs/include/linear_feedback_controller_msgs/eigen_conversions.hpp deleted file mode 100644 index f8806315..00000000 --- a/linear_feedback_controller_msgs/include/linear_feedback_controller_msgs/eigen_conversions.hpp +++ /dev/null @@ -1,170 +0,0 @@ -#ifndef ROS_WBMPC_MSGS_ROS_EIGEN_CONVERSION_HPP -#define ROS_WBMPC_MSGS_ROS_EIGEN_CONVERSION_HPP - -#include -#include -#include - -#include -#include - -namespace linear_feedback_controller_msgs { - -namespace Eigen { - -struct JointState { - std::vector name; - ::Eigen::VectorXd position; - ::Eigen::VectorXd velocity; - ::Eigen::VectorXd effort; -}; - -struct Contact { - bool active; - std::string name; - ::Eigen::Matrix wrench; -}; - -struct Sensor { - ::Eigen::Matrix base_pose; - ::Eigen::Matrix base_twist; - JointState joint_state; - std::vector contacts; -}; - -struct Control { - ::Eigen::MatrixXd feedback_gain; - ::Eigen::VectorXd feedforward; - linear_feedback_controller_msgs::Eigen::Sensor initial_state; -}; -} // namespace Eigen - -// Create an alias to use only one namespace here. -template -void matrixEigenToMsg(const ::Eigen::MatrixBase& e, - std_msgs::Float64MultiArray& m) { - tf::matrixEigenToMsg(e, m); -} - -template -void matrixMsgToEigen(const std_msgs::Float64MultiArray& m, - ::Eigen::MatrixBase& e) { - assert(m.layout.dim.size() == 2 && "The ROS message must be a 2D matrix."); - - // verify the input object. - assert(m.layout.dim[0].stride == e.size() && - "Input and output size do not match."); - assert(m.layout.dim[0].size == e.rows() && - "Input and output size do not match."); - assert(m.layout.dim[1].stride == e.cols() && - "Input and output size do not match."); - assert(m.layout.dim[1].size == e.cols() && - "Input and output size do not match."); - - int ii = 0; - for (int i = 0; i < e.rows(); ++i) { - for (int j = 0; j < e.cols(); ++j) { - e(i, j) = m.data[ii++]; - } - } -} - -/** - * Msg To Eigen. - */ - -inline void jointStateMsgToEigen( - const sensor_msgs::JointState& m, - linear_feedback_controller_msgs::Eigen::JointState& e) { - e.name = m.name; - e.position = ::Eigen::Map(m.position.data(), - m.position.size()); - e.velocity = ::Eigen::Map(m.velocity.data(), - m.velocity.size()); - e.effort = - ::Eigen::Map(m.effort.data(), m.effort.size()); -} - -inline void contactMsgToEigen( - const linear_feedback_controller_msgs::Contact& m, - linear_feedback_controller_msgs::Eigen::Contact& e) { - e.active = m.active; - e.name = m.name; - tf::wrenchMsgToEigen(m.wrench, e.wrench); -} - -inline void sensorMsgToEigen( - const Sensor& m, linear_feedback_controller_msgs::Eigen::Sensor& e) { - e.base_pose(0) = m.base_pose.position.x; - e.base_pose(1) = m.base_pose.position.y; - e.base_pose(2) = m.base_pose.position.z; - e.base_pose(3) = m.base_pose.orientation.x; - e.base_pose(4) = m.base_pose.orientation.y; - e.base_pose(5) = m.base_pose.orientation.z; - e.base_pose(6) = m.base_pose.orientation.w; - tf::twistMsgToEigen(m.base_twist, e.base_twist); - jointStateMsgToEigen(m.joint_state, e.joint_state); - e.contacts.resize(m.contacts.size()); - for (std::size_t i = 0; i < m.contacts.size(); ++i) { - contactMsgToEigen(m.contacts[i], e.contacts[i]); - } -} - -inline void controlMsgToEigen( - const Control& m, linear_feedback_controller_msgs::Eigen::Control& e) { - matrixMsgToEigen(m.feedback_gain, e.feedback_gain); - matrixMsgToEigen(m.feedforward, e.feedforward); - sensorMsgToEigen(m.initial_state, e.initial_state); -} - -/** - * Eigen To Msg. - */ - -inline void jointStateEigenToMsg( - const linear_feedback_controller_msgs::Eigen::JointState& e, - sensor_msgs::JointState& m) { - m.name = e.name; - m.position = std::vector(e.position.data(), - e.position.data() + e.position.size()); - m.velocity = std::vector(e.velocity.data(), - e.velocity.data() + e.velocity.size()); - m.effort = - std::vector(e.effort.data(), e.effort.data() + e.effort.size()); -} - -inline void contactEigenToMsg( - const linear_feedback_controller_msgs::Eigen::Contact& e, - linear_feedback_controller_msgs::Contact& m) { - m.active = e.active; - m.name = e.name; - tf::wrenchEigenToMsg(e.wrench, m.wrench); -} - -inline void sensorEigenToMsg( - const linear_feedback_controller_msgs::Eigen::Sensor e, Sensor& m) { - m.base_pose.position.x = e.base_pose(0); - m.base_pose.position.y = e.base_pose(1); - m.base_pose.position.z = e.base_pose(2); - m.base_pose.orientation.x = e.base_pose(3); - m.base_pose.orientation.y = e.base_pose(4); - m.base_pose.orientation.z = e.base_pose(5); - m.base_pose.orientation.w = e.base_pose(6); - tf::twistEigenToMsg(e.base_twist, m.base_twist); - jointStateEigenToMsg(e.joint_state, m.joint_state); - m.contacts.resize(e.contacts.size()); - for (std::size_t i = 0; i < e.contacts.size(); ++i) { - contactEigenToMsg(e.contacts[i], m.contacts[i]); - } -} - -inline void controlEigenToMsg( - const linear_feedback_controller_msgs::Eigen::Control& e, Control& m) { - matrixEigenToMsg(e.feedback_gain, m.feedback_gain); - matrixEigenToMsg(e.feedforward, m.feedforward); - sensorEigenToMsg(e.initial_state, m.initial_state); -} - -} // namespace linear_feedback_controller_msgs - -#endif // ROS_WBMPC_MSGS_ROS_EIGEN_CONVERSION_HPP diff --git a/linear_feedback_controller_msgs/msg/Contact.msg b/linear_feedback_controller_msgs/msg/Contact.msg deleted file mode 100644 index 8ebff8c5..00000000 --- a/linear_feedback_controller_msgs/msg/Contact.msg +++ /dev/null @@ -1,3 +0,0 @@ -bool active -string name -geometry_msgs/Wrench wrench diff --git a/linear_feedback_controller_msgs/msg/Control.msg b/linear_feedback_controller_msgs/msg/Control.msg deleted file mode 100644 index 82af471c..00000000 --- a/linear_feedback_controller_msgs/msg/Control.msg +++ /dev/null @@ -1,37 +0,0 @@ -# This is a message that holds data to describe the state of a set of torque controlled joints. -# -# The state of each joint (revolute or prismatic) is defined by: -# * the position of the joint (rad or m), -# * the velocity of the joint (rad/s or m/s) and -# * the effort that is applied in the joint (Nm or N). -# -# Each joint is uniquely identified by its name -# The header specifies the time at which the joint states were recorded. All the joint states -# in one message have to be recorded at the same time. -# -# This message consists of a multiple arrays, one for each part of the joint state. -# The goal is to make each of the fields optional. When e.g. your joints have no -# effort associated with them, you can leave the effort array empty. -# -# All arrays in this message should have the same size, or be empty. -# This is the only way to uniquely associate the joint name with the correct -# states. - -# Metadata -########## - -Header header - -# Control data -############## - -# Linear feedback gain matrix. -std_msgs/Float64MultiArray feedback_gain - -# Vector of the feedforward. -std_msgs/Float64MultiArray feedforward - -# Initial state -############### - -Sensor initial_state diff --git a/linear_feedback_controller_msgs/msg/Sensor.msg b/linear_feedback_controller_msgs/msg/Sensor.msg deleted file mode 100644 index ad5a4833..00000000 --- a/linear_feedback_controller_msgs/msg/Sensor.msg +++ /dev/null @@ -1,34 +0,0 @@ -# This is a message that holds data to describe the state of a set of torque controlled joints. -# -# The state of each joint (revolute or prismatic) is defined by: -# * the position of the joint (rad or m), -# * the velocity of the joint (rad/s or m/s) and -# * the effort that is applied in the joint (Nm or N). -# -# Each joint is uniquely identified by its name -# The header specifies the time at which the joint states were recorded. All the joint states -# in one message have to be recorded at the same time. -# -# This message consists of a multiple arrays, one for each part of the joint state. -# The goal is to make each of the fields optional. When e.g. your joints have no -# effort associated with them, you can leave the effort array empty. -# -# All arrays in this message should have the same size, or be empty. -# This is the only way to uniquely associate the joint name with the correct -# states. - -# Metadata -########## - -Header header - -# Sensory data: -############### - -# Free flyer state. -geometry_msgs/Pose base_pose -geometry_msgs/Twist base_twist -# Joint state. -sensor_msgs/JointState joint_state -# Contact state. -Contact[] contacts diff --git a/linear_feedback_controller_msgs/package.xml b/linear_feedback_controller_msgs/package.xml deleted file mode 100644 index 7efa605e..00000000 --- a/linear_feedback_controller_msgs/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - linear_feedback_controller_msgs - 0.1.0 - - ROS msgs that interface the linear_feedback_controller package. - - Guilhem Saurel - Maximilien Naveau - - BSD-2 - /~https://github.com/loco-3d/linear_feedback_controller_msgs - - catkin - - message_generation - roscpp - - message_runtime - roscpp - rospy - - std_msgs - geometry_msgs - sensor_msgs - eigen_conversions - - rospy - gtest - - diff --git a/linear_feedback_controller_msgs/tests/gtest_main.cpp b/linear_feedback_controller_msgs/tests/gtest_main.cpp deleted file mode 100644 index 9996ba02..00000000 --- a/linear_feedback_controller_msgs/tests/gtest_main.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include - -int main(int argc, char **argv) { - ::testing::InitGoogleTest(&argc, argv); - auto ret = RUN_ALL_TESTS(); - - return ret; -} diff --git a/linear_feedback_controller_msgs/tests/test_eigen_conversions.cpp b/linear_feedback_controller_msgs/tests/test_eigen_conversions.cpp deleted file mode 100644 index ef7ec73b..00000000 --- a/linear_feedback_controller_msgs/tests/test_eigen_conversions.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include - -#include "linear_feedback_controller_msgs/Control.h" -#include "linear_feedback_controller_msgs/Sensor.h" -#include "linear_feedback_controller_msgs/eigen_conversions.hpp" - -namespace lfc_msgs = linear_feedback_controller_msgs; - -class LinearFeedbackControllerMsgsTest : public ::testing::Test { - protected: - void SetUp() override {} - - void TearDown() override {} -}; -class DISABLED_LinearFeedbackControllerMsgsTest - : public LinearFeedbackControllerMsgsTest {}; - -TEST_F(LinearFeedbackControllerMsgsTest, checkEigenConstructors) { - lfc_msgs::Eigen::Sensor s; - lfc_msgs::Eigen::Control c; -} - -TEST_F(LinearFeedbackControllerMsgsTest, checkRosConstructors) { - lfc_msgs::Sensor s; - lfc_msgs::Control c; -} - -TEST_F(LinearFeedbackControllerMsgsTest, checkRosEigenMatrixConversion) { - Eigen::MatrixXd eigen_mat = Eigen::MatrixXd::Random(5, 6); - std_msgs::Float64MultiArray ros_mat; - Eigen::MatrixXd eigen_mat_test; - - lfc_msgs::matrixEigenToMsg(eigen_mat, ros_mat); - lfc_msgs::matrixMsgToEigen(ros_mat, eigen_mat_test); - - ASSERT_EQ(eigen_mat, eigen_mat_test); -} - -TEST_F(LinearFeedbackControllerMsgsTest, checkRosEigenJointStateConversion) { - lfc_msgs::Eigen::JointState eigen_joint_state; - sensor_msgs::JointState ros_joint_state; - lfc_msgs::Eigen::JointState eigen_joint_state_test; - - eigen_joint_state.name = {"1", "2", "3", "4", "5", "6"}; - eigen_joint_state.position = - Eigen::VectorXd::Random(eigen_joint_state.name.size()); - eigen_joint_state.velocity = - Eigen::VectorXd::Random(eigen_joint_state.name.size()); - eigen_joint_state.effort = - Eigen::VectorXd::Random(eigen_joint_state.name.size()); - - lfc_msgs::jointStateEigenToMsg(eigen_joint_state, ros_joint_state); - lfc_msgs::jointStateMsgToEigen(ros_joint_state, eigen_joint_state_test); - - ASSERT_EQ(eigen_joint_state.name, eigen_joint_state_test.name); - ASSERT_EQ(eigen_joint_state.position, eigen_joint_state_test.position); - ASSERT_EQ(eigen_joint_state.velocity, eigen_joint_state_test.velocity); - ASSERT_EQ(eigen_joint_state.effort, eigen_joint_state_test.effort); -} - -TEST_F(LinearFeedbackControllerMsgsTest, checkRosEigenSensorConversion) { - lfc_msgs::Eigen::Sensor e; - lfc_msgs::Sensor m; - lfc_msgs::Eigen::Sensor etest; - - e.base_pose = Eigen::Matrix::Random(); - Eigen::Quaterniond q; - q.coeffs() = Eigen::Vector4d::Random(); - q.normalize(); - e.base_pose.tail<4>() = q.coeffs(); - e.base_twist = Eigen::Matrix::Random(); - e.joint_state.name = {"1", "2", "3", "4", "5", "6"}; - e.joint_state.position = Eigen::VectorXd::Random(e.joint_state.name.size()); - e.joint_state.velocity = Eigen::VectorXd::Random(e.joint_state.name.size()); - e.joint_state.effort = Eigen::VectorXd::Random(e.joint_state.name.size()); - - lfc_msgs::sensorEigenToMsg(e, m); - lfc_msgs::sensorMsgToEigen(m, etest); - - ASSERT_TRUE(e.base_pose.isApprox(etest.base_pose)); - ASSERT_EQ(e.base_twist, etest.base_twist); - ASSERT_EQ(e.joint_state.name, etest.joint_state.name); - ASSERT_EQ(e.joint_state.position, etest.joint_state.position); - ASSERT_EQ(e.joint_state.velocity, etest.joint_state.velocity); - ASSERT_EQ(e.joint_state.effort, etest.joint_state.effort); -} - -TEST_F(LinearFeedbackControllerMsgsTest, checkRosEigenControlConversion) { - lfc_msgs::Eigen::Control e; - lfc_msgs::Control m; - lfc_msgs::Eigen::Control etest; - - e.initial_state.base_pose = Eigen::Matrix::Random(); - Eigen::Quaterniond q; - q.coeffs() = Eigen::Vector4d::Random(); - q.normalize(); - e.initial_state.base_pose.tail<4>() = q.coeffs(); - e.initial_state.base_twist = Eigen::Matrix::Random(); - e.initial_state.joint_state.name = {"1", "2", "3", "4", "5", "6"}; - e.initial_state.joint_state.position = - Eigen::VectorXd::Random(e.initial_state.joint_state.name.size()); - e.initial_state.joint_state.velocity = - Eigen::VectorXd::Random(e.initial_state.joint_state.name.size()); - e.initial_state.joint_state.effort = - Eigen::VectorXd::Random(e.initial_state.joint_state.name.size()); - e.feedback_gain = Eigen::MatrixXd::Random(8, 4); - e.feedforward = Eigen::VectorXd::Random(8); - - lfc_msgs::controlEigenToMsg(e, m); - lfc_msgs::controlMsgToEigen(m, etest); - - ASSERT_TRUE( - e.initial_state.base_pose.isApprox(etest.initial_state.base_pose)); - ASSERT_EQ(e.initial_state.base_twist, etest.initial_state.base_twist); - ASSERT_EQ(e.initial_state.joint_state.name, - etest.initial_state.joint_state.name); - ASSERT_EQ(e.initial_state.joint_state.position, - etest.initial_state.joint_state.position); - ASSERT_EQ(e.initial_state.joint_state.velocity, - etest.initial_state.joint_state.velocity); - ASSERT_EQ(e.initial_state.joint_state.effort, - etest.initial_state.joint_state.effort); - ASSERT_EQ(e.feedback_gain, etest.feedback_gain); - ASSERT_EQ(e.feedforward, etest.feedforward); -} diff --git a/linear_feedback_controller/package.xml b/package.xml similarity index 100% rename from linear_feedback_controller/package.xml rename to package.xml diff --git a/linear_feedback_controller/src/contact_detector.cpp b/src/contact_detector.cpp similarity index 100% rename from linear_feedback_controller/src/contact_detector.cpp rename to src/contact_detector.cpp diff --git a/linear_feedback_controller/src/linear_feedback_controller.cpp b/src/linear_feedback_controller.cpp similarity index 100% rename from linear_feedback_controller/src/linear_feedback_controller.cpp rename to src/linear_feedback_controller.cpp diff --git a/linear_feedback_controller/src/min_jerk.cpp b/src/min_jerk.cpp similarity index 100% rename from linear_feedback_controller/src/min_jerk.cpp rename to src/min_jerk.cpp diff --git a/linear_feedback_controller/src/min_jerk.wxmx b/src/min_jerk.wxmx similarity index 100% rename from linear_feedback_controller/src/min_jerk.wxmx rename to src/min_jerk.wxmx diff --git a/linear_feedback_controller/src/pd_controller.py b/src/pd_controller.py similarity index 100% rename from linear_feedback_controller/src/pd_controller.py rename to src/pd_controller.py diff --git a/linear_feedback_controller/src/plugins.cpp b/src/plugins.cpp similarity index 100% rename from linear_feedback_controller/src/plugins.cpp rename to src/plugins.cpp diff --git a/linear_feedback_controller/tests/ros_gtest_main.cpp b/tests/ros_gtest_main.cpp similarity index 100% rename from linear_feedback_controller/tests/ros_gtest_main.cpp rename to tests/ros_gtest_main.cpp diff --git a/linear_feedback_controller/tests/rosconsole.conf b/tests/rosconsole.conf similarity index 100% rename from linear_feedback_controller/tests/rosconsole.conf rename to tests/rosconsole.conf diff --git a/linear_feedback_controller/tests/test_averaging_filter.cpp b/tests/test_averaging_filter.cpp similarity index 100% rename from linear_feedback_controller/tests/test_averaging_filter.cpp rename to tests/test_averaging_filter.cpp diff --git a/linear_feedback_controller/tests/test_linear_feedback_controller.py b/tests/test_linear_feedback_controller.py similarity index 100% rename from linear_feedback_controller/tests/test_linear_feedback_controller.py rename to tests/test_linear_feedback_controller.py diff --git a/linear_feedback_controller/tests/test_linear_feedback_controller.test b/tests/test_linear_feedback_controller.test similarity index 100% rename from linear_feedback_controller/tests/test_linear_feedback_controller.test rename to tests/test_linear_feedback_controller.test diff --git a/linear_feedback_controller/tests/test_linear_feedback_controller_basic.cpp b/tests/test_linear_feedback_controller_basic.cpp similarity index 100% rename from linear_feedback_controller/tests/test_linear_feedback_controller_basic.cpp rename to tests/test_linear_feedback_controller_basic.cpp diff --git a/linear_feedback_controller/tests/test_linear_feedback_controller_basic.test b/tests/test_linear_feedback_controller_basic.test similarity index 100% rename from linear_feedback_controller/tests/test_linear_feedback_controller_basic.test rename to tests/test_linear_feedback_controller_basic.test diff --git a/linear_feedback_controller/tests/test_min_jerk.cpp b/tests/test_min_jerk.cpp similarity index 100% rename from linear_feedback_controller/tests/test_min_jerk.cpp rename to tests/test_min_jerk.cpp