diff --git a/CMakeLists.txt b/CMakeLists.txt index e86b801..fc56aa9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,46 +121,45 @@ target_link_libraries(${PROJECT_NAME} ament_target_dependencies(${PROJECT_NAME} PUBLIC -controller_interface -hardware_interface -pluginlib -rclcpp -rclcpp_lifecycle -pinocchio -generate_parameter_library -realtime_tools + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + pinocchio + generate_parameter_library + realtime_tools ) pluginlib_export_plugin_description_file( controller_interface crisp_controllers.xml) install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) install( - DIRECTORY include/ - DESTINATION include + DIRECTORY include/ + DESTINATION include ) -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) ament_export_dependencies( - controller_interface - pluginlib - rclcpp - rclcpp_lifecycle - hardware_interface - Eigen3 -) + controller_interface + pluginlib + rclcpp + rclcpp_lifecycle + hardware_interface + Eigen3 + pinocchio + realtime_tools + generate_parameter_library +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index c14f4a2..ab3fbbf 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -114,6 +114,12 @@ class CartesianController : public controller_interface::ControllerInterface { */ void setStiffnessAndDamping(); + /** + * @brief Get the current state of the robot from hardware interfaces and update internal variables + * @param initialize If set to true, initialize the exponential moving average filter with the current state + */ + void updateCurrentState(bool initialize = false); + /** * @brief Reads the target pose in realtime loop from the buffer and parses it to be used in the controller. */ @@ -199,6 +205,10 @@ class CartesianController : public controller_interface::ControllerInterface { pinocchio::SE3 end_effector_pose; /** @brief End effector Jacobian matrix */ pinocchio::Data::Matrix6x J; + /** @brief End effector Jacobian matrix pseudoinverse */ + Eigen::MatrixXd J_pinv; + /** @brief Joint-space identity matrix */ + Eigen::MatrixXd Id_nv; /** @brief Friction parameters 1 of size nv */ Eigen::VectorXd fp1; diff --git a/pyproject.toml b/pyproject.toml index 76c81a5..9bf964f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -3,7 +3,7 @@ name = "crisp-controllers" version = "0.1.0" description = "A collection of ROS2 controllers for the CRISP project." readme = "README.md" -requires-python = ">=3.11" +requires-python = ">=3.10" dependencies = [ "mkdocs>=1.6.1", "mkdocs-material>=9.6.15", diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index a7e9568..c16a7c4 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -62,41 +62,11 @@ CartesianController::state_interface_configuration() const { controller_interface::return_type CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - size_t num_joints = params_.joints.size(); - for (size_t i = 0; i < num_joints; i++) { - // TODO(placeholder): later it might be better to get this thing prepared in the - // configuration part (not in the control loop) - auto joint_name = params_.joints[i]; - auto joint_id = model_.getJointId(joint_name); // pinocchio joind id might be different - auto joint = model_.joints[joint_id]; - - q_ref[i] = exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref); - -// Filtering joint position measurement 1 uses previous q, 0 uses new q from measurement. -// q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(), params_.filter.q); -#if ROS2_VERSION_ABOVE_HUMBLE - q[i] = exponential_moving_average( - q[i], state_interfaces_[i].get_optional().value_or(q[i]), params_.filter.q); -#else - q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(), params_.filter.q); -#endif - - if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous - // joint that is SO(2) - q_pin[joint.idx_q()] = std::cos(q[i]); - q_pin[joint.idx_q() + 1] = std::sin(q[i]); - } else { // simple revolute joint case - q_pin[joint.idx_q()] = q[i]; - } -#if ROS2_VERSION_ABOVE_HUMBLE - dq[i] = exponential_moving_average( - dq[i], state_interfaces_[num_joints + i].get_optional().value_or(dq[i]), params_.filter.dq); -#else - dq[i] = exponential_moving_average( - dq[i], state_interfaces_[num_joints + i].get_value(), params_.filter.dq); -#endif - } + + // Update current state information with EMA filtered values + updateCurrentState(); + // Check if new targets available if (new_target_pose_) { parse_target_pose_(); new_target_pose_ = false; @@ -147,15 +117,13 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & : pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED; pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id, reference_frame, J); - Eigen::MatrixXd J_pinv(model_.nv, 6); J_pinv = pseudo_inverse(J, params_.nullspace.regularization); - Eigen::MatrixXd Id_nv = Eigen::MatrixXd::Identity(model_.nv, model_.nv); if (params_.nullspace.projector_type == "dynamic" || params_.use_operational_space) { pinocchio::computeMinverse(model_, data_, q_pin); data_.Minv.triangularView() = data_.Minv.transpose().triangularView(); Mx_inv = J * data_.Minv * J.transpose(); - Mx = pseudo_inverse(Mx_inv); + Mx = pseudo_inverse(Mx_inv, params_.operational_space_regularization); } if (params_.nullspace.projector_type == "dynamic") { @@ -221,7 +189,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & tau_d = exponential_moving_average(tau_d, tau_previous, params_.filter.output_torque); if (!params_.stop_commands) { - for (size_t i = 0; i < num_joints; ++i) { + for (size_t i = 0; i < params_.joints.size(); ++i) { #if ROS2_VERSION_ABOVE_HUMBLE (void)command_interfaces_[i].set_value(tau_d[i]); #else @@ -323,7 +291,8 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta return CallbackReturn::ERROR; } } - + + // Preallocate the matrices and vectors that will be used in the control loop end_effector_frame_id = model_.getFrameId(params_.end_effector_frame); q = Eigen::VectorXd::Zero(model_.nv); q_pin = Eigen::VectorXd::Zero(model_.nq); @@ -333,6 +302,8 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta dq_ref = Eigen::VectorXd::Zero(model_.nv); tau_previous = Eigen::VectorXd::Zero(model_.nv); J = Eigen::MatrixXd::Zero(6, model_.nv); + J_pinv = Eigen::MatrixXd::Zero(model_.nv, 6); + Id_nv = Eigen::MatrixXd::Identity(model_.nv, model_.nv); // Map the friction parameters to Eigen vectors fp1 = Eigen::Map(params_.friction.fp1.data(), model_.nv); @@ -491,41 +462,51 @@ void CartesianController::setStiffnessAndDamping() { } } -CallbackReturn -CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { +void CartesianController::updateCurrentState(bool initialize) { auto num_joints = params_.joints.size(); for (size_t i = 0; i < num_joints; i++) { - // TODO(placeholder): later it might be better to get this thing prepared in the - // configuration part (not in the control loop) auto joint_name = params_.joints[i]; auto joint_id = model_.getJointId(joint_name); // pinocchio joind id might be different auto joint = model_.joints[joint_id]; #if ROS2_VERSION_ABOVE_HUMBLE - q[i] = state_interfaces_[i].get_optional().value_or(q[i]); + double q_meas = state_interfaces_[i].get_optional().value_or(q[i]); + double dq_meas = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]); #else - q[i] = state_interfaces_[i].get_value(); + double q_meas = state_interfaces_[i].get_value(); + double dq_meas = state_interfaces_[num_joints + i].get_value(); #endif - if (joint.shortname() == "JointModelRZ") { // simple revolute joint case - q_pin[joint.idx_q()] = q[i]; - } else if (continous_joint_types.count( - joint.shortname())) { // Then we are handling a continous - // joint that is SO(2) + + q_ref[i] = initialize + ? q_meas + : exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref); + + q[i] = initialize + ? q_meas + : exponential_moving_average(q[i], q_meas, params_.filter.q); + + if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous + // joint that is SO(2) q_pin[joint.idx_q()] = std::cos(q[i]); q_pin[joint.idx_q() + 1] = std::sin(q[i]); + } else { // simple revolute joint case + q_pin[joint.idx_q()] = q[i]; } - q_ref[i] = q[i]; - q_target[i] = q[i]; + dq[i] = initialize + ? dq_meas + : exponential_moving_average(dq[i], dq_meas, params_.filter.dq); -#if ROS2_VERSION_ABOVE_HUMBLE - dq[i] = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]); - dq_ref[i] = dq[i]; -#else - dq[i] = state_interfaces_[num_joints + i].get_value(); - dq_ref[i] = state_interfaces_[num_joints + i].get_value(); -#endif + q_target[i] = initialize ? q_meas : q_target[i]; } +} + +CallbackReturn +CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + + // Update the current state with initial measurements (no EMA filtering) + // to avoid large initial errors + updateCurrentState(true); pinocchio::forwardKinematics(model_, data_, q_pin, dq); pinocchio::updateFramePlacements(model_, data_); diff --git a/src/cartesian_controller.yaml b/src/cartesian_controller.yaml index a147664..b59eb26 100644 --- a/src/cartesian_controller.yaml +++ b/src/cartesian_controller.yaml @@ -19,7 +19,7 @@ cartesian_controller: operational_space_regularization: type: double - default_value: 0.1 + default_value: 0.01 description: "Regularization (damping) for Mx pseudo-inverse. Higher values improve stability near singularities but reduce accuracy. Valid range is 0.001-1.0; typical useful values are in the 0.01-0.5 range." validation: bounds<>: [0.001, 1.0]