From cedf0be27d3cc96bab80c8301c4f089871544489 Mon Sep 17 00:00:00 2001 From: Duc Doan Date: Mon, 29 Jan 2024 17:36:33 -0500 Subject: [PATCH 1/9] Template instantiation macros --- .../huron/utils/template_instantiations.h | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 utils/include/huron/utils/template_instantiations.h diff --git a/utils/include/huron/utils/template_instantiations.h b/utils/include/huron/utils/template_instantiations.h new file mode 100644 index 00000000..59016703 --- /dev/null +++ b/utils/include/huron/utils/template_instantiations.h @@ -0,0 +1,22 @@ +#pragma once + +/** + * These macros are inspired by Drake. + * Source: https://github.com/RobotLocomotion/drake/blob/master/common/default_scalars.h + */ + +/** + * Defines template instantiations for huron's default scalars. + * This should only be used in .cc files, never in .h files. + */ +#define HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( \ + SomeType) \ +template SomeType; + +/** + * Declares that template instantiations exist for huron's default scalars. + * This should only be used in .h files, never in .cc files. + */ +#define HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( \ + SomeType) \ +extern template SomeType; From bef645548afe4617d977a9061b27b7676afd98fa Mon Sep 17 00:00:00 2001 From: Duc Doan Date: Wed, 31 Jan 2024 14:15:08 -0500 Subject: [PATCH 2/9] Add forward declaration header --- multibody/include/huron/multibody/fwd.h | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 multibody/include/huron/multibody/fwd.h diff --git a/multibody/include/huron/multibody/fwd.h b/multibody/include/huron/multibody/fwd.h new file mode 100644 index 00000000..276a27c3 --- /dev/null +++ b/multibody/include/huron/multibody/fwd.h @@ -0,0 +1,9 @@ +#pragma once + +namespace huron { +namespace multibody { + +template class Model; + +} // namespace multibody +} // namespace huron From 89311a961d8347657daca0594dbe490589ee9832 Mon Sep 17 00:00:00 2001 From: Duc Doan Date: Wed, 31 Jan 2024 14:15:31 -0500 Subject: [PATCH 3/9] Templatize with double, all tests working --- multibody/include/huron/multibody/com_frame.h | 12 +- multibody/include/huron/multibody/frame.h | 16 ++- .../include/huron/multibody/logical_frame.h | 17 ++- multibody/include/huron/multibody/model.h | 19 +-- .../huron/multibody/model_impl_factory.h | 4 +- multibody/src/com_frame.cc | 40 +++--- multibody/src/frame.cc | 23 ++-- multibody/src/logical_frame.cc | 35 ++++-- multibody/src/model.cc | 115 ++++++++++++------ multibody/test/test_model.cc | 2 +- .../huron/control_interfaces/legged_robot.h | 10 +- .../include/huron/control_interfaces/robot.h | 7 +- .../control_interfaces/sensor_with_frame.h | 7 +- system/control_interfaces/src/legged_robot.cc | 19 ++- system/control_interfaces/src/robot.cc | 40 +++--- .../src/sensor_with_frame.cc | 23 ++-- .../test/test_legged_zmp.cc | 12 +- .../huron/locomotion/zero_moment_point.h | 8 +- .../locomotion/zero_moment_point_fsr_array.h | 12 +- .../locomotion/zero_moment_point_ft_sensor.h | 12 +- .../locomotion/zero_moment_point_total.h | 12 +- system/locomotion/src/zero_moment_point.cc | 12 +- .../src/zero_moment_point_fsr_array.cc | 17 ++- .../src/zero_moment_point_ft_sensor.cc | 21 ++-- .../locomotion/src/zero_moment_point_total.cc | 15 ++- .../locomotion/test/test_zero_moment_point.cc | 14 +-- .../huron/sensors/force_sensing_resistor.h | 10 +- .../sensors/force_sensing_resistor_array.h | 16 ++- .../force_sensing_resistor_array_serial.h | 10 +- .../huron/sensors/force_torque_sensor.h | 10 +- system/sensors/src/force_sensing_resistor.cc | 17 ++- .../src/force_sensing_resistor_array.cc | 30 +++-- .../force_sensing_resistor_array_serial.cc | 8 +- system/sensors/src/force_torque_sensor.cc | 26 ++-- 34 files changed, 424 insertions(+), 227 deletions(-) diff --git a/multibody/include/huron/multibody/com_frame.h b/multibody/include/huron/multibody/com_frame.h index e861c80c..46a1e819 100644 --- a/multibody/include/huron/multibody/com_frame.h +++ b/multibody/include/huron/multibody/com_frame.h @@ -11,12 +11,13 @@ namespace multibody { /** * @brief Robot center of mass frame. */ -class ComFrame : public Frame { +template +class ComFrame : public Frame { public: ComFrame(FrameIndex index, const std::string& name, bool is_user_defined, - std::weak_ptr model, + std::weak_ptr> model, FrameIndex parent_frame_index); ComFrame(const ComFrame&) = delete; @@ -24,9 +25,9 @@ class ComFrame : public Frame { ~ComFrame() override = default; Eigen::Affine3d GetTransformInWorld() const override; - Eigen::Affine3d GetTransformFromFrame(const Frame& other) const override; + Eigen::Affine3d GetTransformFromFrame(const Frame& other) const override; Eigen::Affine3d GetTransformFromFrame(FrameIndex other) const override; - Eigen::Affine3d GetTransformToFrame(const Frame& other) const override; + Eigen::Affine3d GetTransformToFrame(const Frame& other) const override; Eigen::Affine3d GetTransformToFrame(FrameIndex other) const override; private: @@ -37,3 +38,6 @@ class ComFrame : public Frame { } // namespace multibody } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::ComFrame) diff --git a/multibody/include/huron/multibody/frame.h b/multibody/include/huron/multibody/frame.h index bcd1aedf..a0126adf 100644 --- a/multibody/include/huron/multibody/frame.h +++ b/multibody/include/huron/multibody/frame.h @@ -5,13 +5,13 @@ #include #include +#include "huron/multibody/fwd.h" +#include "huron/utils/template_instantiations.h" #include "huron/enable_protected_make_shared.h" namespace huron { namespace multibody { -class Model; - using FrameIndex = size_t; enum class FrameType { @@ -22,9 +22,10 @@ enum class FrameType { kPhysical, }; -class Frame : public enable_protected_make_shared { +template +class Frame : public enable_protected_make_shared> { public: - friend class Model; + friend class Model; Frame(const Frame&) = delete; Frame& operator=(const Frame&) = delete; @@ -46,15 +47,18 @@ class Frame : public enable_protected_make_shared { const std::string& name, FrameType type, bool is_user_defined, - std::weak_ptr model); + std::weak_ptr> model); /// \brief Frame name. const FrameIndex index_; const std::string name_; const FrameType type_; bool is_user_defined_; - const std::weak_ptr model_; + const std::weak_ptr> model_; }; } // namespace multibody } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::Frame) diff --git a/multibody/include/huron/multibody/logical_frame.h b/multibody/include/huron/multibody/logical_frame.h index 32d0797a..56c839ed 100644 --- a/multibody/include/huron/multibody/logical_frame.h +++ b/multibody/include/huron/multibody/logical_frame.h @@ -3,6 +3,7 @@ #include #include +#include "huron/multibody/fwd.h" #include "huron/multibody/frame.h" namespace huron { @@ -25,25 +26,28 @@ namespace multibody { * @param transform_function The function that defines the transformation from * the parent frame to this frame. */ -class LogicalFrame : public Frame, enable_protected_make_shared { +template +class LogicalFrame + : public Frame, + enable_protected_make_shared> { public: - friend class Model; + friend class Model; LogicalFrame(const LogicalFrame&) = delete; LogicalFrame& operator=(const LogicalFrame&) = delete; ~LogicalFrame() override = default; Eigen::Affine3d GetTransformInWorld() const override; - Eigen::Affine3d GetTransformFromFrame(const Frame& other) const override; + Eigen::Affine3d GetTransformFromFrame(const Frame& other) const override; Eigen::Affine3d GetTransformFromFrame(FrameIndex other) const override; - Eigen::Affine3d GetTransformToFrame(const Frame& other) const override; + Eigen::Affine3d GetTransformToFrame(const Frame& other) const override; Eigen::Affine3d GetTransformToFrame(FrameIndex other) const override; protected: LogicalFrame(FrameIndex index, const std::string& name, bool is_user_defined, - std::weak_ptr model, + std::weak_ptr> model, FrameIndex parent_frame_index, std::function transform_function); @@ -56,3 +60,6 @@ class LogicalFrame : public Frame, enable_protected_make_shared { } // namespace multibody } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::LogicalFrame) diff --git a/multibody/include/huron/multibody/model.h b/multibody/include/huron/multibody/model.h index 523d3a51..9b359d4d 100644 --- a/multibody/include/huron/multibody/model.h +++ b/multibody/include/huron/multibody/model.h @@ -6,6 +6,7 @@ #include #include +#include "huron/utils/template_instantiations.h" #include "huron/multibody/model_impl_types.h" #include "huron/multibody/model_impl_interface.h" #include "huron/multibody/frame.h" @@ -13,7 +14,8 @@ namespace huron { namespace multibody { -class Model : public std::enable_shared_from_this { +template +class Model : public std::enable_shared_from_this> { using ModelImplInterface = internal::ModelImplInterface; public: @@ -73,9 +75,9 @@ class Model : public std::enable_shared_from_this { // AddFrame(const std::string& name, Args&&... args); template - std::weak_ptr AddFrame(const std::string& name, Args&&... args) { + std::weak_ptr> AddFrame(const std::string& name, Args&&... args) { static_assert( - std::is_base_of_v, + std::is_base_of_v, FrameImpl>, "Invalid frame type."); static_assert( std::is_base_of_v, FrameImpl>, @@ -88,8 +90,8 @@ class Model : public std::enable_shared_from_this { return frames_.back(); } - std::weak_ptr GetFrame(FrameIndex index) const; - std::weak_ptr GetFrame(const std::string& name) const; + std::weak_ptr> GetFrame(FrameIndex index) const; + std::weak_ptr> GetFrame(const std::string& name) const; void BuildFromUrdf(const std::string& urdf_path, JointType root_joint_type = JointType::kFixed); @@ -225,7 +227,7 @@ class Model : public std::enable_shared_from_this { frames_.size(), // frame index name, // frame name is_user_defined, - weak_from_this(), // model + this->weak_from_this(), // model std::forward(args)...)); frame_name_to_index_[name] = frames_.size() - 1; } @@ -244,7 +246,7 @@ class Model : public std::enable_shared_from_this { /// \brief All frames, including those defined by the model description file /// and user-defined ones. - std::vector> frames_; + std::vector>> frames_; std::unordered_map frame_name_to_index_; bool is_constructed_ = false; @@ -253,3 +255,6 @@ class Model : public std::enable_shared_from_this { } // namespace multibody } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::Model) diff --git a/multibody/include/huron/multibody/model_impl_factory.h b/multibody/include/huron/multibody/model_impl_factory.h index e23345e4..8cf99017 100644 --- a/multibody/include/huron/multibody/model_impl_factory.h +++ b/multibody/include/huron/multibody/model_impl_factory.h @@ -2,6 +2,7 @@ #include +#include "fwd.h" #include "model_impl_types.h" #include "model_impl_interface.h" #include "pinocchio_model_impl.h" @@ -10,8 +11,9 @@ namespace huron { namespace multibody { namespace internal { +template class ModelImplFactory final { - friend class multibody::Model; + friend class Model; public: ModelImplFactory() = delete; ModelImplFactory(const ModelImplFactory&) = delete; diff --git a/multibody/src/com_frame.cc b/multibody/src/com_frame.cc index 86d8c215..2ceb1710 100644 --- a/multibody/src/com_frame.cc +++ b/multibody/src/com_frame.cc @@ -4,50 +4,60 @@ namespace huron { namespace multibody { -ComFrame::ComFrame( +template +ComFrame::ComFrame( FrameIndex index, const std::string& name, bool is_user_defined, - std::weak_ptr model, + std::weak_ptr> model, FrameIndex parent_frame_index) - : Frame(index, name, FrameType::kLogical, is_user_defined, std::move(model)), + : Frame(index, name, FrameType::kLogical, is_user_defined, std::move(model)), parent_frame_index_(parent_frame_index) {} -Eigen::Affine3d ComFrame::GetTransformInWorld() const { +template +Eigen::Affine3d ComFrame::GetTransformInWorld() const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransformInWorld(parent_frame_index_); + this->model_.lock()->GetFrameTransformInWorld(parent_frame_index_); return parent_transform * ParentToThisTransform(); } -Eigen::Affine3d ComFrame::GetTransformFromFrame(const Frame& other) const { +template +Eigen::Affine3d ComFrame::GetTransformFromFrame(const Frame& other) const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransform(other.index(), parent_frame_index_); + this->model_.lock()->GetFrameTransform(other.index(), parent_frame_index_); return parent_transform * ParentToThisTransform(); } -Eigen::Affine3d ComFrame::GetTransformFromFrame(FrameIndex other) const { +template +Eigen::Affine3d ComFrame::GetTransformFromFrame(FrameIndex other) const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransform(other, parent_frame_index_); + this->model_.lock()->GetFrameTransform(other, parent_frame_index_); return parent_transform * ParentToThisTransform(); } -Eigen::Affine3d ComFrame::GetTransformToFrame(const Frame& other) const { +template +Eigen::Affine3d ComFrame::GetTransformToFrame(const Frame& other) const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransform(parent_frame_index_, other.index()); + this->model_.lock()->GetFrameTransform(parent_frame_index_, other.index()); return ParentToThisTransform().inverse() * parent_transform; } -Eigen::Affine3d ComFrame::GetTransformToFrame(FrameIndex other) const { +template +Eigen::Affine3d ComFrame::GetTransformToFrame(FrameIndex other) const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransform(parent_frame_index_, other); + this->model_.lock()->GetFrameTransform(parent_frame_index_, other); return ParentToThisTransform().inverse() * parent_transform; } -Eigen::Affine3d ComFrame::ParentToThisTransform() const { +template +Eigen::Affine3d ComFrame::ParentToThisTransform() const { Eigen::Affine3d parent_to_this = Eigen::Affine3d::Identity(); - parent_to_this.translate(model_.lock()->GetCenterOfMassPosition()); + parent_to_this.translate(this->model_.lock()->GetCenterOfMassPosition()); return parent_to_this; } } // namespace multibody } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::ComFrame) diff --git a/multibody/src/frame.cc b/multibody/src/frame.cc index 5ba6a006..30882fdd 100644 --- a/multibody/src/frame.cc +++ b/multibody/src/frame.cc @@ -4,36 +4,45 @@ namespace huron { namespace multibody { -Frame::Frame(FrameIndex index, +template +Frame::Frame(FrameIndex index, const std::string& name, FrameType type, bool is_user_defined, - std::weak_ptr model) + std::weak_ptr> model) : index_(index), name_(name), type_(type), is_user_defined_(is_user_defined), model_(std::move(model)) {} -Eigen::Affine3d Frame::GetTransformInWorld() const { +template +Eigen::Affine3d Frame::GetTransformInWorld() const { return model_.lock()->GetFrameTransformInWorld(index_); } -Eigen::Affine3d Frame::GetTransformFromFrame(const Frame& other) const { +template +Eigen::Affine3d Frame::GetTransformFromFrame(const Frame& other) const { return model_.lock()->GetFrameTransform(other.index_, index_); } -Eigen::Affine3d Frame::GetTransformFromFrame(FrameIndex other) const { +template +Eigen::Affine3d Frame::GetTransformFromFrame(FrameIndex other) const { return model_.lock()->GetFrameTransform(other, index_); } -Eigen::Affine3d Frame::GetTransformToFrame(const Frame& other) const { +template +Eigen::Affine3d Frame::GetTransformToFrame(const Frame& other) const { return model_.lock()->GetFrameTransform(index_, other.index_); } -Eigen::Affine3d Frame::GetTransformToFrame(FrameIndex other) const { +template +Eigen::Affine3d Frame::GetTransformToFrame(FrameIndex other) const { return model_.lock()->GetFrameTransform(index_, other); } } // namespace multibody } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::Frame) diff --git a/multibody/src/logical_frame.cc b/multibody/src/logical_frame.cc index 07e2814a..59f7b8e4 100644 --- a/multibody/src/logical_frame.cc +++ b/multibody/src/logical_frame.cc @@ -4,46 +4,55 @@ namespace huron { namespace multibody { -LogicalFrame::LogicalFrame( +template +LogicalFrame::LogicalFrame( FrameIndex index, const std::string& name, bool is_user_defined, - std::weak_ptr model, + std::weak_ptr> model, FrameIndex parent_frame_index, std::function transform_function) - : Frame(index, name, FrameType::kLogical, is_user_defined, std::move(model)), + : Frame(index, name, FrameType::kLogical, is_user_defined, std::move(model)), parent_frame_index_(parent_frame_index), transform_function_(transform_function) {} -Eigen::Affine3d LogicalFrame::GetTransformInWorld() const { +template +Eigen::Affine3d LogicalFrame::GetTransformInWorld() const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransformInWorld(parent_frame_index_); + this->model_.lock()->GetFrameTransformInWorld(parent_frame_index_); return parent_transform * transform_function_(parent_transform); } -Eigen::Affine3d LogicalFrame::GetTransformFromFrame(const Frame& other) const { +template +Eigen::Affine3d LogicalFrame::GetTransformFromFrame(const Frame& other) const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransform(other.index(), parent_frame_index_); + this->model_.lock()->GetFrameTransform(other.index(), parent_frame_index_); return parent_transform * transform_function_(parent_transform); } -Eigen::Affine3d LogicalFrame::GetTransformFromFrame(FrameIndex other) const { +template +Eigen::Affine3d LogicalFrame::GetTransformFromFrame(FrameIndex other) const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransform(other, parent_frame_index_); + this->model_.lock()->GetFrameTransform(other, parent_frame_index_); return parent_transform * transform_function_(parent_transform); } -Eigen::Affine3d LogicalFrame::GetTransformToFrame(const Frame& other) const { +template +Eigen::Affine3d LogicalFrame::GetTransformToFrame(const Frame& other) const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransform(parent_frame_index_, other.index()); + this->model_.lock()->GetFrameTransform(parent_frame_index_, other.index()); return transform_function_(parent_transform).inverse() * parent_transform; } -Eigen::Affine3d LogicalFrame::GetTransformToFrame(FrameIndex other) const { +template +Eigen::Affine3d LogicalFrame::GetTransformToFrame(FrameIndex other) const { Eigen::Affine3d parent_transform = - model_.lock()->GetFrameTransform(parent_frame_index_, other); + this->model_.lock()->GetFrameTransform(parent_frame_index_, other); return transform_function_(parent_transform).inverse() * parent_transform; } } // namespace multibody } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::LogicalFrame) diff --git a/multibody/src/model.cc b/multibody/src/model.cc index b31d7f08..939527fe 100644 --- a/multibody/src/model.cc +++ b/multibody/src/model.cc @@ -4,21 +4,25 @@ namespace huron { namespace multibody { -Model::Model() = default; +template +Model::Model() = default; -void Model::AddModelImpl(ModelImplType type, +template +void Model::AddModelImpl(ModelImplType type, bool set_as_default) { - impls_.push_back(internal::ModelImplFactory::Create(type)); + impls_.push_back(internal::ModelImplFactory::Create(type)); if (set_as_default) { default_impl_index_ = impls_.size() - 1; } } -internal::ModelImplInterface const * Model::GetModelImpl(size_t index) const { +template +internal::ModelImplInterface const * Model::GetModelImpl(size_t index) const { return impls_[index].get(); } -void Model::BuildFromUrdf(const std::string& urdf_path, +template +void Model::BuildFromUrdf(const std::string& urdf_path, JointType root_joint_type) { assert(!is_finalized_); if (impls_.empty()) { @@ -33,7 +37,7 @@ void Model::BuildFromUrdf(const std::string& urdf_path, joints_ = std::vector>( impls_[default_impl_index_]->num_joints()); // Initialize the frame vector - frames_ = std::vector>( + frames_ = std::vector>>( impls_[default_impl_index_]->num_frames()); auto joint_names = impls_[default_impl_index_]->GetJointNames(); @@ -58,7 +62,8 @@ void Model::BuildFromUrdf(const std::string& urdf_path, is_constructed_ = true; } -void Model::Finalize(const Eigen::VectorXd& initial_state) { +template +void Model::Finalize(const Eigen::VectorXd& initial_state) { assert(is_constructed_); // Check if all joints are added to the model, and set the joint indices. for (auto& joint : joints_) { @@ -74,45 +79,52 @@ void Model::Finalize(const Eigen::VectorXd& initial_state) { is_finalized_ = true; } -void Model::Finalize() { +template +void Model::Finalize() { assert(is_constructed_); - Model::Finalize(Eigen::VectorXd::Zero(num_positions_ + num_velocities_)); + Finalize(Eigen::VectorXd::Zero(num_positions_ + num_velocities_)); } -Joint* const Model::GetJoint(JointIndex index) { +template +Joint* const Model::GetJoint(JointIndex index) { assert(is_constructed_); return joints_[index].get(); } -Joint* const Model::GetJoint(const std::string& name) { +template +Joint* const Model::GetJoint(const std::string& name) { assert(is_constructed_); return joints_[GetJointIndex(name)].get(); } -void Model::SetJointStateProvider( +template +void Model::SetJointStateProvider( JointIndex index, std::shared_ptr state_provider) { assert(is_constructed_); joints_[index]->SetStateProvider(std::move(state_provider)); } -JointIndex Model::GetJointIndex(const std::string& joint_name) const { +template +JointIndex Model::GetJointIndex(const std::string& joint_name) const { return impls_[default_impl_index_]->GetJointIndex(joint_name); } -void Model::DoAddFrameFromModelDescription(FrameIndex idx, +template +void Model::DoAddFrameFromModelDescription(FrameIndex idx, const std::string& name, FrameType type) { - frames_[idx] = Frame::make_shared( + frames_[idx] = Frame::make_shared( idx, name, type, false, - weak_from_this()); + this->weak_from_this()); frame_name_to_index_[name] = idx; } -void Model::UpdateJointStates() { +template +void Model::UpdateJointStates() { assert(is_finalized_); for (auto& joint : joints_) { if (joint->Info()->type() == JointType::kUnknown) { @@ -126,114 +138,136 @@ void Model::UpdateJointStates() { } } -std::weak_ptr Model::GetFrame(FrameIndex index) const { +template +std::weak_ptr> Model::GetFrame(FrameIndex index) const { return frames_[index]; } -std::weak_ptr Model::GetFrame(const std::string& name) const { +template +std::weak_ptr> Model::GetFrame(const std::string& name) const { return frames_[frame_name_to_index_.at(name)]; } // Kinematics and Dynamics functions -Eigen::Affine3d Model::GetJointTransformInWorld(size_t joint_index) const { +template +Eigen::Affine3d Model::GetJointTransformInWorld(size_t joint_index) const { assert(is_finalized_); return impls_[default_impl_index_]->GetJointTransformInWorld(joint_index); } -FrameIndex Model::GetFrameIndex(const std::string& frame_name) const { +template +FrameIndex Model::GetFrameIndex(const std::string& frame_name) const { assert(is_finalized_); return impls_[default_impl_index_]->GetFrameIndex(frame_name); } -Eigen::Affine3d Model::GetFrameTransform(FrameIndex from_frame, +template +Eigen::Affine3d Model::GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const { assert(is_finalized_); return impls_[default_impl_index_]->GetFrameTransform(from_frame, to_frame); } -Eigen::Affine3d Model::GetFrameTransformInWorld(FrameIndex frame) const { +template +Eigen::Affine3d Model::GetFrameTransformInWorld(FrameIndex frame) const { assert(is_finalized_); return impls_[default_impl_index_]->GetFrameTransformInWorld(frame); } -Eigen::VectorXd Model::NeutralConfiguration() const { +template +Eigen::VectorXd Model::NeutralConfiguration() const { assert(is_finalized_); return impls_[default_impl_index_]->NeutralConfiguration(); } -Eigen::Vector3d Model::EvalCenterOfMassPosition() { +template +Eigen::Vector3d Model::EvalCenterOfMassPosition() { assert(is_finalized_); return impls_[default_impl_index_]->EvalCenterOfMassPosition(); } -Eigen::Vector3d Model::GetCenterOfMassPosition() const { +template +Eigen::Vector3d Model::GetCenterOfMassPosition() const { assert(is_finalized_); return impls_[default_impl_index_]->GetCenterOfMassPosition(); } -const Eigen::VectorBlock Model::GetPositions() const { +template +const Eigen::VectorBlock Model::GetPositions() const { assert(is_finalized_); return states_.segment(0, num_positions_); } -const Eigen::VectorBlock Model::GetVelocities() const { +template +const Eigen::VectorBlock Model::GetVelocities() const { assert(is_finalized_); return states_.segment(num_positions_, num_velocities_); } -const Eigen::VectorXd& Model::GetAccelerations() const { +template +const Eigen::VectorXd& Model::GetAccelerations() const { assert(is_finalized_); return impls_[default_impl_index_]->GetAccelerations(); } -const Eigen::VectorXd& Model::GetTorques() const { +template +const Eigen::VectorXd& Model::GetTorques() const { assert(is_finalized_); return impls_[default_impl_index_]->GetTorques(); } -const Eigen::MatrixXd& Model::GetMassMatrix() const { +template +const Eigen::MatrixXd& Model::GetMassMatrix() const { assert(is_finalized_); return impls_[default_impl_index_]->GetMassMatrix(); } -const Eigen::MatrixXd& Model::GetCoriolisMatrix() const { +template +const Eigen::MatrixXd& Model::GetCoriolisMatrix() const { assert(is_finalized_); return impls_[default_impl_index_]->GetCoriolisMatrix(); } -const Eigen::VectorXd& Model::GetNonlinearEffects() const { +template +const Eigen::VectorXd& Model::GetNonlinearEffects() const { assert(is_finalized_); return impls_[default_impl_index_]->GetNonlinearEffects(); } -const Eigen::VectorXd& Model::GetGravity() const { +template +const Eigen::VectorXd& Model::GetGravity() const { assert(is_finalized_); return impls_[default_impl_index_]->GetGravity(); } -const huron::Vector6d& Model::GetSpatialMomentum() const { +template +const huron::Vector6d& Model::GetSpatialMomentum() const { assert(is_finalized_); return impls_[default_impl_index_]->GetSpatialMomentum(); } -huron::Vector6d Model::GetCentroidalMomentum() const { +template +huron::Vector6d Model::GetCentroidalMomentum() const { assert(is_finalized_); return impls_[default_impl_index_]->GetCentroidalMomentum(); } -const huron::Matrix6Xd& Model::GetCentroidalMatrix() const { +template +const huron::Matrix6Xd& Model::GetCentroidalMatrix() const { assert(is_finalized_); return impls_[default_impl_index_]->GetCentroidalMatrix(); } -void Model::ComputeAll() { +template +void Model::ComputeAll() { assert(is_finalized_); impls_[default_impl_index_]->ComputeAll( states_.segment(0, num_positions_), states_.segment(num_positions_, num_velocities_)); } -void Model::ForwardKinematics() { +template +void Model::ForwardKinematics() { assert(is_finalized_); impls_[default_impl_index_]->ForwardKinematics( states_.segment(0, num_positions_), @@ -242,3 +276,6 @@ void Model::ForwardKinematics() { } // namespace multibody } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::Model) diff --git a/multibody/test/test_model.cc b/multibody/test/test_model.cc index 598349c9..31d51220 100644 --- a/multibody/test/test_model.cc +++ b/multibody/test/test_model.cc @@ -5,7 +5,7 @@ using namespace huron; //NOLINT -class TestRobot : public Robot { +class TestRobot : public Robot { public: TestRobot() : Robot() {} ~TestRobot() override = default; diff --git a/system/control_interfaces/include/huron/control_interfaces/legged_robot.h b/system/control_interfaces/include/huron/control_interfaces/legged_robot.h index b14f61e3..8f014e5f 100644 --- a/system/control_interfaces/include/huron/control_interfaces/legged_robot.h +++ b/system/control_interfaces/include/huron/control_interfaces/legged_robot.h @@ -9,7 +9,8 @@ namespace huron { -class LeggedRobot : public Robot { +template +class LeggedRobot : public Robot { public: explicit LeggedRobot(std::unique_ptr config); LeggedRobot(); @@ -17,14 +18,17 @@ class LeggedRobot : public Robot { LeggedRobot& operator=(const LeggedRobot&) = delete; ~LeggedRobot() override = default; - void InitializeZmp(std::shared_ptr zmp); + void InitializeZmp(std::shared_ptr> zmp); /** * Computes the Center of Mass in Base frame. */ Eigen::Vector2d EvalZeroMomentPoint(); private: - std::shared_ptr zmp_; + std::shared_ptr> zmp_; }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::LeggedRobot) diff --git a/system/control_interfaces/include/huron/control_interfaces/robot.h b/system/control_interfaces/include/huron/control_interfaces/robot.h index 19d8273b..4db2ad1a 100644 --- a/system/control_interfaces/include/huron/control_interfaces/robot.h +++ b/system/control_interfaces/include/huron/control_interfaces/robot.h @@ -6,6 +6,7 @@ #include #include +#include "huron/utils/template_instantiations.h" #include "huron/control_interfaces/configuration.h" #include "huron/control_interfaces/generic_component.h" #include "huron/control_interfaces/moving_group.h" @@ -35,8 +36,9 @@ class RobotConfiguration : public Configuration { : RobotConfiguration({}, {}) {} }; +template class Robot : public MovingGroup, public GenericComponent { - using Model = multibody::Model; + using Model = multibody::Model; public: explicit Robot(std::unique_ptr config); @@ -76,3 +78,6 @@ class Robot : public MovingGroup, public GenericComponent { }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::Robot) diff --git a/system/control_interfaces/include/huron/control_interfaces/sensor_with_frame.h b/system/control_interfaces/include/huron/control_interfaces/sensor_with_frame.h index 93f20504..39e2eebd 100644 --- a/system/control_interfaces/include/huron/control_interfaces/sensor_with_frame.h +++ b/system/control_interfaces/include/huron/control_interfaces/sensor_with_frame.h @@ -4,13 +4,15 @@ #include +#include "huron/utils/template_instantiations.h" #include "huron/control_interfaces/sensor.h" #include "huron/multibody/frame.h" namespace huron { +template class SensorWithFrame : public Sensor { - using Frame = multibody::Frame; + using Frame = multibody::Frame; public: SensorWithFrame(const Eigen::Vector2i& dim, @@ -39,3 +41,6 @@ class SensorWithFrame : public Sensor { }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::SensorWithFrame) diff --git a/system/control_interfaces/src/legged_robot.cc b/system/control_interfaces/src/legged_robot.cc index 11ac4564..58546d3d 100644 --- a/system/control_interfaces/src/legged_robot.cc +++ b/system/control_interfaces/src/legged_robot.cc @@ -2,18 +2,25 @@ namespace huron { -LeggedRobot::LeggedRobot(std::unique_ptr config) - : Robot(std::move(config)) {} +template +LeggedRobot::LeggedRobot(std::unique_ptr config) + : Robot(std::move(config)) {} -LeggedRobot::LeggedRobot() - : Robot() {} +template +LeggedRobot::LeggedRobot() + : Robot() {} -void LeggedRobot::InitializeZmp(std::shared_ptr zmp) { +template +void LeggedRobot::InitializeZmp(std::shared_ptr> zmp) { zmp_ = std::move(zmp); } -Eigen::Vector2d LeggedRobot::EvalZeroMomentPoint() { +template +Eigen::Vector2d LeggedRobot::EvalZeroMomentPoint() { return zmp_->Eval(); } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::LeggedRobot) diff --git a/system/control_interfaces/src/robot.cc b/system/control_interfaces/src/robot.cc index 3f0a81ce..86e8f35d 100644 --- a/system/control_interfaces/src/robot.cc +++ b/system/control_interfaces/src/robot.cc @@ -2,22 +2,27 @@ namespace huron { -Robot::Robot(std::unique_ptr config) +template +Robot::Robot(std::unique_ptr config) : GenericComponent(std::move(config)), MovingGroup(), - model_(std::make_shared()) {} + model_(std::make_shared()) {} -Robot::Robot() - : Robot::Robot(std::make_unique()) {} +template +Robot::Robot() + : Robot::Robot(std::make_unique()) {} -Robot::Robot(std::unique_ptr config, - std::shared_ptr model) +template +Robot::Robot(std::unique_ptr config, + std::shared_ptr model) : GenericComponent(std::move(config)), MovingGroup(), - model_(std::make_shared()) {} + model_(std::make_shared()) {} -Robot::Robot(std::shared_ptr model) - : Robot::Robot(std::make_unique(), std::move(model)) {} +template +Robot::Robot(std::shared_ptr model) + : Robot::Robot(std::make_unique(), std::move(model)) {} -void Robot::RegisterStateProvider( +template +void Robot::RegisterStateProvider( std::shared_ptr state_provider, bool is_joint_state_provider) { if (!is_joint_state_provider) { @@ -27,25 +32,32 @@ void Robot::RegisterStateProvider( } } -void Robot::UpdateAllStates() { +template +void Robot::UpdateAllStates() { for (auto& state_provider : non_joint_state_providers_) { state_provider->RequestStateUpdate(); } model_->UpdateJointStates(); } -void Robot::UpdateJointStates() { +template +void Robot::UpdateJointStates() { model_->UpdateJointStates(); } +template const Eigen::VectorBlock -Robot::GetJointPositions() const { +Robot::GetJointPositions() const { return model_->GetPositions(); } +template const Eigen::VectorBlock -Robot::GetJointVelocities() const { +Robot::GetJointVelocities() const { return model_->GetVelocities(); } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::Robot) diff --git a/system/control_interfaces/src/sensor_with_frame.cc b/system/control_interfaces/src/sensor_with_frame.cc index 37093513..f0d9c179 100644 --- a/system/control_interfaces/src/sensor_with_frame.cc +++ b/system/control_interfaces/src/sensor_with_frame.cc @@ -2,24 +2,31 @@ namespace huron { -SensorWithFrame::SensorWithFrame(const Eigen::Vector2i& dim, - std::weak_ptr frame) +template +SensorWithFrame::SensorWithFrame(const Eigen::Vector2i& dim, + std::weak_ptr frame) : Sensor(dim), frame_(std::move(frame)) {} -SensorWithFrame::SensorWithFrame(const Eigen::Vector2i& dim, - std::weak_ptr frame, +template +SensorWithFrame::SensorWithFrame(const Eigen::Vector2i& dim, + std::weak_ptr frame, std::unique_ptr config) : Sensor(dim, std::move(config)), frame_(std::move(frame)) {} -SensorWithFrame::SensorWithFrame(int rows, int cols, - std::weak_ptr frame) +template +SensorWithFrame::SensorWithFrame(int rows, int cols, + std::weak_ptr frame) : Sensor(rows, cols), frame_(std::move(frame)) {} -SensorWithFrame::SensorWithFrame(int rows, int cols, - std::weak_ptr frame, +template +SensorWithFrame::SensorWithFrame(int rows, int cols, + std::weak_ptr frame, std::unique_ptr config) : Sensor(rows, cols, std::move(config)), frame_(std::move(frame)) {} } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::SensorWithFrame) diff --git a/system/control_interfaces/test/test_legged_zmp.cc b/system/control_interfaces/test/test_legged_zmp.cc index 041642ba..d25b8cb2 100644 --- a/system/control_interfaces/test/test_legged_zmp.cc +++ b/system/control_interfaces/test/test_legged_zmp.cc @@ -7,7 +7,7 @@ using namespace huron; //NOLINT -class TestLeggedRobot : public LeggedRobot { +class TestLeggedRobot : public LeggedRobot { public: TestLeggedRobot() : LeggedRobot() {} ~TestLeggedRobot() override = default; @@ -21,10 +21,10 @@ class TestLeggedRobot : public LeggedRobot { void Terminate() override {} }; -class FakeForceTorqueSensor : public ForceTorqueSensor { +class FakeForceTorqueSensor : public ForceTorqueSensor { public: FakeForceTorqueSensor(bool reverse_wrench_direction, - std::weak_ptr frame, + std::weak_ptr> frame, const Vector6d& fake_wrench) : ForceTorqueSensor(reverse_wrench_direction, std::move(frame)), fake_wrench_(fake_wrench) {} @@ -89,8 +89,8 @@ class TestLeggedZmp : public testing::Test { // Total ZMP ft_sensor_list.push_back(l_ft_sensor); ft_sensor_list.push_back(r_ft_sensor); - std::shared_ptr zmp = - std::make_shared( + std::shared_ptr> zmp = + std::make_shared>( robot.GetModel()->GetFrame("universe"), 0.005, ft_sensor_list); @@ -101,7 +101,7 @@ class TestLeggedZmp : public testing::Test { double tolerance = 0.0005; TestLeggedRobot robot; - std::vector> ft_sensor_list; + std::vector>> ft_sensor_list; std::shared_ptr l_ft_sensor, r_ft_sensor; }; diff --git a/system/locomotion/include/huron/locomotion/zero_moment_point.h b/system/locomotion/include/huron/locomotion/zero_moment_point.h index 4cbca444..0cf20d49 100644 --- a/system/locomotion/include/huron/locomotion/zero_moment_point.h +++ b/system/locomotion/include/huron/locomotion/zero_moment_point.h @@ -10,9 +10,10 @@ namespace huron { +template class ZeroMomentPoint { public: - ZeroMomentPoint(std::weak_ptr zmp_frame, + ZeroMomentPoint(std::weak_ptr> zmp_frame, double normal_force_threshold); ZeroMomentPoint(const ZeroMomentPoint&) = delete; ZeroMomentPoint& operator=(const ZeroMomentPoint&) = delete; @@ -40,8 +41,11 @@ class ZeroMomentPoint { Eigen::Affine3d ZmpToWorld(const Eigen::Vector2d& zmp) const; protected: - std::weak_ptr zmp_frame_; + std::weak_ptr> zmp_frame_; double normal_force_threshold_; }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ZeroMomentPoint) diff --git a/system/locomotion/include/huron/locomotion/zero_moment_point_fsr_array.h b/system/locomotion/include/huron/locomotion/zero_moment_point_fsr_array.h index 3de040f0..84103ca9 100644 --- a/system/locomotion/include/huron/locomotion/zero_moment_point_fsr_array.h +++ b/system/locomotion/include/huron/locomotion/zero_moment_point_fsr_array.h @@ -6,12 +6,13 @@ namespace huron { -class ZeroMomentPointFSRArray : public ZeroMomentPoint { +template +class ZeroMomentPointFSRArray : public ZeroMomentPoint { public: ZeroMomentPointFSRArray( - std::weak_ptr zmp_frame, + std::weak_ptr> zmp_frame, double normal_force_threshold, - std::shared_ptr fsr_array); + std::shared_ptr> fsr_array); ZeroMomentPointFSRArray(const ZeroMomentPointFSRArray&) = delete; ZeroMomentPointFSRArray& operator=(const ZeroMomentPointFSRArray&) = delete; ~ZeroMomentPointFSRArray() override = default; @@ -19,7 +20,10 @@ class ZeroMomentPointFSRArray : public ZeroMomentPoint { Eigen::Vector2d Eval(double& fz) override; private: - std::shared_ptr fsr_array_; + std::shared_ptr> fsr_array_; }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ForceSensingResistorArray) diff --git a/system/locomotion/include/huron/locomotion/zero_moment_point_ft_sensor.h b/system/locomotion/include/huron/locomotion/zero_moment_point_ft_sensor.h index a5f9f2be..807af03e 100644 --- a/system/locomotion/include/huron/locomotion/zero_moment_point_ft_sensor.h +++ b/system/locomotion/include/huron/locomotion/zero_moment_point_ft_sensor.h @@ -7,12 +7,13 @@ namespace huron { -class ZeroMomentPointFTSensor : public ZeroMomentPoint { +template +class ZeroMomentPointFTSensor : public ZeroMomentPoint { public: ZeroMomentPointFTSensor( - std::weak_ptr zmp_frame, + std::weak_ptr> zmp_frame, double normal_force_threshold, - const std::vector>& ft_sensors); + const std::vector>>& ft_sensors); ZeroMomentPointFTSensor(const ZeroMomentPointFTSensor&) = delete; ZeroMomentPointFTSensor& operator=(const ZeroMomentPointFTSensor&) = delete; ~ZeroMomentPointFTSensor() override = default; @@ -20,7 +21,10 @@ class ZeroMomentPointFTSensor : public ZeroMomentPoint { Eigen::Vector2d Eval(double& fz) override; private: - std::vector> ft_sensors_; + std::vector>> ft_sensors_; }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ZeroMomentPointFTSensor) diff --git a/system/locomotion/include/huron/locomotion/zero_moment_point_total.h b/system/locomotion/include/huron/locomotion/zero_moment_point_total.h index 51db74ae..c5bc2edd 100644 --- a/system/locomotion/include/huron/locomotion/zero_moment_point_total.h +++ b/system/locomotion/include/huron/locomotion/zero_moment_point_total.h @@ -7,11 +7,12 @@ namespace huron { -class ZeroMomentPointTotal : public ZeroMomentPoint { +template +class ZeroMomentPointTotal : public ZeroMomentPoint { public: ZeroMomentPointTotal( - std::weak_ptr zmp_frame, - const std::vector>& zmp_vector); + std::weak_ptr> zmp_frame, + const std::vector>>& zmp_vector); ZeroMomentPointTotal(const ZeroMomentPointTotal&) = delete; ZeroMomentPointTotal& operator=(const ZeroMomentPointTotal&) = delete; ~ZeroMomentPointTotal() override = default; @@ -19,7 +20,10 @@ class ZeroMomentPointTotal : public ZeroMomentPoint { Eigen::Vector2d Eval(double& fz) override; private: - std::vector> zmp_vector_; + std::vector>> zmp_vector_; }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ZeroMomentPointTotal) diff --git a/system/locomotion/src/zero_moment_point.cc b/system/locomotion/src/zero_moment_point.cc index 1060df6e..cc2ab6a8 100644 --- a/system/locomotion/src/zero_moment_point.cc +++ b/system/locomotion/src/zero_moment_point.cc @@ -1,19 +1,25 @@ #include "huron/locomotion/zero_moment_point.h" #include "huron/sensors/force_sensing_resistor_array.h" #include "huron/types.h" + namespace huron { -ZeroMomentPoint::ZeroMomentPoint( - std::weak_ptr zmp_frame, +template +ZeroMomentPoint::ZeroMomentPoint( + std::weak_ptr> zmp_frame, double normal_force_threshold) : zmp_frame_(std::move(zmp_frame)), normal_force_threshold_(normal_force_threshold) { } -Eigen::Affine3d ZeroMomentPoint::ZmpToWorld(const Eigen::Vector2d& zmp) const { +template +Eigen::Affine3d ZeroMomentPoint::ZmpToWorld(const Eigen::Vector2d& zmp) const { Eigen::Affine3d ret; ret.translate(Eigen::Vector3d(zmp.x(), zmp.y(), 0.0)); return ret; } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ZeroMomentPoint) diff --git a/system/locomotion/src/zero_moment_point_fsr_array.cc b/system/locomotion/src/zero_moment_point_fsr_array.cc index 81dc3ebf..3db4d8a0 100644 --- a/system/locomotion/src/zero_moment_point_fsr_array.cc +++ b/system/locomotion/src/zero_moment_point_fsr_array.cc @@ -2,20 +2,22 @@ namespace huron { -ZeroMomentPointFSRArray::ZeroMomentPointFSRArray( - std::weak_ptr zmp_frame, +template +ZeroMomentPointFSRArray::ZeroMomentPointFSRArray( + std::weak_ptr> zmp_frame, double normal_force_threshold, - std::shared_ptr fsr_array) - : ZeroMomentPoint(std::move(zmp_frame), normal_force_threshold), + std::shared_ptr> fsr_array) + : ZeroMomentPoint(std::move(zmp_frame), normal_force_threshold), fsr_array_(std::move(fsr_array)) { } -Eigen::Vector2d ZeroMomentPointFSRArray::Eval(double& fz) { +template +Eigen::Vector2d ZeroMomentPointFSRArray::Eval(double& fz) { Eigen::Vector2d zmp; Eigen::VectorXd fz_array = fsr_array_->GetValue(); double sum_fz = fz_array.colwise().sum().value(); fz = sum_fz; - if (std::abs(sum_fz) < normal_force_threshold_) { + if (std::abs(sum_fz) < this->normal_force_threshold_) { zmp.setZero(); } else { double num_x = 0.0, num_y = 0.0; @@ -30,3 +32,6 @@ Eigen::Vector2d ZeroMomentPointFSRArray::Eval(double& fz) { } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ZeroMomentPointFSRArray) diff --git a/system/locomotion/src/zero_moment_point_ft_sensor.cc b/system/locomotion/src/zero_moment_point_ft_sensor.cc index 5ddcb49b..c12fa705 100644 --- a/system/locomotion/src/zero_moment_point_ft_sensor.cc +++ b/system/locomotion/src/zero_moment_point_ft_sensor.cc @@ -2,22 +2,24 @@ namespace huron { -ZeroMomentPointFTSensor::ZeroMomentPointFTSensor( - std::weak_ptr zmp_frame, +template +ZeroMomentPointFTSensor::ZeroMomentPointFTSensor( + std::weak_ptr> zmp_frame, double normal_force_threshold, - const std::vector>& ft_sensors) - : ZeroMomentPoint(std::move(zmp_frame), normal_force_threshold), + const std::vector>>& ft_sensors) + : ZeroMomentPoint(std::move(zmp_frame), normal_force_threshold), ft_sensors_(ft_sensors) { } // Generate Doxygen documentation for the following function -Eigen::Vector2d ZeroMomentPointFTSensor::Eval(double& fz) { +template +Eigen::Vector2d ZeroMomentPointFTSensor::Eval(double& fz) { Eigen::Vector2d zmp; double num_x = 0.0, num_y = 0.0, den = 0.0; for (auto& ft_sensor : ft_sensors_) { - Eigen::Affine3d zmp_to_sensor = zmp_frame_.lock()->GetTransformToFrame( + Eigen::Affine3d zmp_to_sensor = this->zmp_frame_.lock()->GetTransformToFrame( *ft_sensor->GetSensorFrame().lock()); - Eigen::Affine3d zmp_frame_pose = zmp_frame_.lock()->GetTransformInWorld(); + Eigen::Affine3d zmp_frame_pose = this->zmp_frame_.lock()->GetTransformInWorld(); Vector6d w = ft_sensor->GetValue(); w.segment(0, 3) = zmp_to_sensor.rotation() * w.segment(0, 3); w.segment(3, 3) = zmp_to_sensor.rotation() * w.segment(3, 3); @@ -31,7 +33,7 @@ Eigen::Vector2d ZeroMomentPointFTSensor::Eval(double& fz) { + zmp_to_sensor.translation().y()*w(2)); den += w(2); } - if (std::abs(den) < normal_force_threshold_) { + if (std::abs(den) < this->normal_force_threshold_) { zmp.setZero(); } else { zmp.x() = num_x / den; @@ -42,3 +44,6 @@ Eigen::Vector2d ZeroMomentPointFTSensor::Eval(double& fz) { } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ZeroMomentPointFTSensor) diff --git a/system/locomotion/src/zero_moment_point_total.cc b/system/locomotion/src/zero_moment_point_total.cc index 755b62ac..808329af 100644 --- a/system/locomotion/src/zero_moment_point_total.cc +++ b/system/locomotion/src/zero_moment_point_total.cc @@ -2,13 +2,15 @@ namespace huron { -ZeroMomentPointTotal::ZeroMomentPointTotal( - std::weak_ptr zmp_frame, - const std::vector>& zmp_vector) - : ZeroMomentPoint(std::move(zmp_frame), 0.0), +template +ZeroMomentPointTotal::ZeroMomentPointTotal( + std::weak_ptr> zmp_frame, + const std::vector>>& zmp_vector) + : ZeroMomentPoint(std::move(zmp_frame), 0.0), zmp_vector_(zmp_vector) {} -Eigen::Vector2d ZeroMomentPointTotal::Eval(double& fz) { +template +Eigen::Vector2d ZeroMomentPointTotal::Eval(double& fz) { Eigen::Vector2d zmp; double num_x = 0.0, num_y = 0.0, den = 0.0; for (auto& zmp_obj : zmp_vector_) { @@ -29,3 +31,6 @@ Eigen::Vector2d ZeroMomentPointTotal::Eval(double& fz) { } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ZeroMomentPointTotal) diff --git a/system/locomotion/test/test_zero_moment_point.cc b/system/locomotion/test/test_zero_moment_point.cc index d91b3596..bef7074e 100644 --- a/system/locomotion/test/test_zero_moment_point.cc +++ b/system/locomotion/test/test_zero_moment_point.cc @@ -7,7 +7,7 @@ using namespace huron; //NOLINT -class TestRobot : public Robot { +class TestRobot : public Robot { public: TestRobot() : Robot() {} ~TestRobot() override = default; @@ -21,10 +21,10 @@ class TestRobot : public Robot { void Terminate() override {} }; -class FakeForceTorqueSensor : public ForceTorqueSensor { +class FakeForceTorqueSensor : public ForceTorqueSensor { public: FakeForceTorqueSensor(bool reverse_wrench_direction, - std::weak_ptr frame, + std::weak_ptr> frame, const Vector6d& fake_wrench) : ForceTorqueSensor(reverse_wrench_direction, std::move(frame)), fake_wrench_(fake_wrench) {} @@ -89,10 +89,10 @@ class TestZeroMomentPointFt : public testing::Test { // ZMP frame is the world frame zmp_frame = robot.GetModel()->GetFrame("universe"); // Total ZMP - std::vector> ft_sensor_list; + std::vector>> ft_sensor_list; ft_sensor_list.push_back(l_ft_sensor); ft_sensor_list.push_back(r_ft_sensor); - total_zmp = std::make_shared( + total_zmp = std::make_shared>( zmp_frame, normal_force_threshold, ft_sensor_list); @@ -103,9 +103,9 @@ class TestZeroMomentPointFt : public testing::Test { double tolerance = 0.0005; TestRobot robot; - std::weak_ptr zmp_frame; + std::weak_ptr> zmp_frame; std::shared_ptr l_ft_sensor, r_ft_sensor; - std::shared_ptr total_zmp; + std::shared_ptr> total_zmp; }; TEST_F(TestZeroMomentPointFt, TestGeneral) { diff --git a/system/sensors/include/huron/sensors/force_sensing_resistor.h b/system/sensors/include/huron/sensors/force_sensing_resistor.h index de966ef3..ec3f4799 100644 --- a/system/sensors/include/huron/sensors/force_sensing_resistor.h +++ b/system/sensors/include/huron/sensors/force_sensing_resistor.h @@ -6,10 +6,11 @@ namespace huron { -class ForceSensingResistor : public SensorWithFrame { +template +class ForceSensingResistor : public SensorWithFrame { public: - explicit ForceSensingResistor(std::weak_ptr frame); - ForceSensingResistor(std::weak_ptr frame, + explicit ForceSensingResistor(std::weak_ptr> frame); + ForceSensingResistor(std::weak_ptr> frame, std::unique_ptr config); ForceSensingResistor(const ForceSensingResistor&) = delete; ForceSensingResistor& operator=(const ForceSensingResistor&) = delete; @@ -17,3 +18,6 @@ class ForceSensingResistor : public SensorWithFrame { }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ForceSensingResistor) diff --git a/system/sensors/include/huron/sensors/force_sensing_resistor_array.h b/system/sensors/include/huron/sensors/force_sensing_resistor_array.h index 40d1d71a..fa590a88 100644 --- a/system/sensors/include/huron/sensors/force_sensing_resistor_array.h +++ b/system/sensors/include/huron/sensors/force_sensing_resistor_array.h @@ -11,16 +11,17 @@ namespace huron { -class ForceSensingResistorArray : public SensorWithFrame { +template +class ForceSensingResistorArray : public SensorWithFrame { public: ForceSensingResistorArray( const std::string& name, - std::weak_ptr frame, - const std::vector>& fsr_array); + std::weak_ptr> frame, + const std::vector>>& fsr_array); ForceSensingResistorArray( const std::string& name, - std::weak_ptr frame, - const std::vector>& fsr_array, + std::weak_ptr> frame, + const std::vector>>& fsr_array, std::unique_ptr config); ForceSensingResistorArray(const ForceSensingResistorArray&) = delete; @@ -39,7 +40,10 @@ class ForceSensingResistorArray : public SensorWithFrame { protected: std::string name_; Eigen::VectorXd values_; - std::vector> fsr_array_; + std::vector>> fsr_array_; }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ForceSensingResistorArray) diff --git a/system/sensors/include/huron/sensors/force_sensing_resistor_array_serial.h b/system/sensors/include/huron/sensors/force_sensing_resistor_array_serial.h index c5175a0d..4db8217d 100644 --- a/system/sensors/include/huron/sensors/force_sensing_resistor_array_serial.h +++ b/system/sensors/include/huron/sensors/force_sensing_resistor_array_serial.h @@ -16,17 +16,17 @@ namespace huron { * ,,,...,\n * The sensor values should be sent periodically. */ -class ForceSensingResistorArraySerial : public ForceSensingResistorArray { +class ForceSensingResistorArraySerial : public ForceSensingResistorArray { public: ForceSensingResistorArraySerial( const std::string& name, - std::weak_ptr frame, - const std::vector>& fsr_array, + std::weak_ptr> frame, + const std::vector>>& fsr_array, std::shared_ptr serial); ForceSensingResistorArraySerial( const std::string& name, - std::weak_ptr frame, - const std::vector>& fsr_array, + std::weak_ptr> frame, + const std::vector>>& fsr_array, std::shared_ptr serial, std::unique_ptr config); diff --git a/system/sensors/include/huron/sensors/force_torque_sensor.h b/system/sensors/include/huron/sensors/force_torque_sensor.h index 79ef37e2..b0b9f8a0 100644 --- a/system/sensors/include/huron/sensors/force_torque_sensor.h +++ b/system/sensors/include/huron/sensors/force_torque_sensor.h @@ -9,12 +9,13 @@ namespace huron { -class ForceTorqueSensor : public SensorWithFrame { +template +class ForceTorqueSensor : public SensorWithFrame { public: ForceTorqueSensor(bool reverse_wrench_direction, - std::weak_ptr frame); + std::weak_ptr> frame); ForceTorqueSensor(bool reverse_wrench_direction, - std::weak_ptr frame, + std::weak_ptr> frame, std::unique_ptr config); ForceTorqueSensor(const ForceTorqueSensor&) = delete; ForceTorqueSensor& operator=(const ForceTorqueSensor&) = delete; @@ -43,3 +44,6 @@ class ForceTorqueSensor : public SensorWithFrame { }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ForceTorqueSensor) diff --git a/system/sensors/src/force_sensing_resistor.cc b/system/sensors/src/force_sensing_resistor.cc index cd2c097f..2af0f6ab 100644 --- a/system/sensors/src/force_sensing_resistor.cc +++ b/system/sensors/src/force_sensing_resistor.cc @@ -2,13 +2,18 @@ namespace huron { -ForceSensingResistor::ForceSensingResistor( - std::weak_ptr frame) - : SensorWithFrame(1, 1, std::move(frame)) {} +template +ForceSensingResistor::ForceSensingResistor( + std::weak_ptr> frame) + : SensorWithFrame(1, 1, std::move(frame)) {} -ForceSensingResistor::ForceSensingResistor( - std::weak_ptr frame, +template +ForceSensingResistor::ForceSensingResistor( + std::weak_ptr> frame, std::unique_ptr config) - : SensorWithFrame(1, 1, std::move(frame), std::move(config)) {} + : SensorWithFrame(1, 1, std::move(frame), std::move(config)) {} } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ForceSensingResistor) diff --git a/system/sensors/src/force_sensing_resistor_array.cc b/system/sensors/src/force_sensing_resistor_array.cc index afdd06c0..81763c9e 100644 --- a/system/sensors/src/force_sensing_resistor_array.cc +++ b/system/sensors/src/force_sensing_resistor_array.cc @@ -2,38 +2,46 @@ namespace huron { -ForceSensingResistorArray::ForceSensingResistorArray( +template +ForceSensingResistorArray::ForceSensingResistorArray( const std::string& name, - std::weak_ptr frame, - const std::vector>& fsr_array) - : SensorWithFrame(fsr_array.size(), 1, std::move(frame)), + std::weak_ptr> frame, + const std::vector>>& fsr_array) + : SensorWithFrame(fsr_array.size(), 1, std::move(frame)), name_(name), values_(Eigen::VectorXd::Zero(fsr_array.size())), fsr_array_(fsr_array) {} -ForceSensingResistorArray::ForceSensingResistorArray( +template +ForceSensingResistorArray::ForceSensingResistorArray( const std::string& name, - std::weak_ptr frame, - const std::vector>& fsr_array, + std::weak_ptr> frame, + const std::vector>>& fsr_array, std::unique_ptr config) - : SensorWithFrame(fsr_array.size(), 1, std::move(frame), std::move(config)), + : SensorWithFrame(fsr_array.size(), 1, std::move(frame), std::move(config)), name_(name), values_(Eigen::VectorXd::Zero(fsr_array.size())), fsr_array_(fsr_array) {} -void ForceSensingResistorArray::RequestStateUpdate() { +template +void ForceSensingResistorArray::RequestStateUpdate() { for (size_t i = 0; i < fsr_array_.size(); ++i) { values_(i) = fsr_array_[i]->ReloadAndGetValue()(0); } } -void ForceSensingResistorArray::GetNewState( +template +void ForceSensingResistorArray::GetNewState( Eigen::Ref new_state) const { new_state = values_; } -Eigen::Affine3d ForceSensingResistorArray::GetSensorPose(size_t index) const { +template +Eigen::Affine3d ForceSensingResistorArray::GetSensorPose(size_t index) const { return fsr_array_[index]->GetSensorFrame().lock()->GetTransformInWorld(); } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ForceSensingResistorArray) diff --git a/system/sensors/src/force_sensing_resistor_array_serial.cc b/system/sensors/src/force_sensing_resistor_array_serial.cc index 803c3127..33e3489e 100644 --- a/system/sensors/src/force_sensing_resistor_array_serial.cc +++ b/system/sensors/src/force_sensing_resistor_array_serial.cc @@ -5,16 +5,16 @@ namespace huron { ForceSensingResistorArraySerial::ForceSensingResistorArraySerial( const std::string& name, - std::weak_ptr frame, - const std::vector>& fsr_array, + std::weak_ptr> frame, + const std::vector>>& fsr_array, std::shared_ptr serial) : ForceSensingResistorArray(name, std::move(frame), fsr_array), serial_(std::move(serial)) {} ForceSensingResistorArraySerial::ForceSensingResistorArraySerial( const std::string& name, - std::weak_ptr frame, - const std::vector>& fsr_array, + std::weak_ptr> frame, + const std::vector>>& fsr_array, std::shared_ptr serial, std::unique_ptr config) : ForceSensingResistorArray(name, std::move(frame), diff --git a/system/sensors/src/force_torque_sensor.cc b/system/sensors/src/force_torque_sensor.cc index fc3b9015..a463f190 100644 --- a/system/sensors/src/force_torque_sensor.cc +++ b/system/sensors/src/force_torque_sensor.cc @@ -2,32 +2,40 @@ namespace huron { -ForceTorqueSensor::ForceTorqueSensor( +template +ForceTorqueSensor::ForceTorqueSensor( bool reverse_wrench_direction, - std::weak_ptr frame) - : SensorWithFrame(6, 1, std::move(frame)), + std::weak_ptr> frame) + : SensorWithFrame(6, 1, std::move(frame)), reverse_wrench_direction_(reverse_wrench_direction) {} -ForceTorqueSensor::ForceTorqueSensor( +template +ForceTorqueSensor::ForceTorqueSensor( bool reverse_wrench_direction, - std::weak_ptr frame, + std::weak_ptr> frame, std::unique_ptr config) - : SensorWithFrame(6, 1, std::move(frame), std::move(config)), + : SensorWithFrame(6, 1, std::move(frame), std::move(config)), reverse_wrench_direction_(reverse_wrench_direction) {} -void ForceTorqueSensor::RequestStateUpdate() { +template +void ForceTorqueSensor::RequestStateUpdate() { wrench_ = DoGetWrenchRaw(); } -void ForceTorqueSensor::GetNewState( +template +void ForceTorqueSensor::GetNewState( Eigen::Ref new_state) const { new_state = GetValue(); } -Eigen::VectorXd ForceTorqueSensor::GetValue() const { +template +Eigen::VectorXd ForceTorqueSensor::GetValue() const { if (reverse_wrench_direction_) return -wrench_; return wrench_; } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ForceTorqueSensor) From 53b6d7f5fbd11048f12e10dd888771652efdb0be Mon Sep 17 00:00:00 2001 From: Duc Doan Date: Wed, 31 Jan 2024 16:59:04 -0500 Subject: [PATCH 4/9] Templatize ModelImplInterface --- common/include/huron/types.h | 28 +++- multibody/include/huron/multibody/model.h | 2 +- .../huron/multibody/model_impl_factory.h | 4 +- .../huron/multibody/model_impl_interface.h | 50 ++++--- .../huron/multibody/pinocchio_model_impl.h | 55 +++---- multibody/src/model.cc | 2 +- multibody/src/model_impl_default.cc | 130 ++++++++++------ multibody/src/pinocchio_model_impl.cc | 141 +++++++++++------- multibody/test/test_pinocchio_impl.cc | 4 +- 9 files changed, 261 insertions(+), 155 deletions(-) diff --git a/common/include/huron/types.h b/common/include/huron/types.h index db6d3f0c..f0abfcc2 100644 --- a/common/include/huron/types.h +++ b/common/include/huron/types.h @@ -4,8 +4,30 @@ namespace huron { -typedef Eigen::Matrix< double, 6, 1 > Vector6d; -typedef Eigen::Matrix< double, 6, 6 > Matrix6d; -typedef Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd; +template +using Affine3 = Eigen::Transform; + +template +using Vector3 = Eigen::Matrix; + +template +using VectorX = Eigen::Matrix; + +template +using MatrixX = Eigen::Matrix; + +template +using Vector6 = Eigen::Matrix; + +template +using Matrix6X = Eigen::Matrix; + +/// double aliases +using Affine3d = Affine3; +using Vector3d = Vector3; +using VectorXd = VectorX; +using MatrixXd = MatrixX; +using Vector6d = Vector6; +using Matrix6Xd = Matrix6X; } // namespace huron diff --git a/multibody/include/huron/multibody/model.h b/multibody/include/huron/multibody/model.h index 9b359d4d..00e7f4e7 100644 --- a/multibody/include/huron/multibody/model.h +++ b/multibody/include/huron/multibody/model.h @@ -16,7 +16,7 @@ namespace multibody { template class Model : public std::enable_shared_from_this> { - using ModelImplInterface = internal::ModelImplInterface; + using ModelImplInterface = internal::ModelImplInterface; public: Model(); diff --git a/multibody/include/huron/multibody/model_impl_factory.h b/multibody/include/huron/multibody/model_impl_factory.h index 8cf99017..84a25951 100644 --- a/multibody/include/huron/multibody/model_impl_factory.h +++ b/multibody/include/huron/multibody/model_impl_factory.h @@ -20,11 +20,11 @@ class ModelImplFactory final { ModelImplFactory& operator=(const ModelImplFactory&) = delete; ~ModelImplFactory() = default; private: - static std::unique_ptr + static std::unique_ptr> Create(ModelImplType type) { switch (type) { case ModelImplType::kPinocchio: - return std::make_unique(); + return std::make_unique>(); default: throw std::runtime_error("ModelImplType not implemented."); } diff --git a/multibody/include/huron/multibody/model_impl_interface.h b/multibody/include/huron/multibody/model_impl_interface.h index ef1c11cb..c5f21e14 100644 --- a/multibody/include/huron/multibody/model_impl_interface.h +++ b/multibody/include/huron/multibody/model_impl_interface.h @@ -12,6 +12,7 @@ namespace huron { namespace multibody { namespace internal { +template class ModelImplInterface { public: ModelImplInterface() = default; @@ -34,80 +35,80 @@ class ModelImplInterface { virtual std::unique_ptr GetJointDescription( const std::string& joint_name) const; - virtual Eigen::Affine3d + virtual huron::Affine3 GetJointTransformInWorld(size_t joint_index) const; virtual FrameIndex GetFrameIndex( const std::string& frame_name) const; virtual const std::string& GetFrameName(FrameIndex frame_index) const; virtual FrameType GetFrameType(FrameIndex frame_index) const; - virtual Eigen::Affine3d GetFrameTransform(FrameIndex from_frame, + virtual huron::Affine3 GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const; - virtual Eigen::Affine3d GetFrameTransformInWorld(FrameIndex frame) const; + virtual huron::Affine3 GetFrameTransformInWorld(FrameIndex frame) const; - virtual Eigen::Vector3d EvalCenterOfMassPosition(); - virtual Eigen::Vector3d GetCenterOfMassPosition() const; + virtual huron::Vector3 EvalCenterOfMassPosition(); + virtual huron::Vector3 GetCenterOfMassPosition() const; - virtual Eigen::VectorXd NeutralConfiguration() const; + virtual huron::VectorX NeutralConfiguration() const; /** * @brief Get the generalized accelerations of the model. */ - virtual const Eigen::VectorXd& GetAccelerations() const; + virtual const huron::VectorX& GetAccelerations() const; /** * @brief Get the joint torques. */ - virtual const Eigen::VectorXd& GetTorques() const; + virtual const huron::VectorX& GetTorques() const; /** * @brief Get the mass matrix with the cached value. */ - virtual const Eigen::MatrixXd& GetMassMatrix() const; + virtual const huron::MatrixX& GetMassMatrix() const; /** * @brief Get the Coriolis matrix with the cached value. */ - virtual const Eigen::MatrixXd& GetCoriolisMatrix() const; + virtual const huron::MatrixX& GetCoriolisMatrix() const; /** * @brief Get the nonlinear effects vector. */ - virtual const Eigen::VectorXd& GetNonlinearEffects() const; + virtual const huron::VectorX& GetNonlinearEffects() const; /** * @brief Get the gravity vector. */ - virtual const Eigen::VectorXd& GetGravity() const; + virtual const huron::VectorX& GetGravity() const; /** * @brief Get the spatial momentum with respect to the specified frame. */ - virtual const huron::Vector6d& GetSpatialMomentum() const; + virtual const huron::Vector6& GetSpatialMomentum() const; /** * @brief Get the centroidal momentum. */ - virtual huron::Vector6d GetCentroidalMomentum() const; + virtual huron::Vector6 GetCentroidalMomentum() const; /** * @brief Get the centroidal momentum matrix with the cached value. */ - virtual const huron::Matrix6Xd& GetCentroidalMatrix() const; + virtual const huron::Matrix6X& GetCentroidalMatrix() const; virtual void ComputeAll( - const Eigen::Ref& q, - const Eigen::Ref& v); + const Eigen::Ref>& q, + const Eigen::Ref>& v); virtual void ForwardKinematics( - const Eigen::Ref& q); + const Eigen::Ref>& q); virtual void ForwardKinematics( - const Eigen::Ref& q, - const Eigen::Ref& v); + const Eigen::Ref>& q, + const Eigen::Ref>& v); virtual void ForwardKinematics( - const Eigen::Ref& q, - const Eigen::Ref& v, - const Eigen::Ref& a); + const Eigen::Ref>& q, + const Eigen::Ref>& v, + const Eigen::Ref>& a); virtual bool is_built() const; @@ -120,3 +121,6 @@ class ModelImplInterface { } // namespace internal } // namespace multibody } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::internal::ModelImplInterface) diff --git a/multibody/include/huron/multibody/pinocchio_model_impl.h b/multibody/include/huron/multibody/pinocchio_model_impl.h index 42b579bb..752b1cea 100644 --- a/multibody/include/huron/multibody/pinocchio_model_impl.h +++ b/multibody/include/huron/multibody/pinocchio_model_impl.h @@ -5,6 +5,7 @@ #include #include +#include "huron/types.h" #include "huron/multibody/model_impl_interface.h" #include "huron/multibody/joint_common.h" #include "huron/exceptions/not_implemented_exception.h" @@ -13,7 +14,8 @@ namespace huron { namespace multibody { namespace internal { -class PinocchioModelImpl : public ModelImplInterface { +template +class PinocchioModelImpl : public ModelImplInterface { public: PinocchioModelImpl(); PinocchioModelImpl(const PinocchioModelImpl&) = delete; @@ -37,45 +39,45 @@ class PinocchioModelImpl : public ModelImplInterface { std::unique_ptr GetJointDescription( const std::string& joint_name) const override; - Eigen::Affine3d + huron::Affine3 GetJointTransformInWorld(size_t joint_index) const override; FrameIndex GetFrameIndex(const std::string& frame_name) const override; const std::string& GetFrameName(FrameIndex frame_index) const override; FrameType GetFrameType(FrameIndex frame_index) const override; - Eigen::Affine3d GetFrameTransform(FrameIndex from_frame, - FrameIndex to_frame) const override; - Eigen::Affine3d + huron::Affine3 GetFrameTransform(FrameIndex from_frame, + FrameIndex to_frame) const override; + huron::Affine3 GetFrameTransformInWorld(FrameIndex frame) const override; - Eigen::Vector3d EvalCenterOfMassPosition() override; - Eigen::Vector3d GetCenterOfMassPosition() const override; + huron::Vector3 EvalCenterOfMassPosition() override; + huron::Vector3 GetCenterOfMassPosition() const override; - Eigen::VectorXd NeutralConfiguration() const override; + huron::VectorX NeutralConfiguration() const override; - const Eigen::VectorXd& GetAccelerations() const override; - const Eigen::VectorXd& GetTorques() const override; - const Eigen::MatrixXd& GetMassMatrix() const override; - const Eigen::MatrixXd& GetCoriolisMatrix() const override; - const Eigen::VectorXd& GetNonlinearEffects() const override; - const Eigen::VectorXd& GetGravity() const override; - const huron::Vector6d& GetSpatialMomentum() const override; - huron::Vector6d GetCentroidalMomentum() const override; - const huron::Matrix6Xd& GetCentroidalMatrix() const override; + const huron::VectorX& GetAccelerations() const override; + const huron::VectorX& GetTorques() const override; + const huron::MatrixX& GetMassMatrix() const override; + const huron::MatrixX& GetCoriolisMatrix() const override; + const huron::VectorX& GetNonlinearEffects() const override; + const huron::VectorX& GetGravity() const override; + const huron::Vector6& GetSpatialMomentum() const override; + huron::Vector6 GetCentroidalMomentum() const override; + const huron::Matrix6X& GetCentroidalMatrix() const override; void ComputeAll( - const Eigen::Ref& q, - const Eigen::Ref& v) override; + const Eigen::Ref>& q, + const Eigen::Ref>& v) override; void ForwardKinematics( - const Eigen::Ref& q) override; + const Eigen::Ref>& q) override; void ForwardKinematics( - const Eigen::Ref& q, - const Eigen::Ref& v) override; + const Eigen::Ref>& q, + const Eigen::Ref>& v) override; void ForwardKinematics( - const Eigen::Ref& q, - const Eigen::Ref& v, - const Eigen::Ref& a) override; + const Eigen::Ref>& q, + const Eigen::Ref>& v, + const Eigen::Ref>& a) override; bool is_built() const override { return is_built_; } size_t num_positions() const override { return num_positions_; } @@ -97,3 +99,6 @@ class PinocchioModelImpl : public ModelImplInterface { } // namespace internal } // namespace multibody } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::internal::PinocchioModelImpl) diff --git a/multibody/src/model.cc b/multibody/src/model.cc index 939527fe..c42baa80 100644 --- a/multibody/src/model.cc +++ b/multibody/src/model.cc @@ -17,7 +17,7 @@ void Model::AddModelImpl(ModelImplType type, } template -internal::ModelImplInterface const * Model::GetModelImpl(size_t index) const { +internal::ModelImplInterface const * Model::GetModelImpl(size_t index) const { return impls_[index].get(); } diff --git a/multibody/src/model_impl_default.cc b/multibody/src/model_impl_default.cc index 54dce630..69d47df6 100644 --- a/multibody/src/model_impl_default.cc +++ b/multibody/src/model_impl_default.cc @@ -4,160 +4,198 @@ namespace huron { namespace multibody { namespace internal { -void ModelImplInterface::BuildFromUrdf(const std::string& urdf_path, +template +void ModelImplInterface::BuildFromUrdf(const std::string& urdf_path, JointType root_joint_type) { throw NotImplementedException(); } -const std::vector& ModelImplInterface::GetJointNames() const { +template +const std::vector& ModelImplInterface::GetJointNames() const { throw NotImplementedException(); } +template std::weak_ptr -ModelImplInterface::GetJoint(const std::string& name) const { +ModelImplInterface::GetJoint(const std::string& name) const { throw NotImplementedException(); } +template std::weak_ptr -ModelImplInterface::GetJoint(size_t joint_index) const { +ModelImplInterface::GetJoint(size_t joint_index) const { throw NotImplementedException(); } -JointType ModelImplInterface::GetJointType(size_t joint_index) const { +template +JointType ModelImplInterface::GetJointType(size_t joint_index) const { throw NotImplementedException(); } +template JointIndex -ModelImplInterface::GetJointIndex(const std::string& joint_name) const { +ModelImplInterface::GetJointIndex(const std::string& joint_name) const { throw NotImplementedException(); } -std::unique_ptr ModelImplInterface::GetJointDescription( +template +std::unique_ptr ModelImplInterface::GetJointDescription( JointIndex joint_index) const { throw NotImplementedException(); } -std::unique_ptr ModelImplInterface::GetJointDescription( +template +std::unique_ptr ModelImplInterface::GetJointDescription( const std::string& joint_name) const { throw NotImplementedException(); } -Eigen::Affine3d -ModelImplInterface::GetJointTransformInWorld(size_t joint_index) const { +template +huron::Affine3 +ModelImplInterface::GetJointTransformInWorld(size_t joint_index) const { throw NotImplementedException(); } -FrameIndex ModelImplInterface::GetFrameIndex( +template +FrameIndex ModelImplInterface::GetFrameIndex( const std::string& frame_name) const { throw NotImplementedException(); } +template const std::string& -ModelImplInterface::GetFrameName(FrameIndex frame_index) const { +ModelImplInterface::GetFrameName(FrameIndex frame_index) const { throw NotImplementedException(); } -FrameType ModelImplInterface::GetFrameType(FrameIndex frame_index) const { +template +FrameType ModelImplInterface::GetFrameType(FrameIndex frame_index) const { throw NotImplementedException(); } -Eigen::Affine3d -ModelImplInterface::GetFrameTransform(FrameIndex from_frame, +template +huron::Affine3 +ModelImplInterface::GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const { throw NotImplementedException(); } -Eigen::Affine3d -ModelImplInterface::GetFrameTransformInWorld(FrameIndex frame) const { +template +huron::Affine3 +ModelImplInterface::GetFrameTransformInWorld(FrameIndex frame) const { throw NotImplementedException(); } -Eigen::Vector3d ModelImplInterface::EvalCenterOfMassPosition() { +template +huron::Vector3 ModelImplInterface::EvalCenterOfMassPosition() { throw NotImplementedException(); } -Eigen::Vector3d ModelImplInterface::GetCenterOfMassPosition() const { +template +huron::Vector3 ModelImplInterface::GetCenterOfMassPosition() const { throw NotImplementedException(); } -Eigen::VectorXd ModelImplInterface::NeutralConfiguration() const { +template +huron::VectorX ModelImplInterface::NeutralConfiguration() const { throw NotImplementedException(); } -const Eigen::VectorXd& ModelImplInterface::GetAccelerations() const { +template +const huron::VectorX& ModelImplInterface::GetAccelerations() const { throw NotImplementedException(); } -const Eigen::VectorXd& ModelImplInterface::GetTorques() const { +template +const huron::VectorX& ModelImplInterface::GetTorques() const { throw NotImplementedException(); } -const Eigen::MatrixXd& ModelImplInterface::GetMassMatrix() const { +template +const huron::MatrixX& ModelImplInterface::GetMassMatrix() const { throw NotImplementedException(); } -const Eigen::MatrixXd& ModelImplInterface::GetCoriolisMatrix() const { +template +const huron::MatrixX& ModelImplInterface::GetCoriolisMatrix() const { throw NotImplementedException(); } -const Eigen::VectorXd& ModelImplInterface::GetNonlinearEffects() const { +template +const huron::VectorX& ModelImplInterface::GetNonlinearEffects() const { throw NotImplementedException(); } -const Eigen::VectorXd& ModelImplInterface::GetGravity() const { +template +const huron::VectorX& ModelImplInterface::GetGravity() const { throw NotImplementedException(); } -const huron::Vector6d& ModelImplInterface::GetSpatialMomentum() const { +template +const huron::Vector6& ModelImplInterface::GetSpatialMomentum() const { throw NotImplementedException(); } -huron::Vector6d ModelImplInterface::GetCentroidalMomentum() const { +template +huron::Vector6 ModelImplInterface::GetCentroidalMomentum() const { throw NotImplementedException(); } -const huron::Matrix6Xd& ModelImplInterface::GetCentroidalMatrix() const { +template +const huron::Matrix6X& ModelImplInterface::GetCentroidalMatrix() const { throw NotImplementedException(); } -void ModelImplInterface::ComputeAll( - const Eigen::Ref& q, - const Eigen::Ref& v) { +template +void ModelImplInterface::ComputeAll( + const Eigen::Ref>& q, + const Eigen::Ref>& v) { throw NotImplementedException(); } -void ModelImplInterface::ForwardKinematics( - const Eigen::Ref& q) { +template +void ModelImplInterface::ForwardKinematics( + const Eigen::Ref>& q) { throw NotImplementedException(); } -void ModelImplInterface::ForwardKinematics( - const Eigen::Ref& q, - const Eigen::Ref& v) { +template +void ModelImplInterface::ForwardKinematics( + const Eigen::Ref>& q, + const Eigen::Ref>& v) { throw NotImplementedException(); } -void ModelImplInterface::ForwardKinematics( - const Eigen::Ref& q, - const Eigen::Ref& v, - const Eigen::Ref& a) { +template +void ModelImplInterface::ForwardKinematics( + const Eigen::Ref>& q, + const Eigen::Ref>& v, + const Eigen::Ref>& a) { throw NotImplementedException(); } -bool ModelImplInterface::is_built() const { +template +bool ModelImplInterface::is_built() const { throw NotImplementedException(); } -size_t ModelImplInterface::num_positions() const { +template +size_t ModelImplInterface::num_positions() const { throw NotImplementedException(); } -size_t ModelImplInterface::num_velocities() const { +template +size_t ModelImplInterface::num_velocities() const { throw NotImplementedException(); } -size_t ModelImplInterface::num_joints() const { +template +size_t ModelImplInterface::num_joints() const { throw NotImplementedException(); } -size_t ModelImplInterface::num_frames() const { +template +size_t ModelImplInterface::num_frames() const { throw NotImplementedException(); } } // namespace internal } // namespace multibody } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::internal::ModelImplInterface) diff --git a/multibody/src/pinocchio_model_impl.cc b/multibody/src/pinocchio_model_impl.cc index eb132e22..8a3d474a 100644 --- a/multibody/src/pinocchio_model_impl.cc +++ b/multibody/src/pinocchio_model_impl.cc @@ -18,8 +18,9 @@ namespace internal { namespace helpers { -Eigen::Affine3d Se3ToAffine3d(const pinocchio::SE3& se3) { - Eigen::Affine3d affine; +template +huron::Affine3 Se3ToAffine3(const pinocchio::SE3Tpl& se3) { + huron::Affine3 affine; affine.linear() = se3.rotation(); affine.translation() = se3.translation(); return affine; @@ -27,17 +28,21 @@ Eigen::Affine3d Se3ToAffine3d(const pinocchio::SE3& se3) { } // namespace helpers -struct PinocchioModelImpl::Impl { - mutable pinocchio::Model model_; - mutable pinocchio::Data data_; +template +struct PinocchioModelImpl::Impl { + mutable pinocchio::ModelTpl model_; + mutable pinocchio::DataTpl data_; }; -PinocchioModelImpl::PinocchioModelImpl() +template +PinocchioModelImpl::PinocchioModelImpl() : impl_(std::make_unique()) {} -PinocchioModelImpl::~PinocchioModelImpl() = default; +template +PinocchioModelImpl::~PinocchioModelImpl() = default; -void PinocchioModelImpl::BuildFromUrdf(const std::string& urdf_path, +template +void PinocchioModelImpl::BuildFromUrdf(const std::string& urdf_path, JointType root_joint_type) { pinocchio::Model::JointModel joint_model; if (root_joint_type == JointType::kFixed) { @@ -63,27 +68,32 @@ void PinocchioModelImpl::BuildFromUrdf(const std::string& urdf_path, num_frames_ = impl_->model_.nframes; } -const std::vector& PinocchioModelImpl::GetJointNames() const { +template +const std::vector& PinocchioModelImpl::GetJointNames() const { return impl_->model_.names; } +template std::weak_ptr -PinocchioModelImpl::GetJoint(const std::string& name) const { +PinocchioModelImpl::GetJoint(const std::string& name) const { throw NotImplementedException(); } +template std::weak_ptr -PinocchioModelImpl::GetJoint(size_t joint_index) const { +PinocchioModelImpl::GetJoint(size_t joint_index) const { throw NotImplementedException(); } -std::unique_ptr PinocchioModelImpl::GetJointDescription( +template +std::unique_ptr PinocchioModelImpl::GetJointDescription( JointIndex joint_index) const { return GetJointDescription(impl_->model_.names[joint_index]); } +template std::unique_ptr -PinocchioModelImpl::GetJointDescription( +PinocchioModelImpl::GetJointDescription( const std::string& joint_name) const { // auto frame_id = impl_->model_.getFrameId(joint_name, pinocchio::JOINT); auto frame_id = impl_->model_.getFrameId(joint_name); @@ -109,9 +119,9 @@ PinocchioModelImpl::GetJointDescription( impl_->model_.nvs[joint_index]), impl_->model_.velocityLimit.segment(impl_->model_.idx_vs[joint_index], impl_->model_.nvs[joint_index]), - Eigen::VectorXd::Constant(impl_->model_.nvs[joint_index], + huron::VectorX::Constant(impl_->model_.nvs[joint_index], -std::numeric_limits::infinity()), - Eigen::VectorXd::Constant(impl_->model_.nvs[joint_index], + huron::VectorX::Constant(impl_->model_.nvs[joint_index], std::numeric_limits::infinity()), -impl_->model_.effortLimit.segment(impl_->model_.idx_vs[joint_index], impl_->model_.nvs[joint_index]), @@ -123,27 +133,32 @@ PinocchioModelImpl::GetJointDescription( impl_->model_.nvs[joint_index])); } -Eigen::Affine3d -PinocchioModelImpl::GetJointTransformInWorld(size_t joint_index) const { - return helpers::Se3ToAffine3d(impl_->data_.oMi[joint_index]); +template +huron::Affine3 +PinocchioModelImpl::GetJointTransformInWorld(size_t joint_index) const { + return helpers::Se3ToAffine3(impl_->data_.oMi[joint_index]); } +template JointIndex -PinocchioModelImpl::GetJointIndex(const std::string& joint_name) const { +PinocchioModelImpl::GetJointIndex(const std::string& joint_name) const { return impl_->model_.getJointId(joint_name); } -FrameIndex PinocchioModelImpl::GetFrameIndex( +template +FrameIndex PinocchioModelImpl::GetFrameIndex( const std::string& frame_name) const { return impl_->model_.getFrameId(frame_name); } +template const std::string& -PinocchioModelImpl::GetFrameName(FrameIndex frame_index) const { +PinocchioModelImpl::GetFrameName(FrameIndex frame_index) const { return impl_->model_.frames[frame_index].name; } -FrameType PinocchioModelImpl::GetFrameType(FrameIndex frame_index) const { +template +FrameType PinocchioModelImpl::GetFrameType(FrameIndex frame_index) const { if (impl_->model_.frames[frame_index].type == pinocchio::BODY) { return FrameType::kPhysical; } else if (impl_->model_.frames[frame_index].type == pinocchio::JOINT) { @@ -157,85 +172,104 @@ FrameType PinocchioModelImpl::GetFrameType(FrameIndex frame_index) const { } } -Eigen::Affine3d -PinocchioModelImpl::GetFrameTransform(FrameIndex from_frame, +template +huron::Affine3 +PinocchioModelImpl::GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const { return GetFrameTransformInWorld(from_frame).inverse() * GetFrameTransformInWorld(to_frame); } -Eigen::Affine3d -PinocchioModelImpl::GetFrameTransformInWorld(FrameIndex frame) const { +template +huron::Affine3 +PinocchioModelImpl::GetFrameTransformInWorld(FrameIndex frame) const { pinocchio::updateFramePlacement( impl_->model_, impl_->data_, static_cast(frame)); - return helpers::Se3ToAffine3d(impl_->data_.oMf[frame]); + return helpers::Se3ToAffine3(impl_->data_.oMf[frame]); } -Eigen::Vector3d PinocchioModelImpl::EvalCenterOfMassPosition() { +template +huron::Vector3 PinocchioModelImpl::EvalCenterOfMassPosition() { return pinocchio::centerOfMass(impl_->model_, impl_->data_); } -Eigen::Vector3d PinocchioModelImpl::GetCenterOfMassPosition() const { +template +huron::Vector3 PinocchioModelImpl::GetCenterOfMassPosition() const { return impl_->data_.com[0]; } -Eigen::VectorXd PinocchioModelImpl::NeutralConfiguration() const { +template +huron::VectorX PinocchioModelImpl::NeutralConfiguration() const { return pinocchio::neutral(impl_->model_); } -const Eigen::VectorXd& PinocchioModelImpl::GetAccelerations() const { +template +const huron::VectorX& PinocchioModelImpl::GetAccelerations() const { return impl_->data_.ddq; } -const Eigen::VectorXd& PinocchioModelImpl::GetTorques() const { +template +const huron::VectorX& PinocchioModelImpl::GetTorques() const { return impl_->data_.tau; } -const Eigen::MatrixXd& PinocchioModelImpl::GetMassMatrix() const { +template +const huron::MatrixX& PinocchioModelImpl::GetMassMatrix() const { return impl_->data_.M; } -const Eigen::MatrixXd& PinocchioModelImpl::GetCoriolisMatrix() const { +template +const huron::MatrixX& PinocchioModelImpl::GetCoriolisMatrix() const { return impl_->data_.C; } -const Eigen::VectorXd& PinocchioModelImpl::GetNonlinearEffects() const { +template +const huron::VectorX& PinocchioModelImpl::GetNonlinearEffects() const { return impl_->data_.nle; } -const Eigen::VectorXd& PinocchioModelImpl::GetGravity() const { +template +const huron::VectorX& PinocchioModelImpl::GetGravity() const { return impl_->data_.g; } -const huron::Vector6d& PinocchioModelImpl::GetSpatialMomentum() const { +template +const huron::Vector6& PinocchioModelImpl::GetSpatialMomentum() const { throw NotImplementedException(); } -huron::Vector6d PinocchioModelImpl::GetCentroidalMomentum() const { +template +huron::Vector6 PinocchioModelImpl::GetCentroidalMomentum() const { return impl_->data_.hg; } -const huron::Matrix6Xd& PinocchioModelImpl::GetCentroidalMatrix() const { +template +const huron::Matrix6X& PinocchioModelImpl::GetCentroidalMatrix() const { return impl_->data_.Ag; } -void PinocchioModelImpl::ComputeAll( - const Eigen::Ref& q, - const Eigen::Ref& v) { +template +void PinocchioModelImpl::ComputeAll( + const Eigen::Ref>& q, + const Eigen::Ref>& v) { pinocchio::computeAllTerms(impl_->model_, impl_->data_, q, v); } -void PinocchioModelImpl::ForwardKinematics( - const Eigen::Ref& q) { +template +void PinocchioModelImpl::ForwardKinematics( + const Eigen::Ref>& q) { pinocchio::forwardKinematics(impl_->model_, impl_->data_, q); } -void PinocchioModelImpl::ForwardKinematics( - const Eigen::Ref& q, - const Eigen::Ref& v) { +template +void PinocchioModelImpl::ForwardKinematics( + const Eigen::Ref>& q, + const Eigen::Ref>& v) { pinocchio::forwardKinematics(impl_->model_, impl_->data_, q, v); } -void PinocchioModelImpl::ForwardKinematics( - const Eigen::Ref& q, - const Eigen::Ref& v, - const Eigen::Ref& a) { +template +void PinocchioModelImpl::ForwardKinematics( + const Eigen::Ref>& q, + const Eigen::Ref>& v, + const Eigen::Ref>& a) { pinocchio::forwardKinematics(impl_->model_, impl_->data_, q, v, a); } -JointType PinocchioModelImpl::GetJointType(size_t joint_index) const { +template +JointType PinocchioModelImpl::GetJointType(size_t joint_index) const { if (impl_->model_.joints[joint_index].shortname() == "JointModelFreeFlyer") { return JointType::kFreeFlyer; } else if (impl_->model_.joints[joint_index].shortname() == "JointModelRX") { @@ -264,3 +298,6 @@ JointType PinocchioModelImpl::GetJointType(size_t joint_index) const { } // namespace internal } // namespace multibody } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::internal::PinocchioModelImpl) diff --git a/multibody/test/test_pinocchio_impl.cc b/multibody/test/test_pinocchio_impl.cc index f36c38f5..ec6707a4 100644 --- a/multibody/test/test_pinocchio_impl.cc +++ b/multibody/test/test_pinocchio_impl.cc @@ -11,8 +11,8 @@ class PinocchioModelImplTest : public testing::Test { huron::multibody::JointType::kFreeFlyer); } - huron::multibody::internal::PinocchioModelImpl impl_rrbot; - huron::multibody::internal::PinocchioModelImpl impl_huron; + huron::multibody::internal::PinocchioModelImpl impl_rrbot; + huron::multibody::internal::PinocchioModelImpl impl_huron; }; TEST_F(PinocchioModelImplTest, RRBotGeneralChecks) { From 63dfd59ce8e3ed04085be92a9cdbcd7aa79c6249 Mon Sep 17 00:00:00 2001 From: Duc Doan Date: Thu, 29 Feb 2024 16:28:13 -0500 Subject: [PATCH 5/9] Add SE3 class --- CMakeLists.txt | 14 ++ common/include/huron/types.h | 48 +++++++ math/CMakeLists.txt | 6 +- math/include/huron/math/abs.h | 20 +++ math/include/huron/math/se3.h | 53 ++++++++ math/src/se3.cc | 60 +++++++++ math/test/test_se3.cc | 125 ++++++++++++++++++ multibody/include/huron/multibody/com_frame.h | 14 +- multibody/include/huron/multibody/frame.h | 14 +- .../include/huron/multibody/logical_frame.h | 16 ++- multibody/include/huron/multibody/model.h | 51 +++---- .../huron/multibody/model_impl_interface.h | 12 +- .../huron/multibody/pinocchio_model_impl.h | 12 +- multibody/src/com_frame.cc | 26 ++-- multibody/src/frame.cc | 12 +- multibody/src/logical_frame.cc | 24 ++-- multibody/src/model.cc | 48 +++---- multibody/src/model_impl_default.cc | 12 +- multibody/src/no_pinocchio_model_impl.cc | 16 ++- multibody/src/pinocchio_model_impl.cc | 44 ++++-- .../constant_state_provider.h | 17 ++- .../huron/control_interfaces/encoder.h | 29 ++-- .../include/huron/control_interfaces/joint.h | 20 ++- .../huron/control_interfaces/legged_robot.h | 4 +- .../include/huron/control_interfaces/robot.h | 12 +- .../huron/control_interfaces/rotary_encoder.h | 63 +++++---- .../include/huron/control_interfaces/sensor.h | 12 +- .../control_interfaces/sensor_with_frame.h | 4 +- .../huron/control_interfaces/state_provider.h | 11 +- system/control_interfaces/src/joint.cc | 22 ++- system/control_interfaces/src/legged_robot.cc | 4 +- system/control_interfaces/src/robot.cc | 8 +- system/control_interfaces/src/sensor.cc | 39 ++++-- .../src/sensor_with_frame.cc | 10 +- .../huron/locomotion/zero_moment_point.h | 17 ++- .../locomotion/zero_moment_point_fsr_array.h | 17 ++- .../locomotion/zero_moment_point_ft_sensor.h | 14 +- .../locomotion/zero_moment_point_total.h | 12 +- system/locomotion/src/zero_moment_point.cc | 10 +- .../src/zero_moment_point_fsr_array.cc | 48 ++++++- .../src/zero_moment_point_ft_sensor.cc | 63 +++++++-- .../locomotion/src/zero_moment_point_total.cc | 47 ++++++- .../huron/odrive/odrive_rotary_encoder.h | 4 +- system/odrive/src/odrive_rotary_encoder.cc | 6 +- .../huron/sensors/force_sensing_resistor.h | 2 + .../sensors/force_sensing_resistor_array.h | 8 +- .../huron/sensors/force_torque_sensor.h | 10 +- system/sensors/src/force_sensing_resistor.cc | 2 + .../src/force_sensing_resistor_array.cc | 10 +- system/sensors/src/force_torque_sensor.cc | 6 +- .../huron/utils/template_instantiations.h | 45 ++++++- 51 files changed, 923 insertions(+), 280 deletions(-) create mode 100644 math/include/huron/math/abs.h create mode 100644 math/include/huron/math/se3.h create mode 100644 math/src/se3.cc create mode 100644 math/test/test_se3.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index ba94f5e6..eb6ad19e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,12 @@ if (DEFINED USE_PINOCCHIO) message(pinocchio_INCLUDE_DIRS="${pinocchio_INCLUDE_DIRS}") endif() +# Find Casadi +if (DEFINED BUILD_WITH_CASADI) + find_package(casadi REQUIRED) + message(casadi_INCLUDE_DIRS="${pinocchio_INCLUDE_DIRS}") +endif() + # Find sockcanpp list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}) find_package(sockcanpp REQUIRED) @@ -93,6 +99,14 @@ if (DEFINED USE_PINOCCHIO) target_compile_definitions(huron PUBLIC HURON_USE_PINOCCHIO=1) endif() +if (DEFINED BUILD_WITH_CASADI) + target_link_libraries(huron + PUBLIC + casadi) + target_compile_definitions(huron PUBLIC HURON_ENABLE_AUTODIFF=1) + target_compile_definitions(huron PUBLIC HURON_USE_CASADI=1) +endif() + # Copy urdf files to build directory add_custom_command(TARGET huron PRE_BUILD COMMAND ${CMAKE_COMMAND} -E copy diff --git a/common/include/huron/types.h b/common/include/huron/types.h index f0abfcc2..259b9df5 100644 --- a/common/include/huron/types.h +++ b/common/include/huron/types.h @@ -10,12 +10,21 @@ using Affine3 = Eigen::Transform; template using Vector3 = Eigen::Matrix; +template +using Vector2 = Eigen::Matrix; + template using VectorX = Eigen::Matrix; template using MatrixX = Eigen::Matrix; +template +using Matrix3 = Eigen::Matrix; + +template +using Matrix4 = Eigen::Matrix; + template using Vector6 = Eigen::Matrix; @@ -25,9 +34,48 @@ using Matrix6X = Eigen::Matrix; /// double aliases using Affine3d = Affine3; using Vector3d = Vector3; +using Vector2d = Vector2; using VectorXd = VectorX; using MatrixXd = MatrixX; +using Matrix3d = Matrix3; +using Matrix4d = Matrix4; using Vector6d = Vector6; using Matrix6Xd = Matrix6X; } // namespace huron + +#if HURON_ENABLE_AUTODIFF==1 +#if HURON_USE_CASADI==1 + +#include + +// namespace Eigen { +// +// template +// struct NumTraits> : NumTraits +// { +// using Real = casadi::Matrix; +// using NonInteger = casadi::Matrix; +// using Literal = casadi::Matrix; +// using Nested = casadi::Matrix; +// +// enum { +// // does not support complex Base types +// IsComplex = 0 , +// // does not support integer Base types +// IsInteger = 0 , +// // only support signed Base types +// IsSigned = 1 , +// // must initialize an AD object +// RequireInitialization = 1 , +// // computational cost of the corresponding operations +// ReadCost = 1 , +// AddCost = 2 , +// MulCost = 2 +// }; +// }; +// +// } // namespace Eigen + +#endif // HURON_USE_CASADI +#endif // HURON_ENABLE_AUTODIFF diff --git a/math/CMakeLists.txt b/math/CMakeLists.txt index 5d97033d..63fdece4 100644 --- a/math/CMakeLists.txt +++ b/math/CMakeLists.txt @@ -1,13 +1,15 @@ set(dir "${CMAKE_CURRENT_SOURCE_DIR}") list(APPEND SOURCE_FILES - "${dir}/src/rotation.cc") + "${dir}/src/rotation.cc" + "${dir}/src/se3.cc") FILE(GLOB_RECURSE HEADER_FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h ) list(APPEND TEST_SOURCE_FILES - "${dir}/test/test_rotation.cc") + "${dir}/test/test_rotation.cc" + "${dir}/test/test_se3.cc") set(INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include/") diff --git a/math/include/huron/math/abs.h b/math/include/huron/math/abs.h new file mode 100644 index 00000000..71748e96 --- /dev/null +++ b/math/include/huron/math/abs.h @@ -0,0 +1,20 @@ +#pragma once + +#include "huron/types.h" + +namespace huron { + +template +inline T abs(const T& x) { + return x < 0 ? -x : x; +} + +#if HURON_USE_CASADI==1 + +template <> +inline casadi::SX abs(const casadi::SX& x) { + return casadi::SX::abs(x); +} + +#endif // HURON_USE_CASADI==1 +} // namespace huron diff --git a/math/include/huron/math/se3.h b/math/include/huron/math/se3.h new file mode 100644 index 00000000..6e03ba51 --- /dev/null +++ b/math/include/huron/math/se3.h @@ -0,0 +1,53 @@ +#pragma once + +#include "huron/types.h" +#include "huron/utils/template_instantiations.h" + +namespace huron { + +template +class SE3 { + public: + SE3(); + SE3(const Eigen::Matrix4& tf); + SE3(const Eigen::Matrix3& R, const Eigen::Vector3& t); + SE3(const SE3&); + SE3& operator=(const SE3& other) { + tf_ = other.tf_; + return *this; + } + virtual ~SE3() = default; + + SE3 operator*(const SE3& other) const { + SE3 ret = *this; + ret.tf_ *= other.tf_; + return ret; + } + + SE3& operator*=(const SE3& other) { + tf_ *= other.tf_; + return *this; + } + + Matrix4& matrix() { return tf_; } + const Matrix4& matrix() const { return tf_; } + + Matrix3 rotation() const; + Vector3 translation() const; + + void rotate(const Matrix3& R); + void prerotate(const Matrix3& R); + void translate(const Vector3& v); + + SE3 inverse() const; + + private: + Matrix4 tf_; +}; + +} // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::SE3) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::SE3) diff --git a/math/src/se3.cc b/math/src/se3.cc new file mode 100644 index 00000000..997147f9 --- /dev/null +++ b/math/src/se3.cc @@ -0,0 +1,60 @@ +#include "huron/math/se3.h" + +namespace huron { + +template +SE3::SE3() : tf_(Matrix4::Identity()) {} + +template +SE3::SE3(const Eigen::Matrix4& tf) : tf_(tf) {} + +template +SE3::SE3(const Eigen::Matrix3& R, const Eigen::Vector3& t) { + tf_.block(0, 0, 3, 3) = R; + tf_.block(0, 3, 3, 1) = t; + tf_.row(3) << 0, 0, 0, 1; +} + +template +SE3::SE3(const SE3& other) : tf_(other.tf_) {} + +template +Matrix3 SE3::rotation() const { + return tf_.block(0, 0, 3, 3); +} + +template +Vector3 SE3::translation() const { + return tf_.block(0, 3, 3, 1); +} + +template +void SE3::rotate(const Matrix3& R) { + tf_.block(0, 0, 3, 3) = tf_.block(0, 0, 3, 3); +} + +template +void SE3::prerotate(const Matrix3& R) { + Matrix4 R_tf = Matrix4::Identity(); + R_tf.block(0, 0, 3, 3) = R; + tf_ = R_tf * tf_; +} + +template +void SE3::translate(const Vector3& v) { + tf_.block(0, 3, 3, 1) += v; +} + +template +SE3 SE3::inverse() const { + SE3 ret; + ret.tf_ = tf_.inverse(); + return ret; +} + +} // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::SE3) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::SE3) diff --git a/math/test/test_se3.cc b/math/test/test_se3.cc new file mode 100644 index 00000000..b043eb04 --- /dev/null +++ b/math/test/test_se3.cc @@ -0,0 +1,125 @@ +#include +#include +#include +#include "huron/math/se3.h" + +class TestSe3 : public testing::Test { + protected: + void SetUp() override { + uniform_dist = std::uniform_real_distribution(-3.14159, 3.14159); + } + + // void TearDown() override {} + std::uniform_real_distribution uniform_dist; + std::default_random_engine re; +}; + +TEST_F(TestSe3, Identity) { + EXPECT_EQ(huron::SE3().matrix(), Eigen::Matrix4d::Identity()); +} + +TEST_F(TestSe3, InitFromMatrix) { + Eigen::Affine3d tf_gt; + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + EXPECT_EQ(huron::SE3(tf_gt.matrix()).matrix(), tf_gt); +} + +TEST_F(TestSe3, InitFromRotationTranslation) { + Eigen::Affine3d tf_gt; + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + EXPECT_EQ(huron::SE3(tf_gt.rotation().matrix(), + tf_gt.translation().matrix()).matrix(), + tf_gt); +} + +TEST_F(TestSe3, Translation) { + // Creating a random ground truth transformation + Eigen::Affine3d tf_gt; + tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + // Test object + huron::SE3 tf; + tf.translate(tf_gt.translation().matrix()); + + EXPECT_EQ(tf.matrix(), tf_gt.matrix()); +} + +TEST_F(TestSe3, Prerotation) { + // Creating a random ground truth transformation + std::uniform_real_distribution uniform_dist(-3.14159, 3.14159); + std::default_random_engine re; + Eigen::Affine3d a, b, tf_gt; + a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + b.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + b.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + b.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + tf_gt = a; + tf_gt.prerotate(b.rotation()); + // Test object + huron::SE3 tf(a.matrix(), Eigen::Vector3d::Zero());; + tf.prerotate(b.rotation().matrix()); + + EXPECT_EQ(tf.matrix(), tf_gt.matrix()); +} + +TEST_F(TestSe3, Rotation) { + // Creating a random ground truth transformation + std::uniform_real_distribution uniform_dist(-3.14159, 3.14159); + std::default_random_engine re; + Eigen::Affine3d a, b, tf_gt; + a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + b.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + b.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + b.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + tf_gt = a; + tf_gt.rotate(b.rotation()); + // Test object + huron::SE3 tf(a.matrix(), Eigen::Vector3d::Zero());; + tf.rotate(b.rotation().matrix()); + + EXPECT_EQ(tf.matrix(), tf_gt.matrix()); +} + +TEST_F(TestSe3, TransRot) { + // Creating a random ground truth transformation + Eigen::Affine3d tf_gt; + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + // Test object + huron::SE3 tf; + tf.rotate(tf_gt.rotation().matrix()); + tf.translate(tf_gt.translation().matrix()); + + EXPECT_EQ(tf.matrix(), tf_gt.matrix()); +} + +TEST_F(TestSe3, Multiplication) { + // Creating a random ground truth transformation + Eigen::Affine3d a, b, tf_gt; + a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + a.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + b.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + b.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + b.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + b.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + tf_gt = a * b; + // Test object + huron::SE3 tfa(a.matrix()), tfb(b.matrix()), tf; + tf = tfa * tfb; + + EXPECT_EQ(tf.matrix(), tf_gt.matrix()); + EXPECT_EQ((tfa *= tfb).matrix(), tf_gt.matrix()); +} diff --git a/multibody/include/huron/multibody/com_frame.h b/multibody/include/huron/multibody/com_frame.h index 46a1e819..d989e2c6 100644 --- a/multibody/include/huron/multibody/com_frame.h +++ b/multibody/include/huron/multibody/com_frame.h @@ -24,16 +24,16 @@ class ComFrame : public Frame { ComFrame& operator=(const ComFrame&) = delete; ~ComFrame() override = default; - Eigen::Affine3d GetTransformInWorld() const override; - Eigen::Affine3d GetTransformFromFrame(const Frame& other) const override; - Eigen::Affine3d GetTransformFromFrame(FrameIndex other) const override; - Eigen::Affine3d GetTransformToFrame(const Frame& other) const override; - Eigen::Affine3d GetTransformToFrame(FrameIndex other) const override; + huron::SE3 GetTransformInWorld() const override; + huron::SE3 GetTransformFromFrame(const Frame& other) const override; + huron::SE3 GetTransformFromFrame(FrameIndex other) const override; + huron::SE3 GetTransformToFrame(const Frame& other) const override; + huron::SE3 GetTransformToFrame(FrameIndex other) const override; private: FrameIndex parent_frame_index_; - Eigen::Affine3d ParentToThisTransform() const; + huron::SE3 ParentToThisTransform() const; }; } // namespace multibody @@ -41,3 +41,5 @@ class ComFrame : public Frame { HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::ComFrame) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::ComFrame) diff --git a/multibody/include/huron/multibody/frame.h b/multibody/include/huron/multibody/frame.h index a0126adf..e1036fe8 100644 --- a/multibody/include/huron/multibody/frame.h +++ b/multibody/include/huron/multibody/frame.h @@ -6,6 +6,8 @@ #include #include "huron/multibody/fwd.h" +#include "huron/types.h" +#include "huron/math/se3.h" #include "huron/utils/template_instantiations.h" #include "huron/enable_protected_make_shared.h" @@ -31,11 +33,11 @@ class Frame : public enable_protected_make_shared> { Frame& operator=(const Frame&) = delete; virtual ~Frame() = default; - virtual Eigen::Affine3d GetTransformInWorld() const; - virtual Eigen::Affine3d GetTransformFromFrame(const Frame& other) const; - virtual Eigen::Affine3d GetTransformFromFrame(FrameIndex other) const; - virtual Eigen::Affine3d GetTransformToFrame(const Frame& other) const; - virtual Eigen::Affine3d GetTransformToFrame(FrameIndex other) const; + virtual huron::SE3 GetTransformInWorld() const; + virtual huron::SE3 GetTransformFromFrame(const Frame& other) const; + virtual huron::SE3 GetTransformFromFrame(FrameIndex other) const; + virtual huron::SE3 GetTransformToFrame(const Frame& other) const; + virtual huron::SE3 GetTransformToFrame(FrameIndex other) const; const std::string& name() const { return name_; } FrameIndex index() const { return index_; } @@ -62,3 +64,5 @@ class Frame : public enable_protected_make_shared> { HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::Frame) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::Frame) diff --git a/multibody/include/huron/multibody/logical_frame.h b/multibody/include/huron/multibody/logical_frame.h index 56c839ed..29c90eff 100644 --- a/multibody/include/huron/multibody/logical_frame.h +++ b/multibody/include/huron/multibody/logical_frame.h @@ -37,11 +37,11 @@ class LogicalFrame LogicalFrame& operator=(const LogicalFrame&) = delete; ~LogicalFrame() override = default; - Eigen::Affine3d GetTransformInWorld() const override; - Eigen::Affine3d GetTransformFromFrame(const Frame& other) const override; - Eigen::Affine3d GetTransformFromFrame(FrameIndex other) const override; - Eigen::Affine3d GetTransformToFrame(const Frame& other) const override; - Eigen::Affine3d GetTransformToFrame(FrameIndex other) const override; + huron::SE3 GetTransformInWorld() const override; + huron::SE3 GetTransformFromFrame(const Frame& other) const override; + huron::SE3 GetTransformFromFrame(FrameIndex other) const override; + huron::SE3 GetTransformToFrame(const Frame& other) const override; + huron::SE3 GetTransformToFrame(FrameIndex other) const override; protected: LogicalFrame(FrameIndex index, @@ -49,12 +49,12 @@ class LogicalFrame bool is_user_defined, std::weak_ptr> model, FrameIndex parent_frame_index, - std::function + std::function(const huron::SE3&)> transform_function); private: FrameIndex parent_frame_index_; - const std::function + const std::function(const huron::SE3&)> transform_function_; }; @@ -63,3 +63,5 @@ class LogicalFrame HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::LogicalFrame) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::LogicalFrame) diff --git a/multibody/include/huron/multibody/model.h b/multibody/include/huron/multibody/model.h index 00e7f4e7..e4fde552 100644 --- a/multibody/include/huron/multibody/model.h +++ b/multibody/include/huron/multibody/model.h @@ -6,6 +6,7 @@ #include #include +#include "huron/types.h" #include "huron/utils/template_instantiations.h" #include "huron/multibody/model_impl_types.h" #include "huron/multibody/model_impl_interface.h" @@ -49,14 +50,14 @@ class Model : public std::enable_shared_from_this> { // TODO(dtbpkmte): provide index information in the error message. throw std::runtime_error("Joint already exists at this index."); } - joints_[index] = std::make_shared(std::forward(args)...); + joints_[index] = std::make_shared>(std::forward(args)...); } - Joint* const GetJoint(JointIndex index); - Joint* const GetJoint(const std::string& name); + Joint* const GetJoint(JointIndex index); + Joint* const GetJoint(const std::string& name); void SetJointStateProvider(JointIndex index, - std::shared_ptr state_provider); + std::shared_ptr> state_provider); JointIndex GetJointIndex(const std::string& joint_name) const; /** @@ -103,7 +104,7 @@ class Model : public std::enable_shared_from_this> { * This method also sets the initial state [q, v] of the model. * @throws std::runtime_error if the model is not valid. */ - void Finalize(const Eigen::VectorXd& initial_state); + void Finalize(const huron::VectorX& initial_state); void Finalize(); /** @@ -120,67 +121,67 @@ class Model : public std::enable_shared_from_this> { // Kinematics and Dynamics wrapper functions - Eigen::Affine3d GetJointTransformInWorld(size_t joint_index) const; + huron::SE3 GetJointTransformInWorld(size_t joint_index) const; FrameIndex GetFrameIndex(const std::string& frame_name) const; const std::string& GetFrameName(FrameIndex frame_index) const; - Eigen::Affine3d GetFrameTransform(FrameIndex from_frame, + huron::SE3 GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const; - Eigen::Affine3d GetFrameTransformInWorld(FrameIndex frame) const; + huron::SE3 GetFrameTransformInWorld(FrameIndex frame) const; - Eigen::VectorXd NeutralConfiguration() const; + huron::VectorX NeutralConfiguration() const; - Eigen::Vector3d EvalCenterOfMassPosition(); - Eigen::Vector3d GetCenterOfMassPosition() const; + huron::Vector3 EvalCenterOfMassPosition(); + huron::Vector3 GetCenterOfMassPosition() const; - const Eigen::VectorBlock GetPositions() const; + const Eigen::VectorBlock> GetPositions() const; - const Eigen::VectorBlock GetVelocities() const; + const Eigen::VectorBlock> GetVelocities() const; /** * @brief Get the generalized accelerations of the model. */ - const Eigen::VectorXd& GetAccelerations() const; + const huron::VectorX& GetAccelerations() const; /** * @brief Get the joint torques. */ - const Eigen::VectorXd& GetTorques() const; + const huron::VectorX& GetTorques() const; /** * @brief Get the mass matrix with the cached value. */ - const Eigen::MatrixXd& GetMassMatrix() const; + const huron::MatrixX& GetMassMatrix() const; /** * @brief Get the Coriolis matrix with the cached value. */ - const Eigen::MatrixXd& GetCoriolisMatrix() const; + const huron::MatrixX& GetCoriolisMatrix() const; /** * @brief Get the nonlinear effects vector. */ - const Eigen::VectorXd& GetNonlinearEffects() const; + const huron::VectorX& GetNonlinearEffects() const; /** * @brief Get the gravity vector. */ - const Eigen::VectorXd& GetGravity() const; + const huron::VectorX& GetGravity() const; /** * @brief Get the spatial momentum with respect to the specified frame. */ - const huron::Vector6d& GetSpatialMomentum() const; + const huron::Vector6& GetSpatialMomentum() const; /** * @brief Get the centroidal momentum. */ - huron::Vector6d GetCentroidalMomentum() const; + huron::Vector6 GetCentroidalMomentum() const; /** * @brief Get the centroidal momentum matrix with the cached value. */ - const huron::Matrix6Xd& GetCentroidalMatrix() const; + const huron::Matrix6X& GetCentroidalMatrix() const; void ComputeAll(); @@ -238,9 +239,9 @@ class Model : public std::enable_shared_from_this> { size_t default_impl_index_ = 0; std::vector> impls_; - std::vector> joints_; + std::vector>> joints_; /// \brief The joint states [q, v]. - Eigen::VectorXd states_; + huron::VectorX states_; size_t num_positions_ = 0; size_t num_velocities_ = 0; @@ -258,3 +259,5 @@ class Model : public std::enable_shared_from_this> { HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::Model) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::Model) diff --git a/multibody/include/huron/multibody/model_impl_interface.h b/multibody/include/huron/multibody/model_impl_interface.h index c5f21e14..d3c4a151 100644 --- a/multibody/include/huron/multibody/model_impl_interface.h +++ b/multibody/include/huron/multibody/model_impl_interface.h @@ -24,8 +24,8 @@ class ModelImplInterface { JointType root_joint_type); virtual const std::vector& GetJointNames() const; - virtual std::weak_ptr GetJoint(const std::string& name) const; - virtual std::weak_ptr GetJoint(size_t joint_index) const; + virtual std::weak_ptr> GetJoint(const std::string& name) const; + virtual std::weak_ptr> GetJoint(size_t joint_index) const; virtual JointType GetJointType(size_t joint_index) const; virtual JointIndex GetJointIndex(const std::string& joint_name) const = 0; @@ -35,16 +35,16 @@ class ModelImplInterface { virtual std::unique_ptr GetJointDescription( const std::string& joint_name) const; - virtual huron::Affine3 + virtual huron::SE3 GetJointTransformInWorld(size_t joint_index) const; virtual FrameIndex GetFrameIndex( const std::string& frame_name) const; virtual const std::string& GetFrameName(FrameIndex frame_index) const; virtual FrameType GetFrameType(FrameIndex frame_index) const; - virtual huron::Affine3 GetFrameTransform(FrameIndex from_frame, + virtual huron::SE3 GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const; - virtual huron::Affine3 GetFrameTransformInWorld(FrameIndex frame) const; + virtual huron::SE3 GetFrameTransformInWorld(FrameIndex frame) const; virtual huron::Vector3 EvalCenterOfMassPosition(); virtual huron::Vector3 GetCenterOfMassPosition() const; @@ -124,3 +124,5 @@ class ModelImplInterface { HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::internal::ModelImplInterface) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::internal::ModelImplInterface) diff --git a/multibody/include/huron/multibody/pinocchio_model_impl.h b/multibody/include/huron/multibody/pinocchio_model_impl.h index 752b1cea..86a342f4 100644 --- a/multibody/include/huron/multibody/pinocchio_model_impl.h +++ b/multibody/include/huron/multibody/pinocchio_model_impl.h @@ -28,8 +28,8 @@ class PinocchioModelImpl : public ModelImplInterface { JointType root_joint_type) override; const std::vector& GetJointNames() const override; - std::weak_ptr GetJoint(const std::string& name) const override; - std::weak_ptr GetJoint(size_t joint_index) const override; + std::weak_ptr> GetJoint(const std::string& name) const override; + std::weak_ptr> GetJoint(size_t joint_index) const override; JointType GetJointType(size_t joint_index) const override; JointIndex GetJointIndex(const std::string& joint_name) const override; @@ -39,15 +39,15 @@ class PinocchioModelImpl : public ModelImplInterface { std::unique_ptr GetJointDescription( const std::string& joint_name) const override; - huron::Affine3 + huron::SE3 GetJointTransformInWorld(size_t joint_index) const override; FrameIndex GetFrameIndex(const std::string& frame_name) const override; const std::string& GetFrameName(FrameIndex frame_index) const override; FrameType GetFrameType(FrameIndex frame_index) const override; - huron::Affine3 GetFrameTransform(FrameIndex from_frame, + huron::SE3 GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const override; - huron::Affine3 + huron::SE3 GetFrameTransformInWorld(FrameIndex frame) const override; huron::Vector3 EvalCenterOfMassPosition() override; @@ -102,3 +102,5 @@ class PinocchioModelImpl : public ModelImplInterface { HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::internal::PinocchioModelImpl) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::internal::PinocchioModelImpl) diff --git a/multibody/src/com_frame.cc b/multibody/src/com_frame.cc index 2ceb1710..3536f958 100644 --- a/multibody/src/com_frame.cc +++ b/multibody/src/com_frame.cc @@ -15,43 +15,43 @@ ComFrame::ComFrame( parent_frame_index_(parent_frame_index) {} template -Eigen::Affine3d ComFrame::GetTransformInWorld() const { - Eigen::Affine3d parent_transform = +huron::SE3 ComFrame::GetTransformInWorld() const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransformInWorld(parent_frame_index_); return parent_transform * ParentToThisTransform(); } template -Eigen::Affine3d ComFrame::GetTransformFromFrame(const Frame& other) const { - Eigen::Affine3d parent_transform = +huron::SE3 ComFrame::GetTransformFromFrame(const Frame& other) const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(other.index(), parent_frame_index_); return parent_transform * ParentToThisTransform(); } template -Eigen::Affine3d ComFrame::GetTransformFromFrame(FrameIndex other) const { - Eigen::Affine3d parent_transform = +huron::SE3 ComFrame::GetTransformFromFrame(FrameIndex other) const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(other, parent_frame_index_); return parent_transform * ParentToThisTransform(); } template -Eigen::Affine3d ComFrame::GetTransformToFrame(const Frame& other) const { - Eigen::Affine3d parent_transform = +huron::SE3 ComFrame::GetTransformToFrame(const Frame& other) const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(parent_frame_index_, other.index()); return ParentToThisTransform().inverse() * parent_transform; } template -Eigen::Affine3d ComFrame::GetTransformToFrame(FrameIndex other) const { - Eigen::Affine3d parent_transform = +huron::SE3 ComFrame::GetTransformToFrame(FrameIndex other) const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(parent_frame_index_, other); return ParentToThisTransform().inverse() * parent_transform; } template -Eigen::Affine3d ComFrame::ParentToThisTransform() const { - Eigen::Affine3d parent_to_this = Eigen::Affine3d::Identity(); +huron::SE3 ComFrame::ParentToThisTransform() const { + huron::SE3 parent_to_this; parent_to_this.translate(this->model_.lock()->GetCenterOfMassPosition()); return parent_to_this; } @@ -61,3 +61,5 @@ Eigen::Affine3d ComFrame::ParentToThisTransform() const { HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::ComFrame) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::ComFrame) diff --git a/multibody/src/frame.cc b/multibody/src/frame.cc index 30882fdd..c658238a 100644 --- a/multibody/src/frame.cc +++ b/multibody/src/frame.cc @@ -17,27 +17,27 @@ Frame::Frame(FrameIndex index, model_(std::move(model)) {} template -Eigen::Affine3d Frame::GetTransformInWorld() const { +huron::SE3 Frame::GetTransformInWorld() const { return model_.lock()->GetFrameTransformInWorld(index_); } template -Eigen::Affine3d Frame::GetTransformFromFrame(const Frame& other) const { +huron::SE3 Frame::GetTransformFromFrame(const Frame& other) const { return model_.lock()->GetFrameTransform(other.index_, index_); } template -Eigen::Affine3d Frame::GetTransformFromFrame(FrameIndex other) const { +huron::SE3 Frame::GetTransformFromFrame(FrameIndex other) const { return model_.lock()->GetFrameTransform(other, index_); } template -Eigen::Affine3d Frame::GetTransformToFrame(const Frame& other) const { +huron::SE3 Frame::GetTransformToFrame(const Frame& other) const { return model_.lock()->GetFrameTransform(index_, other.index_); } template -Eigen::Affine3d Frame::GetTransformToFrame(FrameIndex other) const { +huron::SE3 Frame::GetTransformToFrame(FrameIndex other) const { return model_.lock()->GetFrameTransform(index_, other); } @@ -46,3 +46,5 @@ Eigen::Affine3d Frame::GetTransformToFrame(FrameIndex other) const { HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::Frame) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::Frame) diff --git a/multibody/src/logical_frame.cc b/multibody/src/logical_frame.cc index 59f7b8e4..45f1358e 100644 --- a/multibody/src/logical_frame.cc +++ b/multibody/src/logical_frame.cc @@ -11,42 +11,42 @@ LogicalFrame::LogicalFrame( bool is_user_defined, std::weak_ptr> model, FrameIndex parent_frame_index, - std::function transform_function) + std::function(const huron::SE3&)> transform_function) : Frame(index, name, FrameType::kLogical, is_user_defined, std::move(model)), parent_frame_index_(parent_frame_index), transform_function_(transform_function) {} template -Eigen::Affine3d LogicalFrame::GetTransformInWorld() const { - Eigen::Affine3d parent_transform = +huron::SE3 LogicalFrame::GetTransformInWorld() const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransformInWorld(parent_frame_index_); return parent_transform * transform_function_(parent_transform); } template -Eigen::Affine3d LogicalFrame::GetTransformFromFrame(const Frame& other) const { - Eigen::Affine3d parent_transform = +huron::SE3 LogicalFrame::GetTransformFromFrame(const Frame& other) const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(other.index(), parent_frame_index_); return parent_transform * transform_function_(parent_transform); } template -Eigen::Affine3d LogicalFrame::GetTransformFromFrame(FrameIndex other) const { - Eigen::Affine3d parent_transform = +huron::SE3 LogicalFrame::GetTransformFromFrame(FrameIndex other) const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(other, parent_frame_index_); return parent_transform * transform_function_(parent_transform); } template -Eigen::Affine3d LogicalFrame::GetTransformToFrame(const Frame& other) const { - Eigen::Affine3d parent_transform = +huron::SE3 LogicalFrame::GetTransformToFrame(const Frame& other) const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(parent_frame_index_, other.index()); return transform_function_(parent_transform).inverse() * parent_transform; } template -Eigen::Affine3d LogicalFrame::GetTransformToFrame(FrameIndex other) const { - Eigen::Affine3d parent_transform = +huron::SE3 LogicalFrame::GetTransformToFrame(FrameIndex other) const { + huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(parent_frame_index_, other); return transform_function_(parent_transform).inverse() * parent_transform; } @@ -56,3 +56,5 @@ Eigen::Affine3d LogicalFrame::GetTransformToFrame(FrameIndex other) const { HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::LogicalFrame) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::LogicalFrame) diff --git a/multibody/src/model.cc b/multibody/src/model.cc index c42baa80..686fbcc5 100644 --- a/multibody/src/model.cc +++ b/multibody/src/model.cc @@ -34,7 +34,7 @@ void Model::BuildFromUrdf(const std::string& urdf_path, num_positions_ = impls_[default_impl_index_]->num_positions(); num_velocities_ = impls_[default_impl_index_]->num_velocities(); // Initialize the joint vector - joints_ = std::vector>( + joints_ = std::vector>>( impls_[default_impl_index_]->num_joints()); // Initialize the frame vector frames_ = std::vector>>( @@ -63,7 +63,7 @@ void Model::BuildFromUrdf(const std::string& urdf_path, } template -void Model::Finalize(const Eigen::VectorXd& initial_state) { +void Model::Finalize(const huron::VectorX& initial_state) { assert(is_constructed_); // Check if all joints are added to the model, and set the joint indices. for (auto& joint : joints_) { @@ -82,17 +82,17 @@ void Model::Finalize(const Eigen::VectorXd& initial_state) { template void Model::Finalize() { assert(is_constructed_); - Finalize(Eigen::VectorXd::Zero(num_positions_ + num_velocities_)); + Finalize(huron::VectorX::Zero(num_positions_ + num_velocities_)); } template -Joint* const Model::GetJoint(JointIndex index) { +Joint* const Model::GetJoint(JointIndex index) { assert(is_constructed_); return joints_[index].get(); } template -Joint* const Model::GetJoint(const std::string& name) { +Joint* const Model::GetJoint(const std::string& name) { assert(is_constructed_); return joints_[GetJointIndex(name)].get(); } @@ -100,7 +100,7 @@ Joint* const Model::GetJoint(const std::string& name) { template void Model::SetJointStateProvider( JointIndex index, - std::shared_ptr state_provider) { + std::shared_ptr> state_provider) { assert(is_constructed_); joints_[index]->SetStateProvider(std::move(state_provider)); } @@ -150,7 +150,7 @@ std::weak_ptr> Model::GetFrame(const std::string& name) const // Kinematics and Dynamics functions template -Eigen::Affine3d Model::GetJointTransformInWorld(size_t joint_index) const { +huron::SE3 Model::GetJointTransformInWorld(size_t joint_index) const { assert(is_finalized_); return impls_[default_impl_index_]->GetJointTransformInWorld(joint_index); } @@ -162,98 +162,98 @@ FrameIndex Model::GetFrameIndex(const std::string& frame_name) const { } template -Eigen::Affine3d Model::GetFrameTransform(FrameIndex from_frame, +huron::SE3 Model::GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const { assert(is_finalized_); return impls_[default_impl_index_]->GetFrameTransform(from_frame, to_frame); } template -Eigen::Affine3d Model::GetFrameTransformInWorld(FrameIndex frame) const { +huron::SE3 Model::GetFrameTransformInWorld(FrameIndex frame) const { assert(is_finalized_); return impls_[default_impl_index_]->GetFrameTransformInWorld(frame); } template -Eigen::VectorXd Model::NeutralConfiguration() const { +huron::VectorX Model::NeutralConfiguration() const { assert(is_finalized_); return impls_[default_impl_index_]->NeutralConfiguration(); } template -Eigen::Vector3d Model::EvalCenterOfMassPosition() { +huron::Vector3 Model::EvalCenterOfMassPosition() { assert(is_finalized_); return impls_[default_impl_index_]->EvalCenterOfMassPosition(); } template -Eigen::Vector3d Model::GetCenterOfMassPosition() const { +huron::Vector3 Model::GetCenterOfMassPosition() const { assert(is_finalized_); return impls_[default_impl_index_]->GetCenterOfMassPosition(); } template -const Eigen::VectorBlock Model::GetPositions() const { +const Eigen::VectorBlock> Model::GetPositions() const { assert(is_finalized_); return states_.segment(0, num_positions_); } template -const Eigen::VectorBlock Model::GetVelocities() const { +const Eigen::VectorBlock> Model::GetVelocities() const { assert(is_finalized_); return states_.segment(num_positions_, num_velocities_); } template -const Eigen::VectorXd& Model::GetAccelerations() const { +const huron::VectorX& Model::GetAccelerations() const { assert(is_finalized_); return impls_[default_impl_index_]->GetAccelerations(); } template -const Eigen::VectorXd& Model::GetTorques() const { +const huron::VectorX& Model::GetTorques() const { assert(is_finalized_); return impls_[default_impl_index_]->GetTorques(); } template -const Eigen::MatrixXd& Model::GetMassMatrix() const { +const huron::MatrixX& Model::GetMassMatrix() const { assert(is_finalized_); return impls_[default_impl_index_]->GetMassMatrix(); } template -const Eigen::MatrixXd& Model::GetCoriolisMatrix() const { +const huron::MatrixX& Model::GetCoriolisMatrix() const { assert(is_finalized_); return impls_[default_impl_index_]->GetCoriolisMatrix(); } template -const Eigen::VectorXd& Model::GetNonlinearEffects() const { +const huron::VectorX& Model::GetNonlinearEffects() const { assert(is_finalized_); return impls_[default_impl_index_]->GetNonlinearEffects(); } template -const Eigen::VectorXd& Model::GetGravity() const { +const huron::VectorX& Model::GetGravity() const { assert(is_finalized_); return impls_[default_impl_index_]->GetGravity(); } template -const huron::Vector6d& Model::GetSpatialMomentum() const { +const huron::Vector6& Model::GetSpatialMomentum() const { assert(is_finalized_); return impls_[default_impl_index_]->GetSpatialMomentum(); } template -huron::Vector6d Model::GetCentroidalMomentum() const { +huron::Vector6 Model::GetCentroidalMomentum() const { assert(is_finalized_); return impls_[default_impl_index_]->GetCentroidalMomentum(); } template -const huron::Matrix6Xd& Model::GetCentroidalMatrix() const { +const huron::Matrix6X& Model::GetCentroidalMatrix() const { assert(is_finalized_); return impls_[default_impl_index_]->GetCentroidalMatrix(); } @@ -279,3 +279,5 @@ void Model::ForwardKinematics() { HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::Model) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::Model) diff --git a/multibody/src/model_impl_default.cc b/multibody/src/model_impl_default.cc index 69d47df6..7b21d046 100644 --- a/multibody/src/model_impl_default.cc +++ b/multibody/src/model_impl_default.cc @@ -16,13 +16,13 @@ const std::vector& ModelImplInterface::GetJointNames() const { } template -std::weak_ptr +std::weak_ptr> ModelImplInterface::GetJoint(const std::string& name) const { throw NotImplementedException(); } template -std::weak_ptr +std::weak_ptr> ModelImplInterface::GetJoint(size_t joint_index) const { throw NotImplementedException(); } @@ -50,7 +50,7 @@ std::unique_ptr ModelImplInterface::GetJointDescription( } template -huron::Affine3 +huron::SE3 ModelImplInterface::GetJointTransformInWorld(size_t joint_index) const { throw NotImplementedException(); } @@ -73,14 +73,14 @@ FrameType ModelImplInterface::GetFrameType(FrameIndex frame_index) const { } template -huron::Affine3 +huron::SE3 ModelImplInterface::GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const { throw NotImplementedException(); } template -huron::Affine3 +huron::SE3 ModelImplInterface::GetFrameTransformInWorld(FrameIndex frame) const { throw NotImplementedException(); } @@ -199,3 +199,5 @@ size_t ModelImplInterface::num_frames() const { HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::internal::ModelImplInterface) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::internal::ModelImplInterface) diff --git a/multibody/src/no_pinocchio_model_impl.cc b/multibody/src/no_pinocchio_model_impl.cc index 9a20c57a..8e922b91 100644 --- a/multibody/src/no_pinocchio_model_impl.cc +++ b/multibody/src/no_pinocchio_model_impl.cc @@ -5,16 +5,24 @@ namespace huron { namespace multibody { namespace internal { -struct PinocchioModelImpl::Impl { +template +struct PinocchioModelImpl::Impl { int dummy; }; -PinocchioModelImpl::PinocchioModelImpl() - : ModelImplInterface() { +template +PinocchioModelImpl::PinocchioModelImpl() + : ModelImplInterface() { throw NotImplementedException("Pinocchio not available!"); } -PinocchioModelImpl::~PinocchioModelImpl() = default; +template +PinocchioModelImpl::~PinocchioModelImpl() = default; } // namespace internal } // namespace multibody } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::multibody::internal::PinocchioModelImpl) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::internal::PinocchioModelImpl) diff --git a/multibody/src/pinocchio_model_impl.cc b/multibody/src/pinocchio_model_impl.cc index 8a3d474a..bf6cfda0 100644 --- a/multibody/src/pinocchio_model_impl.cc +++ b/multibody/src/pinocchio_model_impl.cc @@ -19,11 +19,11 @@ namespace internal { namespace helpers { template -huron::Affine3 Se3ToAffine3(const pinocchio::SE3Tpl& se3) { - huron::Affine3 affine; - affine.linear() = se3.rotation(); - affine.translation() = se3.translation(); - return affine; +huron::SE3 PinSe3ToHuronSe3(const pinocchio::SE3Tpl& se3) { + huron::SE3 ret; + ret.rotate(se3.rotation()); + ret.translate(se3.translation()); + return ret; } } // namespace helpers @@ -44,9 +44,20 @@ PinocchioModelImpl::~PinocchioModelImpl() = default; template void PinocchioModelImpl::BuildFromUrdf(const std::string& urdf_path, JointType root_joint_type) { + #if HURON_USE_CASADI==1 + pinocchio::Model tmp_model; + #endif + pinocchio::Model::JointModel joint_model; if (root_joint_type == JointType::kFixed) { + + #if HURON_USE_CASADI==1 + pinocchio::urdf::buildModel(urdf_path, tmp_model); + impl_->model_ = tmp_model.cast(); + #else pinocchio::urdf::buildModel(urdf_path, impl_->model_); + #endif + } else { switch (root_joint_type) { case JointType::kFreeFlyer: @@ -59,9 +70,14 @@ void PinocchioModelImpl::BuildFromUrdf(const std::string& urdf_path, throw std::runtime_error("Unsupported root joint type."); break; } + #if HURON_USE_CASADI==1 + pinocchio::urdf::buildModel(urdf_path, joint_model, tmp_model); + impl_->model_ = tmp_model.cast(); + #else pinocchio::urdf::buildModel(urdf_path, joint_model, impl_->model_); + #endif } - impl_->data_ = pinocchio::Data(impl_->model_); + impl_->data_ = pinocchio::DataTpl(impl_->model_); num_positions_ = impl_->model_.nq; num_velocities_ = impl_->model_.nv; num_joints_ = impl_->model_.njoints; @@ -74,13 +90,13 @@ const std::vector& PinocchioModelImpl::GetJointNames() const { } template -std::weak_ptr +std::weak_ptr> PinocchioModelImpl::GetJoint(const std::string& name) const { throw NotImplementedException(); } template -std::weak_ptr +std::weak_ptr> PinocchioModelImpl::GetJoint(size_t joint_index) const { throw NotImplementedException(); } @@ -134,9 +150,9 @@ PinocchioModelImpl::GetJointDescription( } template -huron::Affine3 +huron::SE3 PinocchioModelImpl::GetJointTransformInWorld(size_t joint_index) const { - return helpers::Se3ToAffine3(impl_->data_.oMi[joint_index]); + return helpers::PinSe3ToHuronSe3(impl_->data_.oMi[joint_index]); } template @@ -173,7 +189,7 @@ FrameType PinocchioModelImpl::GetFrameType(FrameIndex frame_index) const { } template -huron::Affine3 +huron::SE3 PinocchioModelImpl::GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const { return GetFrameTransformInWorld(from_frame).inverse() * @@ -181,13 +197,13 @@ PinocchioModelImpl::GetFrameTransform(FrameIndex from_frame, } template -huron::Affine3 +huron::SE3 PinocchioModelImpl::GetFrameTransformInWorld(FrameIndex frame) const { pinocchio::updateFramePlacement( impl_->model_, impl_->data_, static_cast(frame)); - return helpers::Se3ToAffine3(impl_->data_.oMf[frame]); + return helpers::PinSe3ToHuronSe3(impl_->data_.oMf[frame]); } template @@ -301,3 +317,5 @@ JointType PinocchioModelImpl::GetJointType(size_t joint_index) const { HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::multibody::internal::PinocchioModelImpl) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::multibody::internal::PinocchioModelImpl) diff --git a/system/control_interfaces/include/huron/control_interfaces/constant_state_provider.h b/system/control_interfaces/include/huron/control_interfaces/constant_state_provider.h index 3c340e3f..976c1a78 100644 --- a/system/control_interfaces/include/huron/control_interfaces/constant_state_provider.h +++ b/system/control_interfaces/include/huron/control_interfaces/constant_state_provider.h @@ -4,27 +4,32 @@ namespace huron { -class ConstantStateProvider : public StateProvider { +template +class ConstantStateProvider : public StateProvider { public: - explicit ConstantStateProvider(const Eigen::MatrixXd& state) - : StateProvider(state.rows(), state.cols()), + explicit ConstantStateProvider(const huron::MatrixX& state) + : StateProvider(state.rows(), state.cols()), state_(state) {} ConstantStateProvider(const ConstantStateProvider&) = delete; ConstantStateProvider& operator=(const ConstantStateProvider&) = delete; ~ConstantStateProvider() override = default; void RequestStateUpdate() override {} - void GetNewState(Eigen::Ref new_state) const override { + void GetNewState(Eigen::Ref> new_state) const override { new_state = state_; } - void SetState(const Eigen::MatrixXd& state) { + void SetState(const huron::MatrixX& state) { assert(state.rows() == dim()[0] && state.cols() == dim()[1]); state_ = state; } private: - Eigen::MatrixXd state_; + huron::MatrixX state_; }; } // namespace huron +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::ConstantStateProvider) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ConstantStateProvider) diff --git a/system/control_interfaces/include/huron/control_interfaces/encoder.h b/system/control_interfaces/include/huron/control_interfaces/encoder.h index 69226c59..bc0f7c63 100644 --- a/system/control_interfaces/include/huron/control_interfaces/encoder.h +++ b/system/control_interfaces/include/huron/control_interfaces/encoder.h @@ -35,30 +35,33 @@ class EncoderConfiguration : public Configuration { * * @ingroup control_interface */ -class Encoder : public Sensor { +template +class Encoder : public Sensor { public: - Encoder(double gear_ratio, std::unique_ptr config) - : Sensor(2, 1, std::move(config)), gear_ratio_(gear_ratio) {} - explicit Encoder(double gear_ratio) - : Encoder(gear_ratio, std::make_unique()) {} - explicit Encoder(std::unique_ptr config) - : Encoder(1.0, std::move(config)) {} - Encoder() : Encoder(1.0) {} + Encoder(T gear_ratio, std::unique_ptr config) + : Sensor(2, 1, std::move(config)), gear_ratio_(gear_ratio) {} + explicit Encoder(T gear_ratio) + : Encoder(gear_ratio, std::make_unique()) {} Encoder(const Encoder&) = delete; Encoder& operator=(const Encoder&) = delete; virtual ~Encoder() = default; - void GetNewState(Eigen::Ref new_state) const override { - new_state = Eigen::Vector2d(GetPosition(), GetVelocity()); + void GetNewState(Eigen::Ref> new_state) const override { + new_state = huron::Vector2(GetPosition(), GetVelocity()); } - virtual double GetPosition() const = 0; - virtual double GetVelocity() const = 0; + virtual T GetPosition() const = 0; + virtual T GetVelocity() const = 0; virtual void Reset() = 0; protected: - double gear_ratio_; + T gear_ratio_; }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::Encoder) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::Encoder) diff --git a/system/control_interfaces/include/huron/control_interfaces/joint.h b/system/control_interfaces/include/huron/control_interfaces/joint.h index 00737945..1f567014 100644 --- a/system/control_interfaces/include/huron/control_interfaces/joint.h +++ b/system/control_interfaces/include/huron/control_interfaces/joint.h @@ -11,6 +11,7 @@ namespace huron { +template class Joint { using JointDescription = huron::multibody::JointDescription; using JointType = huron::multibody::JointType; @@ -20,7 +21,7 @@ class Joint { * Creates a Joint that connects the specfied parent and child frames. */ explicit Joint(std::unique_ptr joint_desc, - std::shared_ptr state_provider = nullptr); + std::shared_ptr> state_provider = nullptr); Joint(const Joint&) = delete; Joint& operator=(const Joint&) = delete; virtual ~Joint() = default; @@ -36,7 +37,7 @@ class Joint { * and size: * \[ \begin{bmatrix} q \\ \dot{q} \end{bmatrix} \in \mathbb{R}^{nq + nv}\] */ - void SetStateProvider(std::shared_ptr state_provider); + void SetStateProvider(std::shared_ptr> state_provider); /** * @brief Updates the joint state from the state provider. @@ -47,11 +48,11 @@ class Joint { return jd_->type(); } - const Eigen::VectorXd& GetPositions() const { + const huron::VectorX& GetPositions() const { return positions_; } - const Eigen::VectorXd& GetVelocities() const { + const huron::VectorX& GetVelocities() const { return velocities_; } @@ -89,12 +90,17 @@ class Joint { protected: std::unique_ptr jd_; - Eigen::VectorXd positions_; - Eigen::VectorXd velocities_; + huron::VectorX positions_; + huron::VectorX velocities_; size_t id_q_; size_t id_v_; - std::shared_ptr state_provider_; + std::shared_ptr> state_provider_; }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::Joint) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::Joint) diff --git a/system/control_interfaces/include/huron/control_interfaces/legged_robot.h b/system/control_interfaces/include/huron/control_interfaces/legged_robot.h index 8f014e5f..716211da 100644 --- a/system/control_interfaces/include/huron/control_interfaces/legged_robot.h +++ b/system/control_interfaces/include/huron/control_interfaces/legged_robot.h @@ -22,7 +22,7 @@ class LeggedRobot : public Robot { /** * Computes the Center of Mass in Base frame. */ - Eigen::Vector2d EvalZeroMomentPoint(); + huron::Vector2 EvalZeroMomentPoint(); private: std::shared_ptr> zmp_; @@ -32,3 +32,5 @@ class LeggedRobot : public Robot { HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::LeggedRobot) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::LeggedRobot) diff --git a/system/control_interfaces/include/huron/control_interfaces/robot.h b/system/control_interfaces/include/huron/control_interfaces/robot.h index 4db2ad1a..ea17bc32 100644 --- a/system/control_interfaces/include/huron/control_interfaces/robot.h +++ b/system/control_interfaces/include/huron/control_interfaces/robot.h @@ -49,7 +49,7 @@ class Robot : public MovingGroup, public GenericComponent { Model* const GetModel() { return model_.get(); } - void RegisterStateProvider(std::shared_ptr state_provider, + void RegisterStateProvider(std::shared_ptr> state_provider, bool is_joint_state_provider = false); /** @@ -63,9 +63,9 @@ class Robot : public MovingGroup, public GenericComponent { */ void UpdateJointStates(); - const Eigen::VectorBlock GetJointPositions() const; + const Eigen::VectorBlock> GetJointPositions() const; - const Eigen::VectorBlock GetJointVelocities() const; + const Eigen::VectorBlock> GetJointVelocities() const; protected: Robot(std::unique_ptr config, @@ -73,11 +73,13 @@ class Robot : public MovingGroup, public GenericComponent { explicit Robot(std::shared_ptr model); std::shared_ptr model_; - std::vector> non_joint_state_providers_; - std::vector> joint_state_providers_; + std::vector>> non_joint_state_providers_; + std::vector>> joint_state_providers_; }; } // namespace huron HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::Robot) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::Robot) diff --git a/system/control_interfaces/include/huron/control_interfaces/rotary_encoder.h b/system/control_interfaces/include/huron/control_interfaces/rotary_encoder.h index e1ea94df..dbd22dec 100644 --- a/system/control_interfaces/include/huron/control_interfaces/rotary_encoder.h +++ b/system/control_interfaces/include/huron/control_interfaces/rotary_encoder.h @@ -10,6 +10,7 @@ namespace huron { +template class RotaryEncoderConfiguration : public EncoderConfiguration { public: /** @@ -24,7 +25,7 @@ class RotaryEncoderConfiguration : public EncoderConfiguration { return tmp; }()) {} - explicit RotaryEncoderConfiguration(double cpr) + explicit RotaryEncoderConfiguration(T cpr) : RotaryEncoderConfiguration( ConfigMap({{"cpr", cpr}}), {}) {} @@ -37,18 +38,17 @@ class RotaryEncoderConfiguration : public EncoderConfiguration { * * @ingroup control_interfaces */ -class RotaryEncoder : public Encoder { +template +class RotaryEncoder : public Encoder { public: - RotaryEncoder(double gear_ratio, - std::unique_ptr config) - : Encoder(gear_ratio, std::move(config)) { - cpr_ = std::any_cast(config_.get()->Get("cpr")); + RotaryEncoder(T gear_ratio, + std::unique_ptr> config) + : Encoder(gear_ratio, std::move(config)) { + cpr_ = std::any_cast(this->config_.get()->Get("cpr")); } - RotaryEncoder(double gear_ratio, double cpr) + RotaryEncoder(T gear_ratio, T cpr) : RotaryEncoder(gear_ratio, - std::make_unique(cpr)) {} - explicit RotaryEncoder(double cpr) - : RotaryEncoder(1.0, cpr) {} + std::make_unique>(cpr)) {} RotaryEncoder(const RotaryEncoder&) = delete; RotaryEncoder& operator=(const RotaryEncoder&) = delete; ~RotaryEncoder() override = default; @@ -62,28 +62,28 @@ class RotaryEncoder : public Encoder { /** * Gets the current encoder count. */ - double GetCount() const { + T GetCount() const { return count_; } /** * Gets the current encoder velocity in count. */ - double GetVelocityCount() const { + T GetVelocityCount() const { return velocity_; } /** * Gets the previous encoder count. */ - double GetPrevCount() const { + T GetPrevCount() const { return prev_count_; } /** * Gets the counts per revolution (CPR). */ - double GetCPR() const { + T GetCPR() const { return cpr_; } @@ -91,32 +91,32 @@ class RotaryEncoder : public Encoder { * Gets the current angle in radians. This takes into account the gear ratio * and CPR. */ - double GetPosition() const override { - return count_ / cpr_ * 2.0 * M_PI / gear_ratio_; + T GetPosition() const override { + return count_ / cpr_ * 2.0 * M_PI / this->gear_ratio_; } /** * Gets the current angle in degrees. This takes into account the gear ratio * and CPR. */ - double GetAngleDegree() const { - return count_ / cpr_ * 360.0 / gear_ratio_; + T GetAngleDegree() const { + return count_ / cpr_ * 360.0 / this->gear_ratio_; } /** * Gets the current velocity in radians/second. This takes into account the * gear ratio and CPR. */ - double GetVelocity() const override { - return velocity_ / cpr_ * 2 * M_PI / gear_ratio_; + T GetVelocity() const override { + return velocity_ / cpr_ * 2 * M_PI / this->gear_ratio_; } /** * Gets the current velocity in degrees/second. This takes into account the * gear ratio and CPR. */ - double GetVelocityDegree() const { - return velocity_ / cpr_ * 360.0 / gear_ratio_; + T GetVelocityDegree() const { + return velocity_ / cpr_ * 360.0 / this->gear_ratio_; } /** @@ -139,12 +139,21 @@ class RotaryEncoder : public Encoder { virtual void DoUpdateState() = 0; /// \brief Encoder velocity in counts per second. - double velocity_ = 0.0; + T velocity_; /// \brief Encoder previous velocity in counts per second. - double prev_velocity_ = 0.0; - double count_ = 0.0; - double prev_count_ = 0.0; - double cpr_; + T prev_velocity_; + T count_; + T prev_count_; + T cpr_; }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::RotaryEncoderConfiguration) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::RotaryEncoderConfiguration) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::RotaryEncoder) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::RotaryEncoder) diff --git a/system/control_interfaces/include/huron/control_interfaces/sensor.h b/system/control_interfaces/include/huron/control_interfaces/sensor.h index e369f0b0..36e40120 100644 --- a/system/control_interfaces/include/huron/control_interfaces/sensor.h +++ b/system/control_interfaces/include/huron/control_interfaces/sensor.h @@ -9,7 +9,8 @@ namespace huron { -class Sensor : public GenericComponent, public StateProvider { +template +class Sensor : public GenericComponent, public StateProvider { public: Sensor(const Eigen::Vector2i& dim, std::unique_ptr config); @@ -24,8 +25,13 @@ class Sensor : public GenericComponent, public StateProvider { /** * @brief Get the sensor value. */ - virtual Eigen::VectorXd GetValue() const; - virtual Eigen::VectorXd ReloadAndGetValue(); + virtual huron::VectorX GetValue() const; + virtual huron::VectorX ReloadAndGetValue(); }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::Sensor) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::Sensor) diff --git a/system/control_interfaces/include/huron/control_interfaces/sensor_with_frame.h b/system/control_interfaces/include/huron/control_interfaces/sensor_with_frame.h index 39e2eebd..0c1047dd 100644 --- a/system/control_interfaces/include/huron/control_interfaces/sensor_with_frame.h +++ b/system/control_interfaces/include/huron/control_interfaces/sensor_with_frame.h @@ -11,7 +11,7 @@ namespace huron { template -class SensorWithFrame : public Sensor { +class SensorWithFrame : public Sensor { using Frame = multibody::Frame; public: @@ -44,3 +44,5 @@ class SensorWithFrame : public Sensor { HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::SensorWithFrame) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::SensorWithFrame) diff --git a/system/control_interfaces/include/huron/control_interfaces/state_provider.h b/system/control_interfaces/include/huron/control_interfaces/state_provider.h index 63930f49..037de4ec 100644 --- a/system/control_interfaces/include/huron/control_interfaces/state_provider.h +++ b/system/control_interfaces/include/huron/control_interfaces/state_provider.h @@ -1,9 +1,11 @@ #pragma once -#include +#include "huron/types.h" +#include "huron/utils/template_instantiations.h" namespace huron { +template class StateProvider { public: explicit StateProvider(const Eigen::Vector2i& dim) @@ -15,7 +17,7 @@ class StateProvider { virtual ~StateProvider() = default; virtual void RequestStateUpdate() = 0; - virtual void GetNewState(Eigen::Ref new_state) const = 0; + virtual void GetNewState(Eigen::Ref> new_state) const = 0; const Eigen::Vector2i& dim() const { return dim_; } @@ -24,3 +26,8 @@ class StateProvider { }; } // namespace huron + +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::StateProvider) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::StateProvider) diff --git a/system/control_interfaces/src/joint.cc b/system/control_interfaces/src/joint.cc index 08f08f93..57be3db6 100644 --- a/system/control_interfaces/src/joint.cc +++ b/system/control_interfaces/src/joint.cc @@ -2,14 +2,16 @@ namespace huron { -Joint::Joint(std::unique_ptr joint_desc, - std::shared_ptr state_provider) +template +Joint::Joint(std::unique_ptr joint_desc, + std::shared_ptr> state_provider) : jd_(std::move(joint_desc)), - positions_(Eigen::VectorXd::Zero(jd_->num_positions())), - velocities_(Eigen::VectorXd::Zero(jd_->num_velocities())), + positions_(huron::VectorX::Zero(jd_->num_positions())), + velocities_(huron::VectorX::Zero(jd_->num_velocities())), state_provider_(std::move(state_provider)) {} -void Joint::SetStateProvider(std::shared_ptr state_provider) { +template +void Joint::SetStateProvider(std::shared_ptr> state_provider) { assert( state_provider->dim()[0] == (jd_->num_positions() + jd_->num_velocities()) && @@ -17,13 +19,19 @@ void Joint::SetStateProvider(std::shared_ptr state_provider) { state_provider_ = state_provider; } -void Joint::UpdateState() { +template +void Joint::UpdateState() { assert(state_provider_ != nullptr); state_provider_->RequestStateUpdate(); - Eigen::VectorXd tmp(jd_->num_positions() + jd_->num_velocities()); + huron::VectorX tmp(jd_->num_positions() + jd_->num_velocities()); state_provider_->GetNewState(tmp); positions_ = tmp.segment(0, jd_->num_positions()); velocities_ = tmp.segment(jd_->num_positions(), jd_->num_velocities()); } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::Joint) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::Joint) diff --git a/system/control_interfaces/src/legged_robot.cc b/system/control_interfaces/src/legged_robot.cc index 58546d3d..6edeb3ec 100644 --- a/system/control_interfaces/src/legged_robot.cc +++ b/system/control_interfaces/src/legged_robot.cc @@ -16,7 +16,7 @@ void LeggedRobot::InitializeZmp(std::shared_ptr> zmp) { } template -Eigen::Vector2d LeggedRobot::EvalZeroMomentPoint() { +huron::Vector2 LeggedRobot::EvalZeroMomentPoint() { return zmp_->Eval(); } @@ -24,3 +24,5 @@ Eigen::Vector2d LeggedRobot::EvalZeroMomentPoint() { HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::LeggedRobot) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::LeggedRobot) diff --git a/system/control_interfaces/src/robot.cc b/system/control_interfaces/src/robot.cc index 86e8f35d..b44536cd 100644 --- a/system/control_interfaces/src/robot.cc +++ b/system/control_interfaces/src/robot.cc @@ -23,7 +23,7 @@ Robot::Robot(std::shared_ptr model) template void Robot::RegisterStateProvider( - std::shared_ptr state_provider, + std::shared_ptr> state_provider, bool is_joint_state_provider) { if (!is_joint_state_provider) { non_joint_state_providers_.push_back(state_provider); @@ -46,13 +46,13 @@ void Robot::UpdateJointStates() { } template -const Eigen::VectorBlock +const Eigen::VectorBlock> Robot::GetJointPositions() const { return model_->GetPositions(); } template -const Eigen::VectorBlock +const Eigen::VectorBlock> Robot::GetJointVelocities() const { return model_->GetVelocities(); } @@ -61,3 +61,5 @@ Robot::GetJointVelocities() const { HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::Robot) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::Robot) diff --git a/system/control_interfaces/src/sensor.cc b/system/control_interfaces/src/sensor.cc index 2b1f5fae..962a0037 100644 --- a/system/control_interfaces/src/sensor.cc +++ b/system/control_interfaces/src/sensor.cc @@ -3,33 +3,44 @@ namespace huron { -Sensor::Sensor(const Eigen::Vector2i& dim, +template +Sensor::Sensor(const Eigen::Vector2i& dim, std::unique_ptr config) : GenericComponent(std::move(config)), - StateProvider(dim) {} + StateProvider(dim) {} -Sensor::Sensor(const Eigen::Vector2i& dim) +template +Sensor::Sensor(const Eigen::Vector2i& dim) : GenericComponent(), - StateProvider(dim) {} + StateProvider(dim) {} -Sensor::Sensor(int rows, int cols, - std::unique_ptr config) +template +Sensor::Sensor(int rows, int cols, + std::unique_ptr config) : GenericComponent(std::move(config)), - StateProvider(rows, cols) {} + StateProvider(rows, cols) {} -Sensor::Sensor(int rows, int cols) +template +Sensor::Sensor(int rows, int cols) : GenericComponent(), - StateProvider(rows, cols) {} + StateProvider(rows, cols) {} -Eigen::VectorXd Sensor::GetValue() const { - Eigen::VectorXd tmp; - GetNewState(tmp); +template +huron::VectorX Sensor::GetValue() const { + huron::VectorX tmp; + this->GetNewState(tmp); return tmp; } -Eigen::VectorXd Sensor::ReloadAndGetValue() { - RequestStateUpdate(); +template +huron::VectorX Sensor::ReloadAndGetValue() { + this->RequestStateUpdate(); return GetValue(); } } // namespace huron + +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class huron::Sensor) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::Sensor) diff --git a/system/control_interfaces/src/sensor_with_frame.cc b/system/control_interfaces/src/sensor_with_frame.cc index f0d9c179..cb7376b2 100644 --- a/system/control_interfaces/src/sensor_with_frame.cc +++ b/system/control_interfaces/src/sensor_with_frame.cc @@ -5,28 +5,30 @@ namespace huron { template SensorWithFrame::SensorWithFrame(const Eigen::Vector2i& dim, std::weak_ptr frame) - : Sensor(dim), + : Sensor(dim), frame_(std::move(frame)) {} template SensorWithFrame::SensorWithFrame(const Eigen::Vector2i& dim, std::weak_ptr frame, std::unique_ptr config) - : Sensor(dim, std::move(config)), frame_(std::move(frame)) {} + : Sensor(dim, std::move(config)), frame_(std::move(frame)) {} template SensorWithFrame::SensorWithFrame(int rows, int cols, std::weak_ptr frame) - : Sensor(rows, cols), + : Sensor(rows, cols), frame_(std::move(frame)) {} template SensorWithFrame::SensorWithFrame(int rows, int cols, std::weak_ptr frame, std::unique_ptr config) - : Sensor(rows, cols, std::move(config)), frame_(std::move(frame)) {} + : Sensor(rows, cols, std::move(config)), frame_(std::move(frame)) {} } // namespace huron HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::SensorWithFrame) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::SensorWithFrame) diff --git a/system/locomotion/include/huron/locomotion/zero_moment_point.h b/system/locomotion/include/huron/locomotion/zero_moment_point.h index 0cf20d49..f808e510 100644 --- a/system/locomotion/include/huron/locomotion/zero_moment_point.h +++ b/system/locomotion/include/huron/locomotion/zero_moment_point.h @@ -1,11 +1,12 @@ #pragma once -#include +#include #include #include "huron/sensors/force_torque_sensor.h" #include "huron/sensors/force_sensing_resistor_array.h" +#include "huron/math/se3.h" #include "huron/multibody/logical_frame.h" namespace huron { @@ -14,7 +15,7 @@ template class ZeroMomentPoint { public: ZeroMomentPoint(std::weak_ptr> zmp_frame, - double normal_force_threshold); + T normal_force_threshold); ZeroMomentPoint(const ZeroMomentPoint&) = delete; ZeroMomentPoint& operator=(const ZeroMomentPoint&) = delete; virtual ~ZeroMomentPoint() = default; @@ -26,9 +27,9 @@ class ZeroMomentPoint { * @param zmp The zero moment point in the ZMP frame. * @param fz The normal force. */ - virtual Eigen::Vector2d Eval(double& fz) = 0; - Eigen::Vector2d Eval() { - double fz; + virtual huron::Vector2 Eval(T& fz) = 0; + huron::Vector2 Eval() { + T fz; return Eval(fz); } @@ -38,14 +39,16 @@ class ZeroMomentPoint { * @param zmp The zero moment point in the ZMP frame. * @return The zero moment point in the world frame. */ - Eigen::Affine3d ZmpToWorld(const Eigen::Vector2d& zmp) const; + huron::SE3 ZmpToWorld(const huron::Vector2& zmp) const; protected: std::weak_ptr> zmp_frame_; - double normal_force_threshold_; + T normal_force_threshold_; }; } // namespace huron HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ZeroMomentPoint) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ZeroMomentPoint) diff --git a/system/locomotion/include/huron/locomotion/zero_moment_point_fsr_array.h b/system/locomotion/include/huron/locomotion/zero_moment_point_fsr_array.h index 84103ca9..79e00522 100644 --- a/system/locomotion/include/huron/locomotion/zero_moment_point_fsr_array.h +++ b/system/locomotion/include/huron/locomotion/zero_moment_point_fsr_array.h @@ -3,6 +3,7 @@ #include #include "huron/locomotion/zero_moment_point.h" +#include "huron/types.h" namespace huron { @@ -11,13 +12,13 @@ class ZeroMomentPointFSRArray : public ZeroMomentPoint { public: ZeroMomentPointFSRArray( std::weak_ptr> zmp_frame, - double normal_force_threshold, + T normal_force_threshold, std::shared_ptr> fsr_array); ZeroMomentPointFSRArray(const ZeroMomentPointFSRArray&) = delete; ZeroMomentPointFSRArray& operator=(const ZeroMomentPointFSRArray&) = delete; ~ZeroMomentPointFSRArray() override = default; - Eigen::Vector2d Eval(double& fz) override; + huron::Vector2 Eval(T& fz) override; private: std::shared_ptr> fsr_array_; @@ -25,5 +26,15 @@ class ZeroMomentPointFSRArray : public ZeroMomentPoint { } // namespace huron +// Template specializations +#ifdef HURON_USE_CASADI + +template <> huron::Vector2 +huron::ZeroMomentPointFSRArray::Eval(casadi::SX& fz); + +#endif // HURON_USE_CASADI + HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class huron::ForceSensingResistorArray) + class huron::ZeroMomentPointFSRArray) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ZeroMomentPointFSRArray) diff --git a/system/locomotion/include/huron/locomotion/zero_moment_point_ft_sensor.h b/system/locomotion/include/huron/locomotion/zero_moment_point_ft_sensor.h index 807af03e..d88d12a0 100644 --- a/system/locomotion/include/huron/locomotion/zero_moment_point_ft_sensor.h +++ b/system/locomotion/include/huron/locomotion/zero_moment_point_ft_sensor.h @@ -12,13 +12,13 @@ class ZeroMomentPointFTSensor : public ZeroMomentPoint { public: ZeroMomentPointFTSensor( std::weak_ptr> zmp_frame, - double normal_force_threshold, + T normal_force_threshold, const std::vector>>& ft_sensors); ZeroMomentPointFTSensor(const ZeroMomentPointFTSensor&) = delete; ZeroMomentPointFTSensor& operator=(const ZeroMomentPointFTSensor&) = delete; ~ZeroMomentPointFTSensor() override = default; - Eigen::Vector2d Eval(double& fz) override; + huron::Vector2 Eval(T& fz) override; private: std::vector>> ft_sensors_; @@ -26,5 +26,15 @@ class ZeroMomentPointFTSensor : public ZeroMomentPoint { } // namespace huron +// Template specializations +#ifdef HURON_USE_CASADI + +template <> huron::Vector2 +huron::ZeroMomentPointFTSensor::Eval(casadi::SX& fz); + +#endif // HURON_USE_CASADI + HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ZeroMomentPointFTSensor) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ZeroMomentPointFTSensor) diff --git a/system/locomotion/include/huron/locomotion/zero_moment_point_total.h b/system/locomotion/include/huron/locomotion/zero_moment_point_total.h index c5bc2edd..6a277c4b 100644 --- a/system/locomotion/include/huron/locomotion/zero_moment_point_total.h +++ b/system/locomotion/include/huron/locomotion/zero_moment_point_total.h @@ -17,7 +17,7 @@ class ZeroMomentPointTotal : public ZeroMomentPoint { ZeroMomentPointTotal& operator=(const ZeroMomentPointTotal&) = delete; ~ZeroMomentPointTotal() override = default; - Eigen::Vector2d Eval(double& fz) override; + huron::Vector2 Eval(T& fz) override; private: std::vector>> zmp_vector_; @@ -25,5 +25,15 @@ class ZeroMomentPointTotal : public ZeroMomentPoint { } // namespace huron +// Template specializations +#ifdef HURON_USE_CASADI + +template <> huron::Vector2 +huron::ZeroMomentPointTotal::Eval(casadi::SX& fz); + +#endif // HURON_USE_CASADI + HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ZeroMomentPointTotal) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ZeroMomentPointTotal) diff --git a/system/locomotion/src/zero_moment_point.cc b/system/locomotion/src/zero_moment_point.cc index cc2ab6a8..dd9ce011 100644 --- a/system/locomotion/src/zero_moment_point.cc +++ b/system/locomotion/src/zero_moment_point.cc @@ -7,15 +7,15 @@ namespace huron { template ZeroMomentPoint::ZeroMomentPoint( std::weak_ptr> zmp_frame, - double normal_force_threshold) + T normal_force_threshold) : zmp_frame_(std::move(zmp_frame)), normal_force_threshold_(normal_force_threshold) { } template -Eigen::Affine3d ZeroMomentPoint::ZmpToWorld(const Eigen::Vector2d& zmp) const { - Eigen::Affine3d ret; - ret.translate(Eigen::Vector3d(zmp.x(), zmp.y(), 0.0)); +huron::SE3 ZeroMomentPoint::ZmpToWorld(const huron::Vector2& zmp) const { + huron::SE3 ret; + ret.translate(huron::Vector3(zmp.x(), zmp.y(), 0.0)); return ret; } @@ -23,3 +23,5 @@ Eigen::Affine3d ZeroMomentPoint::ZmpToWorld(const Eigen::Vector2d& zmp) const HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ZeroMomentPoint) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ZeroMomentPoint) diff --git a/system/locomotion/src/zero_moment_point_fsr_array.cc b/system/locomotion/src/zero_moment_point_fsr_array.cc index 3db4d8a0..d49ab039 100644 --- a/system/locomotion/src/zero_moment_point_fsr_array.cc +++ b/system/locomotion/src/zero_moment_point_fsr_array.cc @@ -1,26 +1,27 @@ #include "huron/locomotion/zero_moment_point_fsr_array.h" +#include "huron/math/abs.h" namespace huron { template ZeroMomentPointFSRArray::ZeroMomentPointFSRArray( std::weak_ptr> zmp_frame, - double normal_force_threshold, + T normal_force_threshold, std::shared_ptr> fsr_array) : ZeroMomentPoint(std::move(zmp_frame), normal_force_threshold), fsr_array_(std::move(fsr_array)) { } template -Eigen::Vector2d ZeroMomentPointFSRArray::Eval(double& fz) { - Eigen::Vector2d zmp; - Eigen::VectorXd fz_array = fsr_array_->GetValue(); - double sum_fz = fz_array.colwise().sum().value(); +huron::Vector2 ZeroMomentPointFSRArray::Eval(T& fz) { + huron::Vector2 zmp; + huron::VectorX fz_array = fsr_array_->GetValue(); + T sum_fz = fz_array.colwise().sum().value(); fz = sum_fz; - if (std::abs(sum_fz) < this->normal_force_threshold_) { + if (huron::abs(sum_fz) < this->normal_force_threshold_) { zmp.setZero(); } else { - double num_x = 0.0, num_y = 0.0; + T num_x(0.0), num_y(0.0); for (size_t i = 0; i < fsr_array_->num_sensors(); ++i) { num_x += fz_array(i) * fsr_array_->GetSensorPose(i).translation().x(); num_y += fz_array(i) * fsr_array_->GetSensorPose(i).translation().y(); @@ -30,8 +31,41 @@ Eigen::Vector2d ZeroMomentPointFSRArray::Eval(double& fz) { } return zmp; } +#if HURON_USE_CASADI==1 +template <> huron::Vector2 +ZeroMomentPointFSRArray::Eval(casadi::SX& fz) { + huron::VectorX fz_array = fsr_array_->GetValue(); + casadi::SX sum_fz = fz_array.colwise().sum().value(); + fz = sum_fz; + auto f1 = casadi::Function( + "f1", + {sum_fz}, + {0.0, 0.0}); + auto f2 = casadi::Function( + "f2", + {sum_fz}, + ([&]() { + std::vector zmp(0.0, 0.0); + for (size_t i = 0; i < fsr_array_->num_sensors(); ++i) { + zmp[0] += fz_array(i) * fsr_array_->GetSensorPose(i).translation().x(); + zmp[1] += fz_array(i) * fsr_array_->GetSensorPose(i).translation().y(); + } + zmp[0] /= sum_fz; + zmp[1] /= sum_fz; + return zmp; + })()); + std::vector tmp_res = casadi::Function::if_else( + "zmp_fsr_array_eval", f1, f2)( + std::vector{ + casadi::SX::abs(sum_fz) - this->normal_force_threshold_, + sum_fz}); + return huron::Vector2(tmp_res[0], tmp_res[1]); +} +#endif // HURON_USE_CASADI } // namespace huron HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ZeroMomentPointFSRArray) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ZeroMomentPointFSRArray) diff --git a/system/locomotion/src/zero_moment_point_ft_sensor.cc b/system/locomotion/src/zero_moment_point_ft_sensor.cc index c12fa705..0204c001 100644 --- a/system/locomotion/src/zero_moment_point_ft_sensor.cc +++ b/system/locomotion/src/zero_moment_point_ft_sensor.cc @@ -1,11 +1,12 @@ #include "huron/locomotion/zero_moment_point_ft_sensor.h" +#include "huron/math/abs.h" namespace huron { template ZeroMomentPointFTSensor::ZeroMomentPointFTSensor( std::weak_ptr> zmp_frame, - double normal_force_threshold, + T normal_force_threshold, const std::vector>>& ft_sensors) : ZeroMomentPoint(std::move(zmp_frame), normal_force_threshold), ft_sensors_(ft_sensors) { @@ -13,14 +14,14 @@ ZeroMomentPointFTSensor::ZeroMomentPointFTSensor( // Generate Doxygen documentation for the following function template -Eigen::Vector2d ZeroMomentPointFTSensor::Eval(double& fz) { - Eigen::Vector2d zmp; - double num_x = 0.0, num_y = 0.0, den = 0.0; +huron::Vector2 ZeroMomentPointFTSensor::Eval(T& fz) { + huron::Vector2 zmp; + T num_x(0.0), num_y(0.0), den(0.0); for (auto& ft_sensor : ft_sensors_) { - Eigen::Affine3d zmp_to_sensor = this->zmp_frame_.lock()->GetTransformToFrame( + huron::SE3 zmp_to_sensor = this->zmp_frame_.lock()->GetTransformToFrame( *ft_sensor->GetSensorFrame().lock()); - Eigen::Affine3d zmp_frame_pose = this->zmp_frame_.lock()->GetTransformInWorld(); - Vector6d w = ft_sensor->GetValue(); + huron::SE3 zmp_frame_pose = this->zmp_frame_.lock()->GetTransformInWorld(); + huron::Vector6 w = ft_sensor->GetValue(); w.segment(0, 3) = zmp_to_sensor.rotation() * w.segment(0, 3); w.segment(3, 3) = zmp_to_sensor.rotation() * w.segment(3, 3); num_x += @@ -33,7 +34,7 @@ Eigen::Vector2d ZeroMomentPointFTSensor::Eval(double& fz) { + zmp_to_sensor.translation().y()*w(2)); den += w(2); } - if (std::abs(den) < this->normal_force_threshold_) { + if (huron::abs(den) < this->normal_force_threshold_) { zmp.setZero(); } else { zmp.x() = num_x / den; @@ -43,7 +44,53 @@ Eigen::Vector2d ZeroMomentPointFTSensor::Eval(double& fz) { return zmp; } +#if HURON_USE_CASADI==1 +template <> huron::Vector2 +ZeroMomentPointFTSensor::Eval(casadi::SX& fz) { + huron::Vector2 zmp; + casadi::SX num_x(0.0), num_y(0.0), den(0.0); + for (auto& ft_sensor : ft_sensors_) { + huron::SE3 zmp_to_sensor = this->zmp_frame_.lock()->GetTransformToFrame( + *ft_sensor->GetSensorFrame().lock()); + huron::SE3 zmp_frame_pose = this->zmp_frame_.lock()->GetTransformInWorld(); + huron::Vector6 w = ft_sensor->GetValue(); + w.segment(0, 3) = zmp_to_sensor.rotation() * w.segment(0, 3); + w.segment(3, 3) = zmp_to_sensor.rotation() * w.segment(3, 3); + num_x += + (-w(4) - + w(0)*(zmp_to_sensor.translation().z() - zmp_frame_pose.translation().z()) + + zmp_to_sensor.translation().x()*w(2)); + num_y += + (w(3) - + w(1)*(zmp_to_sensor.translation().z() - zmp_frame_pose.translation().z()) + + zmp_to_sensor.translation().y()*w(2)); + den += w(2); + } + fz = den; + auto f1 = casadi::Function( + "f1", + {den}, + {0.0, 0.0}); + auto f2 = casadi::Function( + "f2", + {den}, + ([&]() { + return std::vector{ + num_x / den, + num_y / den}; + })()); + std::vector tmp_res = casadi::Function::if_else( + "zmp_fsr_array_eval", f1, f2)( + std::vector{ + casadi::SX::abs(den) - this->normal_force_threshold_, + den}); + return huron::Vector2(tmp_res[0], tmp_res[1]); +} +#endif + } // namespace huron HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ZeroMomentPointFTSensor) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ZeroMomentPointFTSensor) diff --git a/system/locomotion/src/zero_moment_point_total.cc b/system/locomotion/src/zero_moment_point_total.cc index 808329af..77e6b890 100644 --- a/system/locomotion/src/zero_moment_point_total.cc +++ b/system/locomotion/src/zero_moment_point_total.cc @@ -10,18 +10,18 @@ ZeroMomentPointTotal::ZeroMomentPointTotal( zmp_vector_(zmp_vector) {} template -Eigen::Vector2d ZeroMomentPointTotal::Eval(double& fz) { - Eigen::Vector2d zmp; - double num_x = 0.0, num_y = 0.0, den = 0.0; +huron::Vector2 ZeroMomentPointTotal::Eval(T& fz) { + huron::Vector2 zmp; + T num_x(0.0), num_y(0.0), den(0.0); for (auto& zmp_obj : zmp_vector_) { - double fz_i; - Eigen::Vector2d zmp_i = zmp_obj->Eval(fz_i); + T fz_i; + huron::Vector2 zmp_i = zmp_obj->Eval(fz_i); num_x += zmp_i.x() * fz_i; num_y += zmp_i.y() * fz_i; den += fz_i; } fz = den; - if (den == 0.0) { + if (den == T(0.0)) { zmp.setZero(); } else { zmp.x() = num_x / den; @@ -30,7 +30,42 @@ Eigen::Vector2d ZeroMomentPointTotal::Eval(double& fz) { return zmp; } +#if HURON_USE_CASADI==1 +template <> huron::Vector2 +ZeroMomentPointTotal::Eval(casadi::SX& fz) { + casadi::SX num_x(0.0), num_y(0.0), den(0.0); + for (auto& zmp_obj : zmp_vector_) { + casadi::SX fz_i; + huron::Vector2 zmp_i = zmp_obj->Eval(fz_i); + num_x += zmp_i.x() * fz_i; + num_y += zmp_i.y() * fz_i; + den += fz_i; + } + fz = den; + auto f1 = casadi::Function( + "f1", + {den}, + {0.0, 0.0}); + auto f2 = casadi::Function( + "f2", + {den}, + ([&]() { + return std::vector{ + num_x / den, + num_y / den}; + })()); + std::vector tmp_res = casadi::Function::if_else( + "zmp_fsr_array_eval", f1, f2)( + std::vector{ + casadi::SX::abs(den) - this->normal_force_threshold_, + den}); + return huron::Vector2(tmp_res[0], tmp_res[1]); +} +#endif // HURON_USE_CASADI + } // namespace huron HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ZeroMomentPointTotal) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ZeroMomentPointTotal) diff --git a/system/odrive/include/huron/odrive/odrive_rotary_encoder.h b/system/odrive/include/huron/odrive/odrive_rotary_encoder.h index 5c6ce762..a7c9204b 100644 --- a/system/odrive/include/huron/odrive/odrive_rotary_encoder.h +++ b/system/odrive/include/huron/odrive/odrive_rotary_encoder.h @@ -7,10 +7,10 @@ namespace huron { namespace odrive { -class ODriveEncoder final : public huron::RotaryEncoder { +class ODriveEncoder final : public huron::RotaryEncoder { public: ODriveEncoder(double gear_ratio, - std::unique_ptr config, + std::unique_ptr> config, std::shared_ptr odrive); ODriveEncoder(double gear_ratio, double cpr, std::shared_ptr odrive); ODriveEncoder(double cpr, std::shared_ptr odrive); diff --git a/system/odrive/src/odrive_rotary_encoder.cc b/system/odrive/src/odrive_rotary_encoder.cc index 4a31c5a6..1a8d1c89 100644 --- a/system/odrive/src/odrive_rotary_encoder.cc +++ b/system/odrive/src/odrive_rotary_encoder.cc @@ -4,16 +4,16 @@ namespace huron { namespace odrive { ODriveEncoder::ODriveEncoder(double gear_ratio, - std::unique_ptr config, + std::unique_ptr> config, std::shared_ptr odrive) - : huron::RotaryEncoder(gear_ratio, + : huron::RotaryEncoder(gear_ratio, std::move(config)), odrive_(std::move(odrive)) {} ODriveEncoder::ODriveEncoder(double gear_ratio, double cpr, std::shared_ptr odrive) - : huron::RotaryEncoder(gear_ratio, cpr), + : huron::RotaryEncoder(gear_ratio, cpr), odrive_(std::move(odrive)) {} ODriveEncoder::ODriveEncoder(double cpr, std::shared_ptr odrive) diff --git a/system/sensors/include/huron/sensors/force_sensing_resistor.h b/system/sensors/include/huron/sensors/force_sensing_resistor.h index ec3f4799..a747dce8 100644 --- a/system/sensors/include/huron/sensors/force_sensing_resistor.h +++ b/system/sensors/include/huron/sensors/force_sensing_resistor.h @@ -21,3 +21,5 @@ class ForceSensingResistor : public SensorWithFrame { HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ForceSensingResistor) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ForceSensingResistor) diff --git a/system/sensors/include/huron/sensors/force_sensing_resistor_array.h b/system/sensors/include/huron/sensors/force_sensing_resistor_array.h index fa590a88..8f09fa6c 100644 --- a/system/sensors/include/huron/sensors/force_sensing_resistor_array.h +++ b/system/sensors/include/huron/sensors/force_sensing_resistor_array.h @@ -31,15 +31,15 @@ class ForceSensingResistorArray : public SensorWithFrame { void RequestStateUpdate() override; - void GetNewState(Eigen::Ref new_state) const override; + void GetNewState(Eigen::Ref> new_state) const override; - Eigen::Affine3d GetSensorPose(size_t index) const; + huron::SE3 GetSensorPose(size_t index) const; size_t num_sensors() const { return fsr_array_.size(); } protected: std::string name_; - Eigen::VectorXd values_; + huron::VectorX values_; std::vector>> fsr_array_; }; @@ -47,3 +47,5 @@ class ForceSensingResistorArray : public SensorWithFrame { HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ForceSensingResistorArray) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ForceSensingResistorArray) diff --git a/system/sensors/include/huron/sensors/force_torque_sensor.h b/system/sensors/include/huron/sensors/force_torque_sensor.h index b0b9f8a0..bcc3d213 100644 --- a/system/sensors/include/huron/sensors/force_torque_sensor.h +++ b/system/sensors/include/huron/sensors/force_torque_sensor.h @@ -23,27 +23,29 @@ class ForceTorqueSensor : public SensorWithFrame { void RequestStateUpdate() override; - void GetNewState(Eigen::Ref new_state) const override; + void GetNewState(Eigen::Ref> new_state) const override; /** * Measures the external forces and moments. * * @return Wrench 6x1 vector \f$ [Fx, Fy, Fz, Tx, Ty, Tz]^T \f$. */ - Eigen::VectorXd GetValue() const override; + huron::VectorX GetValue() const override; protected: /** * To be overriden. */ - virtual Vector6d DoGetWrenchRaw() = 0; + virtual huron::Vector6 DoGetWrenchRaw() = 0; bool reverse_wrench_direction_; private: - huron::Vector6d wrench_; + huron::Vector6 wrench_; }; } // namespace huron HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ForceTorqueSensor) +HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ForceTorqueSensor) diff --git a/system/sensors/src/force_sensing_resistor.cc b/system/sensors/src/force_sensing_resistor.cc index 2af0f6ab..e1d841cc 100644 --- a/system/sensors/src/force_sensing_resistor.cc +++ b/system/sensors/src/force_sensing_resistor.cc @@ -17,3 +17,5 @@ ForceSensingResistor::ForceSensingResistor( HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ForceSensingResistor) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ForceSensingResistor) diff --git a/system/sensors/src/force_sensing_resistor_array.cc b/system/sensors/src/force_sensing_resistor_array.cc index 81763c9e..ae162302 100644 --- a/system/sensors/src/force_sensing_resistor_array.cc +++ b/system/sensors/src/force_sensing_resistor_array.cc @@ -9,7 +9,7 @@ ForceSensingResistorArray::ForceSensingResistorArray( const std::vector>>& fsr_array) : SensorWithFrame(fsr_array.size(), 1, std::move(frame)), name_(name), - values_(Eigen::VectorXd::Zero(fsr_array.size())), + values_(huron::VectorX::Zero(fsr_array.size())), fsr_array_(fsr_array) {} template @@ -20,7 +20,7 @@ ForceSensingResistorArray::ForceSensingResistorArray( std::unique_ptr config) : SensorWithFrame(fsr_array.size(), 1, std::move(frame), std::move(config)), name_(name), - values_(Eigen::VectorXd::Zero(fsr_array.size())), + values_(huron::VectorX::Zero(fsr_array.size())), fsr_array_(fsr_array) {} template @@ -32,12 +32,12 @@ void ForceSensingResistorArray::RequestStateUpdate() { template void ForceSensingResistorArray::GetNewState( - Eigen::Ref new_state) const { + Eigen::Ref> new_state) const { new_state = values_; } template -Eigen::Affine3d ForceSensingResistorArray::GetSensorPose(size_t index) const { +huron::SE3 ForceSensingResistorArray::GetSensorPose(size_t index) const { return fsr_array_[index]->GetSensorFrame().lock()->GetTransformInWorld(); } @@ -45,3 +45,5 @@ Eigen::Affine3d ForceSensingResistorArray::GetSensorPose(size_t index) const HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ForceSensingResistorArray) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ForceSensingResistorArray) diff --git a/system/sensors/src/force_torque_sensor.cc b/system/sensors/src/force_torque_sensor.cc index a463f190..f57031df 100644 --- a/system/sensors/src/force_torque_sensor.cc +++ b/system/sensors/src/force_torque_sensor.cc @@ -24,12 +24,12 @@ void ForceTorqueSensor::RequestStateUpdate() { template void ForceTorqueSensor::GetNewState( - Eigen::Ref new_state) const { + Eigen::Ref> new_state) const { new_state = GetValue(); } template -Eigen::VectorXd ForceTorqueSensor::GetValue() const { +huron::VectorX ForceTorqueSensor::GetValue() const { if (reverse_wrench_direction_) return -wrench_; return wrench_; @@ -39,3 +39,5 @@ Eigen::VectorXd ForceTorqueSensor::GetValue() const { HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class huron::ForceTorqueSensor) +HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( + class huron::ForceTorqueSensor) diff --git a/utils/include/huron/utils/template_instantiations.h b/utils/include/huron/utils/template_instantiations.h index 59016703..d22eb5fe 100644 --- a/utils/include/huron/utils/template_instantiations.h +++ b/utils/include/huron/utils/template_instantiations.h @@ -5,6 +5,14 @@ * Source: https://github.com/RobotLocomotion/drake/blob/master/common/default_scalars.h */ +/** + * Declares that template instantiations exist for huron's default scalars. + * This should only be used in .h files, never in .cc files. + */ +#define HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( \ + SomeType) \ +extern template SomeType; + /** * Defines template instantiations for huron's default scalars. * This should only be used in .cc files, never in .h files. @@ -13,10 +21,41 @@ SomeType) \ template SomeType; +#if HURON_USE_CASADI==1 + +/** + * Defines template instantiations for huron's autodiff scalars. + * Currently, Casadi is supported. + * This should only be used in .cc files, never in .h files. + */ +#define HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( \ + SomeType) \ +template SomeType; + /** - * Declares that template instantiations exist for huron's default scalars. + * Declares that template instantiations exist for huron's autodiff scalars. + * Currently, Casadi is supported. * This should only be used in .h files, never in .cc files. */ -#define HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( \ +#define HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( \ SomeType) \ -extern template SomeType; +extern template SomeType; + +#else + +/** + * Defines template instantiations for huron's autodiff scalars. + * Currently, Casadi is supported. + * This should only be used in .cc files, never in .h files. + */ +#define HURON_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( \ + SomeType) + +/** + * Declares that template instantiations exist for huron's autodiff scalars. + * Currently, Casadi is supported. + * This should only be used in .h files, never in .cc files. + */ +#define HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( \ + SomeType) +#endif From 5571155bfb09546451b1dc49e53c2de9f28910d1 Mon Sep 17 00:00:00 2001 From: Duc Doan Date: Mon, 11 Mar 2024 21:40:04 -0400 Subject: [PATCH 6/9] Builds successfully with casadi --- common/include/huron/types.h | 14 +- math/test/test_se3.cc | 8 +- multibody/CMakeLists.txt | 2 +- .../include/huron/multibody/joint_common.h | 161 ++++++++++-------- .../huron/multibody/model_impl_interface.h | 4 +- .../huron/multibody/pinocchio_model_impl.h | 8 +- multibody/src/model_impl_default.cc | 4 +- multibody/src/pinocchio_model_impl.cc | 57 ++++--- multibody/test/test_model.cc | 20 +-- multibody/test/test_pinocchio_impl.cc | 2 +- .../constant_state_provider.h | 8 +- .../huron/control_interfaces/encoder.h | 8 +- .../include/huron/control_interfaces/joint.h | 2 +- .../huron/control_interfaces/rotary_encoder.h | 16 +- .../huron/control_interfaces/state_provider.h | 8 +- .../test/test_legged_zmp.cc | 4 +- .../locomotion/test/test_zero_moment_point.cc | 4 +- 17 files changed, 186 insertions(+), 144 deletions(-) diff --git a/common/include/huron/types.h b/common/include/huron/types.h index 259b9df5..054e1324 100644 --- a/common/include/huron/types.h +++ b/common/include/huron/types.h @@ -51,13 +51,13 @@ using Matrix6Xd = Matrix6X; // namespace Eigen { // -// template -// struct NumTraits> : NumTraits +// template <> +// struct NumTraits : NumTraits // { -// using Real = casadi::Matrix; -// using NonInteger = casadi::Matrix; -// using Literal = casadi::Matrix; -// using Nested = casadi::Matrix; +// using Real = casadi::SX; +// using NonInteger = casadi::SX; +// using Literal = casadi::SX; +// using Nested = casadi::SX; // // enum { // // does not support complex Base types @@ -74,7 +74,7 @@ using Matrix6Xd = Matrix6X; // MulCost = 2 // }; // }; -// + // } // namespace Eigen #endif // HURON_USE_CASADI diff --git a/math/test/test_se3.cc b/math/test/test_se3.cc index b043eb04..a64facb8 100644 --- a/math/test/test_se3.cc +++ b/math/test/test_se3.cc @@ -24,7 +24,7 @@ TEST_F(TestSe3, InitFromMatrix) { tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); - EXPECT_EQ(huron::SE3(tf_gt.matrix()).matrix(), tf_gt); + EXPECT_EQ(huron::SE3(tf_gt.matrix()).matrix(), tf_gt.matrix()); } TEST_F(TestSe3, InitFromRotationTranslation) { @@ -35,7 +35,7 @@ TEST_F(TestSe3, InitFromRotationTranslation) { tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); EXPECT_EQ(huron::SE3(tf_gt.rotation().matrix(), tf_gt.translation().matrix()).matrix(), - tf_gt); + tf_gt.matrix()); } TEST_F(TestSe3, Translation) { @@ -63,7 +63,7 @@ TEST_F(TestSe3, Prerotation) { tf_gt = a; tf_gt.prerotate(b.rotation()); // Test object - huron::SE3 tf(a.matrix(), Eigen::Vector3d::Zero());; + huron::SE3 tf(a.matrix());; tf.prerotate(b.rotation().matrix()); EXPECT_EQ(tf.matrix(), tf_gt.matrix()); @@ -83,7 +83,7 @@ TEST_F(TestSe3, Rotation) { tf_gt = a; tf_gt.rotate(b.rotation()); // Test object - huron::SE3 tf(a.matrix(), Eigen::Vector3d::Zero());; + huron::SE3 tf(a.matrix());; tf.rotate(b.rotation().matrix()); EXPECT_EQ(tf.matrix(), tf_gt.matrix()); diff --git a/multibody/CMakeLists.txt b/multibody/CMakeLists.txt index 2ca0734a..e6eb9a30 100644 --- a/multibody/CMakeLists.txt +++ b/multibody/CMakeLists.txt @@ -2,7 +2,7 @@ set(dir "${CMAKE_CURRENT_SOURCE_DIR}") list(APPEND SOURCE_FILES "${dir}/src/model.cc" - "${dir}/src/joint_common.cc" + # "${dir}/src/joint_common.cc" "${dir}/src/frame.cc" "${dir}/src/logical_frame.cc" "${dir}/src/model_impl_default.cc") diff --git a/multibody/include/huron/multibody/joint_common.h b/multibody/include/huron/multibody/joint_common.h index 59b67184..1a5b28f6 100644 --- a/multibody/include/huron/multibody/joint_common.h +++ b/multibody/include/huron/multibody/joint_common.h @@ -7,6 +7,8 @@ #include #include +#include "huron/types.h" +#include "huron/utils/template_instantiations.h" #include "frame.h" namespace huron { @@ -24,6 +26,7 @@ enum class JointType { kFreeFlyer }; +template struct JointDescription { public: // TODO(dtbpkmte): Properly implement JointIndex/FrameIndex and casts. @@ -32,16 +35,16 @@ struct JointDescription { size_t child_frame_id, size_t num_positions, size_t num_velocities, JointType type, - const Eigen::VectorXd& min_position, - const Eigen::VectorXd& max_position, - const Eigen::VectorXd& min_velocity, - const Eigen::VectorXd& max_velocity, - const Eigen::VectorXd& min_acceleration, - const Eigen::VectorXd& max_acceleration, - const Eigen::VectorXd& min_torque, - const Eigen::VectorXd& max_torque, - const Eigen::VectorXd& friction, - const Eigen::VectorXd& damping) + const huron::VectorX& min_position, + const huron::VectorX& max_position, + const huron::VectorX& min_velocity, + const huron::VectorX& max_velocity, + const huron::VectorX& min_acceleration, + const huron::VectorX& max_acceleration, + const huron::VectorX& min_torque, + const huron::VectorX& max_torque, + const huron::VectorX& friction, + const huron::VectorX& damping) : id_((JointIndex) id), name_(name), parent_frame_id_((FrameIndex) parent_frame_id), @@ -59,24 +62,24 @@ struct JointDescription { max_torque_(max_torque), friction_(friction), damping_(damping) { - assert(min_position.size() == num_positions_); - assert(max_position.size() == num_positions_); - assert((max_position.array() >= min_position.array()).all()); - - assert(min_velocity.size() == num_velocities_); - assert(max_velocity.size() == num_velocities_); - assert((max_velocity.array() >= min_velocity.array()).all()); - - assert(min_acceleration.size() == num_velocities_); - assert(max_acceleration.size() == num_velocities_); - assert((max_acceleration.array() >= min_acceleration.array()).all()); - - assert(min_torque.size() == num_velocities_); - assert(max_torque.size() == num_velocities_); - assert((max_torque.array() >= min_torque.array()).all()); - - assert(friction.size() == num_velocities_); - assert(damping.size() == num_velocities_); + // assert(min_position.size() == num_positions_); + // assert(max_position.size() == num_positions_); + // assert((max_position.array() >= min_position.array()).all()); + // + // assert(min_velocity.size() == num_velocities_); + // assert(max_velocity.size() == num_velocities_); + // assert((max_velocity.array() >= min_velocity.array()).all()); + // + // assert(min_acceleration.size() == num_velocities_); + // assert(max_acceleration.size() == num_velocities_); + // assert((max_acceleration.array() >= min_acceleration.array()).all()); + // + // assert(min_torque.size() == num_velocities_); + // assert(max_torque.size() == num_velocities_); + // assert((max_torque.array() >= min_torque.array()).all()); + // + // assert(friction.size() == num_velocities_); + // assert(damping.size() == num_velocities_); } JointDescription(size_t id, const std::string& name, @@ -84,14 +87,14 @@ struct JointDescription { size_t child_frame_id, size_t num_positions, size_t num_velocities, JointType type, - const Eigen::VectorXd& min_position, - const Eigen::VectorXd& max_position, - const Eigen::VectorXd& min_velocity, - const Eigen::VectorXd& max_velocity, - const Eigen::VectorXd& min_acceleration, - const Eigen::VectorXd& max_acceleration, - const Eigen::VectorXd& min_torque, - const Eigen::VectorXd& max_torque) + const huron::VectorX& min_position, + const huron::VectorX& max_position, + const huron::VectorX& min_velocity, + const huron::VectorX& max_velocity, + const huron::VectorX& min_acceleration, + const huron::VectorX& max_acceleration, + const huron::VectorX& min_torque, + const huron::VectorX& max_torque) : JointDescription(id, name, parent_frame_id, child_frame_id, num_positions, num_velocities, @@ -100,8 +103,8 @@ struct JointDescription { min_velocity, max_velocity, min_acceleration, max_acceleration, min_torque, max_torque, - Eigen::VectorXd::Zero(num_velocities), - Eigen::VectorXd::Zero(num_velocities)) {} + huron::VectorX::Zero(num_velocities), + huron::VectorX::Zero(num_velocities)) {} JointDescription(size_t id, const std::string& name, size_t parent_frame_id, @@ -114,24 +117,24 @@ struct JointDescription { parent_frame_id, child_frame_id, num_positions, num_velocities, type, - Eigen::VectorXd::Constant(num_positions, + huron::VectorX::Constant(num_positions, -std::numeric_limits::infinity()), - Eigen::VectorXd::Constant(num_positions, + huron::VectorX::Constant(num_positions, std::numeric_limits::infinity()), - Eigen::VectorXd::Constant(num_velocities, + huron::VectorX::Constant(num_velocities, -std::numeric_limits::infinity()), - Eigen::VectorXd::Constant(num_velocities, + huron::VectorX::Constant(num_velocities, std::numeric_limits::infinity()), - Eigen::VectorXd::Constant(num_velocities, + huron::VectorX::Constant(num_velocities, -std::numeric_limits::infinity()), - Eigen::VectorXd::Constant(num_velocities, + huron::VectorX::Constant(num_velocities, std::numeric_limits::infinity()), - Eigen::VectorXd::Constant(num_velocities, + huron::VectorX::Constant(num_velocities, -std::numeric_limits::infinity()), - Eigen::VectorXd::Constant(num_velocities, + huron::VectorX::Constant(num_velocities, std::numeric_limits::infinity()), - Eigen::VectorXd::Zero(num_velocities), - Eigen::VectorXd::Zero(num_velocities)) {} + huron::VectorX::Zero(num_velocities), + huron::VectorX::Zero(num_velocities)) {} JointIndex id() const { return id_; } const std::string& name() const { return name_; } @@ -140,16 +143,16 @@ struct JointDescription { size_t num_positions() const { return num_positions_; } size_t num_velocities() const { return num_velocities_; } JointType type() const { return type_; } - const Eigen::VectorXd& min_position() const { return min_position_; } - const Eigen::VectorXd& max_position() const { return max_position_; } - const Eigen::VectorXd& min_velocity() const { return min_velocity_; } - const Eigen::VectorXd& max_velocity() const { return max_velocity_; } - const Eigen::VectorXd& min_acceleration() const { return min_acceleration_; } - const Eigen::VectorXd& max_acceleration() const { return max_acceleration_; } - const Eigen::VectorXd& min_torque() const { return min_torque_; } - const Eigen::VectorXd& max_torque() const { return max_torque_; } - const Eigen::VectorXd& friction() const { return friction_; } - const Eigen::VectorXd& damping() const { return damping_; } + const huron::VectorX& min_position() const { return min_position_; } + const huron::VectorX& max_position() const { return max_position_; } + const huron::VectorX& min_velocity() const { return min_velocity_; } + const huron::VectorX& max_velocity() const { return max_velocity_; } + const huron::VectorX& min_acceleration() const { return min_acceleration_; } + const huron::VectorX& max_acceleration() const { return max_acceleration_; } + const huron::VectorX& min_torque() const { return min_torque_; } + const huron::VectorX& max_torque() const { return max_torque_; } + const huron::VectorX& friction() const { return friction_; } + const huron::VectorX& damping() const { return damping_; } private: JointIndex id_; @@ -159,19 +162,39 @@ struct JointDescription { size_t num_positions_; size_t num_velocities_; JointType type_; - Eigen::VectorXd min_position_; - Eigen::VectorXd max_position_; - Eigen::VectorXd min_velocity_; - Eigen::VectorXd max_velocity_; - Eigen::VectorXd min_acceleration_; - Eigen::VectorXd max_acceleration_; - Eigen::VectorXd min_torque_; - Eigen::VectorXd max_torque_; - Eigen::VectorXd friction_; - Eigen::VectorXd damping_; + huron::VectorX min_position_; + huron::VectorX max_position_; + huron::VectorX min_velocity_; + huron::VectorX max_velocity_; + huron::VectorX min_acceleration_; + huron::VectorX max_acceleration_; + huron::VectorX min_torque_; + huron::VectorX max_torque_; + huron::VectorX friction_; + huron::VectorX damping_; }; -std::ostream& operator<<(std::ostream &os, const JointDescription &jd); +template +std::ostream& operator<<(std::ostream &os, const JointDescription &jd) { + return (os << "ID: " << jd.id() + << "\nName: " << jd.name() + << "\nParent Frame ID: " << jd.parent_frame_id() + << "\nChild Frame ID: " << jd.child_frame_id() + << "\nNum Positions: " << jd.num_positions() + << "\nNum Velocities: " << jd.num_velocities() + << "\nType: " << static_cast(jd.type()) + << "\nMin Position: " << jd.min_position() + << "\nMax Position: " << jd.max_position() + << "\nMin Velocity: " << jd.min_velocity() + << "\nMax Velocity: " << jd.max_velocity() + << "\nMin Acceleration: " << jd.min_acceleration() + << "\nMax Acceleration: " << jd.max_acceleration() + << "\nMin Torque: " << jd.min_torque() + << "\nMax Torque: " << jd.max_torque() + << "\nFriction: " << jd.friction() + << "\nDamping: " << jd.damping() + << std::endl); +} } // namespace multibody } // namespace huron diff --git a/multibody/include/huron/multibody/model_impl_interface.h b/multibody/include/huron/multibody/model_impl_interface.h index d3c4a151..89195191 100644 --- a/multibody/include/huron/multibody/model_impl_interface.h +++ b/multibody/include/huron/multibody/model_impl_interface.h @@ -30,9 +30,9 @@ class ModelImplInterface { virtual JointType GetJointType(size_t joint_index) const; virtual JointIndex GetJointIndex(const std::string& joint_name) const = 0; - virtual std::unique_ptr GetJointDescription( + virtual std::unique_ptr> GetJointDescription( JointIndex joint_index) const; - virtual std::unique_ptr GetJointDescription( + virtual std::unique_ptr> GetJointDescription( const std::string& joint_name) const; virtual huron::SE3 diff --git a/multibody/include/huron/multibody/pinocchio_model_impl.h b/multibody/include/huron/multibody/pinocchio_model_impl.h index 86a342f4..ef2d74f6 100644 --- a/multibody/include/huron/multibody/pinocchio_model_impl.h +++ b/multibody/include/huron/multibody/pinocchio_model_impl.h @@ -34,9 +34,9 @@ class PinocchioModelImpl : public ModelImplInterface { JointType GetJointType(size_t joint_index) const override; JointIndex GetJointIndex(const std::string& joint_name) const override; - std::unique_ptr GetJointDescription( + std::unique_ptr> GetJointDescription( JointIndex joint_index) const override; - std::unique_ptr GetJointDescription( + std::unique_ptr> GetJointDescription( const std::string& joint_name) const override; huron::SE3 @@ -96,6 +96,10 @@ class PinocchioModelImpl : public ModelImplInterface { size_t num_frames_ = 0; }; +template <> +void PinocchioModelImpl::BuildFromUrdf(const std::string& urdf_path, + JointType root_joint_type); + } // namespace internal } // namespace multibody } // namespace huron diff --git a/multibody/src/model_impl_default.cc b/multibody/src/model_impl_default.cc index 7b21d046..0075ed03 100644 --- a/multibody/src/model_impl_default.cc +++ b/multibody/src/model_impl_default.cc @@ -39,12 +39,12 @@ ModelImplInterface::GetJointIndex(const std::string& joint_name) const { } template -std::unique_ptr ModelImplInterface::GetJointDescription( +std::unique_ptr> ModelImplInterface::GetJointDescription( JointIndex joint_index) const { throw NotImplementedException(); } template -std::unique_ptr ModelImplInterface::GetJointDescription( +std::unique_ptr> ModelImplInterface::GetJointDescription( const std::string& joint_name) const { throw NotImplementedException(); } diff --git a/multibody/src/pinocchio_model_impl.cc b/multibody/src/pinocchio_model_impl.cc index bf6cfda0..6ff3b92c 100644 --- a/multibody/src/pinocchio_model_impl.cc +++ b/multibody/src/pinocchio_model_impl.cc @@ -1,8 +1,4 @@ -#include "huron/multibody/pinocchio_model_impl.h" -#include "huron/multibody/joint_common.h" -#include "huron/multibody/model_impl_types.h" -#include "huron/exceptions/not_implemented_exception.h" - +#include "pinocchio/math/casadi.hpp" #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" #include "pinocchio/parsers/urdf.hpp" @@ -12,6 +8,11 @@ #include "pinocchio/algorithm/frames.hpp" #include "pinocchio/algorithm/center-of-mass.hpp" +#include "huron/multibody/pinocchio_model_impl.h" +#include "huron/multibody/joint_common.h" +#include "huron/multibody/model_impl_types.h" +#include "huron/exceptions/not_implemented_exception.h" + namespace huron { namespace multibody { namespace internal { @@ -41,23 +42,41 @@ PinocchioModelImpl::PinocchioModelImpl() template PinocchioModelImpl::~PinocchioModelImpl() = default; +template <> +void PinocchioModelImpl::BuildFromUrdf(const std::string& urdf_path, + JointType root_joint_type) { + pinocchio::Model::JointModel joint_model; + if (root_joint_type == JointType::kFixed) { + pinocchio::urdf::buildModel(urdf_path, impl_->model_); + } else { + switch (root_joint_type) { + case JointType::kFreeFlyer: + joint_model = pinocchio::JointModelFreeFlyer(); + break; + case JointType::kPlanar: + joint_model = pinocchio::JointModelPlanar(); + break; + default: + throw std::runtime_error("Unsupported root joint type."); + break; + } + pinocchio::urdf::buildModel(urdf_path, joint_model, impl_->model_); + } + impl_->data_ = pinocchio::Data(impl_->model_); + num_positions_ = impl_->model_.nq; + num_velocities_ = impl_->model_.nv; + num_joints_ = impl_->model_.njoints; + num_frames_ = impl_->model_.nframes; +} + template void PinocchioModelImpl::BuildFromUrdf(const std::string& urdf_path, JointType root_joint_type) { - #if HURON_USE_CASADI==1 pinocchio::Model tmp_model; - #endif - pinocchio::Model::JointModel joint_model; if (root_joint_type == JointType::kFixed) { - - #if HURON_USE_CASADI==1 pinocchio::urdf::buildModel(urdf_path, tmp_model); impl_->model_ = tmp_model.cast(); - #else - pinocchio::urdf::buildModel(urdf_path, impl_->model_); - #endif - } else { switch (root_joint_type) { case JointType::kFreeFlyer: @@ -70,12 +89,8 @@ void PinocchioModelImpl::BuildFromUrdf(const std::string& urdf_path, throw std::runtime_error("Unsupported root joint type."); break; } - #if HURON_USE_CASADI==1 pinocchio::urdf::buildModel(urdf_path, joint_model, tmp_model); impl_->model_ = tmp_model.cast(); - #else - pinocchio::urdf::buildModel(urdf_path, joint_model, impl_->model_); - #endif } impl_->data_ = pinocchio::DataTpl(impl_->model_); num_positions_ = impl_->model_.nq; @@ -102,13 +117,13 @@ PinocchioModelImpl::GetJoint(size_t joint_index) const { } template -std::unique_ptr PinocchioModelImpl::GetJointDescription( +std::unique_ptr> PinocchioModelImpl::GetJointDescription( JointIndex joint_index) const { return GetJointDescription(impl_->model_.names[joint_index]); } template -std::unique_ptr +std::unique_ptr> PinocchioModelImpl::GetJointDescription( const std::string& joint_name) const { // auto frame_id = impl_->model_.getFrameId(joint_name, pinocchio::JOINT); @@ -119,7 +134,7 @@ PinocchioModelImpl::GetJointDescription( size_t parent_frame_index = frame.previousFrame; JointType joint_type = (frame_id == 0) ? JointType::kUnknown : GetJointType(joint_index); - return std::make_unique( + return std::make_unique>( frame.parent, joint_name, parent_frame_index, diff --git a/multibody/test/test_model.cc b/multibody/test/test_model.cc index 31d51220..0f9859d8 100644 --- a/multibody/test/test_model.cc +++ b/multibody/test/test_model.cc @@ -28,11 +28,11 @@ class TestModelPinocchio : public testing::Test { robot.GetModel()->BuildFromUrdf("rrbot.urdf"); robot.GetModel()->SetJointStateProvider( 1, - std::make_shared( + std::make_shared>( Eigen::Vector2d::Zero())); robot.GetModel()->SetJointStateProvider( 2, - std::make_shared( + std::make_shared>( Eigen::Vector2d::Zero())); robot.GetModel()->Finalize(); } @@ -54,20 +54,20 @@ TEST_F(TestModelPinocchio, TestKinematics) { auto link1_frame = robot.GetModel()->GetFrame("link1"); auto link2_frame = robot.GetModel()->GetFrame("link2"); - Eigen::Affine3d expected_base_link_pose(Eigen::Affine3d::Identity()); - Eigen::Affine3d expected_link1_pose = - expected_base_link_pose * Eigen::Translation3d(0, 0.1, 1.95); - Eigen::Affine3d expected_link2_pose = - expected_link1_pose * Eigen::Translation3d(0, 0.1, 0.9); + SE3 expected_base_link_pose; + SE3 expected_link1_pose(Eigen::Matrix3d::Identity(), + Eigen::Vector3d(0, 0.1, 1.95)); + SE3 expected_link2_pose = expected_link1_pose; + expected_link2_pose.translate(Eigen::Vector3d(0, 0.1, 0.9)); robot.GetModel()->UpdateJointStates(); robot.GetModel()->ForwardKinematics(); - Eigen::Affine3d base_link_pose = + SE3 base_link_pose = base_link_frame.lock()->GetTransformInWorld(); - Eigen::Affine3d link1_pose = + SE3 link1_pose = link1_frame.lock()->GetTransformInWorld(); - Eigen::Affine3d link2_pose = + SE3 link2_pose = link2_frame.lock()->GetTransformInWorld(); EXPECT_EQ(base_link_pose.matrix(), expected_base_link_pose.matrix()); diff --git a/multibody/test/test_pinocchio_impl.cc b/multibody/test/test_pinocchio_impl.cc index ec6707a4..33d488b0 100644 --- a/multibody/test/test_pinocchio_impl.cc +++ b/multibody/test/test_pinocchio_impl.cc @@ -19,7 +19,7 @@ TEST_F(PinocchioModelImplTest, RRBotGeneralChecks) { auto v = impl_rrbot.GetJointNames(); std::cout << "id\tname\t\tparent_frame\tchild_frame" << std::endl; for (auto& n : v) { - std::unique_ptr jd = + std::unique_ptr> jd = impl_rrbot.GetJointDescription(n); std::cout << *jd << std::endl; } diff --git a/system/control_interfaces/include/huron/control_interfaces/constant_state_provider.h b/system/control_interfaces/include/huron/control_interfaces/constant_state_provider.h index 976c1a78..ff288987 100644 --- a/system/control_interfaces/include/huron/control_interfaces/constant_state_provider.h +++ b/system/control_interfaces/include/huron/control_interfaces/constant_state_provider.h @@ -29,7 +29,7 @@ class ConstantStateProvider : public StateProvider { }; } // namespace huron -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class huron::ConstantStateProvider) -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( - class huron::ConstantStateProvider) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( +// class huron::ConstantStateProvider) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( +// class huron::ConstantStateProvider) diff --git a/system/control_interfaces/include/huron/control_interfaces/encoder.h b/system/control_interfaces/include/huron/control_interfaces/encoder.h index bc0f7c63..e5727aee 100644 --- a/system/control_interfaces/include/huron/control_interfaces/encoder.h +++ b/system/control_interfaces/include/huron/control_interfaces/encoder.h @@ -61,7 +61,7 @@ class Encoder : public Sensor { } // namespace huron -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class huron::Encoder) -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( - class huron::Encoder) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( +// class huron::Encoder) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( +// class huron::Encoder) diff --git a/system/control_interfaces/include/huron/control_interfaces/joint.h b/system/control_interfaces/include/huron/control_interfaces/joint.h index 1f567014..a4aabc0b 100644 --- a/system/control_interfaces/include/huron/control_interfaces/joint.h +++ b/system/control_interfaces/include/huron/control_interfaces/joint.h @@ -13,7 +13,7 @@ namespace huron { template class Joint { - using JointDescription = huron::multibody::JointDescription; + using JointDescription = huron::multibody::JointDescription; using JointType = huron::multibody::JointType; public: diff --git a/system/control_interfaces/include/huron/control_interfaces/rotary_encoder.h b/system/control_interfaces/include/huron/control_interfaces/rotary_encoder.h index dbd22dec..b7b3065f 100644 --- a/system/control_interfaces/include/huron/control_interfaces/rotary_encoder.h +++ b/system/control_interfaces/include/huron/control_interfaces/rotary_encoder.h @@ -149,11 +149,11 @@ class RotaryEncoder : public Encoder { } // namespace huron -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class huron::RotaryEncoderConfiguration) -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( - class huron::RotaryEncoderConfiguration) -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class huron::RotaryEncoder) -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( - class huron::RotaryEncoder) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( +// class huron::RotaryEncoderConfiguration) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( +// class huron::RotaryEncoderConfiguration) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( +// class huron::RotaryEncoder) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( +// class huron::RotaryEncoder) diff --git a/system/control_interfaces/include/huron/control_interfaces/state_provider.h b/system/control_interfaces/include/huron/control_interfaces/state_provider.h index 037de4ec..685e49f7 100644 --- a/system/control_interfaces/include/huron/control_interfaces/state_provider.h +++ b/system/control_interfaces/include/huron/control_interfaces/state_provider.h @@ -27,7 +27,7 @@ class StateProvider { } // namespace huron -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class huron::StateProvider) -HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( - class huron::StateProvider) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( +// class huron::StateProvider) +// HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_AD_SCALARS( +// class huron::StateProvider) diff --git a/system/control_interfaces/test/test_legged_zmp.cc b/system/control_interfaces/test/test_legged_zmp.cc index d25b8cb2..24bc5dc9 100644 --- a/system/control_interfaces/test/test_legged_zmp.cc +++ b/system/control_interfaces/test/test_legged_zmp.cc @@ -62,13 +62,13 @@ class TestLeggedZmp : public testing::Test { robot.GetModel()->BuildFromUrdf("huron.urdf", multibody::JointType::kFreeFlyer); auto floating_joint_sp = - std::make_shared(initial_state); + std::make_shared>(initial_state); robot.RegisterStateProvider(floating_joint_sp, true); robot.GetModel()->SetJointStateProvider(1, floating_joint_sp); // Fake joint states for (size_t joint_index = 2; joint_index <= 13; ++joint_index) { auto sp = - std::make_shared(Eigen::Vector2d::Zero()); + std::make_shared>(Eigen::Vector2d::Zero()); robot.RegisterStateProvider(sp, true); robot.GetModel()->SetJointStateProvider( joint_index, diff --git a/system/locomotion/test/test_zero_moment_point.cc b/system/locomotion/test/test_zero_moment_point.cc index bef7074e..4cb526be 100644 --- a/system/locomotion/test/test_zero_moment_point.cc +++ b/system/locomotion/test/test_zero_moment_point.cc @@ -62,13 +62,13 @@ class TestZeroMomentPointFt : public testing::Test { robot.GetModel()->BuildFromUrdf("huron.urdf", multibody::JointType::kFreeFlyer); auto floating_joint_sp = - std::make_shared(initial_state); + std::make_shared>(initial_state); robot.RegisterStateProvider(floating_joint_sp, true); robot.GetModel()->SetJointStateProvider(1, floating_joint_sp); // Fake joint states for (size_t joint_index = 2; joint_index <= 13; ++joint_index) { auto sp = - std::make_shared(Eigen::Vector2d::Zero()); + std::make_shared>(Eigen::Vector2d::Zero()); robot.RegisterStateProvider(sp, true); robot.GetModel()->SetJointStateProvider( joint_index, From 3123dd6105a0aeeec9df9781ab0cc85314c69fa4 Mon Sep 17 00:00:00 2001 From: Duc Doan Date: Wed, 13 Mar 2024 16:46:31 -0400 Subject: [PATCH 7/9] Add casadi utils from Pinocchio --- math/include/huron/math/casadi.h | 9 ++++ math/include/huron/math/casadi_copy.h | 77 +++++++++++++++++++++++++++ 2 files changed, 86 insertions(+) create mode 100644 math/include/huron/math/casadi.h create mode 100644 math/include/huron/math/casadi_copy.h diff --git a/math/include/huron/math/casadi.h b/math/include/huron/math/casadi.h new file mode 100644 index 00000000..fc0d8001 --- /dev/null +++ b/math/include/huron/math/casadi.h @@ -0,0 +1,9 @@ +#pragma once + +#if HURON_ENABLE_AUTODIFF==1 +#if HURON_USE_CASADI==1 + +#include "casadi_copy.h" + +#endif // HURON_USE_CASADI +#endif // HURON_ENABLE_AUTODIFF diff --git a/math/include/huron/math/casadi_copy.h b/math/include/huron/math/casadi_copy.h new file mode 100644 index 00000000..01e6f7cd --- /dev/null +++ b/math/include/huron/math/casadi_copy.h @@ -0,0 +1,77 @@ +/** + * The functions contained in this file are from the Pinocchio project + * (https://github.com/stack-of-tasks/pinocchio) with the following license: + * + * BSD 2-Clause License + + * Copyright (c) 2014-2023, CNRS + * Copyright (c) 2018-2023, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * The views and conclusions contained in the software and documentation are those + * of the authors and should not be interpreted as representing official policies, + * either expressed or implied, of the Pinocchio project. +*/ +#pragma once + +#if HURON_ENABLE_AUTODIFF==1 +#if HURON_USE_CASADI==1 + +#include +#include + +namespace huron { + +template +inline void copy(::casadi::Matrix const & src, + Eigen::MatrixBase & dst) +{ + Eigen::DenseIndex const m = src.size1(); + Eigen::DenseIndex const n = src.size2(); + + dst.resize(m, n); + + for (Eigen::DenseIndex i = 0; i < m; ++i) + for (Eigen::DenseIndex j = 0; j < n; ++j) + dst(i, j) = src(i, j); +} + + +// Copy Eigen matrix to casadi matrix +template +inline void copy(Eigen::MatrixBase const & src, + ::casadi::Matrix & dst) +{ + Eigen::DenseIndex const m = src.rows(); + Eigen::DenseIndex const n = src.cols(); + + dst.resize(m, n); + + for (Eigen::DenseIndex i = 0; i < m; ++i) + for (Eigen::DenseIndex j = 0; j < n; ++j) + dst(i, j) = src(i, j); +} +} // namespace huron + +#endif // HURON_USE_CASADI +#endif // HURON_ENABLE_AUTODIFF From d5e3716ae629b92399958916aed1f23435e7a16a Mon Sep 17 00:00:00 2001 From: Duc Doan Date: Wed, 13 Mar 2024 16:46:53 -0400 Subject: [PATCH 8/9] Add math utils --- math/include/huron/math/utils.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 math/include/huron/math/utils.h diff --git a/math/include/huron/math/utils.h b/math/include/huron/math/utils.h new file mode 100644 index 00000000..ce47e682 --- /dev/null +++ b/math/include/huron/math/utils.h @@ -0,0 +1,14 @@ +#pragma once + +#include "huron/types.h" + +namespace huron { + +template +Matrix3 skew(const Vector3& v) { + Matrix3 ret; + ret << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0; + return ret; +} + +} // namespace huron From 619e4e369c453638bc634b63507be1c3a4177ad7 Mon Sep 17 00:00:00 2001 From: Duc Doan Date: Wed, 13 Mar 2024 16:51:21 -0400 Subject: [PATCH 9/9] SE3 completed --- common/include/huron/types.h | 9 +- math/CMakeLists.txt | 2 - math/include/huron/math/rotation.h | 29 ---- math/include/huron/math/se3.h | 26 ++- math/src/rotation.cc | 62 -------- math/src/se3.cc | 37 +++-- math/test/test_rotation.cc | 51 ------ math/test/test_se3.cc | 148 ++++++++++++++---- multibody/src/logical_frame.cc | 4 +- multibody/src/pinocchio_model_impl.cc | 6 +- multibody/test/test_model.cc | 2 +- .../test/test_legged_zmp.cc | 5 +- system/locomotion/src/zero_moment_point.cc | 4 +- .../src/zero_moment_point_ft_sensor.cc | 7 +- .../locomotion/test/test_zero_moment_point.cc | 5 +- 15 files changed, 186 insertions(+), 211 deletions(-) delete mode 100644 math/include/huron/math/rotation.h delete mode 100644 math/src/rotation.cc delete mode 100644 math/test/test_rotation.cc diff --git a/common/include/huron/types.h b/common/include/huron/types.h index 054e1324..3b96d75d 100644 --- a/common/include/huron/types.h +++ b/common/include/huron/types.h @@ -7,6 +7,9 @@ namespace huron { template using Affine3 = Eigen::Transform; +template +using Vector4 = Eigen::Matrix; + template using Vector3 = Eigen::Matrix; @@ -28,11 +31,15 @@ using Matrix4 = Eigen::Matrix; template using Vector6 = Eigen::Matrix; +template +using Matrix6 = Eigen::Matrix; + template using Matrix6X = Eigen::Matrix; /// double aliases using Affine3d = Affine3; +using Vector4d = Vector4; using Vector3d = Vector3; using Vector2d = Vector2; using VectorXd = VectorX; @@ -47,7 +54,7 @@ using Matrix6Xd = Matrix6X; #if HURON_ENABLE_AUTODIFF==1 #if HURON_USE_CASADI==1 -#include +#include "huron/math/casadi.h" // namespace Eigen { // diff --git a/math/CMakeLists.txt b/math/CMakeLists.txt index 63fdece4..84d0a3bc 100644 --- a/math/CMakeLists.txt +++ b/math/CMakeLists.txt @@ -1,14 +1,12 @@ set(dir "${CMAKE_CURRENT_SOURCE_DIR}") list(APPEND SOURCE_FILES - "${dir}/src/rotation.cc" "${dir}/src/se3.cc") FILE(GLOB_RECURSE HEADER_FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h ) list(APPEND TEST_SOURCE_FILES - "${dir}/test/test_rotation.cc" "${dir}/test/test_se3.cc") set(INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include/") diff --git a/math/include/huron/math/rotation.h b/math/include/huron/math/rotation.h deleted file mode 100644 index 43efdfbb..00000000 --- a/math/include/huron/math/rotation.h +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include -#include - -namespace huron { -namespace math { - -Eigen::Vector3d ZyxToRpy( - const Eigen::Ref& zyx); - -Eigen::Vector3d RpyToZyx( - const Eigen::Ref& zyx); - -Eigen::Matrix3d ZyxToRotationMatrix( - const Eigen::Ref& zyx); - -Eigen::Vector3d RotationMatrixToZyx( - const Eigen::Ref& rotation_matrix); - -Eigen::Matrix3d RpyToRotationMatrix( - const Eigen::Ref& rpy); - -Eigen::Vector3d RotationMatrixToRpy( - const Eigen::Ref& rotation_matrix); - - -} // namespace math -} // namespace huron diff --git a/math/include/huron/math/se3.h b/math/include/huron/math/se3.h index 6e03ba51..72cd7d82 100644 --- a/math/include/huron/math/se3.h +++ b/math/include/huron/math/se3.h @@ -24,27 +24,45 @@ class SE3 { return ret; } + huron::Vector3 operator*(const huron::Vector3& v) const { + huron::Vector4 v_homogeneous; + v_homogeneous << v, 1; + return (tf_ * v_homogeneous).template head<3>(); + } + SE3& operator*=(const SE3& other) { tf_ *= other.tf_; return *this; } + inline friend bool operator==(const SE3& lhs, const SE3& rhs); + inline friend bool operator!=(const SE3& lhs, const SE3& rhs); + Matrix4& matrix() { return tf_; } const Matrix4& matrix() const { return tf_; } Matrix3 rotation() const; Vector3 translation() const; - void rotate(const Matrix3& R); - void prerotate(const Matrix3& R); - void translate(const Vector3& v); + void Rotate(const Matrix3& R); + void Prerotate(const Matrix3& R); + void Translate(const Vector3& v); - SE3 inverse() const; + SE3 Inverse() const; + + huron::Matrix6 AdjointAction() const; private: Matrix4 tf_; }; +inline bool operator==(const SE3& lhs, const SE3& rhs) { + return lhs.tf_ == rhs.tf_; +} +inline bool operator!=(const SE3& lhs, const SE3& rhs) { + return !(lhs == rhs); +} + } // namespace huron HURON_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( diff --git a/math/src/rotation.cc b/math/src/rotation.cc deleted file mode 100644 index 2a677a67..00000000 --- a/math/src/rotation.cc +++ /dev/null @@ -1,62 +0,0 @@ -#include "huron/math/rotation.h" - -namespace huron { -namespace math { - -Eigen::Vector3d ZyxToRpy( - const Eigen::Ref& zyx) { - return zyx.reverse(); -} - -Eigen::Vector3d RpyToZyx( - const Eigen::Ref& rpy) { - return rpy.reverse(); -} - -Eigen::Matrix3d ZyxToRotationMatrix( - const Eigen::Ref& zyx) { - using std::cos, std::sin; - double a = zyx(0), b = zyx(1), g = zyx(2); - double ca = cos(a), cb = cos(b), cg = cos(g); - double sa = sin(a), sb = sin(b), sg = sin(g); - return (Eigen::Matrix3d() << ca*cb, - ca*sb*sg - sa*cg, - ca*sb*cg + sa*sg, - - sa*cb, - sa*sb*sg + ca*cg, - sa*sb*cg - ca*sg, - - -sb, - cb*sg, - cb*cg).finished(); -} - -Eigen::Vector3d RotationMatrixToZyx( - const Eigen::Ref& rotation_matrix) { - using std::atan2; - Eigen::Vector3d zyx; - if (rotation_matrix(0, 0) == 0.0 && rotation_matrix(1, 0) == 0.0) { - zyx << 0, M_PI_2, std::atan2(rotation_matrix(0, 1), rotation_matrix(1, 1)); - } else { - zyx << atan2(rotation_matrix(1, 0), rotation_matrix(0, 0)), - atan2(-rotation_matrix(2, 0), - rotation_matrix.block<2, 1>(0, 0).norm()), - atan2(rotation_matrix(2, 1), rotation_matrix(2, 2)); - } - return zyx; -} - -Eigen::Matrix3d RpyToRotationMatrix( - const Eigen::Ref& rpy) { - return ZyxToRotationMatrix(rpy.reverse()); -} - -Eigen::Vector3d RotationMatrixToRpy( - const Eigen::Ref& rotation_matrix) { - return RotationMatrixToZyx(rotation_matrix).reverse(); -} - - -} // namespace math -} // namespace huron diff --git a/math/src/se3.cc b/math/src/se3.cc index 997147f9..490ee13d 100644 --- a/math/src/se3.cc +++ b/math/src/se3.cc @@ -1,4 +1,5 @@ #include "huron/math/se3.h" +#include "huron/math/utils.h" namespace huron { @@ -10,8 +11,8 @@ SE3::SE3(const Eigen::Matrix4& tf) : tf_(tf) {} template SE3::SE3(const Eigen::Matrix3& R, const Eigen::Vector3& t) { - tf_.block(0, 0, 3, 3) = R; - tf_.block(0, 3, 3, 1) = t; + tf_.topLeftCorner(3, 3) = R; + tf_.topRightCorner(3, 1) = t; tf_.row(3) << 0, 0, 0, 1; } @@ -20,35 +21,45 @@ SE3::SE3(const SE3& other) : tf_(other.tf_) {} template Matrix3 SE3::rotation() const { - return tf_.block(0, 0, 3, 3); + return tf_.topLeftCorner(3, 3); } template Vector3 SE3::translation() const { - return tf_.block(0, 3, 3, 1); + return tf_.topRightCorner(3, 1); } template -void SE3::rotate(const Matrix3& R) { - tf_.block(0, 0, 3, 3) = tf_.block(0, 0, 3, 3); +void SE3::Rotate(const Matrix3& R) { + tf_.template topLeftCorner<3, 3>() *= R; } template -void SE3::prerotate(const Matrix3& R) { +void SE3::Prerotate(const Matrix3& R) { Matrix4 R_tf = Matrix4::Identity(); - R_tf.block(0, 0, 3, 3) = R; + R_tf.template topLeftCorner<3, 3>() = R; tf_ = R_tf * tf_; } template -void SE3::translate(const Vector3& v) { - tf_.block(0, 3, 3, 1) += v; +void SE3::Translate(const Vector3& v) { + tf_.topRightCorner(3, 1) += tf_.topLeftCorner(3, 3) * v; } template -SE3 SE3::inverse() const { - SE3 ret; - ret.tf_ = tf_.inverse(); +SE3 SE3::Inverse() const { + Matrix4 ret_tf = Matrix4::Identity(); + ret_tf.topLeftCorner(3, 3) = tf_.topLeftCorner(3, 3).transpose(); + ret_tf.topRightCorner(3, 1) = -tf_.topLeftCorner(3, 3).transpose() * tf_.topRightCorner(3, 1); + return SE3(ret_tf); +} + +template +Matrix6 SE3::AdjointAction() const { + Matrix6 ret = Matrix6::Zero(); + ret.template topLeftCorner<3, 3>() = rotation(); + ret.template topRightCorner<3, 3>() = huron::skew(translation()) * rotation(); + ret.template bottomRightCorner<3, 3>() = rotation(); return ret; } diff --git a/math/test/test_rotation.cc b/math/test/test_rotation.cc deleted file mode 100644 index 47720b80..00000000 --- a/math/test/test_rotation.cc +++ /dev/null @@ -1,51 +0,0 @@ -#include -#include -#include "huron/math/rotation.h" - -using namespace huron::math; //NOLINT - -TEST(MathRotationTest, ZeroRotation) { - double tolerance = 0.0001; - Eigen::Vector3d angles; - angles.setZero(); - Eigen::Matrix3d identity; - identity.setIdentity(); - - EXPECT_EQ(RpyToRotationMatrix(angles), - Eigen::Matrix3d::Identity()); - EXPECT_EQ(ZyxToRotationMatrix(angles), - Eigen::Matrix3d::Identity()); - EXPECT_EQ(RotationMatrixToRpy(identity), - angles); - EXPECT_EQ(RotationMatrixToZyx(identity), - angles); - EXPECT_LE((RotationMatrixToZyx(ZyxToRotationMatrix(angles)) - angles).norm(), - tolerance); -} - -TEST(MathRotationTest, HalfPiRotation) { - double tolerance = 0.0001; - Eigen::Vector3d angles; - angles << M_PI_2, M_PI_2, M_PI_2; - Eigen::Matrix3d expected; - expected << 0.0, 0.0, 1.0, - 0.0, 1.0, 0.0, - -1.0, 0.0, 0.0; - - EXPECT_LE((huron::math::ZyxToRotationMatrix(angles) - expected).norm(), - tolerance); -} - -TEST(MathRotationTest, ArbitraryRotation) { - double tolerance = 0.0001; - Eigen::Vector3d angles; - angles << 0.957506835434298, - 0.546881519204984, - 0.278498218867048; - Eigen::Matrix3d expected; - expected << 0.491615279747179, -0.703967407781224, 0.512585900606803, - 0.698489270695994, 0.670291899863360, 0.250642190582803, - -0.520026110346088, 0.234816221244084, 0.821239421118450; - EXPECT_LE((huron::math::ZyxToRotationMatrix(angles) - expected).norm(), - tolerance); -} diff --git a/math/test/test_se3.cc b/math/test/test_se3.cc index a64facb8..df09246f 100644 --- a/math/test/test_se3.cc +++ b/math/test/test_se3.cc @@ -15,45 +15,78 @@ class TestSe3 : public testing::Test { }; TEST_F(TestSe3, Identity) { - EXPECT_EQ(huron::SE3().matrix(), Eigen::Matrix4d::Identity()); + EXPECT_TRUE(huron::SE3().matrix().isApprox( + Eigen::Matrix4d::Identity())); } TEST_F(TestSe3, InitFromMatrix) { - Eigen::Affine3d tf_gt; + Eigen::Isometry3d tf_gt = Eigen::Isometry3d::Identity(); tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); - EXPECT_EQ(huron::SE3(tf_gt.matrix()).matrix(), tf_gt.matrix()); + EXPECT_TRUE(huron::SE3(tf_gt.matrix()).matrix().isApprox( + tf_gt.matrix())); } TEST_F(TestSe3, InitFromRotationTranslation) { - Eigen::Affine3d tf_gt; + Eigen::Isometry3d tf_gt = Eigen::Isometry3d::Identity(); tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); - EXPECT_EQ(huron::SE3(tf_gt.rotation().matrix(), - tf_gt.translation().matrix()).matrix(), - tf_gt.matrix()); + huron::SE3 tf(tf_gt.rotation().matrix(), + tf_gt.translation().matrix()); + EXPECT_TRUE(tf.matrix().isApprox(tf_gt.matrix())); +} + +TEST_F(TestSe3, Comparison) { + Eigen::Isometry3d tf_gt = Eigen::Isometry3d::Identity(); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + huron::SE3 tf(tf_gt.matrix()); + huron::SE3 tf2(tf_gt.matrix()); + huron::SE3 tf3; + EXPECT_TRUE(tf == tf2); + EXPECT_FALSE(tf == tf3); + EXPECT_FALSE(tf != tf2); + EXPECT_TRUE(tf != tf3); +} + +TEST_F(TestSe3, Assignment) { + Eigen::Isometry3d tf_gt = Eigen::Isometry3d::Identity(); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + huron::SE3 tf(tf_gt.matrix()); + huron::SE3 tf2, tf3; + EXPECT_TRUE(tf2 == tf3); + tf2 = tf; + EXPECT_TRUE(tf2 != tf3); + EXPECT_TRUE(tf2.matrix().isApprox(tf_gt.matrix())); } TEST_F(TestSe3, Translation) { // Creating a random ground truth transformation - Eigen::Affine3d tf_gt; - tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + Eigen::Isometry3d tf_gt = Eigen::Isometry3d::Identity(); + Eigen::Vector3d t(uniform_dist(re), uniform_dist(re), uniform_dist(re)); + tf_gt.translate(t); // Test object huron::SE3 tf; - tf.translate(tf_gt.translation().matrix()); + tf.Translate(t); - EXPECT_EQ(tf.matrix(), tf_gt.matrix()); + EXPECT_TRUE(tf.matrix().isApprox(tf_gt.matrix())); } TEST_F(TestSe3, Prerotation) { // Creating a random ground truth transformation - std::uniform_real_distribution uniform_dist(-3.14159, 3.14159); - std::default_random_engine re; - Eigen::Affine3d a, b, tf_gt; + Eigen::Isometry3d a, b, tf_gt; + a.setIdentity(); + b.setIdentity(); + tf_gt.setIdentity(); a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); @@ -64,16 +97,18 @@ TEST_F(TestSe3, Prerotation) { tf_gt.prerotate(b.rotation()); // Test object huron::SE3 tf(a.matrix());; - tf.prerotate(b.rotation().matrix()); + tf.Prerotate(b.rotation().matrix()); - EXPECT_EQ(tf.matrix(), tf_gt.matrix()); + EXPECT_TRUE(tf.matrix().isApprox(tf_gt.matrix())); } TEST_F(TestSe3, Rotation) { // Creating a random ground truth transformation - std::uniform_real_distribution uniform_dist(-3.14159, 3.14159); - std::default_random_engine re; - Eigen::Affine3d a, b, tf_gt; + Eigen::Isometry3d a, b, tf_gt; + a.setIdentity(); + b.setIdentity(); + tf_gt.setIdentity(); + std::cout << b.matrix() << std::endl; a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); @@ -84,29 +119,39 @@ TEST_F(TestSe3, Rotation) { tf_gt.rotate(b.rotation()); // Test object huron::SE3 tf(a.matrix());; - tf.rotate(b.rotation().matrix()); + tf.Rotate(b.rotation()); - EXPECT_EQ(tf.matrix(), tf_gt.matrix()); + EXPECT_TRUE(tf.matrix().isApprox(tf_gt.matrix())); } TEST_F(TestSe3, TransRot) { // Creating a random ground truth transformation - Eigen::Affine3d tf_gt; - tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); - tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); - tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); - tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + Eigen::Isometry3d tf_gt; + tf_gt.setIdentity(); + Eigen::Matrix3d R1 = Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX()).toRotationMatrix(); + Eigen::Matrix3d R2 = Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY()).toRotationMatrix(); + Eigen::Matrix3d R3 = Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ()).toRotationMatrix(); + Eigen::Vector3d t(uniform_dist(re), uniform_dist(re), uniform_dist(re)); + tf_gt.rotate(R1); + tf_gt.rotate(R2); + tf_gt.rotate(R3); + tf_gt.translate(t); // Test object huron::SE3 tf; - tf.rotate(tf_gt.rotation().matrix()); - tf.translate(tf_gt.translation().matrix()); + tf.Rotate(R1); + tf.Rotate(R2); + tf.Rotate(R3); + tf.Translate(t); - EXPECT_EQ(tf.matrix(), tf_gt.matrix()); + EXPECT_TRUE(tf.matrix().isApprox(tf_gt.matrix())); } TEST_F(TestSe3, Multiplication) { // Creating a random ground truth transformation - Eigen::Affine3d a, b, tf_gt; + Eigen::Isometry3d a, b, tf_gt; + a.setIdentity(); + b.setIdentity(); + tf_gt.setIdentity(); a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); a.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); @@ -120,6 +165,45 @@ TEST_F(TestSe3, Multiplication) { huron::SE3 tfa(a.matrix()), tfb(b.matrix()), tf; tf = tfa * tfb; - EXPECT_EQ(tf.matrix(), tf_gt.matrix()); - EXPECT_EQ((tfa *= tfb).matrix(), tf_gt.matrix()); + EXPECT_TRUE(tf.matrix().isApprox(tf_gt.matrix())); + EXPECT_TRUE((tfa *= tfb).matrix().isApprox(tf_gt.matrix())); +} + +TEST_F(TestSe3, Inverse) { + Eigen::Isometry3d tf_gt; + tf_gt.setIdentity(); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + huron::SE3 tf(tf_gt.matrix()); + EXPECT_TRUE(tf.Inverse().matrix().isApprox(tf_gt.inverse().matrix())); +} + +TEST_F(TestSe3, TransformVector) { + Eigen::Isometry3d tf_gt; + tf_gt.setIdentity(); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitX())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitY())); + tf_gt.rotate(Eigen::AngleAxisd(uniform_dist(re), Eigen::Vector3d::UnitZ())); + tf_gt.translate(Eigen::Vector3d(uniform_dist(re), uniform_dist(re), uniform_dist(re))); + huron::SE3 tf(tf_gt.matrix()); + Eigen::Vector3d v; + v << uniform_dist(re), uniform_dist(re), uniform_dist(re); + EXPECT_TRUE((tf * v).isApprox(tf_gt * v)); +} + +TEST_F(TestSe3, TransformWrench) { + Eigen::Matrix4d tf = Eigen::Matrix4d::Identity(); + tf.topRightCorner<1, 1>() << -6.0; + huron::Vector6d w; + w << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0; + + huron::Vector6d expected; + expected << 0.0, -5.0, 0.0, 0.0, 0.0, 30.0; + + huron::SE3 se3(tf); + huron::Vector6d computed = se3.Inverse().AdjointAction().transpose() * w; + + EXPECT_TRUE(computed.isApprox(expected)); } diff --git a/multibody/src/logical_frame.cc b/multibody/src/logical_frame.cc index 45f1358e..17651560 100644 --- a/multibody/src/logical_frame.cc +++ b/multibody/src/logical_frame.cc @@ -41,14 +41,14 @@ template huron::SE3 LogicalFrame::GetTransformToFrame(const Frame& other) const { huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(parent_frame_index_, other.index()); - return transform_function_(parent_transform).inverse() * parent_transform; + return transform_function_(parent_transform).Inverse() * parent_transform; } template huron::SE3 LogicalFrame::GetTransformToFrame(FrameIndex other) const { huron::SE3 parent_transform = this->model_.lock()->GetFrameTransform(parent_frame_index_, other); - return transform_function_(parent_transform).inverse() * parent_transform; + return transform_function_(parent_transform).Inverse() * parent_transform; } } // namespace multibody diff --git a/multibody/src/pinocchio_model_impl.cc b/multibody/src/pinocchio_model_impl.cc index 6ff3b92c..fc4c1ebd 100644 --- a/multibody/src/pinocchio_model_impl.cc +++ b/multibody/src/pinocchio_model_impl.cc @@ -21,9 +21,7 @@ namespace helpers { template huron::SE3 PinSe3ToHuronSe3(const pinocchio::SE3Tpl& se3) { - huron::SE3 ret; - ret.rotate(se3.rotation()); - ret.translate(se3.translation()); + huron::SE3 ret(se3.toHomogeneousMatrix()); return ret; } @@ -207,7 +205,7 @@ template huron::SE3 PinocchioModelImpl::GetFrameTransform(FrameIndex from_frame, FrameIndex to_frame) const { - return GetFrameTransformInWorld(from_frame).inverse() * + return GetFrameTransformInWorld(from_frame).Inverse() * GetFrameTransformInWorld(to_frame); } diff --git a/multibody/test/test_model.cc b/multibody/test/test_model.cc index 0f9859d8..06db5faa 100644 --- a/multibody/test/test_model.cc +++ b/multibody/test/test_model.cc @@ -58,7 +58,7 @@ TEST_F(TestModelPinocchio, TestKinematics) { SE3 expected_link1_pose(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, 0.1, 1.95)); SE3 expected_link2_pose = expected_link1_pose; - expected_link2_pose.translate(Eigen::Vector3d(0, 0.1, 0.9)); + expected_link2_pose.Translate(Eigen::Vector3d(0, 0.1, 0.9)); robot.GetModel()->UpdateJointStates(); robot.GetModel()->ForwardKinematics(); diff --git a/system/control_interfaces/test/test_legged_zmp.cc b/system/control_interfaces/test/test_legged_zmp.cc index 24bc5dc9..c109ca50 100644 --- a/system/control_interfaces/test/test_legged_zmp.cc +++ b/system/control_interfaces/test/test_legged_zmp.cc @@ -98,7 +98,7 @@ class TestLeggedZmp : public testing::Test { } double normal_force_threshold = 0.01; - double tolerance = 0.0005; + double tolerance = 0.01; TestLeggedRobot robot; std::vector>> ft_sensor_list; @@ -166,6 +166,8 @@ TEST_F(TestLeggedZmp, TestBigNormalForce) { auto total_zmp_val = robot.EvalZeroMomentPoint(); std::cout << (total_zmp_val - com.segment(0, 2)).transpose() << std::endl; + std::cout << "Computed zmp: " << total_zmp_val.transpose() << std::endl; + std::cout << "Expected zmp: " << expected_zmp.transpose() << std::endl; EXPECT_LE((total_zmp_val - expected_zmp).norm(), tolerance); } @@ -187,7 +189,6 @@ TEST_F(TestLeggedZmp, TestBigNormalForceXY) { auto com = robot.GetModel()->EvalCenterOfMassPosition(); auto total_zmp_val = robot.EvalZeroMomentPoint(); - std::cout << (total_zmp_val - com.segment(0, 2)).transpose() << std::endl; std::cout << total_zmp_val.transpose() << std::endl; diff --git a/system/locomotion/src/zero_moment_point.cc b/system/locomotion/src/zero_moment_point.cc index dd9ce011..d9e7a116 100644 --- a/system/locomotion/src/zero_moment_point.cc +++ b/system/locomotion/src/zero_moment_point.cc @@ -14,8 +14,8 @@ ZeroMomentPoint::ZeroMomentPoint( template huron::SE3 ZeroMomentPoint::ZmpToWorld(const huron::Vector2& zmp) const { - huron::SE3 ret; - ret.translate(huron::Vector3(zmp.x(), zmp.y(), 0.0)); + huron::SE3 ret(Matrix3::Identity(), + huron::Vector3(zmp.x(), zmp.y(), 0.0)); return ret; } diff --git a/system/locomotion/src/zero_moment_point_ft_sensor.cc b/system/locomotion/src/zero_moment_point_ft_sensor.cc index 0204c001..45ec74b1 100644 --- a/system/locomotion/src/zero_moment_point_ft_sensor.cc +++ b/system/locomotion/src/zero_moment_point_ft_sensor.cc @@ -12,7 +12,6 @@ ZeroMomentPointFTSensor::ZeroMomentPointFTSensor( ft_sensors_(ft_sensors) { } -// Generate Doxygen documentation for the following function template huron::Vector2 ZeroMomentPointFTSensor::Eval(T& fz) { huron::Vector2 zmp; @@ -22,8 +21,7 @@ huron::Vector2 ZeroMomentPointFTSensor::Eval(T& fz) { *ft_sensor->GetSensorFrame().lock()); huron::SE3 zmp_frame_pose = this->zmp_frame_.lock()->GetTransformInWorld(); huron::Vector6 w = ft_sensor->GetValue(); - w.segment(0, 3) = zmp_to_sensor.rotation() * w.segment(0, 3); - w.segment(3, 3) = zmp_to_sensor.rotation() * w.segment(3, 3); + w = zmp_to_sensor.Inverse().AdjointAction().transpose() * w; num_x += (-w(4) - w(0)*(zmp_to_sensor.translation().z() - zmp_frame_pose.translation().z()) @@ -54,8 +52,7 @@ ZeroMomentPointFTSensor::Eval(casadi::SX& fz) { *ft_sensor->GetSensorFrame().lock()); huron::SE3 zmp_frame_pose = this->zmp_frame_.lock()->GetTransformInWorld(); huron::Vector6 w = ft_sensor->GetValue(); - w.segment(0, 3) = zmp_to_sensor.rotation() * w.segment(0, 3); - w.segment(3, 3) = zmp_to_sensor.rotation() * w.segment(3, 3); + w = zmp_to_sensor.Inverse().AdjointAction().transpose() * w; num_x += (-w(4) - w(0)*(zmp_to_sensor.translation().z() - zmp_frame_pose.translation().z()) diff --git a/system/locomotion/test/test_zero_moment_point.cc b/system/locomotion/test/test_zero_moment_point.cc index 4cb526be..388db31d 100644 --- a/system/locomotion/test/test_zero_moment_point.cc +++ b/system/locomotion/test/test_zero_moment_point.cc @@ -100,7 +100,7 @@ class TestZeroMomentPointFt : public testing::Test { } double normal_force_threshold = 0.01; - double tolerance = 0.0005; + double tolerance = 0.01; TestRobot robot; std::weak_ptr> zmp_frame; @@ -172,6 +172,9 @@ TEST_F(TestZeroMomentPointFt, TestBigNormalForce) { auto total_zmp_val = total_zmp->Eval(computed_fz); + std::cout << "Computed zmp: " << total_zmp_val.transpose() << std::endl; + std::cout << "Expected zmp: " << expected_zmp.transpose() << std::endl; + EXPECT_LE((total_zmp_val - expected_zmp).norm(), tolerance); EXPECT_DOUBLE_EQ(computed_fz, expected_fz); }