diff --git a/include/odri_control_interface/joint_modules.hpp b/include/odri_control_interface/joint_modules.hpp index 2abe637..544fb66 100644 --- a/include/odri_control_interface/joint_modules.hpp +++ b/include/odri_control_interface/joint_modules.hpp @@ -66,9 +66,9 @@ class JointModules public: JointModules(const std::shared_ptr& robot_if, ConstRefVectorXi motor_numbers, - double motor_constants, - double gear_ratios, - double max_currents, + ConstRefVectorXd motor_constants, + ConstRefVectorXd gear_ratios, + ConstRefVectorXd max_currents, ConstRefVectorXb reverse_polarities, ConstRefVectorXd lower_joint_limits, ConstRefVectorXd upper_joint_limits, @@ -85,7 +85,7 @@ class JointModules void SetDesiredVelocities(ConstRefVectorXd desired_velocities); void SetPositionGains(ConstRefVectorXd desired_gains); void SetVelocityGains(ConstRefVectorXd desired_gains); - void SetMaximumCurrents(double max_currents); + void SetMaximumCurrents(ConstRefVectorXd max_currents); /** * @brief Disables the position and velocity gains by setting them to zero. diff --git a/src/joint_modules.cpp b/src/joint_modules.cpp index 311f595..4a4fca6 100644 --- a/src/joint_modules.cpp +++ b/src/joint_modules.cpp @@ -16,15 +16,17 @@ namespace odri_control_interface JointModules::JointModules( const std::shared_ptr& robot_if, ConstRefVectorXi motor_numbers, - double motor_constants, - double gear_ratios, - double max_currents, + ConstRefVectorXd motor_constants, + ConstRefVectorXd gear_ratios, + ConstRefVectorXd max_currents, ConstRefVectorXb reverse_polarities, ConstRefVectorXd lower_joint_limits, ConstRefVectorXd upper_joint_limits, double max_joint_velocities, double safety_damping) : robot_if_(robot_if), + gear_ratios_(gear_ratios), + motor_constants_(motor_constants), lower_joint_limits_(lower_joint_limits), upper_joint_limits_(upper_joint_limits), max_joint_velocities_(max_joint_velocities), @@ -39,6 +41,18 @@ JointModules::JointModules( nd_ = (n_ + 1) / 2; // Check input arrays for correct sizes. + if (motor_constants.size() != n_) + { + throw std::runtime_error( + "Motor constants has different size than motor numbers"); + } + + if (gear_ratios.size() != n_) + { + throw std::runtime_error( + "Gear ratios has different size than motor numbers"); + } + if (reverse_polarities.size() != n_) { throw std::runtime_error( @@ -57,9 +71,13 @@ JointModules::JointModules( "Upper joint limits has different size than motor numbers"); } + if (max_currents.size() != n_) + { + throw std::runtime_error( + "Max currents has different size than motor numbers"); + } + // Resize and fill the vectors. - gear_ratios_.resize(n_); - motor_constants_.resize(n_); positions_.resize(n_); velocities_.resize(n_); sent_torques_.resize(n_); @@ -80,9 +98,6 @@ JointModules::JointModules( motor_driver_errors_.resize(nd_); motor_driver_errors_.fill(0); - gear_ratios_.fill(gear_ratios); - motor_constants_.fill(motor_constants); - for (int i = 0; i < n_; i++) { motors_.push_back(robot_if_->GetMotor(motor_numbers[i])); @@ -453,11 +468,11 @@ void JointModules::PrintVector(ConstRefVectorXd vector) msg_out_ << vector.transpose().format(CleanFmt); } -void JointModules::SetMaximumCurrents(double max_currents) +void JointModules::SetMaximumCurrents(ConstRefVectorXd max_currents) { for (int i = 0; i < n_; i++) { - motors_[i]->set_current_sat(max_currents); + motors_[i]->set_current_sat(max_currents[i]); } } diff --git a/src/utils.cpp b/src/utils.cpp index 1e407cb..ac4bb56 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -108,18 +108,99 @@ std::shared_ptr JointModulesFromYaml( upper_joint_limits_vec(i) = upper_joint_limits[i].as(); } + // "motor_constants" can be either a scalar (same value for all motors) or a list assert_yaml_parsing(joint_modules_yaml, "joint_modules", "motor_constants"); + const YAML::Node& motor_constants = joint_modules_yaml["motor_constants"]; + VectorXd motor_constants_vec; + motor_constants_vec.resize(n); + switch (motor_constants.Type()) { + case YAML::NodeType::Scalar: + for (std::size_t i = 0; i < n; i++) + { + motor_constants_vec(i) = motor_constants.as(); + } + break; + case YAML::NodeType::Sequence: + if (motor_constants.size() != n) + { + throw std::runtime_error( + "Motor constants list has different size than motor numbers."); + } + for (std::size_t i = 0; i < n; i++) + { + motor_constants_vec(i) = motor_constants[i].as(); + } + break; + default: + throw std::runtime_error( + "Motor constants should be either a scalar or a list."); + } + + // "gear_ratios" can be either a scalar (same value for all motors) or a list assert_yaml_parsing(joint_modules_yaml, "joint_modules", "gear_ratios"); + const YAML::Node& gear_ratios = joint_modules_yaml["gear_ratios"]; + VectorXd gear_ratios_vec; + gear_ratios_vec.resize(n); + switch (gear_ratios.Type()) { + case YAML::NodeType::Scalar: + for (std::size_t i = 0; i < n; i++) + { + gear_ratios_vec(i) = gear_ratios.as(); + } + break; + case YAML::NodeType::Sequence: + if (gear_ratios.size() != n) + { + throw std::runtime_error( + "Gear ratio list has different size than motor numbers."); + } + for (std::size_t i = 0; i < n; i++) + { + gear_ratios_vec(i) = gear_ratios[i].as(); + } + break; + default: + throw std::runtime_error( + "Gear ratio should be either a scalar or a list."); + } + + // "max_currents" can be either a scalar (same value for all motors) or a list assert_yaml_parsing(joint_modules_yaml, "joint_modules", "max_currents"); + const YAML::Node& max_currents = joint_modules_yaml["max_currents"]; + VectorXd max_currents_vec; + max_currents_vec.resize(n); + switch (max_currents.Type()) { + case YAML::NodeType::Scalar: + for (std::size_t i = 0; i < n; i++) + { + max_currents_vec(i) = max_currents.as(); + } + break; + case YAML::NodeType::Sequence: + if (max_currents.size() != n) + { + throw std::runtime_error( + "Max currents list has different size than motor numbers."); + } + for (std::size_t i = 0; i < n; i++) + { + max_currents_vec(i) = max_currents[i].as(); + } + break; + default: + throw std::runtime_error( + "Max currents should be either a scalar or a list."); + } + assert_yaml_parsing( joint_modules_yaml, "joint_modules", "max_joint_velocities"); assert_yaml_parsing(joint_modules_yaml, "joint_modules", "safety_damping"); return std::make_shared( robot_if, motor_numbers_vec, - joint_modules_yaml["motor_constants"].as(), - joint_modules_yaml["gear_ratios"].as(), - joint_modules_yaml["max_currents"].as(), + motor_constants_vec, + gear_ratios_vec, + max_currents_vec, rev_polarities_vec, lower_joint_limits_vec, upper_joint_limits_vec, diff --git a/srcpy/bindings.cpp b/srcpy/bindings.cpp index f37c685..8e78818 100644 --- a/srcpy/bindings.cpp +++ b/srcpy/bindings.cpp @@ -46,9 +46,9 @@ std::shared_ptr joint_calibrator_constructor( std::shared_ptr joint_modules_constructor( const std::shared_ptr& robot_if, ConstRefVectorXl motor_numbers, - double motor_constants, - double gear_ratios, - double max_currents, + ConstRefVectorXd motor_constants, + ConstRefVectorXd gear_ratios, + ConstRefVectorXd max_currents, ConstRefVectorXb reverse_polarities, ConstRefVectorXd lower_joint_limits, ConstRefVectorXd upper_joint_limits,