diff --git a/CHANGELOG.md b/CHANGELOG.md index 4fe4778..eb86bf1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,32 +4,32 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). -## [Unreleased] +## \[Unreleased\] -* Supported fixed-based robots in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/17 -* Add inertial parameters publisher/subscriber and extend whole-body state publisher with accelerations in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/16 +- Supported fixed-based robots in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/17 +- Add inertial parameters publisher/subscriber and extend whole-body state publisher with accelerations in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/16 -## [1.3.1] - 2024-01-27 +## \[1.3.1\] - 2024-01-27 -* Enabled to publish whole-body state and trajectory without the need of passing contact info in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/15 +- Enabled to publish whole-body state and trajectory without the need of passing contact info in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/15 -## [1.2.1] - 2023-12-13 +## \[1.2.1\] - 2023-12-13 -* Extended CI jobs in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/12 +- Extended CI jobs in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/12 -## [1.2.0] - 2023-09-22 +## \[1.2.0\] - 2023-09-22 -* Fixed bug in init functions for empty locked joints in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/11 -* Introduced locked joints in publishers and subscribers in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/10 +- Fixed bug in init functions for empty locked joints in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/11 +- Introduced locked joints in publishers and subscribers in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/10 -## [1.0.1] - 2023-08-24 +## \[1.0.1\] - 2023-08-24 -* Used ROS to print starting message and fixed bug when using reduced models in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/9 -* Developed other unit tests with Pinocchio in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/8 +- Used ROS to print starting message and fixed bug when using reduced models in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/9 +- Developed other unit tests with Pinocchio in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/8 -## [1.0.0] - 2023-07-07 +## \[1.0.0\] - 2023-07-07 -* Supported ROS 2 in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/7 -* Integrated pre-commit in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/6 -* Replaced nv_root finding via frames to using the first joint in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/4 -* Integrated CI in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/3 +- Supported ROS 2 in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/7 +- Integrated pre-commit in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/6 +- Replaced nv_root finding via frames to using the first joint in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/4 +- Integrated CI in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/3 diff --git a/include/crocoddyl_msgs/conversions.h b/include/crocoddyl_msgs/conversions.h index 8802068..fc15ffe 100644 --- a/include/crocoddyl_msgs/conversions.h +++ b/include/crocoddyl_msgs/conversions.h @@ -107,7 +107,9 @@ template class JointCollectionTpl> static inline std::size_t getRootNq( const pinocchio::ModelTpl &model) { const std::size_t root_joint_id = getRootJointId(model); - return model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nq() : 0; + return model.frames[root_joint_id].name != "universe" + ? model.joints[root_joint_id].nq() + : 0; } /** @@ -119,7 +121,9 @@ template class JointCollectionTpl> static inline std::size_t getRootNv( const pinocchio::ModelTpl &model) { const std::size_t root_joint_id = getRootJointId(model); - return model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nv() : 0; + return model.frames[root_joint_id].name != "universe" + ? model.joints[root_joint_id].nv() + : 0; } /** @@ -139,48 +143,58 @@ static inline std::size_t getRootNv( template class JointCollectionTpl> void updateBodyInertialParameters( pinocchio::ModelTpl &model, - const std::string &frame_name, - const Eigen::Ref &psi) { + const std::string &frame_name, const Eigen::Ref &psi) { if (model.existFrame(frame_name)) { const std::size_t frame_id = model.getFrameId(frame_name); switch (model.frames[frame_id].type) { - case pinocchio::FrameType::JOINT: { + case pinocchio::FrameType::JOINT: { const std::size_t joint_id = model.getJointId(frame_name); model.inertias[joint_id] = pinocchio::Inertia::FromDynamicParameters(psi); - } break; - case pinocchio::FrameType::BODY: { -//TODO: Update Pinocchio version after releasing https://github.com/stack-of-tasks/pinocchio/pull/2204 -#if (PINOCCHIO_MAJOR_VERSION >= 2 && PINOCCHIO_MINOR_VERSION >= 7 && PINOCCHIO_PATCH_VERSION >= 1) + } break; + case pinocchio::FrameType::BODY: { +// TODO: Update Pinocchio version after releasing +// https://github.com/stack-of-tasks/pinocchio/pull/2204 +#if (PINOCCHIO_MAJOR_VERSION >= 2 && PINOCCHIO_MINOR_VERSION >= 7 && \ + PINOCCHIO_PATCH_VERSION >= 1) const std::size_t fixed_joint_id = model.frames[frame_id].previousFrame; const std::size_t joint_id = model.frames[fixed_joint_id].parent; - const pinocchio::SE3& jMb = model.frames[fixed_joint_id].placement; - const pinocchio::Inertia& I_updated = pinocchio::Inertia::FromDynamicParameters(psi); - const pinocchio::Inertia& dI = I_updated - model.frames[fixed_joint_id].inertia; + const pinocchio::SE3 &jMb = model.frames[fixed_joint_id].placement; + const pinocchio::Inertia &I_updated = + pinocchio::Inertia::FromDynamicParameters(psi); + const pinocchio::Inertia &dI = + I_updated - model.frames[fixed_joint_id].inertia; model.frames[fixed_joint_id].inertia = I_updated; model.inertias[joint_id] += jMb.act(dI); #else - std::invalid_argument("The installed Pinocchio version doesn't support minus operators in inertia needed for " + frame_name); + std::invalid_argument("The installed Pinocchio version doesn't support " + "minus operators in inertia needed for " + + frame_name); #endif - } break; - case pinocchio::FrameType::FIXED_JOINT: { -#if (PINOCCHIO_MAJOR_VERSION >= 2 && PINOCCHIO_MINOR_VERSION >= 7 && PINOCCHIO_PATCH_VERSION >= 1) + } break; + case pinocchio::FrameType::FIXED_JOINT: { +#if (PINOCCHIO_MAJOR_VERSION >= 2 && PINOCCHIO_MINOR_VERSION >= 7 && \ + PINOCCHIO_PATCH_VERSION >= 1) const std::size_t joint_id = model.frames[frame_id].parent; - const pinocchio::SE3& jMb = model.frames[frame_id].placement; - const pinocchio::Inertia& I_updated = pinocchio::Inertia::FromDynamicParameters(psi); - const pinocchio::Inertia& dI = I_updated - model.frames[frame_id].inertia; + const pinocchio::SE3 &jMb = model.frames[frame_id].placement; + const pinocchio::Inertia &I_updated = + pinocchio::Inertia::FromDynamicParameters(psi); + const pinocchio::Inertia &dI = I_updated - model.frames[frame_id].inertia; model.frames[frame_id].inertia = I_updated; model.inertias[joint_id] += jMb.act(dI); #else - std::invalid_argument("The installed Pinocchio version doesn't support minus operators in inertia needed for " + frame_name); + std::invalid_argument("The installed Pinocchio version doesn't support " + "minus operators in inertia needed for " + + frame_name); #endif - } break; - default: { - std::invalid_argument("The type of frame " + frame_name + " is not supported"); - break; - } + } break; + default: { + std::invalid_argument("The type of frame " + frame_name + + " is not supported"); + break; + } } } else { - std::invalid_argument("Doesn't exist " + frame_name + " frame"); + std::invalid_argument("Doesn't exist " + frame_name + " frame"); } } @@ -193,37 +207,37 @@ void updateBodyInertialParameters( * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the * barycenter. Additionally, the type of frame supported are joints, * fixed joints, and bodies - * + * * @param model[in] Pinocchio model * @param frame_name[in] Frame name * @return inertial parameters. */ template class JointCollectionTpl> -const Vector10d -getBodyInertialParameters( +const Vector10d getBodyInertialParameters( const pinocchio::ModelTpl &model, const std::string &frame_name) { if (model.existFrame(frame_name)) { const std::size_t frame_id = model.getFrameId(frame_name); switch (model.frames[frame_id].type) { - case pinocchio::FrameType::JOINT: { + case pinocchio::FrameType::JOINT: { const std::size_t joint_id = model.getJointId(frame_name); return model.inertias[joint_id].toDynamicParameters(); - } break; - case pinocchio::FrameType::BODY: { + } break; + case pinocchio::FrameType::BODY: { const std::size_t fixed_joint_id = model.frames[frame_id].previousFrame; return model.frames[fixed_joint_id].inertia.toDynamicParameters(); - } break; - case pinocchio::FrameType::FIXED_JOINT: { + } break; + case pinocchio::FrameType::FIXED_JOINT: { return model.frames[frame_id].inertia.toDynamicParameters(); - } break; - default: { - std::invalid_argument("The type of frame " + frame_name + " is not supported"); - break; - } + } break; + default: { + std::invalid_argument("The type of frame " + frame_name + + " is not supported"); + break; + } } } else { - std::invalid_argument("Doesn't exist " + frame_name + " frame"); + std::invalid_argument("Doesn't exist " + frame_name + " frame"); } } @@ -656,7 +670,7 @@ static inline void fromMsg(const Control &msg, Eigen::Ref u, } /** - * @brief Conversion of inertial parameters from a + * @brief Conversion of inertial parameters from a * crocoddyl_msgs::BodyInertia message to Eigen * * @param[in] msg ROS message that contains the inertial parameters @@ -880,9 +894,9 @@ static inline void fromReduced( " but received " + std::to_string(v_in.size())); } if (static_cast(tau_out.size()) != model.nv - nv_root) { - throw std::invalid_argument( - "Expected tau_out to be " + std::to_string(njoints) + - " but received " + std::to_string(tau_out.size())); + throw std::invalid_argument("Expected tau_out to be " + + std::to_string(njoints) + " but received " + + std::to_string(tau_out.size())); } if (static_cast(tau_in.size()) != njoints_reduced) { throw std::invalid_argument( @@ -979,9 +993,9 @@ static inline void fromReduced( " but received " + std::to_string(a_in.size())); } if (static_cast(tau_out.size()) != njoints) { - throw std::invalid_argument( - "Expected tau_out to be " + std::to_string(njoints) + - " but received " + std::to_string(tau_out.size())); + throw std::invalid_argument("Expected tau_out to be " + + std::to_string(njoints) + " but received " + + std::to_string(tau_out.size())); } if (static_cast(tau_in.size()) != njoints_reduced) { throw std::invalid_argument( @@ -1069,9 +1083,9 @@ toReduced(const pinocchio::ModelTpl &model, " but received " + std::to_string(tau_out.size())); } if (static_cast(tau_in.size()) != njoints) { - throw std::invalid_argument( - "Expected tau_in to be " + std::to_string(njoints) + - " but received " + std::to_string(tau_in.size())); + throw std::invalid_argument("Expected tau_in to be " + + std::to_string(njoints) + " but received " + + std::to_string(tau_in.size())); } typedef pinocchio::ModelTpl Model; @@ -1160,9 +1174,9 @@ toReduced(const pinocchio::ModelTpl &model, " but received " + std::to_string(tau_out.size())); } if (static_cast(tau_in.size()) != njoints) { - throw std::invalid_argument( - "Expected tau_in to be " + std::to_string(njoints) + - " but received " + std::to_string(tau_in.size())); + throw std::invalid_argument("Expected tau_in to be " + + std::to_string(njoints) + " but received " + + std::to_string(tau_in.size())); } typedef pinocchio::ModelTpl Model; diff --git a/include/crocoddyl_msgs/multibody_inertia_publisher.h b/include/crocoddyl_msgs/multibody_inertia_publisher.h index 20c2893..8642d90 100644 --- a/include/crocoddyl_msgs/multibody_inertia_publisher.h +++ b/include/crocoddyl_msgs/multibody_inertia_publisher.h @@ -36,15 +36,13 @@ class MultibodyInertiaRosPublisher { : node_("inertial_parameters_publisher"), pub_(node_.create_publisher(topic, 1)) { RCLCPP_INFO_STREAM(node_.get_logger(), - "Publishing MultibodyInertia messages on " - << topic); + "Publishing MultibodyInertia messages on " << topic); } #else { ros::NodeHandle n; pub_.init(n, topic, 1); - ROS_INFO_STREAM("Publishing MultibodyInertia messages on " - << topic); + ROS_INFO_STREAM("Publishing MultibodyInertia messages on " << topic); } #endif @@ -58,7 +56,7 @@ class MultibodyInertiaRosPublisher { * the first moment of inertial (mass * barycenter) and the rotational * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the * barycenter. - * + * * @param parameters[in] Multibody inertial parameters. */ void publish(const std::map> diff --git a/include/crocoddyl_msgs/multibody_inertia_subscriber.h b/include/crocoddyl_msgs/multibody_inertia_subscriber.h index 833e97b..6476c77 100644 --- a/include/crocoddyl_msgs/multibody_inertia_subscriber.h +++ b/include/crocoddyl_msgs/multibody_inertia_subscriber.h @@ -21,9 +21,11 @@ namespace crocoddyl_msgs { #ifdef ROS2 -typedef const crocoddyl_msgs::msg::MultibodyInertia::SharedPtr MultibodyInertiaSharedPtr; +typedef const crocoddyl_msgs::msg::MultibodyInertia::SharedPtr + MultibodyInertiaSharedPtr; #else -typedef const crocoddyl_msgs::MultibodyInertia::ConstPtr &MultibodyInertiaSharedPtr; +typedef const crocoddyl_msgs::MultibodyInertia::ConstPtr + &MultibodyInertiaSharedPtr; #endif class MultibodyInertiaRosSubscriber { @@ -48,8 +50,7 @@ class MultibodyInertiaRosSubscriber { thread_ = std::thread([this]() { this->spin(); }); thread_.detach(); RCLCPP_INFO_STREAM(node_->get_logger(), - "Subscribing MultibodyInertia messages on " - << topic); + "Subscribing MultibodyInertia messages on " << topic); #else : spinner_(2), has_new_msg_(false), is_processing_msg_(false), last_msg_time_(0.) { @@ -58,8 +59,7 @@ class MultibodyInertiaRosSubscriber { topic, 1, &MultibodyInertiaRosSubscriber::callback, this, ros::TransportHints().tcpNoDelay()); spinner_.start(); - ROS_INFO_STREAM("Subscribing MultibodyInertia messages on " - << topic); + ROS_INFO_STREAM("Subscribing MultibodyInertia messages on " << topic); #endif } @@ -73,7 +73,7 @@ class MultibodyInertiaRosSubscriber { * the first moment of inertial (mass * barycenter) and the rotational * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the * barycenter. - * + * * @return A map from body names to inertial parameters. * */ std::map get_parameters() { @@ -104,14 +104,13 @@ class MultibodyInertiaRosSubscriber { rclcpp::executors::SingleThreadedExecutor spinner_; std::thread thread_; void spin() { spinner_.spin(); } - rclcpp::Subscription::SharedPtr - sub_; //!< ROS subscriber + rclcpp::Subscription::SharedPtr sub_; //!< ROS subscriber #else ros::NodeHandle node_; ros::AsyncSpinner spinner_; ros::Subscriber sub_; //!< ROS subscriber #endif - std::mutex mutex_; ///< Mutex to prevent race condition on callback + std::mutex mutex_; ///< Mutex to prevent race condition on callback MultibodyInertia msg_; //!< ROS message bool has_new_msg_; //!< Indcate when a new message has been received diff --git a/include/crocoddyl_msgs/whole_body_state_publisher.h b/include/crocoddyl_msgs/whole_body_state_publisher.h index a953a64..e6e5b5e 100644 --- a/include/crocoddyl_msgs/whole_body_state_publisher.h +++ b/include/crocoddyl_msgs/whole_body_state_publisher.h @@ -180,7 +180,8 @@ class WholeBodyStateRosPublisher { } /** - * @brief Update the Pinocchio model's inertial parameters of a given body frame + * @brief Update the Pinocchio model's inertial parameters of a given body + * frame * * The inertial parameters vector is defined as [m, h_x, h_y, h_z, * I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is @@ -193,21 +194,21 @@ class WholeBodyStateRosPublisher { * @param body_name[in] Body name * @param psi[in] Inertial parameters */ - void update_body_inertial_parameters( - const std::string &body_name, - const Eigen::Ref &psi) { + void update_body_inertial_parameters(const std::string &body_name, + const Eigen::Ref &psi) { updateBodyInertialParameters(model_, body_name, psi); } /** - * @brief Return the Pinocchio model's inertial parameters of a given body frame + * @brief Return the Pinocchio model's inertial parameters of a given body + * frame * * The inertial parameters vector is defined as [m, h_x, h_y, h_z, * I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is * the first moment of inertial (mass * barycenter) and the rotational * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the * barycenter. - * + * * @param body_name[in] Body name */ const Vector10d diff --git a/include/crocoddyl_msgs/whole_body_state_subscriber.h b/include/crocoddyl_msgs/whole_body_state_subscriber.h index 5c72e20..b267533 100644 --- a/include/crocoddyl_msgs/whole_body_state_subscriber.h +++ b/include/crocoddyl_msgs/whole_body_state_subscriber.h @@ -174,7 +174,8 @@ class WholeBodyStateRosSubscriber { bool has_new_msg() const { return has_new_msg_; } /** - * @brief Update the Pinocchio model's inertial parameters of a given body frame + * @brief Update the Pinocchio model's inertial parameters of a given body + * frame * * The inertial parameters vector is defined as [m, h_x, h_y, h_z, * I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is @@ -187,21 +188,21 @@ class WholeBodyStateRosSubscriber { * @param body_name[in] Body name * @param psi[in] Inertial parameters */ - void update_body_inertial_parameters( - const std::string &body_name, - const Eigen::Ref &psi) { + void update_body_inertial_parameters(const std::string &body_name, + const Eigen::Ref &psi) { updateBodyInertialParameters(model_, body_name, psi); } /** - * @brief Return the Pinocchio model's inertial parameters of a given body frame + * @brief Return the Pinocchio model's inertial parameters of a given body + * frame * * The inertial parameters vector is defined as [m, h_x, h_y, h_z, * I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is * the first moment of inertial (mass * barycenter) and the rotational * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the * barycenter. - * + * * @param body_name[in] Body name */ const Vector10d @@ -290,8 +291,10 @@ class WholeBodyStateRosSubscriber { } // Check that locked joint exists to update q and v appropriately if (inexistent_joints != 0) { - q_ = Eigen::VectorXd::Zero(model_.nq - locked_joints.size() + inexistent_joints); - v_ = Eigen::VectorXd::Zero(model_.nv - locked_joints.size() + inexistent_joints); + q_ = Eigen::VectorXd::Zero(model_.nq - locked_joints.size() + + inexistent_joints); + v_ = Eigen::VectorXd::Zero(model_.nv - locked_joints.size() + + inexistent_joints); } pinocchio::buildReducedModel(model_, joint_ids_, qref_, reduced_model_); diff --git a/include/crocoddyl_msgs/whole_body_trajectory_publisher.h b/include/crocoddyl_msgs/whole_body_trajectory_publisher.h index c1d9a90..fb48545 100644 --- a/include/crocoddyl_msgs/whole_body_trajectory_publisher.h +++ b/include/crocoddyl_msgs/whole_body_trajectory_publisher.h @@ -224,21 +224,21 @@ class WholeBodyTrajectoryRosPublisher { * @param body_name[in] Body name * @param psi[in] Inertial parameters */ - void update_body_inertial_parameters( - const std::string &body_name, - const Eigen::Ref &psi) { + void update_body_inertial_parameters(const std::string &body_name, + const Eigen::Ref &psi) { updateBodyInertialParameters(model_, body_name, psi); } /** - * @brief Return the Pinocchio model's inertial parameters of a given body frame + * @brief Return the Pinocchio model's inertial parameters of a given body + * frame * * The inertial parameters vector is defined as [m, h_x, h_y, h_z, * I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is * the first moment of inertial (mass * barycenter) and the rotational * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the * barycenter. - * + * * @param body_name[in] Body name */ const Vector10d diff --git a/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h b/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h index f9cd694..bfe66b3 100644 --- a/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h +++ b/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h @@ -202,21 +202,21 @@ class WholeBodyTrajectoryRosSubscriber { * @param body_name[in] Body name * @param psi[in] Inertial parameters */ - void update_body_inertial_parameters( - const std::string &body_name, - const Eigen::Ref &psi) { + void update_body_inertial_parameters(const std::string &body_name, + const Eigen::Ref &psi) { updateBodyInertialParameters(model_, body_name, psi); } /** - * @brief Return the Pinocchio model's inertial parameters of a given body frame + * @brief Return the Pinocchio model's inertial parameters of a given body + * frame * * The inertial parameters vector is defined as [m, h_x, h_y, h_z, * I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is * the first moment of inertial (mass * barycenter) and the rotational * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the * barycenter. - * + * * @param body_name[in] Body name */ const Vector10d diff --git a/src/crocoddyl_ros.cpp b/src/crocoddyl_ros.cpp index dba0b7d..787c692 100644 --- a/src/crocoddyl_ros.cpp +++ b/src/crocoddyl_ros.cpp @@ -24,6 +24,8 @@ #include #endif +#include "crocoddyl_msgs/multibody_inertia_publisher.h" +#include "crocoddyl_msgs/multibody_inertia_subscriber.h" #include "crocoddyl_msgs/solver_statistics_publisher.h" #include "crocoddyl_msgs/solver_statistics_subscriber.h" #include "crocoddyl_msgs/solver_trajectory_publisher.h" @@ -32,8 +34,6 @@ #include "crocoddyl_msgs/whole_body_state_subscriber.h" #include "crocoddyl_msgs/whole_body_trajectory_publisher.h" #include "crocoddyl_msgs/whole_body_trajectory_subscriber.h" -#include "crocoddyl_msgs/multibody_inertia_publisher.h" -#include "crocoddyl_msgs/multibody_inertia_subscriber.h" PYBIND11_MODULE(crocoddyl_ros, m) { namespace py = pybind11; @@ -213,7 +213,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { py::arg("s") = DEFAULT_FRICTION) .def("update_body_inertial_parameters", &WholeBodyStateRosPublisher::update_body_inertial_parameters, - "Update the Pinocchio model's inertial parameters of a given body frame.\n\n" + "Update the Pinocchio model's inertial parameters of a given body " + "frame.\n\n" "The inertial parameters vector is defined as [m, h_x, h_y, h_z," "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" "the first moment of inertial (mass * barycenter) and the rotational" @@ -225,7 +226,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { py::arg("body_name"), py::arg("psi")) .def("get_body_inertial_parameters", &WholeBodyStateRosPublisher::get_body_inertial_parameters, - "Return the Pinocchio model's inertial parameters of a given frame.\n\n" + "Return the Pinocchio model's inertial parameters of a given " + "frame.\n\n" "The inertial parameters vector is defined as [m, h_x, h_y, h_z," "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" "the first moment of inertial (mass * barycenter) and the rotational" @@ -234,7 +236,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { "fixed joints, and bodies.\n" ":param model: Pinocchio model\n" ":param body_name: body name\n" - ":return inertial parameters", py::arg("body_name")); + ":return inertial parameters", + py::arg("body_name")); py::class_>( @@ -260,7 +263,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { .def("has_new_msg", &WholeBodyStateRosSubscriber::has_new_msg) .def("update_body_inertial_parameters", &WholeBodyStateRosSubscriber::update_body_inertial_parameters, - "Update the Pinocchio model's inertial parameters of a given body frame.\n\n" + "Update the Pinocchio model's inertial parameters of a given body " + "frame.\n\n" "The inertial parameters vector is defined as [m, h_x, h_y, h_z," "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" "the first moment of inertial (mass * barycenter) and the rotational" @@ -272,7 +276,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { py::arg("body_name"), py::arg("psi")) .def("get_body_inertial_parameters", &WholeBodyStateRosSubscriber::get_body_inertial_parameters, - "Return the Pinocchio model's inertial parameters of a given frame.\n\n" + "Return the Pinocchio model's inertial parameters of a given " + "frame.\n\n" "The inertial parameters vector is defined as [m, h_x, h_y, h_z," "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" "the first moment of inertial (mass * barycenter) and the rotational" @@ -281,7 +286,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { "fixed joints, and bodies.\n" ":param model: Pinocchio model\n" ":param body_name: body name\n" - ":return inertial parameters", py::arg("body_name")); + ":return inertial parameters", + py::arg("body_name")); py::class_>( @@ -314,7 +320,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { py::arg("ss") = DEFAULT_FRICTION_VECTOR) .def("update_body_inertial_parameters", &WholeBodyTrajectoryRosPublisher::update_body_inertial_parameters, - "Update the Pinocchio model's inertial parameters of a given body frame.\n\n" + "Update the Pinocchio model's inertial parameters of a given body " + "frame.\n\n" "The inertial parameters vector is defined as [m, h_x, h_y, h_z," "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" "the first moment of inertial (mass * barycenter) and the rotational" @@ -326,7 +333,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { py::arg("body_name"), py::arg("psi")) .def("get_body_inertial_parameters", &WholeBodyTrajectoryRosPublisher::get_body_inertial_parameters, - "Return the Pinocchio model's inertial parameters of a given frame.\n\n" + "Return the Pinocchio model's inertial parameters of a given " + "frame.\n\n" "The inertial parameters vector is defined as [m, h_x, h_y, h_z," "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" "the first moment of inertial (mass * barycenter) and the rotational" @@ -334,7 +342,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { "barycenter. Additionally, the type of frame supported are joints," "fixed joints, and bodies.\n" ":param body_name: body name\n" - ":return inertial parameters", py::arg("body_name")); + ":return inertial parameters", + py::arg("body_name")); py::class_>( @@ -361,7 +370,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { .def("has_new_msg", &WholeBodyTrajectoryRosSubscriber::has_new_msg) .def("update_body_inertial_parameters", &WholeBodyTrajectoryRosSubscriber::update_body_inertial_parameters, - "Update the Pinocchio model's inertial parameters of a given body frame.\n\n" + "Update the Pinocchio model's inertial parameters of a given body " + "frame.\n\n" "The inertial parameters vector is defined as [m, h_x, h_y, h_z," "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" "the first moment of inertial (mass * barycenter) and the rotational" @@ -373,7 +383,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) { py::arg("body_name"), py::arg("psi")) .def("get_body_inertial_parameters", &WholeBodyTrajectoryRosSubscriber::get_body_inertial_parameters, - "Return the Pinocchio model's inertial parameters of a given frame.\n\n" + "Return the Pinocchio model's inertial parameters of a given " + "frame.\n\n" "The inertial parameters vector is defined as [m, h_x, h_y, h_z," "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" "the first moment of inertial (mass * barycenter) and the rotational" @@ -381,11 +392,11 @@ PYBIND11_MODULE(crocoddyl_ros, m) { "barycenter. Additionally, the type of frame supported are joints," "fixed joints, and bodies.\n" ":param body_name: body name\n" - ":return inertial parameters", py::arg("body_name")); + ":return inertial parameters", + py::arg("body_name")); - py::class_< - MultibodyInertiaRosPublisher, - std::unique_ptr>( + py::class_>( m, "MultibodyInertiaRosPublisher") .def(py::init(), py::arg("topic") = "/crocoddyl/inertial_parameters") @@ -399,14 +410,12 @@ PYBIND11_MODULE(crocoddyl_ros, m) { ":param parameters: multibody inertial parameters\n", py::arg("parameters")); - py::class_< - MultibodyInertiaRosSubscriber, - std::unique_ptr>( + py::class_>( m, "MultibodyInertiaRosSubscriber") .def(py::init(), py::arg("topic") = "/crocoddyl/inertial_parameters") - .def("get_parameters", - &MultibodyInertiaRosSubscriber::get_parameters, + .def("get_parameters", &MultibodyInertiaRosSubscriber::get_parameters, "Get the latest multibody inertial parameters.\n\n" "The inertial parameters vector is defined as [m, h_x, h_y, h_z," "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" @@ -414,8 +423,7 @@ PYBIND11_MODULE(crocoddyl_ros, m) { "inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the" "barycenter.\n" ":return: dictionary of body names and inertial parameters pair\n") - .def("has_new_msg", - &MultibodyInertiaRosSubscriber::has_new_msg); + .def("has_new_msg", &MultibodyInertiaRosSubscriber::has_new_msg); m.def("getRootJointId", &getRootJointId<0, pinocchio::JointCollectionDefaultTpl>, @@ -424,41 +432,45 @@ PYBIND11_MODULE(crocoddyl_ros, m) { ":return root joint id"); m.def("getRootNq", &getRootNq<0, pinocchio::JointCollectionDefaultTpl>, - "Return the root joint nq dimension.\n\n" - ":param model: Pinocchio model\n" - ":return root joint nq dimension"); + "Return the root joint nq dimension.\n\n" + ":param model: Pinocchio model\n" + ":return root joint nq dimension"); m.def("getRootNv", &getRootNv<0, pinocchio::JointCollectionDefaultTpl>, - "Return the root joint nv dimension.\n\n" - ":param model: Pinocchio model\n" - ":return root joint nv dimension"); + "Return the root joint nv dimension.\n\n" + ":param model: Pinocchio model\n" + ":return root joint nv dimension"); - m.def("updateBodyInertialParameters", - pinocchio::python::make_pybind11_function(&updateBodyInertialParameters<0, pinocchio::JointCollectionDefaultTpl>), - "Update the Pinocchio model's inertial parameters of a given frame.\n\n" - "The inertial parameters vector is defined as [m, h_x, h_y, h_z," - "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" - "the first moment of inertial (mass * barycenter) and the rotational" - "inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the" - "barycenter. Additionally, the type of frame supported are joints," - "fixed joints, and bodies.\n" - ":param model: Pinocchio model\n" - ":param frame_name: frame name\n" - ":param psi: inertial parameters", - py::arg("model"), py::arg("frame_name"), py::arg("psi")); + m.def( + "updateBodyInertialParameters", + pinocchio::python::make_pybind11_function( + &updateBodyInertialParameters<0, + pinocchio::JointCollectionDefaultTpl>), + "Update the Pinocchio model's inertial parameters of a given frame.\n\n" + "The inertial parameters vector is defined as [m, h_x, h_y, h_z," + "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" + "the first moment of inertial (mass * barycenter) and the rotational" + "inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the" + "barycenter. Additionally, the type of frame supported are joints," + "fixed joints, and bodies.\n" + ":param model: Pinocchio model\n" + ":param frame_name: frame name\n" + ":param psi: inertial parameters", + py::arg("model"), py::arg("frame_name"), py::arg("psi")); - m.def("getBodyInertialParameters", &getBodyInertialParameters<0, pinocchio::JointCollectionDefaultTpl>, - "Return the Pinocchio model's inertial parameters of a given frame.\n\n" - "The inertial parameters vector is defined as [m, h_x, h_y, h_z," - "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" - "the first moment of inertial (mass * barycenter) and the rotational" - "inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the" - "barycenter. Additionally, the type of frame supported are joints," - "fixed joints, and bodies.\n" - ":param model: Pinocchio model\n" - ":param frame_name: frame name\n" - ":return inertial parameters", - py::arg("model"), py::arg("frame_name")); + m.def("getBodyInertialParameters", + &getBodyInertialParameters<0, pinocchio::JointCollectionDefaultTpl>, + "Return the Pinocchio model's inertial parameters of a given frame.\n\n" + "The inertial parameters vector is defined as [m, h_x, h_y, h_z," + "I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is" + "the first moment of inertial (mass * barycenter) and the rotational" + "inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the" + "barycenter. Additionally, the type of frame supported are joints," + "fixed joints, and bodies.\n" + ":param model: Pinocchio model\n" + ":param frame_name: frame name\n" + ":return inertial parameters", + py::arg("model"), py::arg("frame_name")); m.def( "fromReduced", diff --git a/unittest/test_inertial_parameters.py b/unittest/test_inertial_parameters.py index 618aef2..06f0d09 100755 --- a/unittest/test_inertial_parameters.py +++ b/unittest/test_inertial_parameters.py @@ -23,14 +23,12 @@ import rospy import rosunit -from crocoddyl_ros import ( - MultibodyInertiaRosPublisher, - MultibodyInertiaRosSubscriber, -) +from crocoddyl_ros import MultibodyInertiaRosPublisher, MultibodyInertiaRosSubscriber class TestInertialParametersAbstract(unittest.TestCase): MODEL = None + def setUp(self): if ROS_VERSION == 2: if not rclpy.ok(): @@ -70,12 +68,15 @@ def test_communication(self): + str(_parameters[names[i]]), ) + class SampleHumanoidTest(TestInertialParametersAbstract): MODEL = pinocchio.buildSampleModelHumanoid() + class SampleManipulatorTest(TestInertialParametersAbstract): MODEL = pinocchio.buildSampleModelManipulator() + if __name__ == "__main__": test_classes_to_run = [ SampleHumanoidTest, diff --git a/unittest/test_model.py b/unittest/test_model.py index ac7e080..f74ac11 100644 --- a/unittest/test_model.py +++ b/unittest/test_model.py @@ -19,10 +19,10 @@ import rosunit from crocoddyl_ros import ( + getBodyInertialParameters, getRootJointId, getRootNq, getRootNv, - getBodyInertialParameters, updateBodyInertialParameters, ) @@ -44,12 +44,16 @@ def test_root(self): def test_root_with_reduced_model(self): qref = pinocchio.randomConfiguration(self.MODEL) reduced_model = pinocchio.buildReducedModel( - self.MODEL, [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], qref + self.MODEL, + [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], + qref, ) root_joint_id = getRootJointId(reduced_model) root_nq = getRootNq(reduced_model) root_nv = getRootNv(reduced_model) - self.assertEqual(self.ROOT_JOINT_ID, root_joint_id, "Wrong reduced root's joint id") + self.assertEqual( + self.ROOT_JOINT_ID, root_joint_id, "Wrong reduced root's joint id" + ) self.assertEqual(self.ROOT_NQ, root_nq, "Wrong reduced root's nq dimension") self.assertEqual(self.ROOT_NV, root_nv, "Wrong reduced root's nv dimension") @@ -58,7 +62,9 @@ def test_get_joint_inertial_parameters(self): for frame_name in frame_names: joint_id = self.MODEL.getJointId(frame_name) parameters = pinocchio.Inertia.Random().toDynamicParameters() - self.MODEL.inertias[joint_id] = pinocchio.Inertia.FromDynamicParameters(parameters) + self.MODEL.inertias[joint_id] = pinocchio.Inertia.FromDynamicParameters( + parameters + ) new_parameters = getBodyInertialParameters(self.MODEL, frame_name) self.assertTrue( np.allclose( @@ -78,13 +84,19 @@ def test_get_joint_inertial_parameters(self): def test_get_joint_inertial_parameters_with_reduced_model(self): qref = pinocchio.randomConfiguration(self.MODEL) reduced_model = pinocchio.buildReducedModel( - self.MODEL, [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], qref + self.MODEL, + [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], + qref, ) - frame_names = [f.name for f in reduced_model.frames if f.type == pinocchio.JOINT] + frame_names = [ + f.name for f in reduced_model.frames if f.type == pinocchio.JOINT + ] for frame_name in frame_names: joint_id = reduced_model.getJointId(frame_name) parameters = pinocchio.Inertia.Random().toDynamicParameters() - reduced_model.inertias[joint_id] = pinocchio.Inertia.FromDynamicParameters(parameters) + reduced_model.inertias[joint_id] = pinocchio.Inertia.FromDynamicParameters( + parameters + ) new_parameters = getBodyInertialParameters(reduced_model, frame_name) self.assertTrue( np.allclose( @@ -104,12 +116,25 @@ def test_get_joint_inertial_parameters_with_reduced_model(self): def test_update_body_inertial_parameters(self): qref = pinocchio.randomConfiguration(self.MODEL) reduced_model = pinocchio.buildReducedModel( - self.MODEL, [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], qref + self.MODEL, + [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], + qref, ) if pinocchio.__version__ >= "2.7.1": - frame_names = [f.name for f in reduced_model.frames if f.name != "universe" and (f.type == pinocchio.BODY or f.type == pinocchio.JOINT or f.type == pinocchio.FIXED_JOINT)] + frame_names = [ + f.name + for f in reduced_model.frames + if f.name != "universe" + and ( + f.type == pinocchio.BODY + or f.type == pinocchio.JOINT + or f.type == pinocchio.FIXED_JOINT + ) + ] else: - frame_names = [f.name for f in reduced_model.frames if f.type == pinocchio.JOINT] + frame_names = [ + f.name for f in reduced_model.frames if f.type == pinocchio.JOINT + ] for frame_name in frame_names: parameters = pinocchio.Inertia.Random().toDynamicParameters() updateBodyInertialParameters(reduced_model, frame_name, parameters) @@ -131,9 +156,20 @@ def test_update_body_inertial_parameters(self): def test_update_body_inertial_parameters_with_reduced_model(self): if pinocchio.__version__ >= "2.7.1": - frame_names = [f.name for f in self.MODEL.frames if f.name != "universe" and (f.type == pinocchio.BODY or f.type == pinocchio.JOINT or f.type == pinocchio.FIXED_JOINT)] + frame_names = [ + f.name + for f in self.MODEL.frames + if f.name != "universe" + and ( + f.type == pinocchio.BODY + or f.type == pinocchio.JOINT + or f.type == pinocchio.FIXED_JOINT + ) + ] else: - frame_names = [f.name for f in self.MODEL.frames if f.type == pinocchio.JOINT] + frame_names = [ + f.name for f in self.MODEL.frames if f.type == pinocchio.JOINT + ] for frame_name in frame_names: parameters = pinocchio.Inertia.Random().toDynamicParameters() updateBodyInertialParameters(self.MODEL, frame_name, parameters) @@ -153,6 +189,7 @@ def test_update_body_inertial_parameters_with_reduced_model(self): + str(new_parameters), ) + class SampleHumanoidTest(TestModelAbstract): MODEL = pinocchio.buildSampleModelHumanoid() ROOT_JOINT_ID = 1 @@ -160,6 +197,7 @@ class SampleHumanoidTest(TestModelAbstract): ROOT_NV = 6 LOCKED_JOINTS = ["larm_elbow_joint", "rarm_elbow_joint"] + class SampleManipulatorTest(TestModelAbstract): MODEL = pinocchio.buildSampleModelManipulator() ROOT_JOINT_ID = 0 @@ -167,6 +205,7 @@ class SampleManipulatorTest(TestModelAbstract): ROOT_NV = 0 LOCKED_JOINTS = ["wrist1_joint", "wrist2_joint"] + if __name__ == "__main__": test_classes_to_run = [ SampleHumanoidTest, @@ -184,4 +223,4 @@ class SampleManipulatorTest(TestModelAbstract): results = runner.run(big_suite) else: for test_class in test_classes_to_run: - rosunit.unitrun("crocoddyl_msgs", "model", test_class) \ No newline at end of file + rosunit.unitrun("crocoddyl_msgs", "model", test_class) diff --git a/unittest/test_whole_body_state.py b/unittest/test_whole_body_state.py index 89a0371..3ca9645 100755 --- a/unittest/test_whole_body_state.py +++ b/unittest/test_whole_body_state.py @@ -28,8 +28,8 @@ ContactType, WholeBodyStateRosPublisher, WholeBodyStateRosSubscriber, - toReduced, getRootNv, + toReduced, updateBodyInertialParameters, ) @@ -86,7 +86,9 @@ def setUp(self): } def test_publisher_without_contact(self): - sub = WholeBodyStateRosSubscriber(self.MODEL, "whole_body_state_without_contact") + sub = WholeBodyStateRosSubscriber( + self.MODEL, "whole_body_state_without_contact" + ) pub = WholeBodyStateRosPublisher(self.MODEL, "whole_body_state_without_contact") time.sleep(1) # publish whole-body state messages @@ -103,7 +105,10 @@ def test_publisher_without_contact(self): qdiff = pinocchio.difference(self.MODEL, q, _q) mask = ~np.isnan(qdiff) self.assertEqual(self.t, _t, "Wrong time interval") - self.assertTrue(np.allclose(qdiff[mask], np.zeros(self.MODEL.nv)[mask], atol=1e-9), "Wrong q") + self.assertTrue( + np.allclose(qdiff[mask], np.zeros(self.MODEL.nv)[mask], atol=1e-9), + "Wrong q", + ) self.assertTrue(np.allclose(v, _v, atol=1e-9), "Wrong v") self.assertTrue(np.allclose(tau, _tau, atol=1e-9), "Wrong tau") @@ -125,7 +130,10 @@ def test_communication(self): qdiff = pinocchio.difference(self.MODEL, q, _q) mask = ~np.isnan(qdiff) self.assertEqual(self.t, _t, "Wrong time interval") - self.assertTrue(np.allclose(qdiff[mask], np.zeros(self.MODEL.nv)[mask], atol=1e-9), "Wrong q") + self.assertTrue( + np.allclose(qdiff[mask], np.zeros(self.MODEL.nv)[mask], atol=1e-9), + "Wrong q", + ) self.assertTrue(np.allclose(v, _v, atol=1e-9), "Wrong v") self.assertTrue(np.allclose(tau, _tau, atol=1e-9), "Wrong tau") for name in self.p: @@ -161,12 +169,16 @@ def test_communication(self): def test_communication_with_reduced_model(self): qref = pinocchio.randomConfiguration(self.MODEL) reduced_model = pinocchio.buildReducedModel( - self.MODEL, [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], qref + self.MODEL, + [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], + qref, ) sub = WholeBodyStateRosSubscriber( self.MODEL, self.LOCKED_JOINTS, qref, "reduced_whole_body_state" ) - pub = WholeBodyStateRosPublisher(self.MODEL, self.LOCKED_JOINTS, qref, "reduced_whole_body_state") + pub = WholeBodyStateRosPublisher( + self.MODEL, self.LOCKED_JOINTS, qref, "reduced_whole_body_state" + ) time.sleep(1) # publish whole-body state messages nv_root = getRootNv(self.MODEL) @@ -183,7 +195,10 @@ def test_communication_with_reduced_model(self): qdiff = pinocchio.difference(reduced_model, q, _q) mask = ~np.isnan(qdiff) self.assertEqual(self.t, _t, "Wrong time interval") - self.assertTrue(np.allclose(qdiff[mask], np.zeros(reduced_model.nv)[mask], atol=1e-9), "Wrong q") + self.assertTrue( + np.allclose(qdiff[mask], np.zeros(reduced_model.nv)[mask], atol=1e-9), + "Wrong q", + ) self.assertTrue(np.allclose(v, _v, atol=1e-9), "Wrong v") self.assertTrue(np.allclose(tau, _tau, atol=1e-9), "Wrong tau") for name in self.p: @@ -225,7 +240,9 @@ def test_communication_with_non_locked_joints(self): sub = WholeBodyStateRosSubscriber( self.MODEL, locked_joints, qref, "non_locked_whole_body_state" ) - pub = WholeBodyStateRosPublisher(self.MODEL, locked_joints, qref, "non_locked_whole_body_state") + pub = WholeBodyStateRosPublisher( + self.MODEL, locked_joints, qref, "non_locked_whole_body_state" + ) time.sleep(1) # publish whole-body state messages nv_root = getRootNv(self.MODEL) @@ -242,7 +259,10 @@ def test_communication_with_non_locked_joints(self): qdiff = pinocchio.difference(reduced_model, q, _q) mask = ~np.isnan(qdiff) self.assertEqual(self.t, _t, "Wrong time interval") - self.assertTrue(np.allclose(qdiff[mask], np.zeros(reduced_model.nv)[mask], atol=1e-9), "Wrong q") + self.assertTrue( + np.allclose(qdiff[mask], np.zeros(reduced_model.nv)[mask], atol=1e-9), + "Wrong q", + ) self.assertTrue(np.allclose(v, _v, atol=1e-9), "Wrong v") self.assertTrue(np.allclose(tau, _tau, atol=1e-9), "Wrong tau") for name in self.p: @@ -281,9 +301,20 @@ def test_update_model(self): time.sleep(1) # update inertia parameters if pinocchio.__version__ >= "2.7.1": - frame_names = [f.name for f in self.MODEL.frames if f.name != "universe" and (f.type == pinocchio.BODY or f.type == pinocchio.JOINT or f.type == pinocchio.FIXED_JOINT)] + frame_names = [ + f.name + for f in self.MODEL.frames + if f.name != "universe" + and ( + f.type == pinocchio.BODY + or f.type == pinocchio.JOINT + or f.type == pinocchio.FIXED_JOINT + ) + ] else: - frame_names = [f.name for f in self.MODEL.frames if f.type == pinocchio.JOINT] + frame_names = [ + f.name for f in self.MODEL.frames if f.type == pinocchio.JOINT + ] new_parameters = [] for name in frame_names: psi = pinocchio.Inertia.Random().toDynamicParameters() @@ -336,7 +367,10 @@ def test_update_model(self): qdiff = pinocchio.difference(self.MODEL, q, _q) mask = ~np.isnan(qdiff) self.assertEqual(self.t, _t, "Wrong time interval") - self.assertTrue(np.allclose(qdiff[mask], np.zeros(self.MODEL.nv)[mask], atol=1e-9), "Wrong q") + self.assertTrue( + np.allclose(qdiff[mask], np.zeros(self.MODEL.nv)[mask], atol=1e-9), + "Wrong q", + ) self.assertTrue(np.allclose(v, _v, atol=1e-9), "Wrong v") self.assertTrue(np.allclose(tau, _tau, atol=1e-9), "Wrong tau") for name in self.p: @@ -372,16 +406,33 @@ def test_update_model(self): def test_update_reduced_model(self): qref = pinocchio.randomConfiguration(self.MODEL) reduced_model = pinocchio.buildReducedModel( - self.MODEL, [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], qref + self.MODEL, + [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], + qref, + ) + pub = WholeBodyStateRosPublisher( + reduced_model, "whole_body_state_update_reduced_model" + ) + sub = WholeBodyStateRosSubscriber( + reduced_model, "whole_body_state_update_reduced_model" ) - pub = WholeBodyStateRosPublisher(reduced_model, "whole_body_state_update_reduced_model") - sub = WholeBodyStateRosSubscriber(reduced_model, "whole_body_state_update_reduced_model") time.sleep(1) # update inertia parameters if pinocchio.__version__ >= "2.7.1": - frame_names = [f.name for f in reduced_model.frames if f.name != "universe" and (f.type == pinocchio.BODY or f.type == pinocchio.JOINT or f.type == pinocchio.FIXED_JOINT)] + frame_names = [ + f.name + for f in reduced_model.frames + if f.name != "universe" + and ( + f.type == pinocchio.BODY + or f.type == pinocchio.JOINT + or f.type == pinocchio.FIXED_JOINT + ) + ] else: - frame_names = [f.name for f in reduced_model.frames if f.type == pinocchio.JOINT] + frame_names = [ + f.name for f in reduced_model.frames if f.type == pinocchio.JOINT + ] new_parameters = [] for name in frame_names: psi = pinocchio.Inertia.Random().toDynamicParameters() @@ -435,7 +486,10 @@ def test_update_reduced_model(self): qdiff = pinocchio.difference(reduced_model, q, _q) mask = ~np.isnan(qdiff) self.assertEqual(self.t, _t, "Wrong time interval") - self.assertTrue(np.allclose(qdiff[mask], np.zeros(reduced_model.nv)[mask], atol=1e-9), "Wrong q") + self.assertTrue( + np.allclose(qdiff[mask], np.zeros(reduced_model.nv)[mask], atol=1e-9), + "Wrong q", + ) self.assertTrue(np.allclose(v, _v, atol=1e-9), "Wrong v") self.assertTrue(np.allclose(tau, _tau, atol=1e-9), "Wrong tau") for name in self.p: @@ -468,14 +522,17 @@ def test_update_reduced_model(self): S[1], _S[1], "Wrong contact friction coefficient at " + name ) + class SampleHumanoidTest(TestWholeBodyStateAbstract): MODEL = pinocchio.buildSampleModelHumanoid() LOCKED_JOINTS = ["larm_elbow_joint", "rarm_elbow_joint"] + class SampleManipulatorTest(TestWholeBodyStateAbstract): MODEL = pinocchio.buildSampleModelManipulator() LOCKED_JOINTS = ["wrist1_joint", "wrist2_joint"] + if __name__ == "__main__": test_classes_to_run = [ SampleHumanoidTest, diff --git a/unittest/test_whole_body_trajectory.py b/unittest/test_whole_body_trajectory.py index 1bc5066..3b8afdd 100755 --- a/unittest/test_whole_body_trajectory.py +++ b/unittest/test_whole_body_trajectory.py @@ -28,8 +28,8 @@ ContactType, WholeBodyTrajectoryRosPublisher, WholeBodyTrajectoryRosSubscriber, - toReduced, getRootNv, + toReduced, ) @@ -97,8 +97,12 @@ def setUp(self) -> None: ) def test_publisher_without_contact(self): - sub = WholeBodyTrajectoryRosSubscriber(self.MODEL, "whole_body_trajectory_without_contact") - pub = WholeBodyTrajectoryRosPublisher(self.MODEL, "whole_body_trajectory_without_contact") + sub = WholeBodyTrajectoryRosSubscriber( + self.MODEL, "whole_body_trajectory_without_contact" + ) + pub = WholeBodyTrajectoryRosPublisher( + self.MODEL, "whole_body_trajectory_without_contact" + ) time.sleep(1) # publish whole-body trajectory messages N = len(self.ts) @@ -188,7 +192,9 @@ def test_communication(self): def test_communication_with_reduced_model(self): qref = pinocchio.randomConfiguration(self.MODEL) reduced_model = pinocchio.buildReducedModel( - self.MODEL, [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], qref + self.MODEL, + [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], + qref, ) sub = WholeBodyTrajectoryRosSubscriber( self.MODEL, self.LOCKED_JOINTS, qref, "reduced_whole_body_trajectory" @@ -331,14 +337,29 @@ def test_communication_with_non_locked_joints(self): ) def test_update_model(self): - sub = WholeBodyTrajectoryRosSubscriber(self.MODEL, "whole_body_trajectory_update_model") - pub = WholeBodyTrajectoryRosPublisher(self.MODEL, "whole_body_trajectory_update_model") + sub = WholeBodyTrajectoryRosSubscriber( + self.MODEL, "whole_body_trajectory_update_model" + ) + pub = WholeBodyTrajectoryRosPublisher( + self.MODEL, "whole_body_trajectory_update_model" + ) time.sleep(1) # update inertia parameters if pinocchio.__version__ >= "2.7.1": - frame_names = [f.name for f in self.MODEL.frames if f.name != "universe" and (f.type == pinocchio.BODY or f.type == pinocchio.JOINT or f.type == pinocchio.FIXED_JOINT)] + frame_names = [ + f.name + for f in self.MODEL.frames + if f.name != "universe" + and ( + f.type == pinocchio.BODY + or f.type == pinocchio.JOINT + or f.type == pinocchio.FIXED_JOINT + ) + ] else: - frame_names = [f.name for f in self.MODEL.frames if f.type == pinocchio.JOINT] + frame_names = [ + f.name for f in self.MODEL.frames if f.type == pinocchio.JOINT + ] new_parameters = [] for name in frame_names: psi = pinocchio.Inertia.Random().toDynamicParameters() @@ -381,16 +402,33 @@ def test_update_model(self): def test_update_reduced_model(self): qref = pinocchio.randomConfiguration(self.MODEL) reduced_model = pinocchio.buildReducedModel( - self.MODEL, [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], qref + self.MODEL, + [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], + qref, + ) + sub = WholeBodyTrajectoryRosSubscriber( + reduced_model, "whole_body_trajectory_update_model" + ) + pub = WholeBodyTrajectoryRosPublisher( + reduced_model, "whole_body_trajectory_update_model" ) - sub = WholeBodyTrajectoryRosSubscriber(reduced_model, "whole_body_trajectory_update_model") - pub = WholeBodyTrajectoryRosPublisher(reduced_model, "whole_body_trajectory_update_model") time.sleep(1) # update inertia parameters if pinocchio.__version__ >= "2.7.1": - frame_names = [f.name for f in reduced_model.frames if f.name != "universe" and (f.type == pinocchio.BODY or f.type == pinocchio.JOINT or f.type == pinocchio.FIXED_JOINT)] + frame_names = [ + f.name + for f in reduced_model.frames + if f.name != "universe" + and ( + f.type == pinocchio.BODY + or f.type == pinocchio.JOINT + or f.type == pinocchio.FIXED_JOINT + ) + ] else: - frame_names = [f.name for f in reduced_model.frames if f.type == pinocchio.JOINT] + frame_names = [ + f.name for f in reduced_model.frames if f.type == pinocchio.JOINT + ] new_parameters = [] for name in frame_names: psi = pinocchio.Inertia.Random().toDynamicParameters() @@ -430,14 +468,17 @@ def test_update_reduced_model(self): + str(sub_parameters), ) + class SampleHumanoidTest(TestWholeBodyTrajectoryAbstract): MODEL = pinocchio.buildSampleModelHumanoid() LOCKED_JOINTS = ["larm_elbow_joint", "rarm_elbow_joint"] + class SampleManipulatorTest(TestWholeBodyTrajectoryAbstract): MODEL = pinocchio.buildSampleModelManipulator() LOCKED_JOINTS = ["wrist1_joint", "wrist2_joint"] + if __name__ == "__main__": test_classes_to_run = [ SampleHumanoidTest,