76 autodiff

CMakeLists.txt

@@ -34,6 +34,12 @@ if (DEFINED USE_PINOCCHIO)
   message(pinocchio_INCLUDE_DIRS="${pinocchio_INCLUDE_DIRS}")
 endif()
 
+# Find Casadi
+if (DEFINED BUILD_WITH_CASADI)
+  find_package(casadi REQUIRED)
+  message(casadi_INCLUDE_DIRS="${pinocchio_INCLUDE_DIRS}")
+endif()
+
 # Find sockcanpp
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR})
 find_package(sockcanpp REQUIRED)
@@ -93,6 +99,14 @@ if (DEFINED USE_PINOCCHIO)
     target_compile_definitions(huron PUBLIC HURON_USE_PINOCCHIO=1)
 endif()
 
+if (DEFINED BUILD_WITH_CASADI)
+  target_link_libraries(huron
+    PUBLIC
+      casadi)
+    target_compile_definitions(huron PUBLIC HURON_ENABLE_AUTODIFF=1)
+    target_compile_definitions(huron PUBLIC HURON_USE_CASADI=1)
+endif()
+
 # Copy urdf files to build directory
 add_custom_command(TARGET huron PRE_BUILD
                    COMMAND ${CMAKE_COMMAND} -E copy

common/include/huron/types.h

@@ -4,8 +4,85 @@
 
 namespace huron {
 
-typedef Eigen::Matrix< double, 6, 1 > Vector6d;
-typedef Eigen::Matrix< double, 6, 6 > Matrix6d;
-typedef Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd;
+template <typename T>
+using Affine3 = Eigen::Transform<T, 3, Eigen::Affine>;
+
+template <typename T>
+using Vector4 = Eigen::Matrix<T, 4, 1>;
+
+template <typename T>
+using Vector3 = Eigen::Matrix<T, 3, 1>;
+
+template <typename T>
+using Vector2 = Eigen::Matrix<T, 2, 1>;
+
+template <typename T>
+using VectorX = Eigen::Matrix<T, Eigen::Dynamic, 1>;
+
+template <typename T>
+using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+template <typename T>
+using Matrix3 = Eigen::Matrix<T, 3, 3>;
+
+template <typename T>
+using Matrix4 = Eigen::Matrix<T, 4, 4>;
+
+template <typename T>
+using Vector6 = Eigen::Matrix<T, 6, 1>;
+
+template <typename T>
+using Matrix6 = Eigen::Matrix<T, 6, 6>;
+
+template <typename T>
+using Matrix6X = Eigen::Matrix<T, 6, Eigen::Dynamic>;
+
+/// double aliases
+using Affine3d = Affine3<double>;
+using Vector4d = Vector4<double>;
+using Vector3d = Vector3<double>;
+using Vector2d = Vector2<double>;
+using VectorXd = VectorX<double>;
+using MatrixXd = MatrixX<double>;
+using Matrix3d = Matrix3<double>;
+using Matrix4d = Matrix4<double>;
+using Vector6d = Vector6<double>;
+using Matrix6Xd = Matrix6X<double>;
 
 }  // namespace huron
+
+#if HURON_ENABLE_AUTODIFF==1
+#if HURON_USE_CASADI==1
+
+#include "huron/math/casadi.h"
+
+// namespace Eigen {
+//
+// template <>
+// struct NumTraits<casadi::SX> : NumTraits<double>
+// {
+//   using Real = casadi::SX;
+//   using NonInteger = casadi::SX;
+//   using Literal = casadi::SX;
+//   using Nested = casadi::SX;
+//   
+//   enum {
+//     // does not support complex Base types
+//     IsComplex             = 0 ,
+//     // does not support integer Base types
+//     IsInteger             = 0 ,
+//     // only support signed Base types
+//     IsSigned              = 1 ,
+//     // must initialize an AD<Base> object
+//     RequireInitialization = 1 ,
+//     // computational cost of the corresponding operations
+//     ReadCost              = 1 ,
+//     AddCost               = 2 ,
+//     MulCost               = 2
+//   };
+// };
+
+// }  // namespace Eigen
+
+#endif  // HURON_USE_CASADI
+#endif  // HURON_ENABLE_AUTODIFF

math/CMakeLists.txt

@@ -1,13 +1,13 @@
 set(dir "${CMAKE_CURRENT_SOURCE_DIR}")
 
 list(APPEND SOURCE_FILES
-        "${dir}/src/rotation.cc")
+        "${dir}/src/se3.cc")
 FILE(GLOB_RECURSE HEADER_FILES
         ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
 )
 
 list(APPEND TEST_SOURCE_FILES
-        "${dir}/test/test_rotation.cc")
+        "${dir}/test/test_se3.cc")
 
 set(INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include/")
 

math/include/huron/math/abs.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "huron/types.h"
+
+namespace huron {
+
+template <typename T>
+inline T abs(const T& x) {
+  return x < 0 ? -x : x;
+}
+
+#if HURON_USE_CASADI==1
+
+template <>
+inline casadi::SX abs<casadi::SX>(const casadi::SX& x) {
+  return casadi::SX::abs(x);
+}
+
+#endif  // HURON_USE_CASADI==1
+}  // namespace huron

math/include/huron/math/casadi.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#if HURON_ENABLE_AUTODIFF==1
+#if HURON_USE_CASADI==1
+
+#include "casadi_copy.h"
+
+#endif  // HURON_USE_CASADI
+#endif  // HURON_ENABLE_AUTODIFF

math/include/huron/math/casadi_copy.h

@@ -0,0 +1,77 @@
+/**
+ * The functions contained in this file are from the Pinocchio project
+ * (https://github.com/stack-of-tasks/pinocchio) with the following license:
+ *
+ * BSD 2-Clause License
+
+ * Copyright (c) 2014-2023, CNRS 
+ * Copyright (c) 2018-2023, INRIA
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * 
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the Pinocchio project.
+*/
+#pragma once
+
+#if HURON_ENABLE_AUTODIFF==1
+#if HURON_USE_CASADI==1
+
+#include <casadi/casadi.hpp>
+#include <eigen3/Eigen/Core>
+
+namespace huron {
+
+template<typename MT, typename Scalar>
+inline void copy(::casadi::Matrix<Scalar> const & src,
+                 Eigen::MatrixBase<MT> & dst)
+{
+  Eigen::DenseIndex const m = src.size1();
+  Eigen::DenseIndex const n = src.size2();
+
+  dst.resize(m, n);
+
+  for (Eigen::DenseIndex i = 0; i < m; ++i)
+    for (Eigen::DenseIndex j = 0; j < n; ++j)
+      dst(i, j) = src(i, j);
+}
+
+
+// Copy Eigen matrix to casadi matrix
+template<typename MT, typename Scalar>
+inline void copy(Eigen::MatrixBase<MT> const & src,
+                 ::casadi::Matrix<Scalar> & dst)
+{
+  Eigen::DenseIndex const m = src.rows();
+  Eigen::DenseIndex const n = src.cols();
+
+  dst.resize(m, n);
+
+  for (Eigen::DenseIndex i = 0; i < m; ++i)
+    for (Eigen::DenseIndex j = 0; j < n; ++j)
+      dst(i, j) = src(i, j);
+}
+} // namespace huron
+
+#endif  // HURON_USE_CASADI
+#endif  // HURON_ENABLE_AUTODIFF

math/include/huron/math/se3.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include "huron/types.h"
+#include "huron/utils/template_instantiations.h"
+
+namespace huron {
+
+template <typename T>
+class SE3 {
+ public:
+  SE3();
+  SE3(const Eigen::Matrix4<T>& tf);
+  SE3(const Eigen::Matrix3<T>& R, const Eigen::Vector3<T>& t);
+  SE3(const SE3&);
+  SE3& operator=(const SE3& other) {
+    tf_ = other.tf_;
+    return *this;
+  }
+  virtual ~SE3() = default;
+
+  SE3 operator*(const SE3& other) const {
+    SE3 ret = *this;
+    ret.tf_ *= other.tf_;
+    return ret;
+  }
+
+  huron::Vector3<T> operator*(const huron::Vector3<T>& v) const {
+    huron::Vector4<T> v_homogeneous;
+    v_homogeneous << v, 1;
+    return (tf_ * v_homogeneous).template head<3>();
+  }
+
+  SE3& operator*=(const SE3& other) {
+    tf_ *= other.tf_;
+    return *this;
+  }
+
+  inline friend bool operator==(const SE3<double>& lhs, const SE3<double>& rhs);
+  inline friend bool operator!=(const SE3<double>& lhs, const SE3<double>& rhs);
+
+  Matrix4<T>& matrix() { return tf_; }
+  const Matrix4<T>& matrix() const { return tf_; }
+
+  Matrix3<T> rotation() const;
+  Vector3<T> translation() const;
+
+  void Rotate(const Matrix3<T>& R);
+  void Prerotate(const Matrix3<T>& R);
+  void Translate(const Vector3<T>& v);
+
+  SE3<T> Inverse() const;
+
+  huron::Matrix6<T> AdjointAction() const;
+
+ private:
+  Matrix4<T> tf_;
+};
+
+inline bool operator==(const SE3<double>& lhs, const SE3<double>& rhs) {
+  return lhs.tf_ == rhs.tf_;
+}
+inline bool operator!=(const SE3<double>& lhs, const SE3<double>& rhs) {
+  return !(lhs == rhs);
+}
+
+} // namespace huron
+
+HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class huron::SE3)
+HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS(
+    class huron::SE3)

math/include/huron/math/utils.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "huron/types.h"
+
+namespace huron {
+
+template <typename T>
+Matrix3<T> skew(const Vector3<T>& v) {
+  Matrix3<T> ret;
+  ret << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
+  return ret;
+}
+
+}  // namespace huron