<commit_before>//
// Copyright (c) 2019, <NAME>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of  nor the names of its contributors may be used to
//    endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <exotica_core/dynamics_solver.h>
#include <exotica_core/scene.h>

namespace exotica
{
template class AbstractDynamicsSolver<double, Eigen::Dynamic, Eigen::Dynamic>;

template <typename T, int NX, int NU>
AbstractDynamicsSolver<T, NX, NU>::AbstractDynamicsSolver() = default;

template <typename T, int NX, int NU>
AbstractDynamicsSolver<T, NX, NU>::~AbstractDynamicsSolver() = default;

template <typename T, int NX, int NU>
void AbstractDynamicsSolver<T, NX, NU>::InstantiateBase(const Initializer& init)
{
    Object::InstantiateObject(init);
    DynamicsSolverInitializer dynamics_solver_initializer = DynamicsSolverInitializer(init);
    this->SetDt(dynamics_solver_initializer.dt);
    this->SetIntegrator(dynamics_solver_initializer.Integrator);

    // Just store the control limits supplied.
    //  They will need to be reshaped to the correct size when num_velocities_ becomes known.
    raw_control_limits_low_ = dynamics_solver_initializer.ControlLimitsLow;
    raw_control_limits_high_ = dynamics_solver_initializer.ControlLimitsHigh;

    if (debug_) INFO_NAMED(object_name_, "Initialized DynamicsSolver of type " << GetObjectName() << " with dt=" << dynamics_solver_initializer.dt << " and integrator=" << dynamics_solver_initializer.Integrator);
}

template <typename T, int NX, int NU>
void AbstractDynamicsSolver<T, NX, NU>::AssignScene(std::shared_ptr<Scene> scene_in)
{
}

template <typename T, int NX, int NU>
void AbstractDynamicsSolver<T, NX, NU>::SetDt(double dt_in)
{
    if (dt_in < 0.0001) ThrowPretty("dt needs to be strictly greater than 0. Provided: " << dt_in);
    dt_ = dt_in;
}

template <typename T, int NX, int NU>
Eigen::Matrix<T, NX, 1> AbstractDynamicsSolver<T, NX, NU>::SimulateOneStep(const StateVector& x, const ControlVector& u)
{
    switch (integrator_)
    {
        // Forward Euler (RK1)
        case Integrator::RK1:
        {
            StateVector xdot = f(x, u);
            // TODO: This is the explicit RK1. We can switch to semi-implicit.
            return x + dt_ * xdot;
        }
        // Explicit trapezoid rule (RK2)
        case Integrator::RK2:
        {
            StateVector xdot0 = f(x, u);
            StateVector x1est = x + dt_ * xdot0;  // explicit Euler step
            StateVector xdot1 = f(x1est, u);

            // 2nd order result: x = x0 + dt (xd0+xd1)/2.
            return x + (dt_ / 2.) * (xdot0 + xdot1);
        }
        // Runge-Kutta 4
        case Integrator::RK4:
        {
            StateVector k1 = dt_ * f(x, u);
            StateVector k2 = dt_ * f(x + 0.5 * k1, u);<commit_msg>[exotica_core] DynamicsSolver: Use Integrate in SimulateOneStep<commit_after>//
// Copyright (c) 2019, <NAME>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of  nor the names of its contributors may be used to
//    endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <exotica_core/dynamics_solver.h>
#include <exotica_core/scene.h>

namespace exotica
{
template class AbstractDynamicsSolver<double, Eigen::Dynamic, Eigen::Dynamic>;

template <typename T, int NX, int NU>
AbstractDynamicsSolver<T, NX, NU>::AbstractDynamicsSolver() = default;

template <typename T, int NX, int NU>
AbstractDynamicsSolver<T, NX, NU>::~AbstractDynamicsSolver() = default;

template <typename T, int NX, int NU>
void AbstractDynamicsSolver<T, NX, NU>::InstantiateBase(const Initializer& init)
{
    Object::InstantiateObject(init);
    DynamicsSolverInitializer dynamics_solver_initializer = DynamicsSolverInitializer(init);
    this->SetDt(dynamics_solver_initializer.dt);
    this->SetIntegrator(dynamics_solver_initializer.Integrator);

    // Just store the control limits supplied.
    //  They will need to be reshaped to the correct size when num_velocities_ becomes known.
    raw_control_limits_low_ = dynamics_solver_initializer.ControlLimitsLow;
    raw_control_limits_high_ = dynamics_solver_initializer.ControlLimitsHigh;

    if (debug_) INFO_NAMED(object_name_, "Initialized DynamicsSolver of type " << GetObjectName() << " with dt=" << dynamics_solver_initializer.dt << " and integrator=" << dynamics_solver_initializer.Integrator);
}

template <typename T, int NX, int NU>
void AbstractDynamicsSolver<T, NX, NU>::AssignScene(std::shared_ptr<Scene> scene_in)
{
}

template <typename T, int NX, int NU>
void AbstractDynamicsSolver<T, NX, NU>::SetDt(double dt_in)
{
    if (dt_in < 0.0001) ThrowPretty("dt needs to be strictly greater than 0. Provided: " << dt_in);
    dt_ = dt_in;
}

template <typename T, int NX, int NU>
Eigen::Matrix<T, NX, 1> AbstractDynamicsSolver<T, NX, NU>::SimulateOneStep(const StateVector& x, const ControlVector& u)
{
    assert(num_positions_ == num_velocities_);  // If this is not true, we should have specialised methods.

    switch (integrator_)
    {
        // Forward Euler (RK1)
        case Integrator::RK1:
        {
            StateVector xdot = f(x, u);
            StateVector xout(get_num_state());
            Integrate(x, xdot, dt_, xout);
            return xout;
        }
        // Explicit trapezoid rule (RK2)
        case Integrator::RK2:
        {
            StateVector xdot0 = f(x, u);
            StateVector x1est = x + dt_ * xdot0;  // explicit Euler step
            StateVector xdot1 = f(x1est, u);

            // 2nd order result: x = x0 + dt (xd0+xd1)/2.
            return x + (dt_ / 2.) * (xdot0 + xdot1);
        }
        // Runge-Kutta 4
        case Integrator::RK4:
        {
            StateVector k1 = dt_ * f(x, u);
            StateVector k2 = dt_ * f(x + 0.5 * k1, u);
{'fraction_non_alphanumeric': 0.09392722117202268, 'fraction_numerical': 0.008388468809073724, 'mean_word_length': 3.507454739084132, 'pattern_counts': {'":': 0, '<': 65, '<?xml version=': 0, '>': 43, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6870704', 'n_tokens_mistral': 2641, 'n_tokens_neox': 2470, 'n_words': 893}