diff --git a/CMakeLists.txt b/CMakeLists.txt index 30f7744d..2eeebbfb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,12 @@ cmake_minimum_required(VERSION 3.13) project(grbda VERSION 2.1.0) +if(CMAKE_BUILD_TYPE MATCHES Debug) + message(STATUS "Debug build") + add_compile_options(-O0 -g3) + # add_compile_definitions(EIGEN_INITIALIZE_MATRICES_BY_NAN) +endif() + message(STATUS "============= !Generalized Rigid-Body Dynamics Algorithms! =============") ################################################################################ @@ -18,13 +24,20 @@ option(ASAN "set compile options for Address Sanitizer" OFF) ################################################################################ message(STATUS "======> Find Dependencies") -find_package(Eigen3 REQUIRED) -if(Eigen3_FOUND) - message(STATUS "Eigen3 found") - include_directories(${EIGEN3_INCLUDE_DIR}) - include_directories(${EIGEN3_INCLUDE_DIR}/..) + +set(EIGEN_LOCAL_PATH "${PROJECT_SOURCE_DIR}/grbda-dependencies/eigen-3.4.0") +if(EXISTS "${EIGEN_LOCAL_PATH}/Eigen/StdVector") + message(STATUS "Using local Eigen at ${EIGEN_LOCAL_PATH}") + include_directories(${EIGEN_LOCAL_PATH}) else() - message(FATAL_ERROR "Eigen3 not found. Please install it or specify its location.") + find_package(Eigen3 REQUIRED) + if(Eigen3_FOUND) + message(STATUS "Eigen3 found via find_package") + include_directories(${EIGEN3_INCLUDE_DIR}) + include_directories(${EIGEN3_INCLUDE_DIR}/..) + else() + message(FATAL_ERROR "Eigen3 not found. Please install it or specify its location.") + endif() endif() find_package(casadi REQUIRED) @@ -72,7 +85,11 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};${PROJECT_SOURCE_DIR}/cmake") set(CMAKE_PREFIX_PATH "${CMAKE_PREFIX_PATH};${PROJECT_SOURCE_DIR}/cmake") -set(CMAKE_BUILD_TYPE Release) +# Set default build type if not specified +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + if(CMAKE_BUILD_TYPE STREQUAL "Debug") add_definitions(-DDEBUG_MODE) endif() @@ -87,14 +104,23 @@ elseif(MARCH_NATIVE) endif() set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -ggdb -Wall \ + +# Base warning flags for all builds +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ggdb -Wall \ -Wextra -Wcast-align -Wdisabled-optimization -Wformat=2 \ -Winit-self -Wmissing-include-dirs -Woverloaded-virtual \ -Wshadow -Wsign-promo -Wno-sign-compare -Wno-unused-const-variable \ -Wno-unused-parameter -Wno-unused-variable -Wno-uninitialized") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -ggdb -std=gnu99 -I.") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb -std=gnu99 -I.") + +# Optimization flags per build type (don't override Debug's -O0!) +if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3") +endif() if(ASAN) + message(WARNING "ASAN option is deprecated. Use -DCMAKE_BUILD_TYPE=Debug instead.") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") endif() @@ -170,4 +196,4 @@ endif() if(BUILD_BENCHMARKS) message(STATUS "======> Setup Benchmarks ") add_subdirectory(Benchmarking) -endif() +endif() \ No newline at end of file diff --git a/UnitTests/CMakeLists.txt b/UnitTests/CMakeLists.txt index 415cfef4..e4e632f8 100644 --- a/UnitTests/CMakeLists.txt +++ b/UnitTests/CMakeLists.txt @@ -1,6 +1,11 @@ # Create single executable for all tests file(GLOB_RECURSE test_sources "*.cpp") +# Tests that provide their own main() — link gtest only (no gmock_main) +set(TESTS_WITH_CUSTOM_MAIN + testInverseDynamicsDerivativesComplexStep +) + # Create a test for each test file foreach(test_source ${test_sources}) # Extract the file name without the extension @@ -10,7 +15,11 @@ foreach(test_source ${test_sources}) add_executable(${test_name} ${test_source}) # Link the necessary libraries - target_link_libraries(${test_name} PRIVATE gtest gmock_main ${PROJECT_NAME}) + if(${test_name} IN_LIST TESTS_WITH_CUSTOM_MAIN) + target_link_libraries(${test_name} PRIVATE gtest ${PROJECT_NAME}) + else() + target_link_libraries(${test_name} PRIVATE gtest gmock_main ${PROJECT_NAME}) + endif() # Add the test to CTest add_test(NAME ${test_name} COMMAND ${test_name}) diff --git a/UnitTests/googletest-src/googletest/CMakeLists.txt b/UnitTests/googletest-src/googletest/CMakeLists.txt index 60593131..462ea614 100644 --- a/UnitTests/googletest-src/googletest/CMakeLists.txt +++ b/UnitTests/googletest-src/googletest/CMakeLists.txt @@ -8,6 +8,10 @@ # ctest. You can select which tests to run using 'ctest -R regex'. # For more options, run 'ctest --help'. +if(CMAKE_BUILD_TYPE MATCHES Debug) + add_compile_options(-Wno-maybe-uninitialized) +endif() + # When other libraries are using a shared version of runtime libraries, # Google Test also has to use one. option( diff --git a/UnitTests/testClusterJoints.cpp b/UnitTests/testClusterJoints.cpp index 48b2eef3..4bfe8ed5 100644 --- a/UnitTests/testClusterJoints.cpp +++ b/UnitTests/testClusterJoints.cpp @@ -24,9 +24,9 @@ createBiasVelocityCasadiFunction(std::shared_ptr casadi::copy(cs_qd_sym, qd_sym); // Set state and update kinematics - JointState joint_state(JointCoordinate(q_sym, false), + JointState joint_state(JointCoordinate(q_sym, true), JointCoordinate(qd_sym, false)); - joint_state.position = TestHelpers::plus(joint->type(), q_sym, dq_sym); + joint_state.position = TestHelpers::plus(joint, q_sym, dq_sym); joint->updateKinematics(joint_state); // Differentiate the motion subspace matrix with repsect to q, ∂S/∂dq diff --git a/UnitTests/testClusterTreeModel.cpp b/UnitTests/testClusterTreeModel.cpp index 64a1fab8..fcb576db 100644 --- a/UnitTests/testClusterTreeModel.cpp +++ b/UnitTests/testClusterTreeModel.cpp @@ -1,6 +1,14 @@ #include "gtest/gtest.h" +#include +#include #include "config.h" + +namespace { +struct TestSeedInitializer { + TestSeedInitializer() { std::srand(42); } +} g_test_seed_initializer; +} #include "grbda/Dynamics/ClusterTreeModel.h" #include "grbda/Robots/RobotTypes.h" @@ -106,10 +114,6 @@ std::vector GetTestRobots() std::make_shared>(false)}); test_data.push_back({urdf_directory + "mini_cheetah.urdf", std::make_shared>()}); - test_data.push_back({urdf_directory + "mit_humanoid_leg.urdf", - std::make_shared>()}); - test_data.push_back({urdf_directory + "mit_humanoid.urdf", - std::make_shared>()}); return test_data; } diff --git a/UnitTests/testForwardKinematics.cpp b/UnitTests/testForwardKinematics.cpp index 8b664293..c359093f 100644 --- a/UnitTests/testForwardKinematics.cpp +++ b/UnitTests/testForwardKinematics.cpp @@ -242,14 +242,31 @@ GTEST_TEST(ForwardKinematics, HumanoidModelComparison) for (int i = 0; i < 20; i++) { // Initialize Random States - ModelState<> model_state; + ModelState<> rotor_model_state; + ModelState<> no_rotor_model_state; + for (const auto &cluster : rotor_model.clusters()) { JointState<> joint_state = cluster->joint_->randomJointState(); - model_state.push_back(joint_state); + rotor_model_state.push_back(joint_state); + if( joint_state.position.size() == 4 ) // Revolute Pair with Rotor Joint + { + const DMat conv = cluster->joint_->spanningTreeToIndependentCoordsConversion().cast(); + const DVec ind_pos = conv * joint_state.position; + + // Revolute Pair State + JointState<> rp_state(JointCoordinate(ind_pos, false), + JointCoordinate(joint_state.velocity, false)); + + no_rotor_model_state.push_back(rp_state); + } + else + { + no_rotor_model_state.push_back(joint_state); + } } - rotor_model.setState(model_state); - no_rotor_model.setState(model_state); + rotor_model.setState(rotor_model_state); + no_rotor_model.setState(no_rotor_model_state); // Forward Kinematics rotor_model.forwardKinematicsIncludingContactPoints(); diff --git a/UnitTests/testHelpers.hpp b/UnitTests/testHelpers.hpp index c255c339..8f714e8b 100644 --- a/UnitTests/testHelpers.hpp +++ b/UnitTests/testHelpers.hpp @@ -1,7 +1,17 @@ #ifndef GRBDA_TEST_HELPERS_H #define GRBDA_TEST_HELPERS_H +#include +#include +#include #include "grbda/Dynamics/ClusterTreeModel.h" +#include "grbda/Utils/OrientationTools.h" + +namespace { +struct TestSeedInitializer { + TestSeedInitializer() { std::srand(42); } +} g_test_seed_initializer; +} using namespace grbda; @@ -47,28 +57,39 @@ namespace TestHelpers return generic_model; } - inline DVec plus(ClusterJointTypes joint_type, + inline DVec plusFreeJoint(DVec q, DVec dq) + { + using SX = casadi::SX; + const Vec3 pos = q.head<3>(); + const Quat quat = q.tail<4>(); + const Vec3 dquat = dq.head<3>(); + const Vec3 dpos = dq.tail<3>(); + Vec7 q_plus_dq_vec; + const Mat3 R = ori::quaternionToRotationMatrix(quat); + q_plus_dq_vec.head<3>() = pos + R.transpose() * dpos; + Quat dquat_vec(0, dquat[0], dquat[1], dquat[2]); + // First-order retraction: steps off the quaternion sphere for finite dq. + // Acceptable here because this function is used symbolically (SX) — dq is + // later set to zero and the Jacobian w.r.t. dq is taken, recovering the exact + // derivative without any normalization error. + q_plus_dq_vec.template tail<4>() = quat + 0.5 * ori::quatProduct(quat, dquat_vec); + return q_plus_dq_vec; + } + + inline DVec plus(std::shared_ptr> joint, DVec q, DVec dq) { using SX = casadi::SX; - if (joint_type == ClusterJointTypes::Free) + if (joint->type() == ClusterJointTypes::Free) { - const Vec3 pos = q.head<3>(); - const Quat quat = q.tail<4>(); - - const Vec3 dquat = dq.head<3>(); - const Vec3 dpos = dq.tail<3>(); - - Vec7 q_plus_dq_vec; - - const Mat3 R = ori::quaternionToRotationMatrix(quat); - q_plus_dq_vec.head<3>() = pos + R.transpose() * dpos; - - Quat dquat_vec(0, dquat[0], dquat[1], dquat[2]); - q_plus_dq_vec.template tail<4>() = quat + 0.5 * ori::quatProduct(quat, dquat_vec); - - return q_plus_dq_vec; + return plusFreeJoint(q, dq); + } + else if (q.size() != dq.size()) // implicit constraint: q_span + G(q) * dq_ind + { + auto lc = joint->cloneLoopConstraint(); + lc->updateJacobians(JointCoordinate(q, true)); + return q + lc->G() * dq; } else { @@ -91,7 +112,7 @@ namespace TestHelpers DVec dq_vec(6); dq_vec << dq[0], dq[1], dq[2], dq[3], dq[4], dq[5]; - DVec qfb_plus_dq_vec = plus(ClusterJointTypes::Free, q_vec, dq_vec); + DVec qfb_plus_dq_vec = plusFreeJoint(q_vec, dq_vec); for (int i = 0; i < 7; i++) { @@ -111,6 +132,223 @@ namespace TestHelpers return q_plus_dq_vec; } + // Model-aware retraction for flat DM state vectors. + // Handles free joints (quaternion), implicit constraints (G * dq), and simple joints. + inline std::vector plus(const ClusterTreeModel& model, + const std::vector& q_flat, + const std::vector& dq_flat) + { + using DM = casadi::DM; + using SX = casadi::SX; + + std::vector result = q_flat; + + for (const auto& cluster : model.clusters()) { + const int pos_idx = cluster->position_index_; + const int vel_idx = cluster->velocity_index_; + const int num_pos = cluster->num_positions_; + const int num_vel = cluster->num_velocities_; + + const bool is_fb = (num_pos == 7 && num_vel == 6); + const bool is_implicit = (num_pos > num_vel) && !is_fb; + + if (is_fb) { + DVec q_sx(7), dq_sx(6); + for (int i = 0; i < 7; ++i) q_sx(i) = SX(static_cast(q_flat[pos_idx + i])); + for (int i = 0; i < 6; ++i) dq_sx(i) = SX(static_cast(dq_flat[vel_idx + i])); + DVec q_new_sx = plusFreeJoint(q_sx, dq_sx); + for (int i = 0; i < 7; ++i) + result[pos_idx + i] = DM(static_cast(q_new_sx(i))); + } else if (is_implicit) { + auto lc = cluster->joint_->cloneLoopConstraint(); + auto* generic = dynamic_cast*>(lc.get()); + if (!generic) + throw std::runtime_error("plus: implicit constraint is not GenericImplicit — unhandled type"); + DM q_dm(num_pos, 1); + for (int i = 0; i < num_pos; ++i) + q_dm(i) = static_cast(q_flat[pos_idx + i]); + DM G_dm = generic->getGFcn()(casadi::DMVector{q_dm})[0]; + DM dq_dm(num_vel, 1); + for (int i = 0; i < num_vel; ++i) + dq_dm(i) = static_cast(dq_flat[vel_idx + i]); + DM q_new = q_dm + DM::mtimes(G_dm, dq_dm); + for (int i = 0; i < num_pos; ++i) + result[pos_idx + i] = DM(static_cast(q_new(i))); + + } else { + for (int i = 0; i < num_pos; ++i) + result[pos_idx + i] = q_flat[pos_idx + i] + dq_flat[vel_idx + i]; + } + } + + return result; + } + +} // namespace TestHelpers + +// ─── Lie-group state perturbation helpers (shared by simple and complex-step tests) ─── + +// Normalize quaternion for real scalar types; no-op for complex (to preserve imaginary part). +template +typename std::enable_if::value, void>::type +normalizeQuaternionIfReal(Eigen::MatrixBase& quat) { + const_cast&>(quat).normalize(); +} +template +typename std::enable_if::value, void>::type +normalizeQuaternionIfReal(Eigen::MatrixBase&) {} + +// Retraction map for a single joint cluster's configuration. +// q0 : spanning position (7D for free joint, 1D for revolute, etc.) +// dq : velocity-space perturbation (6D for free joint, 1D for revolute, etc.) +template +DVec lieGroupConfigurationAddition(const DVec& q0, const DVec& dq, bool floating_base) { + if (!floating_base) + return q0 + dq; + + const int nj = (int)dq.size() - 6; + DVec q_new = q0; + q_new.tail(nj) += dq.tail(nj); + + Eigen::Matrix p = q0.head(3); + Eigen::Matrix quat = q0.template segment<4>(3); + Eigen::Matrix omega = dq.head(3); + + bool has_imag = false; + if constexpr (!std::is_arithmetic::value) { + for (int i = 0; i < 3; ++i) + if (std::abs(std::imag(omega[i])) > 1e-30) { has_imag = true; break; } + } + + Eigen::Matrix delta_quat; + if (has_imag) { + delta_quat[0] = T(0.0); + delta_quat.template tail<3>() = omega / T(2.0); + } else { + T theta = omega.norm(); + if (std::abs(theta) < 1e-10) { + delta_quat[0] = T(1.0); + delta_quat.template tail<3>() = omega / T(2.0); + } else { + T ht = theta / T(2.0); + delta_quat[0] = std::cos(ht); + delta_quat.template tail<3>() = std::sin(ht) * omega / theta; + } + } + + Eigen::Matrix quat_new; + if (has_imag) { + T sca = delta_quat[0]; + Eigen::Matrix vec = delta_quat.template tail<3>(); + auto qt = quat.template tail<3>(); + T vdq = (vec.transpose() * qt)(0, 0); + quat_new[0] = (T(1.0) + sca) * quat[0] - vdq; + Eigen::Matrix cross; + cross[0] = qt[1]*vec[2] - qt[2]*vec[1]; + cross[1] = qt[2]*vec[0] - qt[0]*vec[2]; + cross[2] = qt[0]*vec[1] - qt[1]*vec[0]; + quat_new.template tail<3>() = vec*quat[0] + (T(1.0)+sca)*qt + cross; + } else { + quat_new = ori::quatProduct(quat, delta_quat); + normalizeQuaternionIfReal(quat_new); + } + + Eigen::Matrix qn = quat / quat.norm(); + Eigen::Matrix R = ori::quaternionToRotationMatrix(qn); + Eigen::Matrix vb = dq.template segment<3>(3); + q_new.head(3) = p + R.transpose() * vb; + q_new.template segment<4>(3) = quat_new; + return q_new; +} + +// Build a ModelState from flat spanning-coordinate vectors. +// Position JointCoordinates are tagged isSpanning=(np>nv) so toSpanningTreeState +// copies them directly rather than calling gamma(). +template +ModelState makeModelState(const ClusterTreeModel& model, + const DVec& q, const DVec& qd) { + ModelState result; + result.reserve(model.clusters().size()); + int q_off = 0, qd_off = 0; + for (const auto& cluster : model.clusters()) { + const int np = cluster->num_positions_; + const int nv = cluster->num_velocities_; + result.push_back(JointState( + JointCoordinate(q.segment(q_off, np).template cast(), (np > nv)), + JointCoordinate(qd.segment(qd_off, nv).template cast(), false))); + q_off += np; + qd_off += nv; + } + return result; +} + +// Apply a perturbation in minimal (independent) coordinates to a ModelState. +// Floating-base clusters : Lie group retraction via lieGroupConfigurationAddition. +// Implicit-constraint clusters : G-based spanning perturbation (dq_span = G * dq_ind). +// Simple joints : direct addition. +// dq / dqd are flat vectors in independent coordinates across all clusters. +template +ModelState applyMinimalPerturbation(const ClusterTreeModel& model_ref, + const ModelState& state, + const DVec& dq, const DVec& dqd) { + ModelState result; + result.reserve(state.size()); + int off = 0; + for (size_t c = 0; c < model_ref.clusters().size(); ++c) { + const auto& cluster = model_ref.clusters()[c]; + const int np = cluster->num_positions_; + const int nv = cluster->num_velocities_; + const bool is_fb = (np == 7 && nv == 6); + const bool is_implicit = (np > nv) && !is_fb; + + DVec new_pos(state[c].position); + DVec new_vel = DVec(state[c].velocity) + dqd.segment(off, nv); + + if (is_fb) { + new_pos = lieGroupConfigurationAddition( + DVec(state[c].position), DVec(dq.segment(off, nv)), true); + } else if (is_implicit) { + DVec q_real(np); + for (int k = 0; k < np; ++k) q_real(k) = std::real(state[c].position[k]); + auto lc = cluster->joint_->cloneLoopConstraint(); + lc->updateJacobians(JointCoordinate(q_real, true)); + new_pos += lc->G().template cast() * DVec(dq.segment(off, nv)); + } else { + new_pos += dq.segment(off, nv); + } + + result.push_back(JointState( + JointCoordinate(new_pos, state[c].position.isSpanning()), + JointCoordinate(new_vel, state[c].velocity.isSpanning()))); + off += nv; + } + return result; +} + +// Five-point stencil finite-difference Jacobian. +// f'(x) ≈ [-f(x+2h) + 8f(x+h) - 8f(x-h) + f(x-2h)] / (12h) +template +Eigen::MatrixXd finiteDifferenceJacobian(Func func, const Eigen::VectorXd& point, double h) { + const int n = point.size(); + Eigen::VectorXd f0 = func(point); + const int m = f0.size(); + Eigen::MatrixXd jacobian(m, n); + for (int i = 0; i < n; ++i) { + Eigen::VectorXd p1 = point, p2 = point, p3 = point, p4 = point; + p1[i] += 2*h; p2[i] += h; p3[i] -= h; p4[i] -= 2*h; + jacobian.col(i) = (-func(p1) + 8*func(p2) - 8*func(p3) + func(p4)) / (12*h); + } + return jacobian; +} + +// Build a random ModelState from each cluster's randomJointState(). +// Pass enforce_constraints=true for models with loop constraints. +inline ModelState randomModelState(const ClusterTreeModel& model, + bool enforce_constraints = false) { + ModelState state; + for (const auto& c : model.clusters()) + state.push_back(c->joint_->randomJointState(enforce_constraints)); + return state; } #endif // GRBDA_TEST_HELPERS_H diff --git a/UnitTests/testInverseDynamicsDerivativesComplexStep.cpp b/UnitTests/testInverseDynamicsDerivativesComplexStep.cpp new file mode 100644 index 00000000..95541307 --- /dev/null +++ b/UnitTests/testInverseDynamicsDerivativesComplexStep.cpp @@ -0,0 +1,602 @@ + +// --- IMPLICIT CONSTRAINT COMPLEX-STEP TESTS --- +#include "grbda/Robots/TelloWithArms.hpp" +#include "grbda/Robots/Tello.hpp" +#include "grbda/Dynamics/ClusterJoints/GenericJoint.h" +#include "grbda/Robots/PlanarLegLinkage.hpp" +#include +#include +#include +#include +#include +#include +#include "gtest/gtest.h" +#include "grbda/Dynamics/ClusterTreeModel.h" +#include "grbda/Robots/RobotTypes.h" +#include "testHelpers.hpp" +#include "config.h" + +using namespace grbda; + +// ── CSV output ────────────────────────────────────────────────────────────── + +struct SummaryRow { + std::string robot_name; + int dof; + double max_err_dq; + double max_err_dqdot; +}; + +struct PerJointRow { + std::string robot_name; + std::string cluster_name; + int joint_idx; + double err_dq; + double err_dqdot; +}; + +struct CsvAccumulator { + std::vector summary; + std::vector per_joint; + + static CsvAccumulator& get() { + static CsvAccumulator inst; + return inst; + } + + void flush() const { + const std::string base = std::string(SOURCE_DIRECTORY) + "/Benchmarking/data/"; + + std::ofstream fs(base + "accuracy_summary.csv"); + if (fs.is_open()) { + fs << "robot_name,dof,max_err_dq,max_err_dqdot\n"; + fs << std::scientific << std::setprecision(6); + for (const auto& r : summary) + fs << r.robot_name << "," << r.dof << "," + << r.max_err_dq << "," << r.max_err_dqdot << "\n"; + } + + std::ofstream fj(base + "minicheetah_per_joint.csv"); + if (fj.is_open()) { + fj << "robot_name,cluster_name,joint_idx,err_dq,err_dqdot\n"; + fj << std::scientific << std::setprecision(6); + for (const auto& r : per_joint) + fj << r.robot_name << "," << r.cluster_name << "," + << r.joint_idx << "," << r.err_dq << "," << r.err_dqdot << "\n"; + } + } +}; + +class CsvWriteEnvironment : public ::testing::Environment { +public: + void TearDown() override { CsvAccumulator::get().flush(); } +}; + +// ── Helper ─────────────────────────────────────────────────────────────────── + +// Helper function for complex-step differentiation +// Generic complex-step derivative test. Caller is responsible for: +// - building both model_real and model_complex with matching structure +// - setting a valid state on model_real before calling +// Uses makeModelState / applyMinimalPerturbation so isSpanning() flags are always correct. +void testInverseDynamicsDerivativesComplexStep( + ClusterTreeModel& model_real, + ClusterTreeModel>& model_complex, + const std::string& robot_name, + double tol_dq = 1e-12, + double tol_dqdot = 1e-12, + bool record_per_joint = false) { + std::cout << std::setprecision(16); + const int nDOF = model_real.getNumDegreesOfFreedom(); + std::cout << "\n========================================\n"; + std::cout << "Complex-Step Derivative Test: " << robot_name << " (DOF=" << nDOF << ")\n"; + std::cout << "========================================\n\n"; + + const DVec ydd_real = DVec::Random(nDOF); + + auto [dtau_dq, dtau_dqdot] = model_real.firstOrderInverseDynamicsDerivatives(ydd_real); + auto [q0, qd0] = model_real.getState(); + + const ModelState> state_complex0 = makeModelState>(model_real, q0, qd0); + const ModelState state_real_base = makeModelState(model_real, q0, qd0); + const DVec> zero_dq = DVec>::Zero(nDOF); + const DVec zero_dqr = DVec::Zero(nDOF); + const DVec> ydd_complex = ydd_real.cast>(); + + auto ID_of_dq_cs = [&](const DVec& dq) -> DVec { + DVec> dq_c = dq.cast>() * std::complex(0.0, 1.0); + model_complex.setState(applyMinimalPerturbation(model_real, state_complex0, dq_c, zero_dq), false); + return model_complex.inverseDynamics(ydd_complex).imag(); + }; + auto ID_of_dqdot_cs = [&](const DVec& dqdot) -> DVec { + DVec> dqdot_c = dqdot.cast>() * std::complex(0.0, 1.0); + model_complex.setState(applyMinimalPerturbation(model_real, state_complex0, zero_dq, dqdot_c), false); + return model_complex.inverseDynamics(ydd_complex).imag(); + }; + auto ID_of_dq_fd = [&](const DVec& dq) -> DVec { + model_real.setState(applyMinimalPerturbation(model_real, state_real_base, dq, zero_dqr), false); + return model_real.inverseDynamics(ydd_real); + }; + auto ID_of_dqdot_fd = [&](const DVec& dqdot) -> DVec { + model_real.setState(applyMinimalPerturbation(model_real, state_real_base, zero_dqr, dqdot), false); + return model_real.inverseDynamics(ydd_real); + }; + + const double h_cs = 1e-20, h_fd = 1e-7; + DMat dtau_dq_cs = finiteDifferenceJacobian(ID_of_dq_cs, zero_dqr, h_cs); + DMat dtau_dqdot_cs = finiteDifferenceJacobian(ID_of_dqdot_cs, zero_dqr, h_cs); + DMat dtau_dq_fd = finiteDifferenceJacobian(ID_of_dq_fd, zero_dqr, h_fd); + DMat dtau_dqdot_fd = finiteDifferenceJacobian(ID_of_dqdot_fd, zero_dqr, h_fd); + + double max_error_dq = (dtau_dq - dtau_dq_cs).cwiseAbs().maxCoeff(); + double max_error_dqdot = (dtau_dqdot - dtau_dqdot_cs).cwiseAbs().maxCoeff(); + double max_cs_fd_dq = (dtau_dq_cs - dtau_dq_fd).cwiseAbs().maxCoeff(); + double max_cs_fd_dqdot = (dtau_dqdot_cs - dtau_dqdot_fd).cwiseAbs().maxCoeff(); + + std::cout << "Max CS vs analytical error (dtau/dq): " << max_error_dq << "\n"; + std::cout << "Max CS vs analytical error (dtau/dqdot): " << max_error_dqdot << "\n"; + std::cout << "Max CS vs FD error (dtau/dq): " << max_cs_fd_dq << "\n"; + std::cout << "Max CS vs FD error (dtau/dqdot): " << max_cs_fd_dqdot << "\n"; + + if( max_error_dq > tol_dq) { + std::cout << "Details for dtau/dq error:\n"; + std::cerr << "Analytical derivatives:\n"; + std::cerr << "dtau/dq:\n" << dtau_dq << "\n"; + std::cerr << "Finite difference derivatives (CS):\n"; + std::cerr << "dtau/dq (CS):\n" << dtau_dq_cs << "\n"; + + std::cerr << "Error (boolean):\n"; + Eigen::Matrix out_of_tol = + (dtau_dq - dtau_dq_cs).array().abs() > tol_dq; + std::cerr << out_of_tol << "\n"; + + } + + if (max_error_dqdot > tol_dqdot) { + std::cout << "Details for dtau/dqdot error:\n"; + std::cerr << "Analytical derivatives:\n"; + std::cerr << "dtau/dqdot:\n" << dtau_dqdot << "\n"; + std::cerr << "Finite difference derivatives (CS):\n"; + std::cerr << "dtau/dqdot (CS):\n" << dtau_dqdot_cs << "\n"; + + std::cerr << "Error (boolean):\n"; + Eigen::Matrix out_of_tol = + (dtau_dqdot - dtau_dqdot_cs).array().abs() > tol_dqdot; + std::cerr << out_of_tol << "\n"; + } + + + // Record summary row + CsvAccumulator::get().summary.push_back({robot_name, nDOF, max_error_dq, max_error_dqdot}); + + // Record per-joint rows (column-wise max of the error matrix across all tau outputs) + if (record_per_joint) { + int dof_idx = 0; + for (const auto& cluster : model_real.clusters()) { + const int nv = cluster->num_velocities_; + for (int k = 0; k < nv; ++k, ++dof_idx) { + CsvAccumulator::get().per_joint.push_back({ + robot_name, + cluster->name_, + dof_idx, + (dtau_dq - dtau_dq_cs ).col(dof_idx).cwiseAbs().maxCoeff(), + (dtau_dqdot - dtau_dqdot_cs).col(dof_idx).cwiseAbs().maxCoeff() + }); + } + } + } + + EXPECT_LT(max_cs_fd_dq, 5e-5) << "CS vs FD mismatch (dtau/dq)"; + EXPECT_LT(max_cs_fd_dqdot, 5e-5) << "CS vs FD mismatch (dtau/dqdot)"; + EXPECT_LT(max_error_dq, tol_dq) << "dtau/dq error exceeds tolerance"; + EXPECT_LT(max_error_dqdot, tol_dqdot) << "dtau/dqdot error exceeds tolerance"; +} + +// Helper: infer revolute axis from the first 6 rows of a motion subspace column. +static ori::CoordinateAxis axisFromS(const DMat& S, int col = 0) +{ + if (std::abs(S(0, col)) > 0.9) return ori::CoordinateAxis::X; + if (std::abs(S(1, col)) > 0.9) return ori::CoordinateAxis::Y; + if (std::abs(S(2, col)) > 0.9) return ori::CoordinateAxis::Z; + throw std::runtime_error("cloneToComplex: non-axis-aligned revolute joint"); +} + +// Helper: cast a double Body to complex, rewriting its parent name via src.bodies(). +static SpatialInertia> castInertia(const Body& b) +{ + using CD = std::complex; + return SpatialInertia(CD(b.inertia_.getMass()), + b.inertia_.getCOM().cast(), + b.inertia_.getInertiaTensor().cast()); +} +static spatial::Transform> castXtree(const Body& b) +{ + using CD = std::complex; + return spatial::Transform(b.Xtree_.getRotation().cast(), + b.Xtree_.getTranslation().cast()); +} +static std::string parentName(const Body& b, const ClusterTreeModel& src) +{ + return b.parent_index_ < 0 ? "ground" : src.bodies()[b.parent_index_].name_; +} + +// Build a complex-scalar copy of a real model by reconstructing each cluster. +// Supports Revolute, RevoluteWithRotor, and RevoluteTripleWithRotor clusters. +ClusterTreeModel> cloneToComplex(const ClusterTreeModel& src) { + using CD = std::complex; + using namespace ClusterJoints; + ClusterTreeModel dst; + + for (const auto& cluster : src.clusters()) { + const auto& bodies = cluster->bodies(); + const auto jtype = cluster->joint_->type(); + const auto& G = cluster->joint_->G(); // loop constraint G matrix + const auto& S = cluster->S(); + + if (jtype == ClusterJointTypes::Revolute) + { + // Single-body, single-DOF revolute cluster + const auto& b = bodies[0]; + dst.template appendBody>( + b.name_, castInertia(b), parentName(b, src), castXtree(b), + axisFromS(S)); + } + else if (jtype == ClusterJointTypes::RevoluteWithRotor) + { + // 2-body cluster: bodies[0]=link, bodies[1]=rotor. + // G is 2x1: G(0)=1, G(1)=gear_ratio. + const auto& link = bodies[0]; + const auto& rotor = bodies[1]; + const double gear_ratio = G(1, 0); + + Body link_c = dst.registerBody(link.name_, castInertia(link), + parentName(link, src), castXtree(link)); + Body rotor_c = dst.registerBody(rotor.name_, castInertia(rotor), + parentName(rotor, src), castXtree(rotor)); + + // Axes from singleJoints(): [link_joint, rotor_joint] + auto getRevAxis = [](const std::shared_ptr>& j) { + auto* rev = dynamic_cast*>(j.get()); + if (!rev) throw std::runtime_error("cloneToComplex: expected revolute joint"); + return rev->getAxis(); + }; + const auto sj = cluster->joint_->singleJoints(); + const ori::CoordinateAxis link_axis = getRevAxis(sj[0]); + const ori::CoordinateAxis rotor_axis = getRevAxis(sj[1]); + + GearedTransmissionModule mod{link_c, rotor_c, + link.name_ + "_joint", + rotor.name_ + "_joint", + link_axis, rotor_axis, + CD(gear_ratio)}; + dst.template appendRegisteredBodiesAsCluster>( + cluster->name_, mod); + } + else if (jtype == ClusterJointTypes::RevoluteTripleWithRotor) + { + // 6-body cluster: [link1, link2, link3, rotor1, rotor2, rotor3]. + // G is 6x3: top 3x3 = I, bottom 3x3 encodes gear*belt products. + // For module i (0-indexed), the bottom row i of G gives the cumulative + // gear*belt products for belts 0..i. We set gear_ratio=1 and recover + // belt_ratios from the row: belt[0]=G(3+i,0), belt[k]=G(3+i,k)/G(3+i,k-1). + const auto& link1 = bodies[0]; + const auto& link2 = bodies[1]; + const auto& link3 = bodies[2]; + const auto& rotor1 = bodies[3]; + const auto& rotor2 = bodies[4]; + const auto& rotor3 = bodies[5]; + + Body link1_c = dst.registerBody(link1.name_, castInertia(link1), parentName(link1, src), castXtree(link1)); + Body link2_c = dst.registerBody(link2.name_, castInertia(link2), parentName(link2, src), castXtree(link2)); + Body link3_c = dst.registerBody(link3.name_, castInertia(link3), parentName(link3, src), castXtree(link3)); + Body rotor1_c = dst.registerBody(rotor1.name_, castInertia(rotor1), parentName(rotor1, src), castXtree(rotor1)); + Body rotor2_c = dst.registerBody(rotor2.name_, castInertia(rotor2), parentName(rotor2, src), castXtree(rotor2)); + Body rotor3_c = dst.registerBody(rotor3.name_, castInertia(rotor3), parentName(rotor3, src), castXtree(rotor3)); + + // Axes from singleJoints(): [link1, link2, link3, rotor1, rotor2, rotor3] + auto getRevAxis = [](const std::shared_ptr>& j) { + auto* rev = dynamic_cast*>(j.get()); + if (!rev) throw std::runtime_error("cloneToComplex: expected revolute joint"); + return rev->getAxis(); + }; + const auto sj = cluster->joint_->singleJoints(); + const ori::CoordinateAxis ax1 = getRevAxis(sj[0]); + const ori::CoordinateAxis ax2 = getRevAxis(sj[1]); + const ori::CoordinateAxis ax3 = getRevAxis(sj[2]); + const ori::CoordinateAxis rax1 = getRevAxis(sj[3]); + const ori::CoordinateAxis rax2 = getRevAxis(sj[4]); + const ori::CoordinateAxis rax3 = getRevAxis(sj[5]); + + // Extract belt products from G rows 3,4,5. + // Module 1 (1 belt): G(3,0) = g1*b1 + // Module 2 (2 belts): G(4,0), G(4,1) = g2*b1, g2*b1*b2 + // Module 3 (3 belts): G(5,0..2) = g3*b1, g3*b1*b2, g3*b1*b2*b3 + // We set gear_ratio=1 and reconstruct belt_ratios so that + // beltMatrixRowFromBeltRatios(belt_ratios) == G.row(3+i).head(i+1). + // belt[0]=G(3+i,0), belt[k]=G(3+i,k)/G(3+i,k-1) for k>0. + Eigen::Matrix br1; + br1(0) = CD(G(3, 0)); + + Eigen::Matrix br2; + br2(0) = CD(G(4, 0)); + br2(1) = CD(G(4, 1) / G(4, 0)); + + Eigen::Matrix br3; + br3(0) = CD(G(5, 0)); + br3(1) = CD(G(5, 1) / G(5, 0)); + br3(2) = CD(G(5, 2) / G(5, 1)); + + ParallelBeltTransmissionModule<1, CD> mod1{link1_c, rotor1_c, ax1, rax1, CD(1.), br1}; + ParallelBeltTransmissionModule<2, CD> mod2{link2_c, rotor2_c, ax2, rax2, CD(1.), br2}; + ParallelBeltTransmissionModule<3, CD> mod3{link3_c, rotor3_c, ax3, rax3, CD(1.), br3}; + + dst.template appendRegisteredBodiesAsCluster>( + cluster->name_, mod1, mod2, mod3); + } + else + { + throw std::runtime_error( + "cloneToComplex: unsupported cluster type (nb=" + + std::to_string(bodies.size()) + ")"); + } + } + + return dst; +} + +TEST(InverseDynamicsDerivativesComplexStep, TwoLinkChain) { + RevoluteChainWithAndWithoutRotor<0, 2> robot(true); + ClusterTreeModel model_real = robot.buildClusterTreeModel(); + ClusterTreeModel> model_complex = cloneToComplex(model_real); + model_real.setState(randomModelState(model_real)); + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "2-link revolute chain"); +} + +TEST(InverseDynamicsDerivativesComplexStep, ThreeLinkChain) { + RevoluteChainWithAndWithoutRotor<0, 3> robot(true); + ClusterTreeModel model_real = robot.buildClusterTreeModel(); + ClusterTreeModel> model_complex = cloneToComplex(model_real); + model_real.setState(randomModelState(model_real)); + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "3-link revolute chain"); +} + +TEST(InverseDynamicsDerivativesComplexStep, FourLinkChain) { + RevoluteChainWithAndWithoutRotor<0, 4> robot(true); + ClusterTreeModel model_real = robot.buildClusterTreeModel(); + ClusterTreeModel> model_complex = cloneToComplex(model_real); + model_real.setState(randomModelState(model_real)); + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "4-link revolute chain"); +} + +template +ClusterTreeModel buildSimpleFBWithRotorModel() { + using namespace ClusterJoints; + ClusterTreeModel m; + SpatialInertia fb_in(1.0, Vec3(0,0,0), Mat3::Identity()*0.01); + Body fb = m.registerBody("floating_base", fb_in, "ground", spatial::Transform()); + m.template appendRegisteredBodiesAsCluster>("floating_base",fb,"fb_joint"); + SpatialInertia lk_in(0.5, Vec3(0.1,0,0), Mat3::Identity()*0.005); + SpatialInertia rt_in(0.05,Vec3(0,0,0), Mat3::Identity()*0.0001); + spatial::Transform Xl(Mat3::Identity(), Vec3(0,0,0.5)); + Body lk = m.registerBody("link1", lk_in, "floating_base", Xl); + Body rt = m.registerBody("rotor1", rt_in, "floating_base", Xl); + GearedTransmissionModule mod{lk, rt, "link1_joint","rotor1_joint", + ori::CoordinateAxis::Z, ori::CoordinateAxis::Z, S(6.0)}; + m.template appendRegisteredBodiesAsCluster>("joint1", mod); + return m; +} + +TEST(InverseDynamicsDerivativesComplexStep, SimpleFloatingBaseWithRotor) { + typedef std::complex CD; + + ClusterTreeModel model_real = buildSimpleFBWithRotorModel(); + ClusterTreeModel model_complex = buildSimpleFBWithRotorModel(); + + model_real.setState(randomModelState(model_real)); + + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "Simple Floating Base + 1 Revolute With Rotor"); +} + +template +ClusterTreeModel buildSimpleFBModel() { + using namespace ClusterJoints; + ClusterTreeModel m; + SpatialInertia fb_in(1.0, Vec3(0,0,0), Mat3::Identity()*0.01); + Body fb = m.registerBody("floating_base", fb_in, "ground", spatial::Transform()); + m.template appendRegisteredBodiesAsCluster>("floating_base",fb,"fb_joint"); + SpatialInertia lk_in(0.5, Vec3(0.1,0,0), Mat3::Identity()*0.005); + spatial::Transform Xl(Mat3::Identity(), Vec3(0,0,0.5)); + Body lk = m.registerBody("link1", lk_in, "floating_base", Xl); + m.template appendRegisteredBodiesAsCluster>("link1", lk, ori::CoordinateAxis::Z, "link1_joint"); + return m; +} + +TEST(InverseDynamicsDerivativesComplexStep, SimpleFloatingBase) { + typedef std::complex CD; + + ClusterTreeModel model_real = buildSimpleFBModel(); + ClusterTreeModel model_complex = buildSimpleFBModel(); + + model_real.setState(randomModelState(model_real)); + + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "Simple Floating Base + 1 Revolute"); +} + +TEST(InverseDynamicsDerivativesComplexStep, MiniCheetahQuaternion) { + MiniCheetah robot_real; + MiniCheetah, ori_representation::Quaternion> robot_complex; + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + ClusterTreeModel> model_complex = robot_complex.buildClusterTreeModel(); + + model_real.setState(randomModelState(model_real)); + + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "MiniCheetah (Quaternion)", + /*tol_dq=*/1e-12, /*tol_dqdot=*/1e-12, /*record_per_joint=*/true); +} + +template class RobotType, typename OriRep> +void testDirectTemplateApproach(const std::string& robot_name) { + std::cout << "\n========================================\n"; + std::cout << "Testing Direct Template Approach for " << robot_name << "\n"; + std::cout << "========================================\n"; + + // Build both models directly from the templated robot class + RobotType robot_real; + RobotType, OriRep> robot_complex; + + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + ClusterTreeModel> model_complex = robot_complex.buildClusterTreeModel(); + + + // Verify they match + EXPECT_EQ(model_real.clusters().size(), model_complex.clusters().size()); + EXPECT_EQ(model_real.bodies().size(), model_complex.bodies().size()); + EXPECT_EQ(model_real.getNumDegreesOfFreedom(), model_complex.getNumDegreesOfFreedom()); + + // Compare G matrices for each cluster + bool all_g_matrices_match = true; + for (size_t i = 0; i < model_real.clusters().size(); ++i) { + const auto& cluster_real = model_real.cluster(i); + const auto& cluster_complex = model_complex.cluster(i); + + const DMat& G_real = cluster_real->joint_->G(); + const DMat>& G_complex = cluster_complex->joint_->G(); + + if (G_real.rows() != G_complex.rows() || G_real.cols() != G_complex.cols()) { + all_g_matrices_match = false; + std::cout << " Cluster " << i << ": G matrix size mismatch!\n"; + continue; + } + + double max_diff = 0.0; + for (int r = 0; r < G_real.rows(); ++r) { + for (int c = 0; c < G_real.cols(); ++c) { + double diff = std::abs(G_real(r,c) - G_complex(r,c).real()); + max_diff = std::max(max_diff, diff); + } + } + } + + + std::cout << "========================================\n"; + EXPECT_TRUE(all_g_matrices_match); +} + +TEST(InverseDynamicsDerivativesComplexStep, DirectTemplateApproachMiniCheetah) { + testDirectTemplateApproach("MiniCheetah"); +} + +TEST(InverseDynamicsDerivativesComplexStep, DirectTemplateApproachMITHumanoid) { + testDirectTemplateApproach("MIT_Humanoid"); +} + +TEST(InverseDynamicsDerivativesComplexStep, MITHumanoidQuaternion) { + MIT_Humanoid robot_real; + MIT_Humanoid, ori_representation::Quaternion> robot_complex; + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + ClusterTreeModel> model_complex = robot_complex.buildClusterTreeModel(); + + model_real.setState(randomModelState(model_real,true)); + + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "MIT Humanoid (Quaternion)", 1e-12, 1e-13); +} + +TEST(InverseDynamicsDerivativesComplexStep, KukaLWR) { + ClusterTreeModel model_real(SOURCE_DIRECTORY "/robot-models/kuka_lwr_4plus.urdf"); + ClusterTreeModel> model_complex = cloneToComplex(model_real); + + model_real.setState(randomModelState(model_real)); + + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "KUKA LWR 4+"); +} + +TEST(InverseDynamicsDerivativesComplexStep, TeleopArm) { + TeleopArm robot_real; + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + ClusterTreeModel> model_complex = cloneToComplex(model_real); + + ASSERT_EQ(model_real.getNumDegreesOfFreedom(), 7); + + model_real.setState(randomModelState(model_real)); + + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "TeleopArm"); +} +TEST(InverseDynamicsDerivativesComplexStep, TelloImplicitConstraint) { + Tello robot_real; + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + model_real.setState(randomModelState(model_real, true), true); + const int nDOF = model_real.getNumDegreesOfFreedom(); + DVec tau = model_real.inverseDynamics(DVec::Zero(nDOF)); + EXPECT_GE(tau.norm(), 0.0); +} + +TEST(InverseDynamicsDerivativesComplexStep, TelloWithArmsImplicitConstraint) { + TelloWithArms robot_real; + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + model_real.setState(randomModelState(model_real, true), true); + const int nDOF = model_real.getNumDegreesOfFreedom(); + DVec tau = model_real.inverseDynamics(DVec::Zero(nDOF)); + EXPECT_GE(tau.norm(), 0.0); +} + +TEST(InverseDynamicsDerivativesComplexStep, PlanarLegLinkageImplicitConstraint) { + PlanarLegLinkage robot_real; + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + model_real.setState(randomModelState(model_real, true), true); + const int nDOF = model_real.getNumDegreesOfFreedom(); + DVec tau_real = model_real.inverseDynamics(DVec::Zero(nDOF)); + EXPECT_GE(tau_real.norm(), 0.0); +} + +TEST(InverseDynamicsDerivativesComplexStep, TelloImplicitConstraintDerivatives) { + Tello robot_real; + Tello> robot_complex; + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + ClusterTreeModel> model_complex = robot_complex.buildClusterTreeModel(); + + model_real.setState(randomModelState(model_real, true), true); + + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "Tello (ImplicitConstraint)", 1e-13, 1e-14); +} + +TEST(InverseDynamicsDerivativesComplexStep, PlanarLegLinkageImplicitConstraintDerivatives) { + PlanarLegLinkage robot_real; + PlanarLegLinkage> robot_complex; + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + ClusterTreeModel> model_complex = robot_complex.buildClusterTreeModel(); + + ASSERT_EQ(model_real.getNumDegreesOfFreedom(), 2); + model_real.setState(randomModelState(model_real, true), true); + + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "PlanarLegLinkage (ImplicitConstraint)", 1e-12, 1e-14); +} + +TEST(InverseDynamicsDerivativesComplexStep, CassieClosedChainDerivatives) { + Cassie robot_real; + Cassie> robot_complex; + ClusterTreeModel model_real = robot_real.buildClusterTreeModel(); + ClusterTreeModel> model_complex = robot_complex.buildClusterTreeModel(); + + model_real.setState(randomModelState(model_real, true), true); + + testInverseDynamicsDerivativesComplexStep( + model_real, model_complex, "Cassie (Closed Chain)", 1e-12, 1e-13); +} + + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + ::testing::AddGlobalTestEnvironment(new CsvWriteEnvironment); + return RUN_ALL_TESTS(); +} diff --git a/UnitTests/testInverseDynamicsDerivativesSimple.cpp b/UnitTests/testInverseDynamicsDerivativesSimple.cpp new file mode 100644 index 00000000..d435799d --- /dev/null +++ b/UnitTests/testInverseDynamicsDerivativesSimple.cpp @@ -0,0 +1,178 @@ +#include +#include +#include +#include +#include +#include "gtest/gtest.h" +#include "grbda/Dynamics/ClusterTreeModel.h" +#include "grbda/Robots/RobotTypes.h" +#include +#include "testHelpers.hpp" + +using namespace grbda; + + + + +void testInverseDynamicsDerivativesFiniteDifference( + ClusterTreeModel& model_real, + const std::string& robot_name, + double tol_dq = 1e-6, + double tol_dqdot = 1e-6) { + std::cout << std::setprecision(16); + const int nDOF = model_real.getNumDegreesOfFreedom(); + std::cout << "\n========================================\n"; + std::cout << "Finite-Difference Derivative Test: " << robot_name << " (DOF=" << nDOF << ")\n"; + std::cout << "========================================\n\n"; + + const DVec ydd_real = DVec::Random(nDOF); + + auto [dtau_dq, dtau_dqdot] = model_real.firstOrderInverseDynamicsDerivatives(ydd_real); + auto [q0, qd0] = model_real.getState(); + + const ModelState state_real_base = makeModelState(model_real, q0, qd0); + const DVec zero_dqr = DVec::Zero(nDOF); + + auto ID_of_dq_fd = [&](const DVec& dq) -> DVec { + model_real.setState(applyMinimalPerturbation(model_real, state_real_base, dq, zero_dqr), false); + return model_real.inverseDynamics(ydd_real); + }; + auto ID_of_dqdot_fd = [&](const DVec& dqdot) -> DVec { + model_real.setState(applyMinimalPerturbation(model_real, state_real_base, zero_dqr, dqdot), false); + return model_real.inverseDynamics(ydd_real); + }; + + const double h_fd = 1e-7; + DMat dtau_dq_fd = finiteDifferenceJacobian(ID_of_dq_fd, zero_dqr, h_fd); + DMat dtau_dqdot_fd = finiteDifferenceJacobian(ID_of_dqdot_fd, zero_dqr, h_fd); + + double max_error_dq = (dtau_dq - dtau_dq_fd).cwiseAbs().maxCoeff(); + double max_error_dqdot = (dtau_dqdot - dtau_dqdot_fd).cwiseAbs().maxCoeff(); + + std::cout << "Max FD vs analytical error (dtau/dq): " << max_error_dq << "\n"; + std::cout << "Max FD vs analytical error (dtau/dqdot): " << max_error_dqdot << "\n"; + + if( max_error_dq > tol_dq) { + std::cout << "Details for dtau/dq error:\n"; + std::cerr << "Analytical derivatives:\n"; + std::cerr << "dtau/dq:\n" << dtau_dq << "\n"; + std::cerr << "Finite difference derivatives:\n"; + std::cerr << "dtau/dq (FD):\n" << dtau_dq_fd << "\n"; + + std::cerr << "Error (boolean):\n"; + Eigen::Matrix out_of_tol = + (dtau_dq - dtau_dq_fd).array().abs() > tol_dq; + std::cerr << out_of_tol << "\n"; + + } + + if (max_error_dqdot > tol_dqdot) { + std::cout << "Details for dtau/dqdot error:\n"; + std::cerr << "Analytical derivatives:\n"; + std::cerr << "dtau/dqdot:\n" << dtau_dqdot << "\n"; + std::cerr << "Finite difference derivatives:\n"; + std::cerr << "dtau/dqdot (FD):\n" << dtau_dqdot_fd << "\n"; + + std::cerr << "Error (boolean):\n"; + Eigen::Matrix out_of_tol = + (dtau_dqdot - dtau_dqdot_fd).array().abs() > tol_dqdot; + std::cerr << out_of_tol << "\n"; + } + + EXPECT_LT(max_error_dq, tol_dq) << "dtau/dq error exceeds tolerance"; + EXPECT_LT(max_error_dqdot, tol_dqdot) << "dtau/dqdot error exceeds tolerance"; +} + + +TEST(InverseDynamicsDerivatives, TelloWithArmsImplicitConstraint) { + using namespace grbda; + TelloWithArms robot; + ClusterTreeModel model = robot.buildClusterTreeModel(); + constexpr bool enforce_constraints = true; + model.setState(randomModelState(model,enforce_constraints)); + testInverseDynamicsDerivativesFiniteDifference(model, "TelloWithArms", 1e-6, 1e-6); +} + +TEST(InverseDynamicsDerivatives, TwoLinkChain) { + // RevoluteChainWithAndWithoutRotor where N=rotors, M=no rotors + // So <0, 2> means 0 with rotors, 2 without rotors = 2 DOF + // NOTE: Random parameters include random rotation axes and transforms + RevoluteChainWithAndWithoutRotor<0, 2> robot(true); // use random parameters + ClusterTreeModel model = robot.buildClusterTreeModel(); + // Tolerance relaxed to 1e-5 due to finite difference truncation error with h=1e-6 + model.setState(randomModelState(model)); + testInverseDynamicsDerivativesFiniteDifference(model, "2-link revolute chain (random geometry)", 1e-5, 1e-5); +} + + +TEST(InverseDynamicsDerivatives, ThreeLinkChain) { + // RevoluteChainWithAndWithoutRotor where N=rotors, M=no rotors + // So <0, 3> means 0 with rotors, 3 without rotors = 3 DOF + // NOTE: Random parameters include random rotation axes and transforms + RevoluteChainWithAndWithoutRotor<0, 3> robot(true); // use random parameters + ClusterTreeModel model = robot.buildClusterTreeModel(); + model.setState(randomModelState(model)); + testInverseDynamicsDerivativesFiniteDifference(model, "3-link revolute chain (random geometry)", 1e-5, 1e-5); +} + +TEST(InverseDynamicsDerivatives, FourLinkChain) { + // RevoluteChainWithAndWithoutRotor where N=rotors, M=no rotors + // So <0, 4> means 0 with rotors, 4 without rotors = 4 DOF + // NOTE: Random parameters include random rotation axes and transforms + RevoluteChainWithAndWithoutRotor<0, 4> robot(true); // use random parameters + ClusterTreeModel model = robot.buildClusterTreeModel(); + model.setState(randomModelState(model)); + testInverseDynamicsDerivativesFiniteDifference(model, "4-link revolute chain (random geometry)", 2e-5, 2e-5); +} + + +TEST(InverseDynamicsDerivatives, MiniCheetahQuaternion) { + MiniCheetah robot; + ClusterTreeModel model = robot.buildClusterTreeModel(); + model.setState(randomModelState(model)); + testInverseDynamicsDerivativesFiniteDifference(model, "MiniCheetah (Quaternion)", 1e-4, 1e-5); +} + +TEST(InverseDynamicsDerivatives, MITHumanoidQuaternionv2) { + MIT_Humanoid robot; + ClusterTreeModel model = robot.buildClusterTreeModel(); + model.setState(randomModelState(model,true)); + + testInverseDynamicsDerivativesFiniteDifference(model, "MIT Humanoid (Quaternion) - Finite Difference", 1e-4, 1e-6); +} + +TEST(InverseDynamicsDerivatives, TeleopArm) { + TeleopArm robot; + ClusterTreeModel model = robot.buildClusterTreeModel(); + model.setState(randomModelState(model)); + testInverseDynamicsDerivativesFiniteDifference(model, "TeleopArm", 1e-6, 1e-6); +} + +TEST(InverseDynamicsDerivatives, TelloImplicitConstraint) { + using namespace grbda; + Tello robot; + ClusterTreeModel model = robot.buildClusterTreeModel(); + constexpr bool enforce_constraints = true; + model.setState(randomModelState(model,enforce_constraints)); + // Tello uses 30 trials with verbose output to track detailed results + testInverseDynamicsDerivativesFiniteDifference(model, "Tello", 1e-6, 1e-6); +} + +TEST(InverseDynamicsDerivatives, PlanarLegLinkageImplicitConstraint) { + using namespace grbda; + PlanarLegLinkage robot; + ClusterTreeModel model = robot.buildClusterTreeModel(); + constexpr bool enforce_constraints = true; + model.setState(randomModelState(model,enforce_constraints)); + testInverseDynamicsDerivativesFiniteDifference(model, "PlanarLegLinkage", 1e-4, 1e-6); +} + +TEST(InverseDynamicsDerivatives, CassieClosedLoop) { + using namespace grbda; + Cassie robot; + ClusterTreeModel model = robot.buildClusterTreeModel(); + constexpr bool enforce_constraints = true; + model.setState(randomModelState(model,enforce_constraints)); + // Cassie has FourBar constraints in the lower legs + testInverseDynamicsDerivativesFiniteDifference(model, "Cassie (closed-loop)", 1e-4, 1e-5); +} diff --git a/UnitTests/testOrientationTools.cpp b/UnitTests/testOrientationTools.cpp index 3a4df33b..c27a0201 100644 --- a/UnitTests/testOrientationTools.cpp +++ b/UnitTests/testOrientationTools.cpp @@ -1,7 +1,14 @@ #include "gtest/gtest.h" +#include #include "grbda/Utils/OrientationTools.h" +namespace { +struct TestSeedInitializer { + TestSeedInitializer() { std::srand(42); } +} g_test_seed_initializer; +} + using namespace grbda; GTEST_TEST(OrientationTools, TestQuaternionIntegration) diff --git a/UnitTests/testReflectedInertiaAlgos.cpp b/UnitTests/testReflectedInertiaAlgos.cpp index 62fddd0e..8c92b762 100644 --- a/UnitTests/testReflectedInertiaAlgos.cpp +++ b/UnitTests/testReflectedInertiaAlgos.cpp @@ -1,6 +1,13 @@ #include "gtest/gtest.h" +#include #include "grbda/Dynamics/RigidBodyTreeModel.h" + +namespace { +struct TestSeedInitializer { + TestSeedInitializer() { std::srand(42); } +} g_test_seed_initializer; +} #include "grbda/Dynamics/ReflectedInertiaTreeModel.h" #include "grbda/Robots/RobotTypes.h" @@ -26,13 +33,30 @@ class ReflectedInertiaDynamicsAlgosTest : public testing::Test { JointState<> joint_state = cluster->joint_->randomJointState(); - if (joint_state.position.isSpanning() || joint_state.velocity.isSpanning()) - throw std::runtime_error("Initializing reflected inertia model requires all independent coordinates"); - - independent_joint_pos_ = appendEigenVector(independent_joint_pos_, + if (joint_state.position.isSpanning()) + { + const DMat& conv = cluster->joint_->spanningTreeToIndependentCoordsConversion(); + DVec ind_pos = conv.cast() * DVec(joint_state.position); + independent_joint_pos_ = appendEigenVector(independent_joint_pos_, ind_pos); + } + else + { + independent_joint_pos_ = appendEigenVector(independent_joint_pos_, joint_state.position); - independent_joint_vel_ = appendEigenVector(independent_joint_vel_, + + } + + if( joint_state.velocity.isSpanning() ) + { + const DMat& conv = cluster->joint_->spanningTreeToIndependentCoordsConversion(); + DVec ind_vel = conv.cast() * DVec(joint_state.velocity); + independent_joint_vel_ = appendEigenVector(independent_joint_vel_, ind_vel); + } + else + { + independent_joint_vel_ = appendEigenVector(independent_joint_vel_, joint_state.velocity); + } model_state.push_back(joint_state); } @@ -65,8 +89,8 @@ using testing::Types; typedef Types, RevoluteChainWithRotor<4>, - RevolutePairChainWithRotor<2>, - RevolutePairChainWithRotor<4>> + RevolutePairChainWithRotor<2>, + RevolutePairChainWithRotor<4>> RobotsCompatibleWithReflectedInertiaModel; TYPED_TEST_SUITE(ReflectedInertiaDynamicsAlgosTest, RobotsCompatibleWithReflectedInertiaModel); diff --git a/UnitTests/testRigidBodyDynamicsAlgos.cpp b/UnitTests/testRigidBodyDynamicsAlgos.cpp index 67e98995..d0d5c4ea 100644 --- a/UnitTests/testRigidBodyDynamicsAlgos.cpp +++ b/UnitTests/testRigidBodyDynamicsAlgos.cpp @@ -3,6 +3,8 @@ #include "testHelpers.hpp" #include "grbda/Dynamics/RigidBodyTreeModel.h" #include "grbda/Robots/RobotTypes.h" +#include "grbda/Dynamics/ClusterJoints/ClusterJoint.h" +#include "grbda/Dynamics/ClusterJoints/GenericJoint.h" using namespace grbda; @@ -50,8 +52,10 @@ class RigidBodyDynamicsAlgosTest : public testing::Test for (const auto &cluster : cluster_models.at(robot_idx).clusters()) { + JointState<> spanning_joint_state(true, true); + // Explicit constraint: fall back to existing random JointState<> joint_state = cluster->joint_->randomJointState(); - JointState<> spanning_joint_state = cluster->joint_->toSpanningTreeState(joint_state); + spanning_joint_state = cluster->joint_->toSpanningTreeState(joint_state); spanning_joint_pos = appendEigenVector(spanning_joint_pos, spanning_joint_state.position); @@ -61,7 +65,10 @@ class RigidBodyDynamicsAlgosTest : public testing::Test if (use_spanning_state) model_state.push_back(spanning_joint_state); else - model_state.push_back(joint_state); + { + // Use the robust spanning state for implicit clusters to avoid invalid random states + model_state.push_back(spanning_joint_state); + } } cluster_models[robot_idx].setState(model_state); @@ -221,6 +228,13 @@ TYPED_TEST(RigidBodyDynamicsAlgosTest, ForwardAndInverseDyanmics) const DVec tau_cluster = cluster_model.inverseDynamics(qdd_cluster); const DVec tau_proj = projection_model.inverseDynamics(qdd_cluster); + + //Inverse Dynamics Derivatives + const std::pair, DMat> tau_derivs_cluster = + cluster_model.firstOrderInverseDynamicsDerivatives(qdd_cluster); + + + // Verify joint acceleration agreement GTEST_ASSERT_LT((qdd_cluster_full - qdd_lg_custom_full).norm(), tol); GTEST_ASSERT_LT((qdd_cluster_full - qdd_lg_eigen_full).norm(), tol); diff --git a/UnitTests/testRigidBodyDynamicsAlgosDerivatives.cpp b/UnitTests/testRigidBodyDynamicsAlgosDerivatives.cpp index 64138fa8..0a1fb0b8 100644 --- a/UnitTests/testRigidBodyDynamicsAlgosDerivatives.cpp +++ b/UnitTests/testRigidBodyDynamicsAlgosDerivatives.cpp @@ -26,6 +26,7 @@ class DynamicsAlgosDerivativesTest : public testing::Test { nq_ = model.getNumPositions(); nv_ = model.getNumDegreesOfFreedom(); + sx_model_ = model; } } } @@ -52,10 +53,10 @@ class DynamicsAlgosDerivativesTest : public testing::Test DVec dq_cluster = dq_sym.segment(vel_idx, num_vel); DVec qd_cluster = qd_sym.segment(vel_idx, num_vel); - JointState joint_state(JointCoordinate(q_cluster, false), + DVec q_perturbed = TestHelpers::plus(cluster->joint_, q_cluster, dq_cluster); + const bool is_spanning = (q_cluster.size() > dq_cluster.size()); + JointState joint_state(JointCoordinate(q_perturbed, is_spanning), JointCoordinate(qd_cluster, false)); - joint_state.position = TestHelpers::plus(cluster->joint_->type(), - q_cluster, dq_cluster); state.push_back(joint_state); } @@ -153,6 +154,7 @@ class DynamicsAlgosDerivativesTest : public testing::Test } int nq_, nv_; + ClusterTreeModel sx_model_; const int num_robots_ = 4; std::vector> contact_jacobian_fcn_maps_; std::vector rnea_fcns_; @@ -244,7 +246,7 @@ TYPED_TEST(DynamicsAlgosDerivativesTest, contactJacobians) { std::vector dq_plus = dq; dq_plus[j] += h; - std::vector q_plus = TestHelpers::plus(q, dq_plus); + std::vector q_plus = TestHelpers::plus(this->sx_model_, q, dq_plus); std::vector res_plus = fcn(std::vector{q_plus, dq, qd, qdd}); DVec contact_point_pos_plus(3); @@ -252,7 +254,7 @@ TYPED_TEST(DynamicsAlgosDerivativesTest, contactJacobians) std::vector dq_minus = dq; dq_minus[j] -= h; - std::vector q_minus = TestHelpers::plus(q, dq_minus); + std::vector q_minus = TestHelpers::plus(this->sx_model_, q, dq_minus); std::vector res_minus = fcn(std::vector{q_minus, dq, qd, qdd}); DVec contact_point_pos_minus(3); @@ -315,7 +317,7 @@ TYPED_TEST(DynamicsAlgosDerivativesTest, rnea) { std::vector dq_plus = dq; dq_plus[j] += h; - std::vector q_plus = TestHelpers::plus(q, dq_plus); + std::vector q_plus = TestHelpers::plus(this->sx_model_, q, dq_plus); std::vector res_plus = fcn(std::vector{q_plus, dq, qd, tau}); DVec qdd_plus(this->nv_); @@ -323,7 +325,7 @@ TYPED_TEST(DynamicsAlgosDerivativesTest, rnea) std::vector dq_minus = dq; dq_minus[j] -= h; - std::vector q_minus = TestHelpers::plus(q, dq_minus); + std::vector q_minus = TestHelpers::plus(this->sx_model_, q, dq_minus); std::vector res_minus = fcn(std::vector{q_minus, dq, qd, tau}); DVec qdd_minus(this->nv_); diff --git a/UnitTests/testSpatialTransforms.cpp b/UnitTests/testSpatialTransforms.cpp index f2b4a4c4..21f12189 100644 --- a/UnitTests/testSpatialTransforms.cpp +++ b/UnitTests/testSpatialTransforms.cpp @@ -1,6 +1,13 @@ #include "gtest/gtest.h" +#include #include + +namespace { +struct TestSeedInitializer { + TestSeedInitializer() { std::srand(42); } +} g_test_seed_initializer; +} #include "grbda/Utils/SpatialInertia.h" #include "grbda/Utils/SpatialTransforms.h" diff --git a/include/grbda/Dynamics/ClusterJoints/ClusterJoint.h b/include/grbda/Dynamics/ClusterJoints/ClusterJoint.h index 491f5f7e..7f16983b 100644 --- a/include/grbda/Dynamics/ClusterJoints/ClusterJoint.h +++ b/include/grbda/Dynamics/ClusterJoints/ClusterJoint.h @@ -60,13 +60,41 @@ namespace grbda const DMat &Psi() const { return Psi_; } const DVec &vJ() const { return vJ_; } const DVec &cJ() const { return cJ_; } + const DMat &S_ring() const { return S_ring_; } + + // Derivative interface for configuration-dependent motion subspaces + // Returns zero by default for cluster joints + + // Returns ∂(Ṡ·q̇)/∂q as a (6*num_bodies x nv) matrix + virtual void getSdotqd_q(DMat& out) const { + out.setZero(num_bodies_ * 6, num_velocities_); + } + + // Contraction-based derivative interface + // Default returns zero (for joints with constant S). Override for configuration-dependent S. + + // Returns true if this joint has configuration-dependent motion subspace S(q) + virtual bool hasConfigurationDependentS() const { return false; } + + // Returns ∂(S*b)/∂q as a (6*num_bodies x nv) matrix + virtual void evalSTimesVec_dq(const DVec& b, DMat& out) const { + (void)b; + out.resize(0, 0); + } + + // Returns ∂(S^T*F)/∂q as a (nv x nv) matrix + virtual void evalSTTimesVec_dq(const DVec& F, DMat& out) const { + (void)F; + out.resize(0, 0); + } + std::shared_ptr> cloneLoopConstraint() const { return loop_constraint_->clone(); } - virtual JointState randomJointState() const; + virtual JointState randomJointState(bool enforce_position_constraint = true) const; const DMat &G() const { return loop_constraint_->G(); } const DVec &g() const { return loop_constraint_->g(); } @@ -74,12 +102,23 @@ namespace grbda const DMat &K() const { return loop_constraint_->K(); } const DVec &k() const { return loop_constraint_->k(); } + bool isImplicit() const { return loop_constraint_->isImplicit(); } + DVec phi(const JointCoordinate &joint_pos) const + { + return loop_constraint_->phi(joint_pos); + } + void updateJacobians(const JointCoordinate &joint_pos) + { + loop_constraint_->updateJacobians(joint_pos); + } + const DMat &spanningTreeToIndependentCoordsConversion() const { return spanning_tree_to_independent_coords_conversion_; } - JointState toSpanningTreeState(const JointState &joint_state); + JointState toSpanningTreeState(const JointState &joint_state, + bool enforce_constraints = false); protected: const int num_bodies_; @@ -90,6 +129,7 @@ namespace grbda DMat Psi_; DVec vJ_; DVec cJ_; + DMat S_ring_; std::shared_ptr> loop_constraint_; std::vector> single_joints_; diff --git a/include/grbda/Dynamics/ClusterJoints/FourBarJoint.h b/include/grbda/Dynamics/ClusterJoints/FourBarJoint.h index 233f3b14..c63fcfef 100644 --- a/include/grbda/Dynamics/ClusterJoints/FourBarJoint.h +++ b/include/grbda/Dynamics/ClusterJoints/FourBarJoint.h @@ -10,10 +10,18 @@ namespace grbda namespace LoopConstraint { template - struct FourBar : Base + struct FourBar : GenericImplicit { typedef typename CorrectMatrixInverseType::type InverseType; + // A four-bar linkage is modeled as two open kinematic chains (path1 and path2) + // whose tips must coincide. The constraint phi(q) = tip1(q) - tip2(q) = 0 enforces + // this closure condition. + // + // path1_link_lengths: lengths of links along the first chain, in order from base to tip + // path2_link_lengths: lengths of links along the second chain, in order from base to tip + // offset: 2D position of path2's base relative to path1's base + // independent_coordinate: index (0, 1, or 2) of the actuated joint coordinate FourBar(std::vector path1_link_lengths, std::vector path2_link_lengths, Vec2 offset, int independent_coordinate); @@ -22,11 +30,6 @@ namespace grbda return std::make_shared>(*this); } - DVec gamma(const JointCoordinate &joint_pos) const override - { - throw std::runtime_error("FourBar: Explicit constraint does not exist"); - } - void updateJacobians(const JointCoordinate &joint_pos) override; void updateBiases(const JointState &joint_state) override; @@ -63,14 +66,15 @@ namespace grbda FourBar(const std::vector> &bodies, const std::vector> &joints, std::shared_ptr> loop_constraint) - : Generic(bodies, joints, loop_constraint), + : Generic(bodies, joints, + std::static_pointer_cast>(loop_constraint)), four_bar_constraint_(loop_constraint) {} virtual ~FourBar() {} ClusterJointTypes type() const override { return ClusterJointTypes::FourBar; } - JointState randomJointState() const override; + JointState randomJointState(bool enforce_position_constraint = true) const override; private: std::shared_ptr> four_bar_constraint_; diff --git a/include/grbda/Dynamics/ClusterJoints/FreeJoint.h b/include/grbda/Dynamics/ClusterJoints/FreeJoint.h index ca5b838f..fd47d2d0 100644 --- a/include/grbda/Dynamics/ClusterJoints/FreeJoint.h +++ b/include/grbda/Dynamics/ClusterJoints/FreeJoint.h @@ -71,10 +71,12 @@ namespace grbda std::vector, JointPtr, DMat>> bodiesJointsAndReflectedInertias() const override; - JointState randomJointState() const override; + JointState randomJointState(bool enforce_position_constraint = true) const override; + private: const Body body_; + }; } diff --git a/include/grbda/Dynamics/ClusterJoints/GenericJoint.h b/include/grbda/Dynamics/ClusterJoints/GenericJoint.h index 083e0c1e..5aba0726 100644 --- a/include/grbda/Dynamics/ClusterJoints/GenericJoint.h +++ b/include/grbda/Dynamics/ClusterJoints/GenericJoint.h @@ -34,24 +34,78 @@ namespace grbda DVec gamma(const JointCoordinate &joint_pos) const override; void updateJacobians(const JointCoordinate &joint_pos) override; void updateBiases(const JointState &joint_state) override; + + bool isValidSpanningPosition(const JointCoordinate &joint_pos) const; const std::vector& isCoordinateIndependent() const; void createRandomStateHelpers() override; + // Symbolic phi function accessor (for creating complex-typed constraints) + const SymPhiFcn& getSymbolicPhi() const { return phi_sym_; } + + // Coordinate permutation matrix: q_span = coord_map * [y; q_dep] + // coord_map^T extracts [y; q_dep] from q_span, so ydot = (coord_map^T * qd_span).head(nv) + const DMat& getCoordMap() const { return coord_map_; } + + // dG/dq CasADi function accessor (for computing G_dot = dG/dt in S_ring) + // Returns the Jacobian of vec(G) w.r.t. q, shape (n_G_elements, n_q) + const casadi::Function& getdGdqFcn() const { return dG_dq_fcn_; } + + // G CasADi function accessor (for evaluating G matrix) + // Returns G matrix, shape (n_spanning, n_independent) + const casadi::Function& getGFcn() const { return G_fcn_; } + + // g CasADi function accessor (for evaluating explicit constraint bias) + // Returns g vector, shape (n_spanning, 1), takes {q, v} as inputs + const casadi::Function& getgFcn() const { return g_fcn_; } + + // K CasADi function accessor (for computing constraint Jacobian analytically) + // Returns K = dphi/dq, shape (n_constraints, n_spanning) + const casadi::Function& getKFcn() const { return K_fcn_; } + private: + // Basic CasADi function evaluation (real-valued) + static DMat runCasadiFcnReal(const casadi::Function &fcn, + const DVec &arg); + static DMat runCasadiFcnReal(const casadi::Function &fcn, + const DVec &pos, + const DVec &vel); + + // Complex-step aware evaluation methods (non-static, use derivative functions) + DMat evalK(const JointCoordinate &joint_pos) const; + DMat evalG(const JointCoordinate &joint_pos) const; + DMat evalk(const JointState &joint_state) const; + DMat evalg(const JointState &joint_state) const; + + // Static methods for phi evaluation static DMat runCasadiFcn(const casadi::Function &fcn, const JointCoordinate &arg); static DMat runCasadiFcn(const casadi::Function &fcn, const JointState &args); const std::vector is_coordinate_independent_; + DMat coord_map_; // permutation: q_span = coord_map * [y; q_dep] SymPhiFcn phi_sym_; + casadi::Function cs_phi_fcn_; casadi::Function K_fcn_; casadi::Function G_fcn_; casadi::Function k_fcn_; casadi::Function g_fcn_; + + // Derivative functions for complex-step support + // dK/dq: for each q_i, gives the Jacobian of K w.r.t. q_i + casadi::Function dK_dq_fcn_; + // dG/dq: for each q_i, gives the Jacobian of G w.r.t. q_i + casadi::Function dG_dq_fcn_; + + // dk/dq and dk/dv: Jacobians of k w.r.t. position and velocity + casadi::Function dk_dq_fcn_; + casadi::Function dk_dv_fcn_; + // dg/dq and dg/dv: Jacobians of g w.r.t. position and velocity + casadi::Function dg_dq_fcn_; + casadi::Function dg_dv_fcn_; }; } @@ -72,13 +126,36 @@ namespace grbda ClusterJointTypes type() const override { return ClusterJointTypes::Generic; } - JointState randomJointState() const override; + JointState randomJointState(bool enforce_position_constraint = true) const override; void updateKinematics(const JointState &joint_state) override; void computeSpatialTransformFromParentToCurrentCluster( spatial::GeneralizedTransform &Xup) const override; + // Motion subspace derivatives for configuration-dependent kinematics + void getSdotqd_q(DMat& out) const override; + + // GenericJoint has configuration-dependent S (uses CasADi) + bool hasConfigurationDependentS() const override { return generic_constraint_ != nullptr; } + + // Contraction-based derivatives (efficient, avoids materializing S_q tensor) + void evalSTimesVec_dq(const DVec& b, DMat& out) const override; + void evalSTTimesVec_dq(const DVec& F, DMat& out) const override; + + // Access to GenericImplicit constraint for complex-step differentiation + std::shared_ptr> getGenericConstraint() const { + return generic_constraint_; + } + + protected: + // Protected members for derived classes (e.g., FourBar) to access + DMat S_spanning_; + DMat X_intra_; + DMat X_intra_ring_; + mutable DVec q_spanning_; + mutable DVec qd_spanning_; + private: void initialize(const std::vector> &joints, std::shared_ptr> loop_constraint); @@ -93,10 +170,38 @@ namespace grbda const std::vector> bodies_; std::shared_ptr> generic_constraint_; - DMat S_spanning_; - DMat X_intra_; - DMat X_intra_ring_; DMat connectivity_; + + // Cached intermediates for derivative evaluation + mutable DMat S_implicit_; + + void initializeDerivativeFunctions() const; + + // CasADi functions for computing dG/dq and Sdotqd derivatives + mutable casadi::Function dG_dq_fcn_; + mutable casadi::Function dSdotqd_dq_fcn_; + + // CasADi functions for efficient contraction-based derivatives + // d(S*b)/dq: inputs {q_span, b}, outputs (mss_dim x nv) matrix + mutable casadi::Function dSb_dy_fcn_; + // d(S^T*F)/dq: inputs {q_span, F}, outputs (nv x nv) matrix + mutable casadi::Function dSTF_dy_fcn_; + + // Pre-allocated work vectors for low-level CasADi evaluation (avoids allocation overhead) + mutable std::vector dSb_work_w_; + mutable std::vector dSb_work_iw_; + mutable std::vector dSb_arg_buf_; // concatenated input buffer [q; b] + mutable std::vector dSb_res_buf_; // output buffer + mutable std::vector dSTF_work_w_; + mutable std::vector dSTF_work_iw_; + mutable std::vector dSTF_arg_buf_; // concatenated input buffer [q; F] + mutable std::vector dSTF_res_buf_; // output buffer + mutable std::vector dSdotqd_work_w_; + mutable std::vector dSdotqd_work_iw_; + mutable std::vector dSdotqd_arg_buf_; // concatenated input buffer [q; ydot] + mutable std::vector dSdotqd_res_buf_; // output buffer + + mutable bool derivative_functions_initialized_ = false; }; } diff --git a/include/grbda/Dynamics/ClusterJoints/RevolutePairJoint.h b/include/grbda/Dynamics/ClusterJoints/RevolutePairJoint.h index 20f70c61..f61eb174 100644 --- a/include/grbda/Dynamics/ClusterJoints/RevolutePairJoint.h +++ b/include/grbda/Dynamics/ClusterJoints/RevolutePairJoint.h @@ -1,7 +1,7 @@ #ifndef GRBDA_GENERALIZED_JOINTS_REVOLUTE_PAIR_JOINT_H #define GRBDA_GENERALIZED_JOINTS_REVOLUTE_PAIR_JOINT_H -#include "grbda/Dynamics/ClusterJoints/ClusterJoint.h" +#include "grbda/Dynamics/ClusterJoints/GenericJoint.h" namespace grbda { @@ -10,7 +10,7 @@ namespace grbda { template - class RevolutePair : public Base + class RevolutePair : public Generic { public: RevolutePair(Body &link_1, Body &link_2, @@ -19,25 +19,15 @@ namespace grbda ClusterJointTypes type() const override { return ClusterJointTypes::RevolutePair; } - void updateKinematics(const JointState &joint_state) override; - - void computeSpatialTransformFromParentToCurrentCluster( - spatial::GeneralizedTransform &Xup) const override; - std::vector, JointPtr, DMat>> bodiesJointsAndReflectedInertias() const override; private: - JointPtr link_1_joint_; - JointPtr link_2_joint_; - - spatial::Transform X21_; - const Body link_1_; const Body link_2_; - DMat X_intra_S_span_; - DMat X_intra_S_span_ring_; + JointPtr link_1_joint_; + JointPtr link_2_joint_; }; } diff --git a/include/grbda/Dynamics/ClusterJoints/RevolutePairWithRotorJoint.h b/include/grbda/Dynamics/ClusterJoints/RevolutePairWithRotorJoint.h index d89ee0b2..b26eee25 100644 --- a/include/grbda/Dynamics/ClusterJoints/RevolutePairWithRotorJoint.h +++ b/include/grbda/Dynamics/ClusterJoints/RevolutePairWithRotorJoint.h @@ -1,7 +1,7 @@ #ifndef GRBDA_GENERALIZED_JOINTS_REVOLUTE_PAIR_WITH_ROTOR_JOINT_H #define GRBDA_GENERALIZED_JOINTS_REVOLUTE_PAIR_WITH_ROTOR_JOINT_H -#include "grbda/Dynamics/ClusterJoints/ClusterJoint.h" +#include "grbda/Dynamics/ClusterJoints/GenericJoint.h" namespace grbda { @@ -10,7 +10,7 @@ namespace grbda { template - class RevolutePairWithRotor : public Base + class RevolutePairWithRotor : public Generic { public: typedef ParallelBeltTransmissionModule<1, Scalar> ProximalTransmission; @@ -24,22 +24,10 @@ namespace grbda return ClusterJointTypes::RevolutePairWithRotor; } - void updateKinematics(const JointState &joint_state) override; - - void computeSpatialTransformFromParentToCurrentCluster( - spatial::GeneralizedTransform &Xup) const override; - std::vector, JointPtr, DMat>> bodiesJointsAndReflectedInertias() const override; private: - JointPtr link1_joint_; - JointPtr rotor1_joint_; - JointPtr rotor2_joint_; - JointPtr link2_joint_; - - spatial::Transform X21_; - const Body link1_; const Body link2_; const Body rotor1_; @@ -50,8 +38,10 @@ namespace grbda const int rotor1_index_; const int rotor2_index_; - DMat X_intra_S_span_; - DMat X_intra_S_span_ring_; + // Gear/belt ratio matrix: ratio_product_(i, j) is the effective ratio from link j + // to rotor i. Stored at construction so bodiesJointsAndReflectedInertias() can + // compute reflected inertia without needing a prior updateKinematics() call. + Mat2 ratio_product_; }; } diff --git a/include/grbda/Dynamics/ClusterJoints/RevoluteTripleWithRotorJoint.h b/include/grbda/Dynamics/ClusterJoints/RevoluteTripleWithRotorJoint.h index 245e6fd1..29aa2ae2 100644 --- a/include/grbda/Dynamics/ClusterJoints/RevoluteTripleWithRotorJoint.h +++ b/include/grbda/Dynamics/ClusterJoints/RevoluteTripleWithRotorJoint.h @@ -36,7 +36,18 @@ namespace grbda std::vector, JointPtr, DMat>> bodiesJointsAndReflectedInertias() const override; + // Derivative methods + void getSdotqd_q(DMat& out) const override; + + // RevoluteTripleWithRotor has configuration-dependent S (uses CasADi) + bool hasConfigurationDependentS() const override { return true; } + + void evalSTimesVec_dq(const DVec& b, DMat& out) const override; + void evalSTTimesVec_dq(const DVec& F, DMat& out) const override; + private: + char axisToChar(ori::CoordinateAxis axis) const; + void initializeCasadiFunctions() const; JointPtr link_1_joint_; JointPtr link_2_joint_; JointPtr link_3_joint_; @@ -55,8 +66,25 @@ namespace grbda const Body rotor_2_; const Body rotor_3_; + ori::CoordinateAxis axis1_; + ori::CoordinateAxis axis2_; + ori::CoordinateAxis axis3_; + + spatial::Transform X_tree_2_; + spatial::Transform X_tree_3_; + DMat X_intra_S_span_; DMat X_intra_S_span_ring_; + + // CasADi functions for derivatives + mutable bool casadi_functions_initialized_ = false; + mutable casadi::Function f_dS_link2_dq_; + mutable casadi::Function f_dS_link3_dq_; + mutable casadi::Function f_Sdotqd_q_; + + // Cache for current state + mutable DVec q_spanning_; + mutable DVec qd_spanning_; }; } diff --git a/include/grbda/Dynamics/ClusterJoints/RevoluteWithRotorJoint.h b/include/grbda/Dynamics/ClusterJoints/RevoluteWithRotorJoint.h index 4d415395..436e843c 100644 --- a/include/grbda/Dynamics/ClusterJoints/RevoluteWithRotorJoint.h +++ b/include/grbda/Dynamics/ClusterJoints/RevoluteWithRotorJoint.h @@ -27,6 +27,7 @@ namespace grbda std::vector, JointPtr, DMat>> bodiesJointsAndReflectedInertias() const override; + private: JointPtr link_joint_; JointPtr rotor_joint_; diff --git a/include/grbda/Dynamics/ClusterTreeModel.h b/include/grbda/Dynamics/ClusterTreeModel.h index 9d1cbca2..70ff8790 100644 --- a/include/grbda/Dynamics/ClusterTreeModel.h +++ b/include/grbda/Dynamics/ClusterTreeModel.h @@ -89,8 +89,11 @@ namespace grbda void print() const; typedef std::pair, DVec> StatePair; - void setState(const ModelState &model_state); + void setState(const ModelState &model_state, bool enforce_constraints = true); void setState(const StatePair &q_qd_pair); + + std::pair, DVec> getState(); + void setState(const DVec& q_qd_vec); ModelState stateVectorToModelState(const StatePair& q_qd_pair); @@ -164,6 +167,9 @@ namespace grbda DMat getMassMatrix() override; DVec getBiasForceVector() override; + std::pair, DMat> firstOrderInverseDynamicsDerivatives(const DVec &qdd); + std::pair, DMat> firstOrderInverseDynamicsDerivativesWorldFrame(const DVec &qdd); + protected: using SX = casadi::SX; @@ -220,6 +226,13 @@ namespace grbda bool force_propagators_updated_ = false; bool qdd_effects_updated_ = false; + // 6 x nDOF accumulators for firstOrderInverseDynamicsDerivativesWorldFrame + D6Mat idDeriv_F1_, idDeriv_F2_, idDeriv_F3_, idDeriv_F4_; + + // nDOF x nDOF output matrices for firstOrderInverseDynamicsDerivatives + // Pre-allocated to avoid heap allocation on each call + mutable DMat dtau_dq_, dtau_dqd_; + template friend class RigidBodyTreeModel; diff --git a/include/grbda/Dynamics/Joints/Joint.h b/include/grbda/Dynamics/Joints/Joint.h index 1b6180a1..a4e0e2e9 100644 --- a/include/grbda/Dynamics/Joints/Joint.h +++ b/include/grbda/Dynamics/Joints/Joint.h @@ -20,6 +20,11 @@ namespace grbda virtual std::shared_ptr> clone() const = 0; + virtual std::shared_ptr> cloneAsSymbolic() const + { + throw std::runtime_error("cloneAsSymbolic not implemented for joint: " + name_); + } + virtual void updateKinematics(const DVec &q, const DVec &qd) = 0; const std::string& name() const { return name_; } @@ -30,6 +35,15 @@ namespace grbda const DMat &Psi() const { return Psi_; } const spatial::Transform &XJ() const { return XJ_; } + // Derivative interface for configuration-dependent motion subspaces + // Returns zero by default for most joint types (Revolute, Free, etc.) + // Override for joints with absolute coordinates or configuration-dependent kinematics + + // Returns ∂(Ṡ·q̇)/∂q as a (6 x nv) matrix + virtual DMat getSdotqd_q() const { + return DMat::Zero(6, num_velocities_); + } + protected: const std::string name_; const int num_positions_; @@ -58,15 +72,19 @@ namespace grbda return std::make_shared>(*this); } + std::shared_ptr> cloneAsSymbolic() const override + { + return std::make_shared>(this->name_); + } + void updateKinematics(const DVec &q, const DVec &qd) override - { + { const int& num_ori_param = OrientationRepresentation::num_ori_parameter; - const RotMat R = - OrientationRepresentation::getRotationMatrix(q.template tail()); - const Vec3 q_pos = q.template head<3>(); - this->XJ_ = spatial::Transform(R, q_pos); + const RotMat R = OrientationRepresentation::getRotationMatrix( + q.template tail()); + this->XJ_ = spatial::Transform(R, q.template head<3>()); } - + OrientationRepresentation orientation_representation_; }; @@ -92,11 +110,18 @@ namespace grbda return std::make_shared>(*this); } + std::shared_ptr> cloneAsSymbolic() const override + { + return std::make_shared>(axis_); + } + void updateKinematics(const DVec &q, const DVec &qd) override { this->XJ_ = spatial::rotation(axis_, q[0]); } + ori::CoordinateAxis getAxis() const { return axis_; } + private: const ori::CoordinateAxis axis_; }; diff --git a/include/grbda/Dynamics/Joints/OrientationRepresentation.h b/include/grbda/Dynamics/Joints/OrientationRepresentation.h index 3bc8da88..4eb6a003 100644 --- a/include/grbda/Dynamics/Joints/OrientationRepresentation.h +++ b/include/grbda/Dynamics/Joints/OrientationRepresentation.h @@ -10,9 +10,9 @@ namespace grbda { struct Quaternion { - static const int num_ori_parameter = 4; - static const int numSpanningPos = 7; - static const int numIndependentPos = 7; + static constexpr int num_ori_parameter = 4; + static constexpr int numSpanningPos = 7; + static constexpr int numIndependentPos = 7; template static const RotMat @@ -30,9 +30,9 @@ namespace grbda struct RollPitchYaw { - static const int num_ori_parameter = 3; - static const int numSpanningPos = 6; - static const int numIndependentPos = 6; + static constexpr int num_ori_parameter = 3; + static constexpr int numSpanningPos = 6; + static constexpr int numIndependentPos = 6; template static const RotMat diff --git a/include/grbda/Dynamics/Nodes/ClusterTreeNode.h b/include/grbda/Dynamics/Nodes/ClusterTreeNode.h index 7ec6a295..b1703dd8 100644 --- a/include/grbda/Dynamics/Nodes/ClusterTreeNode.h +++ b/include/grbda/Dynamics/Nodes/ClusterTreeNode.h @@ -10,6 +10,7 @@ namespace grbda template struct ClusterTreeNode : TreeNode { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef typename CorrectMatrixInverseType::type InverseType; typedef std::shared_ptr> ClusterJointPtr; typedef std::pair, JointPtr> BodyJointPair; @@ -24,6 +25,7 @@ namespace grbda const DVec &vJ() const override { return joint_->vJ(); } const DMat &S() const override { return joint_->S(); } const DVec &cJ() const override { return joint_->cJ(); } + const DMat &S_ring() const override { return joint_->S_ring(); } const spatial::Transform &getAbsoluteTransformForBody(const Body &body) override; DVec getVelocityForBody(const Body &body) override; @@ -53,6 +55,37 @@ namespace grbda DMat qdd_for_subtree_due_to_subtree_root_joint_qdd; DMat K_; DMat L_; + + DMat Psi_dot_; + DMat Psi_ddot_; + DMat Upsilon_dot_; + DMat M_cup_; + DMat B_cup_; + DVec F_; + + // Workspace vectors for parent velocity/acceleration (sized by motion_subspace_dimension_) + DVec v_parent_up_; + DVec a_parent_up_; + + // Workspace matrices for firstOrderInverseDynamicsDerivatives + // Pre-allocated to avoid dynamic allocation in hot loop + DMat t1_workspace_; + DMat t2_workspace_; + DMat t3_workspace_; + DMat t4_workspace_; + DMat t_tmp_workspace_; + DMat alpha_workspace_; + DMat beta_workspace_; + DMat Sdotqd_q_workspace_; + DMat st_dq_workspace_; + + // World-frame quantities for firstOrderInverseDynamicsDerivativesWorldFrame. + // Ic0_ and S0_ (from TreeNode) are reused for IC0 and S0 respectively. + std::vector, Eigen::aligned_allocator>> BC0_; + std::vector, Eigen::aligned_allocator>> Psid0_; + std::vector, Eigen::aligned_allocator>> Psidd0_; + std::vector, Eigen::aligned_allocator>> Upsilond0_; + std::vector, Eigen::aligned_allocator>> f0_; }; } // namespace grbda diff --git a/include/grbda/Dynamics/Nodes/ReflectedInertiaTreeNode.h b/include/grbda/Dynamics/Nodes/ReflectedInertiaTreeNode.h index ba84066f..ef3c3f6e 100644 --- a/include/grbda/Dynamics/Nodes/ReflectedInertiaTreeNode.h +++ b/include/grbda/Dynamics/Nodes/ReflectedInertiaTreeNode.h @@ -10,6 +10,7 @@ namespace grbda template struct ReflectedInertiaTreeNode : TreeNode { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW ReflectedInertiaTreeNode(const int index, const Body &link, const std::shared_ptr> &joint, const int parent_index, @@ -20,6 +21,7 @@ namespace grbda const DVec &vJ() const override { return vJ_; } const DMat &S() const override { return joint_->S(); } const DVec &cJ() const override { return cJ_; } + const DMat &S_ring() const override { return S_ring_; } const spatial::Transform &getAbsoluteTransformForBody(const Body &body) override; DVec getVelocityForBody(const Body &body) override; @@ -31,6 +33,7 @@ namespace grbda DVec vJ_; DVec cJ_ = DVec::Zero(6); + DMat S_ring_; const spatial::Transform Xtree_; Mat6 IA_; // articulated body inertia diff --git a/include/grbda/Dynamics/Nodes/RigidBodyTreeNode.h b/include/grbda/Dynamics/Nodes/RigidBodyTreeNode.h index cce4610d..7edaf9fa 100644 --- a/include/grbda/Dynamics/Nodes/RigidBodyTreeNode.h +++ b/include/grbda/Dynamics/Nodes/RigidBodyTreeNode.h @@ -10,6 +10,7 @@ namespace grbda template struct RigidBodyTreeNode : TreeNode { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW RigidBodyTreeNode(const Body &body, const std::shared_ptr> &joint, const int position_index, const int velocity_index, @@ -19,6 +20,7 @@ namespace grbda const DVec &vJ() const override { return vJ_; } const DMat &S() const override { return joint_->S(); } const DVec &cJ() const override { return cJ_; } + const DMat &S_ring() const { return S_ring_; } const spatial::Transform &getAbsoluteTransformForBody(const Body &body) override; DVec getVelocityForBody(const Body &body) override; @@ -30,6 +32,7 @@ namespace grbda DVec vJ_; DVec cJ_ = DVec::Zero(6); + DMat S_ring_; const spatial::Transform Xtree_; }; diff --git a/include/grbda/Dynamics/Nodes/TreeNode.h b/include/grbda/Dynamics/Nodes/TreeNode.h index 122a43c2..3bab3f79 100644 --- a/include/grbda/Dynamics/Nodes/TreeNode.h +++ b/include/grbda/Dynamics/Nodes/TreeNode.h @@ -15,7 +15,8 @@ namespace grbda template struct TreeNode { - TreeNode(int index, std::string name, int parent_index, int num_parent_bodies, + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + TreeNode(int index, std::string name, int parent_index, int num_parent_bodies, int motion_subspace_index, int motion_subspace_dimension, int position_index, int num_positions, int velocity_index, int num_velocities) @@ -26,7 +27,9 @@ namespace grbda index_(index), name_(name), parent_index_(parent_index), Xup_(num_parent_bodies) { - I_ = DMat::Zero(motion_subspace_dimension_, motion_subspace_dimension_); + const int num_bodies = motion_subspace_dimension / 6; + I_.resize(num_bodies, Mat6::Zero()); + Ic_.resize(num_bodies, Mat6::Zero()); f_ext_ = DVec::Zero(motion_subspace_dimension_); } @@ -39,6 +42,7 @@ namespace grbda virtual const DVec &vJ() const = 0; virtual const DMat &S() const = 0; virtual const DVec &cJ() const = 0; + virtual const DMat &S_ring() const = 0; virtual const spatial::Transform &getAbsoluteTransformForBody(const Body &body) = 0; virtual DVec getVelocityForBody(const Body &body) = 0; @@ -52,6 +56,10 @@ namespace grbda const int motion_subspace_index_; const int motion_subspace_dimension_; + // Number of velocities in this node's subtree (self + all descendants). + // Computed once after the model is fully built; used by CRBA world-frame variant. + int subtree_num_velocities_ = 0; + const int index_; const std::string name_; const int parent_index_; @@ -64,8 +72,14 @@ namespace grbda DVec f_ext_; // net external spatial force acting on the cluster DVec avp_; // acceleration velocity product - DMat I_; // spatial inertia - DMat Ic_; // compisite rigid body inertia + std::vector, Eigen::aligned_allocator>> I_; // spatial inertia, one 6x6 block per body + std::vector, Eigen::aligned_allocator>> Ic_; // composite rigid body inertia, one 6x6 block per body + + std::vector, Eigen::aligned_allocator>> Ic0_; // composite rigid body inertia in World frame, one 6x6 block per body + std::vector, Eigen::aligned_allocator>> S0_; // motion subspace in World frame, one 6xNv block per body + std::vector, Eigen::aligned_allocator>> Ftmp_; // temporary variable used in CRBA, one 6xNv block per body + + spatial::GeneralizedTransform Xup_; // spatial xform from parent to child spatial::GeneralizedAbsoluteTransform Xa_; // spatial xform from world to current diff --git a/include/grbda/Dynamics/TreeModel.h b/include/grbda/Dynamics/TreeModel.h index 54a05175..e65ad965 100644 --- a/include/grbda/Dynamics/TreeModel.h +++ b/include/grbda/Dynamics/TreeModel.h @@ -104,10 +104,26 @@ namespace grbda return contact_points_[contact_name_to_contact_index_.at(name)]; } + // Benchmark/testing methods - run CRBA variants directly + void runStandardCRBA() + { + mass_matrix_updated_ = false; + compositeRigidBodyAlgorithm(); + } + + void runWorldFrameCRBA() + { + mass_matrix_updated_ = false; + compositeRigidBodyAlgorithmWorldFrame(); + } + + const DMat& getH() const { return H_; } + protected: void contactPointForwardKinematics(); void contactPointForwardAccelerationKinematics(const DVec &qdd); void compositeRigidBodyAlgorithm(); + void compositeRigidBodyAlgorithmWorldFrame(); void updateBiasForceVector(); // Takes as input independent (non-spanning) joint accelerations @@ -122,6 +138,8 @@ namespace grbda DMat H_; DVec C_; + D6Mat F_; + int position_index_ = 0; int velocity_index_ = 0; diff --git a/include/grbda/Robots/Cassie.hpp b/include/grbda/Robots/Cassie.hpp new file mode 100644 index 00000000..fa7ff7cf --- /dev/null +++ b/include/grbda/Robots/Cassie.hpp @@ -0,0 +1,171 @@ +#ifndef GRBDA_ROBOTS_CASSIE_H +#define GRBDA_ROBOTS_CASSIE_H + +#include "grbda/Robots/Robot.h" + +namespace grbda +{ + /** + * Cassie biped robot from Agility Robotics. + * + * Model topology follows the Cassie MJCF in MuJoCo Menagerie + * (agility_cassie/cassie.xml). Each leg has two FourBar clusters: + * + * Upper four-bar (parent = hip-pitch): + * path1: knee+shin (link 1) -> tarsus+heel-spring (link 2) + * path2: achilles-rod (closure via achilles tip to heel-spring pivot) + * + * Lower four-bar (parent = tarsus+heel-spring body): + * path1: foot-crank (link 1) -> plantar-rod (link 2) + * path2: foot (zero-length rocker; closure at foot pivot) + */ + template + class Cassie : public Robot + { + public: + + Cassie() {} + + ClusterTreeModel buildClusterTreeModel() const override; + + protected: + const std::string base = "pelvis"; + const Scalar grav = -9.806; + + // Pelvis + const Scalar pelvis_mass = 10.33; + const Vec3 pelvis_CoM = Vec3{0.05066, 0.000346, 0.02841}; + const Mat3 pelvis_inertia = (Mat3() << + 0.085821, 1.276e-05, -0.00016022, + 1.276e-05, 0.049222, -0.000414, + -0.00016022, -0.000414, 0.08626).finished(); + + // Hip-roll (symmetric across sides — CoM z is 0 in MJCF xyaxes frame) + const Scalar hip_roll_mass = 1.82; + // CoM in hip-roll body frame. y-component is identical for both sides in the MJCF. + const Vec3 hip_roll_CoM = Vec3{-0.01793, 0.0001, -0.04428}; + // fullinertia = Ixx Iyy Izz Ixy Ixz Iyz (symmetric, left side; right side flips Ixy,Iyz signs) + const Mat3 hip_roll_inertia_left = (Mat3() << + 0.003431, -6.65e-07, -0.00084, + -6.65e-07, 0.003793, 3.99e-06, + -0.00084, 3.99e-06, 0.002135).finished(); + const Mat3 hip_roll_inertia_right = (Mat3() << + 0.003431, 6.65e-07, -0.00084, + 6.65e-07, 0.003793, -3.99e-06, + -0.00084, -3.99e-06, 0.002135).finished(); + + // Hip-yaw (CoM y flips sign between sides) + const Scalar hip_yaw_mass = 1.171; + const Vec3 hip_yaw_CoM_left = Vec3{0.0, -1e-05, -0.034277}; + const Vec3 hip_yaw_CoM_right = Vec3{0.0, 1e-05, -0.034277}; + const Mat3 hip_yaw_inertia_left = (Mat3() << + 0.002443, -4e-08, 2.462e-07, + -4e-08, 0.002803, -2.71e-08, + 2.462e-07,-2.71e-08, 0.000842).finished(); + const Mat3 hip_yaw_inertia_right = (Mat3() << + 0.002443, 4e-08, 2.462e-07, + 4e-08, 0.002803, 2.71e-08, + 2.462e-07, 2.71e-08, 0.000842).finished(); + + // Hip-pitch (CoM z flips sign between sides) + const Scalar hip_pitch_mass = 5.52; + const Vec3 hip_pitch_CoM_left = Vec3{0.05946, 5e-05, -0.03581}; + const Vec3 hip_pitch_CoM_right = Vec3{0.05946, 5e-05, 0.03581}; + const Mat3 hip_pitch_inertia_left = (Mat3() << + 0.010898, -0.0002669, -5.721e-05, + -0.0002669, 0.029714, 9.17e-06, + -5.721e-05, 9.17e-06, 0.030257).finished(); + const Mat3 hip_pitch_inertia_right = (Mat3() << + 0.010898, -0.0002669, 5.721e-05, + -0.0002669, 0.029714, -9.17e-06, + 5.721e-05, -9.17e-06, 0.030257).finished(); + + const Vec3 hip_roll_joint_pos = Vec3{0.021, 0.135, 0.0}; + const Vec3 hip_yaw_joint_offset = Vec3{0.0, 0.0, -0.07}; + const Vec3 hip_pitch_joint_offset = Vec3{0.0, 0.0, -0.09}; + + // Gear ratios + const Scalar hip_roll_gear = 25.0; + const Scalar hip_yaw_gear = 25.0; + const Scalar hip_pitch_gear = 16.0; + const Scalar knee_gear = 16.0; + const Scalar foot_gear = 50.0; + + // Achilles-rod (symmetric across sides; CoM on x-axis only) + const Scalar achilles_mass = 0.1567; + const Vec3 achilles_CoM = Vec3{0.24719, 0.0, 0.0}; + const Mat3 achilles_inertia = (Mat3() << + 3.754e-06, -3.74e-08, -1.61e-08, + -3.74e-08, 0.004487, 0.0, + -1.61e-08, 0.0, 0.004488).finished(); + + // knee+knee-spring+shin combined (single rigid body in knee body frame). + // Lumps knee (0.7578 kg), knee-spring (0.186 kg), shin (0.577 kg). + // CoM and inertia are in the knee body frame; z-component of CoM flips between sides. + const Scalar knee_shin_mass = 1.5208; + const Vec3 knee_shin_CoM_left = Vec3{0.12170, 0.04491, -0.00101}; + const Vec3 knee_shin_CoM_right = Vec3{0.12170, 0.04491, 0.00101}; + // Lumped inertia computed via parallel-axis theorem from MJCF values. + // Left and right differ only in Ixz and Iyz sign. + const Mat3 knee_shin_inertia_left = (Mat3() << + 0.002032, -0.000641, -5.8e-05, + -0.000641, 0.016670, -2.1e-05, + -5.8e-05, -2.1e-05, 0.017630).finished(); + const Mat3 knee_shin_inertia_right = (Mat3() << + 0.002032, -0.000641, 5.8e-05, + -0.000641, 0.016670, 2.1e-05, + 5.8e-05, 2.1e-05, 0.017630).finished(); + + // tarsus+heel-spring combined (single rigid body in tarsus body frame). + // Lumps tarsus (0.782 kg) and heel-spring (0.126 kg). + // CoM z-component flips between sides. + const Scalar tarsus_mass = 0.9080; + const Vec3 tarsus_CoM_left = Vec3{0.10461, -0.03028, -0.00100}; + const Vec3 tarsus_CoM_right = Vec3{0.10461, -0.03028, 0.00100}; + const Mat3 tarsus_inertia_left = (Mat3() << + 0.000453, 0.000257, -5.2e-05, + 0.000257, 0.013817, -4.9e-05, + -5.2e-05, -4.9e-05, 0.013897).finished(); + const Mat3 tarsus_inertia_right = (Mat3() << + 0.000453, 0.000257, 5.2e-05, + 0.000257, 0.013817, 4.9e-05, + 5.2e-05, 4.9e-05, 0.013897).finished(); + + // Foot-crank (CoM z flips between sides) + const Scalar foot_crank_mass = 0.1261; + const Vec3 foot_crank_CoM_left = Vec3{0.00493, 2e-05, -0.00215}; + const Vec3 foot_crank_CoM_right = Vec3{0.00493, 2e-05, 0.00215}; + const Mat3 foot_crank_inertia_left = (Mat3() << + 2.6941e-05, -2.1e-09, -3.9623e-06, + -2.1e-09, 4.9621e-05, -1.09e-08, + -3.9623e-06, -1.09e-08, 6.3362e-05).finished(); + const Mat3 foot_crank_inertia_right = (Mat3() << + 2.6941e-05, -2.1e-09, 3.9623e-06, + -2.1e-09, 4.9621e-05, 1.09e-08, + 3.9623e-06, 1.09e-08, 6.3362e-05).finished(); + + // Plantar-rod (symmetric; CoM on x-axis only) + const Scalar plantar_rod_mass = 0.1186; + const Vec3 plantar_rod_CoM = Vec3{0.17792, 0.0, 0.0}; + const Mat3 plantar_rod_inertia = (Mat3() << + 2.779e-06, -2.34e-08, -8.1e-09, + -2.34e-08, 0.001774, 0.0, + -8.1e-09, 0.0, 0.001775).finished(); + + // Foot (CoM z flips between sides) + const Scalar foot_mass = 0.1498; + const Vec3 foot_CoM_left = Vec3{0.00474, 0.02748, -0.00014}; + const Vec3 foot_CoM_right = Vec3{0.00474, 0.02748, 0.00014}; + const Mat3 foot_inertia_left = (Mat3() << + 0.00017388, 0.00011814, 1.36e-06, + 0.00011814, 0.00016793, -4e-07, + 1.36e-06, -4e-07, 0.00033261).finished(); + const Mat3 foot_inertia_right = (Mat3() << + 0.00017388, 0.00011814, -1.36e-06, + 0.00011814, 0.00016793, 4e-07, + -1.36e-06, 4e-07, 0.00033261).finished(); + }; + +} // namespace grbda + +#endif // GRBDA_ROBOTS_CASSIE_H diff --git a/include/grbda/Robots/DoublePendulum.hpp b/include/grbda/Robots/DoublePendulum.hpp new file mode 100644 index 00000000..6634e9fe --- /dev/null +++ b/include/grbda/Robots/DoublePendulum.hpp @@ -0,0 +1,69 @@ +#ifndef GRBDA_ROBOTS_DOUBLE_PENDULUM_H +#define GRBDA_ROBOTS_DOUBLE_PENDULUM_H + +#include "grbda/Robots/Robot.h" +#include "grbda/Dynamics/ClusterTreeModel.h" + +namespace grbda +{ + + /** + * @brief Templated Double Pendulum robot class for complex-step differentiation + * + * This is a 2-DOF fixed-base serial chain with two revolute joints. + * The robot matches the structure in double_pendulum.urdf but is templated + * to support complex numbers for complex-step derivative verification. + */ + template + class DoublePendulum : public Robot + { + public: + DoublePendulum() {} + + ClusterTreeModel buildClusterTreeModel() const override + { + ClusterTreeModel model; + using Revolute = ClusterJoints::Revolute; + + Mat3 I3 = Mat3::Identity(); + + // Link 1 inertial parameters (from URDF) + // mass = 2.0, inertia = [0.05, 0.03, 0.01] + Mat3 I1; + I1 << Scalar(0.05), Scalar(0), Scalar(0), + Scalar(0), Scalar(0.03), Scalar(0), + Scalar(0), Scalar(0), Scalar(0.01); + Vec3 com1(Scalar(0), Scalar(0), Scalar(0)); + SpatialInertia link1_inertia(Scalar(2.0), com1, I1); + + // Link 2 inertial parameters (from URDF) + // mass = 1.0, inertia = [0.01, 0.005, 0.2] + Mat3 I2; + I2 << Scalar(0.01), Scalar(0), Scalar(0), + Scalar(0), Scalar(0.005), Scalar(0), + Scalar(0), Scalar(0), Scalar(0.2); + Vec3 com2(Scalar(0), Scalar(0), Scalar(0)); + SpatialInertia link2_inertia(Scalar(1.0), com2, I2); + + // Joint 1: Revolute about Z-axis, attached to ground at origin + spatial::Transform Xtree1(I3, Vec3::Zero()); + + model.template appendBody( + "link1", link1_inertia, "ground", Xtree1, + ori::CoordinateAxis::Z, "joint1"); + + // Joint 2: Revolute about Z-axis, offset from link1 by [0.1, 0, 0] + Vec3 r2(Scalar(0.1), Scalar(0), Scalar(0)); + spatial::Transform Xtree2(I3, r2); + + model.template appendBody( + "link2", link2_inertia, "link1", Xtree2, + ori::CoordinateAxis::Z, "joint2"); + + return model; + } + }; + +} // namespace grbda + +#endif // GRBDA_ROBOTS_DOUBLE_PENDULUM_H diff --git a/include/grbda/Robots/PlanarLegLinkage.hpp b/include/grbda/Robots/PlanarLegLinkage.hpp index c8714c54..d7db73b4 100644 --- a/include/grbda/Robots/PlanarLegLinkage.hpp +++ b/include/grbda/Robots/PlanarLegLinkage.hpp @@ -9,6 +9,7 @@ namespace grbda class PlanarLegLinkage : public Robot { public: + PlanarLegLinkage(); ClusterTreeModel buildClusterTreeModel() const override; diff --git a/include/grbda/Robots/RobotTypes.h b/include/grbda/Robots/RobotTypes.h index 3fa907a7..758c1fe4 100644 --- a/include/grbda/Robots/RobotTypes.h +++ b/include/grbda/Robots/RobotTypes.h @@ -1,5 +1,8 @@ +#include "grbda/Robots/Cassie.hpp" #include "grbda/Robots/TeleopArm.hpp" #include "grbda/Robots/Tello.hpp" +#include "grbda/Robots/TelloNoRotors.hpp" +#include "grbda/Robots/TelloRotorsNoConstraints.hpp" #include "grbda/Robots/TelloWithArms.hpp" #include "grbda/Robots/MIT_Humanoid.hpp" #include "grbda/Robots/MIT_Humanoid_no_rotors.hpp" @@ -8,6 +11,7 @@ #include "grbda/Robots/JVRC1_Humanoid.hpp" #include "grbda/Robots/PlanarLegLinkage.hpp" #include "grbda/Robots/SingleRigidBody.hpp" +#include "grbda/Robots/DoublePendulum.hpp" #include "grbda/Robots/SerialChains/RevoluteChainWithRotor.hpp" #include "grbda/Robots/SerialChains/RevolutePairChain.hpp" #include "grbda/Robots/SerialChains/RevolutePairChainWithRotor.hpp" diff --git a/include/grbda/Robots/TeleopArm.hpp b/include/grbda/Robots/TeleopArm.hpp index 24e63899..8cffcc7f 100644 --- a/include/grbda/Robots/TeleopArm.hpp +++ b/include/grbda/Robots/TeleopArm.hpp @@ -11,7 +11,7 @@ namespace grbda public: TeleopArm(); - ClusterTreeModel<> buildClusterTreeModel() const override; + ClusterTreeModel buildClusterTreeModel() const override; private: // Base Cluster @@ -20,15 +20,15 @@ namespace grbda // Base const std::string base_name_ = "base"; const std::string base_parent_name_ = "ground"; - Vec3 base_location_{0, 0, 0.051}; + Vec3 base_location_; SpatialInertia base_spatial_inertia_; // Base Rotor const std::string base_rotor_name_ = "base-rotor"; const std::string base_rotor_parent_name_ = "ground"; - Vec3 base_rotor_location_{0, 0, 0}; + Vec3 base_rotor_location_; SpatialInertia base_rotor_spatial_inertia_; - double base_rotor_gear_ratio_ = 6.0; + double base_rotor_gear_ratio_; // Shoulder Rx Cluster const std::string shoulder_rx_cluster_name_ = "shoulder-rx-cluster"; @@ -36,15 +36,15 @@ namespace grbda // Shoulder Rx const std::string shoulder_rx_link_name_ = "shoulder-rx-link"; const std::string shoulder_rx_link_parent_name_ = "base"; - Vec3 shoulder_rx_link_location_{0, 0, 0.106}; + Vec3 shoulder_rx_link_location_; SpatialInertia shoulder_rx_link_spatial_inertia_; // Shoulder Rx Rotor const std::string shoulder_rx_rotor_name_ = "shoulder-rx-rotor"; const std::string shoulder_rx_rotor_parent_name_ = "base"; - Vec3 shoulder_rx_rotor_location_{0, 0, 0}; + Vec3 shoulder_rx_rotor_location_; SpatialInertia shoulder_rx_rotor_spatial_inertia_; - double shoulder_rx_rotor_gear_ratio_ = 6.0; + double shoulder_rx_rotor_gear_ratio_; // Shoulder Ry Cluster const std::string shoulder_ry_cluster_name_ = "shoulder-ry-cluster"; @@ -52,15 +52,15 @@ namespace grbda // Shoulder Ry const std::string shoulder_ry_link_name_ = "shoulder-ry-link"; const std::string shoulder_ry_link_parent_name_ = "shoulder-rx-link"; - Vec3 shoulder_ry_link_location_{0, 0, 0.071}; + Vec3 shoulder_ry_link_location_; SpatialInertia shoulder_ry_link_spatial_inertia_; // Shoulder Ry Rotor const std::string shoulder_ry_rotor_name_ = "shoulder-ry-rotor"; const std::string shoulder_ry_rotor_parent_name_ = "shoulder-rx-link"; - Vec3 shoulder_ry_rotor_location_{0, 0, 0}; + Vec3 shoulder_ry_rotor_location_; SpatialInertia shoulder_ry_rotor_spatial_inertia_; - double shoulder_ry_rotor_gear_ratio_ = 6.0; + double shoulder_ry_rotor_gear_ratio_; // Upper Arm Cluster const std::string upper_arm_cluster_name_ = "upper-arm-cluster"; @@ -68,44 +68,44 @@ namespace grbda // Upper Link const std::string upper_link_name_ = "upper-link"; const std::string upper_link_parent_name_ = "shoulder-ry-link"; - const Vec3 upper_link_location_{0, -0.0095, 0.3855}; + Vec3 upper_link_location_; SpatialInertia upper_link_spatial_inertia_; // Wrist Pitch Link const std::string wrist_pitch_link_name_ = "wrist-pitch-link"; const std::string wrist_pitch_link_parent_name_ = "upper-link"; - const Vec3 wrist_pitch_link_location_{0, 0, 0.362}; + Vec3 wrist_pitch_link_location_; SpatialInertia wrist_pitch_link_spatial_inertia_; // Wrist Roll Link const std::string wrist_roll_link_name_ = "wrist-roll-link"; const std::string wrist_roll_link_parent_name_ = "wrist-pitch-link"; - const Vec3 wrist_roll_link_location_{0, 0.004, 0.03574}; + Vec3 wrist_roll_link_location_; SpatialInertia wrist_roll_link_spatial_inertia_; // Elbow Rotor const std::string elbow_rotor_name_ = "elbow-rotor"; const std::string elbow_rotor_parent_name_ = "shoulder-ry-link"; - const Vec3 elbow_rotor_location_{0., 0., 0.}; + Vec3 elbow_rotor_location_; SpatialInertia elbow_rotor_spatial_inertia_; - const double elbow_rotor_gear_ratio_ = 6.0; - const Vec1 elbow_rotor_belt_ratios_{1.0}; + double elbow_rotor_gear_ratio_; + DVec elbow_rotor_belt_ratios_; // Wrist Pitch Rotor const std::string wrist_pitch_rotor_name_ = "wrist-pitch-rotor"; const std::string wrist_pitch_rotor_parent_name_ = "shoulder-ry-link"; - const Vec3 wrist_pitch_rotor_location_{0., 0., 0.}; + Vec3 wrist_pitch_rotor_location_; SpatialInertia wrist_pitch_rotor_spatial_inertia_; - const double wrist_pitch_rotor_gear_ratio_ = 6.0; - const Vec2 wrist_pitch_rotor_belt_ratios_{1.0, 1.0}; + double wrist_pitch_rotor_gear_ratio_; + DVec wrist_pitch_rotor_belt_ratios_; // Wrist Roll Rotor const std::string wrist_roll_rotor_name_ = "wrist-roll-rotor"; const std::string wrist_roll_rotor_parent_name_ = "shoulder-ry-link"; - const Vec3 wrist_roll_rotor_location_{0., 0., 0.}; + Vec3 wrist_roll_rotor_location_; SpatialInertia wrist_roll_rotor_spatial_inertia_; - const double wrist_roll_rotor_gear_ratio_ = 6.0; - const Vec3 wrist_roll_rotor_belt_ratios_{-1.0, 1.0, -1.0}; + double wrist_roll_rotor_gear_ratio_; + DVec wrist_roll_rotor_belt_ratios_; // Gripper Cluster const std::string gripper_cluster_name_ = "gripper-cluster"; @@ -113,15 +113,15 @@ namespace grbda // Gripper const std::string gripper_name_ = "gripper"; const std::string gripper_parent_name_ = "wrist-roll-link"; - Vec3 gripper_location_{0.0004, 0.0375, 0.070995}; + Vec3 gripper_location_; SpatialInertia gripper_spatial_inertia_; // Gripper Rotor const std::string gripper_rotor_name_ = "gripper-rotor"; const std::string gripper_rotor_parent_name_ = "wrist-roll-link"; - Vec3 gripper_rotor_location_{0, 0, 0}; + Vec3 gripper_rotor_location_; SpatialInertia gripper_rotor_spatial_inertia_; - const double gripper_rotor_gear_ratio_ = 2.0; + double gripper_rotor_gear_ratio_; }; } // namespace grbda diff --git a/include/grbda/Robots/Tello.hpp b/include/grbda/Robots/Tello.hpp index 87b4459a..f812632a 100644 --- a/include/grbda/Robots/Tello.hpp +++ b/include/grbda/Robots/Tello.hpp @@ -10,6 +10,7 @@ namespace grbda class Tello : public Robot { public: + Tello() {} ClusterTreeModel buildClusterTreeModel() const override; diff --git a/include/grbda/Robots/TelloNoRotors.hpp b/include/grbda/Robots/TelloNoRotors.hpp new file mode 100644 index 00000000..f94d1a63 --- /dev/null +++ b/include/grbda/Robots/TelloNoRotors.hpp @@ -0,0 +1,25 @@ +#ifndef GRBDA_ROBOTS_TELLO_NO_ROTORS_H +#define GRBDA_ROBOTS_TELLO_NO_ROTORS_H + +#include "grbda/Robots/Tello.hpp" + +namespace grbda +{ + + // Tello robot without rotors or four-bar linkage mechanisms. + // Uses plain Revolute joints for all leg DOFs. + // This provides an apples-to-apples comparison with Tello + // at the same 16 DOF (6 floating base + 10 leg joints). + template + class TelloNoRotors : public Tello + { + public: + + TelloNoRotors() {} + + ClusterTreeModel buildClusterTreeModel() const override; + }; + +} // namespace grbda + +#endif // GRBDA_ROBOTS_TELLO_NO_ROTORS_H diff --git a/include/grbda/Robots/TelloRotorsNoConstraints.hpp b/include/grbda/Robots/TelloRotorsNoConstraints.hpp new file mode 100644 index 00000000..123872d2 --- /dev/null +++ b/include/grbda/Robots/TelloRotorsNoConstraints.hpp @@ -0,0 +1,32 @@ +#ifndef GRBDA_ROBOTS_TELLO_ROTORS_NO_CONSTRAINTS_H +#define GRBDA_ROBOTS_TELLO_ROTORS_NO_CONSTRAINTS_H + +#include "grbda/Robots/Tello.hpp" + +namespace grbda +{ + + /// Tello robot with real rotor inertias but NO constraint couplings. + /// Each joint (hip-clamp, gimbal, thigh, shin, foot) is an independent + /// RevoluteWithRotor cluster with geared transmissions. + /// + /// Purpose: Isolates the computational overhead of rotor inertia dynamics + /// from the overhead of implicit loop constraints (differentials). + /// This is the second factor in the factorial design: + /// - With Rotors, Without Constraints: pure rotor inertia cost + /// + /// Removes all constraints to get an independent cluster structure, + /// isolating pure rotor inertia cost from constraint solving overhead. + template + class TelloRotorsNoConstraints : public Tello + { + public: + + TelloRotorsNoConstraints() {} + + ClusterTreeModel buildClusterTreeModel() const override; + }; + +} // namespace grbda + +#endif // GRBDA_ROBOTS_TELLO_ROTORS_NO_CONSTRAINTS_H diff --git a/include/grbda/Robots/TelloWithArms.hpp b/include/grbda/Robots/TelloWithArms.hpp index 0fef462a..afe17b12 100644 --- a/include/grbda/Robots/TelloWithArms.hpp +++ b/include/grbda/Robots/TelloWithArms.hpp @@ -10,6 +10,7 @@ namespace grbda class TelloWithArms : public Tello { public: + TelloWithArms() { _shoulderRyRotInertia << 0.0013678, 0.0000266, 0.0000021, diff --git a/include/grbda/Robots/TwoLinkChain.hpp b/include/grbda/Robots/TwoLinkChain.hpp new file mode 100644 index 00000000..ef4c3cb9 --- /dev/null +++ b/include/grbda/Robots/TwoLinkChain.hpp @@ -0,0 +1,53 @@ +#ifndef GRBDA_ROBOTS_TWO_LINK_CHAIN_H +#define GRBDA_ROBOTS_TWO_LINK_CHAIN_H + +#include "grbda/Robots/Robot.h" +#include "grbda/Dynamics/ClusterTreeModel.h" + +namespace grbda +{ + + class TwoLinkChain : public Robot<> + { + public: + TwoLinkChain() {} + + ClusterTreeModel<> buildClusterTreeModel() const override + { + ClusterTreeModel<> model; + using Revolute = ClusterJoints::Revolute<>; + + Mat3<> I3 = Mat3<>::Identity(); + + Mat3<> I1; + I1 << 0.05, 0., 0., + 0., 0.03, 0., + 0., 0., 0.01; + Vec3<> com1(0.15, 0., 0.); + SpatialInertia<> link1_inertia(2.0, com1, I1); + + Mat3<> I2; + I2 << 0.02, 0., 0., + 0., 0.015, 0., + 0., 0., 0.01; + Vec3<> com2(0.1, 0., 0.); + SpatialInertia<> link2_inertia(1.5, com2, I2); + + spatial::Transform<> Xtree1(I3, Vec3<>::Zero()); + model.template appendBody( + "link1", link1_inertia, "ground", Xtree1, + ori::CoordinateAxis::Z, "joint1"); + + Vec3<> r2(0.3, 0., 0.); + spatial::Transform<> Xtree2(I3, r2); + model.template appendBody( + "link2", link2_inertia, "link1", Xtree2, + ori::CoordinateAxis::Z, "joint2"); + + return model; + } + }; + +} // namespace grbda + +#endif // GRBDA_ROBOTS_TWO_LINK_CHAIN_H diff --git a/include/grbda/Utils/CasadiDerivatives.h b/include/grbda/Utils/CasadiDerivatives.h new file mode 100644 index 00000000..03f83e36 --- /dev/null +++ b/include/grbda/Utils/CasadiDerivatives.h @@ -0,0 +1,118 @@ +#ifndef GRBDA_CASADI_DERIVATIVES_H +#define GRBDA_CASADI_DERIVATIVES_H + +#include +#include +#include "grbda/Utils/cppTypes.h" +#include "grbda/Utils/SpatialTransforms.h" + +namespace grbda +{ +namespace casadi_derivatives +{ + +/// @brief Cross-product matrix (skew-symmetric) for CasADi SX +inline casadi::SX crossMatrix(const casadi::SX& v) +{ + casadi::SX crm = casadi::SX::zeros(3, 3); + crm(0, 1) = -v(2); + crm(0, 2) = v(1); + crm(1, 0) = v(2); + crm(1, 2) = -v(0); + crm(2, 0) = -v(1); + crm(2, 1) = v(0); + return crm; +} + +/// @brief Spatial cross-product matrix (6x6) for motion vectors +inline casadi::SX spatialCrossMatrix(const casadi::SX& v) +{ + casadi::SX angular = v(casadi::Slice(0, 3)); + casadi::SX linear = v(casadi::Slice(3, 6)); + + casadi::SX crm_ang = crossMatrix(angular); + casadi::SX crm_lin = crossMatrix(linear); + + casadi::SX result = casadi::SX::zeros(6, 6); + result(casadi::Slice(0, 3), casadi::Slice(0, 3)) = crm_ang; + result(casadi::Slice(3, 6), casadi::Slice(0, 3)) = crm_lin; + result(casadi::Slice(3, 6), casadi::Slice(3, 6)) = crm_ang; + + return result; +} + +/// @brief Rotation matrix from axis-angle using CasADi (symbolic) +inline casadi::SX rotationMatrix(char axis, const casadi::SX& angle) +{ + casadi::SX c = cos(angle); + casadi::SX s = sin(angle); + casadi::SX R = casadi::SX::eye(3); + + // NOTE: Using frame transformation convention (passive rotation) to match coordinateRotation + // This is the TRANSPOSE of the standard active rotation matrix + if (axis == 'X' || axis == 'x') + { + R(1, 1) = c; R(1, 2) = s; + R(2, 1) = -s; R(2, 2) = c; + } + else if (axis == 'Y' || axis == 'y') + { + R(0, 0) = c; R(0, 2) = -s; + R(2, 0) = s; R(2, 2) = c; + } + else if (axis == 'Z' || axis == 'z') + { + R(0, 0) = c; R(0, 1) = s; + R(1, 0) = -s; R(1, 1) = c; + } + else + throw std::runtime_error("rotationMatrix: invalid axis '" + std::string(1, axis) + "'"); + + return R; +} + +/// @brief Spatial transform matrix from rotation (6x6) +inline casadi::SX spatialRotation(char axis, const casadi::SX& angle) +{ + casadi::SX R = rotationMatrix(axis, angle); + casadi::SX X = casadi::SX::zeros(6, 6); + X(casadi::Slice(0, 3), casadi::Slice(0, 3)) = R; + X(casadi::Slice(3, 6), casadi::Slice(3, 6)) = R; + return X; +} + +/// @brief Joint motion subspace for revolute joint +inline casadi::SX revoluteMotionSubspace(char axis) +{ + casadi::SX S = casadi::SX::zeros(6, 1); + if (axis == 'X' || axis == 'x') + S(0) = 1.0; + else if (axis == 'Y' || axis == 'y') + S(1) = 1.0; + else if (axis == 'Z' || axis == 'z') + S(2) = 1.0; + else + throw std::runtime_error("revoluteMotionSubspace: invalid axis '" + std::string(1, axis) + "'"); + return S; +} + +/// @brief Transform a motion vector using spatial transform +/// Formula: m_out = [E * m_angular, -E * [r]× * m_angular + E * m_linear] +/// where E is rotation matrix and r is translation vector +inline casadi::SX spatialTransformMotionVector(const casadi::SX& E, const casadi::SX& r, const casadi::SX& m_in) +{ + casadi::SX m_angular = m_in(casadi::Slice(0, 3)); + casadi::SX m_linear = m_in(casadi::Slice(3, 6)); + + casadi::SX r_cross = crossMatrix(r); + + casadi::SX m_out_angular = mtimes(E, m_angular); + casadi::SX m_out_linear = -mtimes(E, mtimes(r_cross, m_angular)) + mtimes(E, m_linear); + + return vertcat(m_out_angular, m_out_linear); +} + +} // namespace casadi_derivatives +} // namespace grbda + +#endif // GRBDA_CASADI_DERIVATIVES_H diff --git a/include/grbda/Utils/OrientationTools.h b/include/grbda/Utils/OrientationTools.h index 758836c5..4a5ec9df 100644 --- a/include/grbda/Utils/OrientationTools.h +++ b/include/grbda/Utils/OrientationTools.h @@ -261,9 +261,9 @@ namespace grbda Mat3 R; - R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3), 2 * (e1 * e3 + e0 * e2), - 2 * (e1 * e2 + e0 * e3), 1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1), - 2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1), 1 - 2 * (e1 * e1 + e2 * e2); + R << typename T::Scalar(1.) - typename T::Scalar(2) * (e2 * e2 + e3 * e3), typename T::Scalar(2.) * (e1 * e2 - e0 * e3), typename T::Scalar(2.) * (e1 * e3 + e0 * e2), + typename T::Scalar(2.) * (e1 * e2 + e0 * e3), typename T::Scalar(1.) - typename T::Scalar(2.) * (e1 * e1 + e3 * e3), typename T::Scalar(2.) * (e2 * e3 - e0 * e1), + typename T::Scalar(2.) * (e1 * e3 - e0 * e2), typename T::Scalar(2.) * (e2 * e3 + e0 * e1), typename T::Scalar(1.) - typename T::Scalar(2.) * (e1 * e1 + e2 * e2); R.transposeInPlace(); return R; } @@ -491,7 +491,7 @@ namespace grbda T theta = sqrt(so3[0] * so3[0] + so3[1] * so3[1] + so3[2] * so3[2]); - if (fabs(theta) < 1.e-6) + if (fabs(theta) < 1.e-20) { quat.setZero(); quat[0] = 1.; diff --git a/include/grbda/Utils/Spatial.h b/include/grbda/Utils/Spatial.h index 90df52cc..4033f835 100644 --- a/include/grbda/Utils/Spatial.h +++ b/include/grbda/Utils/Spatial.h @@ -142,33 +142,361 @@ namespace grbda } /*! - * Compute spatial motion cross product. Faster than the matrix multiplication - * version - * - * This is a general formulation to deal with the the ability of aggregate bodies - * to have 6N dimensional spatial velocities + * Compute spatial motion cross product into a pre-allocated output vector. */ template - DVec generalMotionCrossProduct(const DVec &a, const DVec &b) + void generalMotionCrossProduct(const DVec &a, const DVec &b, DVec &out) { const int n = a.rows(); if (n != b.rows()) throw std::runtime_error("General Motion Cross Product requires vectors of the same size"); if (n == 6) - return motionCrossProduct(a.template head<6>(), b.template head<6>()); + out = motionCrossProduct(a.template head<6>(), b.template head<6>()); else if (n % 6 == 0) { - DVec mv = DVec::Zero(n); + out.setZero(n); for (int i = 0; i < n / 6; i++) - mv.template segment<6>(6 * i) = motionCrossProduct(a.template segment<6>(6 * i), - b.template segment<6>(6 * i)); - return mv; + out.template segment<6>(6 * i) = motionCrossProduct(a.template segment<6>(6 * i), + b.template segment<6>(6 * i)); } else throw std::runtime_error("Invalid number of rows provided to General Motion Cross Product"); } + template + DVec generalMotionCrossProduct(const DVec &a, const DVec &b) + { + DVec out; + generalMotionCrossProduct(a, b, out); + return out; + } + + /*! + * Compute motion cross matrix times a matrix: out = crm(v) * M + * Writes directly into the pre-allocated output matrix to avoid heap allocation. + */ + template + void motionCrossTimesMatrix(const DVec &v, const DMat &M, DMat &out) + { + const int n = v.rows(); + const int cols = M.cols(); + + if (n == 6) + { + out.resize(6, cols); + for (int c = 0; c < cols; ++c) + { + out(0, c) = -v(2)*M(1,c) + v(1)*M(2,c); + out(1, c) = v(2)*M(0,c) - v(0)*M(2,c); + out(2, c) = -v(1)*M(0,c) + v(0)*M(1,c); + out(3, c) = -v(5)*M(1,c) + v(4)*M(2,c) - v(2)*M(4,c) + v(1)*M(5,c); + out(4, c) = v(5)*M(0,c) - v(3)*M(2,c) + v(2)*M(3,c) - v(0)*M(5,c); + out(5, c) = -v(4)*M(0,c) + v(3)*M(1,c) - v(1)*M(3,c) + v(0)*M(4,c); + } + } + else if (n % 6 == 0) + { + out.setZero(n, cols); + const int num_bodies = n / 6; + for (int b = 0; b < num_bodies; ++b) + { + const int o = 6 * b; + for (int c = 0; c < cols; ++c) + { + out(o+0, c) = -v(o+2)*M(o+1,c) + v(o+1)*M(o+2,c); + out(o+1, c) = v(o+2)*M(o+0,c) - v(o+0)*M(o+2,c); + out(o+2, c) = -v(o+1)*M(o+0,c) + v(o+0)*M(o+1,c); + out(o+3, c) = -v(o+5)*M(o+1,c) + v(o+4)*M(o+2,c) - v(o+2)*M(o+4,c) + v(o+1)*M(o+5,c); + out(o+4, c) = v(o+5)*M(o+0,c) - v(o+3)*M(o+2,c) + v(o+2)*M(o+3,c) - v(o+0)*M(o+5,c); + out(o+5, c) = -v(o+4)*M(o+0,c) + v(o+3)*M(o+1,c) - v(o+1)*M(o+3,c) + v(o+0)*M(o+4,c); + } + } + } + else + { + throw std::runtime_error("Invalid dimension for motionCrossTimesMatrix"); + } + } + + template + DMat motionCrossTimesMatrix(const DVec &v, const DMat &M) + { + DMat out; + motionCrossTimesMatrix(v, M, out); + return out; + } + + /*! + * Compute motion cross matrix times a matrix and accumulate: out += crm(v) * M + */ + template + void addMotionCrossTimesMatrix(const DVec &v, const DMat &M, DMat &out) + { + const int n = v.rows(); + const int cols = M.cols(); + + if (n == 6) + { + for (int c = 0; c < cols; ++c) + { + out(0, c) += -v(2)*M(1,c) + v(1)*M(2,c); + out(1, c) += v(2)*M(0,c) - v(0)*M(2,c); + out(2, c) += -v(1)*M(0,c) + v(0)*M(1,c); + out(3, c) += -v(5)*M(1,c) + v(4)*M(2,c) - v(2)*M(4,c) + v(1)*M(5,c); + out(4, c) += v(5)*M(0,c) - v(3)*M(2,c) + v(2)*M(3,c) - v(0)*M(5,c); + out(5, c) += -v(4)*M(0,c) + v(3)*M(1,c) - v(1)*M(3,c) + v(0)*M(4,c); + } + } + else if (n % 6 == 0) + { + const int num_bodies = n / 6; + for (int b = 0; b < num_bodies; ++b) + { + const int o = 6 * b; + for (int c = 0; c < cols; ++c) + { + out(o+0, c) += -v(o+2)*M(o+1,c) + v(o+1)*M(o+2,c); + out(o+1, c) += v(o+2)*M(o+0,c) - v(o+0)*M(o+2,c); + out(o+2, c) += -v(o+1)*M(o+0,c) + v(o+0)*M(o+1,c); + out(o+3, c) += -v(o+5)*M(o+1,c) + v(o+4)*M(o+2,c) - v(o+2)*M(o+4,c) + v(o+1)*M(o+5,c); + out(o+4, c) += v(o+5)*M(o+0,c) - v(o+3)*M(o+2,c) + v(o+2)*M(o+3,c) - v(o+0)*M(o+5,c); + out(o+5, c) += -v(o+4)*M(o+0,c) + v(o+3)*M(o+1,c) - v(o+1)*M(o+3,c) + v(o+0)*M(o+4,c); + } + } + } + else + { + throw std::runtime_error("Invalid dimension for addMotionCrossTimesMatrix"); + } + } + + /*! + * Compute matrix times motion cross matrix: M * crm(v) + * This avoids building the full 6x6 cross-product matrix. + * Result(i,j) = sum_k M(i,k) * crm(v)(k,j) + */ + template + DMat matrixTimesMotionCross(const DMat &M, const DVec &v) + { + const int rows = M.rows(); + const int n = v.rows(); + + if (n == 6) + { + // crm(v) structure (from motionCrossMatrix): + // [ 0 -v2 v1 0 0 0 ] + // [ v2 0 -v0 0 0 0 ] + // [-v1 v0 0 0 0 0 ] + // [ 0 -v5 v4 0 -v2 v1 ] + // [ v5 0 -v3 v2 0 -v0 ] + // [-v4 v3 0 -v1 v0 0 ] + // Column j of result = M * (column j of crm(v)) + DMat result(rows, 6); + for (int r = 0; r < rows; ++r) + { + // Col 0 of crm: [0, v2, -v1, 0, v5, -v4]^T + result(r, 0) = v(2)*M(r,1) - v(1)*M(r,2) + v(5)*M(r,4) - v(4)*M(r,5); + // Col 1 of crm: [-v2, 0, v0, -v5, 0, v3]^T + result(r, 1) = -v(2)*M(r,0) + v(0)*M(r,2) - v(5)*M(r,3) + v(3)*M(r,5); + // Col 2 of crm: [v1, -v0, 0, v4, -v3, 0]^T + result(r, 2) = v(1)*M(r,0) - v(0)*M(r,1) + v(4)*M(r,3) - v(3)*M(r,4); + // Col 3 of crm: [0, 0, 0, 0, v2, -v1]^T + result(r, 3) = v(2)*M(r,4) - v(1)*M(r,5); + // Col 4 of crm: [0, 0, 0, -v2, 0, v0]^T + result(r, 4) = -v(2)*M(r,3) + v(0)*M(r,5); + // Col 5 of crm: [0, 0, 0, v1, -v0, 0]^T + result(r, 5) = v(1)*M(r,3) - v(0)*M(r,4); + } + return result; + } + else if (n % 6 == 0 && rows % 6 == 0) + { + // Block diagonal case: M and crm(v) are both block-diagonal + DMat result = DMat::Zero(rows, n); + const int num_bodies = n / 6; + for (int b = 0; b < num_bodies; ++b) + { + const int o = 6 * b; + for (int r = 0; r < 6; ++r) + { + const int rr = o + r; + result(rr, o+0) = v(o+2)*M(rr,o+1) - v(o+1)*M(rr,o+2) + v(o+5)*M(rr,o+4) - v(o+4)*M(rr,o+5); + result(rr, o+1) = -v(o+2)*M(rr,o+0) + v(o+0)*M(rr,o+2) - v(o+5)*M(rr,o+3) + v(o+3)*M(rr,o+5); + result(rr, o+2) = v(o+1)*M(rr,o+0) - v(o+0)*M(rr,o+1) + v(o+4)*M(rr,o+3) - v(o+3)*M(rr,o+4); + result(rr, o+3) = v(o+2)*M(rr,o+4) - v(o+1)*M(rr,o+5); + result(rr, o+4) = -v(o+2)*M(rr,o+3) + v(o+0)*M(rr,o+5); + result(rr, o+5) = v(o+1)*M(rr,o+3) - v(o+0)*M(rr,o+4); + } + } + return result; + } + else + { + throw std::runtime_error("Invalid dimension for matrixTimesMotionCross"); + } + } + + /*! + * Compute crf(v)*I - I*crm(v) for spatial inertia I and velocity v. + * Writes directly into the pre-allocated output matrix to avoid heap allocation. + */ + template + void spatialInertiaCrossTerms(const DMat &I, const DVec &v, DMat &out) + { + const int n = v.rows(); + + if (n == 6) + { + out.resize(6, 6); + for (int r = 0; r < 6; ++r) + { + for (int c = 0; c < 6; ++c) + { + Scalar crf_part; + switch (r) { + case 0: crf_part = -v(2)*I(1,c) + v(1)*I(2,c) - v(5)*I(4,c) + v(4)*I(5,c); break; + case 1: crf_part = v(2)*I(0,c) - v(0)*I(2,c) + v(5)*I(3,c) - v(3)*I(5,c); break; + case 2: crf_part = -v(1)*I(0,c) + v(0)*I(1,c) - v(4)*I(3,c) + v(3)*I(4,c); break; + case 3: crf_part = -v(2)*I(4,c) + v(1)*I(5,c); break; + case 4: crf_part = v(2)*I(3,c) - v(0)*I(5,c); break; + case 5: crf_part = -v(1)*I(3,c) + v(0)*I(4,c); break; + default: crf_part = Scalar(0); break; + } + + Scalar crm_part; + switch (c) { + case 0: crm_part = v(2)*I(r,1) - v(1)*I(r,2) + v(5)*I(r,4) - v(4)*I(r,5); break; + case 1: crm_part = -v(2)*I(r,0) + v(0)*I(r,2) - v(5)*I(r,3) + v(3)*I(r,5); break; + case 2: crm_part = v(1)*I(r,0) - v(0)*I(r,1) + v(4)*I(r,3) - v(3)*I(r,4); break; + case 3: crm_part = v(2)*I(r,4) - v(1)*I(r,5); break; + case 4: crm_part = -v(2)*I(r,3) + v(0)*I(r,5); break; + case 5: crm_part = v(1)*I(r,3) - v(0)*I(r,4); break; + default: crm_part = Scalar(0); break; + } + + out(r, c) = crf_part - crm_part; + } + } + } + else if (n % 6 == 0) + { + out.setZero(n, n); + const int num_bodies = n / 6; + for (int b = 0; b < num_bodies; ++b) + { + const int o = 6 * b; + for (int r = 0; r < 6; ++r) + { + for (int c = 0; c < 6; ++c) + { + const int rr = o + r; + const int cc = o + c; + + Scalar crf_part; + switch (r) { + case 0: crf_part = -v(o+2)*I(o+1,cc) + v(o+1)*I(o+2,cc) - v(o+5)*I(o+4,cc) + v(o+4)*I(o+5,cc); break; + case 1: crf_part = v(o+2)*I(o+0,cc) - v(o+0)*I(o+2,cc) + v(o+5)*I(o+3,cc) - v(o+3)*I(o+5,cc); break; + case 2: crf_part = -v(o+1)*I(o+0,cc) + v(o+0)*I(o+1,cc) - v(o+4)*I(o+3,cc) + v(o+3)*I(o+4,cc); break; + case 3: crf_part = -v(o+2)*I(o+4,cc) + v(o+1)*I(o+5,cc); break; + case 4: crf_part = v(o+2)*I(o+3,cc) - v(o+0)*I(o+5,cc); break; + case 5: crf_part = -v(o+1)*I(o+3,cc) + v(o+0)*I(o+4,cc); break; + default: crf_part = Scalar(0); break; + } + + Scalar crm_part; + switch (c) { + case 0: crm_part = v(o+2)*I(rr,o+1) - v(o+1)*I(rr,o+2) + v(o+5)*I(rr,o+4) - v(o+4)*I(rr,o+5); break; + case 1: crm_part = -v(o+2)*I(rr,o+0) + v(o+0)*I(rr,o+2) - v(o+5)*I(rr,o+3) + v(o+3)*I(rr,o+5); break; + case 2: crm_part = v(o+1)*I(rr,o+0) - v(o+0)*I(rr,o+1) + v(o+4)*I(rr,o+3) - v(o+3)*I(rr,o+4); break; + case 3: crm_part = v(o+2)*I(rr,o+4) - v(o+1)*I(rr,o+5); break; + case 4: crm_part = -v(o+2)*I(rr,o+3) + v(o+0)*I(rr,o+5); break; + case 5: crm_part = v(o+1)*I(rr,o+3) - v(o+0)*I(rr,o+4); break; + default: crm_part = Scalar(0); break; + } + + out(rr, cc) = crf_part - crm_part; + } + } + } + } + else + { + throw std::runtime_error("Invalid dimension for spatialInertiaCrossTerms"); + } + } + + template + DMat spatialInertiaCrossTerms(const DMat &I, const DVec &v) + { + DMat out; + spatialInertiaCrossTerms(I, v, out); + return out; + } + + /*! + * Compute force cross matrix times a matrix: crf(v) * M + * This avoids building the full 6x6 cross-product matrix. + */ + template + DMat forceCrossTimesMatrix(const DVec &v, const DMat &M) + { + const int n = v.rows(); + const int cols = M.cols(); + + if (n == 6) + { + // crf(v) structure (from forceCrossMatrix): + // [ 0 -v2 v1 0 -v5 v4 ] + // [ v2 0 -v0 v5 0 -v3 ] + // [-v1 v0 0 -v4 v3 0 ] + // [ 0 0 0 0 -v2 v1 ] + // [ 0 0 0 v2 0 -v0 ] + // [ 0 0 0 -v1 v0 0 ] + // Row i of result = dot product of row i of crf(v) with column c of M + DMat result(6, cols); + for (int c = 0; c < cols; ++c) + { + // Row 0: [0, -v2, v1, 0, -v5, v4] . M(:,c) + result(0, c) = -v(2)*M(1,c) + v(1)*M(2,c) - v(5)*M(4,c) + v(4)*M(5,c); + // Row 1: [v2, 0, -v0, v5, 0, -v3] . M(:,c) + result(1, c) = v(2)*M(0,c) - v(0)*M(2,c) + v(5)*M(3,c) - v(3)*M(5,c); + // Row 2: [-v1, v0, 0, -v4, v3, 0] . M(:,c) + result(2, c) = -v(1)*M(0,c) + v(0)*M(1,c) - v(4)*M(3,c) + v(3)*M(4,c); + // Row 3: [0, 0, 0, 0, -v2, v1] . M(:,c) + result(3, c) = -v(2)*M(4,c) + v(1)*M(5,c); + // Row 4: [0, 0, 0, v2, 0, -v0] . M(:,c) + result(4, c) = v(2)*M(3,c) - v(0)*M(5,c); + // Row 5: [0, 0, 0, -v1, v0, 0] . M(:,c) + result(5, c) = -v(1)*M(3,c) + v(0)*M(4,c); + } + return result; + } + else if (n % 6 == 0) + { + DMat result = DMat::Zero(n, cols); + const int num_bodies = n / 6; + for (int b = 0; b < num_bodies; ++b) + { + const int o = 6 * b; + for (int c = 0; c < cols; ++c) + { + result(o+0, c) = -v(o+2)*M(o+1,c) + v(o+1)*M(o+2,c) - v(o+5)*M(o+4,c) + v(o+4)*M(o+5,c); + result(o+1, c) = v(o+2)*M(o+0,c) - v(o+0)*M(o+2,c) + v(o+5)*M(o+3,c) - v(o+3)*M(o+5,c); + result(o+2, c) = -v(o+1)*M(o+0,c) + v(o+0)*M(o+1,c) - v(o+4)*M(o+3,c) + v(o+3)*M(o+4,c); + result(o+3, c) = -v(o+2)*M(o+4,c) + v(o+1)*M(o+5,c); + result(o+4, c) = v(o+2)*M(o+3,c) - v(o+0)*M(o+5,c); + result(o+5, c) = -v(o+1)*M(o+3,c) + v(o+0)*M(o+4,c); + } + } + return result; + } + else + { + throw std::runtime_error("Invalid dimension for forceCrossTimesMatrix"); + } + } + /*! * Compute spatial force cross product. Faster than the matrix multiplication * version @@ -187,38 +515,41 @@ namespace grbda } /*! - * Compute spatial force cross product. Faster than the matrix multiplication - * version - * - * This is a general formulation to deal with the the ability of aggregate bodies - * to have 6N dimensional spatial forces + * Compute spatial force cross product, accumulating into a pre-allocated output vector. */ template - DVec generalForceCrossProduct(const DVec &a, const DVec &b) + void addGeneralForceCrossProduct(const DVec &a, const DVec &b, DVec &out) { const int n = a.rows(); if (n != b.rows()) throw std::runtime_error("General Force Cross Product requires vectors of the same size"); if (n == 6) - return forceCrossProduct(a.template head<6>(), b.template head<6>()); + out += forceCrossProduct(a.template head<6>(), b.template head<6>()); else if (n % 6 == 0) { - DVec fv = DVec::Zero(n); for (int i = 0; i < n / 6; i++) - fv.template segment<6>(6 * i) = forceCrossProduct(a.template segment<6>(6 * i), - b.template segment<6>(6 * i)); - return fv; + out.template segment<6>(6 * i) += forceCrossProduct(a.template segment<6>(6 * i), + b.template segment<6>(6 * i)); } else - throw std::runtime_error("Invalid number of rows provided to General Motion Cross Product"); + throw std::runtime_error("Invalid number of rows provided to General Force Cross Product"); + } + + template + DVec generalForceCrossProduct(const DVec &a, const DVec &b) + { + const int n = a.rows(); + DVec fv = DVec::Zero(n); + addGeneralForceCrossProduct(a, b, fv); + return fv; } /*! - * Compute swaped force cross matrix. + * Compute swapped force cross matrix. */ template - auto swapedforceCrossMatrix(const Eigen::MatrixBase &v) + auto swappedForceCrossMatrix(const Eigen::MatrixBase &v) { Mat6 f; f << 0, v(2), -v(1), 0, v(5), -v(4), @@ -231,6 +562,155 @@ namespace grbda return f; } + /*! + * Compute swapped force cross matrix. Generalized version for multi-body clusters + * + * This is a general formulation to deal with the ability of aggregate bodies to have + * 6N dimensional spatial forces + */ + template + DMat generalSwappedForceCrossMatrix(const DVec &v) + { + const int n = v.rows(); + if (n == 6) + return swappedForceCrossMatrix(v.template head<6>()); + else if (n % 6 == 0) + { + DMat f = DMat::Zero(n, n); + for (int i = 0; i < (n / 6); i++) + f.template block<6, 6>(6 * i, 6 * i) = swappedForceCrossMatrix(v.template segment<6>(6 * i)); + return f; + } + else + throw std::runtime_error("Invalid number of rows provided to General Swapped Force Cross Matrix"); + } + + /*! + * Add swapped force cross matrix to an existing matrix in-place: M += icrf(v) + * This avoids allocating a temporary matrix for multi-body clusters. + */ + template + void addSwappedForceCrossMatrixInPlace(DMat &M, const DVec &v) + { + const int n = v.rows(); + if (n == 6) + { + // icrf(v) structure: + // [ 0 v2 -v1 0 v5 -v4 ] + // [ -v2 0 v0 -v5 0 v3 ] + // [ v1 -v0 0 v4 -v3 0 ] + // [ 0 v5 -v4 0 0 0 ] + // [ -v5 0 v3 0 0 0 ] + // [ v4 -v3 0 0 0 0 ] + M(0, 1) += v(2); M(0, 2) -= v(1); M(0, 4) += v(5); M(0, 5) -= v(4); + M(1, 0) -= v(2); M(1, 2) += v(0); M(1, 3) -= v(5); M(1, 5) += v(3); + M(2, 0) += v(1); M(2, 1) -= v(0); M(2, 3) += v(4); M(2, 4) -= v(3); + M(3, 1) += v(5); M(3, 2) -= v(4); + M(4, 0) -= v(5); M(4, 2) += v(3); + M(5, 0) += v(4); M(5, 1) -= v(3); + } + else if (n % 6 == 0) + { + const int num_bodies = n / 6; + for (int b = 0; b < num_bodies; ++b) + { + const int o = 6 * b; + M(o+0, o+1) += v(o+2); M(o+0, o+2) -= v(o+1); M(o+0, o+4) += v(o+5); M(o+0, o+5) -= v(o+4); + M(o+1, o+0) -= v(o+2); M(o+1, o+2) += v(o+0); M(o+1, o+3) -= v(o+5); M(o+1, o+5) += v(o+3); + M(o+2, o+0) += v(o+1); M(o+2, o+1) -= v(o+0); M(o+2, o+3) += v(o+4); M(o+2, o+4) -= v(o+3); + M(o+3, o+1) += v(o+5); M(o+3, o+2) -= v(o+4); + M(o+4, o+0) -= v(o+5); M(o+4, o+2) += v(o+3); + M(o+5, o+0) += v(o+4); M(o+5, o+1) -= v(o+3); + } + } + else + { + throw std::runtime_error("Invalid dimension for addSwappedForceCrossMatrixInPlace"); + } + } + + /*! + * Compute swapped force cross product: icrf(f) * s for a single 6D force and motion vector. + * This is more efficient than building the full 6x6 matrix when only one column is needed. + * Result: icrf(f) * s where icrf is the swapped force cross matrix + */ + template + auto swappedForceCrossProduct(const Eigen::MatrixBase &f, const Eigen::MatrixBase &s) + { + static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6, "Must have 6x1 vector"); + SVec result; + // icrf(f) * s where icrf is: + // [ 0 f2 -f1 0 f5 -f4 ] + // [ -f2 0 f0 -f5 0 f3 ] + // [ f1 -f0 0 f4 -f3 0 ] + // [ 0 f5 -f4 0 0 0 ] + // [ -f5 0 f3 0 0 0 ] + // [ f4 -f3 0 0 0 0 ] + result(0) = f(2)*s(1) - f(1)*s(2) + f(5)*s(4) - f(4)*s(5); + result(1) = f(0)*s(2) - f(2)*s(0) + f(3)*s(5) - f(5)*s(3); + result(2) = f(1)*s(0) - f(0)*s(1) + f(4)*s(3) - f(3)*s(4); + result(3) = f(5)*s(1) - f(4)*s(2); + result(4) = f(3)*s(2) - f(5)*s(0); + result(5) = f(4)*s(0) - f(3)*s(1); + return result; + } + + /*! + * Compute swapped force cross matrix times a matrix: out += icrf(f) * M + * Accumulates into the pre-allocated output matrix to avoid heap allocation. + */ + template + void addSwappedForceCrossTimesMatrix(const DVec &f, const DMat &M, + DMat &out) + { + const int n = f.rows(); + const int cols = M.cols(); + + if (n == 6) + { + for (int c = 0; c < cols; ++c) + { + out(0, c) += f(2)*M(1,c) - f(1)*M(2,c) + f(5)*M(4,c) - f(4)*M(5,c); + out(1, c) += f(0)*M(2,c) - f(2)*M(0,c) + f(3)*M(5,c) - f(5)*M(3,c); + out(2, c) += f(1)*M(0,c) - f(0)*M(1,c) + f(4)*M(3,c) - f(3)*M(4,c); + out(3, c) += f(5)*M(1,c) - f(4)*M(2,c); + out(4, c) += f(3)*M(2,c) - f(5)*M(0,c); + out(5, c) += f(4)*M(0,c) - f(3)*M(1,c); + } + } + else if (n % 6 == 0) + { + const int num_bodies = n / 6; + for (int b = 0; b < num_bodies; ++b) + { + const int o = 6 * b; + for (int c = 0; c < cols; ++c) + { + out(o+0, c) += f(o+2)*M(o+1,c) - f(o+1)*M(o+2,c) + f(o+5)*M(o+4,c) - f(o+4)*M(o+5,c); + out(o+1, c) += f(o+0)*M(o+2,c) - f(o+2)*M(o+0,c) + f(o+3)*M(o+5,c) - f(o+5)*M(o+3,c); + out(o+2, c) += f(o+1)*M(o+0,c) - f(o+0)*M(o+1,c) + f(o+4)*M(o+3,c) - f(o+3)*M(o+4,c); + out(o+3, c) += f(o+5)*M(o+1,c) - f(o+4)*M(o+2,c); + out(o+4, c) += f(o+3)*M(o+2,c) - f(o+5)*M(o+0,c); + out(o+5, c) += f(o+4)*M(o+0,c) - f(o+3)*M(o+1,c); + } + } + } + else + { + throw std::runtime_error("Invalid dimension for addSwappedForceCrossTimesMatrix"); + } + } + + template + DMat swappedForceCrossTimesMatrix(const DVec &f, const DMat &M) + { + const int n = f.rows(); + const int cols = M.cols(); + DMat out = DMat::Zero(n, cols); + addSwappedForceCrossTimesMatrix(f, M, out); + return out; + } + /*! * Create spatial coordinate transformation from rotation and translation */ @@ -473,6 +953,81 @@ namespace grbda return fs; } + /*! + * Multiply block-diagonal inertia (stored as vector) by a spatial velocity vector. + * Equivalent to assembling the block-diagonal DMat and multiplying, but avoids the allocation. + */ + template + DVec blockDiagonalTimesVector( + const std::vector, Eigen::aligned_allocator>> &I, + const DVec &v) + { + const int n = (int)I.size(); + DVec out(6 * n); + for (int i = 0; i < n; i++) + out.template segment<6>(6 * i).noalias() = I[i] * v.template segment<6>(6 * i); + return out; + } + + /*! + * Convert block-diagonal inertia (stored as vector) to a full block-diagonal DMat. + * Only use when a dense matrix is unavoidable (e.g. initializing IA_). + */ + template + DMat blockDiagonalToMatrix( + const std::vector, Eigen::aligned_allocator>> &I) + { + const int n = (int)I.size(); + DMat M = DMat::Zero(6 * n, 6 * n); + for (int i = 0; i < n; i++) + M.template block<6, 6>(6 * i, 6 * i) = I[i]; + return M; + } + + /*! + * Compute crf(v)*I - I*crm(v) where I is stored as a vector of 6x6 blocks. + */ + template + void spatialInertiaCrossTerms( + const std::vector, Eigen::aligned_allocator>> &I_blocks, + const DVec &v, DMat &out) + { + const int n = (int)I_blocks.size(); + out.setZero(6 * n, 6 * n); + for (int b = 0; b < n; b++) + { + const int o = 6 * b; + const Mat6 &I = I_blocks[b]; + for (int r = 0; r < 6; ++r) + { + for (int c = 0; c < 6; ++c) + { + Scalar crf_part; + switch (r) { + case 0: crf_part = -v(o+2)*I(1,c) + v(o+1)*I(2,c) - v(o+5)*I(4,c) + v(o+4)*I(5,c); break; + case 1: crf_part = v(o+2)*I(0,c) - v(o+0)*I(2,c) + v(o+5)*I(3,c) - v(o+3)*I(5,c); break; + case 2: crf_part = -v(o+1)*I(0,c) + v(o+0)*I(1,c) - v(o+4)*I(3,c) + v(o+3)*I(4,c); break; + case 3: crf_part = -v(o+2)*I(4,c) + v(o+1)*I(5,c); break; + case 4: crf_part = v(o+2)*I(3,c) - v(o+0)*I(5,c); break; + case 5: crf_part = -v(o+1)*I(3,c) + v(o+0)*I(4,c); break; + default: crf_part = Scalar(0); break; + } + Scalar crm_part; + switch (c) { + case 0: crm_part = v(o+2)*I(r,1) - v(o+1)*I(r,2) + v(o+5)*I(r,4) - v(o+4)*I(r,5); break; + case 1: crm_part = -v(o+2)*I(r,0) + v(o+0)*I(r,2) - v(o+5)*I(r,3) + v(o+3)*I(r,5); break; + case 2: crm_part = v(o+1)*I(r,0) - v(o+0)*I(r,1) + v(o+4)*I(r,3) - v(o+3)*I(r,4); break; + case 3: crm_part = v(o+2)*I(r,4) - v(o+1)*I(r,5); break; + case 4: crm_part = -v(o+2)*I(r,3) + v(o+0)*I(r,5); break; + case 5: crm_part = v(o+1)*I(r,3) - v(o+0)*I(r,4); break; + default: crm_part = Scalar(0); break; + } + out(o+r, o+c) = crf_part - crm_part; + } + } + } + } + } // namespace spatial } // namespace grbda diff --git a/include/grbda/Utils/SpatialTransforms.h b/include/grbda/Utils/SpatialTransforms.h index a6c26b15..d98cfd9d 100644 --- a/include/grbda/Utils/SpatialTransforms.h +++ b/include/grbda/Utils/SpatialTransforms.h @@ -14,6 +14,7 @@ namespace grbda class Transform { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW Transform(const Mat3 &E = Mat3::Identity(), const Vec3 &r = Vec3::Zero()); @@ -33,10 +34,19 @@ namespace grbda Mat6 inverseTransformSpatialInertia(const Mat6 &I_in) const; + // Transform spatial inertia from body frame to world frame + // I_world = X^{-T} * I_body * X^{-1} where X transforms world -> body + // This is the inverse operation of inverseTransformSpatialInertia + Mat6 transformSpatialInertiaToWorld(const Mat6 &I_in) const; + D6Mat transformMotionSubspace(const D6Mat &S_in) const; D6Mat inverseTransformMotionSubspace(const D6Mat &S_in) const; D6Mat inverseTransformForceSubspace(const D6Mat &F_in) const; + // Batched version: transforms 4 force subspaces with a single computation of E^T and r_hat*E^T + void inverseTransformForceSubspace4( + DMat &F1, DMat &F2, DMat &F3, DMat &F4) const; + Transform operator*(const Transform &X_in) const; const Mat3 &getRotation() const { return E_; } @@ -55,6 +65,7 @@ namespace grbda class GeneralizedAbsoluteTransform { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW GeneralizedAbsoluteTransform(){}; void appendTransform(const Transform &X); @@ -68,15 +79,22 @@ namespace grbda Transform &operator[](int output_body_index); + // World-frame CRBA support methods + // Transforms motion subspace from local body frames to world frame + // S_local has rows grouped by body, each group in that body's local frame + // Returns S_world with all rows in world frame + DMat transformMotionSubspaceToWorld(const DMat &S_local) const; + private: int num_output_bodies_ = 0; - std::vector> transforms_; + std::vector,Eigen::aligned_allocator>> transforms_; }; template class GeneralizedTransform { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW GeneralizedTransform(int num_parent_bodies); void appendTransformWithClusterAncestorSubIndex(const Transform &X, @@ -99,6 +117,7 @@ namespace grbda DMat inverseTransformSpatialInertia(const DMat &I_in) const; Transform &operator[](int output_body_index); + const Transform &operator[](int output_body_index) const; GeneralizedTransform operator*(const GeneralizedTransform &X_in) const; GeneralizedAbsoluteTransform operator*( const GeneralizedAbsoluteTransform &X_in) const; @@ -106,10 +125,46 @@ namespace grbda DMat rightMultiplyMotionTransform(const DMat &M_in) const; DMat leftMultiplyForceTransform(const DMat &M_in) const; + // Accumulates child's composite inertia blocks to parent's composite inertia blocks. + void accumulateBlockDiagonalInertia( + const std::vector, Eigen::aligned_allocator>> &I_child, + std::vector, Eigen::aligned_allocator>> &I_parent) const; + + // Accumulates two block-diagonal child matrices to corresponding parent matrices. + // Transforms and adds each 6x6 block; does not assume any structure within the blocks. + void accumulateBlockDiagonalPair( + const DMat &M1_child, DMat &M1_parent, + const DMat &M2_child, DMat &M2_parent) const; + + // Computes F = Ic * S exploiting block structure of Ic (vector form). + DMat blockDiagonalInertiaTimesMotionSubspace( + const std::vector, Eigen::aligned_allocator>> &Ic, + const DMat &S) const; + void blockDiagonalInertiaTimesMotionSubspace( + const std::vector, Eigen::aligned_allocator>> &Ic, + const DMat &S, DMat &out) const; + + // Computes F = Ic * S exploiting block-diagonal structure (DMat form, for M_cup/B_cup). + DMat blockDiagonalInertiaTimesMotionSubspace( + const DMat &Ic_block_diag, const DMat &S) const; + void blockDiagonalInertiaTimesMotionSubspace( + const DMat &Ic_block_diag, const DMat &S, DMat &out) const; + + // Transforms F from child frame to parent frame, accumulating to connected parent bodies. + // This is similar to inverseTransformForceSubspace but optimized for the CRBA pattern + // where we know the structure comes from block-diagonal Ic * S. + DMat transformForceSubspaceToParent(const DMat &F_in) const; + + // Batched version: transforms 4 force subspaces in one call. + // For single-body to single-body, delegates to Transform::inverseTransformForceSubspace4. + // For multi-body clusters, shares rotation computation across bodies and matrices. + void inverseTransformForceSubspace4( + DMat &F1, DMat &F2, DMat &F3, DMat &F4) const; + private: int num_output_bodies_ = 0; const int num_parent_bodies_ = 0; - std::vector, int>> transforms_and_parent_subindices_; + std::vector, int>, Eigen::aligned_allocator, int>>> transforms_and_parent_subindices_; }; } // namespace spatial diff --git a/include/grbda/Utils/StateRepresentation.h b/include/grbda/Utils/StateRepresentation.h index 531faa5e..e014bc04 100644 --- a/include/grbda/Utils/StateRepresentation.h +++ b/include/grbda/Utils/StateRepresentation.h @@ -16,7 +16,11 @@ namespace grbda JointCoordinate(const JointCoordinate &other) : DVec(other), _is_spanning(other._is_spanning) {} - const bool &isSpanning() const { return _is_spanning; } + // Move constructor + JointCoordinate(JointCoordinate &&other) noexcept + : DVec(std::move(other)), _is_spanning(other._is_spanning) {} + + bool isSpanning() const { return _is_spanning; } JointCoordinate &operator=(const JointCoordinate &other) { @@ -25,6 +29,14 @@ namespace grbda return *this; } + // Move assignment operator + JointCoordinate &operator=(JointCoordinate &&other) noexcept + { + this->DVec::operator=(std::move(other)); + _is_spanning = other._is_spanning; + return *this; + } + template JointCoordinate &operator=(const Eigen::DenseBase &x) { @@ -50,6 +62,18 @@ namespace grbda : position(JointCoordinate(DVec::Zero(0), false)), velocity(JointCoordinate(DVec::Zero(0), false)) {} + // Copy constructor (explicitly defaulted for clarity) + JointState(const JointState &other) = default; + + // Move constructor + JointState(JointState &&other) noexcept = default; + + // Copy assignment operator + JointState &operator=(const JointState &other) = default; + + // Move assignment operator + JointState &operator=(JointState &&other) noexcept = default; + JointCoordinate position; JointCoordinate velocity; }; diff --git a/include/grbda/Utils/Utilities.h b/include/grbda/Utils/Utilities.h index e47635a0..6cf7cf93 100644 --- a/include/grbda/Utils/Utilities.h +++ b/include/grbda/Utils/Utilities.h @@ -6,6 +6,7 @@ #ifndef GRBDA_UTILITIES_H #define GRBDA_UTILITIES_H +#include #include #include "cppTypes.h" @@ -124,7 +125,7 @@ namespace grbda template bool nearZero(const Eigen::MatrixBase &vec, const typename T::Scalar &tol = 1e-8) { - if (vec.norm() < tol) + if (vec.norm() < std::abs(tol)) return true; return false; } @@ -192,7 +193,17 @@ namespace grbda template DMat matrixLeftPseudoInverse(const Eigen::MatrixBase &mat) { - return mat.completeOrthogonalDecomposition().pseudoInverse(); + using Scalar = typename T::Scalar; + + // For complex types, use algebraic formula which is complex-step safe + if constexpr (std::is_same_v> || + std::is_same_v> || + std::is_same_v>) { + const DMat tmp = mat.transpose() * mat; + return matrixInverse(tmp) * mat.transpose(); + } else { + return mat.completeOrthogonalDecomposition().pseudoInverse(); + } } template <> @@ -210,7 +221,17 @@ namespace grbda template DMat matrixRightPseudoInverse(const Eigen::MatrixBase &mat) { - return mat.completeOrthogonalDecomposition().pseudoInverse(); + using Scalar = typename T::Scalar; + + // For complex types, use algebraic formula which is complex-step safe + if constexpr (std::is_same_v> || + std::is_same_v> || + std::is_same_v>) { + const DMat tmp = mat * mat.transpose(); + return mat.transpose() * matrixInverse(tmp); + } else { + return mat.completeOrthogonalDecomposition().pseudoInverse(); + } } template <> @@ -322,19 +343,51 @@ namespace grbda DMat Ainv_; }; + /*! + * Adapts Eigen's .inverse() to the solve() interface expected by CorrectMatrixInverseType. + * Used for scalar types where a decomposition-based solver is not available (e.g. complex, CasADi). + */ + template + class ExplicitInverse + { + public: + ExplicitInverse() {} + ExplicitInverse(const DMat &mat) : Ainv_(mat.inverse()) {} + + template + DMat solve(const Eigen::MatrixBase &b) const + { + return Ainv_ * b; + } + + private: + DMat Ainv_; + }; + template struct CorrectMatrixInverseType { using type = Eigen::ColPivHouseholderQR>; }; - // Specialization for casadi::SX template <> struct CorrectMatrixInverseType { using type = CasadiInverse; }; + template <> + struct CorrectMatrixInverseType> + { + using type = ExplicitInverse>; + }; + + template <> + struct CorrectMatrixInverseType> + { + using type = ExplicitInverse>; + }; + /*! * Finds the greatest common element between two vectors and throws an exception if none is found. * NOTE: This function assumes that the vectors are sorted in descending order. diff --git a/robot-models/kuka_lwr_4plus.urdf b/robot-models/kuka_lwr_4plus.urdf new file mode 100644 index 00000000..d834f045 --- /dev/null +++ b/robot-models/kuka_lwr_4plus.urdf @@ -0,0 +1,119 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Dynamics/ClusterJoints/ClusterJoint.cpp b/src/Dynamics/ClusterJoints/ClusterJoint.cpp index 5b72d832..fe113792 100644 --- a/src/Dynamics/ClusterJoints/ClusterJoint.cpp +++ b/src/Dynamics/ClusterJoints/ClusterJoint.cpp @@ -1,4 +1,5 @@ #include "grbda/Dynamics/ClusterJoints/ClusterJoint.h" +#include namespace grbda { @@ -17,10 +18,12 @@ namespace grbda Psi_ = DMat::Zero(motion_subspace_dimension, num_velocities_); vJ_ = DVec::Zero(motion_subspace_dimension); cJ_ = DVec::Zero(motion_subspace_dimension); + S_ring_ = DMat::Zero(motion_subspace_dimension, num_velocities_); } template - JointState Base::toSpanningTreeState(const JointState &joint_state) + JointState Base::toSpanningTreeState(const JointState &joint_state, + bool enforce_constraints) { JointState spanning_joint_state(true, true); @@ -40,8 +43,10 @@ namespace grbda } else if (joint_state.position.isSpanning() && loop_constraint_->isImplicit()) { - if (!loop_constraint_->isValidSpanningPosition(joint_state.position)) + if (enforce_constraints && !loop_constraint_->isValidSpanningPosition(joint_state.position)) { + DVec phi_val = loop_constraint_->phi(joint_state.position); + std::cerr << "Spanning position is not valid. phi = " << phi_val.transpose() << std::endl; throw std::runtime_error("Spanning position is not valid"); } spanning_joint_state.position = joint_state.position; @@ -59,7 +64,7 @@ namespace grbda } else { - if (!loop_constraint_->isValidSpanningVelocity(joint_state.velocity)) + if (enforce_constraints && !loop_constraint_->isValidSpanningVelocity(joint_state.velocity)) { throw std::runtime_error("Spanning velocity is not valid"); } @@ -71,7 +76,7 @@ namespace grbda } template - JointState Base::randomJointState() const + JointState Base::randomJointState(bool enforce_position_constraint) const { JointState joint_state(false, false); joint_state.position = DVec::Random(numPositions()); @@ -80,6 +85,7 @@ namespace grbda } template class Base; + template class Base>; template class Base; template class Base; diff --git a/src/Dynamics/ClusterJoints/FourBarJoint.cpp b/src/Dynamics/ClusterJoints/FourBarJoint.cpp index c04daa10..35cac5b5 100644 --- a/src/Dynamics/ClusterJoints/FourBarJoint.cpp +++ b/src/Dynamics/ClusterJoints/FourBarJoint.cpp @@ -1,59 +1,147 @@ #include "grbda/Dynamics/ClusterJoints/FourBarJoint.h" +#include namespace grbda { namespace LoopConstraint { - template - FourBar::FourBar(std::vector path1_link_lengths, - std::vector path2_link_lengths, - Vec2 offset, int independent_coordinate) - : links_in_path1_(path1_link_lengths.size()), - links_in_path2_(path2_link_lengths.size()), - path1_link_lengths_(path1_link_lengths), - path2_link_lengths_(path2_link_lengths), - offset_(offset), independent_coordinate_(independent_coordinate) + // --------------------------------------------------------------------------- + // Factory helpers for GenericImplicit constructor + // --------------------------------------------------------------------------- + + namespace { - if (links_in_path1_ + links_in_path2_ != 3) + // Build the is_coordinate_independent mask (3 coords, one independent) + std::vector makeFourBarIndMask(int ind_coord) { - throw std::runtime_error("FourBar: Must contain 3 links"); + std::vector mask = {false, false, false}; + mask[ind_coord] = true; + return mask; } - this->phi_ = [this, offset](const JointCoordinate &joint_pos) + // Build the symbolic phi (always SX; captures link lengths as SX constants) + template + std::function(const JointCoordinate &)> + makeFourBarSymPhi(const std::vector &p1, const std::vector &p2, + const Vec2 &off, size_t n1, size_t n2) { - DVec phi = DVec::Zero(2); + using SX = casadi::SX; - DVec path1_joints(2), path2_joints(1); - path1_joints << joint_pos(0), joint_pos(2); - path2_joints << joint_pos(1); + std::vector p1_sx, p2_sx; + Vec2 off_sx; - Scalar cumulative_angle = 0.; - DVec path1 = DVec::Zero(2); - for (size_t i = 0; i < links_in_path1_; i++) + if constexpr (std::is_same_v) { - cumulative_angle += path1_joints(i); - path1(0) += path1_link_lengths_[i] * cos(cumulative_angle); - path1(1) += path1_link_lengths_[i] * sin(cumulative_angle); + p1_sx = p1; + p2_sx = p2; + off_sx << off[0], off[1]; } - - cumulative_angle = 0.; - DVec path2 = offset; - for (size_t i = 0; i < links_in_path2_; i++) + else { - cumulative_angle += path2_joints(i); - path2(0) += path2_link_lengths_[i] * cos(cumulative_angle); - path2(1) += path2_link_lengths_[i] * sin(cumulative_angle); + using std::real; + for (const auto &x : p1) p1_sx.push_back(SX(real(x))); + for (const auto &x : p2) p2_sx.push_back(SX(real(x))); + off_sx << SX(real(off[0])), SX(real(off[1])); } - phi = path1 - path2; - return phi; - }; + return [p1_sx, p2_sx, off_sx, n1, n2](const JointCoordinate &jp) -> DVec + { + DVec pj1(2), pj2(1); + pj1 << jp(0), jp(2); + pj2 << jp(1); + + SX ca = SX(0.); + DVec path1 = DVec::Zero(2); + for (size_t i = 0; i < n1; i++) + { + ca = ca + pj1(i); + path1(0) = path1(0) + p1_sx[i] * cos(ca); + path1(1) = path1(1) + p1_sx[i] * sin(ca); + } + + DVec path2(2); + path2 << off_sx[0], off_sx[1]; + ca = SX(0.); + for (size_t i = 0; i < n2; i++) + { + ca = ca + pj2(i); + path2(0) = path2(0) + p2_sx[i] * cos(ca); + path2(1) = path2(1) + p2_sx[i] * sin(ca); + } + + DVec phi = path1 - path2; + return phi; + }; + } - this->G_ = DMat::Zero(3, 1); - this->g_ = DVec::Zero(3); + // Build the four-bar constraint function (works with any Scalar, including complex and SX) + // phi(q) = tip_of_path1(q) - tip_of_path2(q), which must equal zero at all times + template + std::function(const JointCoordinate &)> + makeFourBarPhi(std::vector path1_lengths, std::vector path2_lengths, + Vec2 path2_origin, size_t num_path1_links, size_t num_path2_links) + { + return [path1_lengths, path2_lengths, path2_origin, + num_path1_links, num_path2_links](const JointCoordinate &q) -> DVec + { + using std::cos; + using std::sin; + + // Joint angles for each path: path1 uses q(0),q(2); path2 uses q(1) + DVec path1_angles(2), path2_angles(1); + path1_angles << q(0), q(2); + path2_angles << q(1); + + Scalar cumulative_angle = Scalar(0.); + DVec tip1 = DVec::Zero(2); + for (size_t i = 0; i < num_path1_links; i++) + { + cumulative_angle += path1_angles(i); + tip1(0) += path1_lengths[i] * cos(cumulative_angle); + tip1(1) += path1_lengths[i] * sin(cumulative_angle); + } + + DVec tip2 = path2_origin; + cumulative_angle = Scalar(0.); + for (size_t i = 0; i < num_path2_links; i++) + { + cumulative_angle += path2_angles(i); + tip2(0) += path2_lengths[i] * cos(cumulative_angle); + tip2(1) += path2_lengths[i] * sin(cumulative_angle); + } + + return tip1 - tip2; + }; + } + } // anonymous namespace - this->K_ = DMat::Zero(2, 3); - this->k_ = DVec::Zero(2); + // --------------------------------------------------------------------------- + // FourBar constructor + // --------------------------------------------------------------------------- + + template + FourBar::FourBar(std::vector path1_link_lengths, + std::vector path2_link_lengths, + Vec2 offset, int independent_coordinate) + : GenericImplicit( + makeFourBarIndMask(independent_coordinate), + makeFourBarSymPhi(path1_link_lengths, path2_link_lengths, offset, + path1_link_lengths.size(), + path2_link_lengths.size())), + links_in_path1_(path1_link_lengths.size()), + links_in_path2_(path2_link_lengths.size()), + path1_link_lengths_(path1_link_lengths), + path2_link_lengths_(path2_link_lengths), + offset_(offset), + independent_coordinate_(independent_coordinate) + { + if (links_in_path1_ + links_in_path2_ != 3) + { + throw std::runtime_error("FourBar: Must contain 3 links"); + } + + this->phi_ = makeFourBarPhi(path1_link_lengths_, path2_link_lengths_, + offset_, links_in_path1_, links_in_path2_); switch (independent_coordinate_) { @@ -90,7 +178,7 @@ namespace grbda DVec q1(2), q2(1); q1 << joint_pos(0), joint_pos(2); q2 << joint_pos(1); - + Scalar cumulative_angle = 0.; DMat K1 = DMat::Zero(2, links_in_path1_); for (size_t i = 0; i < path1_link_lengths_.size(); i++) @@ -198,7 +286,6 @@ namespace grbda this->g_ = indepenent_coordinate_map_ * this->g_; } - // TODO(@MatthewChignoli): This is the same as generic joint, so do we need it? Probably not. In fact, we can probably deprecate this entire class. template void FourBar::createRandomStateHelpers() { @@ -210,20 +297,9 @@ namespace grbda using SX = casadi::SX; - // Create symbolic four bar loop constraint - std::vector path1_link_lengths_sym, path2_link_lengths_sym; - for (size_t i = 0; i < path1_link_lengths_.size(); i++) - { - path1_link_lengths_sym.push_back(path1_link_lengths_[i]); - } - for (size_t i = 0; i < path2_link_lengths_.size(); i++) - { - path2_link_lengths_sym.push_back(path2_link_lengths_[i]); - } - Vec2 offset_sym{offset_[0], offset_[1]}; - FourBar symbolic = FourBar(path1_link_lengths_sym, - path2_link_lengths_sym, - offset_sym, independent_coordinate_); + // Build a symbolic phi using the factory (avoids constructing a full FourBar) + auto sym_phi = makeFourBarSymPhi(path1_link_lengths_, path2_link_lengths_, + offset_, links_in_path1_, links_in_path2_); // Root finding { @@ -231,13 +307,11 @@ namespace grbda DVec q_sym(this->numSpanningPos()); casadi::copy(cs_q_sym, q_sym); - // Compute constraint violation JointCoordinate joint_pos(q_sym, true); - DVec phi_sx = symbolic.phi(joint_pos); + DVec phi_sx = sym_phi(joint_pos); SX cs_phi_sym = casadi::SX(casadi::Sparsity::dense(phi_sx.rows(), 1)); casadi::copy(phi_sx, cs_phi_sym); - // Slice depending on independent coordinate casadi::Slice ind_slice, dep_slice; switch (independent_coordinate_) { @@ -257,7 +331,6 @@ namespace grbda throw std::runtime_error("FourBar: Invalid independent coordinate"); } - // Create rootfinder problem casadi::SXDict rootfinder_problem; rootfinder_problem["x"] = cs_q_sym(dep_slice); rootfinder_problem["p"] = cs_q_sym(ind_slice); @@ -270,11 +343,30 @@ namespace grbda options); } - // Explicit constraint jacobian + // Explicit constraint jacobian for random state generation { SX cs_q_sym = SX::sym("q", this->numSpanningPos()); DVec q_sym(this->numSpanningPos()); casadi::copy(cs_q_sym, q_sym); + + // Use a temporary FourBar only for G (updateJacobians is analytic) + std::vector path1_sx, path2_sx; + Vec2 offset_sx; + if constexpr (std::is_same_v) + { + path1_sx = path1_link_lengths_; + path2_sx = path2_link_lengths_; + offset_sx << offset_[0], offset_[1]; + } + else + { + using std::real; + for (const auto &l : path1_link_lengths_) path1_sx.push_back(SX(real(l))); + for (const auto &l : path2_link_lengths_) path2_sx.push_back(SX(real(l))); + offset_sx << SX(real(offset_[0])), SX(real(offset_[1])); + } + FourBar symbolic(path1_sx, path2_sx, offset_sx, independent_coordinate_); + JointCoordinate joint_pos(q_sym, false); symbolic.updateJacobians(joint_pos); DMat G = symbolic.G(); @@ -285,6 +377,7 @@ namespace grbda } template struct FourBar; + template struct FourBar>; template struct FourBar; } // namespace LoopConstraint @@ -292,8 +385,11 @@ namespace grbda namespace ClusterJoints { template - JointState FourBar::randomJointState() const + JointState FourBar::randomJointState(bool enforce_position_constraint) const { + if (!enforce_position_constraint) + return Base::randomJointState(); + using DM = casadi::DM; // Create Helper functions @@ -303,7 +399,7 @@ namespace grbda const int n_ind = four_bar_constraint_->numIndependentPos(); const int n_span = four_bar_constraint_->numSpanningPos(); double ind_range = 1.0; - double dep_range = 0.1; + double dep_range = M_PI; DM q_ind, q_dep; // Call the rootfinder to get dependent position coordinates @@ -359,6 +455,7 @@ namespace grbda } template class FourBar; + template class FourBar>; template class FourBar; } } diff --git a/src/Dynamics/ClusterJoints/FreeJoint.cpp b/src/Dynamics/ClusterJoints/FreeJoint.cpp index 4900958b..f5d61433 100644 --- a/src/Dynamics/ClusterJoints/FreeJoint.cpp +++ b/src/Dynamics/ClusterJoints/FreeJoint.cpp @@ -8,7 +8,8 @@ namespace grbda template Free::Free(const Body &body, std::string name) - : Base(1, OrientationRepresentation::num_ori_parameter + 3, 6), body_(body) + : Base(1, OrientationRepresentation::num_ori_parameter + 3, 6), + body_(body) { if (body.parent_index_ >= 0) throw std::runtime_error("Free joint is only valid as the first joint in a tree and thus cannot have a parent body"); @@ -46,7 +47,7 @@ namespace grbda } template - JointState Free::randomJointState() const + JointState Free::randomJointState(bool enforce_position_constraint) const { const int num_ori_param = OrientationRepresentation::num_ori_parameter; @@ -69,8 +70,11 @@ namespace grbda return bodies_joints_and_ref_inertias; } + template class Free; template class Free; + template class Free, ori_representation::RollPitchYaw>; + template class Free, ori_representation::Quaternion>; template class Free; template class Free; template class Free; diff --git a/src/Dynamics/ClusterJoints/GenericJoint.cpp b/src/Dynamics/ClusterJoints/GenericJoint.cpp index 2af98041..3f9cfffb 100644 --- a/src/Dynamics/ClusterJoints/GenericJoint.cpp +++ b/src/Dynamics/ClusterJoints/GenericJoint.cpp @@ -24,35 +24,53 @@ namespace grbda int ind_dim = ind_coords.size(); int dep_dim = dep_coords.size(); - // The coordinate map is a matrix that maps the stacked indepedent - // coordinates [y;q_dep] to the spanning coordinate vector q such that - // q = coord_map * [y;q_dep] - SX coord_map = SX::zeros(state_dim, state_dim); + if (state_dim == 0 || ind_dim + dep_dim != state_dim) { + std::cerr << "[GenericImplicit] Invalid coordinate sizes!" << std::endl; + } + + // The coordinate map satisfies q = coord_map * [y; q_dep], + // reordering stacked independent+dependent coords into spanning order + SX coord_map_sx = SX::zeros(state_dim, state_dim); + coord_map_ = DMat::Zero(state_dim, state_dim); for (int i = 0; i < ind_dim; i++) { - coord_map(ind_coords[i], i) = 1; + coord_map_sx(ind_coords[i], i) = 1; + coord_map_(ind_coords[i], i) = 1.0; } for (int i = 0; i < dep_dim; i++) { - coord_map(dep_coords[i], i + ind_dim) = 1; + coord_map_sx(dep_coords[i], i + ind_dim) = 1; + coord_map_(dep_coords[i], i + ind_dim) = 1.0; } // Symbolic state SX cs_q_sym = SX::sym("q", state_dim, 1); DVec q_sym(state_dim); + if (cs_q_sym.size2() == 0 || state_dim == 0) { + std::cerr << "[GenericImplicit] cs_q_sym has zero size!" << std::endl; + } casadi::copy(cs_q_sym, q_sym); JointCoordinate joint_pos_sym(q_sym, true); SX cs_v_sym = SX::sym("v", state_dim, 1); DVec v_sym(state_dim); + if (cs_v_sym.size2() == 0 || state_dim == 0) { + std::cerr << "[GenericImplicit] cs_v_sym has zero size!" << std::endl; + } casadi::copy(cs_v_sym, v_sym); + casadi::Dict cse_opts; + cse_opts["cse"] = true; + // Implicit constraint violation function DVec phi_sym = phi_fcn(joint_pos_sym); const int constraint_dim = phi_sym.rows(); + if (constraint_dim == 0) { + std::cerr << "[GenericImplicit] phi_sym has zero rows!" << std::endl; + } SX cs_phi_sym = casadi::SX(casadi::Sparsity::dense(constraint_dim, 1)); casadi::copy(phi_sym, cs_phi_sym); - casadi::Function cs_phi_fcn = casadi::Function("phi", {cs_q_sym}, {cs_phi_sym}); + cs_phi_fcn_ = casadi::Function("phi", {cs_q_sym}, {cs_phi_sym}, cse_opts); // Implicit constraint jacobian SX cs_K_sym = jacobian(cs_phi_sym, cs_q_sym); @@ -81,31 +99,62 @@ namespace grbda casadi::Slice ind_slice = casadi::Slice(0, ind_dim); cs_G_sym(ind_slice, ind_slice) = SX::eye(ind_dim); casadi::Slice dep_slice = casadi::Slice(ind_dim, ind_dim + dep_dim); - cs_G_sym(dep_slice, casadi::Slice()) = -SX::mtimes(SX::inv(cs_Kd_sym), cs_Ki_sym); - cs_G_sym = SX::mtimes(coord_map, cs_G_sym); + if (cs_Kd_sym.size2() == 0 || cs_Ki_sym.size2() == 0) { + std::cerr << "[GenericImplicit] cs_Kd_sym or cs_Ki_sym has zero size!" << std::endl; + } + + + cs_G_sym(dep_slice, casadi::Slice()) = -SX::solve(cs_Kd_sym, cs_Ki_sym); + cs_G_sym = SX::mtimes(coord_map_sx, cs_G_sym); // Explicit constraints bias SX cs_g_sym = SX::zeros(state_dim, 1); - cs_g_sym(dep_slice) = SX::mtimes(SX::inv(cs_Kd_sym), cs_k_sym); - cs_g_sym = SX::mtimes(coord_map, cs_g_sym); + if (cs_Kd_sym.size2() == 0) { + std::cerr << "[GenericImplicit] cs_Kd_sym has zero size for bias!" << std::endl; + } + + + cs_g_sym(dep_slice) = SX::solve(cs_Kd_sym, cs_k_sym); + cs_g_sym = SX::mtimes(coord_map_sx, cs_g_sym); // Assign member variables using casadi functions - this->phi_ = [cs_phi_fcn](const JointCoordinate &joint_pos) + this->phi_ = [this](const JointCoordinate &joint_pos) { - return runCasadiFcn(cs_phi_fcn, joint_pos); + return runCasadiFcn(cs_phi_fcn_, joint_pos); }; this->K_ = DMat::Zero(constraint_dim, state_dim); - K_fcn_ = casadi::Function("K", {cs_q_sym}, {cs_K_sym}); + K_fcn_ = casadi::Function("K", {cs_q_sym}, {cs_K_sym}, cse_opts); this->G_ = DMat::Zero(state_dim, ind_dim); - G_fcn_ = casadi::Function("G", {cs_q_sym}, {cs_G_sym}); + G_fcn_ = casadi::Function("G", {cs_q_sym}, {cs_G_sym}, cse_opts); this->k_ = DVec::Zero(constraint_dim); - k_fcn_ = casadi::Function("k", {cs_q_sym, cs_v_sym}, {cs_k_sym}); + k_fcn_ = casadi::Function("k", {cs_q_sym, cs_v_sym}, {cs_k_sym}, cse_opts); this->g_ = DVec::Zero(state_dim); - g_fcn_ = casadi::Function("g", {cs_q_sym, cs_v_sym}, {cs_g_sym}); + g_fcn_ = casadi::Function("g", {cs_q_sym, cs_v_sym}, {cs_g_sym}, cse_opts); + + // Create derivative functions for complex-step support + // dK/dq: Jacobian of each element of K w.r.t. q + SX dK_dq_sym = jacobian(SX::vec(cs_K_sym), cs_q_sym); + dK_dq_fcn_ = casadi::Function("dK_dq", {cs_q_sym}, {dK_dq_sym}, cse_opts); + + // dG/dq: Jacobian of each element of G w.r.t. q + SX dG_dq_sym = jacobian(SX::vec(cs_G_sym), cs_q_sym); + dG_dq_fcn_ = casadi::Function("dG_dq", {cs_q_sym}, {dG_dq_sym}, cse_opts); + + // dk/dq and dk/dv: Jacobians of k w.r.t. position and velocity + SX dk_dq_sym = jacobian(cs_k_sym, cs_q_sym); + SX dk_dv_sym = jacobian(cs_k_sym, cs_v_sym); + dk_dq_fcn_ = casadi::Function("dk_dq", {cs_q_sym, cs_v_sym}, {dk_dq_sym}, cse_opts); + dk_dv_fcn_ = casadi::Function("dk_dv", {cs_q_sym, cs_v_sym}, {dk_dv_sym}, cse_opts); + + // dg/dq and dg/dv: Jacobians of g w.r.t. position and velocity + SX dg_dq_sym = jacobian(cs_g_sym, cs_q_sym); + SX dg_dv_sym = jacobian(cs_g_sym, cs_v_sym); + dg_dq_fcn_ = casadi::Function("dg_dq", {cs_q_sym, cs_v_sym}, {dg_dq_sym}, cse_opts); + dg_dv_fcn_ = casadi::Function("dg_dv", {cs_q_sym, cs_v_sym}, {dg_dv_sym}, cse_opts); } template @@ -117,15 +166,15 @@ namespace grbda template void GenericImplicit::updateJacobians(const JointCoordinate &joint_pos) { - this->K_ = runCasadiFcn(K_fcn_, joint_pos); - this->G_ = runCasadiFcn(G_fcn_, joint_pos); + this->K_ = evalK(joint_pos); + this->G_ = evalG(joint_pos); } template void GenericImplicit::updateBiases(const JointState &joint_state) { - this->k_ = runCasadiFcn(k_fcn_, joint_state); - this->g_ = runCasadiFcn(g_fcn_, joint_state); + this->k_ = evalk(joint_state); + this->g_ = evalg(joint_state); } template @@ -134,40 +183,323 @@ namespace grbda return is_coordinate_independent_; } + template + bool GenericImplicit::isValidSpanningPosition(const JointCoordinate &joint_pos) const + { + if (!joint_pos.isSpanning()) { + return false; + } + DVec violation = this->phi_(joint_pos); + return nearZeroDefaultTrue(violation, static_cast(1e-6)); + } + template DMat GenericImplicit::runCasadiFcn(const casadi::Function &fcn, const JointCoordinate &arg) { - using CasadiScalar = typename std::conditional::value, casadi::SX, casadi::DM>::type; - using CasadiResult = typename std::conditional::value, double, Scalar>::type; - - CasadiScalar arg_cs(arg.rows()); - casadi::copy(arg, arg_cs); - CasadiScalar res_cs = fcn(arg_cs)[0]; - DMat res(res_cs.size1(), res_cs.size2()); - casadi::copy(res_cs, res); - - return res.template cast(); + // For complex types, extract real part for CasADi evaluation + // (CasADi functions are real-valued) + if constexpr (std::is_same_v>) { + DVec arg_real(arg.size()); + for (int i = 0; i < arg.size(); ++i) { + arg_real(i) = arg(i).real(); + } + + casadi::DM arg_dm; + casadi::copy(arg_real, arg_dm); + + casadi::DM res_dm = fcn(arg_dm)[0]; + + // Convert result through double then cast to complex + DMat res_double(res_dm.size1(), res_dm.size2()); + casadi::copy(res_dm, res_double); + + // Cast to complex (imaginary part is zero, but that's OK for constraint evaluation) + DMat> res = res_double.template cast>(); + return res; + } else { + // For real types, use standard copy + casadi::DM arg_dm; + casadi::copy(arg, arg_dm); + + casadi::DM res_dm = fcn(arg_dm)[0]; + + // Convert through double to handle float specialization + DMat res_double(res_dm.size1(), res_dm.size2()); + casadi::copy(res_dm, res_double); + + // Cast to target scalar type + DMat res = res_double.template cast(); + return res; + } } template DMat GenericImplicit::runCasadiFcn(const casadi::Function &fcn, const JointState &args) { - using CasadiScalar = typename std::conditional::value, casadi::SX, casadi::DM>::type; - using CasadiResult = typename std::conditional::value, double, Scalar>::type; + // For complex types, extract real parts for CasADi evaluation + // (CasADi functions are real-valued) + if constexpr (std::is_same_v>) { + DVec pos_real(args.position.size()); + for (int i = 0; i < args.position.size(); ++i) { + pos_real(i) = args.position(i).real(); + } + + DVec vel_real(args.velocity.size()); + for (int i = 0; i < args.velocity.size(); ++i) { + vel_real(i) = args.velocity(i).real(); + } + + casadi::DM pos_dm, vel_dm; + casadi::copy(pos_real, pos_dm); + casadi::copy(vel_real, vel_dm); + + std::vector arg_vec = {pos_dm, vel_dm}; + std::vector res_vec = fcn(arg_vec); + casadi::DM res_dm = res_vec[0]; + + // Convert result through double then cast to complex + DMat res_double(res_dm.size1(), res_dm.size2()); + casadi::copy(res_dm, res_double); + + // Cast to complex (imaginary part is zero, but that's OK for constraint evaluation) + DMat> res = res_double.template cast>(); + return res; + } else { + // For real types, use standard copy + casadi::DM pos_dm, vel_dm; + casadi::copy(args.position, pos_dm); + casadi::copy(args.velocity, vel_dm); + + std::vector arg_vec = {pos_dm, vel_dm}; + std::vector res_vec = fcn(arg_vec); + casadi::DM res_dm = res_vec[0]; + + // Convert through double to handle float specialization + DMat res_double(res_dm.size1(), res_dm.size2()); + casadi::copy(res_dm, res_double); + + // Cast to target scalar type + DMat res = res_double.template cast(); + return res; + } + } + + // Helper function to evaluate CasADi function with real-valued inputs (single arg) + template + DMat GenericImplicit::runCasadiFcnReal(const casadi::Function &fcn, + const DVec &arg) + { + casadi::DM arg_dm; + casadi::copy(arg, arg_dm); + casadi::DM res_dm = fcn(arg_dm)[0]; + DMat res(res_dm.size1(), res_dm.size2()); + casadi::copy(res_dm, res); + return res; + } - std::vector args_cs(2); - args_cs[0] = CasadiScalar(args.position.rows()); - casadi::copy(args.position, args_cs[0]); - args_cs[1] = CasadiScalar(args.velocity.rows()); - casadi::copy(args.velocity, args_cs[1]); + // Helper function to evaluate CasADi function with real-valued inputs (two args) + template + DMat GenericImplicit::runCasadiFcnReal(const casadi::Function &fcn, + const DVec &pos, + const DVec &vel) + { + casadi::DM pos_dm, vel_dm; + casadi::copy(pos, pos_dm); + casadi::copy(vel, vel_dm); + std::vector arg_vec = {pos_dm, vel_dm}; + std::vector res_vec = fcn(arg_vec); + casadi::DM res_dm = res_vec[0]; + DMat res(res_dm.size1(), res_dm.size2()); + casadi::copy(res_dm, res); + return res; + } - CasadiScalar res_cs = fcn(args_cs)[0]; - DMat res(res_cs.size1(), res_cs.size2()); - casadi::copy(res_cs, res); + // Complex-step aware evaluation of K + // Uses a Taylor series expansion to avoid finite-difference errors: + // K(q + i*δq) = K(q) + i * (dK/dq @ δq) + // + // This achieves machine precision for complex-step differentiation by using + // CasADi's analytical derivatives instead of numerical finite differences. + template + DMat GenericImplicit::evalK(const JointCoordinate &joint_pos) const + { + if constexpr (std::is_same_v>) { + const int n = joint_pos.size(); + + // Taylor series expansion using CasADi symbolic derivatives + // K(q + i*δq) = K(q_real) + i * (dK/dq @ q_imag) + // This avoids finite-difference errors that limit accuracy to ~1e-4 + DVec q_real(n), q_imag(n); + for (int i = 0; i < n; ++i) { + q_real(i) = joint_pos(i).real(); + q_imag(i) = joint_pos(i).imag(); + } - return res.template cast(); + // Evaluate K at real part + DMat K_real = runCasadiFcnReal(K_fcn_, q_real); + const int rows = K_real.rows(); + const int cols = K_real.cols(); + + // Evaluate dK/dq at real part (vectorized K, so output is (rows*cols) x n) + DMat dK_dq = runCasadiFcnReal(dK_dq_fcn_, q_real); + + // Compute imaginary part: dK/dq @ q_imag + // dK_dq is (rows*cols) x n, q_imag is n x 1 + // Result is (rows*cols) x 1, reshape to rows x cols + DVec K_imag_vec = dK_dq * q_imag; + + // Build complex result + // CasADi vec() uses column-major: element K[i,j] is at index j*rows + i + DMat> K_complex(rows, cols); + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + int idx = j * rows + i; // Column-major indexing + K_complex(i, j) = std::complex(K_real(i, j), K_imag_vec(idx)); + } + } + return K_complex; + } else { + // For real types, use standard evaluation + return runCasadiFcn(K_fcn_, joint_pos); + } + } + + // Complex-step aware evaluation of G + // G is computed from K via implicit function theorem: G = [I; -Kd^{-1} * Ki] + // + // Uses a Taylor series expansion to avoid finite-difference errors: + // G(q + i*δq) = G(q) + i * (dG/dq @ δq) + // + // This achieves machine precision for complex-step differentiation by using + // CasADi's analytical derivatives instead of numerical finite differences. + template + DMat GenericImplicit::evalG(const JointCoordinate &joint_pos) const + { + if constexpr (std::is_same_v>) { + const int n = joint_pos.size(); + + // Taylor series expansion using CasADi symbolic derivatives + // G(q + i*δq) = G(q_real) + i * (dG/dq @ q_imag) + // This avoids finite-difference errors that limit accuracy to ~1e-4 + DVec q_real(n), q_imag(n); + for (int i = 0; i < n; ++i) { + q_real(i) = joint_pos(i).real(); + q_imag(i) = joint_pos(i).imag(); + } + + DMat G_real = runCasadiFcnReal(G_fcn_, q_real); + const int rows = G_real.rows(); + const int cols = G_real.cols(); + + DMat dG_dq = runCasadiFcnReal(dG_dq_fcn_, q_real); + // dG_dq has shape (n_G_elements, n_q) where n_G_elements = rows * cols + // dG_dq @ q_imag gives the change in vec(G) for imaginary perturbation + DVec G_imag_vec = dG_dq * q_imag; + + DMat> G_complex(rows, cols); + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + // CasADi vec() uses column-major: element G[i,j] is at index j*rows + i + int idx = j * rows + i; + G_complex(i, j) = std::complex(G_real(i, j), G_imag_vec(idx)); + } + } + + return G_complex; + } else { + // For real types, use standard CasADi evaluation + return runCasadiFcn(G_fcn_, joint_pos); + } + } + + // Complex-step aware evaluation of k (constraint bias) + // k = -K̇ · v where K̇ = dK/dt = sum_j (dK/dq_j * qd_j) + // + // Uses Taylor expansion: k(q + i*dq, v + i*dv) ≈ k(q,v) + i * (dk/dq * dq + dk/dv * dv) + // + // For velocity derivatives (dtau/dqdot), position has no imaginary part (dq=0), + // so the expansion becomes: k(q, v + i*dv) = k(q,v) + i * (dk/dv * dv) + // This is exact (not approximate) since k is linear in v. + template + DMat GenericImplicit::evalk(const JointState &joint_state) const + { + if constexpr (std::is_same_v>) { + const int n_pos = joint_state.position.size(); + const int n_vel = joint_state.velocity.size(); + + // Taylor expansion approach using CasADi symbolic derivatives + DVec q_real(n_pos), q_imag(n_pos); + for (int i = 0; i < n_pos; ++i) { + q_real(i) = joint_state.position(i).real(); + q_imag(i) = joint_state.position(i).imag(); + } + DVec v_real(n_vel), v_imag(n_vel); + for (int i = 0; i < n_vel; ++i) { + v_real(i) = joint_state.velocity(i).real(); + v_imag(i) = joint_state.velocity(i).imag(); + } + + DMat k_real = runCasadiFcnReal(k_fcn_, q_real, v_real); + DMat dk_dq = runCasadiFcnReal(dk_dq_fcn_, q_real, v_real); + DMat dk_dv = runCasadiFcnReal(dk_dv_fcn_, q_real, v_real); + + DVec k_imag = dk_dq * q_imag + dk_dv * v_imag; + + const int rows = k_real.rows(); + DMat> k_complex(rows, 1); + for (int i = 0; i < rows; ++i) { + k_complex(i, 0) = std::complex(k_real(i, 0), k_imag(i)); + } + return k_complex; + } else { + return runCasadiFcn(k_fcn_, joint_state); + } + } + + // Complex-step aware evaluation of g (explicit constraint bias) + // g = [0; Kd^{-1} · k] mapped to spanning coordinates + // + // Uses Taylor expansion: g(q + i*dq, v + i*dv) ≈ g(q,v) + i * (dg/dq * dq + dg/dv * dv) + // + // For velocity derivatives (dtau/dqdot), position has no imaginary part (dq=0), + // so the expansion becomes: g(q, v + i*dv) = g(q,v) + i * (dg/dv * dv) + // This is exact since g depends linearly on v (through k which is linear in v). + template + DMat GenericImplicit::evalg(const JointState &joint_state) const + { + if constexpr (std::is_same_v>) { + const int n_pos = joint_state.position.size(); + const int n_vel = joint_state.velocity.size(); + + // Taylor expansion approach using CasADi symbolic derivatives + DVec q_real(n_pos), q_imag(n_pos); + for (int i = 0; i < n_pos; ++i) { + q_real(i) = joint_state.position(i).real(); + q_imag(i) = joint_state.position(i).imag(); + } + DVec v_real(n_vel), v_imag(n_vel); + for (int i = 0; i < n_vel; ++i) { + v_real(i) = joint_state.velocity(i).real(); + v_imag(i) = joint_state.velocity(i).imag(); + } + + DMat g_real = runCasadiFcnReal(g_fcn_, q_real, v_real); + DMat dg_dq = runCasadiFcnReal(dg_dq_fcn_, q_real, v_real); + DMat dg_dv = runCasadiFcnReal(dg_dv_fcn_, q_real, v_real); + + DVec g_imag = dg_dq * q_imag + dg_dv * v_imag; + + const int rows = g_real.rows(); + DMat> g_complex(rows, 1); + for (int i = 0; i < rows; ++i) { + g_complex(i, 0) = std::complex(g_real(i, 0), g_imag(i)); + } + return g_complex; + } else { + return runCasadiFcn(g_fcn_, joint_state); + } } template @@ -211,7 +543,7 @@ namespace grbda rootfinder_problem["g"] = cs_phi_sym; casadi::Dict options; options["expand"] = true; - options["error_on_fail"] = true; + options["error_on_fail"] = true; // Fail fast on non-convergence this->random_state_helpers_.phi_root_finder = casadi::rootfinder("solver", "newton", rootfinder_problem, options); @@ -227,11 +559,14 @@ namespace grbda DMat G = symbolic.G(); SX G_sym = casadi::SX(casadi::Sparsity::dense(G.rows(), G.cols())); casadi::copy(G, G_sym); - this->random_state_helpers_.G = casadi::Function("G", {cs_q_sym}, {G_sym}, {"q"}, {"G"}); + casadi::Dict cse_opts; + cse_opts["cse"] = true; + this->random_state_helpers_.G = casadi::Function("G", {cs_q_sym}, {G_sym}, {"q"}, {"G"}, cse_opts); } } template struct GenericImplicit; + template struct GenericImplicit>; template struct GenericImplicit; template struct GenericImplicit; } @@ -284,6 +619,52 @@ namespace grbda X_intra_ = DMat::Identity(6 * this->num_bodies_, 6 * this->num_bodies_); X_intra_ring_ = DMat::Zero(6 * this->num_bodies_, 6 * this->num_bodies_); + + // Build spanning-to-independent position conversion for explicit constraints. + // G maps independent → spanning (qdot_span = G * ydot). For each independent + // coordinate j, find the spanning row i where G(i,j)==1 and all other G(*,j)==0 + // (i.e. the unit-selection row), then set conv(j,i)=1. + if (loop_constraint->isExplicit()) { + if constexpr (std::is_same_v || std::is_same_v) { + const int n_ind = loop_constraint->numIndependentPos(); + const int n_span = loop_constraint->numSpanningPos(); + const DMat& G = loop_constraint->G(); + this->spanning_tree_to_independent_coords_conversion_ = + DMat::Zero(n_ind, n_span); + for (int col = 0; col < n_ind; col++) { + for (int row = 0; row < n_span; row++) { + if (std::abs(static_cast(G(row, col)) - 1.0) < 1e-9) { + bool only_nonzero = true; + for (int r = 0; r < n_span; r++) { + if (r != row && std::abs(static_cast(G(r, col))) > 1e-9) { + only_nonzero = false; + break; + } + } + if (only_nonzero) { + this->spanning_tree_to_independent_coords_conversion_(col, row) = 1; + break; + } + } + } + } + } + } else if (generic_constraint_) { + if constexpr (std::is_same_v || std::is_same_v) { + const auto& is_ind = generic_constraint_->isCoordinateIndependent(); + const int n_span = (int)is_ind.size(); + const int n_ind = (int)std::count(is_ind.begin(), is_ind.end(), true); + this->spanning_tree_to_independent_coords_conversion_ = + DMat::Zero(n_ind, n_span); + int ind_idx = 0; + for (int j = 0; j < n_span; j++) { + if (is_ind[j]) { + this->spanning_tree_to_independent_coords_conversion_(ind_idx, j) = 1; + ind_idx++; + } + } + } + } } template @@ -348,7 +729,7 @@ namespace grbda } template - JointState Generic::randomJointState() const + JointState Generic::randomJointState(bool enforce_position_constraint) const { if (this->loop_constraint_->isExplicit()) return Base::randomJointState(); @@ -358,30 +739,125 @@ namespace grbda throw std::runtime_error("GenericImplicit loop constraint not set"); } - // Create Helper functions - this->loop_constraint_->createRandomStateHelpers(); + const int n_span = this->loop_constraint_->numSpanningPos(); + const int n_ind = this->loop_constraint_->numIndependentPos(); + const int n_dep = n_span - n_ind; - // Attempt to find valid spanning position - int attempts = 0; - JointCoordinate joint_pos(findRootsForPhi()); - auto numerical_loop_constraint = generic_constraint_->copyAsDouble(); - bool is_valid = numerical_loop_constraint.isValidSpanningPosition(joint_pos); - while (!is_valid && attempts++ < 10) - { - joint_pos = findRootsForPhi(); - is_valid = numerical_loop_constraint.isValidSpanningPosition(joint_pos); + // Build independent mask + std::vector ind_mask = generic_constraint_->isCoordinateIndependent(); + if ((int)ind_mask.size() != n_span) { + ind_mask.assign(n_span, false); + for (int i = 0; i < n_span; ++i) ind_mask[i] = (i < n_ind); + } + + // Initialize q with random independent and small dependent values + DVec q_span = DVec::Zero(n_span); + for (int i = 0; i < n_span; ++i) { + if (ind_mask[i]) q_span(i) = 0.3 * (2.0 * ((double)rand() / RAND_MAX) - 1.0); + else q_span(i) = 0.01 * (2.0 * ((double)rand() / RAND_MAX) - 1.0); } - if (!is_valid) + auto numerical_lc = generic_constraint_->copyAsDouble(); + auto phi_eval = [&numerical_lc](const DVec &q) -> DVec { + JointCoordinate jc(q, true); + return numerical_lc.phi(jc); + }; + + // Newton solver with adaptive damping for robust convergence to machine precision + // Uses undamped Newton when close to solution, damped when far + const int max_iters = 200; + const double tol_tight = 1e-12; // Target machine precision + const double tol_accept = 1e-10; // Acceptance tolerance + const double h = 1e-8; // FD step for Jacobian + + // Build dep indices + std::vector dep_idx; dep_idx.reserve(n_dep); + for (int i = 0; i < n_span; ++i) if (!ind_mask[i]) dep_idx.push_back(i); + + bool converged = false; + double best_phi_norm = 1e10; + DVec best_q_span = q_span; + + if(enforce_position_constraint) { - throw std::runtime_error("Invalid random state for implicit loop constraint"); + + for (int attempt = 0; attempt < 1000 && !converged; ++attempt) { + double damping = 0.5; // Start with damping for stability + for (int iter = 0; iter < max_iters; ++iter) { + DVec phi = phi_eval(q_span); + double phi_norm = phi.norm(); + + // Track best solution found + if (phi_norm < best_phi_norm) { + best_phi_norm = phi_norm; + best_q_span = q_span; + } + + if (phi_norm < tol_tight) { converged = true; break; } + + // Use undamped Newton when close to solution + if (phi_norm < 1e-3) damping = 1.0; + else if (phi_norm < 1e-2) damping = 0.8; + else damping = 0.5; + + DMat J(phi.size(), n_dep); + for (int j = 0; j < n_dep; ++j) { + DVec q_pert = q_span; + q_pert(dep_idx[j]) += h; + J.col(j) = (phi_eval(q_pert) - phi) / h; + } + Eigen::CompleteOrthogonalDecomposition> cod(J); + DVec dx = cod.solve(-phi); + for (int j = 0; j < n_dep; ++j) q_span(dep_idx[j]) += damping * dx(j); + } + if (!converged) { + // reinitialize dependents with wider range + for (int i = 0; i < n_span; ++i) + { + if (!ind_mask[i]) + { + q_span(i) = 0.3 * (2.0 * ((double)rand() / RAND_MAX) - 1.0); + } + else + { + q_span(i) = 0.3 * (2.0 * ((double)rand() / RAND_MAX) - 1.0); + } + } + } + } + + // Use best solution found if not converged + if (!converged) { + q_span = best_q_span; + converged = (best_phi_norm < tol_accept); + } } + else + { + converged = true; // No constraint to enforce + } + + // Final phi check - use native phi for validation too when available + DVec phi_final = phi_eval(q_span); + double final_phi_norm = phi_final.norm(); - // Random independent joint velocity - DVec v = DVec::Random(this->loop_constraint_->numIndependentVel()); - JointCoordinate joint_vel(v, false); + // Validate using the same phi function that Newton used (native if available) + // This ensures consistency between Newton convergence and validation + bool is_valid = (final_phi_norm < 1e-8); // Use our own tolerance since we know phi + + if (!converged || (!is_valid && enforce_position_constraint)) { + std::cerr << "[Newton debug] converged=" << converged + << ", best_phi_norm=" << best_phi_norm + << ", final_phi_norm=" << final_phi_norm + << ", is_valid=" << is_valid << std::endl; + throw std::runtime_error("Failed to sample valid spanning state for implicit constraint"); + } - return JointState(joint_pos, joint_vel); + // Return with independent velocities + DVec ydot = DVec::Random(this->loop_constraint_->numIndependentVel()); + JointCoordinate pos(q_span, true); + JointCoordinate vel(ydot, false); + return JointState(pos, vel); } template @@ -391,6 +867,10 @@ namespace grbda const DVec &q = spanning_joint_state.position; const DVec &qd = spanning_joint_state.velocity; + // Cache state for derivative computation + q_spanning_ = q; + qd_spanning_ = qd; + int pos_idx = 0; int vel_idx = 0; for (int i = 0; i < this->num_bodies_; i++) @@ -423,9 +903,9 @@ namespace grbda vel_idx += num_vel; } - const DMat S_implicit = X_intra_ * S_spanning_; - this->S_ = S_implicit * this->loop_constraint_->G(); - this->vJ_ = S_implicit * qd; + S_implicit_.noalias() = X_intra_ * S_spanning_; + this->S_.noalias() = S_implicit_ * this->loop_constraint_->G(); + this->vJ_.noalias() = S_implicit_ * qd; for (int i = 0; i < this->num_bodies_; i++) { @@ -434,10 +914,10 @@ namespace grbda { if (connectivity_(i, j)) { - const Mat6 Xup = X_intra_.template block<6, 6>(6 * i, 6 * j); + const auto Xup = X_intra_.template block<6, 6>(6 * i, 6 * j); const SVec v_parent = Xup * this->vJ_.template segment<6>(6 * j); - const SVec v_child = this->vJ_.template segment<6>(6 * i); + const auto v_child = this->vJ_.template segment<6>(6 * i); v_relative = v_child - v_parent; X_intra_ring_.template block<6, 6>(6 * i, 6 * j) = @@ -447,7 +927,65 @@ namespace grbda } this->cJ_ = X_intra_ring_ * this->S_spanning_ * qd + - S_implicit * this->loop_constraint_->g(); + S_implicit_ * this->loop_constraint_->g(); + + // S_ring = dS/dt = d(X_intra * S_spanning * G)/dt + // = dX_intra/dt * S_spanning * G + X_intra * S_spanning * dG/dt + // where dG/dt = sum_j (dG/dq_j * qd_j) + // + // For GenericImplicit constraints, we compute dG/dt using the constraint's + // dG/dq CasADi function which is properly initialized in the constructor. + DMat S_ring_term1 = X_intra_ring_ * this->S_spanning_ * this->loop_constraint_->G(); + DMat S_ring_term2 = DMat::Zero(S_ring_term1.rows(), S_ring_term1.cols()); + + // Compute G_dot for both double and complex types + // For complex types, use the real part of q to evaluate dG/dq (valid for small imaginary parts) + if constexpr (std::is_same_v || std::is_same_v>) { + if (generic_constraint_ && q_spanning_.size() > 0) { + // Get the constraint's dG/dq function (initialized in constructor) + const casadi::Function& dG_dq_fcn = generic_constraint_->getdGdqFcn(); + + // Evaluate dG/dq at current position (real part only for complex types) + std::vector q_vec(q_spanning_.size()); + for (int i = 0; i < q_spanning_.size(); ++i) { + if constexpr (std::is_same_v>) { + q_vec[i] = std::real(q_spanning_(i)); + } else { + q_vec[i] = q_spanning_(i); + } + } + casadi::DM q_dm(q_vec); + casadi::DM dG_dq_dm = dG_dq_fcn(casadi::DMVector{q_dm})[0]; + + // dG_dq_dm has shape (n_G_elements, n_q) where n_G_elements = G.rows() * G.cols() + // G_dot = sum_j (dG/dq_j * qd_j) = dG_dq * qd (matrix-vector product) + const int n_q = q_spanning_.size(); + const int n_G_rows = this->loop_constraint_->G().rows(); + const int n_G_cols = this->loop_constraint_->G().cols(); + const int n_G_elements = n_G_rows * n_G_cols; + + // Compute G_dot_vec = dG_dq * qd + DVec G_dot_vec = DVec::Zero(n_G_elements); + for (int i = 0; i < n_G_elements; ++i) { + for (int j = 0; j < n_q; ++j) { + G_dot_vec(i) += Scalar(static_cast(dG_dq_dm(i, j))) * qd(j); + } + } + + // Reshape G_dot_vec to G_dot matrix (column-major order) + DMat G_dot(n_G_rows, n_G_cols); + for (int col = 0; col < n_G_cols; ++col) { + for (int row = 0; row < n_G_rows; ++row) { + G_dot(row, col) = G_dot_vec(col * n_G_rows + row); + } + } + + // Second term: X_intra * S_spanning * G_dot + S_ring_term2.noalias() = S_implicit_ * G_dot; + } + } + + this->S_ring_.noalias() = S_ring_term1 + S_ring_term2; } template @@ -500,11 +1038,347 @@ namespace grbda if (body.index_ == body_index) return body; throw std::runtime_error("Body is not in the current cluster"); +} + + template + void Generic::initializeDerivativeFunctions() const + { + if (derivative_functions_initialized_ || !generic_constraint_) { + return; + } + derivative_functions_initialized_ = true; + + // Only initialize for double type + if constexpr (!std::is_same_v) { + return; + } + + using SX = casadi::SX; + + const int mss_dim = this->num_bodies_ * 6; + const int n_span_pos = this->loop_constraint_->numSpanningPos(); + const int n_span_vel = this->loop_constraint_->numSpanningVel(); + + const int nv = this->num_velocities_; + + // Symbolic spanning positions and independent velocities. + // qd_span is not an independent symbolic variable — it is determined by + // the velocity constraint: qd_span = G(q_span) * ydot. + // Using ydot as the velocity input ensures the Jacobian d(cJ)/dq_span + // is taken holding ydot fixed (correct), capturing how qd_span also + // changes with q_span through G. + SX q_span_sx = SX::sym("q_span", n_span_pos); + DVec q_span_vec(n_span_pos); + casadi::copy(q_span_sx, q_span_vec); + + SX ydot_sx = SX::sym("ydot", nv); + + // Get G and g symbolically by calling CasADi functions with SX inputs. + // Calling symbolic_constraint.updateJacobians/updateBiases with SX inputs + // fails because runCasadiFcn converts symbolic variables to NaN via DM cast. + // Instead, call the underlying CasADi functions directly with SX arguments, + // which performs symbolic substitution and returns correct SX expressions. + SX G_casadi = generic_constraint_->getGFcn()(casadi::SXVector{q_span_sx})[0]; + + // Express spanning velocity via the constraint: qd_span = G(q_span) * ydot + SX qd_span_constrained = SX::mtimes(G_casadi, ydot_sx); // n_span_vel x 1 + DVec qd_span_vec(n_span_vel); + for (int i = 0; i < n_span_vel; ++i) qd_span_vec(i) = qd_span_constrained(i, 0); + + SX g_casadi = generic_constraint_->getgFcn()(casadi::SXVector{q_span_sx, qd_span_constrained})[0]; + + // Compute dG/dq using CasADi automatic differentiation + std::vector dG_dq_vec; + for (int i = 0; i < n_span_pos; ++i) { + SX dG_dqi = jacobian(G_casadi, q_span_sx(i)); + dG_dq_vec.push_back(dG_dqi); + } + SX dG_dq_stacked = SX::vertcat(dG_dq_vec); + { + casadi::Dict cse_opts; + cse_opts["cse"] = true; + dG_dq_fcn_ = casadi::Function("dG_dq", {q_span_sx}, {dG_dq_stacked}, cse_opts); + } + + // Build symbolic cJ = X_intra_ring * S_spanning * qd_span + X_intra * S_spanning * g + // by constructing SX-typed joint clones and running the same logic as updateKinematics_vJ. + + // Build symbolic cJ using cloneAsSymbolic() on each sub-joint. + // Guarded with if constexpr: bodies_[].Xtree_ and S_spanning_ are DMat, + // so .cast() inside would fail to instantiate for Scalar=complex. + if constexpr (std::is_same_v) { + std::vector>> joints_sx; + for (int i = 0; i < this->num_bodies_; ++i) + joints_sx.push_back(this->single_joints_[i]->cloneAsSymbolic()); + + { + // Drive symbolic joints with q_span_vec / qd_span_vec (already Eigen) + int pos_idx2 = 0, vel_idx2 = 0; + for (int i = 0; i < this->num_bodies_; ++i) { + const int npos = joints_sx[i]->numPositions(); + const int nvel = joints_sx[i]->numVelocities(); + joints_sx[i]->updateKinematics( + q_span_vec.segment(pos_idx2, npos), + qd_span_vec.segment(vel_idx2, nvel)); + pos_idx2 += npos; + vel_idx2 += nvel; + } + + // Build X_intra_sx (same connectivity loop as updateKinematics_vJ) + DMat X_intra_sx = DMat::Identity(mss_dim, mss_dim); + for (int i = 0; i < this->num_bodies_; ++i) { + int k = i; + for (int j = i - 1; j >= 0; --j) { + if (connectivity_(i, j)) { + DMat Xup_prev = X_intra_sx.block(6 * i, 6 * k, 6, 6); + Mat6 XJ_k = joints_sx[k]->XJ().toMatrix(); + Mat6 Xtree_k = bodies_[k].Xtree_.toMatrix().template cast(); + X_intra_sx.block(6 * i, 6 * j, 6, 6) = Xup_prev * XJ_k * Xtree_k; + k = j; + } + } + } + + DMat S_spanning_sx = S_spanning_.template cast(); + DMat S_implicit_sx = X_intra_sx * S_spanning_sx; + DVec vJ_sx = S_implicit_sx * qd_span_vec; + + // Build X_intra_ring_sx + DMat X_intra_ring_sx = DMat::Zero(mss_dim, mss_dim); + for (int i = 0; i < this->num_bodies_; ++i) { + for (int j = i - 1; j >= 0; --j) { + if (connectivity_(i, j)) { + DMat Xup = X_intra_sx.block(6 * i, 6 * j, 6, 6); + SVec v_parent = Xup * vJ_sx.template segment<6>(6 * j); + SVec v_child = vJ_sx.template segment<6>(6 * i); + X_intra_ring_sx.block(6 * i, 6 * j, 6, 6) = + -spatial::motionCrossMatrix(v_child - v_parent) * Xup; + } + } + } + + DVec g_sx_vec(g_casadi.size1()); + for (int r = 0; r < (int)g_casadi.size1(); ++r) g_sx_vec(r) = g_casadi(r, 0); + + DVec cJ_sx = X_intra_ring_sx * S_spanning_sx * qd_span_vec + + S_implicit_sx * g_sx_vec; + + // d(cJ)/dq_span, then contract with G to get d(cJ)/d(independent coords) + SX cJ_casadi = SX::zeros(mss_dim, 1); + casadi::copy(cJ_sx, cJ_casadi); + SX dcJ_dq_sx = jacobian(cJ_casadi, q_span_sx); // mss_dim x n_span_pos + SX dcJ_dy_sx = SX::mtimes(dcJ_dq_sx, G_casadi); // mss_dim x nv + + // Densify output for low-level API compatibility + SX dcJ_dy_dense = SX::densify(dcJ_dy_sx); + + // Use JIT compilation for faster function evaluation (clang with march=native) + casadi::Dict jit_opts_sdot; + jit_opts_sdot["cse"] = true; + jit_opts_sdot["jit"] = true; + jit_opts_sdot["compiler"] = "shell"; + jit_opts_sdot["jit_options"] = casadi::Dict{{"compiler", "clang"}, {"flags", "-O3 -march=native"}}; + + dSdotqd_dq_fcn_ = casadi::Function("dSdotqd_dq", + {q_span_sx, ydot_sx}, {dcJ_dy_dense}, jit_opts_sdot); + + // Build contraction-based derivative functions for efficient ID derivatives + // S = S_implicit * G = X_intra * S_spanning * G + // Convert S_implicit_sx to CasADi SX matrix for multiplication with G_casadi + SX S_implicit_casadi = SX::zeros(mss_dim, n_span_vel); + casadi::copy(S_implicit_sx, S_implicit_casadi); + SX S_casadi = SX::mtimes(S_implicit_casadi, G_casadi); // mss_dim x nv + + // d(S*b)/dy: Jacobian of S*b w.r.t. independent coordinates + // b is a symbolic input vector of size nv + SX b_sx = SX::sym("b", nv); + + SX Sb_casadi = SX::mtimes(S_casadi, b_sx); // mss_dim x 1 + + // Differentiate S*b w.r.t. q_span, then contract with G to get w.r.t. y + SX dSb_dq_sx = jacobian(Sb_casadi, q_span_sx); // mss_dim x n_span_pos + SX dSb_dy_sx = SX::mtimes(dSb_dq_sx, G_casadi); // mss_dim x nv + + // Densify the output to ensure low-level API writes to contiguous memory + SX dSb_dy_dense = SX::densify(dSb_dy_sx); + + // Use JIT compilation for faster function evaluation (clang with march=native) + casadi::Dict jit_opts; + jit_opts["cse"] = true; + jit_opts["jit"] = true; + jit_opts["compiler"] = "shell"; + jit_opts["jit_options"] = casadi::Dict{{"compiler", "clang"}, {"flags", "-O3 -march=native"}}; + + dSb_dy_fcn_ = casadi::Function("dSb_dy", + {q_span_sx, b_sx}, {dSb_dy_dense}, jit_opts); + + // d(S^T*F)/dy: Jacobian of S^T*F w.r.t. independent coordinates + // F is a symbolic input vector of size mss_dim + SX F_sx = SX::sym("F", mss_dim); + + SX STF_casadi = SX::mtimes(S_casadi.T(), F_sx); // nv x 1 + + // Differentiate S^T*F w.r.t. q_span, then contract with G to get w.r.t. y + SX dSTF_dq_sx = jacobian(STF_casadi, q_span_sx); // nv x n_span_pos + SX dSTF_dy_sx = SX::mtimes(dSTF_dq_sx, G_casadi); // nv x nv + + // Densify the output + SX dSTF_dy_dense = SX::densify(dSTF_dy_sx); + + dSTF_dy_fcn_ = casadi::Function("dSTF_dy", + {q_span_sx, F_sx}, {dSTF_dy_dense}, jit_opts); + + // Pre-allocate work vectors for low-level evaluation API + // This avoids allocation overhead on each function call + size_t sz_arg, sz_res, sz_iw, sz_w; + + dSb_dy_fcn_.sz_work(sz_arg, sz_res, sz_iw, sz_w); + dSb_work_w_.resize(sz_w); + dSb_work_iw_.resize(sz_iw); + dSb_arg_buf_.resize(n_span_pos + nv); // q_span + b + dSb_res_buf_.resize(mss_dim * nv); // output matrix + + dSTF_dy_fcn_.sz_work(sz_arg, sz_res, sz_iw, sz_w); + dSTF_work_w_.resize(sz_w); + dSTF_work_iw_.resize(sz_iw); + dSTF_arg_buf_.resize(n_span_pos + mss_dim); // q_span + F + dSTF_res_buf_.resize(nv * nv); // output matrix + + dSdotqd_dq_fcn_.sz_work(sz_arg, sz_res, sz_iw, sz_w); + dSdotqd_work_w_.resize(sz_w); + dSdotqd_work_iw_.resize(sz_iw); + dSdotqd_arg_buf_.resize(n_span_pos + nv); // q_span + ydot + dSdotqd_res_buf_.resize(mss_dim * nv); // output matrix + } + + } // if constexpr (std::is_same_v) + } + + + template + void Generic::getSdotqd_q(DMat& out) const + { + const int mss_dim = this->num_bodies_ * 6; + const int nv = this->num_velocities_; + + if (!generic_constraint_ || q_spanning_.size() == 0 || S_implicit_.size() == 0) + { + out.setZero(mss_dim, nv); + return; + } + + if constexpr (std::is_same_v) { + initializeDerivativeFunctions(); + + if (qd_spanning_.size() == 0 || + !derivative_functions_initialized_ || dSdotqd_dq_fcn_.is_null()) + { + out.setZero(mss_dim, nv); + return; + } + + const DMat& coord_map = generic_constraint_->getCoordMap(); + const DVec ydot_independent = + (coord_map.transpose() * qd_spanning_).head(nv); + + const int n_span_pos = q_spanning_.size(); + + for (int i = 0; i < n_span_pos; ++i) + dSdotqd_arg_buf_[i] = q_spanning_(i); + for (int i = 0; i < nv; ++i) + dSdotqd_arg_buf_[n_span_pos + i] = ydot_independent(i); + + const double* arg_ptrs[2] = {dSdotqd_arg_buf_.data(), dSdotqd_arg_buf_.data() + n_span_pos}; + double* res_ptrs[1] = {dSdotqd_res_buf_.data()}; + + dSdotqd_dq_fcn_(arg_ptrs, res_ptrs, dSdotqd_work_iw_.data(), dSdotqd_work_w_.data(), 0); + + out = Eigen::Map>(dSdotqd_res_buf_.data(), mss_dim, nv); + return; + } + + out.setZero(mss_dim, nv); + } + + template + void Generic::evalSTimesVec_dq(const DVec& b, DMat& out) const + { + const int mss_dim = this->num_bodies_ * 6; + const int nv = this->num_velocities_; + + if (!generic_constraint_ || q_spanning_.size() == 0) { + out.setZero(mss_dim, nv); + return; + } + + if constexpr (std::is_same_v) { + initializeDerivativeFunctions(); + + if (!derivative_functions_initialized_ || dSb_dy_fcn_.is_null()) { + out.setZero(mss_dim, nv); + return; + } + + const int n_span_pos = q_spanning_.size(); + + for (int i = 0; i < n_span_pos; ++i) + dSb_arg_buf_[i] = q_spanning_(i); + for (int i = 0; i < nv; ++i) + dSb_arg_buf_[n_span_pos + i] = b(i); + + const double* arg_ptrs[2] = {dSb_arg_buf_.data(), dSb_arg_buf_.data() + n_span_pos}; + double* res_ptrs[1] = {dSb_res_buf_.data()}; + + dSb_dy_fcn_(arg_ptrs, res_ptrs, dSb_work_iw_.data(), dSb_work_w_.data(), 0); + + out = Eigen::Map>(dSb_res_buf_.data(), mss_dim, nv); + } else { + throw std::runtime_error("evalSTimesVec_dq is not implemented for given type yet"); + } + } + + template + void Generic::evalSTTimesVec_dq(const DVec& F, DMat& out) const + { + const int mss_dim = this->num_bodies_ * 6; + const int nv = this->num_velocities_; + + if (!generic_constraint_ || q_spanning_.size() == 0) { + out.setZero(nv, nv); + return; + } + + if constexpr (std::is_same_v) { + initializeDerivativeFunctions(); + + if (!derivative_functions_initialized_ || dSTF_dy_fcn_.is_null()) { + out.setZero(nv, nv); + return; + } + + const int n_span_pos = q_spanning_.size(); + + for (int i = 0; i < n_span_pos; ++i) + dSTF_arg_buf_[i] = q_spanning_(i); + for (int i = 0; i < mss_dim; ++i) + dSTF_arg_buf_[n_span_pos + i] = F(i); + + const double* arg_ptrs[2] = {dSTF_arg_buf_.data(), dSTF_arg_buf_.data() + n_span_pos}; + double* res_ptrs[1] = {dSTF_res_buf_.data()}; + + dSTF_dy_fcn_(arg_ptrs, res_ptrs, dSTF_work_iw_.data(), dSTF_work_w_.data(), 0); + + out = Eigen::Map>(dSTF_res_buf_.data(), nv, nv); + } else { + throw std::runtime_error("evalSTTimesVec_dq is not implemented for given type yet"); + } } template class Generic; + template class Generic>; template class Generic; template class Generic; } } // namespace grbda + diff --git a/src/Dynamics/ClusterJoints/LoopConstraint.cpp b/src/Dynamics/ClusterJoints/LoopConstraint.cpp index 70250f3c..7e126165 100644 --- a/src/Dynamics/ClusterJoints/LoopConstraint.cpp +++ b/src/Dynamics/ClusterJoints/LoopConstraint.cpp @@ -15,7 +15,8 @@ namespace grbda bool Base::isValidSpanningPosition(const JointCoordinate &joint_pos) const { DVec violation = phi_(joint_pos); - return nearZeroDefaultTrue(violation) && joint_pos.isSpanning(); + const Scalar tol = static_cast(1e-6); + return nearZeroDefaultTrue(violation, tol) && joint_pos.isSpanning(); } template @@ -32,6 +33,7 @@ namespace grbda } template struct Base; + template struct Base>; template struct Base; template struct Base; @@ -52,6 +54,7 @@ namespace grbda } template struct Static; + template struct Static>; template struct Static; template struct Static; @@ -213,6 +216,7 @@ namespace grbda } template struct Collection; + template struct Collection>; template struct Collection; template struct Collection; diff --git a/src/Dynamics/ClusterJoints/RevoluteJoint.cpp b/src/Dynamics/ClusterJoints/RevoluteJoint.cpp index a1c64366..c1aa0921 100644 --- a/src/Dynamics/ClusterJoints/RevoluteJoint.cpp +++ b/src/Dynamics/ClusterJoints/RevoluteJoint.cpp @@ -51,6 +51,7 @@ namespace grbda } template class Revolute; + template class Revolute>; template class Revolute; template class Revolute; diff --git a/src/Dynamics/ClusterJoints/RevolutePairJoint.cpp b/src/Dynamics/ClusterJoints/RevolutePairJoint.cpp index 15fbb196..d9b04906 100644 --- a/src/Dynamics/ClusterJoints/RevolutePairJoint.cpp +++ b/src/Dynamics/ClusterJoints/RevolutePairJoint.cpp @@ -6,92 +6,63 @@ namespace grbda namespace ClusterJoints { + namespace + { + template + std::vector> makeRPBodies(Body &link_1, Body &link_2) + { + return {link_1, link_2}; + } + + template + std::vector> makeRPJoints(ori::CoordinateAxis axis1, + ori::CoordinateAxis axis2) + { + using Rev = Joints::Revolute; + return {std::make_shared(axis1), std::make_shared(axis2)}; + } + + template + std::shared_ptr> makeRPConstraint() + { + DMat G = DMat::Identity(2, 2); + DMat K = DMat::Identity(0, 2); + return std::make_shared>(G, K); + } + } // anonymous namespace + template RevolutePair::RevolutePair(Body &link_1, Body &link_2, ori::CoordinateAxis joint_axis_1, ori::CoordinateAxis joint_axis_2) - : Base(2, 2, 2), link_1_(link_1), link_2_(link_2) - { - using Rev = Joints::Revolute; - link_1_joint_ = this->single_joints_.emplace_back(new Rev(joint_axis_1)); - link_2_joint_ = this->single_joints_.emplace_back(new Rev(joint_axis_2)); - - this->spanning_tree_to_independent_coords_conversion_ = DMat::Identity(2, 2); - - DMat G = DMat::Zero(2, 2); - G << 1., 0., - 0., 1.; - const DMat K = DMat::Identity(0, 2); - this->loop_constraint_ = std::make_shared>(G, K); - - X_intra_S_span_ = DMat::Zero(12, 2); - X_intra_S_span_ring_ = DMat::Zero(12, 2); - - X_intra_S_span_.template block<6, 1>(0, 0) = link_1_joint_->S(); - X_intra_S_span_.template block<6, 1>(6, 1) = link_2_joint_->S(); - - this->S_ = X_intra_S_span_ * this->loop_constraint_->G(); - } - - template - void RevolutePair::updateKinematics(const JointState &joint_state) - { - const JointState spanning_joint_state = this->toSpanningTreeState(joint_state); - const DVec &q = spanning_joint_state.position; - const DVec &qd = spanning_joint_state.velocity; - - link_1_joint_->updateKinematics(q.template segment<1>(0), qd.template segment<1>(0)); - link_2_joint_->updateKinematics(q.template segment<1>(1), qd.template segment<1>(1)); - - X21_ = link_2_joint_->XJ() * link_2_.Xtree_; - const DVec v2_relative = link_2_joint_->S() * qd[1]; - X_intra_S_span_.template block<6, 1>(6, 0) = - X21_.transformMotionSubspace(link_1_joint_->S()); - this->S_.template block<6, 1>(6, 0) = X21_.transformMotionSubspace(link_1_joint_->S()); - - X_intra_S_span_ring_.template block<6, 1>(6, 0) = - -spatial::generalMotionCrossMatrix(v2_relative) * - X_intra_S_span_.template block<6, 1>(6, 0); - - this->vJ_ = X_intra_S_span_ * qd; - this->cJ_ = X_intra_S_span_ring_ * qd; - } - - template - void RevolutePair::computeSpatialTransformFromParentToCurrentCluster( - spatial::GeneralizedTransform &Xup) const - { -#ifdef DEBUG_MODE - if (Xup.getNumOutputBodies() != 2) - throw std::runtime_error("[RevolutePair] Xup must have 12 rows"); -#endif - - Xup[0] = link_1_joint_->XJ() * link_1_.Xtree_; - Xup[1] = link_2_joint_->XJ() * link_2_.Xtree_ * Xup[0]; - } + : Generic( + makeRPBodies(link_1, link_2), + makeRPJoints(joint_axis_1, joint_axis_2), + makeRPConstraint()), + link_1_(link_1), link_2_(link_2), + link_1_joint_(this->single_joints_[0]), + link_2_joint_(this->single_joints_[1]) + {} template std::vector, JointPtr, DMat>> RevolutePair::bodiesJointsAndReflectedInertias() const { - std::vector, JointPtr, DMat>> bodies_joints_and_reflected_inertias; - - const DMat reflected_inertia_1 = DMat::Zero(this->numVelocities(), - this->numVelocities()); - bodies_joints_and_reflected_inertias.push_back( - std::make_tuple(link_1_, link_1_joint_, reflected_inertia_1)); + std::vector, JointPtr, DMat>> result; - const DMat reflected_inertia_2 = DMat::Zero(this->numVelocities(), - this->numVelocities()); - bodies_joints_and_reflected_inertias.push_back( - std::make_tuple(link_2_, link_2_joint_, reflected_inertia_2)); + const DMat zero = DMat::Zero(this->numVelocities(), + this->numVelocities()); + result.push_back(std::make_tuple(link_1_, link_1_joint_, zero)); + result.push_back(std::make_tuple(link_2_, link_2_joint_, zero)); - return bodies_joints_and_reflected_inertias; + return result; } template class RevolutePair; + template class RevolutePair>; template class RevolutePair; template class RevolutePair; + } } // namespace grbda diff --git a/src/Dynamics/ClusterJoints/RevolutePairWithRotorJoint.cpp b/src/Dynamics/ClusterJoints/RevolutePairWithRotorJoint.cpp index 308e9c72..cb09b7be 100644 --- a/src/Dynamics/ClusterJoints/RevolutePairWithRotorJoint.cpp +++ b/src/Dynamics/ClusterJoints/RevolutePairWithRotorJoint.cpp @@ -6,137 +6,160 @@ namespace grbda namespace ClusterJoints { + namespace + { + template + std::vector> makeRPWRBodies( + const ParallelBeltTransmissionModule<1, Scalar> &m1, + const ParallelBeltTransmissionModule<2, Scalar> &m2) + { + // Body has const members so is not copy-assignable; sort a permutation instead. + const Body *src[4] = {&m1.body_, &m1.rotor_, &m2.rotor_, &m2.body_}; + int order[4] = {0, 1, 2, 3}; + std::sort(std::begin(order), std::end(order), [&](int a, int b) { + return src[a]->sub_index_within_cluster_ < src[b]->sub_index_within_cluster_; + }); + std::vector> bodies; + for (int i : order) + bodies.push_back(*src[i]); + return bodies; + } + + template + std::vector> makeRPWRJoints( + const ParallelBeltTransmissionModule<1, Scalar> &m1, + const ParallelBeltTransmissionModule<2, Scalar> &m2) + { + using Rev = Joints::Revolute; + // Indices must match makeRPWRBodies insertion order. + int sub[4] = { + m1.body_.sub_index_within_cluster_, + m1.rotor_.sub_index_within_cluster_, + m2.rotor_.sub_index_within_cluster_, + m2.body_.sub_index_within_cluster_, + }; + JointPtr src[4] = { + std::make_shared(m1.joint_axis_), + std::make_shared(m1.rotor_axis_), + std::make_shared(m2.rotor_axis_), + std::make_shared(m2.joint_axis_), + }; + int order[4] = {0, 1, 2, 3}; + std::sort(std::begin(order), std::end(order), + [&](int a, int b) { return sub[a] < sub[b]; }); + std::vector> joints; + for (int i : order) + joints.push_back(src[i]); + return joints; + } + + template + std::shared_ptr> makeRPWRConstraint( + const ParallelBeltTransmissionModule<1, Scalar> &m1, + const ParallelBeltTransmissionModule<2, Scalar> &m2) + { + using SX = casadi::SX; + + const int l1 = m1.body_.sub_index_within_cluster_; + const int l2 = m2.body_.sub_index_within_cluster_; + const int r1 = m1.rotor_.sub_index_within_cluster_; + const int r2 = m2.rotor_.sub_index_within_cluster_; + + Vec2 gear_ratios{m1.gear_ratio_, m2.gear_ratio_}; + Eigen::DiagonalMatrix rotor_matrix(gear_ratios); + Mat2 belt_matrix; + belt_matrix << beltMatrixRowFromBeltRatios(m1.belt_ratios_), Scalar(0), + beltMatrixRowFromBeltRatios(m2.belt_ratios_); + Mat2 ratio_product = rotor_matrix * belt_matrix; + + // K * q_span = 0: rotor velocities are linearly determined by link velocities + DMat K = DMat::Zero(2, 4); + int cnstr1 = (r1 > r2) ? 1 : 0; + int cnstr2 = (r2 > r1) ? 1 : 0; + K(cnstr1, r1) = Scalar(-1.); + K(cnstr1, l1) = ratio_product(0, 0); + K(cnstr2, r2) = Scalar(-1.); + K(cnstr2, l1) = ratio_product(1, 0); + K(cnstr2, l2) = ratio_product(1, 1); + + // sym_phi is the CasADi version of K * q required by GenericImplicit + DMat K_double(2, 4); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 4; j++) { + if constexpr (std::is_same_v>) + K_double(i, j) = std::real(K(i, j)); + else + K_double(i, j) = static_cast(K(i, j)); + } + std::vector is_ind(4, false); + is_ind[l1] = true; + is_ind[l2] = true; + + auto sym_phi = [K_double](const JointCoordinate &jp) -> DVec + { + DVec phi = DVec::Zero(2); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 4; j++) + phi(i) += SX(K_double(i, j)) * jp(j); + return phi; + }; + + return std::make_shared>( + is_ind, sym_phi); + } + } // anonymous namespace + template RevolutePairWithRotor::RevolutePairWithRotor( ProximalTransmission &module_1, DistalTransmission &module_2) - : Base(4, 2, 2), + : Generic( + makeRPWRBodies(module_1, module_2), + makeRPWRJoints(module_1, module_2), + makeRPWRConstraint(module_1, module_2)), link1_(module_1.body_), link2_(module_2.body_), rotor1_(module_1.rotor_), rotor2_(module_2.rotor_), - link1_index_(link1_.sub_index_within_cluster_), - link2_index_(link2_.sub_index_within_cluster_), - rotor1_index_(rotor1_.sub_index_within_cluster_), - rotor2_index_(rotor2_.sub_index_within_cluster_) + link1_index_(module_1.body_.sub_index_within_cluster_), + link2_index_(module_2.body_.sub_index_within_cluster_), + rotor1_index_(module_1.rotor_.sub_index_within_cluster_), + rotor2_index_(module_2.rotor_.sub_index_within_cluster_) { - using Rev = Joints::Revolute; - - link1_joint_ = this->single_joints_.emplace_back(new Rev(module_1.joint_axis_)); - rotor1_joint_ = this->single_joints_.emplace_back(new Rev(module_1.rotor_axis_)); - rotor2_joint_ = this->single_joints_.emplace_back(new Rev(module_2.rotor_axis_)); - link2_joint_ = this->single_joints_.emplace_back(new Rev(module_2.joint_axis_)); - - this->spanning_tree_to_independent_coords_conversion_ = DMat::Zero(2, 4); - this->spanning_tree_to_independent_coords_conversion_(0, link1_index_) = 1; - this->spanning_tree_to_independent_coords_conversion_(1, link2_index_) = 1; - Vec2 gear_ratios{module_1.gear_ratio_, module_2.gear_ratio_}; Eigen::DiagonalMatrix rotor_matrix(gear_ratios); Mat2 belt_matrix; - belt_matrix << beltMatrixRowFromBeltRatios(module_1.belt_ratios_), 0, + belt_matrix << beltMatrixRowFromBeltRatios(module_1.belt_ratios_), Scalar(0), beltMatrixRowFromBeltRatios(module_2.belt_ratios_); - Mat2 ratio_product = rotor_matrix * belt_matrix; - - DMat G = DMat::Zero(4, 2); - G(link1_index_, 0) = 1.; - G(rotor1_index_, 0) = ratio_product(0, 0); - G(rotor2_index_, 0) = ratio_product(1, 0); - G(rotor2_index_, 1) = ratio_product(1, 1); - G(link2_index_, 1) = 1.; - - DMat K = DMat::Zero(2, 4); - int cnstr1_index = rotor1_index_ > rotor2_index_; - int cnstr2_index = rotor2_index_ > rotor1_index_; - K(cnstr1_index, rotor1_index_) = -1.; - K(cnstr1_index, link1_index_) = G(rotor1_index_, 0); - K(cnstr2_index, rotor2_index_) = -1.; - K(cnstr2_index, link1_index_) = G(rotor2_index_, 0); - K(cnstr2_index, link2_index_) = G(rotor2_index_, 1); - - this->loop_constraint_ = std::make_shared>(G, K); - - X_intra_S_span_ = DMat::Zero(24, 4); - X_intra_S_span_ring_ = DMat::Zero(24, 4); - - const DMat &link1_S = link1_joint_->S(); - X_intra_S_span_.template block<6, 1>(6 * link1_index_, link1_index_) = link1_S; - const DMat &rotor1_S = rotor1_joint_->S(); - X_intra_S_span_.template block<6, 1>(6 * rotor1_index_, rotor1_index_) = rotor1_S; - const DMat &rotor2_S = rotor2_joint_->S(); - X_intra_S_span_.template block<6, 1>(6 * rotor2_index_, rotor2_index_) = rotor2_S; - const DMat &link2_S = link2_joint_->S(); - X_intra_S_span_.template block<6, 1>(6 * link2_index_, link2_index_) = link2_S; - - this->S_ = X_intra_S_span_ * this->loop_constraint_->G(); - } - - template - void RevolutePairWithRotor::updateKinematics(const JointState &joint_state) - { - const JointState spanning_joint_state = this->toSpanningTreeState(joint_state); - const DVec &q = spanning_joint_state.position; - const DVec &qd = spanning_joint_state.velocity; - - link1_joint_->updateKinematics(q.template segment<1>(link1_index_), - qd.template segment<1>(link1_index_)); - rotor1_joint_->updateKinematics(q.template segment<1>(rotor1_index_), - qd.template segment<1>(rotor1_index_)); - rotor2_joint_->updateKinematics(q.template segment<1>(rotor2_index_), - qd.template segment<1>(rotor2_index_)); - link2_joint_->updateKinematics(q.template segment<1>(link2_index_), - qd.template segment<1>(link2_index_)); - - X21_ = link2_joint_->XJ() * link2_.Xtree_; - const DVec v2_relative = link2_joint_->S() * qd[link2_index_]; - - X_intra_S_span_.template block<6, 1>(6 * link2_index_, link1_index_) = - X21_.transformMotionSubspace(link1_joint_->S()); - this->S_.template block<6, 1>(6 * link2_index_, 0) = - X_intra_S_span_.template block<6, 1>(6 * link2_index_, link1_index_); - - X_intra_S_span_ring_.template block<6, 1>(6 * link2_index_, link1_index_) = - -spatial::generalMotionCrossMatrix(v2_relative) * - X_intra_S_span_.template block<6, 1>(6 * link2_index_, link1_index_); - - this->vJ_ = X_intra_S_span_ * qd; - this->cJ_ = X_intra_S_span_ring_ * qd; - } - - template - void RevolutePairWithRotor::computeSpatialTransformFromParentToCurrentCluster( - spatial::GeneralizedTransform &Xup) const - { -#ifdef DEBUG_MODE - if (Xup.getNumOutputBodies() != 4) - throw std::runtime_error("[RevolutePairWithRotor] Xup must have 24 rows"); -#endif - - Xup[link1_index_] = link1_joint_->XJ() * link1_.Xtree_; - Xup[rotor1_index_] = rotor1_joint_->XJ() * rotor1_.Xtree_; - Xup[rotor2_index_] = rotor2_joint_->XJ() * rotor2_.Xtree_; - Xup[link2_index_] = link2_joint_->XJ() * link2_.Xtree_ * Xup[link1_index_]; + ratio_product_ = rotor_matrix * belt_matrix; } template std::vector, JointPtr, DMat>> RevolutePairWithRotor::bodiesJointsAndReflectedInertias() const { - std::vector, JointPtr, DMat>> bodies_joints_and_ref_inertias; + std::vector, JointPtr, DMat>> result; + + // The gear/belt constraint is linear (constant K), so G is state-independent. + // Rotor 1 velocity = ratio_product_(0,0)*q_link1 + 0*q_link2 + // Rotor 2 velocity = ratio_product_(1,0)*q_link1 + ratio_product_(1,1)*q_link2 + // S_dep_i = S_rotor_i * [ratio_product row i] (6 x 2 matrix) + // This avoids relying on this->S_ which is only set after updateKinematics(). - DMat S_dependent_1 = this->S_.template middleRows<6>(6 * rotor1_index_); + const DMat S_rotor1 = this->single_joints_[rotor1_index_]->S(); + DMat S_dep_1 = S_rotor1 * ratio_product_.row(0); Mat6 Ir1 = rotor1_.inertia_.getMatrix(); - DMat ref_inertia_1 = S_dependent_1.transpose() * Ir1 * S_dependent_1; - bodies_joints_and_ref_inertias.push_back(std::make_tuple(link1_, link1_joint_, - ref_inertia_1)); + DMat ref_inertia_1 = S_dep_1.transpose() * Ir1 * S_dep_1; + result.push_back(std::make_tuple(link1_, this->single_joints_[link1_index_], ref_inertia_1)); - DMat S_dependent_2 = this->S_.template middleRows<6>(6 * rotor2_index_); + const DMat S_rotor2 = this->single_joints_[rotor2_index_]->S(); + DMat S_dep_2 = S_rotor2 * ratio_product_.row(1); Mat6 Ir2 = rotor2_.inertia_.getMatrix(); - DMat ref_inertia_2 = S_dependent_2.transpose() * Ir2 * S_dependent_2; - bodies_joints_and_ref_inertias.push_back(std::make_tuple(link2_, link2_joint_, - ref_inertia_2)); + DMat ref_inertia_2 = S_dep_2.transpose() * Ir2 * S_dep_2; + result.push_back(std::make_tuple(link2_, this->single_joints_[link2_index_], ref_inertia_2)); - return bodies_joints_and_ref_inertias; + return result; } template class RevolutePairWithRotor; + template class RevolutePairWithRotor>; template class RevolutePairWithRotor; template class RevolutePairWithRotor; diff --git a/src/Dynamics/ClusterJoints/RevoluteTripleWithRotorJoint.cpp b/src/Dynamics/ClusterJoints/RevoluteTripleWithRotorJoint.cpp index ed911155..645eda2d 100644 --- a/src/Dynamics/ClusterJoints/RevoluteTripleWithRotorJoint.cpp +++ b/src/Dynamics/ClusterJoints/RevoluteTripleWithRotorJoint.cpp @@ -1,4 +1,5 @@ #include "grbda/Dynamics/ClusterJoints/RevoluteTripleWithRotorJoint.h" +#include "grbda/Utils/CasadiDerivatives.h" namespace grbda { @@ -13,7 +14,9 @@ namespace grbda const DistalTransmission &module_3) : Base(6, 3, 3), link_1_(module_1.body_), link_2_(module_2.body_), link_3_(module_3.body_), rotor_1_(module_1.rotor_), rotor_2_(module_2.rotor_), - rotor_3_(module_3.rotor_) + rotor_3_(module_3.rotor_), + axis1_(module_1.joint_axis_), axis2_(module_2.joint_axis_), axis3_(module_3.joint_axis_), + X_tree_2_(module_2.body_.Xtree_), X_tree_3_(module_3.body_.Xtree_) { using Rev = Joints::Revolute; @@ -65,6 +68,10 @@ namespace grbda const DVec &q = spanning_joint_state.position; const DVec &qd = spanning_joint_state.velocity; + // Cache spanning coordinates for derivative methods + q_spanning_ = q; + qd_spanning_ = qd; + link_1_joint_->updateKinematics(q.template segment<1>(0), qd.template segment<1>(0)); link_2_joint_->updateKinematics(q.template segment<1>(1), qd.template segment<1>(1)); link_3_joint_->updateKinematics(q.template segment<1>(2), qd.template segment<1>(2)); @@ -100,6 +107,7 @@ namespace grbda this->vJ_ = X_intra_S_span_ * qd; this->cJ_ = X_intra_S_span_ring_ * qd; + this->S_ring_ = X_intra_S_span_ring_ * this->loop_constraint_->G(); //+X_intra*S_span_*G_dot_; } template @@ -146,7 +154,275 @@ namespace grbda return bodies_joints_and_ref_inertias; } + template + char RevoluteTripleWithRotor::axisToChar(ori::CoordinateAxis axis) const + { + switch (axis) + { + case ori::CoordinateAxis::X: + return 'X'; + case ori::CoordinateAxis::Y: + return 'Y'; + case ori::CoordinateAxis::Z: + return 'Z'; + default: + throw std::runtime_error("Unknown axis"); + } + } + + template + void RevoluteTripleWithRotor::initializeCasadiFunctions() const + { + if (casadi_functions_initialized_) return; + + using namespace casadi; + using namespace casadi_derivatives; + + SX q1 = SX::sym("q1"); + SX q2 = SX::sym("q2"); + SX q3 = SX::sym("q3"); + SX qd1 = SX::sym("qd1"); + SX qd2 = SX::sym("qd2"); + SX qd3 = SX::sym("qd3"); + + char ax1 = axisToChar(axis1_); + char ax2 = axisToChar(axis2_); + char ax3 = axisToChar(axis3_); + + SX S1 = revoluteMotionSubspace(ax1); + SX S2 = revoluteMotionSubspace(ax2); + SX S3 = revoluteMotionSubspace(ax3); + + // Convert Xtree matrices to CasADi SX + DMat Xtree2_eigen = X_tree_2_.toMatrix().template cast(); + DMat Xtree3_eigen = X_tree_3_.toMatrix().template cast(); + + SX Xtree2_sx = SX::zeros(6, 6); + SX Xtree3_sx = SX::zeros(6, 6); + + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 6; ++j) { + Xtree2_sx(i, j) = Xtree2_eigen(i, j); + Xtree3_sx(i, j) = Xtree3_eigen(i, j); + } + } + + // Compute spatial rotations + SX XJ2 = spatialRotation(ax2, q2); + SX XJ3 = spatialRotation(ax3, q3); + + // Compute X21 = XJ2 * Xtree2 + SX X21 = mtimes(XJ2, Xtree2_sx); + + // Compute X32 = XJ3 * Xtree3 + SX X32 = mtimes(XJ3, Xtree3_sx); + + // Compute X31 = X32 * X21 + SX X31 = mtimes(X32, X21); + + // Motion subspaces for link2 and link3 + // Link2, column 0: X21 * S1 + SX S_link2_col0 = mtimes(X21, S1); + + // Link3, column 0: X31 * S1 + SX S_link3_col0 = mtimes(X31, S1); + + // Link3, column 1: X32 * S2 + SX S_link3_col1 = mtimes(X32, S2); + + // Compute jacobians for link2 + SX dS_link2_col0_dq1 = jacobian(S_link2_col0, q1); + SX dS_link2_col0_dq2 = jacobian(S_link2_col0, q2); + SX dS_link2_col0_dq3 = jacobian(S_link2_col0, q3); + + // Compute jacobians for link3 column 0 + SX dS_link3_col0_dq1 = jacobian(S_link3_col0, q1); + SX dS_link3_col0_dq2 = jacobian(S_link3_col0, q2); + SX dS_link3_col0_dq3 = jacobian(S_link3_col0, q3); + + // Compute jacobians for link3 column 1 + SX dS_link3_col1_dq1 = jacobian(S_link3_col1, q1); + SX dS_link3_col1_dq2 = jacobian(S_link3_col1, q2); + SX dS_link3_col1_dq3 = jacobian(S_link3_col1, q3); + + // Combine into output matrices + // Link2 derivatives: 6x3 matrix for each q + SX dS_link2_dq = horzcat(dS_link2_col0_dq1, dS_link2_col0_dq2, dS_link2_col0_dq3); + + // Link3 derivatives: 6x3 matrix for each q (combining both columns) + SX dS_link3_col0_dq = horzcat(dS_link3_col0_dq1, dS_link3_col0_dq2, dS_link3_col0_dq3); + SX dS_link3_col1_dq = horzcat(dS_link3_col1_dq1, dS_link3_col1_dq2, dS_link3_col1_dq3); + + // Create functions + f_dS_link2_dq_ = Function("dS_link2_dq", {q1, q2, q3}, {dS_link2_dq}); + f_dS_link3_dq_ = Function("dS_link3_dq", {q1, q2, q3}, {dS_link3_col0_dq, dS_link3_col1_dq}); + + // For Sdotqd derivatives + // Ṡ = dS/dq1*q̇1 + dS/dq2*q̇2 + dS/dq3*q̇3 + // We need to track derivatives for all configuration-dependent terms + + // For Sdotqd derivatives + // Ṡ = dS/dq1*q̇1 + dS/dq2*q̇2 + dS/dq3*q̇3 + // We need to compute Sdotqd for each link separately + + // Link2: Only has configuration-dependent column 0 + SX Sdot_link2_col0 = dS_link2_col0_dq1 * qd1 + dS_link2_col0_dq2 * qd2 + dS_link2_col0_dq3 * qd3; + SX Sdotqd_link2 = Sdot_link2_col0 * qd1; + + // Link3: Has configuration-dependent columns 0 and 1 + SX Sdot_link3_col0 = dS_link3_col0_dq1 * qd1 + dS_link3_col0_dq2 * qd2 + dS_link3_col0_dq3 * qd3; + SX Sdot_link3_col1 = dS_link3_col1_dq1 * qd1 + dS_link3_col1_dq2 * qd2 + dS_link3_col1_dq3 * qd3; + SX Sdotqd_link3 = Sdot_link3_col0 * qd1 + Sdot_link3_col1 * qd2; + + // Compute ∂(Ṡqd)/∂q for each link separately + SX dSdotqd_link2_dq1 = jacobian(Sdotqd_link2, q1); + SX dSdotqd_link2_dq2 = jacobian(Sdotqd_link2, q2); + SX dSdotqd_link2_dq3 = jacobian(Sdotqd_link2, q3); + + SX dSdotqd_link3_dq1 = jacobian(Sdotqd_link3, q1); + SX dSdotqd_link3_dq2 = jacobian(Sdotqd_link3, q2); + SX dSdotqd_link3_dq3 = jacobian(Sdotqd_link3, q3); + + f_Sdotqd_q_ = Function("Sdotqd_q", {q1, q2, q3, qd1, qd2, qd3}, + {horzcat(dSdotqd_link2_dq1, dSdotqd_link2_dq2, dSdotqd_link2_dq3), + horzcat(dSdotqd_link3_dq1, dSdotqd_link3_dq2, dSdotqd_link3_dq3)}); + + casadi_functions_initialized_ = true; + } + + template + static std::vector> computeSq( + casadi::Function& f_dS_link2_dq, + casadi::Function& f_dS_link3_dq, + const DVec& q_cache, + const DMat& G) + { + const int nv = 3; + const int spatial_dim = 36; + + std::vector input = { + casadi::DM(static_cast(q_cache(0))), + casadi::DM(static_cast(q_cache(1))), + casadi::DM(static_cast(q_cache(2))) + }; + + auto res_link2 = f_dS_link2_dq(input); + auto res_link3 = f_dS_link3_dq(input); + + casadi::DM dS_link2 = res_link2[0]; + casadi::DM dS_link3_col0 = res_link3[0]; + casadi::DM dS_link3_col1 = res_link3[1]; + + std::vector> dX_intra_dq(nv); + for (int i = 0; i < nv; ++i) + dX_intra_dq[i] = DMat::Zero(spatial_dim, 6); + + for (int i = 0; i < 6; ++i) + for (int j = 0; j < nv; ++j) + dX_intra_dq[j](6 + i, 0) = static_cast(static_cast(dS_link2(i, j))); + + for (int i = 0; i < 6; ++i) + for (int j = 0; j < nv; ++j) + dX_intra_dq[j](12 + i, 0) = static_cast(static_cast(dS_link3_col0(i, j))); + + for (int i = 0; i < 6; ++i) + for (int j = 0; j < nv; ++j) + dX_intra_dq[j](12 + i, 1) = static_cast(static_cast(dS_link3_col1(i, j))); + + std::vector> S_q(nv); + for (int i = 0; i < nv; ++i) + S_q[i] = dX_intra_dq[i] * G; + + return S_q; + } + + template + void RevoluteTripleWithRotor::evalSTimesVec_dq(const DVec& b, DMat& out) const + { + initializeCasadiFunctions(); + const int nv = 3; + const int mss_dim = this->num_bodies_ * 6; + const DMat& G = this->loop_constraint_->G(); + const auto S_q = computeSq(f_dS_link2_dq_, f_dS_link3_dq_, q_spanning_, G); + out.setZero(mss_dim, nv); + for (int i = 0; i < nv; ++i) + out.col(i) = S_q[i] * b; + } + + template + void RevoluteTripleWithRotor::evalSTTimesVec_dq(const DVec& F, DMat& out) const + { + initializeCasadiFunctions(); + const int nv = 3; + const DMat& G = this->loop_constraint_->G(); + const auto S_q = computeSq(f_dS_link2_dq_, f_dS_link3_dq_, q_spanning_, G); + out.setZero(nv, nv); + for (int i = 0; i < nv; ++i) + out.col(i) = S_q[i].transpose() * F; + } + + template + void RevoluteTripleWithRotor::getSdotqd_q(DMat& out) const + { + initializeCasadiFunctions(); + const int nv = 3; + const int spatial_dim = 36; + + std::vector input = { + casadi::DM(static_cast(q_spanning_(0))), + casadi::DM(static_cast(q_spanning_(1))), + casadi::DM(static_cast(q_spanning_(2))), + casadi::DM(static_cast(qd_spanning_(0))), + casadi::DM(static_cast(qd_spanning_(1))), + casadi::DM(static_cast(qd_spanning_(2))) + }; + + std::vector result = f_Sdotqd_q_(input); + casadi::DM Sdotqd_q_link2 = result[0]; // 6x3 matrix for link2 + casadi::DM Sdotqd_q_link3 = result[1]; // 6x3 matrix for link3 + + out.setZero(spatial_dim, nv); + + // Link2 contribution (rows 6-11) + for (int i = 0; i < 6; ++i) + for (int j = 0; j < nv; ++j) + out(6 + i, j) = static_cast(static_cast(Sdotqd_q_link2(i, j))); + + // Link3 contribution (rows 12-17) + for (int i = 0; i < 6; ++i) + for (int j = 0; j < nv; ++j) + out(12 + i, j) = static_cast(static_cast(Sdotqd_q_link3(i, j))); + } + + // Complex specializations + template <> + void RevoluteTripleWithRotor>::initializeCasadiFunctions() const + { + casadi_functions_initialized_ = true; + } + + template <> + void RevoluteTripleWithRotor>::evalSTimesVec_dq( + const DVec>&, DMat>& out) const + { + out.setZero(36, 3); + } + + template <> + void RevoluteTripleWithRotor>::evalSTTimesVec_dq( + const DVec>&, DMat>& out) const + { + out.setZero(3, 3); + } + + template <> + void RevoluteTripleWithRotor>::getSdotqd_q(DMat>& out) const + { + out.setZero(36, 3); + } + template class RevoluteTripleWithRotor; + template class RevoluteTripleWithRotor>; template class RevoluteTripleWithRotor; } diff --git a/src/Dynamics/ClusterJoints/RevoluteWithRotorJoint.cpp b/src/Dynamics/ClusterJoints/RevoluteWithRotorJoint.cpp index 476f22e3..032c13b9 100644 --- a/src/Dynamics/ClusterJoints/RevoluteWithRotorJoint.cpp +++ b/src/Dynamics/ClusterJoints/RevoluteWithRotorJoint.cpp @@ -73,7 +73,9 @@ namespace grbda return bodies_joints_and_reflected_inertias; } + template class RevoluteWithRotor; + template class RevoluteWithRotor>; template class RevoluteWithRotor; template class RevoluteWithRotor; } diff --git a/src/Dynamics/ClusterTreeDynamics.cpp b/src/Dynamics/ClusterTreeDynamics.cpp index 3ea381c2..3f1c1423 100644 --- a/src/Dynamics/ClusterTreeDynamics.cpp +++ b/src/Dynamics/ClusterTreeDynamics.cpp @@ -94,7 +94,9 @@ namespace grbda // Forward Pass - Articulated body bias force for (auto &cluster : cluster_nodes_) { - cluster->pA_ = spatial::generalForceCrossProduct(cluster->v_, DVec(cluster->I_ * cluster->v_)); + cluster->pA_.setZero(cluster->motion_subspace_dimension_); + spatial::addGeneralForceCrossProduct(cluster->v_, + spatial::blockDiagonalTimesVector(cluster->I_, cluster->v_), cluster->pA_); } // Account for external forces in bias force @@ -165,7 +167,7 @@ namespace grbda // Forward pass for (auto &cluster : cluster_nodes_) { - cluster->IA_ = cluster->I_; + cluster->IA_ = spatial::blockDiagonalToMatrix(cluster->I_); } // Backward pass (Gauss principal of least constraint) @@ -219,7 +221,9 @@ namespace grbda const auto joint = cluster->joint_; DVec tmp = joint->S().transpose() * f; - lambda_inv += tmp.dot(DVec(cluster->D_inv_.solve(tmp))); + // CRITICAL FIX: Use transpose()*vec instead of dot() to avoid complex conjugation + // Eigen's dot(a,b) computes conj(a)^T * b, but we need a^T * b for complex-step + lambda_inv += (tmp.transpose() * DVec(cluster->D_inv_.solve(tmp)))(0); dstate_out += cluster->qdd_for_subtree_due_to_subtree_root_joint_qdd * cluster->D_inv_.solve(tmp); @@ -301,7 +305,7 @@ namespace grbda this->forwardKinematics(); for (auto &cluster : cluster_nodes_) { - cluster->IA_ = cluster->I_; + cluster->IA_ = spatial::blockDiagonalToMatrix(cluster->I_); } // Reset Force Propagators for the end-effectors @@ -434,7 +438,447 @@ namespace grbda return lambda_inv; } + template + std::pair, DMat> ClusterTreeModel::firstOrderInverseDynamicsDerivatives(const DVec &qdd) + { + const auto [q, qd] = this->getState(); + this->forwardAccelerationKinematics(qdd); + + const int nClusters = static_cast(cluster_nodes_.size()); + DMat &dtau_dq = this->dtau_dq_; + DMat &dtau_dq_dot = this->dtau_dqd_; + dtau_dq.setZero(); + dtau_dq_dot.setZero(); + + // Forward Pass - compute Psi_dot, Psi_ddot, Upsilon_dot, M_cup, B_cup, F for each cluster + for (auto &cluster : cluster_nodes_) + { + const int mss_dim = cluster->motion_subspace_dimension_; + const int num_vel = cluster->num_velocities_; + const DMat &S = cluster->S(); + const auto &I_blocks = cluster->I_; + const DVec &v = cluster->v_; + + // Get parent velocity and acceleration + DVec &v_parent_up = cluster->v_parent_up_; + DVec &a_parent_up = cluster->a_parent_up_; + if (cluster->parent_index_ >= 0) + { + const auto &parent_cluster = cluster_nodes_[cluster->parent_index_]; + v_parent_up = cluster->Xup_.transformMotionVector(parent_cluster->v_); + a_parent_up = cluster->Xup_.transformMotionVector(parent_cluster->a_); + } + else + { + v_parent_up.setZero(); + a_parent_up = cluster->Xup_.transformMotionVector(-this->getGravity()); + } + + // Compute alpha = d(S*qd)/dq and beta = d(S*qdd)/dq using efficient contractions + // Only compute for joints with configuration-dependent S (e.g., GenericJoint with CasADi) + const DVec cluster_qd = qd.segment(cluster->velocity_index_, num_vel); + const DVec cluster_qdd = qdd.segment(cluster->velocity_index_, num_vel); + + const bool has_config_dependent_S = cluster->joint_->hasConfigurationDependentS(); + + DMat &alpha = cluster->alpha_workspace_; + DMat &beta = cluster->beta_workspace_; + DMat &Sdotqd_q = cluster->Sdotqd_q_workspace_; + + if (has_config_dependent_S) { + cluster->joint_->evalSTimesVec_dq(cluster_qd, alpha); + cluster->joint_->evalSTimesVec_dq(cluster_qdd, beta); + cluster->joint_->getSdotqd_q(Sdotqd_q); + } + + // Psi_dot = crm(v_parent_up) * S + alpha + spatial::motionCrossTimesMatrix(v_parent_up, S, cluster->Psi_dot_); + if (has_config_dependent_S) { + cluster->Psi_dot_ += alpha; + } + + // Upsilon_dot = crm(v)*S + Psi_dot + S_ring (compute crm(v)*S directly into Upsilon_dot) + spatial::motionCrossTimesMatrix(v, S, cluster->Upsilon_dot_); + cluster->Upsilon_dot_ += cluster->Psi_dot_ + cluster->S_ring(); + + // Psi_ddot = crm(a_parent_up)*S + crm(v_parent_up)*Psi_dot + Sdotqd_q + beta + crm(v)*alpha + spatial::motionCrossTimesMatrix(a_parent_up, S, cluster->Psi_ddot_); + spatial::addMotionCrossTimesMatrix(v_parent_up, cluster->Psi_dot_, cluster->Psi_ddot_); + if (has_config_dependent_S) { + cluster->Psi_ddot_ += Sdotqd_q + beta; + spatial::addMotionCrossTimesMatrix(v, alpha, cluster->Psi_ddot_); + } + + // M_cup = I (will accumulate children's contributions) + cluster->M_cup_ = spatial::blockDiagonalToMatrix(I_blocks); + + // B_cup = crf(v)*I - I*crm(v) + icrf(I*v) + const DVec Iv = spatial::blockDiagonalTimesVector(I_blocks, v); + spatial::spatialInertiaCrossTerms(I_blocks, v, cluster->B_cup_); + spatial::addSwappedForceCrossMatrixInPlace(cluster->B_cup_, Iv); + + // F = I*a + crf(v)*I*v + cluster->F_ = spatial::blockDiagonalTimesVector(I_blocks, cluster->a_); + spatial::addGeneralForceCrossProduct(v, Iv, cluster->F_); + } + + // Backward Pass - compute derivatives and propagate M_cup, B_cup, F to parents + for (int i = nClusters - 1; i >= 0; i--) + { + auto &cluster_i = cluster_nodes_[i]; + const int ii = cluster_i->velocity_index_; + const int num_vel_i = cluster_i->num_velocities_; + const int mss_dim_i = cluster_i->motion_subspace_dimension_; + + // Cache references + const DMat &M_cup = cluster_i->M_cup_; + const DMat &B_cup = cluster_i->B_cup_; + const DVec &F = cluster_i->F_; + const DMat &S_i = cluster_i->S(); + + DMat &t1 = cluster_i->t1_workspace_; + DMat &t2 = cluster_i->t2_workspace_; + DMat &t3 = cluster_i->t3_workspace_; + DMat &t4 = cluster_i->t4_workspace_; + + DMat &tmp = cluster_i->t_tmp_workspace_; + cluster_i->Xup_.blockDiagonalInertiaTimesMotionSubspace(M_cup, S_i, t1); + cluster_i->Xup_.blockDiagonalInertiaTimesMotionSubspace(B_cup, S_i, t2); + cluster_i->Xup_.blockDiagonalInertiaTimesMotionSubspace(M_cup, cluster_i->Upsilon_dot_, tmp); + t2+= tmp; + cluster_i->Xup_.blockDiagonalInertiaTimesMotionSubspace(B_cup, cluster_i->Psi_dot_, t3); + cluster_i->Xup_.blockDiagonalInertiaTimesMotionSubspace(M_cup, cluster_i->Psi_ddot_, tmp); + t3+= tmp; + spatial::addSwappedForceCrossTimesMatrix(F, S_i, t3); + cluster_i->Xup_.blockDiagonalInertiaTimesMotionSubspace(B_cup.transpose(), S_i, t4); + + // Walk from cluster i to root + // Use optimized path for single-body clusters (most common case) + if (mss_dim_i == 6) + { + // Single-body cluster: use Transform directly for efficiency + int j = i; + while (j >= 0) + { + auto &cluster_j = cluster_nodes_[j]; + const int jj = cluster_j->velocity_index_; + const int num_vel_j = cluster_j->num_velocities_; + const DMat &S_j = cluster_j->S(); + + // dtau_dq(ii, jj) = t1^T * Psi_ddot_j + t4^T * Psi_dot_j + dtau_dq.block(ii, jj, num_vel_i, num_vel_j)= + t1.transpose() * cluster_j->Psi_ddot_ + t4.transpose() * cluster_j->Psi_dot_; + + if (j < i) + { + dtau_dq.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t3; + } + else // j == i (diagonal block) + { + // Only compute S^T derivative for joints with config-dependent S + if (cluster_i->joint_->hasConfigurationDependentS()) { + cluster_i->joint_->evalSTTimesVec_dq(F, cluster_i->st_dq_workspace_); + dtau_dq.block(ii, ii, num_vel_i, num_vel_i) += cluster_i->st_dq_workspace_; + } + } + + dtau_dq_dot.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t2; + dtau_dq_dot.block(ii, jj, num_vel_i, num_vel_j)= + t1.transpose() * cluster_j->Upsilon_dot_ + t4.transpose() * S_j; + + // Transform t1, t2, t3, t4 to parent frame using batched transform + // This computes E^T and r_hat*E^T only once for all 4 matrices + if (cluster_j->parent_index_ >= 0) + { + const auto &X = cluster_j->Xup_[0]; + X.inverseTransformForceSubspace4(t1, t2, t3, t4); + } + j = cluster_j->parent_index_; + } + } + else + { + // Multi-body cluster: use GeneralizedTransform + int j = i; + while (j >= 0) + { + auto &cluster_j = cluster_nodes_[j]; + const int jj = cluster_j->velocity_index_; + const int num_vel_j = cluster_j->num_velocities_; + const DMat &S_j = cluster_j->S(); + + dtau_dq.block(ii, jj, num_vel_i, num_vel_j)= + t1.transpose() * cluster_j->Psi_ddot_ + t4.transpose() * cluster_j->Psi_dot_; + + if (j < i) + { + dtau_dq.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t3; + } + else + { + // Only compute S^T derivative for joints with config-dependent S + if (cluster_i->joint_->hasConfigurationDependentS()) { + cluster_i->joint_->evalSTTimesVec_dq(F, cluster_i->st_dq_workspace_); + dtau_dq.block(ii, ii, num_vel_i, num_vel_i) += cluster_i->st_dq_workspace_; + } + } + + dtau_dq_dot.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t2; + dtau_dq_dot.block(ii, jj, num_vel_i, num_vel_j)= + t1.transpose() * cluster_j->Upsilon_dot_ + t4.transpose() * S_j; + + // Transform t1, t2, t3, t4 to parent frame using batched transform + // This shares E^T and r_hat*E^T computation across all 4 matrices per body + if (cluster_j->parent_index_ >= 0) + { + cluster_j->Xup_.inverseTransformForceSubspace4(t1, t2, t3, t4); + } + j = cluster_j->parent_index_; + } + } + + // Propagate M_cup, B_cup, F to parent + // Use batched inertia accumulation to share E^T and r_hat computation + if (cluster_i->parent_index_ >= 0) + { + auto &parent_cluster = cluster_nodes_[cluster_i->parent_index_]; + cluster_i->Xup_.accumulateBlockDiagonalPair( + M_cup, parent_cluster->M_cup_, + B_cup, parent_cluster->B_cup_); + parent_cluster->F_ += cluster_i->Xup_.inverseTransformForceVector(F); + } + } + + return {dtau_dq, dtau_dq_dot}; + } + + template + std::pair, DMat> ClusterTreeModel::firstOrderInverseDynamicsDerivativesWorldFrame(const DVec &qdd) + { + // World-frame algorithm for ID derivatives following ID_derivatives_world.m. + // Per-node world-frame quantities are stored on the nodes (Ic0_, S0_, BC0_, etc.) + // to avoid per-call allocation. F1-F4 accumulators are class members. + + const auto [q, qd] = this->getState(); + this->forwardAccelerationKinematics(qdd); + + const int nClusters = static_cast(cluster_nodes_.size()); + DMat &dtau_dq = this->dtau_dq_; + DMat &dtau_dq_dot = this->dtau_dqd_; + dtau_dq.setZero(); + dtau_dq_dot.setZero(); + + // Zero the F accumulators (class members, pre-sized in resizeSystemMatrices) + idDeriv_F1_.setZero(); + idDeriv_F2_.setZero(); + idDeriv_F3_.setZero(); + idDeriv_F4_.setZero(); + + // Forward Pass - compute quantities and transform to world frame, storing in nodes + for (int i = 0; i < nClusters; i++) + { + auto &cluster = cluster_nodes_[i]; + const int mss_dim = cluster->motion_subspace_dimension_; + const int & num_vel = cluster->num_velocities_; + const int num_bodies = cluster->Xa_.getNumOutputBodies(); + const DMat &S = cluster->S(); + const auto &I_blocks = cluster->I_; + const DVec &v = cluster->v_; + + // Get parent velocity and acceleration in cluster i's frame + DVec &v_parent_up = cluster->v_parent_up_; + DVec &a_parent_up = cluster->a_parent_up_; + if (cluster->parent_index_ >= 0) + { + const auto &parent_cluster = cluster_nodes_[cluster->parent_index_]; + v_parent_up = cluster->Xup_.transformMotionVector(parent_cluster->v_); + a_parent_up = cluster->Xup_.transformMotionVector(parent_cluster->a_); + } + else + { + v_parent_up.setZero(); + a_parent_up = cluster->Xup_.transformMotionVector(-this->getGravity()); + } + + // Compute alpha = dS/dy * qd and beta = dS/dy * qdd (zero for constant-S joints) + const DVec cluster_qd = qd.segment(cluster->velocity_index_, num_vel); + const DVec cluster_qdd = qdd.segment(cluster->velocity_index_, num_vel); + const bool has_config_dependent_S = cluster->joint_->hasConfigurationDependentS(); + + DMat &alpha = cluster->alpha_workspace_; + DMat &beta = cluster->beta_workspace_; + DMat &Sdotqd_q = cluster->Sdotqd_q_workspace_; + + if (has_config_dependent_S) { + cluster->joint_->evalSTimesVec_dq(cluster_qd, alpha); + cluster->joint_->evalSTimesVec_dq(cluster_qdd, beta); + cluster->joint_->getSdotqd_q(Sdotqd_q); + } + + // Psi_dot = crm(v_parent_up) * S + alpha + spatial::motionCrossTimesMatrix(v_parent_up, S, cluster->Psi_dot_); + if (has_config_dependent_S) { + cluster->Psi_dot_ += alpha; + } + + // Upsilon_dot = crm(v)*S + Psi_dot + S_ring (compute crm(v)*S directly into Upsilon_dot) + spatial::motionCrossTimesMatrix(v, S, cluster->Upsilon_dot_); + cluster->Upsilon_dot_ += cluster->Psi_dot_ + cluster->S_ring(); + + // Psi_ddot = crm(a_parent_up)*S + crm(v_parent_up)*Psi_dot + Sdotqd_q + beta + crm(v)*alpha + spatial::motionCrossTimesMatrix(a_parent_up, S, cluster->Psi_ddot_); + spatial::addMotionCrossTimesMatrix(v_parent_up, cluster->Psi_dot_, cluster->Psi_ddot_); + if (has_config_dependent_S) { + cluster->Psi_ddot_ += Sdotqd_q + beta; + spatial::addMotionCrossTimesMatrix(v, alpha, cluster->Psi_ddot_); + } + + // F = I*a + crf(v)*I*v + const DVec Iv = spatial::blockDiagonalTimesVector(I_blocks, v); + cluster->F_ = spatial::blockDiagonalTimesVector(I_blocks, cluster->a_); + spatial::addGeneralForceCrossProduct(v, Iv, cluster->F_); + + // Transform quantities to world frame, block by block, into node storage + cluster->Ic0_.resize(num_bodies); + cluster->BC0_.resize(num_bodies); + cluster->S0_.resize(num_bodies); + cluster->Psid0_.resize(num_bodies); + cluster->Psidd0_.resize(num_bodies); + cluster->Upsilond0_.resize(num_bodies); + cluster->f0_.resize(num_bodies); + + for (int body = 0; body < num_bodies; body++) + { + const auto &Xa_body = cluster->Xa_.getTransformForOutputBody(body); + const int start = 6 * body; + + // IC0[body] = X^{-T} * I_body * X^{-1} + cluster->Ic0_[body] = + Xa_body.inverseTransformSpatialInertia(I_blocks[body]); + + // v0 = X^{-1} * v_body + const SVec v0_body = + Xa_body.inverseTransformMotionVector(v.template segment<6>(start)); + + // BC0 = crf(v0)*IC0 + icrf(IC0*v0) - IC0*crm(v0) + const SVec I0v0 = cluster->Ic0_[body] * v0_body; + cluster->BC0_[body] = + spatial::forceCrossMatrix(v0_body) * cluster->Ic0_[body] + + spatial::swappedForceCrossMatrix(I0v0) - + cluster->Ic0_[body] * spatial::motionCrossMatrix(v0_body); + + cluster->S0_[body] = + Xa_body.inverseTransformMotionSubspace(S.template middleRows<6>(start)); + cluster->Psid0_[body] = + Xa_body.inverseTransformMotionSubspace(cluster->Psi_dot_.template middleRows<6>(start)); + cluster->Psidd0_[body] = + Xa_body.inverseTransformMotionSubspace(cluster->Psi_ddot_.template middleRows<6>(start)); + cluster->Upsilond0_[body] = + Xa_body.inverseTransformMotionSubspace(cluster->Upsilon_dot_.template middleRows<6>(start)); + cluster->f0_[body] = + Xa_body.inverseTransformForceVector(cluster->F_.template segment<6>(start)); + } + } + + // Backward Pass + for (int i = nClusters - 1; i >= 0; i--) + { + auto &cluster_i = cluster_nodes_[i]; + const int & ii = cluster_i->velocity_index_; + const int & num_vel_i = cluster_i->num_velocities_; + const int & num_bodies_i = cluster_i->Xa_.getNumOutputBodies(); + + // Compute per-body F_tmp blocks and accumulate diagonal H blocks and F columns + dtau_dq_dot.block(ii, ii, num_vel_i, num_vel_i).setZero(); + dtau_dq.block(ii, ii, num_vel_i, num_vel_i).setZero(); + for (int body = 0; body < num_bodies_i; body++) + { + const Mat6 &IC0_b = cluster_i->Ic0_[body]; + const Mat6 &BC0_b = cluster_i->BC0_[body]; + const D6Mat &S0_b = cluster_i->S0_[body]; + const D6Mat &Psid0_b = cluster_i->Psid0_[body]; + const D6Mat &Psidd0_b = cluster_i->Psidd0_[body]; + const D6Mat &Upd0_b = cluster_i->Upsilond0_[body]; + const SVec &f0_b = cluster_i->f0_[body]; + + const D6Mat F1_b = IC0_b * S0_b; + const D6Mat F2_b = BC0_b * S0_b + IC0_b * Upd0_b; + D6Mat F3_b = BC0_b * Psid0_b + IC0_b * Psidd0_b; + F3_b += spatial::swappedForceCrossMatrix(f0_b) * S0_b; + const D6Mat F4_b = BC0_b.transpose() * S0_b; + + // Diagonal blocks: accumulate over all bodies + dtau_dq_dot.block(ii, ii, num_vel_i, num_vel_i)+= + F1_b.transpose() * Upd0_b + F4_b.transpose() * S0_b; + dtau_dq.block(ii, ii, num_vel_i, num_vel_i)+= + F1_b.transpose() * Psidd0_b + F4_b.transpose() * Psid0_b; + + // F(:,ii) = blockRowSum — accumulate into class-member accumulators + idDeriv_F1_.middleCols(ii, num_vel_i) += F1_b; + idDeriv_F2_.middleCols(ii, num_vel_i) += F2_b; + idDeriv_F3_.middleCols(ii, num_vel_i) += F3_b; + idDeriv_F4_.middleCols(ii, num_vel_i) += F4_b; + } + + // contractT(S_q, f) + if (cluster_i->joint_->hasConfigurationDependentS()) { + cluster_i->joint_->evalSTTimesVec_dq(cluster_i->F_, cluster_i->st_dq_workspace_); + dtau_dq.block(ii, ii, num_vel_i, num_vel_i) += cluster_i->st_dq_workspace_; + } + + if (cluster_i->parent_index_ >= 0) + { + const int & parent = cluster_i->parent_index_; + const int & pp = cluster_nodes_[parent]->velocity_index_; + const int & num_vel_parent = cluster_nodes_[parent]->num_velocities_; + + // Subtree is contiguous: starts at vel_idx_i, spans subtree_num_velocities_ + const int & vi_start = ii; + const int & vi_size = cluster_i->subtree_num_velocities_; + + // Parent body subindex + const int & parent_subindex = cluster_i->Xup_.transform_and_parent_subindex(0).second; + + // Parent's world-frame motion subspaces at the connecting body + const D6Mat &Sblock = cluster_nodes_[parent]->S0_[parent_subindex]; + const D6Mat &Upblock = cluster_nodes_[parent]->Upsilond0_[parent_subindex]; + const D6Mat &Psidblock = cluster_nodes_[parent]->Psid0_[parent_subindex]; + const D6Mat &Psiddblock = cluster_nodes_[parent]->Psidd0_[parent_subindex]; + + // Off-diagonal blocks + dtau_dq.block(vi_start, pp, vi_size, num_vel_parent)= + idDeriv_F1_.middleCols(vi_start, vi_size).transpose() * Psiddblock + + idDeriv_F4_.middleCols(vi_start, vi_size).transpose() * Psidblock; + dtau_dq.block(pp, vi_start, num_vel_parent, vi_size)= + Sblock.transpose() * idDeriv_F3_.middleCols(vi_start, vi_size); + dtau_dq_dot.block(pp, vi_start, num_vel_parent, vi_size)= + Sblock.transpose() * idDeriv_F2_.middleCols(vi_start, vi_size); + dtau_dq_dot.block(vi_start, pp, vi_size, num_vel_parent)= + idDeriv_F1_.middleCols(vi_start, vi_size).transpose() * Upblock + + idDeriv_F4_.middleCols(vi_start, vi_size).transpose() * Sblock; + + // Accumulate composite quantities to parent (block-diagonal sum) + auto &parent_IC0 = cluster_nodes_[parent]->Ic0_[parent_subindex]; + auto &parent_BC0 = cluster_nodes_[parent]->BC0_[parent_subindex]; + auto &parent_f0 = cluster_nodes_[parent]->f0_[parent_subindex]; + for (int body = 0; body < num_bodies_i; body++) + { + parent_IC0 += cluster_i->Ic0_[body]; + parent_BC0 += cluster_i->BC0_[body]; + parent_f0 += cluster_i->f0_[body]; + } + + // Propagate f in body frame for parent's RNE + cluster_nodes_[parent]->F_ += cluster_i->Xup_.inverseTransformForceVector(cluster_i->F_); + } + } + + return {dtau_dq, dtau_dq_dot}; + } + + template class ClusterTreeModel; + template class ClusterTreeModel>; template class ClusterTreeModel; template class ClusterTreeModel; diff --git a/src/Dynamics/ClusterTreeModel.cpp b/src/Dynamics/ClusterTreeModel.cpp index dae917a1..90cc823f 100644 --- a/src/Dynamics/ClusterTreeModel.cpp +++ b/src/Dynamics/ClusterTreeModel.cpp @@ -148,10 +148,43 @@ namespace grbda this->H_ = DMat::Zero(num_degrees_of_freedom, num_degrees_of_freedom); this->C_ = DVec::Zero(num_degrees_of_freedom); + this->idDeriv_F1_ = D6Mat::Zero(6, num_degrees_of_freedom); + this->idDeriv_F2_ = D6Mat::Zero(6, num_degrees_of_freedom); + this->idDeriv_F3_ = D6Mat::Zero(6, num_degrees_of_freedom); + this->idDeriv_F4_ = D6Mat::Zero(6, num_degrees_of_freedom); + + this->dtau_dq_ = DMat::Zero(num_degrees_of_freedom, num_degrees_of_freedom); + this->dtau_dqd_ = DMat::Zero(num_degrees_of_freedom, num_degrees_of_freedom); + + // Precompute subtree velocity counts for each node (used by world-frame CRBA). + for (auto &node : this->nodes_) + node->subtree_num_velocities_ = node->num_velocities_; + for (int i = (int)this->nodes_.size() - 1; i >= 0; i--) + { + const auto &node = this->nodes_[i]; + if (node->parent_index_ >= 0) + this->nodes_[node->parent_index_]->subtree_num_velocities_ += + node->subtree_num_velocities_; + } + for (auto &cluster : cluster_nodes_) + { cluster->qdd_for_subtree_due_to_subtree_root_joint_qdd .setZero(num_degrees_of_freedom, cluster->num_velocities_); + const int mss_dim = cluster->motion_subspace_dimension_; + const int num_vel = cluster->num_velocities_; + cluster->t1_workspace_.resize(mss_dim, num_vel); + cluster->t2_workspace_.resize(mss_dim, num_vel); + cluster->t3_workspace_.resize(mss_dim, num_vel); + cluster->t4_workspace_.resize(mss_dim, num_vel); + cluster->t_tmp_workspace_.resize(mss_dim, num_vel); + cluster->alpha_workspace_.resize(mss_dim, num_vel); + cluster->beta_workspace_.resize(mss_dim, num_vel); + cluster->Sdotqd_q_workspace_.resize(mss_dim, num_vel); + cluster->st_dq_workspace_.resize(num_vel, num_vel); + } + for (auto &contact_point : this->contact_points_) { contact_point.jacobian_.setZero(6, num_degrees_of_freedom); @@ -254,16 +287,80 @@ namespace grbda } template - void ClusterTreeModel::setState(const ModelState &model_state) + void ClusterTreeModel::setState(const ModelState &model_state, + bool enforce_constraints) { size_t i = 0; for (auto &cluster : cluster_nodes_) { - cluster->joint_state_ = model_state.at(i); + cluster->joint_state_ = cluster->joint_->toSpanningTreeState( + model_state.at(i), enforce_constraints); i++; } this->setExternalForces(); + // CRITICAL: Invalidate cached kinematics when state changes + // This ensures q_spanning_ in Generic joints is updated on next forwardKinematics() call + this->resetCache(); + } + + template + std::pair, DVec> ClusterTreeModel::getState() + { + const int nq = this->getNumPositions(); + const int nv = this->getNumDegreesOfFreedom(); + + DVec q = DVec::Zero(nq); + DVec qd = DVec::Zero(nv); + + for (const auto &cluster : cluster_nodes_) + { + const auto &pos = cluster->joint_state_.position; + const auto &vel = cluster->joint_state_.velocity; + + if ((int)pos.size() != cluster->num_positions_) + { + const DMat conv = + cluster->joint_->spanningTreeToIndependentCoordsConversion() + .template cast(); + q.segment(cluster->position_index_, cluster->num_positions_).noalias() = + conv * pos; + } + else + { + q.segment(cluster->position_index_, cluster->num_positions_) = pos; + } + + if ((int)vel.size() != cluster->num_velocities_) + { + const int nv_ind = cluster->num_velocities_; + const auto &conv_int = + cluster->joint_->spanningTreeToIndependentCoordsConversion(); + if (conv_int.rows() == nv_ind && conv_int.cols() == (int)vel.size()) + { + const DMat conv = conv_int.template cast(); + qd.segment(cluster->velocity_index_, nv_ind).noalias() = conv * vel; + } + else + { + // Recover independent velocity via G pseudoinverse: v_ind = (G^T G)^{-1} G^T v_span + // Not supported for casadi::SX (symbolic models don't use getState()) + if constexpr (!std::is_same_v) + { + const DMat &G_mat = cluster->joint_->G(); + const DMat GtG = G_mat.transpose() * G_mat; + qd.segment(cluster->velocity_index_, nv_ind) = + GtG.inverse() * (G_mat.transpose() * vel); + } + } + } + else + { + qd.segment(cluster->velocity_index_, cluster->num_velocities_) = vel; + } + } + + return {q, qd}; } template @@ -285,7 +382,10 @@ namespace grbda } template - // NOTE: This function is only for non-spanning joint coordinates + // NOTE: This function converts state vectors to ModelState. + // For joints with implicit constraints, positions are treated as spanning (since they + // cannot be converted from independent to spanning). For explicit constraints, positions + // are treated as independent. Velocities are always treated as independent. ModelState ClusterTreeModel::stateVectorToModelState(const StatePair &q_qd_pair) { @@ -299,7 +399,12 @@ namespace grbda const int &num_vel = cluster->num_velocities_; DVec qd_cluster = q_qd_pair.second.segment(vel_idx, num_vel); - JointState joint_state(JointCoordinate(q_cluster, false), + // For implicit constraints, positions must be spanning since we cannot + // convert independent positions to spanning. For explicit constraints, + // positions are independent and will be converted via gamma(). + const bool pos_is_spanning = cluster->joint_->isImplicit(); + + JointState joint_state(JointCoordinate(q_cluster, pos_is_spanning), JointCoordinate(qd_cluster, false)); state.push_back(joint_state); } @@ -571,6 +676,7 @@ namespace grbda } template class ClusterTreeModel; + template class ClusterTreeModel>; template class ClusterTreeModel; template class ClusterTreeModel; diff --git a/src/Dynamics/ClusterTreeParsing.cpp b/src/Dynamics/ClusterTreeParsing.cpp index 8b0f5834..0914c12b 100644 --- a/src/Dynamics/ClusterTreeParsing.cpp +++ b/src/Dynamics/ClusterTreeParsing.cpp @@ -440,6 +440,7 @@ namespace grbda } template class ClusterTreeModel; + template class ClusterTreeModel>; template class ClusterTreeModel; template class ClusterTreeModel; diff --git a/src/Dynamics/Nodes/ClusterTreeNode.cpp b/src/Dynamics/Nodes/ClusterTreeNode.cpp index a94ba4fb..78dd42cd 100644 --- a/src/Dynamics/Nodes/ClusterTreeNode.cpp +++ b/src/Dynamics/Nodes/ClusterTreeNode.cpp @@ -14,9 +14,13 @@ namespace grbda velocity_index, joint->numVelocities()), bodies_(bodies), joint_(joint) { + const int mss_dim = 6 * (int)bodies.size(); + this->v_parent_up_ = DVec::Zero(mss_dim); + this->a_parent_up_ = DVec::Zero(mss_dim); + for (size_t i = 0; i < bodies.size(); i++) { - this->I_.template block<6, 6>(6 * i, 6 * i) = bodies[i].inertia_.getMatrix(); + this->I_[i] = bodies[i].inertia_.getMatrix(); this->Xup_.appendTransformWithClusterAncestorSubIndex( spatial::Transform{}, bodies[i].cluster_ancestor_sub_index_within_cluster_); this->Xa_.appendTransform(spatial::Transform{}); @@ -97,6 +101,7 @@ namespace grbda } template struct ClusterTreeNode; + template struct ClusterTreeNode>; template struct ClusterTreeNode; template struct ClusterTreeNode; diff --git a/src/Dynamics/Nodes/ReflectedInertiaTreeNode.cpp b/src/Dynamics/Nodes/ReflectedInertiaTreeNode.cpp index da34f849..82192d72 100644 --- a/src/Dynamics/Nodes/ReflectedInertiaTreeNode.cpp +++ b/src/Dynamics/Nodes/ReflectedInertiaTreeNode.cpp @@ -15,9 +15,10 @@ namespace grbda velocity_index, joint->numVelocities()), link_(link), joint_(joint), Xtree_(link.Xtree_) { - this->I_ = link.inertia_.getMatrix(); + this->I_[0] = link.inertia_.getMatrix(); this->Xup_.appendTransformWithClusterAncestorSubIndex(spatial::Transform{}, 0); this->Xa_.appendTransform(spatial::Transform{}); + S_ring_ = DMat::Zero(6, joint->numVelocities()); } template @@ -55,6 +56,7 @@ namespace grbda } template struct ReflectedInertiaTreeNode; + template struct ReflectedInertiaTreeNode>; template struct ReflectedInertiaTreeNode; template struct ReflectedInertiaTreeNode; diff --git a/src/Dynamics/Nodes/RigidBodyTreeNode.cpp b/src/Dynamics/Nodes/RigidBodyTreeNode.cpp index d187a702..e1fc27b1 100644 --- a/src/Dynamics/Nodes/RigidBodyTreeNode.cpp +++ b/src/Dynamics/Nodes/RigidBodyTreeNode.cpp @@ -14,9 +14,10 @@ namespace grbda velocity_index, joint->numVelocities()), body_(body), joint_(joint), Xtree_(body.Xtree_) { - this->I_ = body.inertia_.getMatrix(); + this->I_[0] = body.inertia_.getMatrix(); this->Xup_.appendTransformWithClusterAncestorSubIndex(spatial::Transform{}, 0); this->Xa_.appendTransform(spatial::Transform{}); + S_ring_ = DMat::Zero(6, joint->numVelocities()); } template @@ -54,6 +55,7 @@ namespace grbda } template struct RigidBodyTreeNode; + template struct RigidBodyTreeNode>; template struct RigidBodyTreeNode; template struct RigidBodyTreeNode; diff --git a/src/Dynamics/ReflectedInertiaTreeModel.cpp b/src/Dynamics/ReflectedInertiaTreeModel.cpp index 2050534a..33f8da93 100644 --- a/src/Dynamics/ReflectedInertiaTreeModel.cpp +++ b/src/Dynamics/ReflectedInertiaTreeModel.cpp @@ -347,7 +347,8 @@ namespace grbda // Forward Pass - Articulated body bias force for (auto &link_node : reflected_inertia_nodes_) { - link_node->pA_ = spatial::generalForceCrossProduct(link_node->v_, DVec(link_node->I_ * link_node->v_)); + link_node->pA_ = spatial::generalForceCrossProduct(link_node->v_, + DVec(link_node->I_[0] * link_node->v_)); } // Account for external forces in bias force @@ -421,7 +422,7 @@ namespace grbda // Forward pass for (auto &link_node : reflected_inertia_nodes_) { - link_node->IA_ = link_node->I_; + link_node->IA_ = link_node->I_[0]; } // Backward pass (Gauss principal of least constraint) @@ -499,7 +500,9 @@ namespace grbda const auto joint = node->joint_; DVec tmp = joint->S().transpose() * f; - lambda_inv += tmp.dot(node->D_inv_ * tmp); + // Use transpose()*vec instead of dot() to avoid complex conjugation + // Eigen's dot(a,b) computes conj(a)^T * b, but a^T * b needed for complex-step + lambda_inv += (tmp.transpose() * DVec(node->D_inv_ * tmp))(0); dstate_out += node->qdd_for_subtree_due_to_subtree_root_joint_qdd * node->D_inv_ * tmp; @@ -522,7 +525,8 @@ namespace grbda const DMat H_inv = matrixInverse(H); const DMat inv_ops_inertia = J * H_inv * J.transpose(); dstate_out = H_inv * (J.transpose() * force); - return force.dot(inv_ops_inertia * force); + // Use transpose()*vec instead of dot() to avoid complex conjugation + return (force.transpose() * (inv_ops_inertia * force))(0); } template @@ -741,6 +745,7 @@ namespace grbda } template class ReflectedInertiaTreeModel; + template class ReflectedInertiaTreeModel>; template class ReflectedInertiaTreeModel; } // namespace grbda diff --git a/src/Dynamics/RigidBodyTreeDynamics.cpp b/src/Dynamics/RigidBodyTreeDynamics.cpp index 3a7ecfb2..2b3aca7c 100644 --- a/src/Dynamics/RigidBodyTreeDynamics.cpp +++ b/src/Dynamics/RigidBodyTreeDynamics.cpp @@ -180,10 +180,12 @@ namespace grbda const DMat H_inv = matrixInverse(H); const DMat inv_ops_inertia = J * H_inv * J.transpose(); dstate_out = H_inv * (J.transpose() * force); - return force.dot(inv_ops_inertia * force); + // CRITICAL FIX: Use transpose()*vec instead of dot() to avoid complex conjugation + return (force.transpose() * (inv_ops_inertia * force))(0); } template class RigidBodyTreeModel; + template class RigidBodyTreeModel>; template class RigidBodyTreeModel; } // namespace grbda diff --git a/src/Dynamics/RigidBodyTreeModel.cpp b/src/Dynamics/RigidBodyTreeModel.cpp index 398393f3..a2c26e3d 100644 --- a/src/Dynamics/RigidBodyTreeModel.cpp +++ b/src/Dynamics/RigidBodyTreeModel.cpp @@ -236,6 +236,7 @@ namespace grbda } template class RigidBodyTreeModel; + template class RigidBodyTreeModel>; template class RigidBodyTreeModel; } // namespace grbda diff --git a/src/Dynamics/TreeModel.cpp b/src/Dynamics/TreeModel.cpp index f52081c7..679a1fca 100644 --- a/src/Dynamics/TreeModel.cpp +++ b/src/Dynamics/TreeModel.cpp @@ -25,7 +25,7 @@ namespace grbda node->Xa_ = node->Xup_.toAbsolute(); } - node->avp_ = spatial::generalMotionCrossProduct(node->v_, node->vJ()); + spatial::generalMotionCrossProduct(node->v_, node->vJ(), node->avp_); } kinematics_updated_ = true; @@ -120,9 +120,9 @@ namespace grbda forwardKinematics(); - // Forward Pass + // Forward Pass: Initialize composite inertias to local inertias for (auto &node : nodes_) - node->Ic_ = node->I_; + node->Ic_ = node->I_; // element-wise copy of vector // Backward Pass for (int i = (int)nodes_.size() - 1; i >= 0; i--) @@ -131,27 +131,38 @@ namespace grbda const int vel_idx_i = node_i->velocity_index_; const int num_vel_i = node_i->num_velocities_; + // Accumulate composite inertia to parent using block-diagonal structure + // For cluster B connected to cluster A at body k, we add: + // I_A[k,k] += sum over all bodies j in B of: X_j^{-T} * I_B[j,j] * X_j^{-1} + // where X_j is the transform from body j in B to body k in A if (node_i->parent_index_ >= 0) { - auto parent_node = nodes_[node_i->parent_index_]; - parent_node->Ic_ += node_i->Xup_.inverseTransformSpatialInertia(node_i->Ic_); + auto & parent_node = nodes_[node_i->parent_index_]; + node_i->Xup_.accumulateBlockDiagonalInertia(node_i->Ic_, parent_node->Ic_); } - DMat F = node_i->Ic_ * node_i->S(); - H_.block(vel_idx_i, vel_idx_i, num_vel_i, num_vel_i) = node_i->S().transpose() * F; + // Diagonal block: H_ii = S_i^T * Ic_i * S_i + // Compute F = Ic * S exploiting block-diagonal structure of Ic + DMat F = node_i->Xup_.blockDiagonalInertiaTimesMotionSubspace( + node_i->Ic_, node_i->S()); + H_.block(vel_idx_i, vel_idx_i, num_vel_i, num_vel_i).noalias() = node_i->S().transpose() * F; + // Off-diagonal blocks: H_ij = S_j^T * X_ij^{-T} * Ic_i * S_i + // F is transformed through the chain from node i to ancestor j int j = i; while (nodes_[j]->parent_index_ > -1) { - F = nodes_[j]->Xup_.inverseTransformForceSubspace(F); + // Transform F from current frame to parent frame using block-wise transform + F = nodes_[j]->Xup_.transformForceSubspaceToParent(F); j = nodes_[j]->parent_index_; const int vel_idx_j = nodes_[j]->velocity_index_; const int num_vel_j = nodes_[j]->num_velocities_; - H_.block(vel_idx_i, vel_idx_j, num_vel_i, num_vel_j) = + // H_ij = F^T * S_j + H_.block(vel_idx_i, vel_idx_j, num_vel_i, num_vel_j).noalias() = F.transpose() * nodes_[j]->S(); - H_.block(vel_idx_j, vel_idx_i, num_vel_j, num_vel_i) = + H_.block(vel_idx_j, vel_idx_i, num_vel_j, num_vel_i).noalias() = H_.block(vel_idx_i, vel_idx_j, num_vel_i, num_vel_j).transpose(); } } @@ -159,6 +170,102 @@ namespace grbda mass_matrix_updated_ = true; } + template + void TreeModel::compositeRigidBodyAlgorithmWorldFrame() + { + if (mass_matrix_updated_) + return; + + forwardKinematics(); + + const int n = (int)nodes_.size(); + H_.setZero(); + + // Forward Pass: Transform inertias and motion subspaces to world frame + // Following Hworld_v2.m: transform block-by-block to frame {0} + for (int i = 0; i < n; i++) + { + auto &node = nodes_[i]; + const int num_bodies = node->Xa_.getNumOutputBodies(); + + // Initialize composite inertia (one 6x6 block per body) with each inertia in world frame + node->Ic0_.resize(num_bodies); + node->S0_.resize(num_bodies); + + // Transform down to frame {0}, block by block + for (int body = 0; body < num_bodies; body++) + { + const auto &Xa_body = node->Xa_.getTransformForOutputBody(body); + node->Ic0_[body] = Xa_body.inverseTransformSpatialInertia(node->I_[body]); + + const auto S_body_block = node->S().template middleRows<6>(6 * body); + node->S0_[body] = Xa_body.inverseTransformMotionSubspace(S_body_block); + } + } + + // F is 6 x NV, summing over all blocks (the ancestors see the sum of forces) + F_.setZero(6, this->velocity_index_); + + // Backward Pass: Accumulate composite inertias and compute H + // Following Hworld_v2.m structure + for (int i = n - 1; i >= 0; i--) + { + auto &node_i = nodes_[i]; + const int & vel_idx_i = node_i->velocity_index_; + const int & num_vel_i = node_i->num_velocities_; + const int & num_bodies = node_i->Xa_.getNumOutputBodies(); + + // Compute Ftmp = IC0{i} * S0{i} (block-diagonal multiplication) + node_i->Ftmp_.resize(num_bodies); + H_.block(vel_idx_i, vel_idx_i, num_vel_i, num_vel_i).setZero(); + for (int body = 0; body < num_bodies; body++) + { + node_i->Ftmp_[body].noalias() = + node_i->Ic0_[body] * node_i->S0_[body]; + + H_.block(vel_idx_i, vel_idx_i, num_vel_i, num_vel_i).noalias() += + node_i->S0_[body].transpose() * node_i->Ftmp_[body]; + + F_.middleCols(vel_idx_i, num_vel_i).noalias() += + node_i->Ftmp_[body]; + } + + if (node_i->parent_index_ >= 0) + { + const int & parent = node_i->parent_index_; + const int & vel_idx_parent = nodes_[parent]->velocity_index_; + const int & num_vel_parent = nodes_[parent]->num_velocities_; + + // Get subtree velocity indices (all velocities from node i and its descendants) + const int & subtree_start = vel_idx_i; + const int & subtree_size = node_i->subtree_num_velocities_; + + // parent_body_subindex: which body in the parent cluster does this cluster attach to + const int & parent_subindex = node_i->Xup_.transform_and_parent_subindex(0).second; + + // Sblock = S0{p(i)}(inds, :) - the parent's motion subspace for the connecting body + const auto &Sblock = nodes_[parent]->S0_[parent_subindex]; + + // H(pp, vi) = Sblock'*F(:, vi) + H_.block(vel_idx_parent, subtree_start, num_vel_parent, subtree_size).noalias() = + Sblock.transpose() * F_.middleCols(subtree_start, subtree_size); + // Symmetry: H(vi, pp) = H(pp, vi)' + H_.block(subtree_start, vel_idx_parent, subtree_size, num_vel_parent).noalias() = + H_.block(vel_idx_parent, subtree_start, num_vel_parent, subtree_size).transpose(); + + // Accumulate composite inertia to parent: IC0{p(i)}(inds, inds) += blockDiagSum(IC0{i}) + // blockDiagSum sums all 6x6 diagonal blocks into one 6x6 matrix + auto &parent_IC0_block = nodes_[parent]->Ic0_[parent_subindex]; + for (int body = 0; body < num_bodies; body++) + { + parent_IC0_block.noalias() += node_i->Ic0_[body]; + } + } + } + + mass_matrix_updated_ = true; + } + template void TreeModel::updateBiasForceVector() { @@ -180,9 +287,9 @@ namespace grbda // Forward Pass for (auto &node : nodes_) { - node->f_ = node->I_ * node->a_ + - spatial::generalForceCrossProduct(node->v_, - DVec(node->I_ * node->v_)); + node->f_ = spatial::blockDiagonalTimesVector(node->I_, node->a_); + spatial::addGeneralForceCrossProduct(node->v_, + spatial::blockDiagonalTimesVector(node->I_, node->v_), node->f_); } // Account for external forces in bias force @@ -263,6 +370,7 @@ namespace grbda } template class TreeModel; + template class TreeModel>; template class TreeModel; template class TreeModel; diff --git a/src/Robots/Cassie.cpp b/src/Robots/Cassie.cpp new file mode 100644 index 00000000..8d774539 --- /dev/null +++ b/src/Robots/Cassie.cpp @@ -0,0 +1,235 @@ +#include "grbda/Robots/Cassie.hpp" + +namespace grbda +{ + template + ClusterTreeModel Cassie::buildClusterTreeModel() const + { + using namespace ClusterJoints; + using RevJoint = Joints::Revolute; + using CoordAxis = ori::CoordinateAxis; + + ClusterTreeModel model{}; + model.setGravity(Vec3{0., 0., grav}); + + const std::string pelvis_name = base; + const std::string ground_name = "ground"; + const SpatialInertia pelvis_spatial_inertia = + SpatialInertia{pelvis_mass, pelvis_CoM, pelvis_inertia}; + + model.template appendBody>( + pelvis_name, pelvis_spatial_inertia, + ground_name, spatial::Transform{}, + "pelvis-to-ground"); + + const std::vector sides = {"left", "right"}; + + for (size_t i = 0; i < sides.size(); ++i) { + const std::string& side = sides[i]; + const Scalar side_sign = (i == 0) ? Scalar(1.) : Scalar(-1.); + + // --------------------------------------------------------------- + // Hip serial chain (3 revolute clusters) + // --------------------------------------------------------------- + + const std::string hip_roll_name = side + "-hip-roll"; + Vec3 hip_roll_pos = hip_roll_joint_pos; + hip_roll_pos(1) *= side_sign; + const spatial::Transform hip_roll_Xtree(Mat3::Identity(), hip_roll_pos); + const SpatialInertia hip_roll_si( + hip_roll_mass, hip_roll_CoM, + (i == 0) ? hip_roll_inertia_left : hip_roll_inertia_right); + model.template appendBody>( + hip_roll_name, hip_roll_si, + pelvis_name, hip_roll_Xtree, + CoordAxis::X, side + "-hip-roll-joint"); + + const std::string hip_yaw_name = side + "-hip-yaw"; + Vec3 hip_yaw_pos = hip_yaw_joint_offset; + hip_yaw_pos(1) *= side_sign; + const spatial::Transform hip_yaw_Xtree(Mat3::Identity(), hip_yaw_pos); + const SpatialInertia hip_yaw_si( + hip_yaw_mass, + (i == 0) ? hip_yaw_CoM_left : hip_yaw_CoM_right, + (i == 0) ? hip_yaw_inertia_left : hip_yaw_inertia_right); + model.template appendBody>( + hip_yaw_name, hip_yaw_si, + hip_roll_name, hip_yaw_Xtree, + CoordAxis::Z, side + "-hip-yaw-joint"); + + const std::string hip_pitch_name = side + "-hip-pitch"; + Vec3 hip_pitch_pos = hip_pitch_joint_offset; + hip_pitch_pos(1) *= side_sign; + const spatial::Transform hip_pitch_Xtree(Mat3::Identity(), hip_pitch_pos); + const SpatialInertia hip_pitch_si( + hip_pitch_mass, + (i == 0) ? hip_pitch_CoM_left : hip_pitch_CoM_right, + (i == 0) ? hip_pitch_inertia_left : hip_pitch_inertia_right); + model.template appendBody>( + hip_pitch_name, hip_pitch_si, + hip_yaw_name, hip_pitch_Xtree, + CoordAxis::Y, side + "-hip-pitch-joint"); + + // --------------------------------------------------------------- + // Upper four-bar cluster (parent = hip-pitch) + // + // Bodies: + // [0] knee+shin — child of hip-pitch (path1 link 1, dep) + // [1] achilles-rod — child of hip-pitch (path2, ind) + // [2] tarsus — child of knee+shin (path1 link 2, dep) + // + // independent_coordinate = 1 (achilles, passive — knee/tarsus are dependent) + // + // The FourBar phi is 2D (planar). The MJCF pivots have small z-offsets that + // make the mechanism non-planar in 3D. Zeroing those z-offsets collapses all + // pivots into the hip-pitch XY plane so that phi = 0 is exactly satisfiable. + // + // phi geometry (all in hip-pitch XY plane): + // knee pivot: (0.12, 0) + // achilles pivot: (0, 0) + // L1 = 0.077005 m (knee pivot -> shin joint, |shin_pos_in_knee_frame|) + // L2 = 0.422204 m (shin joint -> heel-spring closure, spanning tarsus body) + // L3 = 0.5012 m (achilles pivot -> closure point) + // offset = achilles_pivot - knee_pivot = (-0.12, 0) + // Note: L1/L2 are phi link lengths, independent of the Xtree translations. + // --------------------------------------------------------------- + + const std::string achilles_name = side + "-achilles-rod"; + const std::string knee_shin_name = side + "-knee-shin"; + const std::string tarsus_name = side + "-tarsus"; + + // z-offsets zeroed to enforce planarity required by the 2D FourBar phi. + const Vec3 achilles_pos{Scalar(0.), Scalar(0.), Scalar(0.)}; + const spatial::Transform achilles_Xtree(Mat3::Identity(), achilles_pos); + + const Vec3 knee_shin_pos{Scalar(0.12), Scalar(0.), Scalar(0.)}; + const spatial::Transform knee_shin_Xtree(Mat3::Identity(), knee_shin_pos); + + // Tarsus Xtree: tarsus joint position in knee+shin body frame. + // = shin_pos_in_knee (0.06068, 0.04741) + tarsus_pos_in_shin (0.43476, 0.02) + const Vec3 tarsus_pos{Scalar(0.49544), Scalar(0.06741), Scalar(0.)}; + const spatial::Transform tarsus_Xtree(Mat3::Identity(), tarsus_pos); + + const SpatialInertia achilles_si( + achilles_mass, achilles_CoM, achilles_inertia); + const SpatialInertia knee_shin_si( + knee_shin_mass, + (i == 0) ? knee_shin_CoM_left : knee_shin_CoM_right, + (i == 0) ? knee_shin_inertia_left : knee_shin_inertia_right); + const SpatialInertia tarsus_si( + tarsus_mass, + (i == 0) ? tarsus_CoM_left : tarsus_CoM_right, + (i == 0) ? tarsus_inertia_left : tarsus_inertia_right); + + auto knee_shin_body = model.registerBody(knee_shin_name, knee_shin_si, + hip_pitch_name, knee_shin_Xtree); + auto achilles_body = model.registerBody(achilles_name, achilles_si, + hip_pitch_name, achilles_Xtree); + auto tarsus_body = model.registerBody(tarsus_name, tarsus_si, + knee_shin_name, tarsus_Xtree); + + std::vector> upper_bodies = {knee_shin_body, achilles_body, tarsus_body}; + std::vector> upper_joints = { + std::make_shared(CoordAxis::Z, side + "-knee-joint"), + std::make_shared(CoordAxis::Z, side + "-achilles-joint"), + std::make_shared(CoordAxis::Z, side + "-shin-joint")}; + + // path1 = {knee+shin (q[0], dep), tarsus (q[2], dep)}, path2 = {achilles (q[1], ind)} + // L1 = 0.077005 m (knee pivot to shin joint, = |shin_pos_in_knee_frame|) + // L2 = 0.422204 m (shin joint to heel-spring closure, spanning the tarsus body) + // L3 = 0.5012 m (achilles pivot to closure point) + // offset = achilles_pivot - knee_pivot = (0,0) - (0.12,0) = (-0.12, 0) + // Note: L1/L2 are phi link lengths independent of the Xtree translations. + std::vector upper_path1 = {Scalar(0.077005), Scalar(0.422204)}; + std::vector upper_path2 = {Scalar(0.5012)}; + Vec2 upper_offset{Scalar(-0.12), Scalar(0.0)}; + + auto upper_constraint = std::make_shared>( + upper_path1, upper_path2, upper_offset, 1); + + model.template appendRegisteredBodiesAsCluster>( + side + "-upper-leg-loop", upper_bodies, upper_joints, upper_constraint); + + // --------------------------------------------------------------- + // Lower four-bar cluster (parent = tarsus+heel-spring body) + // + // Bodies: + // [0] foot-crank — child of tarsus (path1 link 1, dep) + // [1] foot — child of tarsus (path2, zero-length rocker) + // [2] plantar-rod — child of foot-crank (path1 link 2, dep) + // + // independent_coordinate = 1 (foot joint, actuated) + // + // z-offsets zeroed to enforce planarity required by the 2D FourBar phi. + // offset = foot_pivot - foot_crank_pivot = (0.408,-0.04) - (0.058,-0.034) = (0.35,-0.006) + // --------------------------------------------------------------- + + const std::string foot_crank_name = side + "-foot-crank"; + const std::string plantar_rod_name = side + "-plantar-rod"; + const std::string foot_name = side + "-foot"; + + // z-offset zeroed (was ±0.02275) to enforce planarity. + const Vec3 foot_crank_pos{Scalar(0.058), Scalar(-0.034), Scalar(0.)}; + const spatial::Transform foot_crank_Xtree(Mat3::Identity(), + foot_crank_pos); + + // Foot pivot: z already zero in MJCF. + const Vec3 foot_pos{Scalar(0.408), Scalar(-0.04), Scalar(0.)}; + const spatial::Transform foot_Xtree(Mat3::Identity(), foot_pos); + + // z-offset zeroed (was ∓0.00791) to enforce planarity. + const Vec3 plantar_pos{Scalar(0.055), Scalar(0.), Scalar(0.)}; + const spatial::Transform plantar_Xtree(Mat3::Identity(), plantar_pos); + + const SpatialInertia foot_crank_si( + foot_crank_mass, + (i == 0) ? foot_crank_CoM_left : foot_crank_CoM_right, + (i == 0) ? foot_crank_inertia_left : foot_crank_inertia_right); + const SpatialInertia foot_si( + foot_mass, + (i == 0) ? foot_CoM_left : foot_CoM_right, + (i == 0) ? foot_inertia_left : foot_inertia_right); + const SpatialInertia plantar_rod_si( + plantar_rod_mass, plantar_rod_CoM, plantar_rod_inertia); + + auto foot_crank_body = model.registerBody(foot_crank_name, foot_crank_si, + tarsus_name, foot_crank_Xtree); + auto foot_body = model.registerBody(foot_name, foot_si, + tarsus_name, foot_Xtree); + auto plantar_rod_body = model.registerBody(plantar_rod_name, plantar_rod_si, + foot_crank_name, plantar_Xtree); + + std::vector> lower_bodies = {foot_crank_body, foot_body, plantar_rod_body}; + std::vector> lower_joints = { + std::make_shared(CoordAxis::Z, side + "-foot-crank-joint"), + std::make_shared(CoordAxis::Z, side + "-foot-joint"), + std::make_shared(CoordAxis::Z, side + "-plantar-joint")}; + + // path1 = {foot-crank (q[0]), plantar-rod (q[2])}, path2 = {foot (q[1], L=0)} + // independent = 1 (foot joint, actuated) + std::vector lower_path1 = {Scalar(0.055), Scalar(0.35012)}; + std::vector lower_path2 = {Scalar(0.)}; + Vec2 lower_offset{Scalar(0.35), Scalar(-0.006)}; + + auto lower_constraint = std::make_shared>( + lower_path1, lower_path2, lower_offset, 1); + + model.template appendRegisteredBodiesAsCluster>( + side + "-lower-leg-loop", lower_bodies, lower_joints, lower_constraint); + + model.appendContactPoint(foot_name, + Vec3{Scalar(0.069746), Scalar(-0.010224), Scalar(0.)}, + side + "-toe-contact"); + model.appendContactPoint(foot_name, + Vec3{Scalar(-0.052821), Scalar(0.092622), Scalar(0.)}, + side + "-heel-contact"); + } + + return model; + } + + template class Cassie; + template class Cassie>; + template class Cassie; + +} // namespace grbda diff --git a/src/Robots/MIT_Humanoid.cpp b/src/Robots/MIT_Humanoid.cpp index 6c30cfb5..2020574a 100644 --- a/src/Robots/MIT_Humanoid.cpp +++ b/src/Robots/MIT_Humanoid.cpp @@ -169,12 +169,14 @@ namespace grbda const spatial::Transform xtreeAnkleRotor(I3, ankleRotorLocation); // Cluster - Body ankle_rotor = model.registerBody(ankle_rotor_name, ankle_rotor_inertia, - knee_parent_name, xtreeAnkleRotor); Body knee_link = model.registerBody(knee_link_name, knee_link_inertia, knee_parent_name, xtreeKnee); Body knee_rotor = model.registerBody(knee_rotor_name, knee_rotor_inertia, knee_parent_name, xtreeKneeRotor); + + Body ankle_rotor = model.registerBody(ankle_rotor_name, ankle_rotor_inertia, + knee_parent_name, xtreeAnkleRotor); + Body ankle_link = model.registerBody(ankle_link_name, ankle_link_inertia, ankle_parent_name, xtreeAnkle); @@ -359,6 +361,8 @@ namespace grbda template class MIT_Humanoid; template class MIT_Humanoid; + template class MIT_Humanoid, ori_representation::RollPitchYaw>; + template class MIT_Humanoid, ori_representation::Quaternion>; template class MIT_Humanoid; template class MIT_Humanoid; diff --git a/src/Robots/MIT_Humanoid_Leg.cpp b/src/Robots/MIT_Humanoid_Leg.cpp index eda0df1d..8cabad01 100644 --- a/src/Robots/MIT_Humanoid_Leg.cpp +++ b/src/Robots/MIT_Humanoid_Leg.cpp @@ -165,6 +165,7 @@ namespace grbda } template class MIT_Humanoid_Leg; + template class MIT_Humanoid_Leg>; template class MIT_Humanoid_Leg; } // namespace grbda diff --git a/src/Robots/MiniCheetah.cpp b/src/Robots/MiniCheetah.cpp index e9764404..ef2298f2 100644 --- a/src/Robots/MiniCheetah.cpp +++ b/src/Robots/MiniCheetah.cpp @@ -140,6 +140,8 @@ namespace grbda template class MiniCheetah; template class MiniCheetah; + template class MiniCheetah, ori_representation::RollPitchYaw>; + template class MiniCheetah, ori_representation::Quaternion>; template class MiniCheetah; template class MiniCheetah; } diff --git a/src/Robots/PlanarLegLinkage.cpp b/src/Robots/PlanarLegLinkage.cpp index ed000f43..cf2273c6 100644 --- a/src/Robots/PlanarLegLinkage.cpp +++ b/src/Robots/PlanarLegLinkage.cpp @@ -95,5 +95,6 @@ namespace grbda } template class PlanarLegLinkage; + template class PlanarLegLinkage>; template class PlanarLegLinkage; } diff --git a/src/Robots/SerialChains/RevoluteChainWithAndWithoutRotor.cpp b/src/Robots/SerialChains/RevoluteChainWithAndWithoutRotor.cpp index a4f39b31..2b5992b4 100644 --- a/src/Robots/SerialChains/RevoluteChainWithAndWithoutRotor.cpp +++ b/src/Robots/SerialChains/RevoluteChainWithAndWithoutRotor.cpp @@ -59,17 +59,102 @@ namespace grbda ClusterTreeModel RevoluteChainWithAndWithoutRotor::buildUniformClusterTreeModel() const { - throw std::runtime_error("Not implemented"); + using namespace ClusterJoints; + + ClusterTreeModel model{}; + + Mat3 I3 = Mat3::Identity(); + Vec3 z3 = Vec3::Zero(); + + const Scalar grav = 9.81; + model.setGravity(Vec3{grav, 0., 0.}); + + // Inertia params + const Scalar I = 1.; + const Scalar Irot = 1e-4; + const Scalar m = 1.; + const Scalar l = 1.; + const Scalar c = 0.5; + const Scalar gr = 2.; + + // Uniform quantities + ori::CoordinateAxis axis = ori::CoordinateAxis::Z; + + const spatial::Transform Xtree_link = spatial::Transform(I3, Vec3(l, 0, 0.)); + + Mat3 link_inertia; + link_inertia << 0., 0., 0., 0., 0., 0., 0., 0., I; + const SpatialInertia link_spatial_inertia(m, Vec3(c, 0., 0.), + link_inertia); + + Mat3 rotor_inertia; + rotor_inertia << 0., 0., 0., 0., 0., 0., 0., 0., Irot; + const SpatialInertia rotor_spatial_inertia(0., Vec3::Zero(), + rotor_inertia); + + std::string prev_link_name = "ground"; + + // First N links with rotors (cluster joints) + for (size_t i(0); i < N; i++) + { + const spatial::Transform Xtree1 = i == 0 ? spatial::Transform(I3, z3) + : Xtree_link; + + // Link + const std::string link_name = "link-" + std::to_string(i); + auto link = model.registerBody(link_name, link_spatial_inertia, + prev_link_name, Xtree1); + + // Rotor + const std::string rotor_name = "rotor-" + std::to_string(i); + auto rotor = model.registerBody(rotor_name, rotor_spatial_inertia, + prev_link_name, Xtree1); + + // Cluster + const std::string cluster_name = "cluster-" + std::to_string(i); + const std::string link_joint_name = "link-joint-" + std::to_string(i); + const std::string rotor_joint_name = "rotor-joint-" + std::to_string(i); + TransmissionModule module{link, rotor, link_joint_name, rotor_joint_name, + axis, axis, gr}; + model.template appendRegisteredBodiesAsCluster(cluster_name, module); + + prev_link_name = link_name; + } + + // Next M links without rotors (simple Revolute joints) + for (size_t i(N); i < N + M; i++) + { + const spatial::Transform Xtree1 = i == N && N == 0 ? + spatial::Transform(I3, z3) : Xtree_link; + + const std::string link_name = "link-" + std::to_string(i); + model.template appendBody(link_name, link_spatial_inertia, prev_link_name, + Xtree1, axis); + + prev_link_name = link_name; + } + + return model; } + template class RevoluteChainWithAndWithoutRotor<0ul, 2ul>; + template class RevoluteChainWithAndWithoutRotor<0ul, 3ul>; + template class RevoluteChainWithAndWithoutRotor<0ul, 4ul>; template class RevoluteChainWithAndWithoutRotor<0ul, 8ul>; template class RevoluteChainWithAndWithoutRotor<1ul, 7ul>; + template class RevoluteChainWithAndWithoutRotor<2ul, 0ul>; template class RevoluteChainWithAndWithoutRotor<2ul, 6ul>; + template class RevoluteChainWithAndWithoutRotor<3ul, 0ul>; template class RevoluteChainWithAndWithoutRotor<3ul, 5ul>; + template class RevoluteChainWithAndWithoutRotor<4ul, 0ul>; template class RevoluteChainWithAndWithoutRotor<4ul, 4ul>; template class RevoluteChainWithAndWithoutRotor<5ul, 3ul>; template class RevoluteChainWithAndWithoutRotor<6ul, 2ul>; template class RevoluteChainWithAndWithoutRotor<7ul, 1ul>; template class RevoluteChainWithAndWithoutRotor<8ul, 0ul>; + template class RevoluteChainWithAndWithoutRotor<0ul, 2ul, std::complex>; + template class RevoluteChainWithAndWithoutRotor<0ul, 3ul, std::complex>; + template class RevoluteChainWithAndWithoutRotor<0ul, 4ul, std::complex>; + } // namespace grbda diff --git a/src/Robots/SerialChains/RevoluteChainWithRotor.cpp b/src/Robots/SerialChains/RevoluteChainWithRotor.cpp index 9f041c4c..4a4c96da 100644 --- a/src/Robots/SerialChains/RevoluteChainWithRotor.cpp +++ b/src/Robots/SerialChains/RevoluteChainWithRotor.cpp @@ -261,16 +261,33 @@ namespace grbda template class RevoluteChainWithRotor<2ul>; template class RevoluteChainWithRotor<3ul>; template class RevoluteChainWithRotor<4ul>; + template class RevoluteChainWithRotor<5ul>; template class RevoluteChainWithRotor<6ul>; + template class RevoluteChainWithRotor<7ul>; template class RevoluteChainWithRotor<8ul>; + template class RevoluteChainWithRotor<9ul>; template class RevoluteChainWithRotor<10ul>; template class RevoluteChainWithRotor<12ul>; + template class RevoluteChainWithRotor<14ul>; template class RevoluteChainWithRotor<16ul>; + template class RevoluteChainWithRotor<18ul>; template class RevoluteChainWithRotor<20ul>; template class RevoluteChainWithRotor<24ul>; + template class RevoluteChainWithRotor<25ul>; + template class RevoluteChainWithRotor<30ul>; + template class RevoluteChainWithRotor<35ul>; + template class RevoluteChainWithRotor<40ul>; + template class RevoluteChainWithRotor<50ul>; + template class RevoluteChainWithRotor<60ul>; + template class RevoluteChainWithRotor<70ul>; + template class RevoluteChainWithRotor<80ul>; + template class RevoluteChainWithRotor<90ul>; + template class RevoluteChainWithRotor<100ul>; template class RevoluteChainWithRotor<2ul, casadi::SX>; template class RevoluteChainWithRotor<4ul, casadi::SX>; template class RevoluteChainWithRotor<8ul, casadi::SX>; + template class RevoluteChainWithRotor<30ul, std::complex>; + } // namespace grbda diff --git a/src/Robots/SerialChains/RevolutePairChain.cpp b/src/Robots/SerialChains/RevolutePairChain.cpp index d41dbcba..83ce9eea 100644 --- a/src/Robots/SerialChains/RevolutePairChain.cpp +++ b/src/Robots/SerialChains/RevolutePairChain.cpp @@ -110,4 +110,7 @@ namespace grbda template class RevolutePairChain<2ul>; template class RevolutePairChain<4ul>; + template class RevolutePairChain<2ul, std::complex>; + template class RevolutePairChain<4ul, std::complex>; + } // namespace grbda diff --git a/src/Robots/SerialChains/RevolutePairChainWithRotor.cpp b/src/Robots/SerialChains/RevolutePairChainWithRotor.cpp index 57712df8..4cee5057 100644 --- a/src/Robots/SerialChains/RevolutePairChainWithRotor.cpp +++ b/src/Robots/SerialChains/RevolutePairChainWithRotor.cpp @@ -303,6 +303,13 @@ namespace grbda template class RevolutePairChainWithRotor<24ul>; template class RevolutePairChainWithRotor<28ul>; template class RevolutePairChainWithRotor<32ul>; + template class RevolutePairChainWithRotor<40ul>; + template class RevolutePairChainWithRotor<50ul>; + template class RevolutePairChainWithRotor<60ul>; + template class RevolutePairChainWithRotor<70ul>; + template class RevolutePairChainWithRotor<80ul>; + template class RevolutePairChainWithRotor<90ul>; + template class RevolutePairChainWithRotor<100ul>; template class RevolutePairChainWithRotor<2ul, casadi::SX>; template class RevolutePairChainWithRotor<6ul, casadi::SX>; diff --git a/src/Robots/SerialChains/RevoluteTripleChainWithRotor.cpp b/src/Robots/SerialChains/RevoluteTripleChainWithRotor.cpp index d4ef2046..b257d229 100644 --- a/src/Robots/SerialChains/RevoluteTripleChainWithRotor.cpp +++ b/src/Robots/SerialChains/RevoluteTripleChainWithRotor.cpp @@ -82,7 +82,90 @@ namespace grbda ClusterTreeModel RevoluteTripleChainWithRotor::buildUniformClusterTreeModel() const { - throw std::runtime_error("Not implemented"); + ClusterTreeModel model{}; + + Mat3 I3 = Mat3::Identity(); + Vec3 z3 = Vec3::Zero(); + + const Scalar grav = 9.81; + model.setGravity(Vec3{grav, 0., 0.}); + + // Inertia params + const Scalar I = 1.; + const Scalar Irot = 1e-4; + const Scalar m = 1.; + const Scalar l = 1.; + const Scalar c = 0.5; + const Scalar gr = 2.; + DVec br1(1), br2(2), br3(3); + br1 << 3.; + br2 << 3., 3.; + br3 << 3., 3., 3.; + + // Uniform quantities + ori::CoordinateAxis axis = ori::CoordinateAxis::Z; + + const spatial::Transform Xtree_link = spatial::Transform(I3, Vec3(l, 0, 0.)); + + Mat3 link_inertia; + link_inertia << 0., 0., 0., 0., 0., 0., 0., 0., I; + const SpatialInertia link_spatial_inertia(m, Vec3(c, 0., 0.), + link_inertia); + + Mat3 rotor_inertia; + rotor_inertia << 0., 0., 0., 0., 0., 0., 0., 0., Irot; + const SpatialInertia rotor_spatial_inertia(0., Vec3::Zero(), + rotor_inertia); + + std::string parent_name = "ground"; + for (size_t i(0); i < N / 3; i++) + { + const spatial::Transform Xtree1 = i == 0 ? spatial::Transform(I3, z3) + : Xtree_link; + + // Link A + const std::string linkA_name = "link-A-" + std::to_string(i); + auto linkA = model.registerBody(linkA_name, link_spatial_inertia, + parent_name, Xtree1); + + // Link B + const std::string linkB_name = "link-B-" + std::to_string(i); + auto linkB = model.registerBody(linkB_name, link_spatial_inertia, + linkA_name, Xtree_link); + + // Link C + const std::string linkC_name = "link-C-" + std::to_string(i); + auto linkC = model.registerBody(linkC_name, link_spatial_inertia, + linkB_name, Xtree_link); + + // Rotor A + const std::string rotorA_name = "rotor-A-" + std::to_string(i); + auto rotorA = model.registerBody(rotorA_name, rotor_spatial_inertia, + parent_name, Xtree1); + + // Rotor B + const std::string rotorB_name = "rotor-B-" + std::to_string(i); + auto rotorB = model.registerBody(rotorB_name, rotor_spatial_inertia, + parent_name, Xtree1); + + // Rotor C + const std::string rotorC_name = "rotor-C-" + std::to_string(i); + auto rotorC = model.registerBody(rotorC_name, rotor_spatial_inertia, + parent_name, Xtree1); + + // Cluster + ProxTransModule moduleA{linkA, rotorA, axis, axis, gr, br1}; + InterTransModule moduleB{linkB, rotorB, axis, axis, gr, br2}; + DistTransModule moduleC{linkC, rotorC, axis, axis, gr, br3}; + + const std::string cluster_name = "cluster-" + std::to_string(i); + model.template appendRegisteredBodiesAsCluster( + cluster_name, moduleA, moduleB, moduleC); + + parent_name = linkC_name; + } + + return model; } template @@ -111,5 +194,25 @@ namespace grbda template class RevoluteTripleChainWithRotor<9ul>; template class RevoluteTripleChainWithRotor<12ul>; template class RevoluteTripleChainWithRotor<15ul>; + template class RevoluteTripleChainWithRotor<18ul>; + template class RevoluteTripleChainWithRotor<21ul>; + template class RevoluteTripleChainWithRotor<24ul>; + template class RevoluteTripleChainWithRotor<27ul>; + template class RevoluteTripleChainWithRotor<30ul>; + template class RevoluteTripleChainWithRotor<36ul>; + template class RevoluteTripleChainWithRotor<42ul>; + template class RevoluteTripleChainWithRotor<48ul>; + template class RevoluteTripleChainWithRotor<54ul>; + template class RevoluteTripleChainWithRotor<60ul>; + template class RevoluteTripleChainWithRotor<66ul>; + template class RevoluteTripleChainWithRotor<72ul>; + template class RevoluteTripleChainWithRotor<78ul>; + template class RevoluteTripleChainWithRotor<84ul>; + template class RevoluteTripleChainWithRotor<90ul>; + template class RevoluteTripleChainWithRotor<96ul>; + template class RevoluteTripleChainWithRotor<99ul>; + + template class RevoluteTripleChainWithRotor<3ul, std::complex>; + template class RevoluteTripleChainWithRotor<6ul, std::complex>; } // namespace grbda diff --git a/src/Robots/TeleopArm.cpp b/src/Robots/TeleopArm.cpp index 966c90d0..ed29dd6d 100644 --- a/src/Robots/TeleopArm.cpp +++ b/src/Robots/TeleopArm.cpp @@ -3,121 +3,115 @@ namespace grbda { - ClusterTreeModel<> TeleopArm::buildClusterTreeModel() const + ClusterTreeModel TeleopArm::buildClusterTreeModel() const { using namespace ClusterJoints; - using SpatialTransform = spatial::Transform<>; + using SpatialTransform = spatial::Transform; - typedef typename ClusterJoints::ParallelBeltTransmissionModule<1> ProximalTransModule; - typedef typename ClusterJoints::ParallelBeltTransmissionModule<2> IntermedTransModule; - typedef typename ClusterJoints::ParallelBeltTransmissionModule<3> DistalTransModule; + typedef typename ClusterJoints::ParallelBeltTransmissionModule<1, double> ProximalTransModule; + typedef typename ClusterJoints::ParallelBeltTransmissionModule<2, double> IntermedTransModule; + typedef typename ClusterJoints::ParallelBeltTransmissionModule<3, double> DistalTransModule; - ClusterTreeModel<> model{}; + ClusterTreeModel model{}; Mat3 I3 = Mat3::Identity(); // Base SpatialTransform base_Xtree = SpatialTransform(I3, base_location_); - Body<> base = model.registerBody(base_name_, base_spatial_inertia_, - base_parent_name_, base_Xtree); + Body base = model.registerBody(base_name_, base_spatial_inertia_, + base_parent_name_, base_Xtree); SpatialTransform base_rotor_Xtree = SpatialTransform(I3, base_rotor_location_); - Body<> base_rotor = model.registerBody(base_rotor_name_, base_rotor_spatial_inertia_, - base_rotor_parent_name_, base_rotor_Xtree); + Body base_rotor = model.registerBody(base_rotor_name_, base_rotor_spatial_inertia_, + base_rotor_parent_name_, base_rotor_Xtree); - GearedTransmissionModule<> base_module{base, base_rotor, - "base_link_joint", "base_rotor_joint", - ori::CoordinateAxis::Z, - ori::CoordinateAxis::Z, base_rotor_gear_ratio_}; - model.appendRegisteredBodiesAsCluster>(base_cluster_name_, base_module); + GearedTransmissionModule base_module{base, base_rotor, + "base_link_joint", "base_rotor_joint", + ori::CoordinateAxis::Z, + ori::CoordinateAxis::Z, base_rotor_gear_ratio_}; + model.template appendRegisteredBodiesAsCluster>(base_cluster_name_, base_module); // Shoulder Rx SpatialTransform shoulder_rx_link_Xtree = SpatialTransform(I3, shoulder_rx_link_location_); - Body<> shoulder_rx_link = model.registerBody(shoulder_rx_link_name_, - shoulder_rx_link_spatial_inertia_, - shoulder_rx_link_parent_name_, - shoulder_rx_link_Xtree); + Body shoulder_rx_link = model.registerBody(shoulder_rx_link_name_, + shoulder_rx_link_spatial_inertia_, + shoulder_rx_link_parent_name_, + shoulder_rx_link_Xtree); SpatialTransform shoulder_rx_rotor_Xtree = SpatialTransform(I3, shoulder_rx_rotor_location_); - Body<> shoulder_rx_rotor = model.registerBody(shoulder_rx_rotor_name_, - shoulder_rx_rotor_spatial_inertia_, - shoulder_rx_rotor_parent_name_, - shoulder_rx_rotor_Xtree); - - GearedTransmissionModule<> shoulder_rx_module{shoulder_rx_link, shoulder_rx_rotor, - "shoulder_rx_link_joint", - "shoulder_rx_rotor_joint", - ori::CoordinateAxis::X, - ori::CoordinateAxis::X, - shoulder_rx_rotor_gear_ratio_}; - model.appendRegisteredBodiesAsCluster>(shoulder_rx_cluster_name_, - shoulder_rx_module); + Body shoulder_rx_rotor = model.registerBody(shoulder_rx_rotor_name_, + shoulder_rx_rotor_spatial_inertia_, + shoulder_rx_rotor_parent_name_, + shoulder_rx_rotor_Xtree); + + GearedTransmissionModule shoulder_rx_module{shoulder_rx_link, shoulder_rx_rotor, + "shoulder_rx_link_joint", + "shoulder_rx_rotor_joint", + ori::CoordinateAxis::X, + ori::CoordinateAxis::X, + shoulder_rx_rotor_gear_ratio_}; + model.template appendRegisteredBodiesAsCluster>( + shoulder_rx_cluster_name_, shoulder_rx_module); // Shoulder Ry SpatialTransform shoulder_ry_link_Xtree = SpatialTransform(I3, shoulder_ry_link_location_); - Body<> shoulder_ry_link = model.registerBody(shoulder_ry_link_name_, - shoulder_ry_link_spatial_inertia_, - shoulder_ry_link_parent_name_, - shoulder_ry_link_Xtree); + Body shoulder_ry_link = model.registerBody(shoulder_ry_link_name_, + shoulder_ry_link_spatial_inertia_, + shoulder_ry_link_parent_name_, + shoulder_ry_link_Xtree); SpatialTransform shoulder_ry_rotor_Xtree = SpatialTransform(I3, shoulder_ry_rotor_location_); - Body<> shoulder_ry_rotor = model.registerBody(shoulder_ry_rotor_name_, - shoulder_ry_rotor_spatial_inertia_, - shoulder_ry_rotor_parent_name_, - shoulder_ry_rotor_Xtree); - - GearedTransmissionModule<> shoulder_ry_module{shoulder_ry_link, shoulder_ry_rotor, - "shoulder_ry_link_joint", - "shoulder_ry_rotor_joint", - ori::CoordinateAxis::Y, - ori::CoordinateAxis::Y, - shoulder_ry_rotor_gear_ratio_}; - model.appendRegisteredBodiesAsCluster>(shoulder_ry_cluster_name_, - shoulder_ry_module); + Body shoulder_ry_rotor = model.registerBody(shoulder_ry_rotor_name_, + shoulder_ry_rotor_spatial_inertia_, + shoulder_ry_rotor_parent_name_, + shoulder_ry_rotor_Xtree); + + GearedTransmissionModule shoulder_ry_module{shoulder_ry_link, shoulder_ry_rotor, + "shoulder_ry_link_joint", + "shoulder_ry_rotor_joint", + ori::CoordinateAxis::Y, + ori::CoordinateAxis::Y, + shoulder_ry_rotor_gear_ratio_}; + model.template appendRegisteredBodiesAsCluster>( + shoulder_ry_cluster_name_, shoulder_ry_module); // Upper Link SpatialTransform upper_link_Xtree = SpatialTransform(I3, upper_link_location_); - Body<> upper_link = model.registerBody(upper_link_name_, upper_link_spatial_inertia_, - upper_link_parent_name_, upper_link_Xtree); - auto elbow_joint = std::make_shared>(ori::CoordinateAxis::Y); + Body upper_link = model.registerBody(upper_link_name_, upper_link_spatial_inertia_, + upper_link_parent_name_, upper_link_Xtree); // Wrist Pitch Link SpatialTransform wrist_pitch_link_Xtree = SpatialTransform(I3, wrist_pitch_link_location_); - auto wrist_pitch_link = model.registerBody(wrist_pitch_link_name_, - wrist_pitch_link_spatial_inertia_, - wrist_pitch_link_parent_name_, - wrist_pitch_link_Xtree); - auto wrist_pitch_joint = std::make_shared>(ori::CoordinateAxis::Y); + Body wrist_pitch_link = model.registerBody(wrist_pitch_link_name_, + wrist_pitch_link_spatial_inertia_, + wrist_pitch_link_parent_name_, + wrist_pitch_link_Xtree); // Wrist Roll Link SpatialTransform wrist_roll_link_Xtree = SpatialTransform(I3, wrist_roll_link_location_); - auto wrist_roll_link = model.registerBody(wrist_roll_link_name_, - wrist_roll_link_spatial_inertia_, - wrist_roll_link_parent_name_, - wrist_roll_link_Xtree); - auto wrist_roll_joint = std::make_shared>(ori::CoordinateAxis::Z); + Body wrist_roll_link = model.registerBody(wrist_roll_link_name_, + wrist_roll_link_spatial_inertia_, + wrist_roll_link_parent_name_, + wrist_roll_link_Xtree); // Elbow Rotor SpatialTransform elbow_rotor_Xtree = SpatialTransform(I3, elbow_rotor_location_); - Body<> elbow_rotor = model.registerBody(elbow_rotor_name_, elbow_rotor_spatial_inertia_, - elbow_rotor_parent_name_, elbow_rotor_Xtree); - auto elbow_rotor_joint = std::make_shared>(ori::CoordinateAxis::Y); + Body elbow_rotor = model.registerBody(elbow_rotor_name_, elbow_rotor_spatial_inertia_, + elbow_rotor_parent_name_, elbow_rotor_Xtree); // Wrist Pitch Rotor SpatialTransform wrist_pitch_rotor_Xtree = SpatialTransform(I3, wrist_pitch_rotor_location_); - auto wrist_pitch_rotor = model.registerBody(wrist_pitch_rotor_name_, - wrist_pitch_rotor_spatial_inertia_, - wrist_pitch_rotor_parent_name_, - wrist_pitch_rotor_Xtree); - auto wrist_pitch_rotor_joint = std::make_shared>(ori::CoordinateAxis::Y); + Body wrist_pitch_rotor = model.registerBody(wrist_pitch_rotor_name_, + wrist_pitch_rotor_spatial_inertia_, + wrist_pitch_rotor_parent_name_, + wrist_pitch_rotor_Xtree); // Wrist Roll Rotor SpatialTransform wrist_roll_rotor_Xtree = SpatialTransform(I3, wrist_roll_rotor_location_); - auto wrist_roll_rotor = model.registerBody(wrist_roll_rotor_name_, - wrist_roll_rotor_spatial_inertia_, - wrist_roll_rotor_parent_name_, - wrist_roll_rotor_Xtree); - auto wrist_roll_rotor_joint = std::make_shared>(ori::CoordinateAxis::Z); + Body wrist_roll_rotor = model.registerBody(wrist_roll_rotor_name_, + wrist_roll_rotor_spatial_inertia_, + wrist_roll_rotor_parent_name_, + wrist_roll_rotor_Xtree); // Upper Arm Cluster ProximalTransModule upper_arm_module{upper_link, elbow_rotor, @@ -135,29 +129,26 @@ namespace grbda ori::CoordinateAxis::Z, wrist_roll_rotor_gear_ratio_, wrist_roll_rotor_belt_ratios_}; - model.appendRegisteredBodiesAsCluster>(upper_arm_cluster_name_, - upper_arm_module, - wrist_pitch_module, - wrist_roll_module); + model.template appendRegisteredBodiesAsCluster>( + upper_arm_cluster_name_, upper_arm_module, wrist_pitch_module, wrist_roll_module); // Gripper SpatialTransform gripper_Xtree = SpatialTransform(I3, gripper_location_); - Body<> gripper = model.registerBody(gripper_name_, gripper_spatial_inertia_, - gripper_parent_name_, gripper_Xtree); + Body gripper = model.registerBody(gripper_name_, gripper_spatial_inertia_, + gripper_parent_name_, gripper_Xtree); SpatialTransform gripper_rotor_Xtree = SpatialTransform(I3, gripper_rotor_location_); - Body<> gripper_rotor = model.registerBody(gripper_rotor_name_, gripper_rotor_spatial_inertia_, - gripper_rotor_parent_name_, gripper_rotor_Xtree); - - GearedTransmissionModule<> gripper_module{gripper, gripper_rotor, - "gripper_joint", "gripper_rotor_joint", - ori::CoordinateAxis::X, - ori::CoordinateAxis::X, - gripper_rotor_gear_ratio_}; - model.appendRegisteredBodiesAsCluster>(gripper_cluster_name_, - gripper_module); - - // Add contact points + Body gripper_rotor = model.registerBody(gripper_rotor_name_, gripper_rotor_spatial_inertia_, + gripper_rotor_parent_name_, gripper_rotor_Xtree); + + GearedTransmissionModule gripper_module{gripper, gripper_rotor, + "gripper_joint", "gripper_rotor_joint", + ori::CoordinateAxis::X, + ori::CoordinateAxis::X, + gripper_rotor_gear_ratio_}; + model.template appendRegisteredBodiesAsCluster>(gripper_cluster_name_, + gripper_module); + model.appendContactPoint("upper-link", Vec3(0., 0., 0.), "elbow-contact"); model.appendContactPoint("wrist-pitch-link", Vec3(0., 0., 0.), "wrist-pitch-contact"); model.appendContactPoint("wrist-roll-link", Vec3(0., 0., 0.), "wrist-roll-contact"); @@ -167,10 +158,40 @@ namespace grbda TeleopArm::TeleopArm() { + base_location_ = Vec3(0, 0, 0.051); + base_rotor_location_ = Vec3(0, 0, 0); + shoulder_rx_link_location_ = Vec3(0, 0, 0.106); + shoulder_rx_rotor_location_ = Vec3(0, 0, 0); + shoulder_ry_link_location_ = Vec3(0, 0, 0.071); + shoulder_ry_rotor_location_ = Vec3(0, 0, 0); + upper_link_location_ = Vec3(0, -0.0095, 0.3855); + wrist_pitch_link_location_ = Vec3(0, 0, 0.362); + wrist_roll_link_location_ = Vec3(0, 0.004, 0.03574); + elbow_rotor_location_ = Vec3(0., 0., 0.); + wrist_pitch_rotor_location_ = Vec3(0., 0., 0.); + wrist_roll_rotor_location_ = Vec3(0., 0., 0.); + gripper_location_ = Vec3(0.0004, 0.0375, 0.070995); + gripper_rotor_location_ = Vec3(0, 0, 0); + + base_rotor_gear_ratio_ = 6.0; + shoulder_rx_rotor_gear_ratio_ = 6.0; + shoulder_ry_rotor_gear_ratio_ = 6.0; + elbow_rotor_gear_ratio_ = 6.0; + wrist_pitch_rotor_gear_ratio_ = 6.0; + wrist_roll_rotor_gear_ratio_ = 6.0; + gripper_rotor_gear_ratio_ = 2.0; + + elbow_rotor_belt_ratios_ = DVec(1); + elbow_rotor_belt_ratios_ << 1.0; + + wrist_pitch_rotor_belt_ratios_ = DVec(2); + wrist_pitch_rotor_belt_ratios_ << 1.0, 1.0; + + wrist_roll_rotor_belt_ratios_ = DVec(3); + wrist_roll_rotor_belt_ratios_ << -1.0, 1.0, -1.0; - // Rotor Inertias (legacy code from Robot-Software) Mat3 largeRotorRotationalInertiaZ; - largeRotorRotationalInertiaZ << 1.052, 0, 0, 0, 1.046, 0, 0, 0, 1.811; // This is wrong number (These are actuator's inertia) + largeRotorRotationalInertiaZ << 1.052, 0, 0, 0, 1.046, 0, 0, 0, 1.811; largeRotorRotationalInertiaZ = 1e-3 * largeRotorRotationalInertiaZ; Mat3 smallRotorRotationalInertiaZ; @@ -195,74 +216,61 @@ namespace grbda SpatialInertia largeRotorInertiaX(1.081, largeRotorCOM, largeRotorRotationalInertiaX); SpatialInertia largeRotorInertiaY(1.081, largeRotorCOM, largeRotorRotationalInertiaY); - // Base Mat3 base_rotational_inertia; base_rotational_inertia << 0.003411721, -2.78E-09, -1.45E-09, -2.78E-09, 0.003047159, -0.000609866, -1.45E-09, -0.000609866, 0.003412964; Vec3 base_com(-1.32E-07, -0.001991858, 0.042209332); base_spatial_inertia_ = SpatialInertia(0.996728196, base_com, base_rotational_inertia); - // Base Rotor - base_rotor_spatial_inertia_ = largeRotorInertiaZ; + base_rotor_spatial_inertia_ = SpatialInertia(largeRotorInertiaZ.getMass(), + largeRotorInertiaZ.getCOM(), largeRotorInertiaZ.getInertiaTensor()); - // Shoulder Rx Mat3 shoulder_rx_link_rotational_inertia; shoulder_rx_link_rotational_inertia << 0.001190777, 3.21E-12, 2.07E-11, 3.21E-12, 0.001028401, 0.00022441, 2.07E-11, 0.00022441, 0.001459421; Vec3 shoulder_rx_link_com(-4.96E-10, -0.004546817, 0.045690967); - shoulder_rx_link_spatial_inertia_ = SpatialInertia(0.4796, shoulder_rx_link_com, - shoulder_rx_link_rotational_inertia); + shoulder_rx_link_spatial_inertia_ = SpatialInertia(0.4796, shoulder_rx_link_com, shoulder_rx_link_rotational_inertia); - // Shoulder Rx Rotor - shoulder_rx_rotor_spatial_inertia_ = largeRotorInertiaX; + shoulder_rx_rotor_spatial_inertia_ = SpatialInertia(largeRotorInertiaX.getMass(), + largeRotorInertiaX.getCOM(), largeRotorInertiaX.getInertiaTensor()); - // Shoulder Ry Mat3 shoulder_ry_link_rotational_inertia; shoulder_ry_link_rotational_inertia << 0.016496934, 2.23E-07, 7.79E-05, 2.23E-07, 0.018757685, 0.000273553, 7.79E-05, 0.000273553, 0.003224214; Vec3 shoulder_ry_link_com(0.000482465, -0.000659857, 0.168192061); - shoulder_ry_link_spatial_inertia_ = SpatialInertia(1.670980082, shoulder_ry_link_com, - shoulder_ry_link_rotational_inertia); + shoulder_ry_link_spatial_inertia_ = SpatialInertia(1.670980082, shoulder_ry_link_com, shoulder_ry_link_rotational_inertia); - // Shoulder Ry Rotor - shoulder_ry_rotor_spatial_inertia_ = largeRotorInertiaY; + shoulder_ry_rotor_spatial_inertia_ = SpatialInertia(largeRotorInertiaY.getMass(), + largeRotorInertiaY.getCOM(), largeRotorInertiaY.getInertiaTensor()); - // Upper Link Mat3 upper_link_rotational_inertia; upper_link_rotational_inertia << 0.006286029, -1.39E-10, -3.25E-09, -1.39E-10, 0.006437035, 6.24E-06, -3.25E-09, 6.24E-06, 0.00020455; Vec3 upper_link_com(3.99E-08, 0.003531916, 0.130185501); - upper_link_spatial_inertia_ = SpatialInertia(0.4617247, upper_link_com, - upper_link_rotational_inertia); + upper_link_spatial_inertia_ = SpatialInertia(0.4617247, upper_link_com, upper_link_rotational_inertia); - // Wrist Pitch Link Mat3 wrist_pitch_link_rotational_inertia; wrist_pitch_link_rotational_inertia << 1.73E-05, -2.13E-12, -3.11E-13, -2.13E-12, 1.42E-05, -4.98E-07, -3.11E-13, -4.98E-07, 2.03E-05; Vec3 wrist_pitch_link_com(-1.70E-09, 0.004167466, 0.01683806); - wrist_pitch_link_spatial_inertia_ = SpatialInertia(0.063603259, wrist_pitch_link_com, - wrist_pitch_link_rotational_inertia); + wrist_pitch_link_spatial_inertia_ = SpatialInertia(0.063603259, wrist_pitch_link_com, wrist_pitch_link_rotational_inertia); - // Wrist Roll Link Mat3 wrist_roll_link_rotational_inertia; wrist_roll_link_rotational_inertia << 0.000182127, 2.15E-06, -1.57E-06, 2.15E-06, 8.28E-05, -9.44E-06, -1.57E-06, -9.44E-06, 0.00011452; Vec3 wrist_roll_link_com(-0.001177177, 0.004697849, 0.040664076); - wrist_roll_link_spatial_inertia_ = SpatialInertia(0.167365855, wrist_roll_link_com, - wrist_roll_link_rotational_inertia); + wrist_roll_link_spatial_inertia_ = SpatialInertia(0.167365855, wrist_roll_link_com, wrist_roll_link_rotational_inertia); - // Elbow Rotor - elbow_rotor_spatial_inertia_ = largeRotorInertiaY; + elbow_rotor_spatial_inertia_ = SpatialInertia(largeRotorInertiaY.getMass(), + largeRotorInertiaY.getCOM(), largeRotorInertiaY.getInertiaTensor()); - // Wrist Pitch Rotor - wrist_pitch_rotor_spatial_inertia_ = smallRotorInertiaY; + wrist_pitch_rotor_spatial_inertia_ = SpatialInertia(smallRotorInertiaY.getMass(), + smallRotorInertiaY.getCOM(), smallRotorInertiaY.getInertiaTensor()); - // Wrist Roll Rotor - wrist_roll_rotor_spatial_inertia_ = smallRotorInertiaZ; + wrist_roll_rotor_spatial_inertia_ = SpatialInertia(smallRotorInertiaZ.getMass(), + smallRotorInertiaZ.getCOM(), smallRotorInertiaZ.getInertiaTensor()); - // Gripper Mat3 gripper_rotational_inertia; gripper_rotational_inertia << 6.20E-05, 5.26E-09, 1.04E-06, 5.26E-09, 6.64E-05, 8.69E-07, 1.04E-06, 8.69E-07, 1.81E-05; Vec3 gripper_com(0.000564355, -0.003238555, 0.105873754); - gripper_spatial_inertia_ = SpatialInertia(0.170943071, gripper_com, - gripper_rotational_inertia); + gripper_spatial_inertia_ = SpatialInertia(0.170943071, gripper_com, gripper_rotational_inertia); - // Gripper Rotor - gripper_rotor_spatial_inertia_ = smallRotorInertiaX; + gripper_rotor_spatial_inertia_ = SpatialInertia(smallRotorInertiaX.getMass(), + smallRotorInertiaX.getCOM(), smallRotorInertiaX.getInertiaTensor()); } } // namespace grbda diff --git a/src/Robots/Tello.cpp b/src/Robots/Tello.cpp index 35904de8..8f12e386 100644 --- a/src/Robots/Tello.cpp +++ b/src/Robots/Tello.cpp @@ -136,22 +136,25 @@ namespace grbda std::make_shared(CoordAxis::X, gimbal_joint_name), std::make_shared(CoordAxis::Y, thigh_joint_name)}; + // CasADi symbolic phi for automatic differentiation std::function(const JointCoordinate &)> hip_diff_phi = [](const JointCoordinate &q) { double N = 6.0; DVec out = DVec(2); - casadi::SX ql_1 = q(0); - casadi::SX ql_2 = q(1); - casadi::SX y_1 = q(2) / N; - casadi::SX y_2 = q(3) / N; + // q(0), q(1) are independent (rotors), q(2), q(3) are dependent (links) + casadi::SX y_1 = q(0) / N; // rotor 1 post-gearbox (independent) + casadi::SX y_2 = q(1) / N; // rotor 2 post-gearbox (independent) + casadi::SX ql_1 = q(2); // gimbal angle (dependent) + casadi::SX ql_2 = q(3); // thigh angle (dependent) - out[0] = (57 * sin(y_1)) / 2500 - (49 * cos(ql_1)) / 5000 - (399 * sin(ql_1)) / 20000 - (8 * cos(y_1) * cos(ql_2)) / 625 - (57 * cos(ql_1) * sin(ql_2)) / 2500 - (7 * sin(y_1) * sin(ql_1)) / 625 + (7 * sin(ql_1) * sin(ql_2)) / 625 - (8 * cos(ql_1) * sin(y_1) * sin(ql_2)) / 625 + 3021 / 160000; + out[0] = (57 * sin(y_1)) / 2500 - (49 * cos(ql_1)) / 5000 - (399 * sin(ql_1)) / 20000 - (8 * cos(y_1) * cos(ql_2)) / 625 - (57 * cos(ql_1) * sin(ql_2)) / 2500 - (7 * sin(y_1) * sin(ql_1)) / 625 + (7 * sin(ql_1) * sin(ql_2)) / 625 - (8 * cos(ql_1) * sin(y_1) * sin(ql_2)) / 625 + 3021.0 / 160000; - out[1] = (57 * sin(y_2)) / 2500 - (49 * cos(ql_1)) / 5000 + (399 * sin(ql_1)) / 20000 - (8 * cos(y_2) * cos(ql_2)) / 625 - (57 * cos(ql_1) * sin(ql_2)) / 2500 + (7 * sin(y_2) * sin(ql_1)) / 625 - (7 * sin(ql_1) * sin(ql_2)) / 625 - (8 * cos(ql_1) * sin(y_2) * sin(ql_2)) / 625 + 3021 / 160000; + out[1] = (57 * sin(y_2)) / 2500 - (49 * cos(ql_1)) / 5000 + (399 * sin(ql_1)) / 20000 - (8 * cos(y_2) * cos(ql_2)) / 625 - (57 * cos(ql_1) * sin(ql_2)) / 2500 + (7 * sin(y_2) * sin(ql_1)) / 625 - (7 * sin(ql_1) * sin(ql_2)) / 625 - (8 * cos(ql_1) * sin(y_2) * sin(ql_2)) / 625 + 3021.0 / 160000; return out; }; + std::vector hip_diff_independent_coordinates = {true, true, false, false}; std::shared_ptr hip_diff_loop_constraint; @@ -234,24 +237,27 @@ namespace grbda std::make_shared(CoordAxis::Y, shin_joint_name), std::make_shared(CoordAxis::Y, foot_joint_name)}; + // CasADi symbolic phi for automatic differentiation std::function(const JointCoordinate &)> knee_ankle_diff_phi = [](const JointCoordinate &q) { double N = 6.0; DVec out = DVec(2); - casadi::SX ql_1 = q(0); - casadi::SX ql_2 = q(1); - casadi::SX y_1 = q(2) / N; - casadi::SX y_2 = q(3) / N; + // q(0), q(1) are independent (rotors), q(2), q(3) are dependent (links) + casadi::SX y_1 = q(0) / N; // rotor 1 post-gearbox (independent) + casadi::SX y_2 = q(1) / N; // rotor 2 post-gearbox (independent) + casadi::SX ql_1 = q(2); // shin angle (dependent) + casadi::SX ql_2 = q(3); // foot angle (dependent) - out[0] = (21 * cos(y_1 / 2 - y_2 / 2 + (1979 * 3.1415) / 4500)) / 6250 - (13 * cos(y_1 / 2 - y_2 / 2 + (493 * 3.1415) / 1500)) / 625 - (273 * cos(3.1415 / 9)) / 12500 - (7 * sin(y_1 / 2 - y_2 / 2 + ql_2 + (231 * 3.1415) / 500)) / 2500 + (91 * sin(ql_2 + (2 * 3.1415) / 15)) / 5000 - (147 * sin(ql_2 + 3.1415 / 45)) / 50000 + 163349 / 6250000; + out[0] = (21 * cos(y_1 / 2 - y_2 / 2 + (1979 * 3.1415) / 4500)) / 6250 - (13 * cos(y_1 / 2 - y_2 / 2 + (493 * 3.1415) / 1500)) / 625 - (273 * cos(3.1415 / 9)) / 12500 - (7 * sin(y_1 / 2 - y_2 / 2 + ql_2 + (231 * 3.1415) / 500)) / 2500 + (91 * sin(ql_2 + (2 * 3.1415) / 15)) / 5000 - (147 * sin(ql_2 + 3.1415 / 45)) / 50000 + 163349.0 / 6250000; out[1] = ql_1 - y_2 / 2 - y_1 / 2; return out; }; + std::vector knee_ankle_diff_independent_coordinates = {true, true, false, false}; - + std::shared_ptr knee_ankle_diff_loop_constraint; knee_ankle_diff_loop_constraint = std::make_shared( knee_ankle_diff_independent_coordinates, knee_ankle_diff_phi); @@ -277,6 +283,7 @@ namespace grbda } template class Tello; + template class Tello>; template class Tello; } // namespace grbda diff --git a/src/Robots/TelloNoRotors.cpp b/src/Robots/TelloNoRotors.cpp new file mode 100644 index 00000000..4f0a536e --- /dev/null +++ b/src/Robots/TelloNoRotors.cpp @@ -0,0 +1,117 @@ +#include "grbda/Robots/TelloNoRotors.hpp" + +namespace grbda +{ + + template + ClusterTreeModel TelloNoRotors::buildClusterTreeModel() const + { + using namespace ClusterJoints; + typedef spatial::Transform Xform; + + ClusterTreeModel model{}; + + // Set gravity in z direction + model.setGravity(Vec3{0., 0., this->grav}); + + // Torso (floating base) + const std::string torso_name = this->base; + const std::string torso_parent_name = "ground"; + const SpatialInertia torso_spatial_inertia = + SpatialInertia{this->torso_mass, this->torso_CoM, this->torso_inertia}; + model.template appendBody>(torso_name, torso_spatial_inertia, + torso_parent_name, Xform{}, + "torso-to-ground"); + + std::vector sides = {"left", "right"}; + + for (size_t i(0); i < 2; i++) + { + const std::string side = sides[i]; + + // Hip clamp - plain Revolute about Z + const Mat3 R_hip_clamp = i == 0 ? this->R_left_hip_clamp : this->R_right_hip_clamp; + const Vec3 p_hip_clamp = i == 0 ? this->p_left_hip_clamp : this->p_right_hip_clamp; + const Xform hip_clamp_Xtree = Xform(R_hip_clamp, p_hip_clamp); + const std::string hip_clamp_name = side + "-hip-clamp"; + const std::string hip_clamp_parent_name = this->base; + const SpatialInertia hip_clamp_spatial_inertia = + SpatialInertia{this->hip_clamp_mass, this->hip_clamp_CoM, this->hip_clamp_inertia}; + const std::string hip_clamp_joint_name = this->base + "-to-" + side + "-hip-clamp"; + model.template appendBody>(hip_clamp_name, hip_clamp_spatial_inertia, + hip_clamp_parent_name, hip_clamp_Xtree, + ori::CoordinateAxis::Z, hip_clamp_joint_name); + + // Gimbal - plain Revolute about X + const Mat3 R_gimbal = i == 0 ? this->R_left_gimbal : this->R_right_gimbal; + const Vec3 p_gimbal = i == 0 ? this->p_left_gimbal : this->p_right_gimbal; + const Xform gimbal_Xtree = Xform(R_gimbal, p_gimbal); + const std::string gimbal_name = side + "-gimbal"; + const std::string gimbal_parent_name = side + "-hip-clamp"; + const SpatialInertia gimbal_spatial_inertia = + SpatialInertia{this->gimbal_mass, this->gimbal_CoM, this->gimbal_inertia}; + const std::string gimbal_joint_name = side + "-hip-clamp-to-gimbal"; + model.template appendBody>(gimbal_name, gimbal_spatial_inertia, + gimbal_parent_name, gimbal_Xtree, + ori::CoordinateAxis::X, gimbal_joint_name); + + // Thigh - plain Revolute about Y + const Mat3 R_thigh = i == 0 ? this->R_left_thigh : this->R_right_thigh; + const Vec3 p_thigh = i == 0 ? this->p_left_thigh : this->p_right_thigh; + const Xform thigh_Xtree = Xform(R_thigh, p_thigh); + const std::string thigh_name = side + "-thigh"; + const std::string thigh_parent_name = side + "-gimbal"; + const SpatialInertia thigh_spatial_inertia = + SpatialInertia{this->thigh_mass, this->thigh_CoM, this->thigh_inertia}; + const std::string thigh_joint_name = side + "-gimbal-to-thigh"; + model.template appendBody>(thigh_name, thigh_spatial_inertia, + thigh_parent_name, thigh_Xtree, + ori::CoordinateAxis::Y, thigh_joint_name); + + // Shin - plain Revolute about Y + const Mat3 R_shin = i == 0 ? this->R_left_shin : this->R_right_shin; + const Vec3 p_shin = i == 0 ? this->p_left_shin : this->p_right_shin; + const Xform shin_Xtree = Xform(R_shin, p_shin); + const std::string shin_name = side + "-shin"; + const std::string shin_parent_name = side + "-thigh"; + const SpatialInertia shin_spatial_inertia = + SpatialInertia{this->shin_mass, this->shin_CoM, this->shin_inertia}; + const std::string shin_joint_name = side + "-thigh-to-shin"; + model.template appendBody>(shin_name, shin_spatial_inertia, + shin_parent_name, shin_Xtree, + ori::CoordinateAxis::Y, shin_joint_name); + + // Foot - plain Revolute about Y + const Mat3 R_foot = i == 0 ? this->R_left_foot : this->R_right_foot; + const Vec3 p_foot = i == 0 ? this->p_left_foot : this->p_right_foot; + const Xform foot_Xtree = Xform(R_foot, p_foot); + const std::string foot_name = side + "-foot"; + const std::string foot_parent_name = side + "-shin"; + const SpatialInertia foot_spatial_inertia = + SpatialInertia{this->foot_mass, this->foot_CoM, this->foot_inertia}; + const std::string foot_joint_name = side + "-shin-to-foot"; + model.template appendBody>(foot_name, foot_spatial_inertia, + foot_parent_name, foot_Xtree, + ori::CoordinateAxis::Y, foot_joint_name); + + // Append contact points for the feet + const std::string toe_contact_name = side + "-toe_contact"; + const std::string heel_contact_name = side + "-heel_contact"; + if (i == 0) + model.appendEndEffector(foot_name, Vec3(this->_footToeLength, 0, -this->_footHeight), + toe_contact_name); + else + model.appendContactPoint(foot_name, Vec3(-this->_footToeLength, 0, -this->_footHeight), + toe_contact_name); + model.appendContactPoint(foot_name, Vec3(-this->_footHeelLength, 0, -this->_footHeight), + heel_contact_name); + } + + return model; + } + + template class TelloNoRotors; + template class TelloNoRotors>; + template class TelloNoRotors; + +} // namespace grbda diff --git a/src/Robots/TelloRotorsNoConstraints.cpp b/src/Robots/TelloRotorsNoConstraints.cpp new file mode 100644 index 00000000..10c0e365 --- /dev/null +++ b/src/Robots/TelloRotorsNoConstraints.cpp @@ -0,0 +1,225 @@ +#include "grbda/Robots/TelloRotorsNoConstraints.hpp" + +namespace grbda +{ + + template + ClusterTreeModel TelloRotorsNoConstraints::buildClusterTreeModel() const + { + using namespace ClusterJoints; + typedef spatial::Transform Xform; + typedef GearedTransmissionModule GearedTransModule; + typedef RevoluteWithRotor RevoluteWithRotor; + + ClusterTreeModel model{}; + + // Set gravity in z direction + model.setGravity(Vec3{0., 0., this->grav}); + + // Torso + const std::string torso_name = this->base; + const std::string torso_parent_name = "ground"; + const SpatialInertia torso_spatial_inertia = + SpatialInertia{this->torso_mass, this->torso_CoM, this->torso_inertia}; + model.template appendBody>(torso_name, torso_spatial_inertia, + torso_parent_name, Xform{}, + "torso-to-ground"); + + std::vector sides = {"left", "right"}; + + for (size_t i(0); i < 2; i++) + { + const std::string side = sides[i]; + + // ===== Hip Clamp RevoluteWithRotor Cluster ===== + const Mat3 R_hip_clamp = i == 0 ? this->R_left_hip_clamp : this->R_right_hip_clamp; + const Vec3 p_hip_clamp = i == 0 ? this->p_left_hip_clamp : this->p_right_hip_clamp; + const Xform hip_clamp_Xtree = Xform(R_hip_clamp, p_hip_clamp); + const std::string hip_clamp_name = side + "-hip-clamp"; + const SpatialInertia hip_clamp_spatial_inertia = + SpatialInertia{this->hip_clamp_mass, this->hip_clamp_CoM, this->hip_clamp_inertia}; + auto hip_clamp = model.registerBody(hip_clamp_name, hip_clamp_spatial_inertia, + this->base, hip_clamp_Xtree); + + const Mat3 R_hip_clamp_rotor = i == 0 ? this->R_left_hip_clamp_rotor + : this->R_right_hip_clamp_rotor; + const Vec3 p_hip_clamp_rotor = i == 0 ? this->p_left_hip_clamp_rotor + : this->p_right_hip_clamp_rotor; + const Xform hip_clamp_rotor_Xtree = Xform(R_hip_clamp_rotor, p_hip_clamp_rotor); + const std::string hip_clamp_rotor_name = side + "-hip-clamp-rotor"; + const SpatialInertia hip_clamp_rotor_spatial_inertia = + SpatialInertia{this->hip_clamp_rotor_mass, this->hip_clamp_rotor_CoM, + this->hip_clamp_rotor_inertia}; + auto hip_clamp_rotor = model.registerBody(hip_clamp_rotor_name, + hip_clamp_rotor_spatial_inertia, + this->base, hip_clamp_rotor_Xtree); + + const std::string hip_clamp_cluster_name = side + "-hip-clamp"; + const std::string hip_clamp_joint_name = this->base + "-to-" + side + "-hip-clamp"; + const std::string hip_clamp_rotor_joint_name = this->base + "-to-" + side + "-hip-clamp-rotor"; + GearedTransModule hip_clamp_module{hip_clamp, hip_clamp_rotor, + hip_clamp_joint_name, + hip_clamp_rotor_joint_name, + ori::CoordinateAxis::Z, + ori::CoordinateAxis::Z, + this->gear_ratio}; + model.template appendRegisteredBodiesAsCluster( + hip_clamp_cluster_name, hip_clamp_module); + + // ===== Gimbal RevoluteWithRotor Cluster (INDEPENDENT) ===== + const Mat3 R_gimbal = i == 0 ? this->R_left_gimbal : this->R_right_gimbal; + const Vec3 p_gimbal = i == 0 ? this->p_left_gimbal : this->p_right_gimbal; + const Xform gimbal_Xtree = Xform(R_gimbal, p_gimbal); + const std::string gimbal_name = side + "-gimbal"; + const SpatialInertia gimbal_spatial_inertia = + SpatialInertia{this->gimbal_mass, this->gimbal_CoM, this->gimbal_inertia}; + auto gimbal = model.registerBody(gimbal_name, gimbal_spatial_inertia, + hip_clamp_name, gimbal_Xtree); + + // Gimbal rotor + const Mat3 R_gimbal_rotor = i == 0 ? this->R_left_hip_rotor_1 : this->R_right_hip_rotor_1; + const Vec3 p_gimbal_rotor = i == 0 ? this->p_left_hip_rotor_1 : this->p_right_hip_rotor_1; + const Xform gimbal_rotor_Xtree = Xform(R_gimbal_rotor, p_gimbal_rotor); + const std::string gimbal_rotor_name = side + "-gimbal-rotor"; + const SpatialInertia gimbal_rotor_spatial_inertia = + SpatialInertia{this->hip_rotor_1_mass, this->hip_rotor_1_CoM, + this->hip_rotor_1_inertia}; + auto gimbal_rotor = model.registerBody(gimbal_rotor_name, gimbal_rotor_spatial_inertia, + hip_clamp_name, gimbal_rotor_Xtree); + + const std::string gimbal_cluster_name = side + "-gimbal"; + const std::string gimbal_joint_name = hip_clamp_name + "-to-" + side + "-gimbal"; + const std::string gimbal_rotor_joint_name = hip_clamp_name + "-to-" + side + "-gimbal-rotor"; + GearedTransModule gimbal_module{gimbal, gimbal_rotor, + gimbal_joint_name, + gimbal_rotor_joint_name, + ori::CoordinateAxis::X, + ori::CoordinateAxis::X, + this->gear_ratio}; + model.template appendRegisteredBodiesAsCluster( + gimbal_cluster_name, gimbal_module); + + // ===== Thigh RevoluteWithRotor Cluster (INDEPENDENT) ===== + const Mat3 R_thigh = i == 0 ? this->R_left_thigh : this->R_right_thigh; + const Vec3 p_thigh = i == 0 ? this->p_left_thigh : this->p_right_thigh; + const Xform thigh_Xtree = Xform(R_thigh, p_thigh); + const std::string thigh_name = side + "-thigh"; + const SpatialInertia thigh_spatial_inertia = + SpatialInertia{this->thigh_mass, this->thigh_CoM, this->thigh_inertia}; + auto thigh = model.registerBody(thigh_name, thigh_spatial_inertia, + gimbal_name, thigh_Xtree); + + // Thigh rotor + const Mat3 R_thigh_rotor = i == 0 ? this->R_left_hip_rotor_2 : this->R_right_hip_rotor_2; + const Vec3 p_thigh_rotor = i == 0 ? this->p_left_hip_rotor_2 : this->p_right_hip_rotor_2; + const Xform thigh_rotor_Xtree = Xform(R_thigh_rotor, p_thigh_rotor); + const std::string thigh_rotor_name = side + "-thigh-rotor"; + const SpatialInertia thigh_rotor_spatial_inertia = + SpatialInertia{this->hip_rotor_2_mass, this->hip_rotor_2_CoM, + this->hip_rotor_2_inertia}; + auto thigh_rotor = model.registerBody(thigh_rotor_name, thigh_rotor_spatial_inertia, + gimbal_name, thigh_rotor_Xtree); + + const std::string thigh_cluster_name = side + "-thigh"; + const std::string thigh_joint_name = gimbal_name + "-to-" + side + "-thigh"; + const std::string thigh_rotor_joint_name = gimbal_name + "-to-" + side + "-thigh-rotor"; + GearedTransModule thigh_module{thigh, thigh_rotor, + thigh_joint_name, + thigh_rotor_joint_name, + ori::CoordinateAxis::Y, + ori::CoordinateAxis::Y, + this->gear_ratio}; + model.template appendRegisteredBodiesAsCluster( + thigh_cluster_name, thigh_module); + + // ===== Shin RevoluteWithRotor Cluster (INDEPENDENT) ===== + const Mat3 R_shin = i == 0 ? this->R_left_shin : this->R_right_shin; + const Vec3 p_shin = i == 0 ? this->p_left_shin : this->p_right_shin; + const Xform shin_Xtree = Xform(R_shin, p_shin); + const std::string shin_name = side + "-shin"; + const SpatialInertia shin_spatial_inertia = + SpatialInertia{this->shin_mass, this->shin_CoM, this->shin_inertia}; + auto shin = model.registerBody(shin_name, shin_spatial_inertia, + thigh_name, shin_Xtree); + + // Shin rotor + const Mat3 R_shin_rotor = i == 0 ? this->R_left_knee_ankle_rotor_1 + : this->R_right_knee_ankle_rotor_1; + const Vec3 p_shin_rotor = i == 0 ? this->p_left_knee_ankle_rotor_1 + : this->p_right_knee_ankle_rotor_1; + const Xform shin_rotor_Xtree = Xform(R_shin_rotor, p_shin_rotor); + const std::string shin_rotor_name = side + "-shin-rotor"; + const SpatialInertia shin_rotor_spatial_inertia = + SpatialInertia{this->knee_ankle_rotor_1_mass, this->knee_ankle_rotor_1_CoM, + this->knee_ankle_rotor_1_inertia}; + auto shin_rotor = model.registerBody(shin_rotor_name, shin_rotor_spatial_inertia, + thigh_name, shin_rotor_Xtree); + + const std::string shin_cluster_name = side + "-shin"; + const std::string shin_joint_name = thigh_name + "-to-" + side + "-shin"; + const std::string shin_rotor_joint_name = thigh_name + "-to-" + side + "-shin-rotor"; + GearedTransModule shin_module{shin, shin_rotor, + shin_joint_name, + shin_rotor_joint_name, + ori::CoordinateAxis::Y, + ori::CoordinateAxis::Y, + this->gear_ratio}; + model.template appendRegisteredBodiesAsCluster( + shin_cluster_name, shin_module); + + // ===== Foot RevoluteWithRotor Cluster (INDEPENDENT) ===== + const Mat3 R_foot = i == 0 ? this->R_left_foot : this->R_right_foot; + const Vec3 p_foot = i == 0 ? this->p_left_foot : this->p_right_foot; + const Xform foot_Xtree = Xform(R_foot, p_foot); + const std::string foot_name = side + "-foot"; + const SpatialInertia foot_spatial_inertia = + SpatialInertia{this->foot_mass, this->foot_CoM, this->foot_inertia}; + auto foot = model.registerBody(foot_name, foot_spatial_inertia, + shin_name, foot_Xtree); + + // Foot rotor + const Mat3 R_foot_rotor = i == 0 ? this->R_left_knee_ankle_rotor_2 + : this->R_right_knee_ankle_rotor_2; + const Vec3 p_foot_rotor = i == 0 ? this->p_left_knee_ankle_rotor_2 + : this->p_right_knee_ankle_rotor_2; + const Xform foot_rotor_Xtree = Xform(R_foot_rotor, p_foot_rotor); + const std::string foot_rotor_name = side + "-foot-rotor"; + const SpatialInertia foot_rotor_spatial_inertia = + SpatialInertia{this->knee_ankle_rotor_2_mass, this->knee_ankle_rotor_2_CoM, + this->knee_ankle_rotor_2_inertia}; + auto foot_rotor = model.registerBody(foot_rotor_name, foot_rotor_spatial_inertia, + shin_name, foot_rotor_Xtree); + + const std::string foot_cluster_name = side + "-foot"; + const std::string foot_joint_name = shin_name + "-to-" + side + "-foot"; + const std::string foot_rotor_joint_name = shin_name + "-to-" + side + "-foot-rotor"; + GearedTransModule foot_module{foot, foot_rotor, + foot_joint_name, + foot_rotor_joint_name, + ori::CoordinateAxis::Y, + ori::CoordinateAxis::Y, + this->gear_ratio}; + model.template appendRegisteredBodiesAsCluster( + foot_cluster_name, foot_module); + + // Append contact points for the feet + const std::string toe_contact_name = side + "-toe_contact"; + const std::string heel_contact_name = side + "-heel_contact"; + if (i == 0) + model.appendEndEffector(foot_name, Vec3(this->_footToeLength, 0, -this->_footHeight), + toe_contact_name); + else + model.appendContactPoint(foot_name, Vec3(-this->_footToeLength, 0, -this->_footHeight), + toe_contact_name); + model.appendContactPoint(foot_name, Vec3(-this->_footHeelLength, 0, -this->_footHeight), + heel_contact_name); + } + + return model; + } + + template class TelloRotorsNoConstraints; + template class TelloRotorsNoConstraints>; + template class TelloRotorsNoConstraints; + +} // namespace grbda diff --git a/src/Robots/TelloWithArms.cpp b/src/Robots/TelloWithArms.cpp index 1d1431d3..3db8aa27 100644 --- a/src/Robots/TelloWithArms.cpp +++ b/src/Robots/TelloWithArms.cpp @@ -166,6 +166,7 @@ namespace grbda } template class TelloWithArms; + template class TelloWithArms>; template class TelloWithArms; } // namespace grbda diff --git a/src/Utils/Factorization.cpp b/src/Utils/Factorization.cpp index c9895816..4d73c5b0 100644 --- a/src/Utils/Factorization.cpp +++ b/src/Utils/Factorization.cpp @@ -144,6 +144,7 @@ namespace grbda } template class LTL; + template class LTL>; template class LTL; } diff --git a/src/Utils/SpatialTransforms.cpp b/src/Utils/SpatialTransforms.cpp index 590fe501..7b887b97 100644 --- a/src/Utils/SpatialTransforms.cpp +++ b/src/Utils/SpatialTransforms.cpp @@ -1,4 +1,5 @@ #include "grbda/Utils/SpatialTransforms.h" +#include "grbda/Utils/Spatial.h" namespace grbda { @@ -102,19 +103,55 @@ namespace grbda template D6Mat Transform::inverseTransformForceSubspace(const D6Mat &F_in) const { - D6Mat F_out = D6Mat::Zero(6, F_in.cols()); - for (int i = 0; i < F_in.cols(); i++) - F_out.col(i) = inverseTransformForceVector(F_in.col(i)); + // Optimized version using block operations instead of per-column loop + // X^{-T} * F where X^{-T} = [E^T, r_hat * E^T; 0, E^T] + const int num_cols = F_in.cols(); + D6Mat F_out(6, num_cols); + const Mat3 ET = E_.transpose(); + const Mat3 r_hat_ET = ori::vectorToSkewMat(r_) * ET; + + // Top 3 rows: E^T * F_top + r_hat * E^T * F_bottom + F_out.template topRows<3>().noalias() = ET * F_in.template topRows<3>(); + F_out.template topRows<3>().noalias() += r_hat_ET * F_in.template bottomRows<3>(); + + // Bottom 3 rows: E^T * F_bottom + F_out.template bottomRows<3>().noalias() = ET * F_in.template bottomRows<3>(); + return F_out; } + template + void Transform::inverseTransformForceSubspace4( + DMat &F1, DMat &F2, DMat &F3, DMat &F4) const + { + // Batched version: transforms 4 force subspaces with shared E^T and r_hat*E^T computation + // This is used in the ID derivatives walk-to-root loop where t1, t2, t3, t4 are all transformed + const Mat3 ET = E_.transpose(); + const Mat3 r_hat_ET = ori::vectorToSkewMat(r_) * ET; + + // Scratch buffer shared across all 4 transforms (F1-F4 are all the same size) + DMat F_out(6, F1.cols()); + + auto transformInPlace = [&ET, &r_hat_ET, &F_out](DMat &F) { + F_out.template topRows<3>().noalias() = ET * F.template topRows<3>(); + F_out.template topRows<3>().noalias() += r_hat_ET * F.template bottomRows<3>(); + F_out.template bottomRows<3>().noalias() = ET * F.template bottomRows<3>(); + std::swap(F, F_out); + }; + + transformInPlace(F1); + transformInPlace(F2); + transformInPlace(F3); + transformInPlace(F4); + } + template Mat6 Transform::inverseTransformSpatialInertia(const Mat6 &I_in) const { Mat6 I_out; - Mat3 E_trans = E_.transpose(); - Mat3 r_hat = ori::vectorToSkewMat(r_); + const Mat3 E_trans = E_.transpose(); + const Mat3 r_hat = ori::vectorToSkewMat(r_); const Mat3 &I_TL = I_in.template topLeftCorner<3, 3>(); const Mat3 &I_TR = I_in.template topRightCorner<3, 3>(); @@ -134,6 +171,56 @@ namespace grbda return I_out; } + template + Mat6 + Transform::transformSpatialInertiaToWorld(const Mat6 &I_in) const + { + // Transform inertia from body frame to world frame + // If X transforms world -> body (X * v_world = v_body), then + // I_world = X^{-T} * I_body * X^{-1} + // + // X = [E, 0; -E*r_hat, E] (transforms world -> body) + // X^{-1} = [E^T, 0; r_hat*E^T, E^T] (transforms body -> world) + // X^{-T} = [E, -E*r_hat^T; 0, E] + // + // Note: r_hat^T = -r_hat, so X^{-T} = [E, E*r_hat; 0, E] + + Mat6 I_out; + const Mat3 E_trans = E_.transpose(); + const Mat3 r_hat = ori::vectorToSkewMat(r_); + + const Mat3 &I_TL = I_in.template topLeftCorner<3, 3>(); + const Mat3 &I_TR = I_in.template topRightCorner<3, 3>(); + const Mat3 &I_BL = I_in.template bottomLeftCorner<3, 3>(); + const Mat3 &I_BR = I_in.template bottomRightCorner<3, 3>(); + + // Compute X^{-T} * I * X^{-1} + // X^{-T} = [E, E*r_hat; 0, E] + // X^{-1} = [E^T, 0; r_hat*E^T, E^T] + + // First compute I * X^{-1}: + // [I_TL, I_TR] [E^T, 0 ] [I_TL*E^T + I_TR*r_hat*E^T, I_TR*E^T] + // [I_BL, I_BR] * [r_hat*E^T, E^T] = [I_BL*E^T + I_BR*r_hat*E^T, I_BR*E^T] + + const Mat3 r_hat_ET = r_hat * E_trans; + const Mat3 temp_TL = I_TL * E_trans + I_TR * r_hat_ET; + const Mat3 temp_TR = I_TR * E_trans; + const Mat3 temp_BL = I_BL * E_trans + I_BR * r_hat_ET; + const Mat3 temp_BR = I_BR * E_trans; + + // Now compute X^{-T} * (I * X^{-1}): + // [E, E*r_hat] [temp_TL, temp_TR] + // [0, E ] * [temp_BL, temp_BR] + + const Mat3 E_r_hat = E_ * r_hat; + I_out.template topLeftCorner<3, 3>() = E_ * temp_TL + E_r_hat * temp_BL; + I_out.template topRightCorner<3, 3>() = E_ * temp_TR + E_r_hat * temp_BR; + I_out.template bottomLeftCorner<3, 3>() = E_ * temp_BL; + I_out.template bottomRightCorner<3, 3>() = E_ * temp_BR; + + return I_out; + } + template Vec3 Transform::transformPoint(const Vec3 &local_offset) const { @@ -197,6 +284,7 @@ namespace grbda } template class Transform; + template class Transform>; template class Transform; template class Transform; @@ -255,7 +343,29 @@ namespace grbda return transforms_[output_body_index]; } + template + DMat GeneralizedAbsoluteTransform::transformMotionSubspaceToWorld( + const DMat &S_local) const + { + // Transform motion subspace from local body frames to world frame + // Each 6-row block is transformed by the inverse of the corresponding Xa + const int num_cols = S_local.cols(); + DMat S_world = DMat::Zero(6 * num_output_bodies_, num_cols); + + for (int body = 0; body < num_output_bodies_; body++) + { + const Transform &Xa = transforms_[body]; + // S_world = X^{-1} * S_local (body -> world) + // inverseTransformMotionSubspace does X^{-1} * S + S_world.template middleRows<6>(6 * body) = + Xa.inverseTransformMotionSubspace(S_local.template middleRows<6>(6 * body)); + } + + return S_world; + } + template class GeneralizedAbsoluteTransform; + template class GeneralizedAbsoluteTransform>; template class GeneralizedAbsoluteTransform; template class GeneralizedAbsoluteTransform; @@ -374,6 +484,12 @@ namespace grbda return transforms_and_parent_subindices_[output_body_index].first; } + template + const Transform &GeneralizedTransform::operator[](int output_body_index) const + { + return transforms_and_parent_subindices_[output_body_index].first; + } + template GeneralizedTransform GeneralizedTransform::operator*(const GeneralizedTransform &X_in) const @@ -476,7 +592,202 @@ namespace grbda return M_out; } + template + void GeneralizedTransform::accumulateBlockDiagonalInertia( + const std::vector, Eigen::aligned_allocator>> &I_child, + std::vector, Eigen::aligned_allocator>> &I_parent) const + { + for (int i = 0; i < num_output_bodies_; i++) + { + const Transform &X = transforms_and_parent_subindices_[i].first; + const int parent_subindex = transforms_and_parent_subindices_[i].second; + I_parent[parent_subindex] += X.inverseTransformSpatialInertia(I_child[i]); + } + } + + template + DMat GeneralizedTransform::blockDiagonalInertiaTimesMotionSubspace( + const std::vector, Eigen::aligned_allocator>> &Ic, + const DMat &S) const + { + DMat out; + blockDiagonalInertiaTimesMotionSubspace(Ic, S, out); + return out; + } + + template + void GeneralizedTransform::blockDiagonalInertiaTimesMotionSubspace( + const std::vector, Eigen::aligned_allocator>> &Ic, + const DMat &S, DMat &out) const + { + const int num_cols = S.cols(); + out.resize(6 * num_output_bodies_, num_cols); + for (int body = 0; body < num_output_bodies_; body++) + out.template middleRows<6>(6 * body).noalias() = + Ic[body] * S.template middleRows<6>(6 * body); + } + + template + void GeneralizedTransform::accumulateBlockDiagonalPair( + const DMat &M1_child, DMat &M1_parent, + const DMat &M2_child, DMat &M2_parent) const + { + for (int i = 0; i < num_output_bodies_; i++) + { + const Transform &X = transforms_and_parent_subindices_[i].first; + const int parent_subindex = transforms_and_parent_subindices_[i].second; + + const Mat3 &E = X.getRotation(); + const Mat3 E_trans = E.transpose(); + const Mat3 r_hat = ori::vectorToSkewMat(X.getTranslation()); + + auto transformBlock = [&](const Mat6 &M_in) -> Mat6 { + Mat6 M_out; + const Mat3 &TL = M_in.template topLeftCorner<3, 3>(); + const Mat3 &TR = M_in.template topRightCorner<3, 3>(); + const Mat3 &BL = M_in.template bottomLeftCorner<3, 3>(); + const Mat3 &BR = M_in.template bottomRightCorner<3, 3>(); + M_out.template topLeftCorner<3, 3>() = E_trans * TL * E + + r_hat * E_trans * BL * E - + E_trans * TR * E * r_hat - + r_hat * E_trans * BR * E * r_hat; + M_out.template topRightCorner<3, 3>() = E_trans * TR * E + + r_hat * E_trans * BR * E; + M_out.template bottomLeftCorner<3, 3>() = E_trans * BL * E - + E_trans * BR * E * r_hat; + M_out.template bottomRightCorner<3, 3>() = E_trans * BR * E; + return M_out; + }; + + const int ps = 6 * parent_subindex; + M1_parent.template block<6, 6>(ps, ps) += + transformBlock(M1_child.template block<6, 6>(6 * i, 6 * i)); + M2_parent.template block<6, 6>(ps, ps) += + transformBlock(M2_child.template block<6, 6>(6 * i, 6 * i)); + } + } + + template + DMat GeneralizedTransform::blockDiagonalInertiaTimesMotionSubspace( + const DMat &Ic_block_diag, const DMat &S) const + { + DMat out; + blockDiagonalInertiaTimesMotionSubspace(Ic_block_diag, S, out); + return out; + } + + template + void GeneralizedTransform::blockDiagonalInertiaTimesMotionSubspace( + const DMat &Ic_block_diag, const DMat &S, DMat &out) const + { + const int num_cols = S.cols(); + out.resize(6 * num_output_bodies_, num_cols); + for (int body = 0; body < num_output_bodies_; body++) + out.template middleRows<6>(6 * body).noalias() = + Ic_block_diag.template block<6, 6>(6 * body, 6 * body) * + S.template middleRows<6>(6 * body); + } + + template + DMat GeneralizedTransform::transformForceSubspaceToParent( + const DMat &F_in) const + { + // This is essentially the same as inverseTransformForceSubspace + // but we're being explicit about its role in the CRBA + const int num_cols = F_in.cols(); + + // Fast path for single-body clusters (most common case) + // Single body connecting to single parent body - avoid loop and dynamic indexing + if (num_output_bodies_ == 1 && num_parent_bodies_ == 1) + { + const Transform &X = transforms_and_parent_subindices_[0].first; + return X.inverseTransformForceSubspace(F_in); + } + + DMat F_out = DMat::Zero(6 * num_parent_bodies_, num_cols); + + int output_body = 0; + for (const auto &transform_and_parent_subindex : transforms_and_parent_subindices_) + { + const Transform &X = transform_and_parent_subindex.first; + const int parent_subindex = transform_and_parent_subindex.second; + + // Transform force from child body frame to parent body frame and accumulate + F_out.template middleRows<6>(6 * parent_subindex).noalias() += + X.inverseTransformForceSubspace(F_in.template middleRows<6>(6 * output_body)); + + output_body++; + } + + return F_out; + } + + template + void GeneralizedTransform::inverseTransformForceSubspace4( + DMat &F1, DMat &F2, DMat &F3, DMat &F4) const + { + // Fast path for single-body to single-body (most common case) + // Delegates to Transform::inverseTransformForceSubspace4 which batches the computation + if (num_output_bodies_ == 1 && num_parent_bodies_ == 1) + { + const Transform &X = transforms_and_parent_subindices_[0].first; + X.inverseTransformForceSubspace4(F1, F2, F3, F4); + return; + } + + // Multi-body case: transform each body's portion and accumulate to parent bodies + // We batch the rotation computation across all 4 matrices for each body + const int num_cols_1 = F1.cols(); + const int num_cols_2 = F2.cols(); + const int num_cols_3 = F3.cols(); + const int num_cols_4 = F4.cols(); + + DMat F1_out = DMat::Zero(6 * num_parent_bodies_, num_cols_1); + DMat F2_out = DMat::Zero(6 * num_parent_bodies_, num_cols_2); + DMat F3_out = DMat::Zero(6 * num_parent_bodies_, num_cols_3); + DMat F4_out = DMat::Zero(6 * num_parent_bodies_, num_cols_4); + + int output_body = 0; + for (const auto &transform_and_parent_subindex : transforms_and_parent_subindices_) + { + const Transform &X = transform_and_parent_subindex.first; + const int parent_subindex = transform_and_parent_subindex.second; + + // Compute E^T and r_hat*E^T once for this body + const Mat3 ET = X.getRotation().transpose(); + const Mat3 r_hat_ET = ori::vectorToSkewMat(X.getTranslation()) * ET; + + // Helper to transform a single body's portion of F and accumulate + auto transformAndAccumulate = [&](const DMat &F_in, DMat &F_out, int num_cols) { + // Extract this body's 6 rows from input + const auto F_top = F_in.template block<3, Eigen::Dynamic>(6 * output_body, 0, 3, num_cols); + const auto F_bot = F_in.template block<3, Eigen::Dynamic>(6 * output_body + 3, 0, 3, num_cols); + + // Transform and accumulate to parent body's rows + // Top: E^T * F_top + r_hat * E^T * F_bot + F_out.template block<3, Eigen::Dynamic>(6 * parent_subindex, 0, 3, num_cols).noalias() += + ET * F_top + r_hat_ET * F_bot; + // Bottom: E^T * F_bot + F_out.template block<3, Eigen::Dynamic>(6 * parent_subindex + 3, 0, 3, num_cols).noalias() += + ET * F_bot; + }; + + transformAndAccumulate(F1, F1_out, num_cols_1); + transformAndAccumulate(F2, F2_out, num_cols_2); + transformAndAccumulate(F3, F3_out, num_cols_3); + transformAndAccumulate(F4, F4_out, num_cols_4); + + output_body++; + } + + F1 = std::move(F1_out); + F2 = std::move(F2_out); + F3 = std::move(F3_out); + F4 = std::move(F4_out); + } + template class GeneralizedTransform; + template class GeneralizedTransform>; template class GeneralizedTransform; template class GeneralizedTransform;