From 9646e428c66b59cc307d8ca9dcca14577a581665 Mon Sep 17 00:00:00 2001 From: Zhenbiao Huang Date: Sat, 14 Sep 2024 17:54:39 +0800 Subject: [PATCH] fixed the frequency problem --- commands/keyboard_input/src/KeyboardInput.cpp | 4 +- .../unitree_guide_controller/CMakeLists.txt | 13 +- .../config/controller.yaml | 1 - .../unitree_guide_controller/FSM/FSMState.h | 2 +- .../FSM/StateFixedDown.h | 2 +- .../FSM/StateFixedStand.h | 2 +- .../UnitreeGuideController.h | 3 + .../interface.h => control/CtrlComponent.h} | 4 +- .../control/Estimator.h | 16 + .../quadProgpp/Array.hh | 2215 +++++++++++++++++ .../quadProgpp/QuadProg++.hh | 78 + .../{robotics => robot}/QuadrupedRobot.h | 0 .../{robotics => robot}/RobotLeg.h | 0 .../src/FSM/StateFixedDown.cpp | 7 +- .../src/FSM/StateFixedStand.cpp | 7 +- .../src/FSM/StateFreeStand.cpp | 4 +- .../src/FSM/StatePassive.cpp | 4 +- .../src/FSM/StateSwingTest.cpp | 4 +- .../src/UnitreeGuideController.cpp | 6 +- .../src/control/Estimator.cpp | 5 + .../src/quadProgpp/Array.cc | 28 + .../src/quadProgpp/QuadProg++.cc | 729 ++++++ .../{robotics => robot}/QuadrupedRobot.cpp | 4 +- .../src/{robotics => robot}/RobotLeg.cpp | 4 +- .../go2_description/config/robot_control.yaml | 36 +- .../go2_description/launch/hardware.launch.py | 94 +- descriptions/go2_description/package.xml | 1 - 27 files changed, 3205 insertions(+), 68 deletions(-) rename controllers/unitree_guide_controller/include/unitree_guide_controller/{common/interface.h => control/CtrlComponent.h} (91%) create mode 100644 controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h create mode 100755 controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/Array.hh create mode 100755 controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/QuadProg++.hh rename controllers/unitree_guide_controller/include/unitree_guide_controller/{robotics => robot}/QuadrupedRobot.h (100%) rename controllers/unitree_guide_controller/include/unitree_guide_controller/{robotics => robot}/RobotLeg.h (100%) create mode 100644 controllers/unitree_guide_controller/src/control/Estimator.cpp create mode 100755 controllers/unitree_guide_controller/src/quadProgpp/Array.cc create mode 100755 controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc rename controllers/unitree_guide_controller/src/{robotics => robot}/QuadrupedRobot.cpp (96%) rename controllers/unitree_guide_controller/src/{robotics => robot}/RobotLeg.cpp (93%) diff --git a/commands/keyboard_input/src/KeyboardInput.cpp b/commands/keyboard_input/src/KeyboardInput.cpp index b18c4be..74d4da7 100644 --- a/commands/keyboard_input/src/KeyboardInput.cpp +++ b/commands/keyboard_input/src/KeyboardInput.cpp @@ -27,10 +27,8 @@ void KeyboardInput::timer_callback() { inputs_.rx = 0; inputs_.ry = 0; } - } else { - inputs_.command = 0; + publisher_->publish(inputs_); } - publisher_->publish(inputs_); } void KeyboardInput::check_command(const char key) { diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index 1f0909d..c595cb1 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/controllers/unitree_guide_controller/CMakeLists.txt @@ -24,14 +24,23 @@ foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS}) endforeach () add_library(${PROJECT_NAME} SHARED + src/UnitreeGuideController.cpp + src/FSM/StatePassive.cpp src/FSM/StateFixedDown.cpp src/FSM/StateFixedStand.cpp src/FSM/StateSwingTest.cpp src/FSM/StateFreeStand.cpp - src/robotics/QuadrupedRobot.cpp - src/robotics/RobotLeg.cpp + + src/robot/QuadrupedRobot.cpp + src/robot/RobotLeg.cpp + + src/control/Estimator.cpp + + src/quadProgpp/Array.cc + src/quadProgpp/QuadProg++.cc + ) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/controllers/unitree_guide_controller/config/controller.yaml b/controllers/unitree_guide_controller/config/controller.yaml index a386298..e21fb2a 100644 --- a/controllers/unitree_guide_controller/config/controller.yaml +++ b/controllers/unitree_guide_controller/config/controller.yaml @@ -1,7 +1,6 @@ unitree_guide_controller: ros__parameters: type: unitree_guide_controller/UnitreeGuideController - update_rate: 200 # Hz joints: - FR_hip_joint - FR_thigh_joint diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h index 8705395..dc21bd5 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include class FSMState { public: diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h index 1bbcfdf..1e7ec6b 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h @@ -28,7 +28,7 @@ private: double start_pos_[12] = {}; rclcpp::Time start_time_; - double duration_ = 6000; // steps + double duration_ = 600; // steps double percent_ = 0; //% double phase = 0.0; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h index bd44671..e88d11f 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h @@ -29,7 +29,7 @@ private: double start_pos_[12] = {}; rclcpp::Time start_time_; - double duration_ = 6000; // steps + double duration_ = 600; // steps double percent_ = 0; //% double phase = 0.0; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h index 3c67fb3..b2b1390 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h @@ -96,6 +96,9 @@ namespace unitree_guide_controller { std::shared_ptr current_state_; std::shared_ptr next_state_; + std::chrono::time_point last_update_time_; + double update_frequency_; + std::shared_ptr getNextState(FSMStateName stateName) const; std::unordered_map< diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h similarity index 91% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h rename to controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h index 2cb7a85..a02b478 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include struct CtrlComponent { std::vector > @@ -38,7 +38,7 @@ struct CtrlComponent { QuadrupedRobot default_robot_model_; std::reference_wrapper robot_model_; - CtrlComponent() : control_inputs_(default_inputs_), default_robot_model_(), robot_model_(default_robot_model_) { + CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_) { } void clear() { diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h new file mode 100644 index 0000000..8187df6 --- /dev/null +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h @@ -0,0 +1,16 @@ +// +// Created by biao on 24-9-14. +// + +#ifndef ESTIMATOR_H +#define ESTIMATOR_H + + + +class Estimator { + +}; + + + +#endif //ESTIMATOR_H diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/Array.hh b/controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/Array.hh new file mode 100755 index 0000000..bf8b1bd --- /dev/null +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/Array.hh @@ -0,0 +1,2215 @@ +// $Id: Array.hh 249 2008-11-20 09:58:23Z schaerf $ +// This file is part of EasyLocalpp: a C++ Object-Oriented framework +// aimed at easing the development of Local Search algorithms. +// Copyright (C) 2001--2008 Andrea Schaerf, Luca Di Gaspero. +// +// This software may be modified and distributed under the terms +// of the MIT license. See the LICENSE file for details. + +#if !defined(_ARRAY_HH) +#define _ARRAY_HH + +#include +#include +#include +#include +#include +#include + +namespace quadprogpp { + +enum MType { DIAG }; + +template +class Vector { + public: + Vector(); + Vector(const unsigned int n); + Vector(const T& a, const unsigned int n); // initialize to constant value + Vector(const T* a, const unsigned int n); // Initialize to array + Vector(const Vector& rhs); // copy constructor + ~Vector(); // destructor + + inline void set(const T* a, const unsigned int n); + Vector extract(const std::set& indexes) const; + inline T& operator[](const unsigned int& i); // i-th element + inline const T& operator[](const unsigned int& i) const; + + inline unsigned int size() const; + inline void resize(const unsigned int n); + inline void resize(const T& a, const unsigned int n); + + Vector& operator=(const Vector& rhs); // assignment + Vector& operator=(const T& a); // assign a to every element + inline Vector& operator+=(const Vector& rhs); + inline Vector& operator-=(const Vector& rhs); + inline Vector& operator*=(const Vector& rhs); + inline Vector& operator/=(const Vector& rhs); + inline Vector& operator^=(const Vector& rhs); + inline Vector& operator+=(const T& a); + inline Vector& operator-=(const T& a); + inline Vector& operator*=(const T& a); + inline Vector& operator/=(const T& a); + inline Vector& operator^=(const T& a); + + private: + unsigned int n; // size of array. upper index is n-1 + T* v; // storage for data +}; + +template +Vector::Vector() : n(0), v(0) {} + +template +Vector::Vector(const unsigned int n) : v(new T[n]) { + this->n = n; +} + +template +Vector::Vector(const T& a, const unsigned int n) : v(new T[n]) { + this->n = n; + for (unsigned int i = 0; i < n; i++) v[i] = a; +} + +template +Vector::Vector(const T* a, const unsigned int n) : v(new T[n]) { + this->n = n; + for (unsigned int i = 0; i < n; i++) v[i] = *a++; +} + +template +Vector::Vector(const Vector& rhs) : v(new T[rhs.n]) { + this->n = rhs.n; + for (unsigned int i = 0; i < n; i++) v[i] = rhs[i]; +} + +template +Vector::~Vector() { + if (v != 0) delete[] (v); +} + +template +void Vector::resize(const unsigned int n) { + if (n == this->n) return; + if (v != 0) delete[] (v); + v = new T[n]; + this->n = n; +} + +template +void Vector::resize(const T& a, const unsigned int n) { + resize(n); + for (unsigned int i = 0; i < n; i++) v[i] = a; +} + +template +inline Vector& Vector::operator=(const Vector& rhs) +// postcondition: normal assignment via copying has been performed; +// if vector and rhs were different sizes, vector +// has been resized to match the size of rhs +{ + if (this != &rhs) { + resize(rhs.n); + for (unsigned int i = 0; i < n; i++) v[i] = rhs[i]; + } + return *this; +} + +template +inline Vector& Vector::operator=(const T& a) // assign a to every element +{ + for (unsigned int i = 0; i < n; i++) v[i] = a; + return *this; +} + +template +inline T& Vector::operator[](const unsigned int& i) // subscripting +{ + return v[i]; +} + +template +inline const T& Vector::operator[]( + const unsigned int& i) const // subscripting +{ + return v[i]; +} + +template +inline unsigned int Vector::size() const { + return n; +} + +template +inline void Vector::set(const T* a, unsigned int n) { + resize(n); + for (unsigned int i = 0; i < n; i++) v[i] = a[i]; +} + +template +inline Vector Vector::extract( + const std::set& indexes) const { + Vector tmp(indexes.size()); + unsigned int i = 0; + + for (std::set::const_iterator el = indexes.begin(); + el != indexes.end(); el++) { + if (*el >= n) + throw std::logic_error( + "Error extracting subvector: the indexes are out of vector bounds"); + tmp[i++] = v[*el]; + } + + return tmp; +} + +template +inline Vector& Vector::operator+=(const Vector& rhs) { + if (this->size() != rhs.size()) + throw std::logic_error("Operator+=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) v[i] += rhs[i]; + + return *this; +} + +template +inline Vector& Vector::operator+=(const T& a) { + for (unsigned int i = 0; i < n; i++) v[i] += a; + + return *this; +} + +template +inline Vector operator+(const Vector& rhs) { + return rhs; +} + +template +inline Vector operator+(const Vector& lhs, const Vector& rhs) { + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator+: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] + rhs[i]; + + return tmp; +} + +template +inline Vector operator+(const Vector& lhs, const T& a) { + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] + a; + + return tmp; +} + +template +inline Vector operator+(const T& a, const Vector& rhs) { + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = a + rhs[i]; + + return tmp; +} + +template +inline Vector& Vector::operator-=(const Vector& rhs) { + if (this->size() != rhs.size()) + throw std::logic_error("Operator-=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) v[i] -= rhs[i]; + + return *this; +} + +template +inline Vector& Vector::operator-=(const T& a) { + for (unsigned int i = 0; i < n; i++) v[i] -= a; + + return *this; +} + +template +inline Vector operator-(const Vector& rhs) { + return (T)(-1) * rhs; +} + +template +inline Vector operator-(const Vector& lhs, const Vector& rhs) { + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator-: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] - rhs[i]; + + return tmp; +} + +template +inline Vector operator-(const Vector& lhs, const T& a) { + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] - a; + + return tmp; +} + +template +inline Vector operator-(const T& a, const Vector& rhs) { + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = a - rhs[i]; + + return tmp; +} + +template +inline Vector& Vector::operator*=(const Vector& rhs) { + if (this->size() != rhs.size()) + throw std::logic_error("Operator*=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) v[i] *= rhs[i]; + + return *this; +} + +template +inline Vector& Vector::operator*=(const T& a) { + for (unsigned int i = 0; i < n; i++) v[i] *= a; + + return *this; +} + +template +inline Vector operator*(const Vector& lhs, const Vector& rhs) { + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator*: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] * rhs[i]; + + return tmp; +} + +template +inline Vector operator*(const Vector& lhs, const T& a) { + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] * a; + + return tmp; +} + +template +inline Vector operator*(const T& a, const Vector& rhs) { + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = a * rhs[i]; + + return tmp; +} + +template +inline Vector& Vector::operator/=(const Vector& rhs) { + if (this->size() != rhs.size()) + throw std::logic_error("Operator/=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) v[i] /= rhs[i]; + + return *this; +} + +template +inline Vector& Vector::operator/=(const T& a) { + for (unsigned int i = 0; i < n; i++) v[i] /= a; + + return *this; +} + +template +inline Vector operator/(const Vector& lhs, const Vector& rhs) { + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator/: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] / rhs[i]; + + return tmp; +} + +template +inline Vector operator/(const Vector& lhs, const T& a) { + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] / a; + + return tmp; +} + +template +inline Vector operator/(const T& a, const Vector& rhs) { + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = a / rhs[i]; + + return tmp; +} + +template +inline Vector operator^(const Vector& lhs, const Vector& rhs) { + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator^: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = pow(lhs[i], rhs[i]); + + return tmp; +} + +template +inline Vector operator^(const Vector& lhs, const T& a) { + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = pow(lhs[i], a); + + return tmp; +} + +template +inline Vector operator^(const T& a, const Vector& rhs) { + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = pow(a, rhs[i]); + + return tmp; +} + +template +inline Vector& Vector::operator^=(const Vector& rhs) { + if (this->size() != rhs.size()) + throw std::logic_error("Operator^=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) v[i] = pow(v[i], rhs[i]); + + return *this; +} + +template +inline Vector& Vector::operator^=(const T& a) { + for (unsigned int i = 0; i < n; i++) v[i] = pow(v[i], a); + + return *this; +} + +template +inline bool operator==(const Vector& v, const Vector& w) { + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] != w[i]) return false; + return true; +} + +template +inline bool operator!=(const Vector& v, const Vector& w) { + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] != w[i]) return true; + return false; +} + +template +inline bool operator<(const Vector& v, const Vector& w) { + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] >= w[i]) return false; + return true; +} + +template +inline bool operator<=(const Vector& v, const Vector& w) { + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] > w[i]) return false; + return true; +} + +template +inline bool operator>(const Vector& v, const Vector& w) { + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] <= w[i]) return false; + return true; +} + +template +inline bool operator>=(const Vector& v, const Vector& w) { + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] < w[i]) return false; + return true; +} + +/** + Input/Output +*/ +template +inline std::ostream& operator<<(std::ostream& os, const Vector& v) { + os << std::endl << v.size() << std::endl; + for (unsigned int i = 0; i < v.size() - 1; i++) + os << std::setw(20) << std::setprecision(16) << v[i] << ", "; + os << std::setw(20) << std::setprecision(16) << v[v.size() - 1] << std::endl; + + return os; +} + +template +std::istream& operator>>(std::istream& is, Vector& v) { + int elements; + char comma; + is >> elements; + v.resize(elements); + for (unsigned int i = 0; i < elements; i++) is >> v[i] >> comma; + + return is; +} + +/** + Index utilities +*/ + +std::set seq(unsigned int s, unsigned int e); + +std::set singleton(unsigned int i); + +template +class CanonicalBaseVector : public Vector { + public: + CanonicalBaseVector(unsigned int i, unsigned int n); + inline void reset(unsigned int i); + + private: + unsigned int e; +}; + +template +CanonicalBaseVector::CanonicalBaseVector(unsigned int i, unsigned int n) + : Vector((T)0, n), e(i) { + (*this)[e] = (T)1; +} + +template +inline void CanonicalBaseVector::reset(unsigned int i) { + (*this)[e] = (T)0; + e = i; + (*this)[e] = (T)1; +} + +#include + +template +inline T sum(const Vector& v) { + T tmp = (T)0; + for (unsigned int i = 0; i < v.size(); i++) tmp += v[i]; + + return tmp; +} + +template +inline T prod(const Vector& v) { + T tmp = (T)1; + for (unsigned int i = 0; i < v.size(); i++) tmp *= v[i]; + + return tmp; +} + +template +inline T mean(const Vector& v) { + T sum = (T)0; + for (unsigned int i = 0; i < v.size(); i++) sum += v[i]; + return sum / v.size(); +} + +template +inline T median(const Vector& v) { + Vector tmp = sort(v); + if (v.size() % 2 == 1) // it is an odd-sized vector + return tmp[v.size() / 2]; + else + return 0.5 * (tmp[v.size() / 2 - 1] + tmp[v.size() / 2]); +} + +template +inline T stdev(const Vector& v, bool sample_correction = false) { + return sqrt(var(v, sample_correction)); +} + +template +inline T var(const Vector& v, bool sample_correction = false) { + T sum = (T)0, ssum = (T)0; + unsigned int n = v.size(); + for (unsigned int i = 0; i < n; i++) { + sum += v[i]; + ssum += (v[i] * v[i]); + } + if (!sample_correction) + return (ssum / n) - (sum / n) * (sum / n); + else + return n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); +} + +template +inline T max(const Vector& v) { + T value = v[0]; + for (unsigned int i = 1; i < v.size(); i++) value = std::max(v[i], value); + + return value; +} + +template +inline T min(const Vector& v) { + T value = v[0]; + for (unsigned int i = 1; i < v.size(); i++) value = std::min(v[i], value); + + return value; +} + +template +inline unsigned int index_max(const Vector& v) { + unsigned int max = 0; + for (unsigned int i = 1; i < v.size(); i++) + if (v[i] > v[max]) max = i; + + return max; +} + +template +inline unsigned int index_min(const Vector& v) { + unsigned int min = 0; + for (unsigned int i = 1; i < v.size(); i++) + if (v[i] < v[min]) min = i; + + return min; +} + +template +inline T dot_prod(const Vector& a, const Vector& b) { + T sum = (T)0; + if (a.size() != b.size()) + throw std::logic_error("Dotprod error: the vectors are not the same size"); + for (unsigned int i = 0; i < a.size(); i++) sum += a[i] * b[i]; + + return sum; +} + +/** + Single element mathematical functions +*/ + +template +inline Vector exp(const Vector& v) { + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) tmp[i] = exp(v[i]); + + return tmp; +} + +template +inline Vector log(const Vector& v) { + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) tmp[i] = log(v[i]); + + return tmp; +} + +template +inline Vector vec_sqrt(const Vector& v) { + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) tmp[i] = sqrt(v[i]); + + return tmp; +} + +template +inline Vector pow(const Vector& v, double a) { + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) tmp[i] = pow(v[i], a); + + return tmp; +} + +template +inline Vector abs(const Vector& v) { + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) tmp[i] = (T)fabs(v[i]); + + return tmp; +} + +template +inline Vector sign(const Vector& v) { + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = v[i] > 0 ? +1 : v[i] == 0 ? 0 : -1; + + return tmp; +} + +template +inline unsigned int partition(Vector& v, unsigned int begin, + unsigned int end) { + unsigned int i = begin + 1, j = begin + 1; + T pivot = v[begin]; + while (j <= end) { + if (v[j] < pivot) { + std::swap(v[i], v[j]); + i++; + } + j++; + } + v[begin] = v[i - 1]; + v[i - 1] = pivot; + return i - 2; +} + +template +inline void quicksort(Vector& v, unsigned int begin, unsigned int end) { + if (end > begin) { + unsigned int index = partition(v, begin, end); + quicksort(v, begin, index); + quicksort(v, index + 2, end); + } +} + +template +inline Vector sort(const Vector& v) { + Vector tmp(v); + + quicksort(tmp, 0, tmp.size() - 1); + + return tmp; +} + +template +inline Vector rank(const Vector& v) { + Vector tmp(v); + Vector tmp_rank(0.0, v.size()); + + for (unsigned int i = 0; i < tmp.size(); i++) { + unsigned int smaller = 0, equal = 0; + for (unsigned int j = 0; j < tmp.size(); j++) + if (i == j) + continue; + else if (tmp[j] < tmp[i]) + smaller++; + else if (tmp[j] == tmp[i]) + equal++; + tmp_rank[i] = smaller + 1; + if (equal > 0) { + for (unsigned int j = 1; j <= equal; j++) tmp_rank[i] += smaller + 1 + j; + tmp_rank[i] /= (double)(equal + 1); + } + } + + return tmp_rank; +} + +// enum MType { DIAG }; + +template +class Matrix { + public: + Matrix(); // Default constructor + Matrix(const unsigned int n, + const unsigned int m); // Construct a n x m matrix + Matrix(const T& a, const unsigned int n, + const unsigned int m); // Initialize the content to constant a + Matrix(MType t, const T& a, const T& o, const unsigned int n, + const unsigned int m); + Matrix(MType t, const Vector& v, const T& o, const unsigned int n, + const unsigned int m); + Matrix(const T* a, const unsigned int n, + const unsigned int m); // Initialize to array + Matrix(const Matrix& rhs); // Copy constructor + ~Matrix(); // destructor + + inline T* operator[](const unsigned int& i) { + return v[i]; + } // Subscripting: row i + inline const T* operator[](const unsigned int& i) const { + return v[i]; + }; // const subsctipting + + inline void resize(const unsigned int n, const unsigned int m); + inline void resize(const T& a, const unsigned int n, const unsigned int m); + + inline Vector extractRow(const unsigned int i) const; + inline Vector extractColumn(const unsigned int j) const; + inline Vector extractDiag() const; + inline Matrix extractRows(const std::set& indexes) const; + inline Matrix extractColumns(const std::set& indexes) const; + inline Matrix extract(const std::set& r_indexes, + const std::set& c_indexes) const; + + inline void set(const T* a, unsigned int n, unsigned int m); + inline void set(const std::set& r_indexes, + const std::set& c_indexes, const Matrix& m); + inline void setRow(const unsigned int index, const Vector& v); + inline void setRow(const unsigned int index, const Matrix& v); + inline void setRows(const std::set& indexes, + const Matrix& m); + inline void setColumn(const unsigned int index, const Vector& v); + inline void setColumn(const unsigned int index, const Matrix& v); + inline void setColumns(const std::set& indexes, + const Matrix& m); + + inline unsigned int nrows() const { return n; } // number of rows + inline unsigned int ncols() const { return m; } // number of columns + + inline Matrix& operator=(const Matrix& rhs); // Assignment operator + inline Matrix& operator=(const T& a); // Assign to every element value a + inline Matrix& operator+=(const Matrix& rhs); + inline Matrix& operator-=(const Matrix& rhs); + inline Matrix& operator*=(const Matrix& rhs); + inline Matrix& operator/=(const Matrix& rhs); + inline Matrix& operator^=(const Matrix& rhs); + inline Matrix& operator+=(const T& a); + inline Matrix& operator-=(const T& a); + inline Matrix& operator*=(const T& a); + inline Matrix& operator/=(const T& a); + inline Matrix& operator^=(const T& a); + inline operator Vector(); + + private: + unsigned int n; // number of rows + unsigned int m; // number of columns + T** v; // storage for data +}; + +template +Matrix::Matrix() : n(0), m(0), v(0) {} + +template +Matrix::Matrix(unsigned int n, unsigned int m) : v(new T*[n]) { + this->n = n; + this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) v[i] = v[i - 1] + m; +} + +template +Matrix::Matrix(const T& a, unsigned int n, unsigned int m) : v(new T*[n]) { + this->n = n; + this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) v[i] = v[i - 1] + m; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] = a; +} + +template +Matrix::Matrix(const T* a, unsigned int n, unsigned int m) : v(new T*[n]) { + this->n = n; + this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) v[i] = v[i - 1] + m; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] = *a++; +} + +template +Matrix::Matrix(MType t, const T& a, const T& o, unsigned int n, + unsigned int m) + : v(new T*[n]) { + this->n = n; + this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) v[i] = v[i - 1] + m; + switch (t) { + case DIAG: + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + if (i != j) + v[i][j] = o; + else + v[i][j] = a; + break; + default: + throw std::logic_error("Matrix type not supported"); + } +} + +template +Matrix::Matrix(MType t, const Vector& a, const T& o, unsigned int n, + unsigned int m) + : v(new T*[n]) { + this->n = n; + this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) v[i] = v[i - 1] + m; + switch (t) { + case DIAG: + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + if (i != j) + v[i][j] = o; + else + v[i][j] = a[i]; + break; + default: + throw std::logic_error("Matrix type not supported"); + } +} + +template +Matrix::Matrix(const Matrix& rhs) : v(new T*[rhs.n]) { + n = rhs.n; + m = rhs.m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) v[i] = v[i - 1] + m; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] = rhs[i][j]; +} + +template +Matrix::~Matrix() { + if (v != 0) { + delete[] (v[0]); + delete[] (v); + } +} + +template +inline Matrix& Matrix::operator=(const Matrix& rhs) +// postcondition: normal assignment via copying has been performed; +// if matrix and rhs were different sizes, matrix +// has been resized to match the size of rhs +{ + if (this != &rhs) { + resize(rhs.n, rhs.m); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] = rhs[i][j]; + } + return *this; +} + +template +inline Matrix& Matrix::operator=(const T& a) // assign a to every element +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] = a; + return *this; +} + +template +inline void Matrix::resize(const unsigned int n, const unsigned int m) { + if (n == this->n && m == this->m) return; + if (v != 0) { + delete[] (v[0]); + delete[] (v); + } + this->n = n; + this->m = m; + v = new T*[n]; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) v[i] = v[i - 1] + m; +} + +template +inline void Matrix::resize(const T& a, const unsigned int n, + const unsigned int m) { + resize(n, m); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] = a; +} + +template +inline Vector Matrix::extractRow(const unsigned int i) const { + if (i >= n) + throw std::logic_error( + "Error in extractRow: trying to extract a row out of matrix bounds"); + Vector tmp(v[i], m); + + return tmp; +} + +template +inline Vector Matrix::extractColumn(const unsigned int j) const { + if (j >= m) + throw std::logic_error( + "Error in extractRow: trying to extract a row out of matrix bounds"); + Vector tmp(n); + + for (unsigned int i = 0; i < n; i++) tmp[i] = v[i][j]; + + return tmp; +} + +template +inline Vector Matrix::extractDiag() const { + unsigned int d = std::min(n, m); + + Vector tmp(d); + + for (unsigned int i = 0; i < d; i++) tmp[i] = v[i][i]; + + return tmp; +} + +template +inline Matrix Matrix::extractRows( + const std::set& indexes) const { + Matrix tmp(indexes.size(), m); + unsigned int i = 0; + + for (std::set::const_iterator el = indexes.begin(); + el != indexes.end(); el++) { + for (unsigned int j = 0; j < m; j++) { + if (*el >= n) + throw std::logic_error( + "Error extracting rows: the indexes are out of matrix bounds"); + tmp[i][j] = v[*el][j]; + } + i++; + } + + return tmp; +} + +template +inline Matrix Matrix::extractColumns( + const std::set& indexes) const { + Matrix tmp(n, indexes.size()); + unsigned int j = 0; + + for (std::set::const_iterator el = indexes.begin(); + el != indexes.end(); el++) { + for (unsigned int i = 0; i < n; i++) { + if (*el >= m) + throw std::logic_error( + "Error extracting columns: the indexes are out of matrix bounds"); + tmp[i][j] = v[i][*el]; + } + j++; + } + + return tmp; +} + +template +inline Matrix Matrix::extract( + const std::set& r_indexes, + const std::set& c_indexes) const { + Matrix tmp(r_indexes.size(), c_indexes.size()); + unsigned int i = 0, j; + + for (std::set::const_iterator r_el = r_indexes.begin(); + r_el != r_indexes.end(); r_el++) { + if (*r_el >= n) + throw std::logic_error( + "Error extracting submatrix: the indexes are out of matrix bounds"); + j = 0; + for (std::set::const_iterator c_el = c_indexes.begin(); + c_el != c_indexes.end(); c_el++) { + if (*c_el >= m) + throw std::logic_error( + "Error extracting rows: the indexes are out of matrix bounds"); + tmp[i][j] = v[*r_el][*c_el]; + j++; + } + i++; + } + + return tmp; +} + +template +inline void Matrix::setRow(unsigned int i, const Vector& a) { + if (i >= n) + throw std::logic_error( + "Error in setRow: trying to set a row out of matrix bounds"); + if (this->m != a.size()) + throw std::logic_error( + "Error setting matrix row: ranges are not compatible"); + for (unsigned int j = 0; j < ncols(); j++) v[i][j] = a[j]; +} + +template +inline void Matrix::setRow(unsigned int i, const Matrix& a) { + if (i >= n) + throw std::logic_error( + "Error in setRow: trying to set a row out of matrix bounds"); + if (this->m != a.ncols()) + throw std::logic_error( + "Error setting matrix column: ranges are not compatible"); + if (a.nrows() != 1) + throw std::logic_error("Error setting matrix column with a non-row matrix"); + for (unsigned int j = 0; j < ncols(); j++) v[i][j] = a[0][j]; +} + +template +inline void Matrix::setRows(const std::set& indexes, + const Matrix& m) { + unsigned int i = 0; + + if (indexes.size() != m.nrows() || this->m != m.ncols()) + throw std::logic_error( + "Error setting matrix rows: ranges are not compatible"); + for (std::set::const_iterator el = indexes.begin(); + el != indexes.end(); el++) { + for (unsigned int j = 0; j < ncols(); j++) { + if (*el >= n) + throw std::logic_error( + "Error in setRows: trying to set a row out of matrix bounds"); + v[*el][j] = m[i][j]; + } + i++; + } +} + +template +inline void Matrix::setColumn(unsigned int j, const Vector& a) { + if (j >= m) + throw std::logic_error( + "Error in setColumn: trying to set a column out of matrix bounds"); + if (this->n != a.size()) + throw std::logic_error( + "Error setting matrix column: ranges are not compatible"); + for (unsigned int i = 0; i < nrows(); i++) v[i][j] = a[i]; +} + +template +inline void Matrix::setColumn(unsigned int j, const Matrix& a) { + if (j >= m) + throw std::logic_error( + "Error in setColumn: trying to set a column out of matrix bounds"); + if (this->n != a.nrows()) + throw std::logic_error( + "Error setting matrix column: ranges are not compatible"); + if (a.ncols() != 1) + throw std::logic_error( + "Error setting matrix column with a non-column matrix"); + for (unsigned int i = 0; i < nrows(); i++) v[i][j] = a[i][0]; +} + +template +inline void Matrix::setColumns(const std::set& indexes, + const Matrix& a) { + unsigned int j = 0; + + if (indexes.size() != a.ncols() || this->n != a.nrows()) + throw std::logic_error( + "Error setting matrix columns: ranges are not compatible"); + for (std::set::const_iterator el = indexes.begin(); + el != indexes.end(); el++) { + for (unsigned int i = 0; i < nrows(); i++) { + if (*el >= m) + throw std::logic_error( + "Error in setColumns: trying to set a column out of matrix bounds"); + v[i][*el] = a[i][j]; + } + j++; + } +} + +template +inline void Matrix::set(const std::set& r_indexes, + const std::set& c_indexes, + const Matrix& a) { + unsigned int i = 0, j; + if (c_indexes.size() != a.ncols() || r_indexes.size() != a.nrows()) + throw std::logic_error( + "Error setting matrix elements: ranges are not compatible"); + + for (std::set::const_iterator r_el = r_indexes.begin(); + r_el != r_indexes.end(); r_el++) { + if (*r_el >= n) + throw std::logic_error( + "Error in set: trying to set a row out of matrix bounds"); + j = 0; + for (std::set::const_iterator c_el = c_indexes.begin(); + c_el != c_indexes.end(); c_el++) { + if (*c_el >= m) + throw std::logic_error( + "Error in set: trying to set a column out of matrix bounds"); + v[*r_el][*c_el] = a[i][j]; + j++; + } + i++; + } +} + +template +inline void Matrix::set(const T* a, unsigned int n, unsigned int m) { + if (this->n != n || this->m != m) resize(n, m); + unsigned int k = 0; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] = a[k++]; +} + +template +Matrix operator+(const Matrix& rhs) { + return rhs; +} + +template +Matrix operator+(const Matrix& lhs, const Matrix& rhs) { + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator+: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] + rhs[i][j]; + + return tmp; +} + +template +Matrix operator+(const Matrix& lhs, const T& a) { + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] + a; + + return tmp; +} + +template +Matrix operator+(const T& a, const Matrix& rhs) { + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) tmp[i][j] = a + rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator+=(const Matrix& rhs) { + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator+=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] += rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator+=(const T& a) { + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] += a; + + return *this; +} + +template +Matrix operator-(const Matrix& rhs) { + return (T)(-1) * rhs; +} + +template +Matrix operator-(const Matrix& lhs, const Matrix& rhs) { + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator-: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] - rhs[i][j]; + + return tmp; +} + +template +Matrix operator-(const Matrix& lhs, const T& a) { + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] - a; + + return tmp; +} + +template +Matrix operator-(const T& a, const Matrix& rhs) { + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) tmp[i][j] = a - rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator-=(const Matrix& rhs) { + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator-=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] -= rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator-=(const T& a) { + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] -= a; + + return *this; +} + +template +Matrix operator*(const Matrix& lhs, const Matrix& rhs) { + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator*: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] * rhs[i][j]; + + return tmp; +} + +template +Matrix operator*(const Matrix& lhs, const T& a) { + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] * a; + + return tmp; +} + +template +Matrix operator*(const T& a, const Matrix& rhs) { + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) tmp[i][j] = a * rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator*=(const Matrix& rhs) { + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator*=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] *= rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator*=(const T& a) { + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] *= a; + + return *this; +} + +template +Matrix operator/(const Matrix& lhs, const Matrix& rhs) { + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator+: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] / rhs[i][j]; + + return tmp; +} + +template +Matrix operator/(const Matrix& lhs, const T& a) { + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] / a; + + return tmp; +} + +template +Matrix operator/(const T& a, const Matrix& rhs) { + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) tmp[i][j] = a / rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator/=(const Matrix& rhs) { + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator+=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] /= rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator/=(const T& a) { + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] /= a; + + return *this; +} + +template +Matrix operator^(const Matrix& lhs, const T& a) { + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = pow(lhs[i][j], a); + + return tmp; +} + +template +inline Matrix& Matrix::operator^=(const Matrix& rhs) { + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator^=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] = pow(v[i][j], rhs[i][j]); + + return *this; +} + +template +inline Matrix& Matrix::operator^=(const T& a) { + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) v[i][j] = pow(v[i][j], a); + + return *this; +} + +template +inline Matrix::operator Vector() { + if (n > 1 && m > 1) + throw std::logic_error( + "Error matrix cast to vector: trying to cast a multi-dimensional " + "matrix"); + if (n == 1) + return extractRow(0); + else + return extractColumn(0); +} + +template +inline bool operator==(const Matrix& a, const Matrix& b) { + if (a.nrows() != b.nrows() || a.ncols() != b.ncols()) + throw std::logic_error("Matrices of different size are not confrontable"); + for (unsigned i = 0; i < a.nrows(); i++) + for (unsigned j = 0; j < a.ncols(); j++) + if (a[i][j] != b[i][j]) return false; + return true; +} + +template +inline bool operator!=(const Matrix& a, const Matrix& b) { + if (a.nrows() != b.nrows() || a.ncols() != b.ncols()) + throw std::logic_error("Matrices of different size are not confrontable"); + for (unsigned i = 0; i < a.nrows(); i++) + for (unsigned j = 0; j < a.ncols(); j++) + if (a[i][j] != b[i][j]) return true; + return false; +} + +/** + Input/Output +*/ +template +std::ostream& operator<<(std::ostream& os, const Matrix& m) { + os << std::endl << m.nrows() << " " << m.ncols() << std::endl; + for (unsigned int i = 0; i < m.nrows(); i++) { + for (unsigned int j = 0; j < m.ncols() - 1; j++) + os << std::setw(20) << std::setprecision(16) << m[i][j] << ", "; + os << std::setw(20) << std::setprecision(16) << m[i][m.ncols() - 1] + << std::endl; + } + + return os; +} + +template +std::istream& operator>>(std::istream& is, Matrix& m) { + int rows, cols; + char comma; + is >> rows >> cols; + m.resize(rows, cols); + for (unsigned int i = 0; i < rows; i++) + for (unsigned int j = 0; j < cols; j++) is >> m[i][j] >> comma; + + return is; +} + +template +T sign(const T& v) { + if (v >= (T)0.0) + return (T)1.0; + else + return (T)-1.0; +} + +template +T dist(const T& a, const T& b) { + T abs_a = (T)fabs(a), abs_b = (T)fabs(b); + if (abs_a > abs_b) + return abs_a * sqrt((T)1.0 + (abs_b / abs_a) * (abs_b / abs_a)); + else + return (abs_b == (T)0.0 + ? (T)0.0 + : abs_b * sqrt((T)1.0 + (abs_a / abs_b) * (abs_a / abs_b))); +} + +template +void svd(const Matrix& A, Matrix& U, Vector& W, Matrix& V) { + int m = A.nrows(), n = A.ncols(), i, j, k, l, jj, nm; + const unsigned int max_its = 30; + bool flag; + Vector rv1(n); + U = A; + W.resize(n); + V.resize(n, n); + T anorm, c, f, g, h, s, scale, x, y, z; + g = scale = anorm = (T)0.0; + + // Householder reduction to bidiagonal form + for (i = 0; i < n; i++) { + l = i + 1; + rv1[i] = scale * g; + g = s = scale = (T)0.0; + if (i < m) { + for (k = i; k < m; k++) scale += fabs(U[k][i]); + if (scale != (T)0.0) { + for (k = i; k < m; k++) { + U[k][i] /= scale; + s += U[k][i] * U[k][i]; + } + f = U[i][i]; + g = -sign(f) * sqrt(s); + h = f * g - s; + U[i][i] = f - g; + for (j = l; j < n; j++) { + s = (T)0.0; + for (k = i; k < m; k++) s += U[k][i] * U[k][j]; + f = s / h; + for (k = i; k < m; k++) U[k][j] += f * U[k][i]; + } + for (k = i; k < m; k++) U[k][i] *= scale; + } + } + W[i] = scale * g; + g = s = scale = (T)0.0; + if (i < m && i != n - 1) { + for (k = l; k < n; k++) scale += fabs(U[i][k]); + if (scale != (T)0.0) { + for (k = l; k < n; k++) { + U[i][k] /= scale; + s += U[i][k] * U[i][k]; + } + f = U[i][l]; + g = -sign(f) * sqrt(s); + h = f * g - s; + U[i][l] = f - g; + for (k = l; k < n; k++) rv1[k] = U[i][k] / h; + for (j = l; j < m; j++) { + s = (T)0.0; + for (k = l; k < n; k++) s += U[j][k] * U[i][k]; + for (k = l; k < n; k++) U[j][k] += s * rv1[k]; + } + for (k = l; k < n; k++) U[i][k] *= scale; + } + } + anorm = std::max(anorm, fabs(W[i]) + fabs(rv1[i])); + } + // Accumulation of right-hand transformations + for (i = n - 1; i >= 0; i--) { + if (i < n - 1) { + if (g != (T)0.0) { + for (j = l; j < n; j++) V[j][i] = (U[i][j] / U[i][l]) / g; + for (j = l; j < n; j++) { + s = (T)0.0; + for (k = l; k < n; k++) s += U[i][k] * V[k][j]; + for (k = l; k < n; k++) V[k][j] += s * V[k][i]; + } + } + for (j = l; j < n; j++) V[i][j] = V[j][i] = (T)0.0; + } + V[i][i] = (T)1.0; + g = rv1[i]; + l = i; + } + // Accumulation of left-hand transformations + for (i = std::min(m, n) - 1; i >= 0; i--) { + l = i + 1; + g = W[i]; + for (j = l; j < n; j++) U[i][j] = (T)0.0; + if (g != (T)0.0) { + g = (T)1.0 / g; + for (j = l; j < n; j++) { + s = (T)0.0; + for (k = l; k < m; k++) s += U[k][i] * U[k][j]; + f = (s / U[i][i]) * g; + for (k = i; k < m; k++) U[k][j] += f * U[k][i]; + } + for (j = i; j < m; j++) U[j][i] *= g; + } else + for (j = i; j < m; j++) U[j][i] = (T)0.0; + U[i][i]++; + } + // Diagonalization of the bidiagonal form: loop over singular values, and over + // allowed iterations. + for (k = n - 1; k >= 0; k--) { + for (unsigned int its = 0; its < max_its; its++) { + flag = true; + for (l = k; l >= 0; l--) // FIXME: in NR it was l >= 1 but there + // subscripts start from one + { // Test for splitting + nm = l - 1; // Note that rV[0] is always zero + if ((T)(fabs(rv1[l]) + anorm) == anorm) { + flag = false; + break; + } + if ((T)(fabs(W[nm]) + anorm) == anorm) break; + } + if (flag) { + // Cancellation of rv1[l], if l > 0 FIXME: it was l > 1 in NR + c = (T)0.0; + s = (T)1.0; + for (i = l; i <= k; i++) { + f = s * rv1[i]; + rv1[i] *= c; + if ((T)(fabs(f) + anorm) == anorm) break; + g = W[i]; + h = dist(f, g); + W[i] = h; + h = (T)1.0 / h; + c = g * h; + s = -f * h; + for (j = 0; j < m; j++) { + y = U[j][nm]; + z = U[j][i]; + U[j][nm] = y * c + z * s; + U[j][i] = z * c - y * s; + } + } + } + z = W[k]; + if (l == k) { // Convergence + if (z < (T)0.0) { // Singular value is made nonnegative + W[k] = -z; + for (j = 0; j < n; j++) V[j][k] = -V[j][k]; + } + break; + } + if (its == max_its) + throw std::logic_error( + "Error svd: no convergence in the maximum number of iterations"); + x = W[l]; + nm = k - 1; + y = W[nm]; + g = rv1[nm]; + h = rv1[k]; + f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y); + g = dist(f, (T)1.0); + f = ((x - z) * (x + z) + h * ((y / (f + sign(f) * fabs(g))) - h)) / x; + c = s = (T)1.0; // Next QR transformation + for (j = l; j <= nm; j++) { + i = j + 1; + g = rv1[i]; + y = W[i]; + h = s * g; + g *= c; + z = dist(f, h); + rv1[j] = z; + c = f / z; + s = h / z; + f = x * c + g * s; + g = g * c - x * s; + h = y * s; + y *= c; + for (jj = 0; jj < n; jj++) { + x = V[jj][j]; + z = V[jj][i]; + V[jj][j] = x * c + z * s; + V[jj][i] = z * c - x * s; + } + z = dist(f, h); + W[j] = z; + if (z != 0) // Rotation can be arbitrary if z = 0 + { + z = (T)1.0 / z; + c = f * z; + s = h * z; + } + f = c * g + s * y; + x = c * y - s * g; + for (jj = 0; jj < m; jj++) { + y = U[jj][j]; + z = U[jj][i]; + U[jj][j] = y * c + z * s; + U[jj][i] = z * c - y * s; + } + } + rv1[l] = (T)0.0; + rv1[k] = f; + W[k] = x; + } + } +} + +template +Matrix pinv(const Matrix& A) { + Matrix U, V, x, tmp(A.ncols(), A.nrows()); + Vector W; + CanonicalBaseVector e(0, A.nrows()); + svd(A, U, W, V); + for (unsigned int i = 0; i < A.nrows(); i++) { + e.reset(i); + tmp.setColumn( + i, dot_prod(dot_prod(dot_prod(V, Matrix(DIAG, 1.0 / W, 0.0, + W.size(), W.size())), + t(U)), + e)); + } + + return tmp; +} + +template +int lu(const Matrix& A, Matrix& LU, Vector& index) { + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in LU decomposition: matrix must be squared"); + int i, p, j, k, n = A.ncols(), ex; + T val, tmp; + Vector d(n); + LU = A; + index.resize(n); + + ex = 1; + for (i = 0; i < n; i++) { + index[i] = i; + val = (T)0.0; + for (j = 0; j < n; j++) val = std::max(val, (T)fabs(LU[i][j])); + if (val == (T)0.0) + std::logic_error("Error in LU decomposition: matrix was singular"); + d[i] = val; + } + + for (k = 0; k < n - 1; k++) { + p = k; + val = fabs(LU[k][k]) / d[k]; + for (i = k + 1; i < n; i++) { + tmp = fabs(LU[i][k]) / d[i]; + if (tmp > val) { + val = tmp; + p = i; + } + } + if (val == (T)0.0) + std::logic_error("Error in LU decomposition: matrix was singular"); + if (p > k) { + ex = -ex; + std::swap(index[k], index[p]); + std::swap(d[k], d[p]); + for (j = 0; j < n; j++) std::swap(LU[k][j], LU[p][j]); + } + + for (i = k + 1; i < n; i++) { + LU[i][k] /= LU[k][k]; + for (j = k + 1; j < n; j++) LU[i][j] -= LU[i][k] * LU[k][j]; + } + } + if (LU[n - 1][n - 1] == (T)0.0) + std::logic_error("Error in LU decomposition: matrix was singular"); + + return ex; +} + +template +Vector lu_solve(const Matrix& LU, const Vector& b, + Vector& index) { + if (LU.ncols() != LU.nrows()) + throw std::logic_error("Error in LU solve: LU matrix should be squared"); + unsigned int n = LU.ncols(); + if (b.size() != n) + throw std::logic_error( + "Error in LU solve: b vector must be of the same dimensions of LU " + "matrix"); + Vector x((T)0.0, n); + int i, j, p; + T sum; + + p = index[0]; + x[0] = b[p]; + + for (i = 1; i < n; i++) { + sum = (T)0.0; + for (j = 0; j < i; j++) sum += LU[i][j] * x[j]; + p = index[i]; + x[i] = b[p] - sum; + } + x[n - 1] /= LU[n - 1][n - 1]; + for (i = n - 2; i >= 0; i--) { + sum = (T)0.0; + for (j = i + 1; j < n; j++) sum += LU[i][j] * x[j]; + x[i] = (x[i] - sum) / LU[i][i]; + } + return x; +} + +template +void lu_solve(const Matrix& LU, Vector& x, const Vector& b, + Vector& index) { + x = lu_solve(LU, b, index); +} + +template +Matrix lu_inverse(const Matrix& A) { + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in LU invert: matrix must be squared"); + unsigned int n = A.ncols(); + Matrix A1(n, n), LU; + Vector index; + + lu(A, LU, index); + CanonicalBaseVector e(0, n); + for (unsigned i = 0; i < n; i++) { + e.reset(i); + A1.setColumn(i, lu_solve(LU, e, index)); + } + + return A1; +} + +template +T lu_det(const Matrix& A) { + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in LU determinant: matrix must be squared"); + unsigned int d; + Matrix LU; + Vector index; + + d = lu(A, LU, index); + + return d * prod(LU.extractDiag()); +} + +template +void cholesky(const Matrix A, Matrix& LL) { + if (A.ncols() != A.nrows()) + throw std::logic_error( + "Error in Cholesky decomposition: matrix must be squared"); + int n = A.ncols(); + double sum; + LL = A; + + for (unsigned int i = 0; i < n; i++) { + for (unsigned int j = i; j < n; j++) { + sum = LL[i][j]; + for (int k = i - 1; k >= 0; k--) sum -= LL[i][k] * LL[j][k]; + if (i == j) { + if (sum <= 0.0) + throw std::logic_error( + "Error in Cholesky decomposition: matrix is not postive " + "definite"); + LL[i][i] = sqrt(sum); + } else + LL[j][i] = sum / LL[i][i]; + } + for (unsigned int k = i + 1; k < n; k++) LL[i][k] = LL[k][i]; + } +} + +template +Matrix cholesky(const Matrix A) { + Matrix LL; + cholesky(A, LL); + + return LL; +} + +template +Vector cholesky_solve(const Matrix& LL, const Vector& b) { + if (LL.ncols() != LL.nrows()) + throw std::logic_error("Error in Cholesky solve: matrix must be squared"); + unsigned int n = LL.ncols(); + if (b.size() != n) + throw std::logic_error( + "Error in Cholesky decomposition: b vector must be of the same " + "dimensions of LU matrix"); + Vector x, y; + + /* Solve L * y = b */ + forward_elimination(LL, y, b); + /* Solve L^T * x = y */ + backward_elimination(LL, x, y); + + return x; +} + +template +void cholesky_solve(const Matrix& LL, Vector& x, const Vector& b) { + x = cholesky_solve(LL, b); +} + +template +void forward_elimination(const Matrix& L, Vector& y, const Vector b) { + if (L.ncols() != L.nrows()) + throw std::logic_error( + "Error in Forward elimination: matrix must be squared (lower " + "triangular)"); + if (b.size() != L.nrows()) + throw std::logic_error( + "Error in Forward elimination: b vector must be of the same dimensions " + "of L matrix"); + unsigned int n = b.size(); + y.resize(n); + + y[0] = b[0] / L[0][0]; + for (unsigned int i = 1; i < n; i++) { + y[i] = b[i]; + for (unsigned int j = 0; j < i; j++) y[i] -= L[i][j] * y[j]; + y[i] = y[i] / L[i][i]; + } +} + +template +Vector forward_elimination(const Matrix& L, const Vector b) { + Vector y; + forward_elimination(L, y, b); + + return y; +} + +template +void backward_elimination(const Matrix& U, Vector& x, + const Vector& y) { + if (U.ncols() != U.nrows()) + throw std::logic_error( + "Error in Backward elimination: matrix must be squared (upper " + "triangular)"); + if (y.size() != U.nrows()) + throw std::logic_error( + "Error in Backward elimination: b vector must be of the same " + "dimensions of U matrix"); + int n = y.size(); + x.resize(n); + + x[n - 1] = y[n - 1] / U[n - 1][n - 1]; + for (int i = n - 2; i >= 0; i--) { + x[i] = y[i]; + for (int j = i + 1; j < n; j++) x[i] -= U[i][j] * x[j]; + x[i] = x[i] / U[i][i]; + } +} + +template +Vector backward_elimination(const Matrix& U, const Vector y) { + Vector x; + forward_elimination(U, x, y); + + return x; +} + +/* Setting default linear systems machinery */ + +#define det lu_det +#define inverse lu_inverse +#define solve lu_solve + +/* Random */ + +template +void random(Matrix& m) { + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + m[i][j] = (T)(rand() / double(RAND_MAX)); +} + +/** + Aggregate functions +*/ + +template +Vector sum(const Matrix& m) { + Vector tmp((T)0, m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + for (unsigned int i = 0; i < m.nrows(); i++) tmp[j] += m[i][j]; + return tmp; +} + +template +Vector r_sum(const Matrix& m) { + Vector tmp((T)0, m.nrows()); + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) tmp[i] += m[i][j]; + return tmp; +} + +template +T all_sum(const Matrix& m) { + T tmp = (T)0; + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) tmp += m[i][j]; + return tmp; +} + +template +Vector prod(const Matrix& m) { + Vector tmp((T)1, m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + for (unsigned int i = 0; i < m.nrows(); i++) tmp[j] *= m[i][j]; + return tmp; +} + +template +Vector r_prod(const Matrix& m) { + Vector tmp((T)1, m.nrows()); + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) tmp[i] *= m[i][j]; + return tmp; +} + +template +T all_prod(const Matrix& m) { + T tmp = (T)1; + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) tmp *= m[i][j]; + return tmp; +} + +template +Vector mean(const Matrix& m) { + Vector res((T)0, m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) { + for (unsigned int i = 0; i < m.nrows(); i++) res[j] += m[i][j]; + res[j] /= m.nrows(); + } + + return res; +} + +template +Vector r_mean(const Matrix& m) { + Vector res((T)0, m.rows()); + for (unsigned int i = 0; i < m.nrows(); i++) { + for (unsigned int j = 0; j < m.ncols(); j++) res[i] += m[i][j]; + res[i] /= m.nrows(); + } + + return res; +} + +template +T all_mean(const Matrix& m) { + T tmp = (T)0; + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) tmp += m[i][j]; + return tmp / (m.nrows() * m.ncols()); +} + +template +Vector var(const Matrix& m, bool sample_correction = false) { + Vector res((T)0, m.ncols()); + unsigned int n = m.nrows(); + double sum, ssum; + for (unsigned int j = 0; j < m.ncols(); j++) { + sum = (T)0.0; + ssum = (T)0.0; + for (unsigned int i = 0; i < m.nrows(); i++) { + sum += m[i][j]; + ssum += (m[i][j] * m[i][j]); + } + if (!sample_correction) + res[j] = (ssum / n) - (sum / n) * (sum / n); + else + res[j] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); + } + + return res; +} + +template +Vector stdev(const Matrix& m, bool sample_correction = false) { + return vec_sqrt(var(m, sample_correction)); +} + +template +Vector r_var(const Matrix& m, bool sample_correction = false) { + Vector res((T)0, m.nrows()); + double sum, ssum; + unsigned int n = m.ncols(); + for (unsigned int i = 0; i < m.nrows(); i++) { + sum = 0.0; + ssum = 0.0; + for (unsigned int j = 0; j < m.ncols(); j++) { + sum += m[i][j]; + ssum += (m[i][j] * m[i][j]); + } + if (!sample_correction) + res[i] = (ssum / n) - (sum / n) * (sum / n); + else + res[i] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); + } + + return res; +} + +template +Vector r_stdev(const Matrix& m, bool sample_correction = false) { + return vec_sqrt(r_var(m, sample_correction)); +} + +template +Vector max(const Matrix& m) { + Vector res(m.ncols()); + double value; + for (unsigned int j = 0; j < m.ncols(); j++) { + value = m[0][j]; + for (unsigned int i = 1; i < m.nrows(); i++) + value = std::max(m[i][j], value); + res[j] = value; + } + + return res; +} + +template +Vector r_max(const Matrix& m) { + Vector res(m.nrows()); + double value; + for (unsigned int i = 0; i < m.nrows(); i++) { + value = m[i][0]; + for (unsigned int j = 1; j < m.ncols(); j++) + value = std::max(m[i][j], value); + res[i] = value; + } + + return res; +} + +template +Vector min(const Matrix& m) { + Vector res(m.ncols()); + double value; + for (unsigned int j = 0; j < m.ncols(); j++) { + value = m[0][j]; + for (unsigned int i = 1; i < m.nrows(); i++) + value = std::min(m[i][j], value); + res[j] = value; + } + + return res; +} + +template +Vector r_min(const Matrix& m) { + Vector res(m.nrows()); + double value; + for (unsigned int i = 0; i < m.nrows(); i++) { + value = m[i][0]; + for (unsigned int j = 1; j < m.ncols(); j++) + value = std::min(m[i][j], value); + res[i] = value; + } + + return res; +} + +/** + Single element mathematical functions +*/ + +template +Matrix exp(const Matrix& m) { + Matrix tmp(m.nrows(), m.ncols()); + + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) tmp[i][j] = exp(m[i][j]); + + return tmp; +} + +template +Matrix mat_sqrt(const Matrix& m) { + Matrix tmp(m.nrows(), m.ncols()); + + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) tmp[i][j] = sqrt(m[i][j]); + + return tmp; +} + +/** + Matrix operators +*/ + +template +Matrix kron(const Vector& b, const Vector& a) { + Matrix tmp(b.size(), a.size()); + for (unsigned int i = 0; i < b.size(); i++) + for (unsigned int j = 0; j < a.size(); j++) tmp[i][j] = a[j] * b[i]; + + return tmp; +} + +template +Matrix t(const Matrix& a) { + Matrix tmp(a.ncols(), a.nrows()); + for (unsigned int i = 0; i < a.nrows(); i++) + for (unsigned int j = 0; j < a.ncols(); j++) tmp[j][i] = a[i][j]; + + return tmp; +} + +template +Matrix dot_prod(const Matrix& a, const Matrix& b) { + if (a.ncols() != b.nrows()) + throw std::logic_error( + "Error matrix dot product: dimensions of the matrices are not " + "compatible"); + Matrix tmp(a.nrows(), b.ncols()); + for (unsigned int i = 0; i < tmp.nrows(); i++) + for (unsigned int j = 0; j < tmp.ncols(); j++) { + tmp[i][j] = (T)0; + for (unsigned int k = 0; k < a.ncols(); k++) + tmp[i][j] += a[i][k] * b[k][j]; + } + + return tmp; +} + +template +Matrix dot_prod(const Matrix& a, const Vector& b) { + if (a.ncols() != b.size()) + throw std::logic_error( + "Error matrix dot product: dimensions of the matrix and the vector are " + "not compatible"); + Matrix tmp(a.nrows(), 1); + for (unsigned int i = 0; i < tmp.nrows(); i++) { + tmp[i][0] = (T)0; + for (unsigned int k = 0; k < a.ncols(); k++) tmp[i][0] += a[i][k] * b[k]; + } + + return tmp; +} + +template +Matrix dot_prod(const Vector& a, const Matrix& b) { + if (a.size() != b.nrows()) + throw std::logic_error( + "Error matrix dot product: dimensions of the vector and matrix are not " + "compatible"); + Matrix tmp(1, b.ncols()); + for (unsigned int j = 0; j < tmp.ncols(); j++) { + tmp[0][j] = (T)0; + for (unsigned int k = 0; k < a.size(); k++) tmp[0][j] += a[k] * b[k][j]; + } + + return tmp; +} + +template +inline Matrix rank(const Matrix m) { + Matrix tmp(m.nrows(), m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + tmp.setColumn(j, rank(m.extractColumn(j))); + + return tmp; +} + +template +inline Matrix r_rank(const Matrix m) { + Matrix tmp(m.nrows(), m.ncols()); + for (unsigned int i = 0; i < m.nrows(); i++) + tmp.setRow(i, rank(m.extractRow(i))); + + return tmp; +} + +} // namespace quadprogpp + +#endif // define _ARRAY_HH_ diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/QuadProg++.hh b/controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/QuadProg++.hh new file mode 100755 index 0000000..b0bdc90 --- /dev/null +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/QuadProg++.hh @@ -0,0 +1,78 @@ +/* + File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $ + + The quadprog_solve() function implements the algorithm of Goldfarb and Idnani + for the solution of a (convex) Quadratic Programming problem + by means of an active-set dual method. + +The problem is in the form: + +min 0.5 * x G x + g0 x +s.t. + CE^T x + ce0 = 0 + CI^T x + ci0 >= 0 + + The matrix and vectors dimensions are as follows: + G: n * n + g0: n + + CE: n * p + ce0: p + + CI: n * m + ci0: m + + x: n + + The function will return the cost of the solution written in the x vector or + std::numeric_limits::infinity() if the problem is infeasible. In the latter +case the value of the x vector is not correct. + + References: D. Goldfarb, A. Idnani. A numerically stable dual method for +solving strictly convex quadratic programs. Mathematical Programming 27 (1983) +pp. 1-33. + + Notes: + 1. pay attention in setting up the vectors ce0 and ci0. + If the constraints of your problem are specified in the form + A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d. + 2. The matrices have column dimension equal to MATRIX_DIM, + a constant set to 20 in this file (by means of a #define macro). + If the matrices are bigger than 20 x 20 the limit could be + increased by means of a -DMATRIX_DIM=n on the compiler command +line. + 3. The matrix G is modified within the function since it is used to compute + the G = L^T L cholesky factorization for further computations inside the +function. If you need the original matrix G you should make a copy of it and +pass the copy to the function. + + Author: Luca Di Gaspero + DIEGM - University of Udine, Italy + luca.digaspero@uniud.it + http://www.diegm.uniud.it/digaspero/ + + The author will be grateful if the researchers using this software will + acknowledge the contribution of this function in their research papers. + + Copyright (c) 2007-2016 Luca Di Gaspero + + This software may be modified and distributed under the terms + of the MIT license. See the LICENSE file for details. +*/ + +#ifndef _QUADPROGPP +#define _QUADPROGPP + +#include "Array.hh" +#include + +namespace quadprogpp { + +double solve_quadprog(Matrix& G, Vector& g0, + const Matrix& CE, const Vector& ce0, + const Matrix& CI, const Vector& ci0, + Vector& x); + +} // namespace quadprogpp + +#endif // #define _QUADPROGPP diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/QuadrupedRobot.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h similarity index 100% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/QuadrupedRobot.h rename to controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/RobotLeg.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h similarity index 100% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/RobotLeg.h rename to controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp index 3b93c07..83aa425 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp @@ -2,12 +2,13 @@ // Created by tlab-uav on 24-9-11. // +#include "unitree_guide_controller/FSM/StateFixedDown.h" + #include -#include StateFixedDown::StateFixedDown(CtrlComponent ctrlComp): FSMState( FSMStateName::FIXEDDOWN, "fixed down", std::move(ctrlComp)) { - // duration_ = ctrlComp_.frequency_ * 15; + duration_ = ctrlComp_.frequency_ * 1.2; } void StateFixedDown::enter() { @@ -34,7 +35,7 @@ void StateFixedDown::exit() { } FSMStateName StateFixedDown::checkChange() { - if (percent_ < 2) { + if (percent_ < 1.5) { return FSMStateName::FIXEDDOWN; } switch (ctrlComp_.control_inputs_.get().command) { diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp index eb7ee9b..525a084 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp @@ -2,12 +2,13 @@ // Created by biao on 24-9-10. // +#include "unitree_guide_controller/FSM/StateFixedStand.h" + #include -#include -#include StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState( FSMStateName::FIXEDSTAND, "fixed stand", std::move(ctrlComp)) { + duration_ = ctrlComp_.frequency_ * 1.2; } void StateFixedStand::enter() { @@ -35,7 +36,7 @@ void StateFixedStand::exit() { } FSMStateName StateFixedStand::checkChange() { - if (percent_ < 2) { + if (percent_ < 1.5) { return FSMStateName::FIXEDSTAND; } switch (ctrlComp_.control_inputs_.get().command) { diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp index d5f3687..9b65dc7 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp @@ -2,8 +2,8 @@ // Created by tlab-uav on 24-9-13. // -#include -#include +#include "unitree_guide_controller/FSM/StateFreeStand.h" +#include "unitree_guide_controller/common/mathTools.h" StateFreeStand::StateFreeStand(CtrlComponent ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand", std::move(ctrl_component)) { diff --git a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp index 38ef5df..20dd210 100644 --- a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp @@ -2,9 +2,9 @@ // Created by tlab-uav on 24-9-6. // -#include -#include +#include "unitree_guide_controller/FSM/StatePassive.h" +#include #include StatePassive::StatePassive(CtrlComponent ctrlComp) : FSMState( diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp index dc8a70a..1623595 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp @@ -3,8 +3,8 @@ // #include -#include -#include +#include "unitree_guide_controller/FSM/StateSwingTest.h" +#include "unitree_guide_controller/common/mathTools.h" StateSwingTest::StateSwingTest(CtrlComponent ctrlComp): FSMState( FSMStateName::SWINGTEST, "swing test", std::move(ctrlComp)) { diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index f9fe0f9..6d57fbe 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -2,9 +2,9 @@ // Created by tlab-uav on 24-9-6. // -#include -#include -#include +#include "unitree_guide_controller/UnitreeGuideController.h" +#include "unitree_guide_controller/FSM/StatePassive.h" +#include "unitree_guide_controller/robot/QuadrupedRobot.h" namespace unitree_guide_controller { using config_type = controller_interface::interface_configuration_type; diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp new file mode 100644 index 0000000..c8b1bdc --- /dev/null +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -0,0 +1,5 @@ +// +// Created by biao on 24-9-14. +// + +#include "unitree_guide_controller/control/Estimator.h" diff --git a/controllers/unitree_guide_controller/src/quadProgpp/Array.cc b/controllers/unitree_guide_controller/src/quadProgpp/Array.cc new file mode 100755 index 0000000..4045bee --- /dev/null +++ b/controllers/unitree_guide_controller/src/quadProgpp/Array.cc @@ -0,0 +1,28 @@ +// $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $ +// This file is part of QuadProg++: +// Copyright (C) 2006--2009 Luca Di Gaspero. +// +// This software may be modified and distributed under the terms +// of the MIT license. See the LICENSE file for details. + +#include "unitree_guide_controller/quadProgpp/Array.hh" + +/** + Index utilities + */ + +namespace quadprogpp { + std::set seq(unsigned int s, unsigned int e) { + std::set tmp; + for (unsigned int i = s; i <= e; i++) tmp.insert(i); + + return tmp; + } + + std::set singleton(unsigned int i) { + std::set tmp; + tmp.insert(i); + + return tmp; + } +} // namespace quadprogpp diff --git a/controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc b/controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc new file mode 100755 index 0000000..d7fb6c3 --- /dev/null +++ b/controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc @@ -0,0 +1,729 @@ +/* +File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $ + + Author: Luca Di Gaspero + DIEGM - University of Udine, Italy + luca.digaspero@uniud.it + http://www.diegm.uniud.it/digaspero/ + + This software may be modified and distributed under the terms + of the MIT license. See the LICENSE file for details. + + */ + +#include +#include +#include +#include +#include +#include +#include "unitree_guide_controller/quadProgpp/QuadProg++.hh" +// #define TRACE_SOLVER + +namespace quadprogpp { + // Utility functions for updating some data needed by the solution method + void compute_d(Vector &d, const Matrix &J, + const Vector &np); + + void update_z(Vector &z, const Matrix &J, + const Vector &d, int iq); + + void update_r(const Matrix &R, Vector &r, + const Vector &d, int iq); + + bool add_constraint(Matrix &R, Matrix &J, Vector &d, + unsigned int &iq, double &rnorm); + + void delete_constraint(Matrix &R, Matrix &J, Vector &A, + Vector &u, unsigned int n, int p, + unsigned int &iq, int l); + + // Utility functions for computing the Cholesky decomposition and solving + // linear systems + void cholesky_decomposition(Matrix &A); + + void cholesky_solve(const Matrix &L, Vector &x, + const Vector &b); + + void forward_elimination(const Matrix &L, Vector &y, + const Vector &b); + + void backward_elimination(const Matrix &U, Vector &x, + const Vector &y); + + // Utility functions for computing the scalar product and the euclidean + // distance between two numbers + double scalar_product(const Vector &x, const Vector &y); + + double distance(double a, double b); + + // Utility functions for printing vectors and matrices + void print_matrix(const char *name, const Matrix &A, int n = -1, + int m = -1); + + template + void print_vector(const char *name, const Vector &v, int n = -1); + + // The Solving function, implementing the Goldfarb-Idnani method + double solve_quadprog(Matrix &G, Vector &g0, + const Matrix &CE, const Vector &ce0, + const Matrix &CI, const Vector &ci0, + Vector &x) { + std::ostringstream msg; + unsigned int n = G.ncols(), p = CE.ncols(), m = CI.ncols(); + if (G.nrows() != n) { + msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " + << G.ncols() << ")"; + throw std::logic_error(msg.str()); + } + if (CE.nrows() != n) { + msg << "The matrix CE is incompatible (incorrect number of rows " + << CE.nrows() << " , expecting " << n << ")"; + throw std::logic_error(msg.str()); + } + if (ce0.size() != p) { + msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() + << ", expecting " << p << ")"; + throw std::logic_error(msg.str()); + } + if (CI.nrows() != n) { + msg << "The matrix CI is incompatible (incorrect number of rows " + << CI.nrows() << " , expecting " << n << ")"; + throw std::logic_error(msg.str()); + } + if (ci0.size() != m) { + msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() + << ", expecting " << m << ")"; + throw std::logic_error(msg.str()); + } + x.resize(n); + unsigned int i, j, k, l; /* indices */ + int ip; // this is the index of the constraint to be added to the active set + Matrix R(n, n), J(n, n); + Vector s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), + u_old(m + p); + double f_value, psi, c1, c2, sum, ss, R_norm; + double inf; + if (std::numeric_limits::has_infinity) + inf = std::numeric_limits::infinity(); + else + inf = 1.0E300; + double t, t1, t2; /* t is the step lenght, which is the minimum of the partial + * step length t1 and the full step length t2 */ + Vector A(m + p), A_old(m + p), iai(m + p); + unsigned int iq, iter = 0; + Vector iaexcl(m + p); + + /* p is the number of equality constraints */ + /* m is the number of inequality constraints */ +#ifdef TRACE_SOLVER + std::cout << std::endl << "Starting solve_quadprog" << std::endl; + print_matrix("G", G); + print_vector("g0", g0); + print_matrix("CE", CE); + print_vector("ce0", ce0); + print_matrix("CI", CI); + print_vector("ci0", ci0); +#endif + + /* + * Preprocessing phase + */ + + /* compute the trace of the original matrix G */ + c1 = 0.0; + for (i = 0; i < n; i++) { + c1 += G[i][i]; + } + /* decompose the matrix G in the form L^T L */ + cholesky_decomposition(G); +#ifdef TRACE_SOLVER + print_matrix("G", G); +#endif + /* initialize the matrix R */ + for (i = 0; i < n; i++) { + d[i] = 0.0; + for (j = 0; j < n; j++) R[i][j] = 0.0; + } + R_norm = 1.0; /* this variable will hold the norm of the matrix R */ + + /* compute the inverse of the factorized matrix G^-1, this is the initial + * value for H */ + c2 = 0.0; + for (i = 0; i < n; i++) { + d[i] = 1.0; + forward_elimination(G, z, d); + for (j = 0; j < n; j++) J[i][j] = z[j]; + c2 += z[i]; + d[i] = 0.0; + } +#ifdef TRACE_SOLVER + print_matrix("J", J); +#endif + + /* c1 * c2 is an estimate for cond(G) */ + + /* + * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x + * this is a feasible point in the dual space + * x = G^-1 * g0 + */ + cholesky_solve(G, x, g0); + for (i = 0; i < n; i++) x[i] = -x[i]; + /* and compute the current solution value */ + f_value = 0.5 * scalar_product(g0, x); +#ifdef TRACE_SOLVER + std::cout << "Unconstrained solution: " << f_value << std::endl; + print_vector("x", x); +#endif + + /* Add equality constraints to the working set A */ + iq = 0; + for (i = 0; i < p; i++) { + for (j = 0; j < n; j++) np[j] = CE[j][i]; + compute_d(d, J, np); + update_z(z, J, d, iq); + update_r(R, r, d, iq); +#ifdef TRACE_SOLVER + print_matrix("R", R, n, iq); + print_vector("z", z); + print_vector("r", r, iq); + print_vector("d", d); +#endif + + /* compute full step length t2: i.e., the minimum step in primal space s.t. + the contraint becomes feasible */ + t2 = 0.0; + if (fabs(scalar_product(z, z)) > + std::numeric_limits::epsilon()) // i.e. z != 0 + t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np); + + /* set x = x + t2 * z */ + for (k = 0; k < n; k++) x[k] += t2 * z[k]; + + /* set u = u+ */ + u[iq] = t2; + for (k = 0; k < iq; k++) u[k] -= t2 * r[k]; + + /* compute the new solution value */ + f_value += 0.5 * (t2 * t2) * scalar_product(z, np); + A[i] = -i - 1; + + if (!add_constraint(R, J, d, iq, R_norm)) { + // Equality constraints are linearly dependent + throw std::runtime_error("Constraints are linearly dependent"); + return f_value; + } + } + + /* set iai = K \ A */ + for (i = 0; i < m; i++) iai[i] = i; + + l1: + iter++; +#ifdef TRACE_SOLVER + print_vector("x", x); +#endif + /* step 1: choose a violated constraint */ + for (i = p; i < iq; i++) { + ip = A[i]; + iai[ip] = -1; + } + + /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */ + ss = 0.0; + psi = 0.0; /* this value will contain the sum of all infeasibilities */ + ip = 0; /* ip will be the index of the chosen violated constraint */ + for (i = 0; i < m; i++) { + iaexcl[i] = true; + sum = 0.0; + for (j = 0; j < n; j++) sum += CI[j][i] * x[j]; + sum += ci0[i]; + s[i] = sum; + psi += std::min(0.0, sum); + } +#ifdef TRACE_SOLVER + print_vector("s", s, m); +#endif + + if (fabs(psi) <= + m * std::numeric_limits::epsilon() * c1 * c2 * 100.0) { + /* numerically there are not infeasibilities anymore */ + return f_value; + } + + /* save old values for u and A */ + for (i = 0; i < iq; i++) { + u_old[i] = u[i]; + A_old[i] = A[i]; + } + /* and for x */ + for (i = 0; i < n; i++) x_old[i] = x[i]; + + l2: /* Step 2: check for feasibility and determine a new S-pair */ + for (i = 0; i < m; i++) { + if (s[i] < ss && iai[i] != -1 && iaexcl[i]) { + ss = s[i]; + ip = i; + } + } + if (ss >= 0.0) { + return f_value; + } + + /* set np = n[ip] */ + for (i = 0; i < n; i++) np[i] = CI[i][ip]; + /* set u = [u 0]^T */ + u[iq] = 0.0; + /* add ip to the active set A */ + A[iq] = ip; + +#ifdef TRACE_SOLVER + std::cout << "Trying with constraint " << ip << std::endl; + print_vector("np", np); +#endif + + l2a: /* Step 2a: determine step direction */ + /* compute z = H np: the step direction in the primal space (through J, see + * the paper) */ + compute_d(d, J, np); + update_z(z, J, d, iq); + /* compute N* np (if q > 0): the negative of the step direction in the dual + * space */ + update_r(R, r, d, iq); +#ifdef TRACE_SOLVER + std::cout << "Step direction z" << std::endl; + print_vector("z", z); + print_vector("r", r, iq + 1); + print_vector("u", u, iq + 1); + print_vector("d", d); + print_vector("A", A, iq + 1); +#endif + + /* Step 2b: compute step length */ + l = 0; + /* Compute t1: partial step length (maximum step in dual space without + * violating dual feasibility */ + t1 = inf; /* +inf */ + /* find the index l s.t. it reaches the minimum of u+[x] / r */ + for (k = p; k < iq; k++) { + if (r[k] > 0.0) { + if (u[k] / r[k] < t1) { + t1 = u[k] / r[k]; + l = A[k]; + } + } + } + /* Compute t2: full step length (minimum step in primal space such that the + * constraint ip becomes feasible */ + if (fabs(scalar_product(z, z)) > + std::numeric_limits::epsilon()) // i.e. z != 0 + { + t2 = -s[ip] / scalar_product(z, np); + if (t2 < 0) // patch suggested by Takano Akio for handling numerical + // inconsistencies + t2 = inf; + } else + t2 = inf; /* +inf */ + + /* the step is chosen as the minimum of t1 and t2 */ + t = std::min(t1, t2); +#ifdef TRACE_SOLVER + std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 + << ") "; +#endif + + /* Step 2c: determine new S-pair and take step: */ + + /* case (i): no step in primal or dual space */ + if (t >= inf) { + /* QPP is infeasible */ + // FIXME: unbounded to raise + return inf; + } + /* case (ii): step in dual space */ + if (t2 >= inf) { + /* set u = u + t * [-r 1] and drop constraint l from the active set A */ + for (k = 0; k < iq; k++) u[k] -= t * r[k]; + u[iq] += t; + iai[l] = l; + delete_constraint(R, J, A, u, n, p, iq, l); +#ifdef TRACE_SOLVER + std::cout << " in dual space: " << f_value << std::endl; + print_vector("x", x); + print_vector("z", z); + print_vector("A", A, iq + 1); +#endif + goto l2a; + } + + /* case (iii): step in primal and dual space */ + + /* set x = x + t * z */ + for (k = 0; k < n; k++) x[k] += t * z[k]; + /* update the solution value */ + f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]); + /* u = u + t * [-r 1] */ + for (k = 0; k < iq; k++) u[k] -= t * r[k]; + u[iq] += t; +#ifdef TRACE_SOLVER + std::cout << " in both spaces: " << f_value << std::endl; + print_vector("x", x); + print_vector("u", u, iq + 1); + print_vector("r", r, iq + 1); + print_vector("A", A, iq + 1); +#endif + + if (fabs(t - t2) < std::numeric_limits::epsilon()) { +#ifdef TRACE_SOLVER + std::cout << "Full step has taken " << t << std::endl; + print_vector("x", x); +#endif + /* full step has taken */ + /* add constraint ip to the active set*/ + if (!add_constraint(R, J, d, iq, R_norm)) { + iaexcl[ip] = false; + delete_constraint(R, J, A, u, n, p, iq, ip); +#ifdef TRACE_SOLVER + print_matrix("R", R); + print_vector("A", A, iq); + print_vector("iai", iai); +#endif + for (i = 0; i < m; i++) iai[i] = i; + for (i = p; i < iq; i++) { + A[i] = A_old[i]; + u[i] = u_old[i]; + iai[A[i]] = -1; + } + for (i = 0; i < n; i++) x[i] = x_old[i]; + goto l2; /* go to step 2 */ + } else + iai[ip] = -1; +#ifdef TRACE_SOLVER + print_matrix("R", R); + print_vector("A", A, iq); + print_vector("iai", iai); +#endif + goto l1; + } + + /* a patial step has taken */ +#ifdef TRACE_SOLVER + std::cout << "Partial step has taken " << t << std::endl; + print_vector("x", x); +#endif + /* drop constraint l */ + iai[l] = l; + delete_constraint(R, J, A, u, n, p, iq, l); +#ifdef TRACE_SOLVER + print_matrix("R", R); + print_vector("A", A, iq); +#endif + + /* update s[ip] = CI * x + ci0 */ + sum = 0.0; + for (k = 0; k < n; k++) sum += CI[k][ip] * x[k]; + s[ip] = sum + ci0[ip]; + +#ifdef TRACE_SOLVER + print_vector("s", s, m); +#endif + goto l2a; + } + + inline void compute_d(Vector &d, const Matrix &J, + const Vector &np) { + int i, j, n = d.size(); + double sum; + + /* compute d = H^T * np */ + for (i = 0; i < n; i++) { + sum = 0.0; + for (j = 0; j < n; j++) sum += J[j][i] * np[j]; + d[i] = sum; + } + } + + inline void update_z(Vector &z, const Matrix &J, + const Vector &d, int iq) { + int i, j, n = z.size(); + + /* setting of z = H * d */ + for (i = 0; i < n; i++) { + z[i] = 0.0; + for (j = iq; j < n; j++) z[i] += J[i][j] * d[j]; + } + } + + inline void update_r(const Matrix &R, Vector &r, + const Vector &d, int iq) { + int i, j; + double sum; + + /* setting of r = R^-1 d */ + for (i = iq - 1; i >= 0; i--) { + sum = 0.0; + for (j = i + 1; j < iq; j++) sum += R[i][j] * r[j]; + r[i] = (d[i] - sum) / R[i][i]; + } + } + + bool add_constraint(Matrix &R, Matrix &J, Vector &d, + unsigned int &iq, double &R_norm) { + unsigned int n = d.size(); +#ifdef TRACE_SOLVER + std::cout << "Add constraint " << iq << '/'; +#endif + unsigned int i, j, k; + double cc, ss, h, t1, t2, xny; + + /* we have to find the Givens rotation which will reduce the element + d[j] to zero. + if it is already zero we don't have to do anything, except of + decreasing j */ + for (j = n - 1; j >= iq + 1; j--) { + /* The Givens rotation is done with the matrix (cc cs, cs -cc). + If cc is one, then element (j) of d is zero compared with element + (j - 1). Hence we don't have to do anything. + If cc is zero, then we just have to switch column (j) and column (j - 1) + of J. Since we only switch columns in J, we have to be careful how we + update d depending on the sign of gs. + Otherwise we have to apply the Givens rotation to these columns. + The i - 1 element of d has to be updated to h. */ + cc = d[j - 1]; + ss = d[j]; + h = distance(cc, ss); + if (fabs(h) < std::numeric_limits::epsilon()) // h == 0 + continue; + d[j] = 0.0; + ss = ss / h; + cc = cc / h; + if (cc < 0.0) { + cc = -cc; + ss = -ss; + d[j - 1] = -h; + } else + d[j - 1] = h; + xny = ss / (1.0 + cc); + for (k = 0; k < n; k++) { + t1 = J[k][j - 1]; + t2 = J[k][j]; + J[k][j - 1] = t1 * cc + t2 * ss; + J[k][j] = xny * (t1 + J[k][j - 1]) - t2; + } + } + /* update the number of constraints added*/ + iq++; + /* To update R we have to put the iq components of the d vector + into column iq - 1 of R + */ + for (i = 0; i < iq; i++) R[i][iq - 1] = d[i]; +#ifdef TRACE_SOLVER + std::cout << iq << std::endl; + print_matrix("R", R, iq, iq); + print_matrix("J", J); + print_vector("d", d, iq); +#endif + + if (fabs(d[iq - 1]) <= std::numeric_limits::epsilon() * R_norm) { + // problem degenerate + return false; + } + R_norm = std::max(R_norm, fabs(d[iq - 1])); + return true; + } + + void delete_constraint(Matrix &R, Matrix &J, Vector &A, + Vector &u, unsigned int n, int p, + unsigned int &iq, int l) { +#ifdef TRACE_SOLVER + std::cout << "Delete constraint " << l << ' ' << iq; +#endif + unsigned int i, j, k, + qq = 0; // just to prevent warnings from smart compilers + double cc, ss, h, xny, t1, t2; + + bool found = false; + /* Find the index qq for active constraint l to be removed */ + for (i = p; i < iq; i++) + if (A[i] == l) { + qq = i; + found = true; + break; + } + + if (!found) { + std::ostringstream os; + os << "Attempt to delete non existing constraint, constraint: " << l; + throw std::invalid_argument(os.str()); + } + /* remove the constraint from the active set and the duals */ + for (i = qq; i < iq - 1; i++) { + A[i] = A[i + 1]; + u[i] = u[i + 1]; + for (j = 0; j < n; j++) R[j][i] = R[j][i + 1]; + } + + A[iq - 1] = A[iq]; + u[iq - 1] = u[iq]; + A[iq] = 0; + u[iq] = 0.0; + for (j = 0; j < iq; j++) R[j][iq - 1] = 0.0; + /* constraint has been fully removed */ + iq--; +#ifdef TRACE_SOLVER + std::cout << '/' << iq << std::endl; +#endif + + if (iq == 0) return; + + for (j = qq; j < iq; j++) { + cc = R[j][j]; + ss = R[j + 1][j]; + h = distance(cc, ss); + if (fabs(h) < std::numeric_limits::epsilon()) // h == 0 + continue; + cc = cc / h; + ss = ss / h; + R[j + 1][j] = 0.0; + if (cc < 0.0) { + R[j][j] = -h; + cc = -cc; + ss = -ss; + } else + R[j][j] = h; + + xny = ss / (1.0 + cc); + for (k = j + 1; k < iq; k++) { + t1 = R[j][k]; + t2 = R[j + 1][k]; + R[j][k] = t1 * cc + t2 * ss; + R[j + 1][k] = xny * (t1 + R[j][k]) - t2; + } + for (k = 0; k < n; k++) { + t1 = J[k][j]; + t2 = J[k][j + 1]; + J[k][j] = t1 * cc + t2 * ss; + J[k][j + 1] = xny * (J[k][j] + t1) - t2; + } + } + } + + inline double distance(double a, double b) { + double a1, b1, t; + a1 = fabs(a); + b1 = fabs(b); + if (a1 > b1) { + t = (b1 / a1); + return a1 * sqrt(1.0 + t * t); + } else if (b1 > a1) { + t = (a1 / b1); + return b1 * sqrt(1.0 + t * t); + } + return a1 * sqrt(2.0); + } + + inline double scalar_product(const Vector &x, const Vector &y) { + int i, n = x.size(); + double sum; + + sum = 0.0; + for (i = 0; i < n; i++) sum += x[i] * y[i]; + return sum; + } + + void cholesky_decomposition(Matrix &A) { + int i, j, k, n = A.nrows(); + double sum; + + for (i = 0; i < n; i++) { + for (j = i; j < n; j++) { + sum = A[i][j]; + for (k = i - 1; k >= 0; k--) sum -= A[i][k] * A[j][k]; + if (i == j) { + if (sum <= 0.0) { + std::ostringstream os; + // raise error + print_matrix("A", A); + os << "Error in cholesky decomposition, sum: " << sum; + throw std::logic_error(os.str()); + exit(-1); + } + A[i][i] = sqrt(sum); + } else + A[j][i] = sum / A[i][i]; + } + for (k = i + 1; k < n; k++) A[i][k] = A[k][i]; + } + } + + void cholesky_solve(const Matrix &L, Vector &x, + const Vector &b) { + int n = L.nrows(); + Vector y(n); + + /* Solve L * y = b */ + forward_elimination(L, y, b); + /* Solve L^T * x = y */ + backward_elimination(L, x, y); + } + + inline void forward_elimination(const Matrix &L, Vector &y, + const Vector &b) { + int i, j, n = L.nrows(); + + y[0] = b[0] / L[0][0]; + for (i = 1; i < n; i++) { + y[i] = b[i]; + for (j = 0; j < i; j++) y[i] -= L[i][j] * y[j]; + y[i] = y[i] / L[i][i]; + } + } + + inline void backward_elimination(const Matrix &U, Vector &x, + const Vector &y) { + int i, j, n = U.nrows(); + + x[n - 1] = y[n - 1] / U[n - 1][n - 1]; + for (i = n - 2; i >= 0; i--) { + x[i] = y[i]; + for (j = i + 1; j < n; j++) x[i] -= U[i][j] * x[j]; + x[i] = x[i] / U[i][i]; + } + } + + void print_matrix(const char *name, const Matrix &A, int n, int m) { + std::ostringstream s; + std::string t; + if (n == -1) n = A.nrows(); + if (m == -1) m = A.ncols(); + + s << name << ": " << std::endl; + for (int i = 0; i < n; i++) { + s << " "; + for (int j = 0; j < m; j++) s << A[i][j] << ", "; + s << std::endl; + } + t = s.str(); + t = t.substr( + 0, t.size() - 3); // To remove the trailing space, comma and newline + + std::cout << t << std::endl; + } + + template + void print_vector(const char *name, const Vector &v, int n) { + std::ostringstream s; + std::string t; + if (n == -1) n = v.size(); + + s << name << ": " << std::endl << " "; + for (int i = 0; i < n; i++) { + s << v[i] << ", "; + } + t = s.str(); + t = t.substr(0, t.size() - 2); // To remove the trailing space and comma + + std::cout << t << std::endl; + } +} // namespace quadprogpp diff --git a/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp similarity index 96% rename from controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp rename to controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index f8a5337..76e674f 100644 --- a/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp @@ -3,8 +3,8 @@ // #include -#include -#include +#include "unitree_guide_controller/control/CtrlComponent.h" +#include "unitree_guide_controller/robot/QuadrupedRobot.h" void QuadrupedRobot::init(const std::string &robot_description) { KDL::Tree robot_tree; diff --git a/controllers/unitree_guide_controller/src/robotics/RobotLeg.cpp b/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp similarity index 93% rename from controllers/unitree_guide_controller/src/robotics/RobotLeg.cpp rename to controllers/unitree_guide_controller/src/robot/RobotLeg.cpp index e2c3af4..cfc4d87 100644 --- a/controllers/unitree_guide_controller/src/robotics/RobotLeg.cpp +++ b/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp @@ -3,7 +3,7 @@ // #include -#include +#include "unitree_guide_controller/robot/RobotLeg.h" RobotLeg::RobotLeg(const KDL::Chain &chain) { chain_ = chain; @@ -36,6 +36,6 @@ KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const K const KDL::Wrenches &force) const { KDL::JntArray torque(chain_.getNrOfJoints()); id_solver_->CartToJnt(joint_positions, joint_velocities, KDL::JntArray(chain_.getNrOfJoints()), force, - torque); + torque); return torque; } diff --git a/descriptions/go2_description/config/robot_control.yaml b/descriptions/go2_description/config/robot_control.yaml index de546fb..3817005 100644 --- a/descriptions/go2_description/config/robot_control.yaml +++ b/descriptions/go2_description/config/robot_control.yaml @@ -1,8 +1,7 @@ # Controller Manager configuration controller_manager: ros__parameters: - update_rate: 200 # Hz - use_sim_time: true # If running in simulation + update_rate: 500 # Hz # Define the available controllers joint_state_broadcaster: @@ -11,7 +10,38 @@ controller_manager: imu_sensor_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster + unitree_guide_controller: + type: unitree_guide_controller/UnitreeGuideController + imu_sensor_broadcaster: ros__parameters: sensor_name: "imu_sensor" - frame_id: "imu_link" \ No newline at end of file + frame_id: "imu_link" + +unitree_guide_controller: + ros__parameters: + joints: + - FR_hip_joint + - FR_thigh_joint + - FR_calf_joint + - FL_hip_joint + - FL_thigh_joint + - FL_calf_joint + - RR_hip_joint + - RR_thigh_joint + - RR_calf_joint + - RL_hip_joint + - RL_thigh_joint + - RL_calf_joint + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity \ No newline at end of file diff --git a/descriptions/go2_description/launch/hardware.launch.py b/descriptions/go2_description/launch/hardware.launch.py index 911ed46..249c04f 100644 --- a/descriptions/go2_description/launch/hardware.launch.py +++ b/descriptions/go2_description/launch/hardware.launch.py @@ -3,8 +3,8 @@ import os import xacro from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -29,40 +29,66 @@ def launch_setup(context, *args, **kwargs): "robot_control.yaml", ] ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[ + { + 'publish_frequency': 20.0, + 'use_tf_static': True, + 'robot_description': robot_description, + 'ignore_timestamp': True + } + ], + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + output="both", + ) + + joint_state_publisher = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", + "--controller-manager", "/controller_manager"], + ) + + imu_sensor_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["imu_sensor_broadcaster", + "--controller-manager", "/controller_manager"], + ) + + unitree_guide_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"], + ) + return [ - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - parameters=[ - { - 'publish_frequency': 20.0, - 'use_tf_static': True, - 'robot_description': robot_description, - 'ignore_timestamp': True - } - ], + robot_state_publisher, + controller_manager, + joint_state_publisher, + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_publisher, + on_exit=[imu_sensor_broadcaster], + ) ), - Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - remappings=[ - ("~/robot_description", "/robot_description"), - ], - output="both", - ), - Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", - "--controller-manager", "/controller_manager"], - ), - Node( - package="controller_manager", - executable="spawner", - arguments=["imu_sensor_broadcaster", - "--controller-manager", "/controller_manager"], + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=imu_sensor_broadcaster, + on_exit=[unitree_guide_controller], + ) ), ] diff --git a/descriptions/go2_description/package.xml b/descriptions/go2_description/package.xml index e4f0422..6535ac4 100644 --- a/descriptions/go2_description/package.xml +++ b/descriptions/go2_description/package.xml @@ -15,7 +15,6 @@ joint_state_publisher robot_state_publisher imu_sensor_broadcaster - ament_cmake