Skip to content

Commit

Permalink
Release Version 5.2.0
Browse files Browse the repository at this point in the history
Version 5.2.0 contains a few fixes and a new feature for the Impedance controller.
  • Loading branch information
domire8 authored Apr 8, 2022
2 parents 0f945c8 + 8d414b8 commit fd606a6
Show file tree
Hide file tree
Showing 21 changed files with 168 additions and 40 deletions.
18 changes: 18 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
# CHANGELOG

Release Versions:
- [5.2.0](#520)
- [5.1.0](#510)
- [5.0.0](#500)
- [4.1.0](#410)
Expand All @@ -10,6 +11,23 @@ Release Versions:
- [2.0.0](#200)
- [1.0.0](#100)

## 5.2.0

Version 5.2.0 contains a few fixes and a new feature for the Impedance controller.

### Features

**controllers**
- Add force limit parameter to Impedance controller (#276)

### Fixes

**general**
- Throw exception if setting state variable from vector with wrong size (#273)
- Improve installation script and python installation guide (#274)
- Fix path in protocol installation guide (#275)
- Fix python bindings import issues (#279)

## 5.1.0

Version 5.1.0 contains a few new features and improvements to the behaviour and usage of the libraries.
Expand Down
2 changes: 1 addition & 1 deletion VERSION
Original file line number Diff line number Diff line change
@@ -1 +1 @@
5.1.0
5.2.0
2 changes: 1 addition & 1 deletion doxygen/doxygen.conf
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ PROJECT_NAME = "Control Libraries"
# could be handy for archiving the generated documentation or if some version
# control system is used.

PROJECT_NUMBER = 5.1.0
PROJECT_NUMBER = 5.2.0

# Using the PROJECT_BRIEF tag one can provide an optional one line description
# for a project that appears at the top of each page and should give viewer a
Expand Down
2 changes: 1 addition & 1 deletion protocol/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ By supplying the `--auto` flag to this script, it will automatically and recursi
If Protobuf is not yet installed, this step will take some time.
```shell
git clone https://github.com/epfl-lasa/control-libraries.git
sudo control-libraries/clproto/install.sh --auto
sudo control-libraries/protocol/install.sh --auto
```

### Copying protobuf dependencies
Expand Down
2 changes: 1 addition & 1 deletion protocol/clproto_cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.9)
project(clproto VERSION 5.1.0)
project(clproto VERSION 5.2.0)

set(CMAKE_CXX_STANDARD 17)

Expand Down
10 changes: 8 additions & 2 deletions python/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,19 @@ Additionally, the installation of the bindings requires the following prerequisi
- `pip3` >= 10.0.0

The installation itself is then quite straightforward:
```shell script
```shell
git clone https://github.com/epfl-lasa/control-libraries

## install control-libraries (skip this stage if already done)
bash control-libraries/source/install.sh
sudo control-libraries/source/install.sh

## install the bindings using the pip installer
pip3 install control-libraries/python
```

If the installation fails, it may be because of non-default installation directories for some dependencies.
In this case, the include path for OSQP and OpenRobots can be set through environment variables before the pip install.
```shell
export OSQP_INCLUDE_DIR='/path/to/include/osqp' # default /usr/local/include/osqp
export OPENROBOTS_INCLUDE_DIR='/path/to/openrobots/include' # default /opt/openrobots/include
pip3 install control-libraries/python
Expand Down
2 changes: 1 addition & 1 deletion python/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
osqp_path_var = 'OSQP_INCLUDE_DIR'
openrobots_path_var = 'OPENROBOTS_INCLUDE_DIR'

__version__ = "5.1.0"
__version__ = "5.2.0"
__libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
__include_dirs__ = ['include']

Expand Down
3 changes: 2 additions & 1 deletion python/source/controllers/bind_cartesian_controllers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,8 @@ using namespace state_representation;
using namespace py_parameter;

void cartesian_controller(py::module_& m) {
py::class_<IController<CartesianState>, std::shared_ptr<IController<CartesianState>>, ParameterMap, PyController<CartesianState>> c(m, "ICartesianController");
py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap");
py::class_<IController<CartesianState>, std::shared_ptr<IController<CartesianState>>, PyController<CartesianState>> c(m, "ICartesianController", parameter_map);

c.def(
"compute_command", py::overload_cast<const CartesianState&, const CartesianState&>(&IController<CartesianState>::compute_command),
Expand Down
3 changes: 2 additions & 1 deletion python/source/controllers/bind_joint_controllers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,8 @@ using namespace state_representation;
using namespace py_parameter;

void joint_controller(py::module_& m) {
py::class_<IController<JointState>, std::shared_ptr<IController<JointState>>, ParameterMap, PyController<JointState>> c(m, "IJointController");
py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap");
py::class_<IController<JointState>, std::shared_ptr<IController<JointState>>, PyController<JointState>> c(m, "IJointController", parameter_map);

c.def(
"compute_command", py::overload_cast<const JointState&, const JointState&>(&IController<JointState>::compute_command),
Expand Down
2 changes: 2 additions & 0 deletions python/source/controllers/controllers_bindings.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@ PYBIND11_MODULE(controllers, m) {
m.attr("__version__") = "dev";
#endif

py::module_::import("state_representation");

bind_type(m);
bind_computational_space(m);
bind_cartesian_controllers(m);
Expand Down
3 changes: 2 additions & 1 deletion python/source/dynamical_systems/bind_cartesian.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,8 @@
using namespace state_representation;

void cartesian(py::module_& m) {
py::class_<IDynamicalSystem<CartesianState>, std::shared_ptr<IDynamicalSystem<CartesianState>>, ParameterMap, PyDynamicalSystem<CartesianState>> c(m, "ICartesianDS");
py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap");
py::class_<IDynamicalSystem<CartesianState>, std::shared_ptr<IDynamicalSystem<CartesianState>>, PyDynamicalSystem<CartesianState>> c(m, "ICartesianDS", parameter_map);

c.def(py::init<>());

Expand Down
3 changes: 2 additions & 1 deletion python/source/dynamical_systems/bind_joint.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,8 @@
using namespace state_representation;

void joint(py::module_& m) {
py::class_<IDynamicalSystem<JointState>, std::shared_ptr<IDynamicalSystem<JointState>>, ParameterMap, PyDynamicalSystem<JointState>> c(m, "IJointDS");
py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap");
py::class_<IDynamicalSystem<JointState>, std::shared_ptr<IDynamicalSystem<JointState>>, PyDynamicalSystem<JointState>> c(m, "IJointDS", parameter_map);

c.def(py::init<>());

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@ PYBIND11_MODULE(dynamical_systems, m) {
m.attr("__version__") = "dev";
#endif

py::module_::import("state_representation");

bind_type(m);
bind_cartesian(m);
bind_joint(m);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,8 @@ def test_get_set_fields(self):
[self.assertAlmostEqual(cs.get_position()[i], position[i]) for i in range(3)]
cs.set_position(1.1, 2.2, 3.3)
assert_array_equal(np.array([1.1, 2.2, 3.3]), cs.get_position())
with self.assertRaises(RuntimeError):
cs.set_position([1., 2., 3., 4.])

# orientation
orientation_vec = np.random.rand(4)
Expand Down
2 changes: 1 addition & 1 deletion source/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
cmake_minimum_required(VERSION 3.15)

project(control_libraries VERSION 5.1.0)
project(control_libraries VERSION 5.2.0)

# Build options
option(BUILD_TESTING "Build all tests." OFF)
Expand Down
24 changes: 22 additions & 2 deletions source/controllers/include/controllers/impedance/Impedance.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,8 @@ class Impedance : public IController<S> {

protected:

void clamp_force(Eigen::VectorXd& force);

/**
* @brief Validate and set parameters for damping, stiffness and inertia gain matrices.
* @param parameter A parameter interface pointer
Expand All @@ -62,8 +64,10 @@ class Impedance : public IController<S> {
damping_; ///< damping matrix of the controller associated to velocity
std::shared_ptr<state_representation::Parameter<Eigen::MatrixXd>>
inertia_; ///< inertia matrix of the controller associated to acceleration
std::shared_ptr<state_representation::Parameter<Eigen::VectorXd>>
force_limit_; ///< vector of force limits for each degree of freedom

const unsigned int dimensions_; ///< dimensionality of the control space and associated gain matricess
const unsigned int dimensions_; ///< dimensionality of the control space and associated gain matrices
};

template<class S>
Expand All @@ -74,10 +78,13 @@ Impedance<S>::Impedance(unsigned int dimensions) :
state_representation::make_shared_parameter<Eigen::MatrixXd>(
"damping", Eigen::MatrixXd::Identity(dimensions, dimensions))), inertia_(
state_representation::make_shared_parameter<Eigen::MatrixXd>(
"inertia", Eigen::MatrixXd::Identity(dimensions, dimensions))), dimensions_(dimensions) {
"inertia", Eigen::MatrixXd::Identity(dimensions, dimensions))), force_limit_(
state_representation::make_shared_parameter<Eigen::VectorXd>(
"force_limit", Eigen::VectorXd::Zero(dimensions))), dimensions_(dimensions) {
this->parameters_.insert(std::make_pair("stiffness", stiffness_));
this->parameters_.insert(std::make_pair("damping", damping_));
this->parameters_.insert(std::make_pair("inertia", inertia_));
this->parameters_.insert(std::make_pair("force_limit", inertia_));
}

template<class S>
Expand All @@ -88,6 +95,16 @@ Impedance<S>::Impedance(
this->set_parameters(parameters);
}

template<class S>
void Impedance<S>::clamp_force(Eigen::VectorXd& force) {
auto limit = this->force_limit_->get_value();
for (std::size_t index = 0; index < this->dimensions_; ++index) {
if (limit(index) > 0.0 && abs(force(index)) > limit(index)) {
force(index) = force(index) > 0.0 ? limit(index) : -limit(index);
}
}
}

template<class S>
void Impedance<S>::validate_and_set_parameter(
const std::shared_ptr<state_representation::ParameterInterface>& parameter
Expand All @@ -98,6 +115,9 @@ void Impedance<S>::validate_and_set_parameter(
this->damping_->set_value(this->gain_matrix_from_parameter(parameter));
} else if (parameter->get_name() == "inertia") {
this->inertia_->set_value(this->gain_matrix_from_parameter(parameter));
} else if (parameter->get_name() == "force_limit") {
auto limit_matrix = this->gain_matrix_from_parameter(parameter);
this->force_limit_->set_value(limit_matrix.diagonal());
}
}

Expand Down
10 changes: 8 additions & 2 deletions source/controllers/src/impedance/Impedance.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,14 +25,19 @@ CartesianState Impedance<CartesianState>::compute_command(
+ this->damping_->get_value().topLeftCorner<3, 3>() * state_error.get_linear_velocity()
+ this->inertia_->get_value().topLeftCorner<3, 3>() * command_state.get_linear_acceleration();
Eigen::Vector3d commanded_force = position_control + state_error.get_force();
command.set_force(commanded_force);

// compute torque (orientation requires special care)
Eigen::Vector3d orientation_control =
this->stiffness_->get_value().bottomRightCorner<3, 3>() * state_error.get_orientation().vec()
+ this->damping_->get_value().bottomRightCorner<3, 3>() * state_error.get_angular_velocity()
+ this->inertia_->get_value().bottomRightCorner<3, 3>() * command_state.get_angular_acceleration();
Eigen::Vector3d commanded_torque = orientation_control + state_error.get_torque();
command.set_torque(commanded_torque);

Eigen::VectorXd wrench(6);
wrench << commanded_force, commanded_torque;
clamp_force(wrench);

command.set_wrench(wrench);
return command;
}

Expand All @@ -48,6 +53,7 @@ JointState Impedance<JointState>::compute_command(
+ this->damping_->get_value() * state_error.get_velocities()
+ this->inertia_->get_value() * command_state.get_accelerations();
Eigen::VectorXd commanded_torques = state_control + state_error.get_torques();
clamp_force(commanded_torques);
command.set_torques(commanded_torques);
return command;
}
Expand Down
82 changes: 81 additions & 1 deletion source/controllers/test/tests/test_impedance.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
#include "state_representation/space/cartesian/CartesianState.hpp"
#include "state_representation/space/cartesian/CartesianWrench.hpp"


using namespace controllers;
using namespace state_representation;

Expand All @@ -29,6 +28,46 @@ TEST(ImpedanceControllerTest, TestCartesianImpedance) {
EXPECT_TRUE(command.data().norm() > 0.);
}

TEST(ImpedanceControllerTest, TestCartesianImpedanceLimits) {
auto controller = CartesianControllerFactory::create_controller(CONTROLLER_TYPE::IMPEDANCE);

auto desired_state = CartesianState::Identity("test");
auto feedback_state = desired_state;

Eigen::VectorXd twist(6);
twist << 1.0, -2.0, 3.0, -4.0, 5.0, -6.0;
feedback_state.set_twist(twist);

// with default unit gains, expect some equivalent response data
CartesianWrench command = controller->compute_command(desired_state, feedback_state);
for (int index = 0; index < 6; ++index) {
EXPECT_NEAR(abs(command.data()(index)), abs(twist(index)), 1e-6);
}

// set a scalar force limit
double limit = 1.0;
EXPECT_NO_THROW(controller->set_parameter_value("force_limit", limit));
// expect all degrees of freedom to have the same force limit
command = controller->compute_command(desired_state, feedback_state);
for (int index = 0; index < 6; ++index) {
EXPECT_LE(abs(command.data()(index)), limit);
}

// set a force limit on each degree of freedom
std::vector<double> limits = {0.5, 1.0, 1.5, 2.0, 3.5, 4.0};
EXPECT_NO_THROW(controller->set_parameter_value("force_limit", limits));

// expect all degrees of freedom to respect the individual force limits
command = controller->compute_command(desired_state, feedback_state);
for (int index = 0; index < 6; ++index) {
EXPECT_LE(abs(command.data()(index)), limits.at(index));
}

// ensure the limit must match the degrees of freedom
EXPECT_THROW(controller->set_parameter_value("force_limit", std::vector<double>({1.0, 2.0})),
state_representation::exceptions::IncompatibleSizeException);
}

TEST(ImpedanceControllerTest, TestJointImpedance) {
int nb_joints = 3;
auto controller = JointControllerFactory::create_controller(CONTROLLER_TYPE::IMPEDANCE, nb_joints);
Expand All @@ -46,6 +85,47 @@ TEST(ImpedanceControllerTest, TestJointImpedance) {
EXPECT_TRUE(command.data().norm() > 0.);
}

TEST(ImpedanceControllerTest, TestJointImpedanceLimits) {
int nb_joints = 3;
auto controller = JointControllerFactory::create_controller(CONTROLLER_TYPE::IMPEDANCE, nb_joints);

auto desired_state = JointState::Zero("test", nb_joints);
auto feedback_state = desired_state;

Eigen::VectorXd positions(3);
positions << 1.0, -2.0, 3.0;
feedback_state.set_positions(positions);

// with default unit gains, expect some equivalent response data
JointTorques command = controller->compute_command(desired_state, feedback_state);
for (int index = 0; index < nb_joints; ++index) {
EXPECT_NEAR(abs(command.data()(index)), abs(positions(index)), 1e-6);
}

// set a scalar force limit
double limit = 1.0;
EXPECT_NO_THROW(controller->set_parameter_value("force_limit", limit));
// expect all degrees of freedom to have the same force limit
command = controller->compute_command(desired_state, feedback_state);
for (int index = 0; index < nb_joints; ++index) {
EXPECT_LE(abs(command.data()(index)), limit);
}

// set a force limit on each degree of freedom
std::vector<double> limits = {0.5, 1.0, 1.5};
EXPECT_NO_THROW(controller->set_parameter_value("force_limit", limits));

// expect all degrees of freedom to respect the individual force limits
command = controller->compute_command(desired_state, feedback_state);
for (int index = 0; index < nb_joints; ++index) {
EXPECT_LE(abs(command.data()(index)), limits.at(index));
}

// ensure the limit must match the degrees of freedom
EXPECT_THROW(controller->set_parameter_value("force_limit", std::vector<double>({1.0, 2.0})),
state_representation::exceptions::IncompatibleSizeException);
}

TEST(ImpedanceControllerTest, TestCartesianToJointImpedance) {
auto controller = CartesianControllerFactory::create_controller(CONTROLLER_TYPE::IMPEDANCE);

Expand Down
5 changes: 4 additions & 1 deletion source/install.sh
Original file line number Diff line number Diff line change
Expand Up @@ -79,13 +79,16 @@ if [ "${BUILD_CONTROLLERS}" == "ON" ] && [ "${BUILD_ROBOT_MODEL}" == "OFF" ]; th
exit 1
fi

# cleanup any previous build folders
rm -rf "${SOURCE_PATH}"/tmp

# install base dependencies
echo ">>> INSTALLING BASE DEPENDENCIES"
INSTALLED_EIGEN=$(pkg-config --modversion eigen3)
if [ "${INSTALLED_EIGEN::4}" != "${EIGEN_VERSION::4}" ]; then
mkdir -p "${SOURCE_PATH}"/tmp/lib && cd "${SOURCE_PATH}"/tmp/lib || exit 1
wget -c "https://gitlab.com/libeigen/eigen/-/archive/${EIGEN_VERSION}/eigen-${EIGEN_VERSION}.tar.gz" -O - | tar -xz || exit 1
cd "eigen-${EIGEN_VERSION}" && mkdir build && cd build && cmake .. && make install || exit 1
cd "eigen-${EIGEN_VERSION}" && mkdir -p build && cd build && cmake .. && make install || exit 1
fi
EIGEN_PATH=$(cmake --find-package -DNAME=Eigen3 -DCOMPILER_ID=GNU -DLANGUAGE=C -DMODE=COMPILE)
if [ "${EIGEN_PATH::14}" != "-I/usr/include" ]; then
Expand Down
Loading

0 comments on commit fd606a6

Please sign in to comment.