diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 7402960..261ce4a 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -20,6 +20,14 @@ if(UNGAR_ENABLE_OPTIMIZATION) target_link_libraries(ungar.rc_car.example ungar::ungar) endif() +if(UNGAR_ENABLE_PINOCCHIO) + add_executable(ungar.quantity.example rbd/quantity.example.cpp) + target_link_libraries(ungar.quantity.example ungar::ungar) + + add_executable(ungar.robot.example rbd/robot.example.cpp) + target_link_libraries(ungar.robot.example ungar::ungar) +endif() + # Add examples to the test targets if testing is enabled. if(UNGAR_BUILD_TESTS) add_test(NAME ungar.variable.example COMMAND ungar.variable.example) @@ -34,4 +42,9 @@ if(UNGAR_BUILD_TESTS) add_test(NAME ungar.quadruped.example COMMAND ungar.quadruped.example) add_test(NAME ungar.rc_car.example COMMAND ungar.rc_car.example) endif() + + if(UNGAR_ENABLE_PINOCCHIO) + add_test(NAME ungar.quantity.example COMMAND ungar.quantity.example) + add_test(NAME ungar.robot.example COMMAND ungar.robot.example) + endif() endif() diff --git a/example/rbd/quantity.example.cpp b/example/rbd/quantity.example.cpp new file mode 100644 index 0000000..0ae16e4 --- /dev/null +++ b/example/rbd/quantity.example.cpp @@ -0,0 +1,148 @@ +/****************************************************************************** + * + * @file ungar/example/rbd/quantity.example.cpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#include "ungar/rbd/robot.hpp" + +#include "pinocchio/algorithm/frames.hpp" +#include "ungar/mvariable_lazy_map.hpp" + +using namespace Ungar; + +namespace ANYmalB { +namespace Variables { + +/*************** ANYMAL B ***************/ +/*************** Define helper m-variables. ***************/ +inline constexpr auto NUM_LEGS = 4_idx; + +inline constexpr auto LF = 0_idx; +inline constexpr auto LH = 1_idx; +inline constexpr auto RF = 2_idx; +inline constexpr auto RH = 3_idx; + +inline constexpr auto FOOT_FRAME_IDS = std::array{12UL, 22UL, 32UL, 42UL}; + +UNGAR_LEAF_MVARIABLE(position, 3); +UNGAR_LEAF_MVARIABLE(orientation, Q); +UNGAR_BRANCH_MVARIABLE(base_pose, position, orientation); + +UNGAR_LEAF_MVARIABLE(hip_aa, 1); +UNGAR_LEAF_MVARIABLE(hip_fe, 1); +UNGAR_LEAF_MVARIABLE(knee_fe, 1); +UNGAR_BRANCH_MVARIABLE(leg_joint_coords, hip_aa, hip_fe, knee_fe); +UNGAR_MVARIABLE_ARRAY(joint_coords, leg_joint_coords, NUM_LEGS); +UNGAR_BRANCH_MVARIABLE(q, base_pose, joint_coords); + +UNGAR_LEAF_MVARIABLE(b_linear_velocity, 3); +UNGAR_LEAF_MVARIABLE(b_angular_velocity, 3); +UNGAR_BRANCH_MVARIABLE(base_twist, b_linear_velocity, b_angular_velocity); + +UNGAR_BRANCH_MVARIABLE(leg_joint_vels, hip_aa, hip_fe, knee_fe); +UNGAR_MVARIABLE_ARRAY(joint_vels, leg_joint_vels, NUM_LEGS); +UNGAR_BRANCH_MVARIABLE(v, base_twist, joint_vels); + +UNGAR_BRANCH_MVARIABLE(qv, q, v); + +} // namespace Variables + +namespace Quantities { + +UNGAR_MAKE_QUANTITY(foot_pose); + +} +} // namespace ANYmalB + +namespace Ungar { +namespace RBD { + +template +struct Evaluator { + void At(const auto& q) { + namespace vs = ANYmalB::Variables; + + pinocchio::forwardKinematics(model, data, q); + for (const auto frameID : vs::FOOT_FRAME_IDS) { + pinocchio::updateFramePlacement(model, data, frameID); + } + } + + const pinocchio::ModelTpl<_Scalar>& model; + pinocchio::DataTpl<_Scalar>& data; +}; + +template +struct Getter { + const auto& Get(const index_t foot) const { + namespace vs = ANYmalB::Variables; + + return data.oMf[vs::FOOT_FRAME_IDS[static_cast(foot)]]; + } + + auto& Get(const index_t foot) { + namespace vs = ANYmalB::Variables; + + return data.oMf[vs::FOOT_FRAME_IDS[static_cast(foot)]]; + } + + const pinocchio::ModelTpl<_Scalar>& model; + pinocchio::DataTpl<_Scalar>& data; +}; + +} // namespace RBD +} // namespace Ungar + +int main() { + namespace vs = ANYmalB::Variables; + namespace qs = ANYmalB::Quantities; + + /*************** Create robot. ***************/ + // Create the quadruped robot ANYmal B with Ungar. + const std::string urdfFilename{UNGAR_DATA_FOLDER + "/robots/anymal_b_description/robots/anymal.urdf"}; + Robot robot{urdfFilename}; + + /*************** Testing custom quantity. ***************/ + // Create data required by the RBD algorithms. + VectorXr q = robot.RandomConfiguration(); + + /// @note The limits of the free flyer joint default to infinite. The following + /// lines prevent infinite numbers that would break the computations. + { + // Create an m-lazy variable map for the configuration vector. + auto q_ = MakeMVariableLazyMap(q, vs::q); + q_.Get(vs::position).setRandom(); + } + + UNGAR_LOG(info, "Computing the pose of feet..."); + robot.Compute(qs::foot_pose).At(q); + UNGAR_LOG(info, "Done."); + for (const auto foot : {vs::LF, vs::LH, vs::RF, vs::RH}) { + UNGAR_LOG(info, "Pose of the {}-th foot:\n{}", foot, robot.Get(qs::foot_pose, foot)); + } + UNGAR_LOG(info, Utils::DASH_LINE_SEPARATOR); + + return 0; +} diff --git a/example/rbd/robot.example.cpp b/example/rbd/robot.example.cpp new file mode 100644 index 0000000..6c88a53 --- /dev/null +++ b/example/rbd/robot.example.cpp @@ -0,0 +1,164 @@ +/****************************************************************************** + * + * @file ungar/example/rbd/robot.example.cpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#include "ungar/rbd/robot.hpp" +#include "ungar/autodiff/function.hpp" +#include "ungar/mvariable_lazy_map.hpp" + +#include "ungar/rbd/quantities/centroidal_momentum.hpp" +#include "ungar/rbd/quantities/com_acceleration.hpp" +#include "ungar/rbd/quantities/composite_rigid_body_inertia.hpp" +#include "ungar/rbd/quantities/generalized_accelerations.hpp" + +using namespace Ungar; + +namespace ANYmalB { +namespace Variables { + +/*************** ANYMAL B ***************/ +/*************** Define helper m-variables. ***************/ +inline constexpr auto NUM_LEGS = 4_idx; + +inline constexpr auto LF = 0_idx; +inline constexpr auto LH = 1_idx; +inline constexpr auto RF = 2_idx; +inline constexpr auto RH = 3_idx; + +inline constexpr auto LF_FOOT_FRAME_ID = 12UL; +inline constexpr auto LH_FOOT_FRAME_ID = 22UL; +inline constexpr auto RF_FOOT_FRAME_ID = 32UL; +inline constexpr auto RH_FOOT_FRAME_ID = 42UL; + +UNGAR_LEAF_MVARIABLE(position, 3); +UNGAR_LEAF_MVARIABLE(orientation, Q); +UNGAR_BRANCH_MVARIABLE(base_pose, position, orientation); + +UNGAR_LEAF_MVARIABLE(hip_aa, 1); +UNGAR_LEAF_MVARIABLE(hip_fe, 1); +UNGAR_LEAF_MVARIABLE(knee_fe, 1); +UNGAR_BRANCH_MVARIABLE(leg_joint_coords, hip_aa, hip_fe, knee_fe); +UNGAR_MVARIABLE_ARRAY(joint_coords, leg_joint_coords, NUM_LEGS); +UNGAR_BRANCH_MVARIABLE(q, base_pose, joint_coords); + +UNGAR_LEAF_MVARIABLE(b_linear_velocity, 3); +UNGAR_LEAF_MVARIABLE(b_angular_velocity, 3); +UNGAR_BRANCH_MVARIABLE(base_twist, b_linear_velocity, b_angular_velocity); + +UNGAR_BRANCH_MVARIABLE(leg_joint_vels, hip_aa, hip_fe, knee_fe); +UNGAR_MVARIABLE_ARRAY(joint_vels, leg_joint_vels, NUM_LEGS); +UNGAR_BRANCH_MVARIABLE(v, base_twist, joint_vels); + +UNGAR_BRANCH_MVARIABLE(qv, q, v); + +} // namespace Variables +} // namespace ANYmalB + +int main() { + namespace vs = ANYmalB::Variables; + namespace qs = RBD::Quantities; + + /*************** Create robot. ***************/ + // Create the quadruped robot ANYmal B with Ungar. + const std::string urdfFilename{UNGAR_DATA_FOLDER + "/robots/anymal_b_description/robots/anymal.urdf"}; + Robot robot{urdfFilename}; + + // Log information about ANYmal B. + UNGAR_LOG(info, "Model name: {}", robot.Model().name); + UNGAR_LOG(info, "Dimension of configuration vector representation: {}", robot.Model().nq); + UNGAR_LOG(info, "Dimension of velocity vector space: {}", robot.Model().nv); + UNGAR_LOG(info, "Number of joints: {}", robot.Model().njoints); + UNGAR_LOG(info, Utils::DASH_LINE_SEPARATOR); + + /*************** Use algorithms. ***************/ + // Create data required by the RBD algorithms. + VectorXr q = robot.RandomConfiguration(); + VectorXr v = VectorXr::Random(vs::v.Size()); + VectorXr a = VectorXr::Random(vs::v.Size()); + VectorXr tau = VectorXr::Random(vs::v.Size()); + + /// @note The limits of the free flyer joint default to infinite. The following + /// lines prevent infinite numbers that would break the computations. + { + // Create an m-lazy variable map for the configuration vector. + auto q_ = MakeMVariableLazyMap(q, vs::q); + q_.Get(vs::position).setRandom(); + } + + UNGAR_LOG(info, "Computing the center of mass acceleration..."); + auto res = robot.Compute(qs::com_acceleration); + res.At(q, v, a); + UNGAR_LOG(info, "Done. Center of mass acceleration:\n{}", robot.Get(qs::com_acceleration)); + UNGAR_LOG(info, Utils::DASH_LINE_SEPARATOR); + + UNGAR_LOG(info, "Computing the composite rigid body inertia..."); + robot.Compute(qs::composite_rigid_body_inertia).At(q, v); + UNGAR_LOG(info, + "Done. Composite rigid body inertia:\n{}", + robot.Get(qs::composite_rigid_body_inertia)); + UNGAR_LOG(info, Utils::DASH_LINE_SEPARATOR); + + UNGAR_LOG(info, "Computing the generalized accelerations..."); + robot.Compute(qs::generalized_accelerations).At(q, v, tau); + UNGAR_LOG( + info, "Done. Generalized accelerations:\n{}", robot.Get(qs::generalized_accelerations)); + UNGAR_LOG(info, Utils::DASH_LINE_SEPARATOR); + + /*************** Generate derivatives. ***************/ + // Implement autodiff function that computes the centroidal momentum of the robot. + UNGAR_LOG( + info, + "Generating the autodiff function to compute the centroidal momentum of the robot..."); + auto functionImpl = [&](const VectorXad& qvUnderlying, VectorXad& y) -> void { + Robot robotAD{urdfFilename}; + const auto qv_ = MakeMVariableLazyMap(qvUnderlying, vs::qv); + robotAD.Compute(qs::centroidal_momentum).At(qv_.Get(vs::q), qv_.Get(vs::v)); + y = robotAD.Get(qs::centroidal_momentum).toVector_impl(); + }; + Autodiff::Function::Blueprint blueprint{ + functionImpl, vs::qv.Size(), 0_idx, "robot_example", EnabledDerivatives::JACOBIAN}; + Autodiff::Function function = MakeFunction(blueprint, true); + + // Compare results. + robot.Compute(qs::centroidal_momentum).At(q, v); + + VectorXr qv{q.size() + v.size()}; + qv << q, v; + const bool ok = Utils::CompareMatrices(function(qv), + "Autodiff function"sv, + robot.Get(qs::centroidal_momentum).toVector_impl(), + "Ground truth"sv); + UNGAR_ASSERT(ok); + + UNGAR_LOG(info, "Autodiff function:\n{}", function(qv).transpose()); + UNGAR_LOG( + info, "Ground truth:\n{}", robot.Get(qs::centroidal_momentum).toVector_impl().transpose()); + UNGAR_LOG(info, + "Autodiff Jacobian (6 rightmost columns):\n{}", + function.Jacobian(qv).toDense().rightCols(6_idx)); + + return 0; +}