update

CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.1)
+project(cpp-utils)
+
+set(CMAKE_CXX_STANDARD 17)
+
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+set(CMAKE_CXX_FLAGS_DEBUG "-Wall -Wextra -g")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3")
+
+
+add_library(${PROJECT_NAME} INTERFACE)
+
+find_package(Eigen3 REQUIRED)
+
+target_include_directories(${PROJECT_NAME} INTERFACE include/)
+

include/cpp-utils/dart-helper.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <Eigen/Geometry>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+
+#include <dart/dart.hpp>
+#include "math.h"
+
+namespace utils {
+
+Eigen::Vector3d getRPY(dart::dynamics::BodyNode *body) {
+  Eigen::Matrix3d rotMatrix = body->getTransform().rotation();
+  return utils::getRPY(rotMatrix);
+}
+
+} // end namespace utils
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

include/cpp-utils/math.h

@@ -0,0 +1,254 @@
+#pragma once
+
+#include <Eigen/Geometry>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+
+
+namespace utils {
+
+inline bool isEven(int n) { return not (n % 2); }
+
+inline bool isOdd(int n) { return (n % 2); }
+
+inline double wrapToPi(double angle){
+  double ret=angle;
+  while(ret>M_PI)
+    ret-=2*M_PI;
+
+  while(ret<=-M_PI)
+    ret+=2*M_PI;
+
+  return ret;
+}
+
+inline Eigen::MatrixXd matrixPower(const Eigen::MatrixXd& A, int exp){
+  Eigen::MatrixXd result = Eigen::MatrixXd::Identity(A.rows(),A.cols());
+
+  for (int i=0; i<exp;++i)
+    result *= A;
+  return result;
+}
+
+inline double sign(double x){
+  if(x>0) return +1;
+  if(x<0) return -1;
+  return -1;
+}
+
+// Elementary rotation around xyz axes
+    inline Eigen::Matrix3d rotx(double ax){
+      Eigen::Matrix3d rx = Eigen::Matrix3d::Zero();
+
+      rx(0,0) = 1;
+
+      rx(1,1) = cos(ax);
+      rx(1,2) = -sin(ax);
+
+      rx(2,1) = sin(ax);
+      rx(2,2) = cos(ax);
+
+      return rx;
+    }
+
+    inline Eigen::Matrix3d roty(double ay){
+      Eigen::Matrix3d ry = Eigen::Matrix3d::Zero();
+
+      ry(0,0) = cos(ay);
+      ry(0,2) = sin(ay);
+
+      ry(1,1) = 1;
+
+      ry(2,0) = -sin(ay);
+      ry(2,2) = cos(ay);
+
+      return ry;
+    }
+
+    inline Eigen::Matrix3d rotz(double az){
+      Eigen::Matrix3d rz = Eigen::Matrix3d::Zero();
+
+      rz(0,0) = cos(az);
+      rz(0,1) = -sin(az);
+
+      rz(1,0) = sin(az);
+      rz(1,1) = cos(az);
+
+      rz(2,2) = 1;
+      return rz;
+    }
+
+double angleSignedDistance(double a, double b){
+  double d = a - b;
+  while(d >  M_PI) d = d - 2.0*M_PI;
+  while(d < -M_PI) d = d + 2.0*M_PI;
+
+  return d;
+}
+
+Eigen::Matrix3d rot(const Eigen::Vector3d & eul){
+
+  Eigen::Matrix3d rx = rotx(eul(0));
+  Eigen::Matrix3d ry = roty(eul(1));
+  Eigen::Matrix3d rz = rotz(eul(2));
+
+  return rz*ry*rx;
+}
+
+Eigen::Matrix2d rot(const double angle){
+
+  Eigen::Matrix2d R;
+  const double c = cos(angle);
+  const double s = sin(angle);
+  R(0,0) = c;
+  R(0,1) = -s;
+  R(1,0) = s;
+  R(1,1) = c;
+
+  return R;
+}
+
+Eigen::Vector3d angleSignedDistance(Eigen::Vector3d a, Eigen::Vector3d b) {
+  Eigen::Matrix3d Ra = rot(a);
+  Eigen::Matrix3d Rb = rot(b);
+
+  Eigen::Matrix3d Rdiff = Rb.transpose() * Ra;
+  auto aa = Eigen::AngleAxisd(Rdiff);
+  return aa.angle() * Ra * aa.axis();
+}
+
+Eigen::Vector3d getRPY(const Eigen::Matrix3d & rotMatrix) {
+  Eigen::Vector3d RPY;
+  RPY << atan2(rotMatrix(2, 1), rotMatrix(2, 2)),
+      atan2(-rotMatrix(2, 0), sqrt(rotMatrix(2, 1) * rotMatrix(2, 1) + rotMatrix(2, 2) * rotMatrix(2, 2))),
+      atan2(rotMatrix(1, 0), rotMatrix(0, 0));
+  return RPY;
+}
+
+
+
+Eigen::Matrix4d v2t(const Eigen::VectorXd & v){
+
+  Eigen::Matrix4d m = Eigen::Matrix4d::Identity();
+
+  Eigen::Vector3d eul = v.head(3);
+  Eigen::Matrix3d r = utils::rot(eul);
+
+  m.block<3,3>(0,0) = r;
+  m.block<3,1>(0,3) = v.tail(3);
+
+  return m;
+}
+
+Eigen::VectorXd t2v(const Eigen::Matrix4d & m) {
+  Eigen::VectorXd v(6);
+
+  double beta = atan2( m(0,2), sqrt(pow(m(0,0), 2) + pow(m(0,1), 2)) );
+  double alpha = atan2( -m(1,2)/cos(beta), m(2,2)/cos(beta) );
+  double gamma = atan2( -m(0,1)/cos(beta), m(0,0)/cos(beta) );
+
+  v(0) = alpha;
+  v(1) = beta;
+  v(2) = gamma;
+  v(3) = m(0,3);
+  v(4) = m(1,3);
+  v(5) = m(2,3);
+
+  return v;
+}
+
+// Express v2 in the frame of v1
+inline Eigen::VectorXd vvRel(const Eigen::VectorXd & v2, const Eigen::VectorXd & v1) {
+      return utils::t2v(utils::v2t(v1).inverse() * utils::v2t(v2));
+    }
+
+// Check if an element is in a vector
+template<typename T>
+bool is_in_vector(const std::vector<T> & vector, const T & elt) {
+  return vector.end() != std::find(vector.begin(),vector.end(),elt);
+}
+
+// arrange elements in a block diagonal matrix
+template<typename MatrixEigen>
+Eigen::MatrixXd blkdiag(const std::vector<MatrixEigen> & a) {
+  int count = a.size();
+  int block_rows = a[0].rows();
+  int block_cols = a[0].cols();
+  Eigen::MatrixXd bdm = Eigen::MatrixXd::Zero(block_rows * count, block_cols * count);
+  for (int i = 0; i < count; ++i)
+    bdm.block(i * block_rows, i * block_cols, block_rows, block_cols) = a[i];
+
+  return bdm;
+}
+
+// Repeat in a vertical stack for a certain amount of times
+template<typename Derived>
+Eigen::MatrixXd vrepeat(const Eigen::MatrixBase<Derived> & a, int num) {
+  int block_rows = a.rows();
+  Eigen::MatrixXd vstack = Eigen::MatrixXd::Zero(block_rows * num, a.cols());
+  for (int i = 0; i < num; ++i)
+    vstack.middleRows(i * block_rows, block_rows) = a;
+  return vstack;
+}
+
+inline Eigen::MatrixXd vrepeat(const Eigen::MatrixXd & a, int num) {
+  int block_rows = a.rows();
+  Eigen::MatrixXd vstack = Eigen::MatrixXd::Zero(block_rows * num, a.cols());
+  for (int i = 0; i < num; ++i)
+    vstack.middleRows(i * block_rows, block_rows) = a;
+  return vstack;
+}
+
+
+// Repeat elements diagonally for a certain amount of times
+template<typename Derived>
+Eigen::MatrixXd diagrepeat(const Eigen::DenseBase<Derived> & a, int num) {
+  int block_rows = a.rows();
+  int block_cols = a.cols();
+  Eigen::MatrixXd diag = Eigen::MatrixXd::Zero(num*block_rows, num*block_cols);
+  for (int i = 0; i < num; ++i)
+    diag.block(i * block_rows,i * block_cols, block_rows, block_cols) = a;
+  return diag;
+}
+
+// Stack elements vertically from std::vector
+template<typename VectorEigen>
+Eigen::VectorXd vstack(const std::vector<VectorEigen> & a) {
+  int num = a.size();
+  int block_rows = a[0].rows();
+  Eigen::VectorXd vstack = Eigen::VectorXd::Zero(block_rows * num);
+  for (int i = 0; i < num; ++i)
+    vstack.segment(i * block_rows, block_rows) = a[i];
+  return vstack;
+}
+
+inline Eigen::MatrixXd vstack(const std::vector<Eigen::MatrixXd> & a) {
+  int num = a.size();
+  int block_rows = a[0].rows();
+  Eigen::MatrixXd vstack = Eigen::MatrixXd::Zero(block_rows * num, a[0].cols());
+  for (int i = 0; i < num; ++i)
+    vstack.middleRows(i * block_rows, block_rows) = a[i];
+  return vstack;
+}
+
+
+} // end namespace utils
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

simulation/utils.h → include/cpp-utils/simulation.h

@@ -3,6 +3,9 @@
 #include <Eigen/Dense>
 #include <vector>
 #include <fstream>
+#include <chrono>
+#include <memory>
+#include <iostream>
 
 namespace utils {