From 9dbbcfdf6dc5031706518009e908a15ad305caa6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mag=C3=AD=20Romany=C3=A0?= Date: Wed, 29 Jan 2025 15:59:28 +0100 Subject: [PATCH 1/6] Add Differentiable Parameters Support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Magí Romanyà --- src/Mandos/Core/DiffParameters.hpp | 69 ++++++++ src/Mandos/Core/DiffRigidBody.cpp | 8 +- src/Mandos/Core/DiffRigidBody.hpp | 6 +- src/Mandos/Core/Differentiable.cpp | 147 ++++++++++++++--- src/Mandos/Core/Differentiable.hpp | 55 ++++++- .../Core/Energies/CosseratBendingRod.cpp | 26 +++ .../Core/Energies/CosseratBendingRod.hpp | 13 ++ src/Mandos/Core/Energies/MassSpring.cpp | 48 ++++++ src/Mandos/Core/Energies/MassSpring.hpp | 19 ++- src/Mandos/Core/Model.cpp | 154 +++++++++++++++++- src/Mandos/Core/Model.hpp | 17 ++ 11 files changed, 520 insertions(+), 42 deletions(-) create mode 100644 src/Mandos/Core/DiffParameters.hpp diff --git a/src/Mandos/Core/DiffParameters.hpp b/src/Mandos/Core/DiffParameters.hpp new file mode 100644 index 00000000..42dceae3 --- /dev/null +++ b/src/Mandos/Core/DiffParameters.hpp @@ -0,0 +1,69 @@ +#ifndef MANDOS_CORE_DIFFERENTIABLE_PARAMETERS_H_ +#define MANDOS_CORE_DIFFERENTIABLE_PARAMETERS_H_ + +#include + +#include +#include +#include +#include +#include + +namespace mandos::core +{ + +template +struct DiffParameterHandle { + using ParamTag = Tag; // + using ParamEnergy = Energy; + static constexpr auto Param = Parameter; + + using ReturnType = decltype(std::declval().template computeEnergyGradientParameterDerivative( + std::declval &>(), + 0)); + + static constexpr std::array parameterShape = []() -> std::array { + if constexpr (std::is_floating_point_v) { + return {1, 1}; + } else { // We assume that if it is not a scalar it is an Eigen::Matrix Object + return {ReturnType::RowsAtCompileTime, ReturnType::ColsAtCompileTime}; + } + }(); + + explicit DiffParameterHandle(SimulationObjectHandle simObj) + : m_simObj(simObj) + { + } + + SimulationObjectHandle m_simObj; // + std::vector m_indices; + + inline int size() const + { + return parameterShape[0] * parameterShape[1] * static_cast(m_indices.size()); + } +}; + +using DiffParametersList = + utilities::typelist, + DiffParameterHandle, + DiffParameterHandle>; + +using DiffParameterHandleV = DiffParametersList::as; + +struct DiffParameters { + std::vector parameters; + + inline int size() const + { + int size = 0; + for (const auto ¶meter : parameters) { + std::visit([&size](const auto &p) { size += p.size(); }, parameter); + } + return size; + } +}; + +} // namespace mandos::core + +#endif // MANDOS_CORE_DIFFERENTIABLE_PARAMETERS_H_ diff --git a/src/Mandos/Core/DiffRigidBody.cpp b/src/Mandos/Core/DiffRigidBody.cpp index ea4a78d6..ebf37284 100644 --- a/src/Mandos/Core/DiffRigidBody.cpp +++ b/src/Mandos/Core/DiffRigidBody.cpp @@ -6,7 +6,7 @@ namespace mandos::core { -void applyGlobalToLocal(const Model &model, Vec &x) +void applyGlobalToLocal(const Model &model, Eigen::Ref x) { auto offset = 0; for (auto node : model.freeSimulationObjects()) { @@ -24,7 +24,7 @@ void applyGlobalToLocal(const Model &model, Vec &x) } } -void applyLocalToGlobal(const Model &model, Vec &x) +void applyLocalToGlobal(const Model &model, Eigen::Ref x) { auto offset = 0; for (auto node : model.freeSimulationObjects()) { @@ -42,7 +42,7 @@ void applyLocalToGlobal(const Model &model, Vec &x) } } -void applyComposeAxisAngleJacobian(Scalar h, const Model &model, Vec &v) +void applyComposeAxisAngleJacobian(Scalar h, const Model &model, Eigen::Ref v) { unsigned int offset = 0; for (auto node : model.freeSimulationObjects()) { @@ -57,7 +57,7 @@ void applyComposeAxisAngleJacobian(Scalar h, const Model &model, Vec &v) const Vec3 theta = simulationObject.mstate.m_x[i].template segment<3>(3); const Vec3 omega = simulationObject.mstate.m_v[i].template segment<3>(3); const Mat3 J = rotationExpMap(h * omega); - v.segment(offset + 6 * i + 3, 3) = v.segment(offset + 6 * i + 3, 3).transpose() * J; + v.segment<3>(offset + 6 * i + 3) = v.segment<3>(offset + 6 * i + 3).transpose() * J; } } offset += static_cast(size); diff --git a/src/Mandos/Core/DiffRigidBody.hpp b/src/Mandos/Core/DiffRigidBody.hpp index 4b2f11f3..92b5b2f1 100644 --- a/src/Mandos/Core/DiffRigidBody.hpp +++ b/src/Mandos/Core/DiffRigidBody.hpp @@ -17,7 +17,7 @@ namespace mandos::core * @param model The simulation model * @param x The derivative vector (size = nDof) */ -void applyGlobalToLocal(const Model &model, Vec &x); +void applyGlobalToLocal(const Model &model, Eigen::Ref x); /** * @brief Transform the angular segments of a derivative vector from Local to Global Axis Angle derivatives. @@ -29,9 +29,9 @@ void applyGlobalToLocal(const Model &model, Vec &x); * @param model The simulation model * @param x The derivative vector (size = nDof) */ -void applyLocalToGlobal(const Model &model, Vec &x); +void applyLocalToGlobal(const Model &model, Eigen::Ref x); -void applyComposeAxisAngleJacobian(Scalar h, const Model &model, Vec &v); +void applyComposeAxisAngleJacobian(Scalar h, const Model &model, Eigen::Ref v); } // namespace mandos::core diff --git a/src/Mandos/Core/Differentiable.cpp b/src/Mandos/Core/Differentiable.cpp index 6669875e..ec4e29fd 100644 --- a/src/Mandos/Core/Differentiable.cpp +++ b/src/Mandos/Core/Differentiable.cpp @@ -2,6 +2,8 @@ #include #include +#include + namespace { @@ -33,18 +35,23 @@ mandos::core::Vec approximateHzFiniteDiff(mandos::core::Model &model, namespace mandos::core { -Vec computeLossFunctionGradientBackpropagation(Scalar h, - Model &model, - const Trajectory &trajectory, - const LossFunctionAndGradients &loss, - const Mat &dx0_dp, - const Mat &dv0_dp, - unsigned int maxIterations) +BackpropagationResult computeLossFunctionGradientBackpropagation(Scalar h, + Model &model, + const Trajectory &trajectory, + const LossFunctionAndGradients &loss, + const DiffParameters &diffParameters, + unsigned int maxIterations) { const int nParameters = loss.getNParameters(); const int nDof = trajectory.getNDof(); const int nSteps = trajectory.getNSteps(); + if (static_cast(nParameters) != diffParameters.size()) { + throw std::invalid_argument( + "Size missmatch, parameter sizes are different in the loss function parameter gradient and the input " + "diffParameters"); + } + // Initialize the loss function gradients dg_dp, dg_dx and dg_dv // ------------------------------------------------------------------------- Vec lossGradient = loss.lossParameterPartialDerivative; @@ -52,7 +59,7 @@ Vec computeLossFunctionGradientBackpropagation(Scalar h, Vec lossVelocityGradient = loss.lossVelocityPartialDerivative[static_cast(nSteps)]; // Backward loop // ------------------------------------------------------------------------- - const Scalar one_over_h = 1.0 / h; + const Scalar oneOverH = 1.0 / h; for (int i = nSteps - 1; i >= 0; --i) { // Set the correc state const Vec &x = trajectory.positions[static_cast(i) + 1]; @@ -72,13 +79,8 @@ Vec computeLossFunctionGradientBackpropagation(Scalar h, Vec grad = Vec::Zero(nDof); model.computeEnergyGradientAndHessian(h, grad, hessian); - Vec grad_test = Vec::Zero(nDof); - model.computeEnergyAndGradient(h, grad_test); - // TODO Handle gradients with respect to paramters! - const Mat dgradE_dp = Mat::Zero(nDof, nParameters); - - const Vec equation_vector = -(lossPositionGradient + one_over_h * lossVelocityGradient); + const Vec equation_vector = -(lossPositionGradient + oneOverH * lossVelocityGradient); Eigen::ConjugateGradient solver; solver.compute(hessian); @@ -91,8 +93,7 @@ Vec computeLossFunctionGradientBackpropagation(Scalar h, for (unsigned int j = 0; j < maxIterations; ++j) { const Vec Hz = approximateHzFiniteDiff(model, grad, x0, v0, z, h, dx); if (Hz.hasNaN()) { - throw std::runtime_error( - "Back Propagation failed to converge with the current number of backward iterations."); + throw std::runtime_error("Backward Step: NaN detected"); } h_grad = Hz - equation_vector; dz = solver.solve(-h_grad); @@ -106,20 +107,126 @@ Vec computeLossFunctionGradientBackpropagation(Scalar h, model.computeEnergyRetardedPositionHessian(h, dgradE_dx0); lossPositionGradient = loss.lossPositionPartialDerivative[static_cast(i)].transpose() - - one_over_h * lossVelocityGradient.transpose() + z.transpose() * dgradE_dx0; + oneOverH * lossVelocityGradient.transpose() + z.transpose() * dgradE_dx0; model.computeEnergyRetardedVelocityHessian(h, dgradE_dv0); lossVelocityGradient = loss.lossVelocityPartialDerivative[static_cast(i)].transpose() + z.transpose() * dgradE_dv0; - lossGradient += z.transpose() * dgradE_dp; + // lossGradient += z.transpose() * dgradE_dp; + model.computeEnergyGradientParameterDerivative(z, diffParameters, lossGradient); } // Add the initial conditions term // ------------------------------------------------------------------------- + applyLocalToGlobal(model, lossPositionGradient); - lossGradient += lossPositionGradient.transpose() * dx0_dp + lossVelocityGradient.transpose() * dv0_dp; + return {.gradient = lossGradient, .dLoss_dx0 = lossPositionGradient, .dLoss_dv0 = lossVelocityGradient}; +} - return lossGradient; +void BackwardEngine::backwardStep(Eigen::Ref lossPositionGradient, + Eigen::Ref lossVelocityGradient, + Eigen::Ref lossParameterGradient, + const DiffParameters &diffParameters, + unsigned int maxIterations) +{ + FrameMark; + FrameMarkNamed("Backward"); + ZoneScopedN("BackwardEngine.backwardStep"); + if (static_cast(lossParameterGradient.size()) != diffParameters.size()) { + throw std::invalid_argument( + "Size missmatch, parameter sizes are different in the loss function parameter gradient (" + + std::to_string(lossParameterGradient.size()) + + ") and the input " + "diffParameters (" + + std::to_string(diffParameters.size()) + ")"); + } + + // Decrease the step + if (m_NSteps < --m_step) { + throw std::out_of_range("Trying to go beyond the initial conditions of the problem!"); + return; + } + // Set the correc state + const Vec &x = m_trajectory.positions[static_cast(m_step) + 1]; + const Vec &v = m_trajectory.velocities[static_cast(m_step) + 1]; + const Vec &x0 = m_trajectory.positions[static_cast(m_step)]; + const Vec &v0 = m_trajectory.velocities[static_cast(m_step)]; + + m_model.setState(x0, v0); + m_model.computeAdvection(m_h); + m_model.setState(x, v); + + // Compute 2nd order derivatives + SystemMatrix hessian(static_cast(m_NDof), static_cast(m_NDof)); + // At the moment they ignore mappings + SparseMat dgradE_dx0(static_cast(m_NDof), static_cast(m_NDof)); + SparseMat dgradE_dv0(static_cast(m_NDof), static_cast(m_NDof)); + + Vec grad = Vec::Zero(m_NDof); + m_model.computeEnergyGradientAndHessian(m_h, grad, hessian); + + const Vec equation_vector = -(lossPositionGradient + m_oneOverH * lossVelocityGradient); + + Eigen::ConjugateGradient solver; + solver.compute(hessian); + auto solve = [&solver](const Vec &vector) { + ZoneScopedN("cgSolve"); + return solver.solve(vector).eval(); + }; + Vec z = solve(equation_vector); // Adjoint + // Multiple backward iterations + Vec h_grad = -equation_vector; + Vec dz = Vec::Zero(m_NDof); + constexpr Scalar dx = 1e-8; + for (unsigned int j = 0; j < maxIterations; ++j) { + ZoneScopedN("backwardIteration"); + const Vec Hz = approximateHzFiniteDiff(m_model, grad, x0, v0, z, m_h, dx); + if (Hz.hasNaN()) { + throw std::runtime_error("WARNING::DIFFERENTIABLE: Hz has NaN"); + } + h_grad = Hz - equation_vector; + const Scalar backwardIterationEnergy = z.transpose() * (0.5 * Hz - equation_vector); + TracyPlot("backwardIterationEnergy", backwardIterationEnergy); + TracyPlot("h_grad.norm()", h_grad.norm()); + dz = solve(-h_grad); + + // z = TinyAD::line_search(z, dz, backwardIterationEnergy, h_grad, computeEnergy); + z += dz; + } + TracyPlot("z.norm", z.norm()); + // Update the loss function gradients + // ------------------------------------------------------------------------- + applyComposeAxisAngleJacobian(m_h, m_model, lossVelocityGradient); + + m_model.computeEnergyRetardedPositionHessian(m_h, dgradE_dx0); + lossPositionGradient = -m_oneOverH * lossVelocityGradient.transpose() + z.transpose() * dgradE_dx0; + + m_model.computeEnergyRetardedVelocityHessian(m_h, dgradE_dv0); + lossVelocityGradient = z.transpose() * dgradE_dv0; + + { + ZoneScopedN("computeEnergyGradientParameterDerivative"); + // lossGradient += z.transpose() * dgradE_dp; + m_model.computeEnergyGradientParameterDerivative(z, diffParameters, lossParameterGradient); + } +} + +BackpropagationResult BackwardEngine::backwardSimulation(const LossFunctionAndGradients &loss, + const DiffParameters &diffParameters, + unsigned int maxIterations) +{ + Vec lossPositionGradient = loss.lossPositionPartialDerivative[static_cast(m_NSteps)]; + Vec lossVelocityGradient = loss.lossVelocityPartialDerivative[static_cast(m_NSteps)]; + Vec lossParameterGradient = loss.lossParameterPartialDerivative; + + for (int i = m_NSteps - 1; i >= 0; i--) { + backwardStep(lossPositionGradient, lossVelocityGradient, lossParameterGradient, diffParameters, maxIterations); + // Accumulate loss function partial derivatives + lossPositionGradient += loss.lossPositionPartialDerivative[static_cast(i)]; + lossVelocityGradient += loss.lossVelocityPartialDerivative[static_cast(i)]; + } + applyLocalToGlobal(m_model, lossPositionGradient); + return {.gradient = lossParameterGradient, .dLoss_dx0 = lossPositionGradient, .dLoss_dv0 = lossVelocityGradient}; } } // namespace mandos::core diff --git a/src/Mandos/Core/Differentiable.hpp b/src/Mandos/Core/Differentiable.hpp index e3383e28..66b11523 100644 --- a/src/Mandos/Core/Differentiable.hpp +++ b/src/Mandos/Core/Differentiable.hpp @@ -36,13 +36,54 @@ struct MANDOS_CORE_EXPORT LossFunctionAndGradients { } }; -MANDOS_CORE_EXPORT Vec computeLossFunctionGradientBackpropagation(Scalar h, - Model &model, - const Trajectory &trajectory, - const LossFunctionAndGradients &loss, - const Mat &dx0_dp, - const Mat &dv0_dp, - unsigned int maxIterations = 0); +struct BackpropagationResult { + Vec gradient; + Vec dLoss_dx0; + Vec dLoss_dv0; +}; + +struct MANDOS_CORE_EXPORT BackwardEngine { + BackwardEngine(Scalar h, Model &model, const Trajectory &trajectory) + : m_model(model) + , m_trajectory(trajectory) + , m_NDof(trajectory.getNDof()) + , m_NSteps(trajectory.getNSteps()) + , m_h(h) + , m_oneOverH(1.0 / h) + , m_step(m_trajectory.getNSteps()) + { + } + Model &m_model; + const Trajectory &m_trajectory; + const int m_NDof; + const int m_NSteps; + const Scalar m_h; + const Scalar m_oneOverH; + int m_step; + + void backwardStep(Eigen::Ref lossPositionGradient, + Eigen::Ref lossVelocityGradient, + Eigen::Ref lossParameterGradient, + const DiffParameters &diffParameters, + unsigned int maxIterations); + + /* + * Compute simulation gradients of the loss function provided + * + * NOTE: Gradients with respect to orientations will be "local gradients" + */ + BackpropagationResult backwardSimulation(const LossFunctionAndGradients &loss, + const DiffParameters &diffParameters, + unsigned int maxIterations); +}; + +MANDOS_CORE_EXPORT BackpropagationResult +computeLossFunctionGradientBackpropagation(Scalar h, + Model &model, + const Trajectory &trajectory, + const LossFunctionAndGradients &loss, + const DiffParameters &diffParameters, + unsigned int maxIterations = 0); } // namespace mandos::core diff --git a/src/Mandos/Core/Energies/CosseratBendingRod.cpp b/src/Mandos/Core/Energies/CosseratBendingRod.cpp index 4cf2de37..d9ea454a 100644 --- a/src/Mandos/Core/Energies/CosseratBendingRod.cpp +++ b/src/Mandos/Core/Energies/CosseratBendingRod.cpp @@ -193,4 +193,30 @@ CosseratBendingRod::ParameterSet::ParameterSet(const Mat26 &x0, const mandos::co intrinsicDarboux = computeDarbouxVector( restLength, rotationExpMap(x0.row(0).segment<3>(3)), rotationExpMap(x0.row(1).segment<3>(3))); } + +template <> +Vec3 CosseratBendingRod::computeEnergyGradientParameterDerivative( + MechanicalState &mstate, + std::size_t index) const +{ + const auto iA = static_cast(m_indices[index][0]); + const auto iB = static_cast(m_indices[index][1]); + + const Mat3 RA = mandos::core::rotationExpMap(mstate.m_x[iA].segment<3>(3)); + const Mat3 RB = mandos::core::rotationExpMap(mstate.m_x[iB].segment<3>(3)); + + const Vec3 &stiffnessTensor = m_stiffnessTensor[index]; + + const Mat3 J = 0.5 * Vec3(-stiffnessTensor(0) + stiffnessTensor(1) + stiffnessTensor(2), + stiffnessTensor(0) - stiffnessTensor(1) + stiffnessTensor(2), + stiffnessTensor(0) + stiffnessTensor(1) - stiffnessTensor(2)) + .asDiagonal(); + const Mat3 RBA = RB.transpose() * RA; + + const Mat3 dgradE_dp = + 0.5 * RB * (RBA.trace() * Mat3::Identity() - RBA.transpose()) * (J.trace() * Mat3::Identity() - J); + + return mstate.m_grad[iA].segment<3>(3).transpose() * dgradE_dp + + mstate.m_grad[iB].segment<3>(3).transpose() * (-dgradE_dp); +} } // namespace mandos::core diff --git a/src/Mandos/Core/Energies/CosseratBendingRod.hpp b/src/Mandos/Core/Energies/CosseratBendingRod.hpp index 060aeca8..24708014 100644 --- a/src/Mandos/Core/Energies/CosseratBendingRod.hpp +++ b/src/Mandos/Core/Energies/CosseratBendingRod.hpp @@ -24,6 +24,10 @@ public: mandos::core::Vec3 stiffnessTensor; }; + enum Parameters { + IntrinsicDarboux, + }; + /** * @brief Number of energy elements * @@ -58,6 +62,9 @@ public: */ Scalar computeEnergyGradientAndHessian(MechanicalState &mstate) const; + template + Vec3 computeEnergyGradientParameterDerivative(MechanicalState &mstate, std::size_t index) const; + /** * @brief Add a new bending element * @@ -77,6 +84,12 @@ private: }; MANDOS_CORE_EXPORT Vec3 computeDarbouxVector(Scalar L0, const Mat3 &R1, const Mat3 &R2); + +template <> +Vec3 CosseratBendingRod::computeEnergyGradientParameterDerivative( + MechanicalState &mstate, + std::size_t index) const; + } // namespace mandos::core #endif // MANDOS_ENERGIES_COSSEATRODBENDING_H diff --git a/src/Mandos/Core/Energies/MassSpring.cpp b/src/Mandos/Core/Energies/MassSpring.cpp index 3823b91c..71259daf 100644 --- a/src/Mandos/Core/Energies/MassSpring.cpp +++ b/src/Mandos/Core/Energies/MassSpring.cpp @@ -259,4 +259,52 @@ MassSpring::ParameterSet::ParameterSet(const Mat23 &x0, mandos::core::Scalar sti , damping(damping_) { } +template <> +Scalar MassSpring::computeEnergyGradientParameterDerivative( + MechanicalState &mstate, + std::size_t index) const +{ + const auto &indices{m_indices[index]}; + const auto iA = static_cast(indices[0]); + const auto iB = static_cast(indices[1]); + const Vec3 xA = mstate.m_x[iA]; + const Vec3 xB = mstate.m_x[iB]; + + const Scalar L = (xA - xB).norm(); + const Scalar L0 = m_restLength[index]; + const Vec3 u = (xA - xB) / L; + + const Vec3 dgrad_dL0 = [&]() -> Vec3 { + if (m_restLength[index] > std::numeric_limits::epsilon()) { // divide by L0 + return -m_stiffness[index] / L0 * u * (1.0 - (L - L0) / L0); + } + return -L0 * u; + }(); + + return mstate.m_grad[iA].dot(dgrad_dL0) + mstate.m_grad[iB].dot(-dgrad_dL0); +} + +template <> +Scalar MassSpring::computeEnergyGradientParameterDerivative( + MechanicalState &mstate, + std::size_t index) const +{ + const auto &indices{m_indices[index]}; + const auto iA = static_cast(indices[0]); + const auto iB = static_cast(indices[1]); + const Vec3 xA = mstate.m_x[iA]; + const Vec3 xB = mstate.m_x[iB]; + + const Scalar L = (xA - xB).norm(); + const Vec3 u = (xA - xB) / L; + + const Vec3 dgrad_dK = [&]() -> Vec3 { + if (m_restLength[index] > std::numeric_limits::epsilon()) { // divide by L0 + return (L - m_restLength[index]) / m_restLength[index] * u; + } + return (L - m_restLength[index]) * u; + }(); + + return mstate.m_grad[iA].dot(dgrad_dK) + mstate.m_grad[iB].dot(-dgrad_dK); +} } // namespace mandos::core diff --git a/src/Mandos/Core/Energies/MassSpring.hpp b/src/Mandos/Core/Energies/MassSpring.hpp index b1ae2e7d..8bb23c75 100644 --- a/src/Mandos/Core/Energies/MassSpring.hpp +++ b/src/Mandos/Core/Energies/MassSpring.hpp @@ -6,8 +6,6 @@ #include -#include - namespace mandos::core { class MANDOS_CORE_EXPORT MassSpring @@ -22,6 +20,10 @@ public: mandos::core::Scalar stiffness; mandos::core::Scalar damping; }; + enum Parameters { + Stiffness, + RestLength, + }; /** * @brief Number of energy elements @@ -60,6 +62,9 @@ public: Scalar computeEnergyGradientAndHessian(MechanicalState &mstate) const; Scalar computeEnergyGradientAndHessian(MechanicalState &mstate) const; + template + Scalar computeEnergyGradientParameterDerivative(MechanicalState &mstate, std::size_t index) const; + /** * @brief Add a new spring element. The rest length of the spring is taken from the current mechanical state * @@ -81,6 +86,16 @@ private: std::vector m_restLength; }; +template <> +Scalar MassSpring::computeEnergyGradientParameterDerivative( + MechanicalState &mstate, + std::size_t index) const; + +template <> +Scalar MassSpring::computeEnergyGradientParameterDerivative( + MechanicalState &mstate, + std::size_t index) const; + } // namespace mandos::core #endif // MANDOS_ENERGIES_SPRINGS_H diff --git a/src/Mandos/Core/Model.cpp b/src/Mandos/Core/Model.cpp index 6589a9de..9e65ab3b 100644 --- a/src/Mandos/Core/Model.cpp +++ b/src/Mandos/Core/Model.cpp @@ -14,13 +14,9 @@ #include #include "Mandos/Core/Collisions/CollisionDetection.hpp" -#include "Mandos/Core/Collisions/SphereCloud.hpp" #include "Mandos/Core/Energies/CollisionSpring.hpp" -#include "Mandos/Core/KinematicGraph.hpp" #include "Mandos/Core/Mappings/CollisionMapping.hpp" #include "Mandos/Core/MechanicalState.hpp" -#include "Mandos/Core/MechanicalStates/Particle3D.hpp" -#include "Mandos/Core/MechanicalStates/RigidBody.hpp" #include #include @@ -112,6 +108,69 @@ mandos::core::Scalar computeEnergyGradientAndHessian(SimulationObjectT &simulati namespace mandos::core { +void Model::clearSimulationObjectsGradients() +{ + utilities::static_for_each( + [](auto &simulationObjects) { + for (auto &simulationObject : simulationObjects) { + using SimulationObjectT = std::remove_cvref_t; + if constexpr (SimulationObjectT::hasPotentials || SimulationObjectT::hasInertias) { + simulationObject.mstate.clearGradient(); + } + } + }, + m_simulationObjects); +} +void Model::populateGeneralizedGradient(Vec &gradient) +{ + // Propagate the forces back to the free simulation objects + for (auto node : m_backwardSortedList) { + auto &simObjectV = m_simulationObjectsGraph[node]; + std::visit( + [this](const auto handle) { + const auto &simulationObject = this->simulationObject(handle); + using SimulationObjectT = std::remove_pointer_t>; + if constexpr (SimulationObjectT::hasMappings) { + utilities::static_for_each( + [this](const auto &mappings) { + for (const auto &mapping : mappings) { + auto fromHandle = mapping.from(); + auto toHandle = mapping.to(); + + auto &from = this->simulationObject(fromHandle); + auto &to = this->simulationObject(toHandle); + + mapping.applyJT(from.mstate.m_grad, to.mstate.m_grad); + } + }, + simulationObject.m_mappings); + } + }, + simObjectV); + } + + // And accumulate on the generalized gradient + auto offset = 0; + for (auto node : m_freeSimulationObjects) { + const auto &simObjectV = m_simulationObjectsGraph[node]; + std::visit( + [this, &gradient, &offset](auto handle) { + auto &simulationObject = this->simulationObject(handle); + const auto size = simulationObject.mstate.size(); + using SimulationObjectT = std::remove_reference_t>; + if constexpr (SimulationObjectT::hasProjections) { + mandos::core::utilities::static_for_each( + [&simulationObject](const auto &projection) { + projection.applyPT(simulationObject.mstate.gradientView()); + }, + simulationObject.projections); + } + simulationObject.mstate.gradient(gradient.segment(offset, size)); + offset += size; + }, + simObjectV); + } +} void Model::computeAdvection(Scalar h) { @@ -643,7 +702,8 @@ void Model::computeEnergyRetardedPositionHessian(Scalar h, SparseMat &hessian) { ZoneScopedN("Model.computeEnergyRetardedPositionHessian"); std::vector triplets; - // // We compute the retarded position hessian for each simulation object with inertia + // We compute the retarded position hessian for each simulation object with inertia + // Only compute free objects unsigned int offset = 0; for (auto node : m_freeSimulationObjects) { @@ -670,7 +730,7 @@ void Model::computeEnergyRetardedPositionHessian(Scalar h, SparseMat &hessian) void Model::computeEnergyRetardedVelocityHessian(Scalar h, SparseMat &hessian) { ZoneScopedN("Model.computeEnergyRetardedVelocityHessian"); - // // We compute the retarded position hessian for each simulation object with inertia + // We compute the retarded position hessian for each simulation object with inertia std::vector triplets; // Only compute free objects unsigned int offset = 0; @@ -694,4 +754,86 @@ void Model::computeEnergyRetardedVelocityHessian(Scalar h, SparseMat &hessian) } hessian.setFromTriplets(triplets.begin(), triplets.end()); } + +void Model::computeEnergyGradientParameterDerivative(const Vec &z, const DiffParameters &dp, Eigen::Ref result) +{ + int parameterIndex = 0; + + // Compute z_mapped, and place it in each SimulationObject gradient container + // Set z in the Free Simulation Objects Gradients + int offset = 0; + for (auto node : m_freeSimulationObjects) { + const auto &simObjectV = m_simulationObjectsGraph[node]; + std::visit( + [this, &offset, &z](auto handle) { + auto &simulationObject = this->simulationObject(handle); + const auto size = simulationObject.mstate.size(); + simulationObject.mstate.setGradient(z.segment(offset, size)); + + using SimulationObjectT = std::remove_reference_t>; + if constexpr (SimulationObjectT::hasProjections) { + mandos::core::utilities::static_for_each( + [&simulationObject](const auto &projection) { + projection.applyP(simulationObject.mstate.gradientView()); + }, + simulationObject.projections); + } + + offset += size; + }, + simObjectV); + } + // Propagate z to the mapped objects + for (auto node : m_forwardSortedList) { + const auto &simObjectV = m_simulationObjectsGraph[node]; + std::visit( + [this](auto handle) { + const auto &simulationObject = this->simulationObject(handle); + using SimulationObjectT = std::remove_reference_t>; + if constexpr (SimulationObjectT::hasMappings) { + mandos::core::utilities::static_for_each( + [this, &simulationObject](const auto &mappings) { + for (const auto &mapping : mappings) { + auto to = mapping.to(); + + mapping.applyJ(simulationObject.mstate.m_grad, + this->simulationObject(to).mstate.m_grad); + } + }, + simulationObject.m_mappings); + } + }, + simObjectV); + } + + for (const auto ¶meter : dp.parameters) { + std::visit( + [&](const auto ¶m) { + using Parameter_t = std::remove_cvref_t; + using E = Parameter_t::ParamEnergy; + constexpr auto P = Parameter_t::Param; + constexpr auto parameterShape = Parameter_t::parameterShape; + auto &simObj = param.m_simObj.simulationObject(); + + for (const auto index : param.m_indices) { + // Compute the parameter derivative + const auto zTdEdp = + simObj.template potential().template computeEnergyGradientParameterDerivative

( + simObj.mstate, index); + + constexpr auto size = parameterShape[1] * parameterShape[0]; + if constexpr (parameterShape[0] == 1 && parameterShape[1] == 1) { // Scalar parameter + result(parameterIndex) += zTdEdp; + } else if (parameterShape[1] == 1) { // Vector parameter + result.segment(parameterIndex) += zTdEdp; + } else { // Matrix Parameter + result.segment(parameterIndex) += zTdEdp.reshaped(); + } + parameterIndex += size; + } + }, + parameter); + } +} + } // namespace mandos::core diff --git a/src/Mandos/Core/Model.hpp b/src/Mandos/Core/Model.hpp index baaa6215..1fab7577 100644 --- a/src/Mandos/Core/Model.hpp +++ b/src/Mandos/Core/Model.hpp @@ -11,6 +11,7 @@ #include #include "Mandos/Core/Collisions/CollisionPair.hpp" +#include #include "Mandos/Core/Collisions/SimulationCollider.hpp" #include "Mandos/Core/Mappings/BarycentricMapping.hpp" #include "Mandos/Core/MechanicalStates/Particle3D.hpp" @@ -288,6 +289,20 @@ public: */ void computeEnergyRetardedVelocityHessian(Scalar h, SparseMat &hessian); + /** + * @brief Computes the vector-matrix product of a given vector `z` with the derivative of the + * energy gradient wrt the given DiffParameters + * + * This function calculates the product of the input vector `z` with a matrix representing the + * derivative of the energy gradient with respect to the model parameters. The result of this + * vector-matrix product is stored in the `result` vector. + * + * @param z Input vector. + * @param dp The parameters with respect to which the derivatives are computed. + * @param result Result of the vector-matrix product + */ + void computeEnergyGradientParameterDerivative(const Vec &z, const DiffParameters &dp, Eigen::Ref result); + void updateColliders(); void detectCollisions(); void updateCollisionMappings(); @@ -299,6 +314,8 @@ public: const KinematicGraph &graph() const; private: + void clearSimulationObjectsGradients(); + void populateGeneralizedGradient(Vec &gradient); /** * @brief The SimulationObjects that define the Model * -- GitLab From b74e9f5d174434ddf6f4ff08ff0b076dd978c7f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mag=C3=AD=20Romany=C3=A0?= Date: Mon, 27 Jan 2025 16:06:33 +0100 Subject: [PATCH 2/6] Differentiable Parameters Bindings MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Magí Romanyà --- bindings/Mandos/python/CMakeLists.txt | 1 + bindings/Mandos/python/Deformable3D.cpp | 6 ++- bindings/Mandos/python/Deformable3D.hpp | 2 +- bindings/Mandos/python/Differentiable.cpp | 49 +++++++++++++++++-- .../python/Energies/CosseratBendingRod.cpp | 5 ++ .../Mandos/python/Energies/MassSpring.cpp | 7 +++ bindings/Mandos/python/Model.cpp | 2 +- bindings/Mandos/python/RigidBody.cpp | 4 +- bindings/Mandos/python/RigidBody.hpp | 2 +- src/Mandos/Core/DiffParameters.hpp | 2 +- src/Mandos/Core/Differentiable.cpp | 2 +- 11 files changed, 71 insertions(+), 11 deletions(-) diff --git a/bindings/Mandos/python/CMakeLists.txt b/bindings/Mandos/python/CMakeLists.txt index 558f0c62..793f1077 100644 --- a/bindings/Mandos/python/CMakeLists.txt +++ b/bindings/Mandos/python/CMakeLists.txt @@ -28,6 +28,7 @@ set(HEADERS Energies/CosseratBendingRod.hpp Energies/CosseratRodAlignment.hpp Energies/ConstantForce.hpp + Differentiable.hpp ) nanobind_add_module(mandos_pylib STABLE_ABI ${SOURCES} ${HEADERS}) diff --git a/bindings/Mandos/python/Deformable3D.cpp b/bindings/Mandos/python/Deformable3D.cpp index ba35427a..a6d78fcb 100644 --- a/bindings/Mandos/python/Deformable3D.cpp +++ b/bindings/Mandos/python/Deformable3D.cpp @@ -13,7 +13,7 @@ #include #include -#include +NB_MAKE_OPAQUE(mandos::core::DiffParameterHandleV) void mandos::py::wrapDeformable3D(nb::module_ &m) { @@ -31,6 +31,8 @@ void mandos::py::wrapDeformable3D(nb::module_ &m) .def_prop_ro("mass_spring", &Deformable3D::massSpring) .def_prop_ro("constant_force", &Deformable3D::constantForce) .def_prop_rw("particle_mass", &Deformable3D::vertexMass, &Deformable3D::setVertexMass) + .def("disable_gravity", + [](Deformable3D &self) { self.simObject().potential().disable(); }) .def( "add_sphere_cloud", [](Deformable3D &def3d, mandos::core::Scalar radius) { @@ -159,7 +161,7 @@ mandos::py::Deformable3D::Deformable3D(mandos::core::SimulationObjectHandle mandos::py::Deformable3D::handle() +mandos::core::SimulationObjectHandle mandos::py::Deformable3D::handle() const { return m_handle; } diff --git a/bindings/Mandos/python/Deformable3D.hpp b/bindings/Mandos/python/Deformable3D.hpp index ef52902c..42a66545 100644 --- a/bindings/Mandos/python/Deformable3D.hpp +++ b/bindings/Mandos/python/Deformable3D.hpp @@ -21,7 +21,7 @@ struct Deformable3D { core::SimulationObject &simObject(); const core::SimulationObject &simObject() const; - core::SimulationObjectHandle handle(); + core::SimulationObjectHandle handle() const; Eigen::Map> x() const; diff --git a/bindings/Mandos/python/Differentiable.cpp b/bindings/Mandos/python/Differentiable.cpp index 7feef955..184e88de 100644 --- a/bindings/Mandos/python/Differentiable.cpp +++ b/bindings/Mandos/python/Differentiable.cpp @@ -7,6 +7,8 @@ namespace nb = nanobind; +NB_MAKE_OPAQUE(mandos::core::DiffParameterHandleV) + namespace mandos::py { void wrapDifferentiable(nb::module_ &m) @@ -20,8 +22,8 @@ void wrapDifferentiable(nb::module_ &m) .def("get_n_steps", &core::Trajectory::getNSteps) .def("append_state", [](core::Trajectory &trajectory, core::Model &model) { // Get state - core::Vec x(model.nDof()); - core::Vec v(model.nDof()); + mandos::core::Vec x(model.nDof()); + mandos::core::Vec v(model.nDof()); model.state(x, v); // Append state trajectory.positions.push_back(std::move(x)); @@ -36,6 +38,47 @@ void wrapDifferentiable(nb::module_ &m) .def_rw("loss_parameter_partial_derivative", &core::LossFunctionAndGradients::lossParameterPartialDerivative) .def("get_n_parameters", &core::LossFunctionAndGradients::getNParameters); - m.def("compute_loss_function_gradient_backpropagation", &core::computeLossFunctionGradientBackpropagation); + nb::class_(m, "BackpropagationResult") + .def_ro("loss_parameter_gradient", &mandos::core::BackpropagationResult::gradient) + .def_ro("loss_initial_position_gradient", &mandos::core::BackpropagationResult::dLoss_dx0) + .def_ro("loss_initial_velocity_gradient", &mandos::core::BackpropagationResult::dLoss_dv0); + + nb::class_(m, "DiffParameterHandle") + .def("size", + [](const mandos::core::DiffParameterHandleV &self) -> int { + return std::visit([](const auto ¶meter) -> int { return parameter.size(); }, self); + }) + .def_prop_rw( + "indices", + [](const mandos::core::DiffParameterHandleV &self) -> const std::vector & { + return std::visit( + [](const auto ¶meter) -> const std::vector & { return parameter.m_indices; }, + self); + }, + [](mandos::core::DiffParameterHandleV &self, const std::vector &indices) { + std::visit([&](auto ¶meter) { parameter.m_indices = indices; }, self); + }); + nb::class_(m, "DiffParameters") // + .def(nb::init()) + .def(nb::init>()) + .def_prop_ro("size", &mandos::core::DiffParameters::size); + + nb::class_(m, "BackwardEngine") + .def(nb::init()) + .def("backward_step", &mandos::core::BackwardEngine::backwardStep) + .def("backward_simulation", + &mandos::core::BackwardEngine::backwardSimulation, + nb::arg("loss"), + nb::arg("diff_parameters") = mandos::core::DiffParameters(), + nb::arg("max_iterations") = 0); + + m.def("compute_loss_function_gradient_backpropagation", + &mandos::core::computeLossFunctionGradientBackpropagation, + nb::arg("h"), + nb::arg("model"), + nb::arg("trajectory"), + nb::arg("loss"), + nb::arg("diff_parameters") = mandos::core::DiffParameters(), + nb::arg("max_iterations") = 0); } } // namespace mandos::py diff --git a/bindings/Mandos/python/Energies/CosseratBendingRod.cpp b/bindings/Mandos/python/Energies/CosseratBendingRod.cpp index 7d949d78..21fbd3e7 100644 --- a/bindings/Mandos/python/Energies/CosseratBendingRod.cpp +++ b/bindings/Mandos/python/Energies/CosseratBendingRod.cpp @@ -35,6 +35,7 @@ void mandos::py::energies::wrapCosseratBendingRod(nb::module_ &m) const mandos::core::CosseratBendingRod::ParameterSet ¶meterSet) { spring.addElement(indices, parameterSet); }) + .def("size", &core::CosseratBendingRod::size) .def("get_parameter_set", [](const mandos::core::CosseratBendingRod &massSpring, int elementId) { return massSpring.getParameterSet(elementId); @@ -45,4 +46,8 @@ void mandos::py::energies::wrapCosseratBendingRod(nb::module_ &m) const mandos::core::CosseratBendingRod::ParameterSet ¶meterSet) { massSpring.setParameterSet(elementId, parameterSet); }); + + nb::enum_(cosseratBendingRod, "Parameters") + .value("intrinsic_darboux", mandos::core::CosseratBendingRod::IntrinsicDarboux) + .export_values(); } diff --git a/bindings/Mandos/python/Energies/MassSpring.cpp b/bindings/Mandos/python/Energies/MassSpring.cpp index ce304077..8aa42c69 100644 --- a/bindings/Mandos/python/Energies/MassSpring.cpp +++ b/bindings/Mandos/python/Energies/MassSpring.cpp @@ -1,5 +1,6 @@ #include +#include #include #include @@ -36,6 +37,7 @@ void mandos::py::energies::wrapMassSpring(nb::module_ &m) const mandos::core::MassSpring::ParameterSet ¶meterSet) { spring.addElement(indices, parameterSet); }) + .def("size", &core::MassSpring::size) .def("get_parameter_set", [](const mandos::core::MassSpring &massSpring, int elementId) { return massSpring.getParameterSet(elementId); @@ -46,4 +48,9 @@ void mandos::py::energies::wrapMassSpring(nb::module_ &m) mandos::core::MassSpring::ParameterSet parameterSet) { massSpring.setParameterSet(elementId, parameterSet); }); + + nb::enum_(springs, "Parameters") + .value("stiffness", mandos::core::MassSpring::Stiffness) + .value("rest_length", mandos::core::MassSpring::RestLength) + .export_values(); } diff --git a/bindings/Mandos/python/Model.cpp b/bindings/Mandos/python/Model.cpp index 253e4e13..dad2d154 100644 --- a/bindings/Mandos/python/Model.cpp +++ b/bindings/Mandos/python/Model.cpp @@ -246,4 +246,4 @@ void mandos::py::wrapModel(nb::module_ &m) model.commit(); }) .def("commit", [](Model &model) { model.commit(); }); -} \ No newline at end of file +} diff --git a/bindings/Mandos/python/RigidBody.cpp b/bindings/Mandos/python/RigidBody.cpp index be0654de..25258bda 100644 --- a/bindings/Mandos/python/RigidBody.cpp +++ b/bindings/Mandos/python/RigidBody.cpp @@ -18,6 +18,8 @@ #include #include +NB_MAKE_OPAQUE(mandos::core::DiffParameterHandleV) + namespace mandos::py { void wrapRigidBody(nb::module_ &m) @@ -336,7 +338,7 @@ std::vector &RigidBodyCloud3D::getFixedDofVector() template struct mandos::py::RigidBody3D; template struct mandos::py::RigidBody3D; -mandos::core::SimulationObjectHandle RigidBodyCloud3D::handle() +mandos::core::SimulationObjectHandle RigidBodyCloud3D::handle() const { return m_handle; } diff --git a/bindings/Mandos/python/RigidBody.hpp b/bindings/Mandos/python/RigidBody.hpp index 21b06f5a..4c55d9b4 100644 --- a/bindings/Mandos/python/RigidBody.hpp +++ b/bindings/Mandos/python/RigidBody.hpp @@ -59,7 +59,7 @@ struct RigidBodyCloud3D { mandos::core::SimulationObject &simObject(); const mandos::core::SimulationObject &simObject() const; - mandos::core::SimulationObjectHandle handle(); + mandos::core::SimulationObjectHandle handle() const; void resize(int n); diff --git a/src/Mandos/Core/DiffParameters.hpp b/src/Mandos/Core/DiffParameters.hpp index 42dceae3..66067ed1 100644 --- a/src/Mandos/Core/DiffParameters.hpp +++ b/src/Mandos/Core/DiffParameters.hpp @@ -38,7 +38,7 @@ struct DiffParameterHandle { SimulationObjectHandle m_simObj; // std::vector m_indices; - inline int size() const + int size() const { return parameterShape[0] * parameterShape[1] * static_cast(m_indices.size()); } diff --git a/src/Mandos/Core/Differentiable.cpp b/src/Mandos/Core/Differentiable.cpp index ec4e29fd..b9ed2685 100644 --- a/src/Mandos/Core/Differentiable.cpp +++ b/src/Mandos/Core/Differentiable.cpp @@ -185,7 +185,7 @@ void BackwardEngine::backwardStep(Eigen::Ref lossPositionGradient, throw std::runtime_error("WARNING::DIFFERENTIABLE: Hz has NaN"); } h_grad = Hz - equation_vector; - const Scalar backwardIterationEnergy = z.transpose() * (0.5 * Hz - equation_vector); + [[maybe_unused]] const Scalar backwardIterationEnergy = z.transpose() * (0.5 * Hz - equation_vector); TracyPlot("backwardIterationEnergy", backwardIterationEnergy); TracyPlot("h_grad.norm()", h_grad.norm()); dz = solve(-h_grad); -- GitLab From 3dd3853d9d8dc81e03cd60286a3d207507a0c4af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mag=C3=AD=20Romany=C3=A0?= Date: Wed, 8 Jan 2025 18:14:06 +0100 Subject: [PATCH 3/6] Adapt python examples MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Magí Romanyà --- .../Mappings/rigid_body_point_mapping.py | 32 +++++++++++------- examples/python/RigidBody/local_vs_global.py | 33 +++++++++++++++---- examples/python/RigidBody/rigidbody.py | 4 +-- examples/python/Rod/rod.py | 4 +-- examples/python/diff/diff.py | 26 +++++++-------- examples/python/diff/diffRB.py | 12 ++++--- .../python/diff/rigid-body-optimization.py | 11 +++---- examples/python/diff/rod-optimization.py | 23 +++++++------ examples/python/diff/tkdraw.py | 14 +++----- 9 files changed, 89 insertions(+), 70 deletions(-) diff --git a/examples/python/Mappings/rigid_body_point_mapping.py b/examples/python/Mappings/rigid_body_point_mapping.py index a61672c5..f14b1f50 100644 --- a/examples/python/Mappings/rigid_body_point_mapping.py +++ b/examples/python/Mappings/rigid_body_point_mapping.py @@ -8,6 +8,7 @@ import pymandos n_points = 2 delta_x = 2.0 + def create_model(): model = pymandos.Model() rigid_body_cloud = model.add_rigidbody_cloud() @@ -20,7 +21,7 @@ def create_model(): initial_vel = 1.0 for i, velocity in enumerate(velocities): - velocity[5] = -initial_vel + 2.0*initial_vel*i + velocity[5] = -initial_vel + 2.0 * initial_vel * i rigid_body_cloud.size = n_points rigid_body_cloud.x = positions @@ -35,13 +36,18 @@ def create_model(): rigid_body_cloud.inertiaTensor = [np.diag([1.0, 1.0, 1.0]) for _ in range(n_points)] # Create the mapping - mapping = model.add_rigidbody_point_mapping(rigid_body_cloud); - mapping.add_particle([2.0,0.0,0.0], 0); - mapping.add_particle([0.0,2.0,0.0], 1); + mapping = model.add_rigidbody_point_mapping(rigid_body_cloud) + mapping.add_particle([2.0, 0.0, 0.0], 0) + mapping.add_particle([0.0, 2.0, 0.0], 1) # mapping.add_particle([-4.0, 0.0,0.0], 0); mass_spring = mapping.deformable.mass_spring - mass_spring.add_element([0,1], pymandos.energies.MassSpring.ParameterSet(x0 = rigid_body_cloud.x[[0,1], 0:3], stiffness = 100.0)) + mass_spring.add_element( + [0, 1], + pymandos.energies.MassSpring.ParameterSet( + x0=rigid_body_cloud.x[[0, 1], 0:3], stiffness=100.0 + ), + ) mapping.deformable.particle_mass = np.zeros(mapping.deformable.size) mapping.deformable.particle_mass = np.ones(mapping.deformable.size) @@ -50,19 +56,22 @@ def create_model(): # Render mesh mesh = meshio.read("../Rod/axis.obj") for i in range(n_points): - ps_mesh = ps.register_surface_mesh("rigid_body_"+str(i), mesh.points, mesh.get_cells_type("triangle")) + ps_mesh = ps.register_surface_mesh( + "rigid_body_" + str(i), mesh.points, mesh.get_cells_type("triangle") + ) ps_mesh.add_color_quantity("xyz", mesh.points, enabled=True) - ps.register_curve_network("spring", mapping.deformable.x, np.array([[0,1]])) + ps.register_curve_network("spring", mapping.deformable.x, np.array([[0, 1]])) ps.register_point_cloud("particles", mapping.deformable.x, radius=0.1) return model, rigid_body_cloud, mapping + def simulate_callback(model, rigid_body_cloud, mapping): stepParameters = pymandos.StepParameters() stepParameters.h = 0.01 - stepParameters.newton_iterations =5 + stepParameters.newton_iterations = 5 stepParameters.cg_iterations = 20 stepParameters.cg_error = 1e-4 stepParameters.grad_norm = 1e-3 @@ -72,16 +81,17 @@ def simulate_callback(model, rigid_body_cloud, mapping): for i in range(n_points): transform = rigid_body_cloud.get_transform(i) - ps.get_surface_mesh("rigid_body_"+str(i)).set_transform(transform) + ps.get_surface_mesh("rigid_body_" + str(i)).set_transform(transform) ps.get_curve_network("spring").update_node_positions(mapping.deformable.x) ps.get_point_cloud("particles").update_point_positions(mapping.deformable.x) + if __name__ == "__main__": ps.init() ps.set_up_dir("z_up") ps.look_at((0.0, 10.0, 2.0), (0.0, 0.0, 0.0)) - ps.set_ground_plane_mode("none") # Disable ground rendering + ps.set_ground_plane_mode("none") # Disable ground rendering model, rigid_body_cloud, mapping = create_model() - ps.set_user_callback(lambda : simulate_callback(model, rigid_body_cloud, mapping)) + ps.set_user_callback(lambda: simulate_callback(model, rigid_body_cloud, mapping)) ps.show() diff --git a/examples/python/RigidBody/local_vs_global.py b/examples/python/RigidBody/local_vs_global.py index 6a36b93a..66c0be24 100644 --- a/examples/python/RigidBody/local_vs_global.py +++ b/examples/python/RigidBody/local_vs_global.py @@ -6,6 +6,7 @@ import meshio import polyscope as ps import pymandos +import rotation_utilities def create_model(): @@ -19,6 +20,7 @@ def create_model(): vel = np.zeros(6) vel[3] = 1.0 vel[4] = 0.01 + vel[5] = 0.00 rigid_body_local.x = pos rigid_body_local.v = vel @@ -35,7 +37,7 @@ def create_model(): rigid_body_global.mass = 1.0 rigid_body_global.inertiaTensor = np.diag([2.0, 1.0, 4.0]) - model.compute_dag() + model.commit() # Render mesh mesh = meshio.read("tennis.obj") @@ -54,19 +56,35 @@ def create_model(): return model, rigid_body_local, rigid_body_global +positions = [] +velocities = [] def simulate_callback(model, rigid_body_local, rigid_body_global): stepParameters = pymandos.StepParameters() stepParameters.h = 0.1 - stepParameters.newton_iterations = 5 + stepParameters.newton_iterations = 200 stepParameters.cg_error = 1e-7 - stepParameters.grad_norm = 1e-1 + stepParameters.grad_norm = 1e20 + stepParameters.grad_norm = 1e-9 stepParameters.line_search_iterations = 0 - pymandos.step(model, stepParameters) + result = pymandos.step(model, stepParameters) + x, v = model.state + positions.append(x.copy()) + velocities.append(v.copy()) + if not result: + print(result) ps.get_surface_mesh("rb_local_mesh").set_transform(rigid_body_local.get_transform()) - ps.get_surface_mesh("rb_global_mesh").set_transform( - rigid_body_global.get_transform() - ) + global_mesh = ps.get_surface_mesh("rb_global_mesh") + transform = rigid_body_global.get_transform() + com = (transform @ np.array([0.0, 0.0, 0.0, 1.0]))[:3] + global_mesh.set_transform(transform) + + # omega = rigid_body_local.v[3:6] + # axis_angle = rigid_body_local.x[3:6] + # omega2 = rotation_utilities.expMapJacobian(axis_angle).T @ omega + # phi_dot = rigid_body_global.v[3:6] + # # print(np.linalg.norm(phi_dot)) + # print(f"{np.linalg.norm(omega)} {np.linalg.norm(omega2)} {np.linalg.norm(phi_dot)}") if __name__ == "__main__": @@ -77,3 +95,4 @@ if __name__ == "__main__": lambda: simulate_callback(model, rigid_body_local, rigid_body_global) ) ps.show() + np.savez("output", positions=np.array(positions), velocities=np.array(velocities)) diff --git a/examples/python/RigidBody/rigidbody.py b/examples/python/RigidBody/rigidbody.py index 860010ca..9bf57713 100644 --- a/examples/python/RigidBody/rigidbody.py +++ b/examples/python/RigidBody/rigidbody.py @@ -24,7 +24,7 @@ def create_model(): rigid_body.mass = 1.0 rigid_body.inertiaTensor = np.diag([2.0, 1.0, 4.0]) - model.compute_dag() + model.commit() # Render mesh mesh = meshio.read("tennis.obj") @@ -40,7 +40,7 @@ def simulate_callback(model, rigid_body): stepParameters.cg_iterations = 20 stepParameters.cg_error = 1e-4 stepParameters.grad_norm = 1e-3 - stepParameters.line_search_iterations = 0 + stepParameters.line_search_iterations = 5 pymandos.step(model, stepParameters) ps.get_surface_mesh("rb_mesh").set_transform(rigid_body.get_transform()) diff --git a/examples/python/Rod/rod.py b/examples/python/Rod/rod.py index 8aacb601..6cbe402a 100644 --- a/examples/python/Rod/rod.py +++ b/examples/python/Rod/rod.py @@ -36,9 +36,9 @@ def create_rod(model, n_points): ] # A Rod is made of three diferent energies - # - MassSpring energy, which tries to keep distance between frames at a particular rest length + # - MassSpring energy, which tries to keep distance between frames at a particular rest length # - CosseratBendingRod energy, which tries to keep frames aligned along the Z direction - # - CosseratRodAlignment energy, + # - CosseratRodAlignment energy, mass_spring = rigid_body_cloud.mass_spring cosserat_rod = rigid_body_cloud.cosserat_rod_alignment for i in range(n_points-1): diff --git a/examples/python/diff/diff.py b/examples/python/diff/diff.py index df4185cb..a6036f67 100644 --- a/examples/python/diff/diff.py +++ b/examples/python/diff/diff.py @@ -61,21 +61,17 @@ def compute_loss(trajectory: pymandos.Trajectory) -> pymandos.LossFunctionAndGra loss.loss_velocity_partial_derivative = [ np.zeros(3) for _ in range(len(trajectory.positions)) ] - loss.loss_parameter_partial_derivative = np.zeros( - 3 - ) # 3 parameters (one for each initial velocity vector coefficient) + loss.loss_parameter_partial_derivative = np.array([]) return loss def sim_wrapper(v0): trajectory, model = simulate(v0, 100) loss = compute_loss(trajectory) - dx0_dp = np.zeros((3, 3)) - dv0_dp = np.identity(3) - gradient = pymandos.compute_loss_function_gradient_backpropagation( - stepParameters.h, model, trajectory, loss, dx0_dp, dv0_dp, 0 + result = pymandos.compute_loss_function_gradient_backpropagation( + stepParameters.h, model, trajectory, loss ) - return loss.loss, gradient + return loss.loss, result.loss_initial_velocity_gradient if __name__ == "__main__": @@ -84,16 +80,16 @@ if __name__ == "__main__": losses = [] gradients = [] - for vel in np.linspace(1,5, 100): - trajectory, model = simulate([0,0,vel], 100) + for vel in np.linspace(1, 5, 100): + trajectory, model = simulate([0, 0, vel], 100) loss = compute_loss(trajectory) - dx0_dp = np.zeros((3,3)) - dv0_dp = np.identity(3) - gradient = pymandos.compute_loss_function_gradient_backpropagation(stepParameters.h, model, trajectory, loss, dx0_dp, dv0_dp, 0) + result = pymandos.compute_loss_function_gradient_backpropagation( + stepParameters.h, model, trajectory, loss + ) losses.append(loss.loss) - gradients.append(gradient[-1]) + gradients.append(result.loss_initial_velocity_gradient[-1]) - gradients2 = np.gradient(np.array(losses), 4.0/100.0) + gradients2 = np.gradient(np.array(losses), 4.0 / 100.0) plt.plot(gradients2) plt.plot(gradients) plt.show() diff --git a/examples/python/diff/diffRB.py b/examples/python/diff/diffRB.py index fac95f40..908c656c 100644 --- a/examples/python/diff/diffRB.py +++ b/examples/python/diff/diffRB.py @@ -81,7 +81,7 @@ def compute_loss(trajectory: pymandos.Trajectory) -> pymandos.LossFunctionAndGra loss.loss_velocity_partial_derivative = [ np.zeros(dof) for _ in trajectory.positions ] - loss.loss_parameter_partial_derivative = np.zeros(2 * dof) + loss.loss_parameter_partial_derivative = np.array([]) return loss @@ -89,6 +89,7 @@ def sim_wrapper(x0: list, v0: list): trajectory, model = simulate(x0, v0, 100) loss = compute_loss(trajectory) dof = trajectory.get_n_dof() + # p = (x0, v0) dx0_dp = np.block( [ [np.eye(dof), np.zeros((dof, dof))], @@ -99,10 +100,11 @@ def sim_wrapper(x0: list, v0: list): [np.zeros((dof, dof)), np.eye(dof)], ] ) - # dx0_dp = np.zeros((dof, dof)) - # dv0_dp = np.identity(dof) - gradient = pymandos.compute_loss_function_gradient_backpropagation( - stepParameters.h, model, trajectory, loss, dx0_dp, dv0_dp, 10 + backward_engine = pymandos.BackwardEngine(stepParameters.h, model, trajectory) + result = backward_engine.backward_simulation(loss) + gradient = ( + result.loss_initial_position_gradient.T @ dx0_dp + + result.loss_initial_velocity_gradient.T @ dv0_dp ) return loss.loss, gradient diff --git a/examples/python/diff/rigid-body-optimization.py b/examples/python/diff/rigid-body-optimization.py index a86cbeec..299563eb 100644 --- a/examples/python/diff/rigid-body-optimization.py +++ b/examples/python/diff/rigid-body-optimization.py @@ -76,20 +76,17 @@ def compute_loss(trajectory: pymandos.Trajectory) -> pymandos.LossFunctionAndGra loss.loss_velocity_partial_derivative = [ np.zeros(dof) for _ in trajectory.positions ] - loss.loss_parameter_partial_derivative = np.zeros(dof) + loss.loss_parameter_partial_derivative = np.array([]) return loss def sim_wrapper(v0): trajectory, model = simulate(v0, 100) loss = compute_loss(trajectory) - dof = trajectory.get_n_dof() - dx0_dp = np.zeros((dof, dof)) - dv0_dp = np.identity(dof) - gradient = pymandos.compute_loss_function_gradient_backpropagation( - stepParameters.h, model, trajectory, loss, dx0_dp, dv0_dp, 0 + result = pymandos.compute_loss_function_gradient_backpropagation( + stepParameters.h, model, trajectory, loss ) - return loss.loss, gradient + return loss.loss, result.loss_initial_velocity_gradient if __name__ == "__main__": diff --git a/examples/python/diff/rod-optimization.py b/examples/python/diff/rod-optimization.py index 6dceaae5..4599742f 100644 --- a/examples/python/diff/rod-optimization.py +++ b/examples/python/diff/rod-optimization.py @@ -65,9 +65,9 @@ def create_model(initial_velocities): rigid_body_cloud.inertiaTensor = [np.diag([1.0, 1.0, 1.0]) for _ in range(n_points)] # Rod energy - Ks = [2000.0 for _ in range(n_points-1)] - stiffness_tensor = [100.0 * np.ones(3) for _ in range(n_points-2)] - cosserat_stiffness = [1000.0 for _ in range(n_points-1)] + Ks = [2000.0 for _ in range(n_points - 1)] + stiffness_tensor = [100.0 * np.ones(3) for _ in range(n_points - 2)] + cosserat_stiffness = [1000.0 for _ in range(n_points - 1)] cosserat_rod = rigid_body_cloud.cosserat_rod cosserat_rod.set_parameters(Ks, cosserat_stiffness, stiffness_tensor) @@ -94,7 +94,7 @@ def compute_loss(trajectory: pymandos.Trajectory) -> pymandos.LossFunctionAndGra loss.loss_velocity_partial_derivative = [ np.zeros(dof) for _ in trajectory.positions ] - loss.loss_parameter_partial_derivative = np.zeros(int(dof / 6 * 3)) + loss.loss_parameter_partial_derivative = np.array([]) return loss @@ -120,14 +120,13 @@ def sim_wrapper(v0): dof = trajectory.get_n_dof() dx0_dp = np.zeros((dof, int(dof / 6 * 3))) dv0_dp = linear_dof_matrix.copy().T - # dv0_dp = np.diag( - # np.array( - # [[1.0, 1.0, 1.0, 0.0, 0.0, 0.0] for _ in range(int(dof / 6))] - # ).flatten() - # ) - - gradient = pymandos.compute_loss_function_gradient_backpropagation( - stepParameters.h, model, trajectory, loss, dx0_dp, dv0_dp, backward_iterations + + result = pymandos.compute_loss_function_gradient_backpropagation( + stepParameters.h, model, trajectory, loss + ) + gradient = ( + result.loss_initial_position_gradient.T @ dx0_dp + + result.loss_initial_velocity_gradient.T @ dv0_dp ) return loss.loss, gradient diff --git a/examples/python/diff/tkdraw.py b/examples/python/diff/tkdraw.py index 83294728..9e493161 100644 --- a/examples/python/diff/tkdraw.py +++ b/examples/python/diff/tkdraw.py @@ -219,7 +219,7 @@ def compute_loss(trajectory: pymandos.Trajectory) -> pymandos.LossFunctionAndGra loss.loss_velocity_partial_derivative = [ np.zeros(dof) for _ in trajectory.positions ] - loss.loss_parameter_partial_derivative = np.zeros(int(dof / 6 * 3)) + loss.loss_parameter_partial_derivative = np.zeros(0) return loss @@ -247,15 +247,11 @@ def sim_wrapper(v0): dof = trajectory.get_n_dof() dx0_dp = np.zeros((dof, int(dof / 6 * 3))) dv0_dp = linear_dof_matrix.copy().T - # dv0_dp = np.diag( - # np.array( - # [[1.0, 1.0, 1.0, 0.0, 0.0, 0.0] for _ in range(int(dof / 6))] - # ).flatten() - # ) - - gradient = pymandos.compute_loss_function_gradient_backpropagation( - stepParameters.h, model, trajectory, loss, dx0_dp, dv0_dp, backward_iterations + + result = pymandos.compute_loss_function_gradient_backpropagation( + stepParameters.h, model, trajectory, loss, pymandos.DiffParameters([]), backward_iterations ) + gradient = result.loss_initial_velocity_gradient @ dv0_dp return loss.loss, gradient -- GitLab From 070530475b5e03276f765fdf69bf45f0fd7372e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mag=C3=AD=20Romany=C3=A0?= Date: Wed, 8 Jan 2025 18:14:41 +0100 Subject: [PATCH 4/6] New Diff parameter examples MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Magí Romanyà --- examples/python/diff/coin.stl | Bin 0 -> 10384 bytes .../python/diff/mass-spring-parameter-test.py | 134 +++++++ examples/python/diff/parallel_transport.py | 1 + examples/python/diff/path.obj | 144 ++++++++ examples/python/diff/path3D.obj | 140 +++++++ examples/python/diff/path3D2.obj | 132 +++++++ examples/python/diff/path3D3.obj | 76 ++++ examples/python/diff/path3D4.obj | 100 +++++ examples/python/diff/rod_arm.py | 291 +++++++++++++++ examples/python/diff/rod_to_curve.py | 347 ++++++++++++++++++ examples/python/diff/rod_to_point.py | 238 ++++++++++++ examples/python/diff/save_transforms.py | 46 +++ 12 files changed, 1649 insertions(+) create mode 100644 examples/python/diff/coin.stl create mode 100644 examples/python/diff/mass-spring-parameter-test.py create mode 120000 examples/python/diff/parallel_transport.py create mode 100644 examples/python/diff/path.obj create mode 100644 examples/python/diff/path3D.obj create mode 100644 examples/python/diff/path3D2.obj create mode 100644 examples/python/diff/path3D3.obj create mode 100644 examples/python/diff/path3D4.obj create mode 100644 examples/python/diff/rod_arm.py create mode 100644 examples/python/diff/rod_to_curve.py create mode 100644 examples/python/diff/rod_to_point.py create mode 100644 examples/python/diff/save_transforms.py diff --git a/examples/python/diff/coin.stl b/examples/python/diff/coin.stl new file mode 100644 index 0000000000000000000000000000000000000000..e26506e97de5ac277744e7721c05604368acdbc6 GIT binary patch literal 10384 zcmZReGC0S;z##u~jv)g>!~TQK`X*invh8H}6ZgKfU1ZlKD7;ycKXLD#t`l}<>D`;; z`4jivZf~+%@tnQqpKOT5zi|i*;Sv!K(juI4rHH$ z`~eFQ28PY@!C)PH+eN^x(&taytMTajUSvO^y6Wq_sbC#<0wqv%Agcng_4pI_Iz=qp z%fMh&^bD*cIndk$+3g@ZVD_Qv*zT*k8RT=%H&!S*kX0f3iGg9t4@A0hKajl-6h3ck z7wtn1H&mae*+9}&;r|B2RN*(8+yPxxFGSd50tJ9AAPsw+_ev`8&!vGk^@-B zO-Ovebs(z(u|es|6P&J`S*yW$a6zi4Nx*^ZeV{z}!gi4z$PSo&s5%;M%mv%=XpR?( z4rEoEiX``e^59=%P#)}srrk?wekRCy8`(Zo9TTBx_fWGwiVkE|$a$WDVS6pZN>D1f zo#<-<3QU0r!30?blxyugXYcuM91Je6EFmR>9RtG}uS4MSs!K4z1ms?r49FiK z9e<32?LgrMvICOOA9y-}^T&~@6cc0_5F6ww&xnP4k^RTOz+K4$_W7=k1Qeg6`w3(R z#2=v24HQ-e+rm-&f$Tz%eIOkWS8+C{tAljx=s@*3vMN+pF)%dPL+oK_0I7ndI1n3_ z!a;19pFnEiIv7AQ$U0!EK;j5j6+=quhN6n%{h&Air8rQj5Ap}B&H%Atq0YciYCdlt zQ^bZXg;;dJRDnza#SJJn85s6T`|odC&SWA{DP|7}QBVm0sxv_G5Ap}7R0FX=E(E0* z28Mmo{`*0`!KMSI3S<%@CGM8?x9?ouWFl25W)Jcq$Xrkf018o%eV`Ht#0KdArD+C+ zJ<|U6ED;7+b-+}COaj>lN-^Mi(9R_N@|OIH;{Bj7frSq!C4#~Ylv6=$kk3J>l7XSl ze4ZW1AJ}xjRDn!_g&Rn11IXtMAQmW0K>mP*4=6T4X$d3_ia}UP#H|CS3Zw(%4@l^i zL0wf?QEU$i6Il3w@;R&~1F>Owo`Ip*e4ZVD#0C>=I$)|mCc(lD61vUHnYKt&itPu5 z2`qd-Aqw&bsN@8(K`sRO37kso*&+VT;NnFI?rupRqbmN#vYsubG~@*&7vkXfMA z2eJ>8jzMgY4oFJe4N6xL8?fqtsREewwjwAaPi22a0KM41yR8AQ>b6L|c7m9R;%wB#u=_14ss02TT=6 z99H{)LwEmkjn&5Dm16dwI>UiK(H2%mA?sG-PuvTt9}#&Fn+}*NkV&xGj)9>DQkF1) zQVFPza^+971+_mxs$lkk*a#haKy5>ixv=&w z%sx~dl@J|ZSE1{GnF|sJ*#|NS9D|^8w*gd^fZ9}`@X?00MPc?~)d5N+_UJlb=7PjQ z;Rb5eg8YmqsX;QJc-Dfp6Jhp&V$O9qe(vJRLkkT@v*K`l;j4BEfYSiM=iQfxn{ zod`;=FKrj?1F1sRjj99WHZd$ZV5&eSfzmO^BydX~T*85BgPR#^R0JS#4sKJ!>?2+W z%v|JFCIiEkcb;^I+rJ*bL%8RM(RsA~Gc4g?wz5g<9+J3z?-?)P9 z!(NxOT&4^R9~vF(CiE8V53X+6C%^ZZ(TdQOd!NU$?fb>|#P;-E$GxV)7W?$BaNB{{ zb3#=2F)+Mubg=91E!rQBrUO|OvfT^}%acDFPwFk&zvSRzJ7ky4maDh9d)IOAxsTy? zeDd{s?m^iM46UE_jVJaN?O%wd16h^!*SUMwgs$BCJ>#Y=1H-kwv-eHzE!uxK^OP;J z%-$l8eGl(C?)5sg*tW?xXy5(2j(hW+{cIT+ZbEfDN7I3!>g{A{p6>_LE zFla>UuT&44Z31dFf#RTPd587ckKuMg^7VTvp={)kW?*o$iYPI&>Ydl!2UL7#MEJ zSnYEyDc*lpqyZ()kz)-tNBxtrvU4ga-hV-)0VPKv=Lu9DnnI z1m~z%n_~8TK9rB416dVv7&3tCjn#|}#-PwePJPH}7sP(Gyu-Q!oCotNitW{+*PFDP z&$C0<0aLZJ$iwd5@($~8aB5es6tn*lR=s75wEuqO5(l}w0A7 z{z=P?t}gGeeiqBNkAdORrWpG#o2-ltcQV=|=N#l%1F=E*<}WxtKso}h%bOTs(E(Eh zG6|NO85mwqvbVp`B553VUUNSv++pPha!TAX<-hH-- z#bWYvb=H1R+=9#n)l0~6gBrTNGtcbvyFPylC_a#NAgcn2!{VQT;pV28{dFI<7;Efg zv`5Yp$R!+d`N+U|$VeG|3*+$6-jrq64N1WYV9SLXZ z_NU$H-lB&^2TT=m3C_T9d{d0wwIxv|Cx2JkgVHFhWr3W7QFHCNO)+-8{dW`{eGPEua=8tThShrC9JM+Wv!%epvG-+CoNar=Mp6>rix_grWn~ zYlNu+wRd4{UvP^IW$aZ7VitH5$I^4Q&F%Ik@F=I9=WH9uC{Bs_ynQz5i1A&R4p0vp zrV7NCI#8IaqLsh z_M_-PR)riQ3=C7M^TBo~I8WMw+_D7qOp#rPaFv_&4$#<=eOgx^iVkGAgW967b}0jc z$V?fq9f$AMf?Wk`6@&bt#-C{G0v*4C`H6vHo$3s*j)S+$F?7IGA%_S9!@RT8z^>|f z-M0nVc4Su}+sD8ly^?tYs1>7r5>*GXDv%v8*D^53Af+q!1KImPJ_PklL49&i_%K07 z@nGS`z@UVbu8?&ghc0rcGcd@{xx60asvXh2AfN98*$(OngL?6xu=0YAs=@4IU@(Km zc@iYf;X06Ag={wiL)YQu;MBek5+D0O=?c^n2H61$T~MDK#D>|&z_9g-HCV@PNPM8^ zfT;rYgki2_VE9sc2^@n?!tGl?y-`pe1oebLV+)`>_!~N&2(yoYfkXc^*bd(PttdJ` zE`g~6*#UDc1A{;fBJCpQPUKvRY##$d0yOO=clB*S)`6@F*|p%&T1`-V+)nh}0!mA; zoC=CTSUv}ZG)NpW;`P82JX&jiq$*_#vJA){KAy8}K%-nBcAV#I8_+lkc*D`fvMFmP1zfPKELBLT(d=zap(0r3Z@PYVhwgKgm`{y=si z$Ucw`h^x4p)0cyE?Ce1GIkGBLSAkSBFff4o=b*5Hr3?@oWG=`hAU4QPppXW+2|fk@ z9^XaQ0aFFi0ZYdW3}xo?_L-+&hL631(krOWKn`7GKQS;=LdIl3<0atnU6>AJRUkI3 zE`f{`wJmQl0gd~Bdi2BjrXodF63kPAU^1JTj4oC!k*vMLZ8 zkJiF`2=Xl~y&|V$28Jf{d3L5qa@=V{j_j-?qGIi!^k67i1PFOpsN9*dV(>xd1*= zWPf`DWTa?u=t^5nF}1zW9;EGonZHdyEwc5YD{X&)`ya0w9qf90i|mu3<30;QSK7Xe zW!nepO~7=6TKYThI@-u9?W9J&u7<39Gs zq2oTVI6#gyP>cNLT}Rt6u&baWMfOM0bReq&_3p0Tb+l~(``qciw5A5qNYSxn9lJpz zMaUtF9Ma&CBCI-)Re^d)d!Z&VFbGtN?e{F}+zcD{v0u@#8#Gdc>|5kez#LnG=|ENm zV#luN*bPZnpfT5)J-1EDp(8~S%R6?1Mv9PQ4LLR;V@tmoKNzQ@=|ENmVw*4T*bN$I zVqmx~W3|t@q}cv8bfgFt=g2tWI_IhGXL--nJA!BPgY4&>a$z_1B2Qe?jm zI#L9)137;n>tb?9 zH)yN}H6{AbJYy%3+6Nw)K-Ph*3dBZ~44`qJ z$+@y7u#qBINe0S;u>1&$&EVx7yAdNr=4?Fhks_E5WK|$GtaOEp6r~IMZGnyZz(Nre zqOi~fjmdDW=-3SzDS9)>-ahF4>Mb$Qks_E5WL2nX7c}mp;OMtm6*}$%E3c4sAjb`4 zY{|R*gON0v4rEmzHY{ct7(nAbQqa&vjwuiu7SG6W!@%G<^UOYbmDgKfV@t5OMOK9z zPmpn+q+Od$VB%ex?CK2eK*<8&Ru)#(lmlUTLBW9ruCNETFL-Sc!^k9|HquY)PU- z!5B8S1k-`63dDxFmVp5@?z8Jv8+@b)mf}FE50+k$^E?B?u}v{{OG;M4M~YxNkX0e) te{dUaUy=E|C7{^= pymandos.LossFunctionAndGradients: + loss = pymandos.LossFunctionAndGradients() + TARGET = np.array([0.0, 0.0, -2.0]) + lastPos = trajectory.positions[-1][3:] + delta = lastPos - TARGET + loss_value = 0.5 * np.dot(delta, delta) + loss_position_gradient = delta + + dof = trajectory.get_n_dof() + loss.loss = loss_value + loss_position_partial_derivative = [np.zeros(dof) for _ in trajectory.positions] + loss_position_partial_derivative[-1][3:] = loss_position_gradient + loss.loss_position_partial_derivative = loss_position_partial_derivative + loss.loss_velocity_partial_derivative = [ + np.zeros(dof) for _ in trajectory.positions + ] + loss.loss_parameter_partial_derivative = np.zeros(2) + return loss + + +def diff_sim_wrapper(K: float, L0: float): + model, deformable = create_model(K, L0) + mass_spring = deformable.mass_spring + diff_stiffness = deformable.diff(mass_spring.stiffness) + diff_rest_length = deformable.diff(mass_spring.rest_length) + diff_stiffness.indices = [0] + diff_rest_length.indices = [0] + + diff_parameters = pymandos.DiffParameters([diff_rest_length, diff_stiffness]) + + trajectory = simulate(model, 100) + + loss = compute_loss(trajectory) + + be = pymandos.BackwardEngine(stepParameters.h, model, trajectory) + result = be.backward_simulation(loss, diff_parameters) + # result = pymandos.compute_loss_function_gradient_backpropagation( + # stepParameters.h, model, trajectory, loss, diff_parameters, 10 + # ) + return loss.loss, result.loss_parameter_gradient + + +if __name__ == "__main__": + Ks = np.linspace(100.0, 1000.0, 100) + dk = Ks[1] - Ks[0] + Ks_loss = [] + Ks_diff_grad = [] + for K in Ks: + L0 = 1.0 + loss, grad = diff_sim_wrapper(K, L0) + Ks_loss.append(loss) + Ks_diff_grad.append(grad[1]) + + L0s = np.linspace(0.5, 2.0, 100) + dL0 = L0s[1] - L0s[0] + L0s_loss = [] + L0s_diff_grad = [] + for L0 in L0s: + K = 500.0 + loss, grad = diff_sim_wrapper(K, L0) + L0s_loss.append(loss) + L0s_diff_grad.append(grad[0]) + + Ks_finite_grad = np.gradient(Ks_loss, dk) + L0s_finite_grad = np.gradient(L0s_loss, dL0) + + plt.plot(Ks, Ks_diff_grad, label="K diff") + plt.plot(Ks, Ks_finite_grad, label="K finite") + plt.title("Stiffness") + plt.legend() + plt.grid() + plt.show() + + plt.plot(L0s, L0s_diff_grad, label="L0 diff") + plt.plot(L0s, L0s_finite_grad, label="L0 finite") + plt.title("Rest Length") + + plt.legend() + plt.grid() + plt.show() diff --git a/examples/python/diff/parallel_transport.py b/examples/python/diff/parallel_transport.py new file mode 120000 index 00000000..4a543afe --- /dev/null +++ b/examples/python/diff/parallel_transport.py @@ -0,0 +1 @@ +../Rod/parallel_transport.py \ No newline at end of file diff --git a/examples/python/diff/path.obj b/examples/python/diff/path.obj new file mode 100644 index 00000000..b6ac4ceb --- /dev/null +++ b/examples/python/diff/path.obj @@ -0,0 +1,144 @@ +# Blender 4.2.3 LTS +# www.blender.org +o heart +v -0.000000 0.002503 -0.086708 +v -0.000000 0.105731 -0.006437 +v 0.000000 0.208202 0.074797 +v 0.000000 0.309683 0.157265 +v 0.000000 0.409800 0.241385 +v 0.000000 0.508513 0.327147 +v 0.000000 0.605755 0.414575 +v 0.000000 0.701292 0.503862 +v 0.000000 0.794844 0.595224 +v 0.000000 0.886121 0.688863 +v 0.000000 0.974755 0.785006 +v 0.000000 1.060277 0.883928 +v 0.000000 1.142082 0.985945 +v 0.000000 1.219395 1.091407 +v 0.000000 1.291197 1.200695 +v 0.000000 1.356142 1.314192 +v 0.000000 1.412423 1.432226 +v 0.000000 1.457647 1.554923 +v 0.000000 1.488793 1.681926 +v 0.000000 1.502381 1.811984 +v 0.000000 1.494933 1.942542 +v 0.000000 1.464410 2.069695 +v 0.000000 1.412512 2.189716 +v 0.000000 1.344376 2.301328 +v 0.000000 1.264244 2.404668 +v 0.000001 1.172308 2.497667 +v 0.000001 1.066529 2.574554 +v 0.000001 0.947043 2.627692 +v 0.000001 0.818418 2.651230 +v 0.000001 0.687830 2.644441 +v 0.000001 0.561192 2.611872 +v 0.000001 0.440989 2.560351 +v 0.000001 0.327882 2.494701 +v 0.000000 0.220513 2.420358 +v 0.000000 0.121802 2.332839 +v 0.000000 0.000000 2.289556 +v 0.000000 -0.121802 2.332839 +v 0.000000 -0.220513 2.420358 +v 0.000001 -0.327882 2.494701 +v 0.000001 -0.440989 2.560351 +v 0.000001 -0.561192 2.611872 +v 0.000001 -0.687830 2.644441 +v 0.000001 -0.818418 2.651230 +v 0.000001 -0.947043 2.627692 +v 0.000001 -1.066529 2.574554 +v 0.000001 -1.172308 2.497667 +v 0.000000 -1.264244 2.404668 +v 0.000000 -1.344376 2.301328 +v 0.000000 -1.412512 2.189716 +v 0.000000 -1.464410 2.069695 +v 0.000000 -1.494933 1.942542 +v 0.000000 -1.502381 1.811984 +v 0.000000 -1.488793 1.681926 +v 0.000000 -1.457647 1.554923 +v 0.000000 -1.412423 1.432226 +v 0.000000 -1.356142 1.314192 +v 0.000000 -1.291197 1.200695 +v 0.000000 -1.219395 1.091407 +v 0.000000 -1.142082 0.985945 +v 0.000000 -1.060277 0.883928 +v 0.000000 -0.974755 0.785006 +v 0.000000 -0.886121 0.688863 +v 0.000000 -0.794844 0.595224 +v 0.000000 -0.701292 0.503862 +v 0.000000 -0.605755 0.414575 +v 0.000000 -0.508513 0.327147 +v 0.000000 -0.409800 0.241385 +v 0.000000 -0.309683 0.157265 +v 0.000000 -0.208202 0.074797 +v -0.000000 -0.105731 -0.006437 +v -0.000000 -0.002503 -0.086708 +l 1 2 +l 2 3 +l 3 4 +l 4 5 +l 5 6 +l 6 7 +l 7 8 +l 8 9 +l 9 10 +l 10 11 +l 11 12 +l 12 13 +l 13 14 +l 14 15 +l 15 16 +l 16 17 +l 17 18 +l 18 19 +l 19 20 +l 20 21 +l 21 22 +l 22 23 +l 23 24 +l 24 25 +l 25 26 +l 26 27 +l 27 28 +l 28 29 +l 29 30 +l 30 31 +l 31 32 +l 32 33 +l 33 34 +l 34 35 +l 35 36 +l 36 37 +l 37 38 +l 38 39 +l 39 40 +l 40 41 +l 41 42 +l 42 43 +l 43 44 +l 44 45 +l 45 46 +l 46 47 +l 47 48 +l 48 49 +l 49 50 +l 50 51 +l 51 52 +l 52 53 +l 53 54 +l 54 55 +l 55 56 +l 56 57 +l 57 58 +l 58 59 +l 59 60 +l 60 61 +l 61 62 +l 62 63 +l 63 64 +l 64 65 +l 65 66 +l 66 67 +l 67 68 +l 68 69 +l 69 70 +l 70 71 diff --git a/examples/python/diff/path3D.obj b/examples/python/diff/path3D.obj new file mode 100644 index 00000000..3a899ae5 --- /dev/null +++ b/examples/python/diff/path3D.obj @@ -0,0 +1,140 @@ +# Blender 4.3.0 +# www.blender.org +o Cube +v 0.000000 0.000000 0.000000 +v 0.036297 -0.052383 0.112347 +v 0.063940 -0.100782 0.227916 +v 0.077068 -0.142206 0.350061 +v 0.072088 -0.176667 0.473178 +v 0.059729 -0.207239 0.598956 +v 0.023901 -0.233019 0.718385 +v -0.024420 -0.254069 0.837765 +v -0.084876 -0.277749 0.946844 +v -0.165776 -0.301407 1.046804 +v -0.251296 -0.327932 1.136084 +v -0.365163 -0.363320 1.189464 +v -0.475636 -0.397646 1.246341 +v -0.596672 -0.435178 1.257351 +v -0.720867 -0.480804 1.254520 +v -0.843279 -0.518224 1.260383 +v -0.970816 -0.523605 1.258208 +v -1.103555 -0.537909 1.250706 +v -1.223580 -0.512173 1.254699 +v -1.338653 -0.451084 1.268585 +v -1.404889 -0.341060 1.277267 +v -1.450587 -0.223350 1.270069 +v -1.481484 -0.098355 1.251346 +v -1.493234 0.025656 1.221036 +v -1.501600 0.150062 1.185914 +v -1.487850 0.274785 1.157478 +v -1.470419 0.399488 1.129680 +v -1.421032 0.516752 1.106093 +v -1.368550 0.630970 1.081742 +v -1.284233 0.725719 1.053904 +v -1.203350 0.822942 1.026316 +v -1.091243 0.876814 1.000480 +v -0.972077 0.926313 0.973272 +v -0.854371 0.965687 0.950071 +v -0.725687 0.965112 0.935954 +v -0.595692 0.964328 0.921457 +v -0.468102 0.966234 0.906917 +v -0.340436 0.949936 0.898529 +v -0.212850 0.932615 0.890251 +v -0.088080 0.903063 0.881300 +v 0.031482 0.855185 0.875183 +v 0.151769 0.807198 0.873980 +v 0.261679 0.745835 0.854856 +v 0.374901 0.681006 0.833935 +v 0.483546 0.616095 0.811139 +v 0.544112 0.524786 0.745846 +v 0.604196 0.431318 0.678388 +v 0.606821 0.338457 0.589948 +v 0.606929 0.245291 0.500390 +v 0.582232 0.161450 0.405617 +v 0.558068 0.076974 0.310708 +v 0.509780 0.007496 0.213976 +v 0.461503 -0.063055 0.115904 +v 0.390575 -0.113813 0.023850 +v 0.308472 -0.158061 -0.068700 +v 0.225175 -0.196829 -0.156348 +v 0.115396 -0.199412 -0.223552 +v 0.004443 -0.207250 -0.296541 +v -0.106021 -0.195167 -0.349821 +v -0.226865 -0.149793 -0.376586 +v -0.349050 -0.112109 -0.411665 +v -0.439199 -0.032645 -0.387086 +v -0.534842 0.057042 -0.355776 +v -0.526298 0.145240 -0.264390 +v -0.472293 0.201320 -0.177590 +v -0.372794 0.223720 -0.096880 +v -0.252350 0.195666 -0.053216 +v -0.139933 0.143838 -0.037591 +v -0.030445 0.074339 -0.041009 +l 1 2 +l 2 3 +l 3 4 +l 4 5 +l 5 6 +l 6 7 +l 7 8 +l 8 9 +l 9 10 +l 10 11 +l 11 12 +l 12 13 +l 13 14 +l 14 15 +l 15 16 +l 16 17 +l 17 18 +l 18 19 +l 19 20 +l 20 21 +l 21 22 +l 22 23 +l 23 24 +l 24 25 +l 25 26 +l 26 27 +l 27 28 +l 28 29 +l 29 30 +l 30 31 +l 31 32 +l 32 33 +l 33 34 +l 34 35 +l 35 36 +l 36 37 +l 37 38 +l 38 39 +l 39 40 +l 40 41 +l 41 42 +l 42 43 +l 43 44 +l 44 45 +l 45 46 +l 46 47 +l 47 48 +l 48 49 +l 49 50 +l 50 51 +l 51 52 +l 52 53 +l 53 54 +l 54 55 +l 55 56 +l 56 57 +l 57 58 +l 58 59 +l 59 60 +l 60 61 +l 61 62 +l 62 63 +l 63 64 +l 64 65 +l 65 66 +l 66 67 +l 67 68 +l 68 69 diff --git a/examples/python/diff/path3D2.obj b/examples/python/diff/path3D2.obj new file mode 100644 index 00000000..4b31d12e --- /dev/null +++ b/examples/python/diff/path3D2.obj @@ -0,0 +1,132 @@ +# Blender 4.3.0 +# www.blender.org +o Cube +v 0.000000 0.000000 0.000000 +v -0.031514 0.069401 0.026450 +v -0.060592 0.139822 0.052956 +v -0.088421 0.210741 0.079498 +v -0.119891 0.280205 0.105995 +v -0.160889 0.344616 0.132057 +v -0.213751 0.400195 0.157188 +v -0.273440 0.448470 0.182228 +v -0.334904 0.493890 0.208012 +v -0.395498 0.540699 0.233439 +v -0.455180 0.590820 0.254529 +v -0.514346 0.644019 0.267550 +v -0.573280 0.698892 0.273459 +v -0.632146 0.753934 0.277107 +v -0.691114 0.808722 0.281713 +v -0.749693 0.864391 0.283219 +v -0.805652 0.921646 0.273949 +v -0.856323 0.979836 0.250114 +v -0.902563 1.038154 0.218913 +v -0.947995 1.096856 0.187433 +v -0.994530 1.153736 0.153880 +v -1.040785 1.204326 0.111570 +v -1.084175 1.247739 0.059050 +v -1.124326 1.289379 0.002842 +v -1.164163 1.331801 -0.052937 +v -1.208906 1.365746 -0.111307 +v -1.259597 1.375083 -0.172834 +v -1.311956 1.365517 -0.233824 +v -1.364136 1.353599 -0.293931 +v -1.415969 1.337947 -0.354214 +v -1.462101 1.301604 -0.408699 +v -1.499305 1.244111 -0.451979 +v -1.533995 1.182238 -0.490148 +v -1.568110 1.120636 -0.529730 +v -1.591723 1.054398 -0.568977 +v -1.598943 0.981948 -0.603910 +v -1.599865 0.908010 -0.636096 +v -1.600864 0.834234 -0.668793 +v -1.595801 0.760819 -0.701745 +v -1.580859 0.688605 -0.734487 +v -1.561987 0.617658 -0.767938 +v -1.545377 0.545212 -0.799328 +v -1.532785 0.469171 -0.823015 +v -1.521747 0.391608 -0.842307 +v -1.511052 0.313751 -0.860711 +v -1.508045 0.233481 -0.862590 +v -1.514257 0.154463 -0.847254 +v -1.521392 0.076153 -0.829000 +v -1.528788 0.005581 -0.791528 +v -1.536959 -0.054030 -0.737581 +v -1.545136 -0.112928 -0.683136 +v -1.554079 -0.159654 -0.618371 +v -1.563294 -0.177877 -0.540111 +v -1.571965 -0.189586 -0.461044 +v -1.581183 -0.197035 -0.380708 +v -1.590894 -0.165849 -0.308291 +v -1.599622 -0.110720 -0.250006 +v -1.608202 -0.052829 -0.194124 +v -1.614563 0.021095 -0.164934 +v -1.618652 0.101720 -0.161372 +v -1.622560 0.182311 -0.159061 +v -1.622989 0.261705 -0.171392 +v -1.618445 0.334280 -0.206499 +v -1.612715 0.403842 -0.246940 +v -1.607770 0.475408 -0.283836 +l 1 2 +l 2 3 +l 3 4 +l 4 5 +l 5 6 +l 6 7 +l 7 8 +l 8 9 +l 9 10 +l 10 11 +l 11 12 +l 12 13 +l 13 14 +l 14 15 +l 15 16 +l 16 17 +l 17 18 +l 18 19 +l 19 20 +l 20 21 +l 21 22 +l 22 23 +l 23 24 +l 24 25 +l 25 26 +l 26 27 +l 27 28 +l 28 29 +l 29 30 +l 30 31 +l 31 32 +l 32 33 +l 33 34 +l 34 35 +l 35 36 +l 36 37 +l 37 38 +l 38 39 +l 39 40 +l 40 41 +l 41 42 +l 42 43 +l 43 44 +l 44 45 +l 45 46 +l 46 47 +l 47 48 +l 48 49 +l 49 50 +l 50 51 +l 51 52 +l 52 53 +l 53 54 +l 54 55 +l 55 56 +l 56 57 +l 57 58 +l 58 59 +l 59 60 +l 60 61 +l 61 62 +l 62 63 +l 63 64 +l 64 65 diff --git a/examples/python/diff/path3D3.obj b/examples/python/diff/path3D3.obj new file mode 100644 index 00000000..55588733 --- /dev/null +++ b/examples/python/diff/path3D3.obj @@ -0,0 +1,76 @@ +# Blender 4.3.0 +# www.blender.org +o Cube +v 0.000227 0.001023 0.001297 +v -0.045577 0.256280 -0.034426 +v -0.095936 0.529891 -0.050820 +v -0.152528 0.760730 0.008682 +v -0.212763 0.903536 0.156230 +v -0.278071 0.921890 0.390069 +v -0.344134 0.750173 0.668336 +v -0.399620 0.397500 0.894783 +v -0.451788 -0.000000 1.000000 +v -0.514288 -0.382684 0.923880 +v -0.576788 -0.707107 0.707107 +v -0.639288 -0.923880 0.382683 +v -0.701788 -1.000000 0.000000 +v -0.764289 -0.923880 -0.382683 +v -0.826789 -0.707107 -0.707107 +v -0.889289 -0.382683 -0.923879 +v -0.951789 0.000000 -1.000000 +v -1.014289 0.382684 -0.923879 +v -1.076789 0.707107 -0.707106 +v -1.139289 0.923880 -0.382683 +v -1.201788 1.000000 0.000001 +v -1.264288 0.923879 0.382684 +v -1.326788 0.707107 0.707107 +v -1.389288 0.382683 0.923880 +v -1.451788 -0.000000 1.000000 +v -1.514288 -0.382684 0.923880 +v -1.576788 -0.707107 0.707107 +v -1.639288 -0.923880 0.382684 +v -1.701788 -1.000000 0.000001 +v -1.764289 -0.923879 -0.382683 +v -1.826789 -0.707107 -0.707105 +v -1.889289 -0.382683 -0.923878 +v -1.951789 0.000001 -0.999998 +v -2.014289 0.382683 -0.923878 +v -2.076789 0.707107 -0.707105 +v -2.139289 0.923880 -0.382682 +v -2.201788 1.000000 0.000002 +l 1 2 +l 2 3 +l 3 4 +l 4 5 +l 5 6 +l 6 7 +l 7 8 +l 8 9 +l 9 10 +l 10 11 +l 11 12 +l 12 13 +l 13 14 +l 14 15 +l 15 16 +l 16 17 +l 17 18 +l 18 19 +l 19 20 +l 20 21 +l 21 22 +l 22 23 +l 23 24 +l 24 25 +l 25 26 +l 26 27 +l 27 28 +l 28 29 +l 29 30 +l 30 31 +l 31 32 +l 32 33 +l 33 34 +l 34 35 +l 35 36 +l 36 37 diff --git a/examples/python/diff/path3D4.obj b/examples/python/diff/path3D4.obj new file mode 100644 index 00000000..e6459dd8 --- /dev/null +++ b/examples/python/diff/path3D4.obj @@ -0,0 +1,100 @@ +# Blender 4.3.0 +# www.blender.org +o BézierCurve +v -0.002326 -0.000695 0.000528 +v -0.101366 0.010909 0.111275 +v -0.155608 0.043302 0.195810 +v -0.175739 0.092854 0.257572 +v -0.172440 0.155934 0.300001 +v -0.156399 0.228912 0.326537 +v -0.138298 0.308158 0.340618 +v -0.128823 0.390042 0.345686 +v -0.138658 0.470934 0.345180 +v -0.178488 0.547203 0.342539 +v -0.258997 0.615220 0.341203 +v -0.390869 0.671353 0.344611 +v -0.584791 0.711974 0.356204 +v -0.796250 0.746901 0.351409 +v -0.943785 0.780854 0.306253 +v -1.038063 0.810227 0.229154 +v -1.089756 0.831413 0.128531 +v -1.109532 0.840807 0.012803 +v -1.108060 0.834803 -0.109612 +v -1.096010 0.809795 -0.230295 +v -1.084051 0.762178 -0.340828 +v -1.082853 0.688344 -0.432791 +v -1.103085 0.584690 -0.497767 +v -1.155416 0.447608 -0.527335 +v -1.250516 0.273493 -0.513079 +v -1.343710 0.083411 -0.453657 +v -1.372964 -0.069032 -0.364362 +v -1.352023 -0.185642 -0.252274 +v -1.294632 -0.268228 -0.124470 +v -1.214535 -0.318594 0.011970 +v -1.125478 -0.338549 0.149968 +v -1.041204 -0.329898 0.282443 +v -0.975458 -0.294447 0.402318 +v -0.941986 -0.234005 0.502513 +v -0.954531 -0.150376 0.575950 +v -1.026838 -0.045368 0.615550 +v -1.172653 0.079212 0.614233 +v -1.356126 0.230514 0.579109 +v -1.505311 0.390694 0.526910 +v -1.625198 0.552900 0.459145 +v -1.720775 0.710281 0.377318 +v -1.797031 0.855982 0.282937 +v -1.858954 0.983152 0.177507 +v -1.911533 1.084939 0.062534 +v -1.959757 1.154490 -0.060474 +v -2.008613 1.184953 -0.190011 +v -2.063090 1.169474 -0.324572 +v -2.128178 1.101203 -0.462650 +v -2.208864 0.973286 -0.602738 +l 1 2 +l 2 3 +l 3 4 +l 4 5 +l 5 6 +l 6 7 +l 7 8 +l 8 9 +l 9 10 +l 10 11 +l 11 12 +l 12 13 +l 13 14 +l 14 15 +l 15 16 +l 16 17 +l 17 18 +l 18 19 +l 19 20 +l 20 21 +l 21 22 +l 22 23 +l 23 24 +l 24 25 +l 25 26 +l 26 27 +l 27 28 +l 28 29 +l 29 30 +l 30 31 +l 31 32 +l 32 33 +l 33 34 +l 34 35 +l 35 36 +l 36 37 +l 37 38 +l 38 39 +l 39 40 +l 40 41 +l 41 42 +l 42 43 +l 43 44 +l 44 45 +l 45 46 +l 46 47 +l 47 48 +l 48 49 diff --git a/examples/python/diff/rod_arm.py b/examples/python/diff/rod_arm.py new file mode 100644 index 00000000..17e31ab5 --- /dev/null +++ b/examples/python/diff/rod_arm.py @@ -0,0 +1,291 @@ +#!/usr/bin/env python3 + +import numpy as np +import polyscope as ps +import scipy +import meshio +from tqdm import tqdm +import matplotlib.pyplot as plt + +import pymandos +import save_transforms +from parallel_transport import compute_rotations_parallel_transport + +ENABLE_RENDER = True + +# ROD +total_length = 0.5 # meters +total_mass = 0.15 # Kg +N_POINTS = 15 +N_SEGMENTS = N_POINTS - 1 + + +TOTAL_FRAMES = 200 +TOTAL_NUM_STATES = TOTAL_FRAMES + 1 + +# CURVE +curve = meshio.read("path3D3.obj") +points = np.array(curve.points) +segments = curve.get_cells_type("line") + +# Take the first point of the path and make it the origin +offset = points[0] +points = points - offset +# Scale the curve +lengths = np.linalg.norm(points, axis=1) +scale = 0.4 / np.max(lengths) +print(f"scale = {scale}") +points = scale * points + +# Bring the coin to the end of the rod +offset = np.array([-total_length, 0.0, 0.0]) +points = points - offset +N_CURVE_POINTS = points.shape[0] +print(N_CURVE_POINTS) + + +def sample_curve(i: int): + s = float(i) / float(TOTAL_NUM_STATES) + index = int(s * (N_CURVE_POINTS - 1)) + ss = s * (N_CURVE_POINTS - 1) - index + + # Linear interpolation between index and index+1 + + if index + 1 < N_CURVE_POINTS: + pA = points[index] + pB = points[index + 1] + else: + print(f"Exceeded total states: frame {i} / {TOTAL_NUM_STATES}") + pA = points[-1] + pB = pA.copy() + return (1.0 - ss) * pA + ss * pB + + +def coin_transform(i: int): + time = i * stepParameters.h + mat = np.zeros((4, 4)) + S = 0.1 * np.identity(3) + R = scipy.spatial.transform.Rotation.from_rotvec([0.0, 0.0, 10 * time]).as_matrix() + mat[:3, :3] = S @ R + mat[:3, 3] = sample_curve(i) + mat[3, 3] = 1.0 + return mat + + +stepParameters = pymandos.StepParameters() +stepParameters.h = 0.01 +stepParameters.newton_iterations = 300 +stepParameters.cg_iterations = 20 +stepParameters.cg_error = 1e-8 +stepParameters.grad_norm = 1e-4 +stepParameters.line_search_iterations = 0 + + +def create_rod(model): + delta_x = total_length / N_SEGMENTS + rigid_body_cloud = model.add_rigidbody_cloud() + + # Set initial conditions + positions = np.zeros((N_POINTS, 6)) + velocities = np.zeros((N_POINTS, 6)) + for i, position in enumerate(positions): + position[0] += delta_x * i + + lin_pos = np.array([pos[:3] for pos in positions]) + rotvecs = compute_rotations_parallel_transport(lin_pos) + for rotvec, pos in zip(rotvecs, positions): + pos[3:] = rotvec + rigid_body_cloud.size = N_POINTS + rigid_body_cloud.x = positions + rigid_body_cloud.v = velocities + + rigid_body_cloud.disable_gravity() + + # Set up rigid body mass and inertia + node_mass = total_mass / N_POINTS + rigid_body_cloud.mass = node_mass * np.ones(N_POINTS) + rigid_body_cloud.inertiaTensor = [ + node_mass * np.diag([1.0, 1.0, 1.0]) for _ in range(N_POINTS) + ] + + # Rod energy + cosserat_rod = rigid_body_cloud.cosserat_rod + Ks = [500.0 for _ in range(N_POINTS - 1)] + stiffness_tensor = [1.0 * np.ones(3) for _ in range(N_POINTS - 2)] + cosserat_stiffness = [50.0 for _ in range(N_POINTS - 1)] + cosserat_rod.set_parameters(Ks, cosserat_stiffness, stiffness_tensor) + + # Freeze one side + rigid_body_cloud.fix_translation(0) + rigid_body_cloud.fix_rotation(0) + + model.commit() + + # Render + radius = 1.5 * total_length / 10.0 + radius = 0.1 * total_length / 10.0 + + positions = np.array([x[:3] for x in rigid_body_cloud.x]) + indices = np.array([(i, i + 1) for i in range(N_POINTS - 1)]) + ps.register_curve_network("rod", positions, indices, radius=radius) + + return rigid_body_cloud + + +def simulate(model: pymandos.Model, rod, intrinsic_darboux_list) -> pymandos.Trajectory: + trajectory = pymandos.Trajectory() + trajectory.append_state(model) + for i in range(TOTAL_FRAMES): + rod.cosserat_rod.set_intrinsic_darboux( + intrinsic_darboux_list[i].reshape(N_SEGMENTS, 3) + ) + success = pymandos.step(model, stepParameters) + if not success: + print(i, success) + exit() + trajectory.append_state(model) + + # Render + if ENABLE_RENDER: + positions = np.array([x[:3] for x in rod.x]) + ps.get_curve_network("rod").update_node_positions(positions) + ps.get_surface_mesh("coin_mesh").set_transform(coin_transform(i)) + if ps.window_requests_close(): + exit() + ps.frame_tick() + return trajectory + + +def compute_loss(trajectory: pymandos.Trajectory) -> pymandos.LossFunctionAndGradients: + loss = pymandos.LossFunctionAndGradients() + dof = trajectory.get_n_dof() + + # Target position + positions = np.array(trajectory.positions) + positions = positions[:, -6:-3] + target = np.array([sample_curve(i) for i in range(TOTAL_NUM_STATES)]) + delta = positions - target + loss_value = np.trace(delta @ delta.T) + loss_pos_gradient = 2.0 * delta + + before_zeros = np.zeros((loss_pos_gradient.shape[0], dof - 6)) + after_zeros = np.zeros((loss_pos_gradient.shape[0], 3)) + loss_pos_gradient = np.hstack((before_zeros, loss_pos_gradient, after_zeros)) + + loss.loss = loss_value + zeros = np.array([np.zeros(dof) for _ in trajectory.positions]) + + loss.loss_position_partial_derivative = loss_pos_gradient + loss_velocity_partial_derivative = zeros.copy() + loss.loss_velocity_partial_derivative = loss_velocity_partial_derivative + return loss + + +def sim_wrapper(rest_lengths: list, gradient=True): + # print("Sim call!") + intrinsic_darboux_list = rest_lengths.reshape((TOTAL_FRAMES, 3 * N_SEGMENTS)) + model = pymandos.Model() + rod = create_rod(model) + + # Differentiable parameters + diff_intrinsic_darboux = rod.cosserat_rod.diff(rod.cosserat_rod.IntrinsicDarboux) + diff_intrinsic_darboux.indices = [i for i in range(N_SEGMENTS)] + diff_parameters = pymandos.DiffParameters([diff_intrinsic_darboux]) + + # Simulate and evaluate loss + trajectory = simulate(model, rod, intrinsic_darboux_list) + # save_transforms.save_rod_trajectory("helix_trajectory.npy", trajectory.positions) + loss = compute_loss(trajectory) + + if not gradient: + return loss.loss + + # Compute gradients + be = pymandos.BackwardEngine(stepParameters.h, model, trajectory) + dloss_dp = np.zeros((TOTAL_FRAMES, 3 * N_SEGMENTS)) + dloss_dx = loss.loss_position_partial_derivative[-1].copy() + dloss_dv = loss.loss_velocity_partial_derivative[-1].copy() + + # Backward loop + for i in range(TOTAL_FRAMES - 1, -1, -1): + dloss_dpi = np.zeros(3 * N_SEGMENTS) + be.backward_step(dloss_dx, dloss_dv, dloss_dpi, diff_parameters, 0) + dloss_dx += loss.loss_position_partial_derivative[i] + dloss_dv += loss.loss_velocity_partial_derivative[i] + dloss_dp[i] = dloss_dpi + + return loss.loss, dloss_dp.flatten() + + +frame = 0 +counter = 0 + + +def measure_gradients(intrinsic_darboux): + nP = len(intrinsic_darboux) + l, g = sim_wrapper(intrinsic_darboux) + dx = 1e-2 + gradient_f = np.zeros(nP) + for i in tqdm(range(0, nP, 4)): + intrinsic_darboux[i] += dx + dl = sim_wrapper(intrinsic_darboux, gradient=False) + intrinsic_darboux[i] -= dx + + gradient_f[i] = (dl - l) / dx + + # Plotting + bp = np.array([g[i] for i in range(0, nP, 4)]) + fd = np.array([gradient_f[i] for i in range(0, nP, 4)]) + x = np.arange(0, nP / 4) + plt.title("Loss Gradient components") + plt.plot(x, bp, ".", label="BackPropagation") + plt.plot(x, fd, ".", label="FiniteDifferences") + # error = np.abs(bp - fd) + # plt.errorbar(x, bp, error) + plt.grid() + plt.legend() + global counter + counter += 1 + plt.show() + plt.savefig(f"output/fig_{counter}.png") + plt.clf() + return l, g + + +if __name__ == "__main__": + ps.init() + ps.set_up_dir("z_up") + ps.look_at((0.5 * total_length, 2.0, 0.5), (0.5 * total_length, 0.0, 0.0)) + ps.set_ground_plane_mode("none") # Disable ground rendering + + # Target Coin + coin = meshio.read("coin.stl") + ps.register_surface_mesh("coin_mesh", coin.points, coin.cells_dict.get("triangle")) + + indices = np.array([(i, i + 1) for i in range(len(points) - 1)]) + ps.register_curve_network("path", points, indices, radius=0.001) + + # Initial rest lengths + intrinsic_darboux = np.zeros((TOTAL_FRAMES, 3 * N_SEGMENTS)) + intrinsic_darboux = intrinsic_darboux.flatten() + + # measure_gradients(intrinsic_darboux) + # exit() + + # model = pymandos.Model() + # rod, mapping = create_rod(model) + # ps.set_user_callback(lambda: simulate_callback(model, rod, mapping)) + # ps.show() + # exit() + result = scipy.optimize.minimize( + sim_wrapper, + intrinsic_darboux, + method="L-BFGS-B", + jac=True, + options={"disp": True}, + ) + print(result) + + ENABLE_RENDER = True + while ps.window_requests_close(): + sim_wrapper(result.x) diff --git a/examples/python/diff/rod_to_curve.py b/examples/python/diff/rod_to_curve.py new file mode 100644 index 00000000..2b8c6ef4 --- /dev/null +++ b/examples/python/diff/rod_to_curve.py @@ -0,0 +1,347 @@ +#!/usr/bin/env python3 + +import numpy as np +import polyscope as ps +import scipy +import meshio +from tqdm import tqdm +import matplotlib.pyplot as plt + +import pymandos +import save_transforms +from parallel_transport import compute_rotations_parallel_transport + +ENABLE_RENDER = True + +# ROD +total_length = 0.5 # meters +total_mass = 0.15 # Kg +N_POINTS = 15 + +# CONTROL +N_CONTROL_POINTS = 3 +N_SPRINGS_PER_POINT = 4 +N_SPRINGS = (N_CONTROL_POINTS - 1) * 4 + +TOTAL_FRAMES = 250 +TOTAL_NUM_STATES = TOTAL_FRAMES + 1 +IGNORE_X_MATRIX = np.array([[0.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) + +# CURVE +curve = meshio.read("path.obj") +points = np.array(curve.points) +# Take the first point of the path and make it the origin +offset = points[0] +points = points - offset +# Scale the curve +lengths = np.linalg.norm(points, axis=1) +scale = 0.2 / np.max(lengths) +points = scale * points + +# Bring the coin to the end of the rod +offset = np.array([-total_length, 0.0, 0.0]) +points = points - offset +N_CURVE_POINTS = points.shape[0] + + +def sample_curve(i: int): + s = float(i) / float(TOTAL_NUM_STATES) + index = int(s * (N_CURVE_POINTS - 1)) + ss = s * (N_CURVE_POINTS - 1) - index + # Linear interpolation between index and index+1 + + if index + 1 < N_CURVE_POINTS: + pA = points[index] + pB = points[index + 1] + else: + print(f"Exceeded total states: frame {i} / {TOTAL_NUM_STATES}") + pA = points[-1] + pB = pA.copy() + return (1.0 - ss) * pA + ss * pB + + +stepParameters = pymandos.StepParameters() +stepParameters.h = 0.01 +stepParameters.newton_iterations = 300 +stepParameters.cg_iterations = 20 +stepParameters.cg_error = 1e-8 +stepParameters.grad_norm = 1e-4 +stepParameters.line_search_iterations = 0 + + +def coin_transform(i: int): + time = i * stepParameters.h + mat = np.zeros((4, 4)) + S = 0.1 * np.identity(3) + R = scipy.spatial.transform.Rotation.from_rotvec([0.0, 0.0, 10 * time]).as_matrix() + mat[:3, :3] = S @ R + mat[:3, 3] = sample_curve(i) + mat[3, 3] = 1.0 + return mat + + +def create_rod(model): + delta_x = total_length / N_POINTS + rigid_body_cloud = model.add_rigidbody_cloud() + + # Set initial conditions + positions = np.zeros((N_POINTS, 6)) + velocities = np.zeros((N_POINTS, 6)) + for i, position in enumerate(positions): + position[0] += delta_x * i + + lin_pos = np.array([pos[:3] for pos in positions]) + rotvecs = compute_rotations_parallel_transport(lin_pos) + for rotvec, pos in zip(rotvecs, positions): + pos[3:] = rotvec + rigid_body_cloud.size = N_POINTS + rigid_body_cloud.x = positions + rigid_body_cloud.v = velocities + + rigid_body_cloud.disable_gravity() + + # Set up rigid body mass and inertia + node_mass = total_mass / N_POINTS + rigid_body_cloud.mass = node_mass * np.ones(N_POINTS) + rigid_body_cloud.inertiaTensor = [ + node_mass * np.diag([1.0, 1.0, 1.0]) for _ in range(N_POINTS) + ] + + # Rod energy + N_POINTS + Ks = 500.0 + stiffness_tensor = 1.0 * np.ones(3) + cosserat_stiffness = 50.0 + stretch_rod = rigid_body_cloud.mass_spring + cosserat_rod = rigid_body_cloud.cosserat_rod_alignment + for i in range(N_POINTS-1): + stretch_rod.add_element([i, i+1], pymandos.energies.MassSpring.ParameterSet(x0 = rigid_body_cloud.x[[i, i+1], 0:3], stiffness=Ks)) + cosserat_rod.add_element([i, i+1], pymandos.energies.CosseratRodAlignment.ParameterSet(x0 = rigid_body_cloud.x[[i, i+1], :], cosserat_stiffness=cosserat_stiffness)) + + bending_rod = rigid_body_cloud.cosserat_bending_rod + for i in range(N_POINTS-2): + bending_rod.add_element([i, i+1], pymandos.energies.CosseratBendingRod.ParameterSet(x0 = rigid_body_cloud.x[[i, i+1], :], stiffness_tensor=stiffness_tensor)) + + # Mapping + radius = 1.5 * total_length / 10.0 + + mapping = model.add_rigidbody_point_mapping(rigid_body_cloud) + N = N_POINTS - 2 + indices = np.linspace(0, N, N_CONTROL_POINTS, dtype=int) + for index in indices: + mapping.add_particle([radius, 0.0, 0.0], index) # +x + mapping.add_particle([-radius, 0.0, 0.0], index) # -x + + mapping.add_particle([0.0, radius, 0.0], index) # +y + mapping.add_particle([0.0, -radius, 0.0], index) # -y + + mapping.deformable.particle_mass = np.zeros(mapping.deformable.size) + mass_spring = mapping.deformable.mass_spring + for i in range(len(indices) - 1): + for j in range(4): + edge = (4 * i + j, 4 * i + j + 4) + mass_spring.add_element(edge, pymandos.energies.MassSpring.ParameterSet(x0 = mapping.deformable.x[edge, :], stiffness = 400.0)) + + # Freeze one side + rigid_body_cloud.fix_translation(0) + rigid_body_cloud.fix_rotation(0) + + model.commit() + + # Render + positions = np.array([x[:3] for x in rigid_body_cloud.x]) + indices = np.array([(i, i + 1) for i in range(N_POINTS - 1)]) + ps.register_curve_network("rod", positions, indices, radius=radius) + + # ps.register_point_cloud("particles", mapping.deformable.x, radius=radius) + + return rigid_body_cloud, mapping + + +def update_rest_lengths(mass_spring, rest_lengths): + for i in range(mass_spring.size()): + parameters = mass_spring.get_parameter_set(i) + parameters.rest_length = rest_lengths[i] + mass_spring.set_parameter_set(i, parameters) + +def simulate( + model: pymandos.Model, rod, mass_spring, rest_lengths +) -> pymandos.Trajectory: + trajectory = pymandos.Trajectory() + trajectory.append_state(model) + for i in range(TOTAL_FRAMES): + update_rest_lengths(mass_spring, rest_lengths[i]) + success = pymandos.step(model, stepParameters) + if not success: + print(i, success) + exit() + trajectory.append_state(model) + + # Render + if ENABLE_RENDER: + positions = np.array([x[:3] for x in rod.x]) + ps.get_curve_network("rod").update_node_positions(positions) + ps.get_surface_mesh("coin_mesh").set_transform(coin_transform(i)) + if ps.window_requests_close(): + exit() + ps.frame_tick() + return trajectory + + +def compute_loss(trajectory: pymandos.Trajectory) -> pymandos.LossFunctionAndGradients: + loss = pymandos.LossFunctionAndGradients() + dof = trajectory.get_n_dof() + + # Target position + positions = np.array(trajectory.positions) + positions = positions[:, -6:-3] + target = np.array([sample_curve(i) for i in range(TOTAL_NUM_STATES)]) + delta = positions - target + loss_value = np.trace(delta @ IGNORE_X_MATRIX @ delta.T) + loss_pos_gradient = 2.0 * delta @ IGNORE_X_MATRIX + + before_zeros = np.zeros((loss_pos_gradient.shape[0], dof - 6)) + after_zeros = np.zeros((loss_pos_gradient.shape[0], 3)) + loss_pos_gradient = np.hstack((before_zeros, loss_pos_gradient, after_zeros)) + + loss.loss = loss_value + zeros = np.array([np.zeros(dof) for _ in trajectory.positions]) + + loss.loss_position_partial_derivative = loss_pos_gradient + loss_velocity_partial_derivative = zeros.copy() + loss.loss_velocity_partial_derivative = loss_velocity_partial_derivative + return loss + + +op_trajectories = [] +def sim_wrapper(rest_lengths: list, gradient=True): + print("Sim call!") + rest_lengths = rest_lengths.reshape((TOTAL_FRAMES, N_SPRINGS)) + model = pymandos.Model() + rod, mapping = create_rod(model) + + # Differentiable parameters + mass_spring = mapping.deformable.mass_spring + diff_rest_lengths = mapping.deformable.diff(mass_spring.rest_length) + diff_rest_lengths.indices = [i for i in range(N_SPRINGS)] + diff_parameters = pymandos.DiffParameters([diff_rest_lengths]) + + # Simulate and evaluate loss + trajectory = simulate(model, rod, mapping.deformable.mass_spring, rest_lengths) + op_trajectories.append(trajectory.positions) + # np.save("rest_lengths.npy", rest_lengths) + save_transforms.save_rod_optimization("heart_optimization", op_trajectories) + loss = compute_loss(trajectory) + + if not gradient: + return loss.loss + + # Compute gradients + be = pymandos.BackwardEngine(stepParameters.h, model, trajectory) + dloss_dp = np.zeros((TOTAL_FRAMES, N_SPRINGS)) + dloss_dx = loss.loss_position_partial_derivative[-1].copy() + dloss_dv = loss.loss_velocity_partial_derivative[-1].copy() + + # Backward loop + for i in range(TOTAL_FRAMES - 1, -1, -1): + dloss_dpi = np.zeros(N_SPRINGS) + be.backward_step(dloss_dx, dloss_dv, dloss_dpi, diff_parameters, 10) + dloss_dx += loss.loss_position_partial_derivative[i] + dloss_dv += loss.loss_velocity_partial_derivative[i] + dloss_dp[i] = dloss_dpi + + return loss.loss, dloss_dp.flatten() + + +frame = 0 + + +def simulate_callback(model, rigid_body_cloud, mapping): + global frame + frame += 1 + result = pymandos.step(model, stepParameters) + if not result: + print(result) + + N_POINTS = int(np.shape(rigid_body_cloud.x)[0]) + positions = np.array([x[:3] for x in rigid_body_cloud.x]) + ps.get_curve_network("rod").update_node_positions(positions) + + ps.get_surface_mesh("coin_mesh").set_transform(coin_transform(frame)) + + # ps.get_point_cloud("particles").update_point_positions(mapping.deformable.x) + + +counter = 0 + + +def measure_gradients(rest_lengths): + nP = len(rest_lengths) + l, g = sim_wrapper(rest_lengths) + dx = 1e-2 + gradient_f = np.zeros(nP) + for i in tqdm(range(0, nP, 4)): + rest_lengths[i] += dx + dl = sim_wrapper(rest_lengths, gradient=False) + rest_lengths[i] -= dx + + gradient_f[i] = (dl - l) / dx + + # Plotting + bp = np.array([g[i] for i in range(0, nP, 4)]) + fd = np.array([gradient_f[i] for i in range(0, nP, 4)]) + error = np.abs(bp - fd) + x = np.arange(0, nP / 4) + plt.title("Loss Gradient components") + plt.plot(x, bp, ".", label="BackPropagation") + plt.plot(x, fd, ".", label="FiniteDifferences") + # plt.errorbar(x, bp, error) + plt.grid() + plt.legend() + global counter + counter += 1 + plt.savefig(f"output/fig_{counter}.png") + plt.clf() + return l, g + + +if __name__ == "__main__": + ps.init() + ps.set_up_dir("z_up") + ps.look_at((0.5 * total_length, 2.0, 0.5), (0.5 * total_length, 0.0, 0.0)) + ps.set_ground_plane_mode("none") # Disable ground rendering + + # Target Coin + coin = meshio.read("coin.stl") + ps.register_surface_mesh("coin_mesh", coin.points, coin.cells_dict.get("triangle")) + # Target Curve + indices = np.array([(i, i + 1) for i in range(len(points) - 1)]) + ps.register_curve_network("path", points, indices, radius=0.001) + + # Initial rest lengths + delta_x = total_length / N_POINTS + rest_length = (total_length - 2 * delta_x) / (N_CONTROL_POINTS - 1) + print(f"Rest length = {rest_length}") + rest_lengths = rest_length * np.ones((TOTAL_FRAMES, N_SPRINGS)) + rest_lengths = rest_lengths.flatten() + + # measure_gradients(rest_lengths) + # exit() + + # model = pymandos.Model() + # rod, mapping = create_rod(model) + # ps.set_user_callback(lambda: simulate_callback(model, rod, mapping)) + # ps.show() + # exit() + result = scipy.optimize.minimize( + sim_wrapper, + rest_lengths, + method="L-BFGS-B", + jac=True, + options={"disp": True}, + ) + print(result) + + ENABLE_RENDER = True + sim_wrapper(result.x) + ps.show() diff --git a/examples/python/diff/rod_to_point.py b/examples/python/diff/rod_to_point.py new file mode 100644 index 00000000..c7402d10 --- /dev/null +++ b/examples/python/diff/rod_to_point.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 + +import numpy as np +import polyscope as ps +import scipy +import meshio + +import pymandos +from parallel_transport import compute_rotations_parallel_transport + +total_length = 1.0 # meters +total_mass = 0.3 # Kg +N_POINTS = 20 +N_CONTROL_POINTS = 2 +N_SPRINGS_PER_POINT = 4 +IGNORE_X_MATRIX = np.array([[0.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) +TARGET_POS = np.array([1.0, 1.0, 1.0]) +TARGET_POS = TARGET_POS / np.linalg.norm(TARGET_POS) * 0.6 * total_length +TARGET_POS[0] = total_length + +stepParameters = pymandos.StepParameters() +stepParameters.h = 0.01 +stepParameters.newton_iterations = 300 +stepParameters.cg_iterations = 20 +stepParameters.cg_error = 1e-8 +stepParameters.grad_norm = 1e-2 +stepParameters.line_search_iterations = 20 + + +def coin_transform(time): + mat = np.zeros((4, 4)) + S = 0.1 * np.identity(3) + R = scipy.spatial.transform.Rotation.from_rotvec([0.0, 0.0, 10 * time]).as_matrix() + mat[:3, :3] = S @ R + mat[:3, 3] = TARGET_POS + mat[3, 3] = 1.0 + return mat + + +def get_N_springs(): + return (N_CONTROL_POINTS - 1) * 4 + + +def create_rod(model, rest_lengths): + delta_x = total_length / N_POINTS + rigid_body_cloud = model.add_rigidbody_cloud() + + # Set initial conditions + positions = np.zeros((N_POINTS, 6)) + velocities = np.zeros((N_POINTS, 6)) + for i, position in enumerate(positions): + position[0] += delta_x * i + + lin_pos = np.array([pos[:3] for pos in positions]) + rotvecs = compute_rotations_parallel_transport(lin_pos) + for rotvec, pos in zip(rotvecs, positions): + pos[3:] = rotvec + rigid_body_cloud.size = N_POINTS + rigid_body_cloud.x = positions + rigid_body_cloud.v = velocities + + # Set up rigid body mass and inertia + node_mass = total_mass / N_POINTS + rigid_body_cloud.mass = node_mass * np.ones(N_POINTS) + rigid_body_cloud.inertiaTensor = [ + node_mass * np.diag([1.0, 1.0, 1.0]) for _ in range(N_POINTS) + ] + + # Rod energy + cosserat_rod = rigid_body_cloud.cosserat_rod + Ks = [1000.0 for _ in range(N_POINTS - 1)] + stiffness_tensor = [1.0 * np.ones(3) for _ in range(N_POINTS - 2)] + cosserat_stiffness = [100.0 for _ in range(N_POINTS - 1)] + cosserat_rod.set_parameters(Ks, cosserat_stiffness, stiffness_tensor) + + # Mapping + radius = total_length / 50.0 + + mapping = model.add_rigidbody_point_mapping(rigid_body_cloud) + N = N_POINTS - 2 + indices = np.linspace(0, N, N_CONTROL_POINTS, dtype=int) + for index in indices: + mapping.add_particle([radius, 0.0, 0.0], index) # +x + mapping.add_particle([-radius, 0.0, 0.0], index) # -x + + mapping.add_particle([0.0, radius, 0.0], index) # +y + mapping.add_particle([0.0, -radius, 0.0], index) # -y + + mapping.deformable.particle_mass = np.zeros(mapping.deformable.size) + mass_spring = mapping.deformable.mass_spring + for i in range(len(indices) - 1): + for j in range(4): + mass_spring.add_element((4 * i + j, 4 * i + j + 4), 400.0, 0.0) + + mass_spring.rest_length = rest_lengths + # Freeze one side + rigid_body_cloud.fix_translation(0) + rigid_body_cloud.fix_rotation(0) + + model.commit() + + # Render + positions = np.array([x[:3] for x in rigid_body_cloud.x]) + indices = np.array([(i, i + 1) for i in range(N_POINTS - 1)]) + ps.register_curve_network(f"rod_{N_POINTS}", positions, indices, radius=radius) + + # ps.register_point_cloud("particles", mapping.deformable.x, radius=radius) + + return rigid_body_cloud, mapping + + +def simulate(model: pymandos.Model, steps: int) -> pymandos.Trajectory: + trajectory = pymandos.Trajectory() + trajectory.append_state(model) + for i in range(steps): + success = pymandos.step(model, stepParameters) + if not success: + print(i, success) + exit() + trajectory.append_state(model) + return trajectory + + +def compute_loss(trajectory: pymandos.Trajectory) -> pymandos.LossFunctionAndGradients: + loss = pymandos.LossFunctionAndGradients() + + # Target position + lin_pos = trajectory.positions[-1][-6:-3] + delta = lin_pos - TARGET_POS + loss_value = delta.T @ IGNORE_X_MATRIX @ delta + loss_pos_gradient = 2.0 * IGNORE_X_MATRIX @ delta + + # Low Speed regularizer + low_speed_stiffness = 2.0 + lin_vel = trajectory.velocities[-1][-6:-3] + loss_value += low_speed_stiffness * np.dot(lin_vel, lin_vel) + loss_vel_gradient = 2.0 * low_speed_stiffness * lin_vel + + dof = trajectory.get_n_dof() + loss.loss = loss_value + zeros = np.array([np.zeros(dof) for _ in trajectory.positions]) + # zeros = [np.zeros(dof) for _ in trajectory.positions] + + loss_position_partial_derivative = zeros.copy() + loss_position_partial_derivative[-1][-6:-3] = loss_pos_gradient + + loss.loss_position_partial_derivative = loss_position_partial_derivative + loss_velocity_partial_derivative = zeros.copy() + loss_velocity_partial_derivative[-1][-6:-3] = loss_vel_gradient + loss.loss_velocity_partial_derivative = loss_velocity_partial_derivative + loss.loss_parameter_partial_derivative = np.zeros(get_N_springs()) + return loss + + +def sim_wrapper(rest_lengths: list): + print("Simulation invoked with: ", rest_lengths) + model = pymandos.Model() + rod, mapping = create_rod(model, rest_lengths) + + # Differentiable parameters + mass_spring = mapping.deformable.mass_spring + diff_rest_lengths = mass_spring.diff(mass_spring.RestLength) + diff_rest_lengths.indices = [i for i in range(get_N_springs())] + diff_parameters = pymandos.DiffParameters([diff_rest_lengths]) + + # Simulate and evaluate loss + trajectory = simulate(model, 100) + loss = compute_loss(trajectory) + + # Compute gradients + be = pymandos.BackwardEngine(stepParameters.h, model, trajectory) + result = be.backward_simulation(loss, diff_parameters, 10) + return loss.loss, result.loss_parameter_gradient + + +time = 0.0 + + +def simulate_callback(model, rigid_body_cloud, mapping): + global time + time += stepParameters.h + result = pymandos.step(model, stepParameters) + if not result: + print(result) + + N_POINTS = int(np.shape(rigid_body_cloud.x)[0]) + positions = np.array([x[:3] for x in rigid_body_cloud.x]) + ps.get_curve_network(f"rod_{N_POINTS}").update_node_positions(positions) + + ps.get_surface_mesh("coin_mesh").set_transform(coin_transform(time)) + + # ps.get_point_cloud("particles").update_point_positions(mapping.deformable.x) + + +if __name__ == "__main__": + ps.init() + ps.set_up_dir("z_up") + ps.look_at((0.5 * total_length, 2.0, 0.5), (0.5 * total_length, 0.0, 0.0)) + ps.set_ground_plane_mode("none") # Disable ground rendering + + # Target Coin + coin = meshio.read("coin.stl") + ps.register_surface_mesh("coin_mesh", coin.points, coin.cells_dict.get("triangle")) + + # Initial rest lengths + delta_x = total_length / N_POINTS + rest_length = (total_length - 2 * delta_x) / (N_CONTROL_POINTS - 1) + rest_lengths = rest_length * np.ones(get_N_springs()) + + # model = pymandos.Model() + # rod, mapping = create_rod(model, rest_lengths) + # ps.set_user_callback(lambda: simulate_callback(model, rod, mapping)) + # ps.show() + # exit() + result = scipy.optimize.minimize( + sim_wrapper, + rest_lengths, + method="L-BFGS-B", + jac=True, + tol=1e-3, + options={"disp": True}, + ) + print(result) + + model = pymandos.Model() + rod, mapping = create_rod(model, result.x) + ps.set_user_callback(lambda: simulate_callback(model, rod, mapping)) + ps.show() + + # loss, grad = sim_wrapper(rest_lengths) + # print(loss) + # print(grad) + # dx = 1e-5 + # for i in range(4): + # rest_lengths[i] += dx + # loss2, grad2 = sim_wrapper(rest_lengths) + # print((loss2 - loss) / dx) + # rest_lengths[i] -= dx diff --git a/examples/python/diff/save_transforms.py b/examples/python/diff/save_transforms.py new file mode 100644 index 00000000..bdd44799 --- /dev/null +++ b/examples/python/diff/save_transforms.py @@ -0,0 +1,46 @@ +import numpy as np + +import rotation_utilities + + +def generalized_to_pos_rot(vec6): + pos = vec6[:3] + R = rotation_utilities.rotation_exp_map(vec6[3:]) + return pos, R + +def pos_rot_to_transform(pos, R): + transform = np.eye(4) + transform[:3, :3] = R + transform[:3, 3] = pos + return transform + +def save_rod_trajectory(file_path: str, state): + state = np.array(state) + frames, nDof = state.shape + nRB = int(nDof / 6) + print(f"Frames = {frames}, nDof = {nDof}") + + transforms = np.zeros((frames, nRB, 4, 4)) + + for i in range(frames): + for j in range(nRB): + pos, R = generalized_to_pos_rot(state[i][6 * j : 6 * j + 6]) + transforms[i][j] = pos_rot_to_transform(pos, R) + + np.save(file_path, transforms) + +def save_rod_optimization(file_path: str, optimization_trajectories): + trajectories = np.array(optimization_trajectories) + op_steps, frames, nDof = trajectories.shape + nRB = int(nDof / 6) + print(f"Optimization steps = {op_steps} Frames = {frames}, nDof = {nDof}") + + transforms = np.zeros((op_steps, frames, nRB, 4, 4)) + + for o in range(op_steps): + for i in range(frames): + for j in range(nRB): + pos, R = generalized_to_pos_rot(trajectories[o][i][6 * j : 6 * j + 6]) + transforms[o][i][j] = pos_rot_to_transform(pos, R) + + np.save(file_path, transforms) -- GitLab From 412e325ac10b489f7b0a6db9874daae5d16acc17 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mag=C3=AD=20Romany=C3=A0?= Date: Mon, 27 Jan 2025 18:08:04 +0100 Subject: [PATCH 5/6] Tidy Up MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Magí Romanyà --- bindings/Mandos/python/Deformable3D.cpp | 4 +- bindings/Mandos/python/RigidBody.cpp | 10 +-- .../Core/Collisions/CollisionDetection.cpp | 62 +++++++++---------- src/Mandos/Core/DiffRigidBody.cpp | 6 +- src/Mandos/Core/Differentiable.cpp | 6 +- src/Mandos/Core/Energies/ARAP.cpp | 36 +++++------ .../Core/Energies/CosseratBendingRod.cpp | 8 +-- .../Core/Energies/CosseratRodAlignment.cpp | 8 +-- .../Core/Energies/LumpedMassInertia.cpp | 18 +++--- src/Mandos/Core/Energies/MassSpring.cpp | 22 +++---- src/Mandos/Core/Energies/RigidBodyInertia.cpp | 22 ++++--- src/Mandos/Core/Energies/StableNeoHookean.cpp | 36 +++++------ src/Mandos/Core/Mappings/CollisionMapping.cpp | 6 +- .../Core/Mappings/RigidBodyPointMapping.cpp | 4 +- .../Core/MechanicalStates/RigidBody.cpp | 4 +- .../Core/MechanicalStates/RigidBodyGlobal.cpp | 4 +- src/Mandos/Core/Model.cpp | 7 +-- src/Mandos/Core/RotationUtilities.cpp | 48 +++++++------- .../tst_CosseratBendingRod.cpp | 4 +- tests/CosseratRod/tst_CosseratRod.cpp | 2 +- tests/FixedProjection/tst_FixedProjection.cpp | 26 ++++---- 21 files changed, 171 insertions(+), 172 deletions(-) diff --git a/bindings/Mandos/python/Deformable3D.cpp b/bindings/Mandos/python/Deformable3D.cpp index a6d78fcb..1146c028 100644 --- a/bindings/Mandos/python/Deformable3D.cpp +++ b/bindings/Mandos/python/Deformable3D.cpp @@ -138,10 +138,10 @@ void mandos::py::Deformable3D::fixParticle(int index, bool x, bool y, bool z) getFixedDofVector().push_back(3 * index); } if (y) { - getFixedDofVector().push_back(3 * index + 1); + getFixedDofVector().push_back((3 * index) + 1); } if (z) { - getFixedDofVector().push_back(3 * index + 2); + getFixedDofVector().push_back((3 * index) + 2); } } diff --git a/bindings/Mandos/python/RigidBody.cpp b/bindings/Mandos/python/RigidBody.cpp index 25258bda..0c0de255 100644 --- a/bindings/Mandos/python/RigidBody.cpp +++ b/bindings/Mandos/python/RigidBody.cpp @@ -313,17 +313,17 @@ void RigidBodyCloud3D::fixRigidBodyTranslation(int index, bool x, bool y, bool z getFixedDofVector().push_back(6 * index); } if (y) { - getFixedDofVector().push_back(6 * index + 1); + getFixedDofVector().push_back((6 * index) + 1); } if (z) { - getFixedDofVector().push_back(6 * index + 2); + getFixedDofVector().push_back((6 * index) + 2); } } void RigidBodyCloud3D::fixRigidBodyRotation(int index) { - getFixedDofVector().push_back(6 * index + 3); - getFixedDofVector().push_back(6 * index + 4); - getFixedDofVector().push_back(6 * index + 5); + getFixedDofVector().push_back((6 * index) + 3); + getFixedDofVector().push_back((6 * index) + 4); + getFixedDofVector().push_back((6 * index) + 5); } void RigidBodyCloud3D::clearFixing() diff --git a/src/Mandos/Core/Collisions/CollisionDetection.cpp b/src/Mandos/Core/Collisions/CollisionDetection.cpp index eace345f..a4829cd5 100644 --- a/src/Mandos/Core/Collisions/CollisionDetection.cpp +++ b/src/Mandos/Core/Collisions/CollisionDetection.cpp @@ -17,37 +17,37 @@ std::vector> collisions(const SDF &sdf, const Sph tbb::enumerable_thread_specific>> contactsTLS; - tbb::parallel_for(tbb::blocked_range(0, centers.size()), - [&](const tbb::blocked_range &range) { - auto &contacts = contactsTLS.local(); - for (auto sId = range.begin(); sId != range.end(); ++sId) { - const auto ¢er = centers[sId]; - const auto r = radius[sId]; - - auto d = sdf.vdb().distance(center); - // If the distance is below the radius of the sphere, we consider there is a contact - if (d < r) { - // Compute the gradient of the distance field at this particular location using finite - // differences - const auto rdiff = 0.01 * sdf.vdb().voxelSize(); - auto xplus = sdf.vdb().distance(center + Vec3{rdiff, 0, 0}); - auto xminus = sdf.vdb().distance(center - Vec3{rdiff, 0, 0}); - auto yplus = sdf.vdb().distance(center + Vec3{0, rdiff, 0}); - auto yminus = sdf.vdb().distance(center - Vec3{0, rdiff, 0}); - auto zplus = sdf.vdb().distance(center + Vec3{0, 0, rdiff}); - auto zminus = sdf.vdb().distance(center - Vec3{0, 0, rdiff}); - - const auto n = Vec3{xplus - xminus, yplus - yminus, zplus - zminus}.normalized(); - - const auto primitiveId = sdf.vdb().closestPolygon(center); - - const ContactEventSide c0Side{primitiveId, center - d * n}; - const ContactEventSide c1Side{static_cast(sId)}; - - contacts.emplace_back(c0Side, c1Side, d, n); - } - } - }); + tbb::parallel_for( + tbb::blocked_range(0, centers.size()), [&](const tbb::blocked_range &range) { + auto &contacts = contactsTLS.local(); + for (auto sId = range.begin(); sId != range.end(); ++sId) { + const auto ¢er = centers[sId]; + const auto r = radius[sId]; + + auto d = sdf.vdb().distance(center); + // If the distance is below the radius of the sphere, we consider there is a contact + if (d < r) { + // Compute the gradient of the distance field at this particular location using finite + // differences + const auto rdiff = 0.01 * sdf.vdb().voxelSize(); + auto xplus = sdf.vdb().distance(center + Vec3{rdiff, 0, 0}); + auto xminus = sdf.vdb().distance(center - Vec3{rdiff, 0, 0}); + auto yplus = sdf.vdb().distance(center + Vec3{0, rdiff, 0}); + auto yminus = sdf.vdb().distance(center - Vec3{0, rdiff, 0}); + auto zplus = sdf.vdb().distance(center + Vec3{0, 0, rdiff}); + auto zminus = sdf.vdb().distance(center - Vec3{0, 0, rdiff}); + + const auto n = Vec3{xplus - xminus, yplus - yminus, zplus - zminus}.normalized(); + + const auto primitiveId = sdf.vdb().closestPolygon(center); + + const ContactEventSide c0Side{.primitiveId = primitiveId, .contactPoint = center - d * n}; + const ContactEventSide c1Side{static_cast(sId)}; + + contacts.emplace_back(c0Side, c1Side, d, n); + } + } + }); std::vector> contacts; const auto flattened = tbb::flatten2d(contactsTLS); diff --git a/src/Mandos/Core/DiffRigidBody.cpp b/src/Mandos/Core/DiffRigidBody.cpp index ebf37284..59eff8ea 100644 --- a/src/Mandos/Core/DiffRigidBody.cpp +++ b/src/Mandos/Core/DiffRigidBody.cpp @@ -17,7 +17,7 @@ void applyGlobalToLocal(const Model &model, Eigen::Ref x) for (auto i = 0; i < static_cast(simObj.mstate.m_x.size()); ++i) { const Vec3 theta = handle->simulationObject().mstate.m_x[static_cast(i)].segment(3, 3); const Mat3 &J = computeGlobalToLocalAxisAngleJacobian(theta); - x.segment<3>(offset + 6 * i + 3) = x.segment<3>(offset + 6 * i + 3).transpose() * J; + x.segment<3>(offset + (6 * i) + 3) = x.segment<3>(offset + (6 * i) + 3).transpose() * J; } offset += size; } @@ -35,7 +35,7 @@ void applyLocalToGlobal(const Model &model, Eigen::Ref x) for (auto i = 0; i < static_cast(simObj.mstate.m_x.size()); ++i) { const Vec3 theta = handle->simulationObject().mstate.m_x[static_cast(i)].segment(3, 3); const Mat3 &J = computeLocalToGlobalAxisAngleJacobian(theta); - x.segment<3>(offset + 6 * i + 3) = x.segment<3>(offset + 6 * i + 3).transpose() * J; + x.segment<3>(offset + (6 * i) + 3) = x.segment<3>(offset + (6 * i) + 3).transpose() * J; } offset += size; } @@ -57,7 +57,7 @@ void applyComposeAxisAngleJacobian(Scalar h, const Model &model, Eigen::Ref const Vec3 theta = simulationObject.mstate.m_x[i].template segment<3>(3); const Vec3 omega = simulationObject.mstate.m_v[i].template segment<3>(3); const Mat3 J = rotationExpMap(h * omega); - v.segment<3>(offset + 6 * i + 3) = v.segment<3>(offset + 6 * i + 3).transpose() * J; + v.segment<3>(offset + (6 * i) + 3) = v.segment<3>(offset + (6 * i) + 3).transpose() * J; } } offset += static_cast(size); diff --git a/src/Mandos/Core/Differentiable.cpp b/src/Mandos/Core/Differentiable.cpp index b9ed2685..1e43b4ac 100644 --- a/src/Mandos/Core/Differentiable.cpp +++ b/src/Mandos/Core/Differentiable.cpp @@ -123,9 +123,11 @@ BackpropagationResult computeLossFunctionGradientBackpropagation(Scalar h, return {.gradient = lossGradient, .dLoss_dx0 = lossPositionGradient, .dLoss_dv0 = lossVelocityGradient}; } +/* All three loss vectors here are mutable references. The nolint directive avoids clang tidy to complain that the + * parameter is const when it is not.*/ void BackwardEngine::backwardStep(Eigen::Ref lossPositionGradient, Eigen::Ref lossVelocityGradient, - Eigen::Ref lossParameterGradient, + Eigen::Ref lossParameterGradient /*NOLINT*/, const DiffParameters &diffParameters, unsigned int maxIterations) { @@ -182,7 +184,7 @@ void BackwardEngine::backwardStep(Eigen::Ref lossPositionGradient, ZoneScopedN("backwardIteration"); const Vec Hz = approximateHzFiniteDiff(m_model, grad, x0, v0, z, m_h, dx); if (Hz.hasNaN()) { - throw std::runtime_error("WARNING::DIFFERENTIABLE: Hz has NaN"); + throw std::runtime_error("ERROR::BackwardStep: NaN detected"); } h_grad = Hz - equation_vector; [[maybe_unused]] const Scalar backwardIterationEnergy = z.transpose() * (0.5 * Hz - equation_vector); diff --git a/src/Mandos/Core/Energies/ARAP.cpp b/src/Mandos/Core/Energies/ARAP.cpp index cd52fce1..049c60c9 100644 --- a/src/Mandos/Core/Energies/ARAP.cpp +++ b/src/Mandos/Core/Energies/ARAP.cpp @@ -280,34 +280,34 @@ Scalar ARAP::computeEnergyGradientAndHessian(MechanicalState &mst const auto block{hessian.block<3, 3>(3 * i, 3 * j)}; // Assemble the 9 elements of this block - triplets.emplace_back(3 * indices[static_cast(i)] + 0, - 3 * indices[static_cast(j)] + 0, + triplets.emplace_back((3 * indices[static_cast(i)]) + 0, + (3 * indices[static_cast(j)]) + 0, block(0, 0)); - triplets.emplace_back(3 * indices[static_cast(i)] + 1, - 3 * indices[static_cast(j)] + 0, + triplets.emplace_back((3 * indices[static_cast(i)]) + 1, + (3 * indices[static_cast(j)]) + 0, block(1, 0)); - triplets.emplace_back(3 * indices[static_cast(i)] + 2, - 3 * indices[static_cast(j)] + 0, + triplets.emplace_back((3 * indices[static_cast(i)]) + 2, + (3 * indices[static_cast(j)]) + 0, block(2, 0)); - triplets.emplace_back(3 * indices[static_cast(i)] + 0, - 3 * indices[static_cast(j)] + 1, + triplets.emplace_back((3 * indices[static_cast(i)]) + 0, + (3 * indices[static_cast(j)]) + 1, block(0, 1)); - triplets.emplace_back(3 * indices[static_cast(i)] + 1, - 3 * indices[static_cast(j)] + 1, + triplets.emplace_back((3 * indices[static_cast(i)]) + 1, + (3 * indices[static_cast(j)]) + 1, block(1, 1)); - triplets.emplace_back(3 * indices[static_cast(i)] + 2, - 3 * indices[static_cast(j)] + 1, + triplets.emplace_back((3 * indices[static_cast(i)]) + 2, + (3 * indices[static_cast(j)]) + 1, block(2, 1)); - triplets.emplace_back(3 * indices[static_cast(i)] + 0, - 3 * indices[static_cast(j)] + 2, + triplets.emplace_back((3 * indices[static_cast(i)]) + 0, + (3 * indices[static_cast(j)]) + 2, block(0, 2)); - triplets.emplace_back(3 * indices[static_cast(i)] + 1, - 3 * indices[static_cast(j)] + 2, + triplets.emplace_back((3 * indices[static_cast(i)]) + 1, + (3 * indices[static_cast(j)]) + 2, block(1, 2)); - triplets.emplace_back(3 * indices[static_cast(i)] + 2, - 3 * indices[static_cast(j)] + 2, + triplets.emplace_back((3 * indices[static_cast(i)]) + 2, + (3 * indices[static_cast(j)]) + 2, block(2, 2)); } } diff --git a/src/Mandos/Core/Energies/CosseratBendingRod.cpp b/src/Mandos/Core/Energies/CosseratBendingRod.cpp index d9ea454a..21623f5d 100644 --- a/src/Mandos/Core/Energies/CosseratBendingRod.cpp +++ b/src/Mandos/Core/Energies/CosseratBendingRod.cpp @@ -65,13 +65,13 @@ Scalar CosseratBendingRod::computeEnergyGradientAndHessian(MechanicalState(iA) + k, 6 * static_cast(iA) + j, hessianA(k, j)); + (6 * static_cast(iA)) + k, (6 * static_cast(iA)) + j, hessianA(k, j)); triplets.emplace_back( - 6 * static_cast(iA) + k, 6 * static_cast(iB) + j, hessianAB(k, j)); + (6 * static_cast(iA)) + k, (6 * static_cast(iB)) + j, hessianAB(k, j)); triplets.emplace_back( - 6 * static_cast(iB) + k, 6 * static_cast(iA) + j, hessianAB(j, k)); + (6 * static_cast(iB)) + k, (6 * static_cast(iA)) + j, hessianAB(j, k)); triplets.emplace_back( - 6 * static_cast(iB) + k, 6 * static_cast(iB) + j, hessianB(k, j)); + (6 * static_cast(iB)) + k, (6 * static_cast(iB)) + j, hessianB(k, j)); } } } diff --git a/src/Mandos/Core/Energies/CosseratRodAlignment.cpp b/src/Mandos/Core/Energies/CosseratRodAlignment.cpp index 7b4fcf61..a77e0c85 100644 --- a/src/Mandos/Core/Energies/CosseratRodAlignment.cpp +++ b/src/Mandos/Core/Energies/CosseratRodAlignment.cpp @@ -125,13 +125,13 @@ Scalar CosseratRodAlignment::computeEnergyGradientAndHessian(MechanicalState(iA) + k, 6 * static_cast(iA) + j, hessianA(k, j)); + (6 * static_cast(iA)) + k, (6 * static_cast(iA)) + j, hessianA(k, j)); triplets.emplace_back( - 6 * static_cast(iA) + k, 6 * static_cast(iB) + j, hessianAB(k, j)); + (6 * static_cast(iA)) + k, (6 * static_cast(iB)) + j, hessianAB(k, j)); triplets.emplace_back( - 6 * static_cast(iB) + k, 6 * static_cast(iA) + j, hessianAB(j, k)); + (6 * static_cast(iB)) + k, (6 * static_cast(iA)) + j, hessianAB(j, k)); triplets.emplace_back( - 6 * static_cast(iB) + k, 6 * static_cast(iB) + j, hessianB(k, j)); + (6 * static_cast(iB)) + k, (6 * static_cast(iB)) + j, hessianB(k, j)); } } } diff --git a/src/Mandos/Core/Energies/LumpedMassInertia.cpp b/src/Mandos/Core/Energies/LumpedMassInertia.cpp index b1a1738f..111ff5c7 100644 --- a/src/Mandos/Core/Energies/LumpedMassInertia.cpp +++ b/src/Mandos/Core/Energies/LumpedMassInertia.cpp @@ -40,9 +40,9 @@ Scalar LumpedMassInertia::computeEnergyGradientAndHessian(MechanicalState -#include - #include namespace @@ -17,7 +15,6 @@ mandos::core::Scalar computeEnergy(const mandos::core::Vec3 &xA, { const mandos::core::Scalar L = (xA - xB).norm(); - // TODO add "damping potential" if (restLength > std::numeric_limits::epsilon()) { stiffness = stiffness / restLength; } @@ -92,7 +89,6 @@ Scalar MassSpring::computeEnergy(const MechanicalState &mstate) co const Vec3 xA = mstate.m_x[static_cast(m_indices[i][0])].segment<3>(0); const Vec3 xB = mstate.m_x[static_cast(m_indices[i][1])].segment<3>(0); - // TODO add "damping potential" energy += ::computeEnergy(xA, xB, m_stiffness[i], m_damping[i], m_restLength[i]); } return energy; @@ -112,8 +108,6 @@ Scalar MassSpring::computeEnergyAndGradient(MechanicalState &msta mstate.m_grad[static_cast(indices[0])] += grad; mstate.m_grad[static_cast(indices[1])] -= grad; - - // TODO add "damping potential" } return energy; } @@ -154,10 +148,10 @@ Scalar MassSpring::computeEnergyGradientAndHessian(MechanicalState for (auto j = 0; j < 3; ++j) { for (auto k = 0; k < 3; ++k) { - triplets.emplace_back(6 * indices[0] + j, 6 * indices[0] + k, hessian(j, k)); - triplets.emplace_back(6 * indices[0] + j, 6 * indices[1] + k, -hessian(j, k)); - triplets.emplace_back(6 * indices[1] + j, 6 * indices[0] + k, -hessian(j, k)); - triplets.emplace_back(6 * indices[1] + j, 6 * indices[1] + k, hessian(j, k)); + triplets.emplace_back((6 * indices[0]) + j, (6 * indices[0]) + k, hessian(j, k)); + triplets.emplace_back((6 * indices[0]) + j, (6 * indices[1]) + k, -hessian(j, k)); + triplets.emplace_back((6 * indices[1]) + j, (6 * indices[0]) + k, -hessian(j, k)); + triplets.emplace_back((6 * indices[1]) + j, (6 * indices[1]) + k, hessian(j, k)); } } } diff --git a/src/Mandos/Core/Energies/RigidBodyInertia.cpp b/src/Mandos/Core/Energies/RigidBodyInertia.cpp index d3cb9192..d5952b3b 100644 --- a/src/Mandos/Core/Energies/RigidBodyInertia.cpp +++ b/src/Mandos/Core/Energies/RigidBodyInertia.cpp @@ -304,10 +304,10 @@ Scalar RigidBodyInertia::computeEnergyGradientAndHessian(MechanicalState(0), R, m_advX[i], m_advXR[i], m_mass[i], m_inertiaTensor0[i], h); for (unsigned int j = 0; j < 3; ++j) { - triplets.emplace_back(6 * i + j, 6 * i + j, hess(j, j)); // Diagonal translation hessian + triplets.emplace_back((6 * i) + j, (6 * i) + j, hess(j, j)); // Diagonal translation hessian for (unsigned int k = 0; k < 3; ++k) { triplets.emplace_back( - 6 * i + j + 3, 6 * i + k + 3, hess(3 + j, 3 + k)); // Non-diagonal rotation hessian + (6 * i) + j + 3, (6 * i) + k + 3, hess(3 + j, 3 + k)); // Non-diagonal rotation hessian } } energy += @@ -337,10 +337,10 @@ Scalar RigidBodyInertia::computeEnergyGradientAndHessian(MechanicalState(3), m_v0[i].segment<3>(3), m_mass[i], m_inertiaTensor0[i], h); for (unsigned int j = 0; j < 3; ++j) { - triplets.emplace_back(offset + 6 * i + j, offset + 6 * i + j, hess(j, j)); // Diagonal translation hessian + triplets.emplace_back( + offset + (6 * i) + j, offset + (6 * i) + j, hess(j, j)); // Diagonal translation hessian for (unsigned int k = 0; k < 3; ++k) { - triplets.emplace_back(offset + 6 * i + j + 3, - offset + 6 * i + k + 3, + triplets.emplace_back(offset + (6 * i) + j + 3, + offset + (6 * i) + k + 3, hess(3 + j, 3 + k)); // Non-diagonal rotation hessian } } @@ -391,10 +392,11 @@ void RigidBodyInertia::computeEnergyRetardedVelocityHessian(MechanicalState(3), m_v0[i].segment<3>(3), m_mass[i], m_inertiaTensor0[i], h); for (unsigned int j = 0; j < 3; ++j) { - triplets.emplace_back(offset + 6 * i + j, offset + 6 * i + j, hess(j, j)); // Diagonal translation hessian + triplets.emplace_back( + offset + (6 * i) + j, offset + (6 * i) + j, hess(j, j)); // Diagonal translation hessian for (unsigned int k = 0; k < 3; ++k) { - triplets.emplace_back(offset + 6 * i + j + 3, - offset + 6 * i + k + 3, + triplets.emplace_back(offset + (6 * i) + j + 3, + offset + (6 * i) + k + 3, hess(3 + j, 3 + k)); // Non-diagonal rotation hessian } } diff --git a/src/Mandos/Core/Energies/StableNeoHookean.cpp b/src/Mandos/Core/Energies/StableNeoHookean.cpp index 8cfb547b..edff1105 100644 --- a/src/Mandos/Core/Energies/StableNeoHookean.cpp +++ b/src/Mandos/Core/Energies/StableNeoHookean.cpp @@ -290,34 +290,34 @@ Scalar StableNeoHookean::computeEnergyGradientAndHessian(MechanicalState(3 * i, 3 * j)}; // Assemble the 9 elements of this block - triplets.emplace_back(3 * indices[static_cast(i)] + 0, - 3 * indices[static_cast(j)] + 0, + triplets.emplace_back((3 * indices[static_cast(i)]) + 0, + (3 * indices[static_cast(j)]) + 0, block(0, 0)); - triplets.emplace_back(3 * indices[static_cast(i)] + 1, - 3 * indices[static_cast(j)] + 0, + triplets.emplace_back((3 * indices[static_cast(i)]) + 1, + (3 * indices[static_cast(j)]) + 0, block(1, 0)); - triplets.emplace_back(3 * indices[static_cast(i)] + 2, - 3 * indices[static_cast(j)] + 0, + triplets.emplace_back((3 * indices[static_cast(i)]) + 2, + (3 * indices[static_cast(j)]) + 0, block(2, 0)); - triplets.emplace_back(3 * indices[static_cast(i)] + 0, - 3 * indices[static_cast(j)] + 1, + triplets.emplace_back((3 * indices[static_cast(i)]) + 0, + (3 * indices[static_cast(j)]) + 1, block(0, 1)); - triplets.emplace_back(3 * indices[static_cast(i)] + 1, - 3 * indices[static_cast(j)] + 1, + triplets.emplace_back((3 * indices[static_cast(i)]) + 1, + (3 * indices[static_cast(j)]) + 1, block(1, 1)); - triplets.emplace_back(3 * indices[static_cast(i)] + 2, - 3 * indices[static_cast(j)] + 1, + triplets.emplace_back((3 * indices[static_cast(i)]) + 2, + (3 * indices[static_cast(j)]) + 1, block(2, 1)); - triplets.emplace_back(3 * indices[static_cast(i)] + 0, - 3 * indices[static_cast(j)] + 2, + triplets.emplace_back((3 * indices[static_cast(i)]) + 0, + (3 * indices[static_cast(j)]) + 2, block(0, 2)); - triplets.emplace_back(3 * indices[static_cast(i)] + 1, - 3 * indices[static_cast(j)] + 2, + triplets.emplace_back((3 * indices[static_cast(i)]) + 1, + (3 * indices[static_cast(j)]) + 2, block(1, 2)); - triplets.emplace_back(3 * indices[static_cast(i)] + 2, - 3 * indices[static_cast(j)] + 2, + triplets.emplace_back((3 * indices[static_cast(i)]) + 2, + (3 * indices[static_cast(j)]) + 2, block(2, 2)); } } diff --git a/src/Mandos/Core/Mappings/CollisionMapping.cpp b/src/Mandos/Core/Mappings/CollisionMapping.cpp index 84084061..3af6021e 100644 --- a/src/Mandos/Core/Mappings/CollisionMapping.cpp +++ b/src/Mandos/Core/Mappings/CollisionMapping.cpp @@ -17,8 +17,8 @@ void updateMapping(const collisions::ContactEventSide &contact, int collisionParticle) { ZoneScopedN("updateMapping"); - const MappingInfo mappingInfo{static_cast(contact.sphereId), - static_cast(collisionParticle)}; + const MappingInfo mappingInfo{.m_fromId = static_cast(contact.sphereId), + .m_toId = static_cast(collisionParticle)}; mapping.addMappingInfo(mappingInfo); } @@ -33,7 +33,7 @@ void updateMapping(const collisions::ContactEventSide &contact, (contact.contactPoint - mstate.m_x[0].segment<3>(0)); const MappingInfo mappingInfo{ - r, mapping.from()->mstate, static_cast(collisionParticle)}; + .m_r = r, .mstate = mapping.from()->mstate, .m_toId = static_cast(collisionParticle)}; mapping.addMappingInfo(mappingInfo); } diff --git a/src/Mandos/Core/Mappings/RigidBodyPointMapping.cpp b/src/Mandos/Core/Mappings/RigidBodyPointMapping.cpp index fe159873..b3a387ed 100644 --- a/src/Mandos/Core/Mappings/RigidBodyPointMapping.cpp +++ b/src/Mandos/Core/Mappings/RigidBodyPointMapping.cpp @@ -72,9 +72,9 @@ SparseMat RigidBodyPointMapping::J() const const Mat3 R = rotationExpMap(m_from->mstate.m_x[iR].segment<3>(3)); const Mat3 thetaJ = -skew(R * m_localCoord[i]); for (unsigned int j = 0; j < 3; ++j) { - triplets.emplace_back(3 * i + j, 6 * iR + j, 1.0); // Identity + triplets.emplace_back((3 * i) + j, (6 * iR) + j, 1.0); // Identity for (unsigned int k = 0; k < 3; ++k) { - triplets.emplace_back(3 * i + j, 6 * iR + 3 + k, thetaJ(j, k)); + triplets.emplace_back((3 * i) + j, (6 * iR) + 3 + k, thetaJ(j, k)); } } } diff --git a/src/Mandos/Core/MechanicalStates/RigidBody.cpp b/src/Mandos/Core/MechanicalStates/RigidBody.cpp index f3d5dc02..f133d763 100644 --- a/src/Mandos/Core/MechanicalStates/RigidBody.cpp +++ b/src/Mandos/Core/MechanicalStates/RigidBody.cpp @@ -19,8 +19,8 @@ void MechanicalState::updateState(const Eigen::Ref &dx, // The rotational part is a bit more tricky const Vec3 theta = m_x[i].segment<3>(3); - const Vec3 theta0 = x0.segment<3>(6 * static_cast(i) + 3); - const Vec3 dtheta = dx.segment<3>(6 * static_cast(i) + 3); + const Vec3 theta0 = x0.segment<3>((6 * static_cast(i)) + 3); + const Vec3 dtheta = dx.segment<3>((6 * static_cast(i)) + 3); const Vec3 newTheta = clampAxisAngle(composeAxisAngle(dtheta, theta)); m_x[i].segment<3>(3) = newTheta; diff --git a/src/Mandos/Core/MechanicalStates/RigidBodyGlobal.cpp b/src/Mandos/Core/MechanicalStates/RigidBodyGlobal.cpp index dee8b466..ee3e16c8 100644 --- a/src/Mandos/Core/MechanicalStates/RigidBodyGlobal.cpp +++ b/src/Mandos/Core/MechanicalStates/RigidBodyGlobal.cpp @@ -17,8 +17,8 @@ void MechanicalState::updateState(const Eigen::Ref(3); - const Vec3 theta0 = x0.segment<3>(6 * i + 3); - const Vec3 dtheta = dx.segment<3>(6 * i + 3); + const Vec3 theta0 = x0.segment<3>((6 * i) + 3); + const Vec3 dtheta = dx.segment<3>((6 * i) + 3); const Vec3 new_theta = theta + dtheta; const Scalar new_angle = new_theta.norm(); diff --git a/src/Mandos/Core/Model.cpp b/src/Mandos/Core/Model.cpp index 9e65ab3b..99b9cdf6 100644 --- a/src/Mandos/Core/Model.cpp +++ b/src/Mandos/Core/Model.cpp @@ -452,8 +452,7 @@ void Model::updateState(const Vec &dx, const Vec &x0, const Vec &v0, Scalar h) ZoneScopedN("Model.updateState"); // First, reset the state of all the mapped objects, so we can later accumulate through mappings for (auto node : m_forwardSortedList) { - if (std::find(std::begin(m_freeSimulationObjects), std::end(m_freeSimulationObjects), node) == - std::end(m_freeSimulationObjects)) { + if (std::ranges::find(m_freeSimulationObjects, node) == std::end(m_freeSimulationObjects)) { std::visit( [this](auto handle) { auto &simulationObject = this->simulationObject(handle); @@ -663,11 +662,11 @@ void Model::commit() sortedList.reserve(boost::num_vertices(m_simulationObjectsGraph)); boost::topological_sort(m_simulationObjectsGraph, std::back_inserter(sortedList)); - std::reverse_copy(std::begin(sortedList), std::end(sortedList), std::back_inserter(m_forwardSortedList)); + std::ranges::reverse_copy(sortedList, std::back_inserter(m_forwardSortedList)); sortedList.clear(); boost::topological_sort(reverseGraph, std::back_inserter(sortedList)); - std::reverse_copy(std::begin(sortedList), std::end(sortedList), std::back_inserter(m_backwardSortedList)); + std::ranges::reverse_copy(sortedList, std::back_inserter(m_backwardSortedList)); // Initializing mappings this way might be a bit inneficient, but it avoid having to duplicate code, and its not a // hot path diff --git a/src/Mandos/Core/RotationUtilities.cpp b/src/Mandos/Core/RotationUtilities.cpp index 1d973624..71a0f15f 100644 --- a/src/Mandos/Core/RotationUtilities.cpp +++ b/src/Mandos/Core/RotationUtilities.cpp @@ -17,41 +17,42 @@ inline void computeAxisAngleJacobianParts(const mandos::core::Vec3 &phi, mandos: A = mandos::core::rotationExpMap(phi) - mandos::core::Mat3::Identity() + phi_phiT; B = mandos::core::skew(phi) + phi_phiT; } -} // namespace -namespace mandos::core -{ - -Scalar sinc(Scalar x) +inline mandos::core::Scalar sinc_g_function(mandos::core::Scalar x) { if (abs(x) < THRESHOLD) { - const Scalar x2 = x * x; - const Scalar x4 = x2 * x2; - return 1.0 - x2 / 6.0 + x4 / 120.0; + const mandos::core::Scalar x2 = x * x; + const mandos::core::Scalar x4 = x2 * x2; + return (-1.0 / 3.0) + (x2 / 30.0) + (x4 / 840.0); } - return std::sin(x) / x; + + return (x * std::cos(x) - std::sin(x)) / (x * x * x); } -inline Scalar sinc_g_function(Scalar x) +inline mandos::core::Scalar sinc_h_function(mandos::core::Scalar x) { + const mandos::core::Scalar x2 = x * x; + const mandos::core::Scalar x4 = x2 * x2; if (abs(x) < THRESHOLD) { - const Scalar x2 = x * x; - const Scalar x4 = x2 * x2; - return -1.0 / 3.0 + x2 / 30.0 + x4 / 840.0; + return (1.0 / 15.0) + (x2 / 210.0) + (x4 / 7560.0); } - return (x * std::cos(x) - std::sin(x)) / (x * x * x); + return (-x2 * std::sin(x) - 3.0 * x * std::cos(x) + 3.0 * std::sin(x)) / (x4 * x); } -inline Scalar sinc_h_function(Scalar x) +} // namespace + +namespace mandos::core +{ + +Scalar sinc(Scalar x) { - const Scalar x2 = x * x; - const Scalar x4 = x2 * x2; if (abs(x) < THRESHOLD) { - return 1.0 / 15.0 + x2 / 210.0 + x4 / 7560.0; + const Scalar x2 = x * x; + const Scalar x4 = x2 * x2; + return 1.0 - (x2 / 6.0) + (x4 / 120.0); } - - return (-x2 * std::sin(x) - 3.0 * x * std::cos(x) + 3.0 * std::sin(x)) / (x4 * x); + return std::sin(x) / x; } Vec3 grad_sinc(const Vec3 &theta) @@ -113,7 +114,7 @@ Mat3 computeTangentialTransform(const Vec3 &phi) } const Scalar angle = phi.norm(); const Vec3 axis = phi / angle; - return sin(angle) / angle * Mat3::Identity() + (1.0 - sin(angle) / angle) * axis * axis.transpose() + + return sin(angle) / angle * Mat3::Identity() + (1.0 - (sin(angle) / angle)) * axis * axis.transpose() + (1.0 - cos(angle)) / angle * skew(axis); } @@ -156,7 +157,7 @@ Vec3 subtractAxisAngle(const Vec3 &a, const Vec3 &b) if (angle < THRESHOLD) { // a close to zero return clampAxisAngle(-b); } - return a / angle * (angle - 2 * M_PI) - b; + return a / angle * (angle - (2 * M_PI)) - b; } return a - b; } @@ -190,7 +191,8 @@ Vec3 composeAxisAngle(const Vec3 &a, const Vec3 &b) const Vec3 a_axis = a / a_angle; - Scalar new_angle = 2.0 * std::acos(cos_a_angle2 * cos_b_angle2 - sin_a_angle2 * sin_b_angle2 * b_axis.dot(a_axis)); + Scalar new_angle = + 2.0 * std::acos((cos_a_angle2 * cos_b_angle2) - (sin_a_angle2 * sin_b_angle2 * b_axis.dot(a_axis))); if (fabs(new_angle) < tol) { return Vec3::Zero(); diff --git a/tests/CosseratBendingRod/tst_CosseratBendingRod.cpp b/tests/CosseratBendingRod/tst_CosseratBendingRod.cpp index 5ede50fb..13358c34 100644 --- a/tests/CosseratBendingRod/tst_CosseratBendingRod.cpp +++ b/tests/CosseratBendingRod/tst_CosseratBendingRod.cpp @@ -24,7 +24,7 @@ template inline Eigen::Matrix3 rotationExpMap0(const Eigen::Vector3 &w) { using mandos::core::skew; - return Eigen::Matrix3::Identity() + skew(w) + 0.5 * skew(w) * skew(w); + return Eigen::Matrix3::Identity() + skew(w) + (0.5 * skew(w) * skew(w)); } } // namespace @@ -183,4 +183,4 @@ TEST_CASE("CosseratBendingRod") Catch::Matchers::WithinAbs(0, 1e-8)); } } -} \ No newline at end of file +} diff --git a/tests/CosseratRod/tst_CosseratRod.cpp b/tests/CosseratRod/tst_CosseratRod.cpp index dd6e29a3..2d830ca3 100644 --- a/tests/CosseratRod/tst_CosseratRod.cpp +++ b/tests/CosseratRod/tst_CosseratRod.cpp @@ -22,7 +22,7 @@ template inline Eigen::Matrix3 rotationExpMap0(const Eigen::Vector3 &w) { using mandos::core::skew; - return Eigen::Matrix3::Identity() + skew(w) + 0.5 * skew(w) * skew(w); + return Eigen::Matrix3::Identity() + skew(w) + (0.5 * skew(w) * skew(w)); } } // namespace diff --git a/tests/FixedProjection/tst_FixedProjection.cpp b/tests/FixedProjection/tst_FixedProjection.cpp index fef393fb..00f06b1b 100644 --- a/tests/FixedProjection/tst_FixedProjection.cpp +++ b/tests/FixedProjection/tst_FixedProjection.cpp @@ -24,7 +24,7 @@ TEST_CASE("FixedProjection") SECTION("2 constraint indices") { - projection.setIndices({3 * 4, 3 * 4 + 1, 3 * 4 + 2, 3 * 9, 3 * 9 + 1, 3 * 9 + 2}); + projection.setIndices({3 * 4, (3 * 4) + 1, (3 * 4) + 2, 3 * 9, (3 * 9) + 1, (3 * 9) + 2}); SECTION("Vec") { mandos::core::Vec v; @@ -34,22 +34,22 @@ TEST_CASE("FixedProjection") auto projectedV = v; projection.applyP(projectedV); - REQUIRE(projectedV[3 * 4 + 0] == 0); - REQUIRE(projectedV[3 * 4 + 1] == 0); - REQUIRE(projectedV[3 * 4 + 2] == 0); + REQUIRE(projectedV[(3 * 4) + 0] == 0); + REQUIRE(projectedV[(3 * 4) + 1] == 0); + REQUIRE(projectedV[(3 * 4) + 2] == 0); - REQUIRE(projectedV[3 * 9 + 0] == 0); - REQUIRE(projectedV[3 * 9 + 1] == 0); - REQUIRE(projectedV[3 * 9 + 2] == 0); + REQUIRE(projectedV[(3 * 9) + 0] == 0); + REQUIRE(projectedV[(3 * 9) + 1] == 0); + REQUIRE(projectedV[(3 * 9) + 2] == 0); // Check the rest are equal - projectedV[3 * 4 + 0] = v[3 * 4 + 0]; - projectedV[3 * 4 + 1] = v[3 * 4 + 1]; - projectedV[3 * 4 + 2] = v[3 * 4 + 2]; + projectedV[(3 * 4) + 0] = v[(3 * 4) + 0]; + projectedV[(3 * 4) + 1] = v[(3 * 4) + 1]; + projectedV[(3 * 4) + 2] = v[(3 * 4) + 2]; - projectedV[3 * 9 + 0] = v[3 * 9 + 0]; - projectedV[3 * 9 + 1] = v[3 * 9 + 1]; - projectedV[3 * 9 + 2] = v[3 * 9 + 2]; + projectedV[(3 * 9) + 0] = v[(3 * 9) + 0]; + projectedV[(3 * 9) + 1] = v[(3 * 9) + 1]; + projectedV[(3 * 9) + 2] = v[(3 * 9) + 2]; REQUIRE(projectedV.cwiseEqual(v).all()); } } -- GitLab From 1dce78c89a7652509fe1675d68a2a8ca60b05b4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mag=C3=AD=20Romany=C3=A0?= Date: Mon, 10 Feb 2025 12:34:16 +0100 Subject: [PATCH 6/6] Reuse populate generalized gradient MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Magí Romanyà --- src/Mandos/Core/Model.cpp | 109 +------------------------------------- src/Mandos/Core/Model.hpp | 1 - 2 files changed, 2 insertions(+), 108 deletions(-) diff --git a/src/Mandos/Core/Model.cpp b/src/Mandos/Core/Model.cpp index 99b9cdf6..a6b1b729 100644 --- a/src/Mandos/Core/Model.cpp +++ b/src/Mandos/Core/Model.cpp @@ -108,19 +108,6 @@ mandos::core::Scalar computeEnergyGradientAndHessian(SimulationObjectT &simulati namespace mandos::core { -void Model::clearSimulationObjectsGradients() -{ - utilities::static_for_each( - [](auto &simulationObjects) { - for (auto &simulationObject : simulationObjects) { - using SimulationObjectT = std::remove_cvref_t; - if constexpr (SimulationObjectT::hasPotentials || SimulationObjectT::hasInertias) { - simulationObject.mstate.clearGradient(); - } - } - }, - m_simulationObjects); -} void Model::populateGeneralizedGradient(Vec &gradient) { // Propagate the forces back to the free simulation objects @@ -224,53 +211,7 @@ Scalar Model::computeEnergyAndGradient(Scalar h, Vec &gradient) }, m_simulationObjects); - // Propagate the forces back to the free simulation objects - for (auto node : m_backwardSortedList) { - auto &simObjectV = m_simulationObjectsGraph[node]; - std::visit( - [this](const auto handle) { - const auto &simulationObject = this->simulationObject(handle); - using SimulationObjectT = std::remove_pointer_t>; - if constexpr (SimulationObjectT::hasMappings) { - utilities::static_for_each( - [this](const auto &mappings) { - for (const auto &mapping : mappings) { - auto fromHandle = mapping.from(); - auto toHandle = mapping.to(); - - auto &from = this->simulationObject(fromHandle); - auto &to = this->simulationObject(toHandle); - - mapping.applyJT(from.mstate.m_grad, to.mstate.m_grad); - } - }, - simulationObject.m_mappings); - } - }, - simObjectV); - } - - // And accumulate on the generalized gradient - auto offset = 0; - for (auto node : m_freeSimulationObjects) { - const auto &simObjectV = m_simulationObjectsGraph[node]; - std::visit( - [this, &gradient, &offset](auto handle) { - auto &simulationObject = this->simulationObject(handle); - const auto size = simulationObject.mstate.size(); - using SimulationObjectT = std::remove_reference_t>; - if constexpr (SimulationObjectT::hasProjections) { - mandos::core::utilities::static_for_each( - [&simulationObject](const auto &projection) { - projection.applyPT(simulationObject.mstate.gradientView()); - }, - simulationObject.projections); - } - simulationObject.mstate.gradient(gradient.segment(offset, size)); - offset += size; - }, - simObjectV); - } + populateGeneralizedGradient(gradient); return accumulatedEnergy; } @@ -295,53 +236,7 @@ Scalar Model::computeEnergyGradientAndHessian(Scalar h, Vec &gradient, SystemMat }, m_simulationObjects); - // Propagate the forces back to the free simulation objects - for (auto node : m_backwardSortedList) { - auto &simObjectV = m_simulationObjectsGraph[node]; - std::visit( - [this](auto handle) { - const auto &simulationObject = this->simulationObject(handle); - using SimulationObjectT = std::remove_reference_t>; - if constexpr (SimulationObjectT::hasMappings) { - utilities::static_for_each( - [this](const auto &mappings) { - for (const auto &mapping : mappings) { - const auto fromHandle = mapping.from(); - const auto toHandle = mapping.to(); - - auto &from = this->simulationObject(fromHandle); - auto &to = this->simulationObject(toHandle); - - mapping.applyJT(from.mstate.m_grad, to.mstate.m_grad); - } - }, - simulationObject.m_mappings); - } - }, - simObjectV); - } - - // And accumulate on the generalized gradient - auto offset = 0; - for (auto node : m_freeSimulationObjects) { - const auto &simObjectV = m_simulationObjectsGraph[node]; - std::visit( - [this, &gradient, &offset](auto handle) { - auto &simulationObject = this->simulationObject(handle); - const auto size = simulationObject.mstate.size(); - using SimulationObjectT = std::remove_reference_t>; - if constexpr (SimulationObjectT::hasProjections) { - mandos::core::utilities::static_for_each( - [&simulationObject](const auto &projection) { - projection.applyPT(simulationObject.mstate.gradientView()); - }, - simulationObject.projections); - } - simulationObject.mstate.gradient(gradient.segment(offset, size)); - offset += size; - }, - simObjectV); - } + populateGeneralizedGradient(gradient); hessian.setModel(*this); hessian.setBackwardSortedList(m_backwardSortedList); diff --git a/src/Mandos/Core/Model.hpp b/src/Mandos/Core/Model.hpp index 1fab7577..e69d57c2 100644 --- a/src/Mandos/Core/Model.hpp +++ b/src/Mandos/Core/Model.hpp @@ -314,7 +314,6 @@ public: const KinematicGraph &graph() const; private: - void clearSimulationObjectsGradients(); void populateGeneralizedGradient(Vec &gradient); /** * @brief The SimulationObjects that define the Model -- GitLab