From 939e88ed028a29564139b7e57af5ae75d9735002 Mon Sep 17 00:00:00 2001 From: Guma <44319454+Cody-Vu@users.noreply.github.com> Date: Tue, 22 Apr 2025 14:00:22 +0200 Subject: [PATCH 1/6] Delete include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h --- .../cartesian_impedance_controller_ros.h | 350 ------------------ 1 file changed, 350 deletions(-) delete mode 100644 include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h deleted file mode 100644 index 99b119b..0000000 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h +++ /dev/null @@ -1,350 +0,0 @@ -#pragma once - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -namespace cartesian_impedance_controller -{ - /*! \brief The ROS control implementation of the Cartesian impedance controller - * - * It utilizes a list of joint names and the URDF description to control these joints. - */ - class CartesianImpedanceControllerRos - : public controller_interface::Controller, public CartesianImpedanceController - { - - public: - /*! \brief Initializes the controller - * - * - Reads ROS parameters - * - Initializes - * - joint handles - * - ROS messaging - * - RBDyn - * - rqt_reconfigure - * - Trajectory handling - * \param[in] hw Hardware interface - * \param[in] node_handle Node Handle - * \return True on success, false on failure - */ - bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) override; - - /*! \brief Starts the controller - * - * Updates the states and sets the desired pose and nullspace configuration to the current state. - * \param[in] time Not used - */ - void starting(const ros::Time &) override; - - /*! \brief Periodically called update function - * - * Updates the state and the trajectory. Calculated new commands and sets them. - * Finally publishes ROS messages and tf transformations. - * \param[in] time Not used - * \param[in] period Control period - */ - void update(const ros::Time &, const ros::Duration &period) override; - - private: - /*! \brief Initializes dynamic reconfigure - * - * Initiliazes dynamic reconfigure for stiffness, damping and wrench. - * \param[in] nh Nodehandle - * \return True on success, false on failure. - */ - bool initDynamicReconfigure(const ros::NodeHandle &nh); - - /*! \brief Initializes the joint handles - * - * Fetches the joint names from the parameter server and initializes the joint handles. - * \param[in] hw Hardware interface to obtain handles - * \param[in] nh Nodehandle - * \return True on success, false on failure. - */ - bool initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh); - - /*! \brief Initializes messaging - * - * Initializes realtime publishers and the subscribers. - * \param[in] nh Nodehandle - * \return True on success, false on failure. - */ - bool initMessaging(ros::NodeHandle *nh); - - /*! \brief Initializes RBDyn - * - * Reads the robot URDF and initializes RBDyn. - * \param[in] nh Nodehandle - * \return True on success, false on failure. - */ - bool initRBDyn(const ros::NodeHandle &nh); - - /*! \brief Initializes trajectory handling - * - * Subscribes to joint trajectory topic and starts the trajectory action server. - * \param[in] nh Nodehandle - * \return Always true. - */ - bool initTrajectories(ros::NodeHandle *nh); - - /*! \brief Get forward kinematics solution. - * - * Calls RBDyn to get the forward kinematics solution. - * \param[in] q Joint position vector - * \param[out] position End-effector position - * \param[out] orientation End-effector orientation - * \return Always true. - */ - bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *rotation) const; - - /*! \brief Get Jacobian from RBDyn - * - * Gets the Jacobian for given joint positions and joint velocities. - * \param[in] q Joint position vector - * \param[in] dq Joint velocity vector - * \param[out] jacobian Calculated Jacobian - * \return True on success, false on failure. - */ - bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian); - - /*! \brief Updates the state based on the joint handles. - * - * Gets latest joint positions, velocities and efforts and updates the forward kinematics as well as the Jacobian. - */ - void updateState(); - - /*! \brief Sets damping for Cartesian space and nullspace. - * - * Long - * \param[in] cart_damping Cartesian damping [0,1] - * \param[in] nullspace Nullspace damping [0,1] - */ - void setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace); - - /*! \brief Sets Cartesian and nullspace stiffness - * - * Sets Cartesian and nullspace stiffness. Allows to set if automatic damping should be applied. - * \param[in] cart_stiffness Cartesian stiffness - * \param[in] nullspace Nullspace stiffness - * \param[in] auto_damping Apply automatic damping - */ - void setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping = true); - - /*! \brief Message callback for Cartesian damping. - * - * Calls setDampingFactors function. - * @sa setDampingFactors. - * \param[in] msg Received message - */ - void cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg); - - /*! \brief Message callback for Cartesian stiffness. - * - * Calls setStiffness function. - * @sa setStiffness - * \param[in] msg Received message - */ - void cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg); - - /*! \brief Message callback for the whole controller configuration. - * - * Sets stiffness, damping and nullspace. - * @sa setDampingFactors, setStiffness - * \param[in] msg Received message - */ - void controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg); - - /*! \brief Message callback for a Cartesian reference pose. - * - * Accepts new reference poses in the root frame - ignores them otherwise. - * Sets the reference target pose. - * @sa setReferencePose. - * \param[in] msg Received message - */ - void referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg); - - /*! \brief Message callback for Cartesian wrench messages. - * - * If the wrench is not given in end-effector frame, it will be transformed in the root frame. Once when a new wrench message arrives. - * Sets the wrench using the base library. - * @sa applyWrench. - * \param[in] msg Received message - */ - void wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg); - - /*! \brief Transforms the wrench in a target frame. - * - * Takes a vector with the wrench and transforms it to a given coordinate frame. E.g. from_frame= "world" , to_frame = "bh_link_ee" - - * @sa wrenchCommandCb - * \param[in] cartesian_wrench Vector with the Cartesian wrench - * \param[in] from_frame Source frame - * \param[in] to_frame Target frame - * \return True on success, false on failure. - */ - bool transformWrench(Eigen::Matrix *cartesian_wrench, const std::string &from_frame, const std::string &to_frame) const; - - /*! \brief Verbose printing; publishes ROS messages and tf frames. - * - * Always publishes commanded torques. - * Optional: request publishes tf frames for end-effector forward kinematics and the reference pose. - * Optional: verbose printing - * Optional: publishes state messages - */ - void publishMsgsAndTf(); - - /*! \brief Callback for stiffness dynamic reconfigure. - * - * Takes the dynamic reconfigure stiffness configuration, applies the limits and sets it. - * \param[in] config - */ - void dynamicStiffnessCb(cartesian_impedance_controller::stiffnessConfig &config, uint32_t level); - - /*! \brief Callback for damping dynamic reconfigure. - * - * Takes the dynamic reconfigure configuration, applies limits and sets it. - * \param[in] config - */ - void dynamicDampingCb(cartesian_impedance_controller::dampingConfig &config, uint32_t level); - - /*! \brief Callback for wrench dynamic reconfigure. - * - * Takes the dynamic reconfigure configuration, applies limits and sets it. - * \param[in] config - */ - void dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, uint32_t level); - - /*! \brief Callback for a joint trajectory message. - * - * Preempts the action server if that one has a running goal. - * \param[in] msg Joint Trajectory Message - */ - void trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg); - - /*! \brief Callback for a trajectory action goal. - * - * Accepts the new goal and starts the trajectory. - */ - void trajGoalCb(); - - /*! \brief Preempt function of the action server. - * - * Sets the goal as preempted. - */ - void trajPreemptCb(); - - /*! \brief Starts the trajectory. - * - * Resets the trajectory member variables. - */ - void trajStart(const trajectory_msgs::JointTrajectory &trajectory); - - /*! \brief Updates the trajectory. - * - * Called periodically from the update function if a trajectory is running. - * A trajectory is run by going through it point by point, calculating forward kinematics and applying - * the joint configuration to the nullspace control. - */ - void trajUpdate(); - - std::vector joint_handles_; //!< Joint handles for states and commands - rbdyn_wrapper rbdyn_wrapper_; //!< Wrapper for RBDyn library for kinematics - std::string end_effector_; //!< End-effector link name - std::string robot_description_; //!< URDF of the robot - std::string root_frame_; //!< Base frame obtained from URDF - - Eigen::VectorXd tau_m_; //!< Measured joint torques - - ros::Subscriber sub_cart_stiffness_; //!< Cartesian stiffness subscriber - ros::Subscriber sub_cart_wrench_; //!< Cartesian wrench subscriber - ros::Subscriber sub_damping_factors_; //!< Damping subscriber - ros::Subscriber sub_controller_config_; //!< Controller configuration subscriber - ros::Subscriber sub_reference_pose_; //!< Cartesian reference pose subscriber - - tf::TransformListener tf_listener_; //!< tf transformation listener - std::string wrench_ee_frame_; //!< Frame for the application of the commanded wrench - - // Hard limits. They are enforced on input. - const double trans_stf_min_{0}; //!< Minimum translational stiffness - const double trans_stf_max_{1500}; //!< Maximum translational stiffness - const double rot_stf_min_{0}; //!< Minimum rotational stiffness - const double rot_stf_max_{100}; //!< Maximum rotational stiffness - const double ns_min_{0}; //!< Minimum nullspace stiffness - const double ns_max_{100}; //!< Maximum nullspace stiffness - const double dmp_factor_min_{0.001}; //!< Minimum damping factor - const double dmp_factor_max_{2.0}; //!< Maximum damping factor - - // The Jacobian of RBDyn comes with orientation in the first three lines. Needs to be interchanged. - const Eigen::VectorXi perm_indices_ = - (Eigen::Matrix() << 3, 4, 5, 0, 1, 2).finished(); //!< Permutation indices to switch position and orientation - const Eigen::PermutationMatrix jacobian_perm_ = - Eigen::PermutationMatrix(perm_indices_); //!< Permutation matrix to switch position and orientation entries - - // Dynamic reconfigure - std::unique_ptr> - dynamic_server_compliance_param_; //!< Dybanic reconfigure server for stiffness - std::unique_ptr> - dynamic_server_damping_param_; //!< Dynamic reconfigure server for damping - std::unique_ptr> - dynamic_server_wrench_param_; //!< Dynamic reconfigure server for commanded wrench - - // Trajectory handling - ros::Subscriber sub_trajectory_; //!< Subscriber for a single trajectory - std::unique_ptr> traj_as_; //!< Trajectory action server - boost::shared_ptr traj_as_goal_; //!< Trajectory action server goal - trajectory_msgs::JointTrajectory trajectory_; //!< Currently played trajectory - ros::Time traj_start_; //!< Time the current trajectory is started - ros::Duration traj_duration_; //!< Duration of the current trajectory - unsigned int traj_index_{0}; //!< Index of the current trajectory point - bool traj_running_{false}; //!< True when running a trajectory - - // Extra output - bool verbose_print_{false}; //!< Verbose printing enabled - bool verbose_state_{false}; //!< Verbose state messages enabled - bool verbose_tf_{false}; //!< Verbose tf pubishing enabled - tf::TransformBroadcaster tf_br_; //!< tf transform broadcaster for verbose tf - realtime_tools::RealtimePublisher pub_torques_; //!< Realtime publisher for commanded torques - realtime_tools::RealtimePublisher pub_state_; //!< Realtime publisher for controller state - tf::Transform tf_br_transform_; //!< tf transform for publishing - tf::Vector3 tf_pos_; //!< tf position for publishing - tf::Quaternion tf_rot_; //!< tf orientation for publishing - ros::Time tf_last_time_ = ros::Time::now(); //!< Last published tf message - }; - - // Declares this controller - PLUGINLIB_EXPORT_CLASS(cartesian_impedance_controller::CartesianImpedanceControllerRos, - controller_interface::ControllerBase); - -} // namespace cartesian_impedance_controller From 91eeaee6c5b4d106c09365bcbf09ed7b400d0b6b Mon Sep 17 00:00:00 2001 From: Guma <44319454+Cody-Vu@users.noreply.github.com> Date: Tue, 22 Apr 2025 14:01:24 +0200 Subject: [PATCH 2/6] cartesian_impedance_controller_ros.h Modified cartesian_impedance_controller_ros.h. Added 2 parameters for position and orientation --- .../cartesian_impedance_controller_ros.h | 354 ++++++++++++++++++ 1 file changed, 354 insertions(+) create mode 100644 include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h new file mode 100644 index 0000000..b2fb1cd --- /dev/null +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h @@ -0,0 +1,354 @@ +#pragma once + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace cartesian_impedance_controller +{ + /*! \brief The ROS control implementation of the Cartesian impedance controller + * + * It utilizes a list of joint names and the URDF description to control these joints. + */ + class CartesianImpedanceControllerRos + : public controller_interface::Controller, public CartesianImpedanceController + { + + public: + /*! \brief Initializes the controller + * + * - Reads ROS parameters + * - Initializes + * - joint handles + * - ROS messaging + * - RBDyn + * - rqt_reconfigure + * - Trajectory handling + * \param[in] hw Hardware interface + * \param[in] node_handle Node Handle + * \return True on success, false on failure + */ + bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) override; + + /*! \brief Starts the controller + * + * Updates the states and sets the desired pose and nullspace configuration to the current state. + * \param[in] time Not used + */ + void starting(const ros::Time &) override; + + /*! \brief Periodically called update function + * + * Updates the state and the trajectory. Calculated new commands and sets them. + * Finally publishes ROS messages and tf transformations. + * \param[in] time Not used + * \param[in] period Control period + */ + void update(const ros::Time &, const ros::Duration &period) override; + + private: + /*! \brief Initializes dynamic reconfigure + * + * Initiliazes dynamic reconfigure for stiffness, damping and wrench. + * \param[in] nh Nodehandle + * \return True on success, false on failure. + */ + bool initDynamicReconfigure(const ros::NodeHandle &nh); + + /*! \brief Initializes the joint handles + * + * Fetches the joint names from the parameter server and initializes the joint handles. + * \param[in] hw Hardware interface to obtain handles + * \param[in] nh Nodehandle + * \return True on success, false on failure. + */ + bool initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh); + + /*! \brief Initializes messaging + * + * Initializes realtime publishers and the subscribers. + * \param[in] nh Nodehandle + * \return True on success, false on failure. + */ + bool initMessaging(ros::NodeHandle *nh); + + /*! \brief Initializes RBDyn + * + * Reads the robot URDF and initializes RBDyn. + * \param[in] nh Nodehandle + * \return True on success, false on failure. + */ + bool initRBDyn(const ros::NodeHandle &nh); + + /*! \brief Initializes trajectory handling + * + * Subscribes to joint trajectory topic and starts the trajectory action server. + * \param[in] nh Nodehandle + * \return Always true. + */ + bool initTrajectories(ros::NodeHandle *nh); + + /*! \brief Get forward kinematics solution. + * + * Calls RBDyn to get the forward kinematics solution. + * \param[in] q Joint position vector + * \param[out] position End-effector position + * \param[out] orientation End-effector orientation + * \return Always true. + */ + bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *rotation) const; + + /*! \brief Get Jacobian from RBDyn + * + * Gets the Jacobian for given joint positions and joint velocities. + * \param[in] q Joint position vector + * \param[in] dq Joint velocity vector + * \param[out] jacobian Calculated Jacobian + * \return True on success, false on failure. + */ + bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian); + + /*! \brief Updates the state based on the joint handles. + * + * Gets latest joint positions, velocities and efforts and updates the forward kinematics as well as the Jacobian. + */ + void updateState(); + + /*! \brief Sets damping for Cartesian space and nullspace. + * + * Long + * \param[in] cart_damping Cartesian damping [0,1] + * \param[in] nullspace Nullspace damping [0,1] + */ + void setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace); + + /*! \brief Sets Cartesian and nullspace stiffness + * + * Sets Cartesian and nullspace stiffness. Allows to set if automatic damping should be applied. + * \param[in] cart_stiffness Cartesian stiffness + * \param[in] nullspace Nullspace stiffness + * \param[in] auto_damping Apply automatic damping + */ + void setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping = true); + + /*! \brief Message callback for Cartesian damping. + * + * Calls setDampingFactors function. + * @sa setDampingFactors. + * \param[in] msg Received message + */ + void cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg); + + /*! \brief Message callback for Cartesian stiffness. + * + * Calls setStiffness function. + * @sa setStiffness + * \param[in] msg Received message + */ + void cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg); + + /*! \brief Message callback for the whole controller configuration. + * + * Sets stiffness, damping and nullspace. + * @sa setDampingFactors, setStiffness + * \param[in] msg Received message + */ + void controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg); + + /*! \brief Message callback for a Cartesian reference pose. + * + * Accepts new reference poses in the root frame - ignores them otherwise. + * Sets the reference target pose. + * @sa setReferencePose. + * \param[in] msg Received message + */ + void referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg); + + /*! \brief Message callback for Cartesian wrench messages. + * + * If the wrench is not given in end-effector frame, it will be transformed in the root frame. Once when a new wrench message arrives. + * Sets the wrench using the base library. + * @sa applyWrench. + * \param[in] msg Received message + */ + void wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg); + + /*! \brief Transforms the wrench in a target frame. + * + * Takes a vector with the wrench and transforms it to a given coordinate frame. E.g. from_frame= "world" , to_frame = "bh_link_ee" + + * @sa wrenchCommandCb + * \param[in] cartesian_wrench Vector with the Cartesian wrench + * \param[in] from_frame Source frame + * \param[in] to_frame Target frame + * \return True on success, false on failure. + */ + bool transformWrench(Eigen::Matrix *cartesian_wrench, const std::string &from_frame, const std::string &to_frame) const; + + /*! \brief Verbose printing; publishes ROS messages and tf frames. + * + * Always publishes commanded torques. + * Optional: request publishes tf frames for end-effector forward kinematics and the reference pose. + * Optional: verbose printing + * Optional: publishes state messages + */ + void publishMsgsAndTf(); + + /*! \brief Callback for stiffness dynamic reconfigure. + * + * Takes the dynamic reconfigure stiffness configuration, applies the limits and sets it. + * \param[in] config + */ + void dynamicStiffnessCb(cartesian_impedance_controller::stiffnessConfig &config, uint32_t level); + + /*! \brief Callback for damping dynamic reconfigure. + * + * Takes the dynamic reconfigure configuration, applies limits and sets it. + * \param[in] config + */ + void dynamicDampingCb(cartesian_impedance_controller::dampingConfig &config, uint32_t level); + + /*! \brief Callback for wrench dynamic reconfigure. + * + * Takes the dynamic reconfigure configuration, applies limits and sets it. + * \param[in] config + */ + void dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, uint32_t level); + + /*! \brief Callback for a joint trajectory message. + * + * Preempts the action server if that one has a running goal. + * \param[in] msg Joint Trajectory Message + */ + void trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg); + + /*! \brief Callback for a trajectory action goal. + * + * Accepts the new goal and starts the trajectory. + */ + void trajGoalCb(); + + /*! \brief Preempt function of the action server. + * + * Sets the goal as preempted. + */ + void trajPreemptCb(); + + /*! \brief Starts the trajectory. + * + * Resets the trajectory member variables. + */ + void trajStart(const trajectory_msgs::JointTrajectory &trajectory); + + /*! \brief Updates the trajectory. + * + * Called periodically from the update function if a trajectory is running. + * A trajectory is run by going through it point by point, calculating forward kinematics and applying + * the joint configuration to the nullspace control. + */ + void trajUpdate(); + + std::vector joint_handles_; //!< Joint handles for states and commands + rbdyn_wrapper rbdyn_wrapper_; //!< Wrapper for RBDyn library for kinematics + std::string end_effector_; //!< End-effector link name + std::string robot_description_; //!< URDF of the robot + std::string root_frame_; //!< Base frame obtained from URDF + + Eigen::VectorXd tau_m_; //!< Measured joint torques + + ros::Subscriber sub_cart_stiffness_; //!< Cartesian stiffness subscriber + ros::Subscriber sub_cart_wrench_; //!< Cartesian wrench subscriber + ros::Subscriber sub_damping_factors_; //!< Damping subscriber + ros::Subscriber sub_controller_config_; //!< Controller configuration subscriber + ros::Subscriber sub_reference_pose_; //!< Cartesian reference pose subscriber + + tf::TransformListener tf_listener_; //!< tf transformation listener + std::string wrench_ee_frame_; //!< Frame for the application of the commanded wrench + + // Hard limits. They are enforced on input. + const double trans_stf_min_{0}; //!< Minimum translational stiffness + const double trans_stf_max_{1500}; //!< Maximum translational stiffness + const double rot_stf_min_{0}; //!< Minimum rotational stiffness + const double rot_stf_max_{100}; //!< Maximum rotational stiffness + const double ns_min_{0}; //!< Minimum nullspace stiffness + const double ns_max_{100}; //!< Maximum nullspace stiffness + const double dmp_factor_min_{0.001}; //!< Minimum damping factor + const double dmp_factor_max_{2.0}; //!< Maximum damping factor + + // The Jacobian of RBDyn comes with orientation in the first three lines. Needs to be interchanged. + const Eigen::VectorXi perm_indices_ = + (Eigen::Matrix() << 3, 4, 5, 0, 1, 2).finished(); //!< Permutation indices to switch position and orientation + const Eigen::PermutationMatrix jacobian_perm_ = + Eigen::PermutationMatrix(perm_indices_); //!< Permutation matrix to switch position and orientation entries + + // Dynamic reconfigure + std::unique_ptr> + dynamic_server_compliance_param_; //!< Dybanic reconfigure server for stiffness + std::unique_ptr> + dynamic_server_damping_param_; //!< Dynamic reconfigure server for damping + std::unique_ptr> + dynamic_server_wrench_param_; //!< Dynamic reconfigure server for commanded wrench + + // Trajectory handling + ros::Subscriber sub_trajectory_; //!< Subscriber for a single trajectory + std::unique_ptr> traj_as_; //!< Trajectory action server + boost::shared_ptr traj_as_goal_; //!< Trajectory action server goal + trajectory_msgs::JointTrajectory trajectory_; //!< Currently played trajectory + ros::Time traj_start_; //!< Time the current trajectory is started + ros::Duration traj_duration_; //!< Duration of the current trajectory + unsigned int traj_index_{0}; //!< Index of the current trajectory point + bool traj_running_{false}; //!< True when running a trajectory + + // Cartesian trajectory interpolation + std::vector traj_positions_; //!< Cached Cartesian positions for trajectory points + std::vector traj_orientations_; //!< Cached Cartesian orientations for trajectory points + + // Extra output + bool verbose_print_{false}; //!< Verbose printing enabled + bool verbose_state_{false}; //!< Verbose state messages enabled + bool verbose_tf_{false}; //!< Verbose tf pubishing enabled + tf::TransformBroadcaster tf_br_; //!< tf transform broadcaster for verbose tf + realtime_tools::RealtimePublisher pub_torques_; //!< Realtime publisher for commanded torques + realtime_tools::RealtimePublisher pub_state_; //!< Realtime publisher for controller state + tf::Transform tf_br_transform_; //!< tf transform for publishing + tf::Vector3 tf_pos_; //!< tf position for publishing + tf::Quaternion tf_rot_; //!< tf orientation for publishing + ros::Time tf_last_time_ = ros::Time::now(); //!< Last published tf message + }; + + // Declares this controller + PLUGINLIB_EXPORT_CLASS(cartesian_impedance_controller::CartesianImpedanceControllerRos, + controller_interface::ControllerBase); + +} // namespace cartesian_impedance_controller From 99b92406c8960af21c4128a7dc3d587aa92056a0 Mon Sep 17 00:00:00 2001 From: Guma <44319454+Cody-Vu@users.noreply.github.com> Date: Tue, 22 Apr 2025 14:01:44 +0200 Subject: [PATCH 3/6] Delete src/cartesian_impedance_controller_ros.cpp --- src/cartesian_impedance_controller_ros.cpp | 598 --------------------- 1 file changed, 598 deletions(-) delete mode 100644 src/cartesian_impedance_controller_ros.cpp diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp deleted file mode 100644 index 6354ff0..0000000 --- a/src/cartesian_impedance_controller_ros.cpp +++ /dev/null @@ -1,598 +0,0 @@ -#include - -#include -#include - -namespace cartesian_impedance_controller -{ - /*! \brief Saturate a variable x with the limits x_min and x_max - * - * \param[in] x Value - * \param[in] x_min Minimal value - * \param[in] x_max Maximum value - * \return Saturated value - */ - double saturateValue(double x, double x_min, double x_max) - { - return std::min(std::max(x, x_min), x_max); - } - - /*! \brief Populates a wrench msg with value from Eigen vector - * - * It is assumed that the vector has the form transl_x, transl_y, transl_z, rot_x, rot_y, rot_z - * \param[in] v Input vector - * \param[out] wrench Wrench message - */ - void EigenVectorToWrench(const Eigen::Matrix &v, geometry_msgs::Wrench *wrench) - { - wrench->force.x = v(0); - wrench->force.y = v(1); - wrench->force.z = v(2); - wrench->torque.x = v(3); - wrench->torque.y = v(4); - wrench->torque.z = v(5); - } - - bool CartesianImpedanceControllerRos::initDynamicReconfigure(const ros::NodeHandle &nh) - { - this->dynamic_server_compliance_param_ = std::make_unique>(ros::NodeHandle(std::string(nh.getNamespace() + "/stiffness_reconfigure"))); - this->dynamic_server_compliance_param_->setCallback( - boost::bind(&CartesianImpedanceControllerRos::dynamicStiffnessCb, this, _1, _2)); - - this->dynamic_server_damping_param_ = std::make_unique>(ros::NodeHandle(std::string(nh.getNamespace() + "/damping_factors_reconfigure"))); - dynamic_server_damping_param_->setCallback( - boost::bind(&CartesianImpedanceControllerRos::dynamicDampingCb, this, _1, _2)); - - this->dynamic_server_wrench_param_ = std::make_unique>(ros::NodeHandle(std::string(nh.getNamespace() + "/cartesian_wrench_reconfigure"))); - dynamic_server_wrench_param_->setCallback( - boost::bind(&CartesianImpedanceControllerRos::dynamicWrenchCb, this, _1, _2)); - return true; - } - - bool CartesianImpedanceControllerRos::initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh) - { - std::vector joint_names; - if (!nh.getParam("joints", joint_names)) - { - ROS_ERROR("Invalid or no 'joints' parameter provided, aborting controller init!"); - return false; - } - for (size_t i = 0; i < joint_names.size(); ++i) - { - try - { - this->joint_handles_.push_back(hw->getHandle(joint_names[i])); - } - catch (const hardware_interface::HardwareInterfaceException &ex) - { - ROS_ERROR_STREAM("Exception getting joint handles: " << ex.what()); - return false; - } - } - ROS_INFO_STREAM("Number of joints specified in parameters: " << joint_names.size()); - this->setNumberOfJoints(joint_names.size()); - return true; - } - - bool CartesianImpedanceControllerRos::initMessaging(ros::NodeHandle *nh) - { - // Queue size of 1 since we are only interested in the last message - this->sub_cart_stiffness_ = nh->subscribe("set_cartesian_stiffness", 1, - &CartesianImpedanceControllerRos::cartesianStiffnessCb, this); - this->sub_cart_wrench_ = nh->subscribe("set_cartesian_wrench", 1, - &CartesianImpedanceControllerRos::wrenchCommandCb, this); - this->sub_damping_factors_ = nh->subscribe("set_damping_factors", 1, - &CartesianImpedanceControllerRos::cartesianDampingFactorCb, this); - this->sub_controller_config_ = - nh->subscribe("set_config", 1, &CartesianImpedanceControllerRos::controllerConfigCb, this); - this->sub_reference_pose_ = nh->subscribe("reference_pose", 1, &CartesianImpedanceControllerRos::referencePoseCb, this); - - // Initializing the realtime publisher and the message - this->pub_torques_.init(*nh, "commanded_torques", 20); - this->pub_torques_.msg_.layout.dim.resize(1); - this->pub_torques_.msg_.layout.data_offset = 0; - this->pub_torques_.msg_.layout.dim[0].size = this->n_joints_; - this->pub_torques_.msg_.layout.dim[0].stride = 0; - this->pub_torques_.msg_.data.resize(this->n_joints_); - - std::vector joint_names; - nh->getParam("joints", joint_names); - this->pub_state_.init(*nh, "controller_state", 10); - this->pub_state_.msg_.header.seq = 0; - for (size_t i = 0; i < this->n_joints_; i++) - { - this->pub_state_.msg_.joint_state.name.push_back(joint_names.at(i)); - } - this->pub_state_.msg_.joint_state.position = std::vector(this->n_joints_); - this->pub_state_.msg_.joint_state.velocity = std::vector(this->n_joints_); - this->pub_state_.msg_.joint_state.effort = std::vector(this->n_joints_); - this->pub_state_.msg_.commanded_torques = std::vector(this->n_joints_); - this->pub_state_.msg_.nullspace_config = std::vector(this->n_joints_); - return true; - } - - bool CartesianImpedanceControllerRos::initRBDyn(const ros::NodeHandle &nh) - { - // Get the URDF XML from the parameter server. Wait if needed. - std::string urdf_string; - nh.param("robot_description", robot_description_, "/robot_description"); - while (!nh.getParam(robot_description_, urdf_string)) - { - ROS_INFO_ONCE("Waiting for robot description in parameter %s on the ROS param server.", - robot_description_.c_str()); - usleep(100000); - } - try - { - this->rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_); - } - catch (std::runtime_error e) - { - ROS_ERROR("Error when intializing RBDyn: %s", e.what()); - return false; - } - ROS_INFO_STREAM("Number of joints found in urdf: " << this->rbdyn_wrapper_.n_joints()); - if (this->rbdyn_wrapper_.n_joints() < this->n_joints_) - { - ROS_ERROR("Number of joints in the URDF is smaller than supplied number of joints. %i < %zu", this->rbdyn_wrapper_.n_joints(), this->n_joints_); - return false; - } - else if (this->rbdyn_wrapper_.n_joints() > this->n_joints_) - { - ROS_WARN("Number of joints in the URDF is greater than supplied number of joints: %i > %zu. Assuming that the actuated joints come first.", this->rbdyn_wrapper_.n_joints(), this->n_joints_); - } - return true; - } - - bool CartesianImpedanceControllerRos::initTrajectories(ros::NodeHandle *nh) - { - this->sub_trajectory_ = nh->subscribe("joint_trajectory", 1, &CartesianImpedanceControllerRos::trajCb, this); - this->traj_as_ = std::unique_ptr>( - new actionlib::SimpleActionServer( - *nh, std::string("follow_joint_trajectory"), false)); - this->traj_as_->registerGoalCallback(boost::bind(&CartesianImpedanceControllerRos::trajGoalCb, this)); - this->traj_as_->registerPreemptCallback(boost::bind(&CartesianImpedanceControllerRos::trajPreemptCb, this)); - this->traj_as_->start(); - return true; - } - - bool CartesianImpedanceControllerRos::init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) - { - ROS_INFO("Initializing Cartesian impedance controller in namespace: %s", node_handle.getNamespace().c_str()); - - // Fetch parameters - node_handle.param("end_effector", this->end_effector_, "iiwa_link_ee"); - ROS_INFO_STREAM("End effektor link is: " << this->end_effector_); - // Frame for applying commanded Cartesian wrenches - node_handle.param("wrench_ee_frame", this->wrench_ee_frame_, this->end_effector_); - bool dynamic_reconfigure{true}; - node_handle.param("dynamic_reconfigure", dynamic_reconfigure, true); - bool enable_trajectories{true}; - node_handle.param("handle_trajectories", enable_trajectories, true); - node_handle.param("delta_tau_max", this->delta_tau_max_, 1.); - node_handle.param("update_frequency", this->update_frequency_, 500.); - node_handle.param("filtering/nullspace_config", this->filter_params_nullspace_config_, 0.1); - node_handle.param("filtering/stiffness", this->filter_params_stiffness_, 0.1); - node_handle.param("filtering/pose", this->filter_params_pose_, 0.1); - node_handle.param("filtering/wrench", this->filter_params_wrench_, 0.1); - node_handle.param("verbosity/verbose_print", this->verbose_print_, false); - node_handle.param("verbosity/state_msgs", this->verbose_state_, false); - node_handle.param("verbosity/tf_frames", this->verbose_tf_, false); - - if (!this->initJointHandles(hw, node_handle) || !this->initMessaging(&node_handle) || !this->initRBDyn(node_handle)) - { - return false; - } - if (enable_trajectories && !this->initTrajectories(&node_handle)) - { - return false; - } - this->root_frame_ = this->rbdyn_wrapper_.root_link(); - node_handle.setParam("root_frame", this->root_frame_); - - // Initialize base_tools and member variables - this->setNumberOfJoints(this->joint_handles_.size()); - if (this->n_joints_ < 6) - { - ROS_WARN("Number of joints is below 6. Functions might be limited."); - } - if (this->n_joints_ < 7) - { - ROS_WARN("Number of joints is below 7. No redundant joint for nullspace."); - } - this->tau_m_ = Eigen::VectorXd(this->n_joints_); - - // Needs to be after base_tools init since the wrench callback calls it - if (dynamic_reconfigure && !this->initDynamicReconfigure(node_handle)) - { - return false; - } - - ROS_INFO("Finished initialization."); - return true; - } - - void CartesianImpedanceControllerRos::starting(const ros::Time & /*time*/) - { - this->updateState(); - - // Set reference pose to current pose and q_d_nullspace - this->initDesiredPose(this->position_, this->orientation_); - this->initNullspaceConfig(this->q_); - ROS_INFO("Started Cartesian Impedance Controller"); - } - - void CartesianImpedanceControllerRos::update(const ros::Time & /*time*/, const ros::Duration &period /*period*/) - { - if (this->traj_running_) - { - trajUpdate(); - } - - this->updateState(); - - // Apply control law in base library - this->calculateCommandedTorques(); - - // Write commands - for (size_t i = 0; i < this->n_joints_; ++i) - { - this->joint_handles_[i].setCommand(this->tau_c_(i)); - } - - publishMsgsAndTf(); - } - - bool CartesianImpedanceControllerRos::getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, - Eigen::Quaterniond *orientation) const - { - rbdyn_wrapper::EefState ee_state; - // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known - if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) - { - Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); - q_rb.head(q.size()) = q; - ee_state = this->rbdyn_wrapper_.perform_fk(q_rb); - } - else - { - ee_state = this->rbdyn_wrapper_.perform_fk(q); - } - *position = ee_state.translation; - *orientation = ee_state.orientation; - return true; - } - - bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, - Eigen::MatrixXd *jacobian) - { - // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known - if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) - { - Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); - q_rb.head(q.size()) = q; - Eigen::VectorXd dq_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); - dq_rb.head(dq.size()) = dq; - *jacobian = this->rbdyn_wrapper_.jacobian(q_rb, dq_rb); - } - else - { - *jacobian = this->rbdyn_wrapper_.jacobian(q, dq); - } - *jacobian = jacobian_perm_ * *jacobian; - return true; - } - - void CartesianImpedanceControllerRos::updateState() - { - for (size_t i = 0; i < this->n_joints_; ++i) - { - this->q_[i] = this->joint_handles_[i].getPosition(); - this->dq_[i] = this->joint_handles_[i].getVelocity(); - this->tau_m_[i] = this->joint_handles_[i].getEffort(); - } - getJacobian(this->q_, this->dq_, &this->jacobian_); - getFk(this->q_, &this->position_, &this->orientation_); - } - - void CartesianImpedanceControllerRos::controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg) - { - this->setStiffness(msg->cartesian_stiffness, msg->nullspace_stiffness, false); - this->setDampingFactors(msg->cartesian_damping_factors, msg->nullspace_damping_factor); - - if (msg->q_d_nullspace.size() == this->n_joints_) - { - Eigen::VectorXd q_d_nullspace(this->n_joints_); - for (size_t i = 0; i < this->n_joints_; i++) - { - q_d_nullspace(i) = msg->q_d_nullspace.at(i); - } - this->setNullspaceConfig(q_d_nullspace); - } - else - { - ROS_WARN_STREAM("Nullspace configuration does not have the correct amount of entries. Got " << msg->q_d_nullspace.size() << " expected " << this->n_joints_ << ". Ignoring."); - } - } - - void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg) - { - this->setDampingFactors(*msg, this->damping_factors_[6]); - } - - void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg) - { - if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_) - { - ROS_WARN_STREAM("Reference poses need to be in the root frame '" << this->root_frame_ << "'. Ignoring."); - return; - } - Eigen::Vector3d position_d; - position_d << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; - const Eigen::Quaterniond last_orientation_d_target(this->orientation_d_); - Eigen::Quaterniond orientation_d; - orientation_d.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, - msg->pose.orientation.w; - if (last_orientation_d_target.coeffs().dot(this->orientation_d_.coeffs()) < 0.0) - { - this->orientation_d_.coeffs() << -this->orientation_d_.coeffs(); - } - this->setReferencePose(position_d, orientation_d); - } - - void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg) - { - this->setStiffness(msg->wrench, this->nullspace_stiffness_target_); - } - - void CartesianImpedanceControllerRos::setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace) - { - CartesianImpedanceController::setDampingFactors(saturateValue(cart_damping.force.x, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.force.y, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.force.z, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.torque.x, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.torque.y, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.torque.z, dmp_factor_min_, dmp_factor_max_), - saturateValue(nullspace, dmp_factor_min_, dmp_factor_max_)); - } - - void CartesianImpedanceControllerRos::setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping) - { - CartesianImpedanceController::setStiffness(saturateValue(cart_stiffness.force.x, trans_stf_min_, trans_stf_max_), - saturateValue(cart_stiffness.force.y, trans_stf_min_, trans_stf_max_), - saturateValue(cart_stiffness.force.z, trans_stf_min_, trans_stf_max_), - saturateValue(cart_stiffness.torque.x, rot_stf_min_, rot_stf_max_), - saturateValue(cart_stiffness.torque.y, rot_stf_min_, rot_stf_max_), - saturateValue(cart_stiffness.torque.z, rot_stf_min_, rot_stf_max_), - saturateValue(nullspace, ns_min_, ns_max_), auto_damping); - } - - void CartesianImpedanceControllerRos::wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg) - { - Eigen::Matrix F; - F << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, msg->wrench.torque.y, - msg->wrench.torque.z; - - if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_) - { - if (!transformWrench(&F, msg->header.frame_id, this->root_frame_)) - { - ROS_ERROR("Could not transform wrench. Not applying it."); - return; - } - } - else if (msg->header.frame_id.empty()) - { - if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) - { - ROS_ERROR("Could not transform wrench. Not applying it."); - return; - } - } - this->applyWrench(F); - } - - bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix *cartesian_wrench, - const std::string &from_frame, const std::string &to_frame) const - { - try - { - tf::StampedTransform transform; - tf_listener_.lookupTransform(to_frame, from_frame, ros::Time(0), transform); - tf::Vector3 v_f(cartesian_wrench->operator()(0), cartesian_wrench->operator()(1), cartesian_wrench->operator()(2)); - tf::Vector3 v_t(cartesian_wrench->operator()(3), cartesian_wrench->operator()(4), cartesian_wrench->operator()(5)); - tf::Vector3 v_f_rot = tf::quatRotate(transform.getRotation(), v_f); - tf::Vector3 v_t_rot = tf::quatRotate(transform.getRotation(), v_t); - *cartesian_wrench << v_f_rot[0], v_f_rot[1], v_f_rot[2], v_t_rot[0], v_t_rot[1], v_t_rot[2]; - return true; - } - catch (const tf::TransformException &ex) - { - ROS_ERROR_THROTTLE(1, "%s", ex.what()); - return false; - } - } - - void CartesianImpedanceControllerRos::publishMsgsAndTf() - { - // publish commanded torques - if (this->pub_torques_.trylock()) - { - for (size_t i = 0; i < this->n_joints_; i++) - { - this->pub_torques_.msg_.data[i] = this->tau_c_[i]; - } - this->pub_torques_.unlockAndPublish(); - } - - const Eigen::Matrix error{this->getPoseError()}; - - if (this->verbose_print_) - { - ROS_INFO_STREAM_THROTTLE(0.1, "\nCartesian Position:\n" - << this->position_ << "\nError:\n" - << error << "\nCartesian Stiffness:\n" - << this->cartesian_stiffness_ << "\nCartesian damping:\n" - << this->cartesian_damping_ << "\nNullspace stiffness:\n" - << this->nullspace_stiffness_ << "\nq_d_nullspace:\n" - << this->q_d_nullspace_ << "\ntau_d:\n" - << this->tau_c_); - } - if (this->verbose_tf_ && ros::Time::now() > this->tf_last_time_) - { - // Publish result of forward kinematics - tf::vectorEigenToTF(this->position_, this->tf_pos_); - this->tf_br_transform_.setOrigin(this->tf_pos_); - tf::quaternionEigenToTF(this->orientation_, this->tf_rot_); - this->tf_br_transform_.setRotation(this->tf_rot_); - tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_fk")); - // Publish tf to the reference pose - tf::vectorEigenToTF(this->position_d_, this->tf_pos_); - this->tf_br_transform_.setOrigin(this->tf_pos_); - tf::quaternionEigenToTF(this->orientation_d_, this->tf_rot_); - this->tf_br_transform_.setRotation(this->tf_rot_); - tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_ref_pose")); - this->tf_last_time_ = ros::Time::now(); - } - if (this->verbose_state_ && this->pub_state_.trylock()) - { - this->pub_state_.msg_.header.stamp = ros::Time::now(); - tf::pointEigenToMsg(this->position_, this->pub_state_.msg_.current_pose.position); - tf::quaternionEigenToMsg(this->orientation_, this->pub_state_.msg_.current_pose.orientation); - tf::pointEigenToMsg(this->position_d_, this->pub_state_.msg_.reference_pose.position); - tf::quaternionEigenToMsg(this->orientation_d_, this->pub_state_.msg_.reference_pose.orientation); - tf::pointEigenToMsg(error.head(3), this->pub_state_.msg_.pose_error.position); - Eigen::Quaterniond q = Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ()); - tf::quaternionEigenToMsg(q, this->pub_state_.msg_.pose_error.orientation); - - EigenVectorToWrench(this->cartesian_stiffness_.diagonal(), &this->pub_state_.msg_.cartesian_stiffness); - EigenVectorToWrench(this->cartesian_damping_.diagonal(), &this->pub_state_.msg_.cartesian_damping); - EigenVectorToWrench(this->getAppliedWrench(), &this->pub_state_.msg_.commanded_wrench); - - for (size_t i = 0; i < this->n_joints_; i++) - { - this->pub_state_.msg_.joint_state.position.at(i) = this->q_(i); - this->pub_state_.msg_.joint_state.velocity.at(i) = this->dq_(i); - this->pub_state_.msg_.joint_state.effort.at(i) = this->tau_m_(i); - this->pub_state_.msg_.nullspace_config.at(i) = this->q_d_nullspace_(i); - this->pub_state_.msg_.commanded_torques.at(i) = this->tau_c_(i); - } - this->pub_state_.msg_.nullspace_stiffness = this->nullspace_stiffness_; - this->pub_state_.msg_.nullspace_damping = this->nullspace_damping_; - const Eigen::Matrix dx = this->jacobian_ * this->dq_; - this->pub_state_.msg_.cartesian_velocity = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); - - this->pub_state_.unlockAndPublish(); - this->pub_state_.msg_.header.seq++; - } - } - - // Dynamic reconfigure - // -------------------------------------------------------------------------------------------------------------------------------------- - void CartesianImpedanceControllerRos::dynamicStiffnessCb( - cartesian_impedance_controller::stiffnessConfig &config, uint32_t level) - { - if (config.update_stiffness) - { - CartesianImpedanceController::setStiffness(saturateValue(config.translation_x, trans_stf_min_, trans_stf_max_), - saturateValue(config.translation_y, trans_stf_min_, trans_stf_max_), - saturateValue(config.translation_z, trans_stf_min_, trans_stf_max_), - saturateValue(config.rotation_x, trans_stf_min_, trans_stf_max_), - saturateValue(config.rotation_y, trans_stf_min_, trans_stf_max_), - saturateValue(config.rotation_z, trans_stf_min_, trans_stf_max_), config.nullspace_stiffness); - } - } - - void CartesianImpedanceControllerRos::dynamicDampingCb( - cartesian_impedance_controller::dampingConfig &config, uint32_t level) - { - if (config.update_damping_factors) - { - CartesianImpedanceController::setDampingFactors( - config.translation_x, config.translation_y, config.translation_z, config.rotation_x, config.rotation_y, config.rotation_z, config.nullspace_damping); - } - } - - void CartesianImpedanceControllerRos::dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, - uint32_t level) - { - Eigen::Vector6d F{Eigen::Vector6d::Zero()}; - if (config.apply_wrench) - { - F << config.f_x, config.f_y, config.f_z, config.tau_x, config.tau_y, config.tau_z; - if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) - { - ROS_ERROR("Could not transform wrench. Not applying it."); - return; - } - } - this->applyWrench(F); - } - - void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg) - { - ROS_INFO("Got trajectory msg from trajectory topic."); - if (this->traj_as_->isActive()) - { - this->traj_as_->setPreempted(); - ROS_INFO("Preempted running action server goal."); - } - trajStart(*msg); - } - - void CartesianImpedanceControllerRos::trajGoalCb() - { - this->traj_as_goal_ = this->traj_as_->acceptNewGoal(); - ROS_INFO("Accepted new goal from action server."); - trajStart(this->traj_as_goal_->trajectory); - } - - void CartesianImpedanceControllerRos::trajPreemptCb() - { - ROS_INFO("Actionserver got preempted."); - this->traj_as_->setPreempted(); - } - - void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::JointTrajectory &trajectory) - { - this->traj_duration_ = trajectory.points[trajectory.points.size() - 1].time_from_start; - ROS_INFO_STREAM("Starting a new trajectory with " << trajectory.points.size() << " points that takes " << this->traj_duration_ << "s."); - this->trajectory_ = trajectory; - this->traj_running_ = true; - this->traj_start_ = ros::Time::now(); - this->traj_index_ = 0; - trajUpdate(); - if (this->nullspace_stiffness_ < 5.) - { - ROS_WARN("Nullspace stiffness is low. The joints might not follow the planned path."); - } - } - - void CartesianImpedanceControllerRos::trajUpdate() - { - if (ros::Time::now() > (this->traj_start_ + trajectory_.points.at(this->traj_index_).time_from_start)) - { - // Get end effector pose - Eigen::VectorXd q = Eigen::VectorXd::Map(trajectory_.points.at(this->traj_index_).positions.data(), - trajectory_.points.at(this->traj_index_).positions.size()); - if (this->verbose_print_) - { - ROS_INFO_STREAM("Index " << this->traj_index_ << " q_nullspace: " << q.transpose()); - } - // Update end-effector pose and nullspace - getFk(q, &this->position_d_target_, &this->orientation_d_target_); - this->setNullspaceConfig(q); - this->traj_index_++; - } - - if (ros::Time::now() > (this->traj_start_ + this->traj_duration_)) - { - ROS_INFO_STREAM("Finished executing trajectory."); - if (this->traj_as_->isActive()) - { - this->traj_as_->setSucceeded(); - } - this->traj_running_ = false; - } - } -} // namespace cartesian_impedance_controller From 334d5cf681481cd11fb9b7f5b2f5c10310a19198 Mon Sep 17 00:00:00 2001 From: Guma <44319454+Cody-Vu@users.noreply.github.com> Date: Tue, 22 Apr 2025 14:03:13 +0200 Subject: [PATCH 4/6] cartesian_impedance_controller_ros.cpp Modified cartesian_impedance_controller_ros.cpp. Implementation of linear Interpolation and Slerp algorithm for orientation. --- src/cartesian_impedance_controller_ros.cpp | 714 +++++++++++++++++++++ 1 file changed, 714 insertions(+) create mode 100644 src/cartesian_impedance_controller_ros.cpp diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp new file mode 100644 index 0000000..1de0210 --- /dev/null +++ b/src/cartesian_impedance_controller_ros.cpp @@ -0,0 +1,714 @@ +#include + +#include +#include + +namespace cartesian_impedance_controller +{ + /*! \brief Saturate a variable x with the limits x_min and x_max + * + * \param[in] x Value + * \param[in] x_min Minimal value + * \param[in] x_max Maximum value + * \return Saturated value + */ + double saturateValue(double x, double x_min, double x_max) + { + return std::min(std::max(x, x_min), x_max); + } + + /*! \brief Populates a wrench msg with value from Eigen vector + * + * It is assumed that the vector has the form transl_x, transl_y, transl_z, rot_x, rot_y, rot_z + * \param[in] v Input vector + * \param[out] wrench Wrench message + */ + void EigenVectorToWrench(const Eigen::Matrix &v, geometry_msgs::Wrench *wrench) + { + wrench->force.x = v(0); + wrench->force.y = v(1); + wrench->force.z = v(2); + wrench->torque.x = v(3); + wrench->torque.y = v(4); + wrench->torque.z = v(5); + } + + bool CartesianImpedanceControllerRos::initDynamicReconfigure(const ros::NodeHandle &nh) + { + this->dynamic_server_compliance_param_ = std::make_unique>(ros::NodeHandle(std::string(nh.getNamespace() + "/stiffness_reconfigure"))); + this->dynamic_server_compliance_param_->setCallback( + boost::bind(&CartesianImpedanceControllerRos::dynamicStiffnessCb, this, _1, _2)); + + this->dynamic_server_damping_param_ = std::make_unique>(ros::NodeHandle(std::string(nh.getNamespace() + "/damping_factors_reconfigure"))); + dynamic_server_damping_param_->setCallback( + boost::bind(&CartesianImpedanceControllerRos::dynamicDampingCb, this, _1, _2)); + + this->dynamic_server_wrench_param_ = std::make_unique>(ros::NodeHandle(std::string(nh.getNamespace() + "/cartesian_wrench_reconfigure"))); + dynamic_server_wrench_param_->setCallback( + boost::bind(&CartesianImpedanceControllerRos::dynamicWrenchCb, this, _1, _2)); + return true; + } + + bool CartesianImpedanceControllerRos::initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh) + { + std::vector joint_names; + if (!nh.getParam("joints", joint_names)) + { + ROS_ERROR("Invalid or no 'joints' parameter provided, aborting controller init!"); + return false; + } + for (size_t i = 0; i < joint_names.size(); ++i) + { + try + { + this->joint_handles_.push_back(hw->getHandle(joint_names[i])); + } + catch (const hardware_interface::HardwareInterfaceException &ex) + { + ROS_ERROR_STREAM("Exception getting joint handles: " << ex.what()); + return false; + } + } + ROS_INFO_STREAM("Number of joints specified in parameters: " << joint_names.size()); + this->setNumberOfJoints(joint_names.size()); + return true; + } + + bool CartesianImpedanceControllerRos::initMessaging(ros::NodeHandle *nh) + { + // Queue size of 1 since we are only interested in the last message + this->sub_cart_stiffness_ = nh->subscribe("set_cartesian_stiffness", 1, + &CartesianImpedanceControllerRos::cartesianStiffnessCb, this); + this->sub_cart_wrench_ = nh->subscribe("set_cartesian_wrench", 1, + &CartesianImpedanceControllerRos::wrenchCommandCb, this); + this->sub_damping_factors_ = nh->subscribe("set_damping_factors", 1, + &CartesianImpedanceControllerRos::cartesianDampingFactorCb, this); + this->sub_controller_config_ = + nh->subscribe("set_config", 1, &CartesianImpedanceControllerRos::controllerConfigCb, this); + this->sub_reference_pose_ = nh->subscribe("reference_pose", 1, &CartesianImpedanceControllerRos::referencePoseCb, this); + + // Initializing the realtime publisher and the message + this->pub_torques_.init(*nh, "commanded_torques", 20); + this->pub_torques_.msg_.layout.dim.resize(1); + this->pub_torques_.msg_.layout.data_offset = 0; + this->pub_torques_.msg_.layout.dim[0].size = this->n_joints_; + this->pub_torques_.msg_.layout.dim[0].stride = 0; + this->pub_torques_.msg_.data.resize(this->n_joints_); + + std::vector joint_names; + nh->getParam("joints", joint_names); + this->pub_state_.init(*nh, "controller_state", 10); + this->pub_state_.msg_.header.seq = 0; + for (size_t i = 0; i < this->n_joints_; i++) + { + this->pub_state_.msg_.joint_state.name.push_back(joint_names.at(i)); + } + this->pub_state_.msg_.joint_state.position = std::vector(this->n_joints_); + this->pub_state_.msg_.joint_state.velocity = std::vector(this->n_joints_); + this->pub_state_.msg_.joint_state.effort = std::vector(this->n_joints_); + this->pub_state_.msg_.commanded_torques = std::vector(this->n_joints_); + this->pub_state_.msg_.nullspace_config = std::vector(this->n_joints_); + return true; + } + + bool CartesianImpedanceControllerRos::initRBDyn(const ros::NodeHandle &nh) + { + // Get the URDF XML from the parameter server. Wait if needed. + std::string urdf_string; + nh.param("robot_description", robot_description_, "/robot_description"); + while (!nh.getParam(robot_description_, urdf_string)) + { + ROS_INFO_ONCE("Waiting for robot description in parameter %s on the ROS param server.", + robot_description_.c_str()); + usleep(100000); + } + try + { + this->rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_); + } + catch (std::runtime_error e) + { + ROS_ERROR("Error when intializing RBDyn: %s", e.what()); + return false; + } + ROS_INFO_STREAM("Number of joints found in urdf: " << this->rbdyn_wrapper_.n_joints()); + if (this->rbdyn_wrapper_.n_joints() < this->n_joints_) + { + ROS_ERROR("Number of joints in the URDF is smaller than supplied number of joints. %i < %zu", this->rbdyn_wrapper_.n_joints(), this->n_joints_); + return false; + } + else if (this->rbdyn_wrapper_.n_joints() > this->n_joints_) + { + ROS_WARN("Number of joints in the URDF is greater than supplied number of joints: %i > %zu. Assuming that the actuated joints come first.", this->rbdyn_wrapper_.n_joints(), this->n_joints_); + } + return true; + } + + bool CartesianImpedanceControllerRos::initTrajectories(ros::NodeHandle *nh) + { + this->sub_trajectory_ = nh->subscribe("joint_trajectory", 1, &CartesianImpedanceControllerRos::trajCb, this); + this->traj_as_ = std::unique_ptr>( + new actionlib::SimpleActionServer( + *nh, std::string("follow_joint_trajectory"), false)); + this->traj_as_->registerGoalCallback(boost::bind(&CartesianImpedanceControllerRos::trajGoalCb, this)); + this->traj_as_->registerPreemptCallback(boost::bind(&CartesianImpedanceControllerRos::trajPreemptCb, this)); + this->traj_as_->start(); + return true; + } + + bool CartesianImpedanceControllerRos::init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) + { + ROS_INFO("Initializing Cartesian impedance controller in namespace: %s", node_handle.getNamespace().c_str()); + + // Fetch parameters + node_handle.param("end_effector", this->end_effector_, "iiwa_link_ee"); + ROS_INFO_STREAM("End effektor link is: " << this->end_effector_); + // Frame for applying commanded Cartesian wrenches + node_handle.param("wrench_ee_frame", this->wrench_ee_frame_, this->end_effector_); + bool dynamic_reconfigure{true}; + node_handle.param("dynamic_reconfigure", dynamic_reconfigure, true); + bool enable_trajectories{true}; + node_handle.param("handle_trajectories", enable_trajectories, true); + node_handle.param("delta_tau_max", this->delta_tau_max_, 1.); + node_handle.param("update_frequency", this->update_frequency_, 500.); + node_handle.param("filtering/nullspace_config", this->filter_params_nullspace_config_, 0.1); + node_handle.param("filtering/stiffness", this->filter_params_stiffness_, 0.1); + node_handle.param("filtering/pose", this->filter_params_pose_, 0.1); + node_handle.param("filtering/wrench", this->filter_params_wrench_, 0.1); + node_handle.param("verbosity/verbose_print", this->verbose_print_, false); + node_handle.param("verbosity/state_msgs", this->verbose_state_, false); + node_handle.param("verbosity/tf_frames", this->verbose_tf_, false); + + if (!this->initJointHandles(hw, node_handle) || !this->initMessaging(&node_handle) || !this->initRBDyn(node_handle)) + { + return false; + } + if (enable_trajectories && !this->initTrajectories(&node_handle)) + { + return false; + } + this->root_frame_ = this->rbdyn_wrapper_.root_link(); + node_handle.setParam("root_frame", this->root_frame_); + + // Initialize base_tools and member variables + this->setNumberOfJoints(this->joint_handles_.size()); + if (this->n_joints_ < 6) + { + ROS_WARN("Number of joints is below 6. Functions might be limited."); + } + if (this->n_joints_ < 7) + { + ROS_WARN("Number of joints is below 7. No redundant joint for nullspace."); + } + this->tau_m_ = Eigen::VectorXd(this->n_joints_); + + // Needs to be after base_tools init since the wrench callback calls it + if (dynamic_reconfigure && !this->initDynamicReconfigure(node_handle)) + { + return false; + } + + ROS_INFO("Finished initialization."); + return true; + } + + void CartesianImpedanceControllerRos::starting(const ros::Time & /*time*/) + { + this->updateState(); + + // Set reference pose to current pose and q_d_nullspace + this->initDesiredPose(this->position_, this->orientation_); + this->initNullspaceConfig(this->q_); + ROS_INFO("Started Cartesian Impedance Controller"); + } + + void CartesianImpedanceControllerRos::update(const ros::Time & /*time*/, const ros::Duration &period /*period*/) + { + if (this->traj_running_) + { + trajUpdate(); + } + + this->updateState(); + + // Apply control law in base library + this->calculateCommandedTorques(); + + // Write commands + for (size_t i = 0; i < this->n_joints_; ++i) + { + this->joint_handles_[i].setCommand(this->tau_c_(i)); + } + + publishMsgsAndTf(); + } + + bool CartesianImpedanceControllerRos::getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, + Eigen::Quaterniond *orientation) const + { + rbdyn_wrapper::EefState ee_state; + // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known + if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) + { + Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); + q_rb.head(q.size()) = q; + ee_state = this->rbdyn_wrapper_.perform_fk(q_rb); + } + else + { + ee_state = this->rbdyn_wrapper_.perform_fk(q); + } + *position = ee_state.translation; + *orientation = ee_state.orientation; + return true; + } + + bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, + Eigen::MatrixXd *jacobian) + { + // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known + if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) + { + Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); + q_rb.head(q.size()) = q; + Eigen::VectorXd dq_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); + dq_rb.head(dq.size()) = dq; + *jacobian = this->rbdyn_wrapper_.jacobian(q_rb, dq_rb); + } + else + { + *jacobian = this->rbdyn_wrapper_.jacobian(q, dq); + } + *jacobian = jacobian_perm_ * *jacobian; + return true; + } + + void CartesianImpedanceControllerRos::updateState() + { + for (size_t i = 0; i < this->n_joints_; ++i) + { + this->q_[i] = this->joint_handles_[i].getPosition(); + this->dq_[i] = this->joint_handles_[i].getVelocity(); + this->tau_m_[i] = this->joint_handles_[i].getEffort(); + } + getJacobian(this->q_, this->dq_, &this->jacobian_); + getFk(this->q_, &this->position_, &this->orientation_); + } + + void CartesianImpedanceControllerRos::controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg) + { + this->setStiffness(msg->cartesian_stiffness, msg->nullspace_stiffness, false); + this->setDampingFactors(msg->cartesian_damping_factors, msg->nullspace_damping_factor); + + if (msg->q_d_nullspace.size() == this->n_joints_) + { + Eigen::VectorXd q_d_nullspace(this->n_joints_); + for (size_t i = 0; i < this->n_joints_; i++) + { + q_d_nullspace(i) = msg->q_d_nullspace.at(i); + } + this->setNullspaceConfig(q_d_nullspace); + } + else + { + ROS_WARN_STREAM("Nullspace configuration does not have the correct amount of entries. Got " << msg->q_d_nullspace.size() << " expected " << this->n_joints_ << ". Ignoring."); + } + } + + void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg) + { + this->setDampingFactors(*msg, this->damping_factors_[6]); + } + + void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg) + { + if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_) + { + ROS_WARN_STREAM("Reference poses need to be in the root frame '" << this->root_frame_ << "'. Ignoring."); + return; + } + Eigen::Vector3d position_d; + position_d << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; + const Eigen::Quaterniond last_orientation_d_target(this->orientation_d_); + Eigen::Quaterniond orientation_d; + orientation_d.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, + msg->pose.orientation.w; + if (last_orientation_d_target.coeffs().dot(this->orientation_d_.coeffs()) < 0.0) + { + this->orientation_d_.coeffs() << -this->orientation_d_.coeffs(); + } + this->setReferencePose(position_d, orientation_d); + } + + void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg) + { + this->setStiffness(msg->wrench, this->nullspace_stiffness_target_); + } + + void CartesianImpedanceControllerRos::setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace) + { + CartesianImpedanceController::setDampingFactors(saturateValue(cart_damping.force.x, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.force.y, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.force.z, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.torque.x, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.torque.y, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.torque.z, dmp_factor_min_, dmp_factor_max_), + saturateValue(nullspace, dmp_factor_min_, dmp_factor_max_)); + } + + void CartesianImpedanceControllerRos::setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping) + { + CartesianImpedanceController::setStiffness(saturateValue(cart_stiffness.force.x, trans_stf_min_, trans_stf_max_), + saturateValue(cart_stiffness.force.y, trans_stf_min_, trans_stf_max_), + saturateValue(cart_stiffness.force.z, trans_stf_min_, trans_stf_max_), + saturateValue(cart_stiffness.torque.x, rot_stf_min_, rot_stf_max_), + saturateValue(cart_stiffness.torque.y, rot_stf_min_, rot_stf_max_), + saturateValue(cart_stiffness.torque.z, rot_stf_min_, rot_stf_max_), + saturateValue(nullspace, ns_min_, ns_max_), auto_damping); + } + + void CartesianImpedanceControllerRos::wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg) + { + Eigen::Matrix F; + F << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, msg->wrench.torque.y, + msg->wrench.torque.z; + + if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_) + { + if (!transformWrench(&F, msg->header.frame_id, this->root_frame_)) + { + ROS_ERROR("Could not transform wrench. Not applying it."); + return; + } + } + else if (msg->header.frame_id.empty()) + { + if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) + { + ROS_ERROR("Could not transform wrench. Not applying it."); + return; + } + } + this->applyWrench(F); + } + + bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix *cartesian_wrench, + const std::string &from_frame, const std::string &to_frame) const + { + try + { + tf::StampedTransform transform; + tf_listener_.lookupTransform(to_frame, from_frame, ros::Time(0), transform); + tf::Vector3 v_f(cartesian_wrench->operator()(0), cartesian_wrench->operator()(1), cartesian_wrench->operator()(2)); + tf::Vector3 v_t(cartesian_wrench->operator()(3), cartesian_wrench->operator()(4), cartesian_wrench->operator()(5)); + tf::Vector3 v_f_rot = tf::quatRotate(transform.getRotation(), v_f); + tf::Vector3 v_t_rot = tf::quatRotate(transform.getRotation(), v_t); + *cartesian_wrench << v_f_rot[0], v_f_rot[1], v_f_rot[2], v_t_rot[0], v_t_rot[1], v_t_rot[2]; + return true; + } + catch (const tf::TransformException &ex) + { + ROS_ERROR_THROTTLE(1, "%s", ex.what()); + return false; + } + } + + void CartesianImpedanceControllerRos::publishMsgsAndTf() + { + // publish commanded torques + if (this->pub_torques_.trylock()) + { + for (size_t i = 0; i < this->n_joints_; i++) + { + this->pub_torques_.msg_.data[i] = this->tau_c_[i]; + } + this->pub_torques_.unlockAndPublish(); + } + + const Eigen::Matrix error{this->getPoseError()}; + + if (this->verbose_print_) + { + ROS_INFO_STREAM_THROTTLE(0.1, "\nCartesian Position:\n" + << this->position_ << "\nError:\n" + << error << "\nCartesian Stiffness:\n" + << this->cartesian_stiffness_ << "\nCartesian damping:\n" + << this->cartesian_damping_ << "\nNullspace stiffness:\n" + << this->nullspace_stiffness_ << "\nq_d_nullspace:\n" + << this->q_d_nullspace_ << "\ntau_d:\n" + << this->tau_c_); + } + if (this->verbose_tf_ && ros::Time::now() > this->tf_last_time_) + { + // Publish result of forward kinematics + tf::vectorEigenToTF(this->position_, this->tf_pos_); + this->tf_br_transform_.setOrigin(this->tf_pos_); + tf::quaternionEigenToTF(this->orientation_, this->tf_rot_); + this->tf_br_transform_.setRotation(this->tf_rot_); + tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_fk")); + // Publish tf to the reference pose + tf::vectorEigenToTF(this->position_d_, this->tf_pos_); + this->tf_br_transform_.setOrigin(this->tf_pos_); + tf::quaternionEigenToTF(this->orientation_d_, this->tf_rot_); + this->tf_br_transform_.setRotation(this->tf_rot_); + tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_ref_pose")); + this->tf_last_time_ = ros::Time::now(); + } + if (this->verbose_state_ && this->pub_state_.trylock()) + { + this->pub_state_.msg_.header.stamp = ros::Time::now(); + tf::pointEigenToMsg(this->position_, this->pub_state_.msg_.current_pose.position); + tf::quaternionEigenToMsg(this->orientation_, this->pub_state_.msg_.current_pose.orientation); + tf::pointEigenToMsg(this->position_d_, this->pub_state_.msg_.reference_pose.position); + tf::quaternionEigenToMsg(this->orientation_d_, this->pub_state_.msg_.reference_pose.orientation); + tf::pointEigenToMsg(error.head(3), this->pub_state_.msg_.pose_error.position); + Eigen::Quaterniond q = Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ()); + tf::quaternionEigenToMsg(q, this->pub_state_.msg_.pose_error.orientation); + + EigenVectorToWrench(this->cartesian_stiffness_.diagonal(), &this->pub_state_.msg_.cartesian_stiffness); + EigenVectorToWrench(this->cartesian_damping_.diagonal(), &this->pub_state_.msg_.cartesian_damping); + EigenVectorToWrench(this->getAppliedWrench(), &this->pub_state_.msg_.commanded_wrench); + + for (size_t i = 0; i < this->n_joints_; i++) + { + this->pub_state_.msg_.joint_state.position.at(i) = this->q_(i); + this->pub_state_.msg_.joint_state.velocity.at(i) = this->dq_(i); + this->pub_state_.msg_.joint_state.effort.at(i) = this->tau_m_(i); + this->pub_state_.msg_.nullspace_config.at(i) = this->q_d_nullspace_(i); + this->pub_state_.msg_.commanded_torques.at(i) = this->tau_c_(i); + } + this->pub_state_.msg_.nullspace_stiffness = this->nullspace_stiffness_; + this->pub_state_.msg_.nullspace_damping = this->nullspace_damping_; + const Eigen::Matrix dx = this->jacobian_ * this->dq_; + this->pub_state_.msg_.cartesian_velocity = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); + + this->pub_state_.unlockAndPublish(); + this->pub_state_.msg_.header.seq++; + } + } + + // Dynamic reconfigure + // -------------------------------------------------------------------------------------------------------------------------------------- + void CartesianImpedanceControllerRos::dynamicStiffnessCb( + cartesian_impedance_controller::stiffnessConfig &config, uint32_t level) + { + if (config.update_stiffness) + { + CartesianImpedanceController::setStiffness(saturateValue(config.translation_x, trans_stf_min_, trans_stf_max_), + saturateValue(config.translation_y, trans_stf_min_, trans_stf_max_), + saturateValue(config.translation_z, trans_stf_min_, trans_stf_max_), + saturateValue(config.rotation_x, trans_stf_min_, trans_stf_max_), + saturateValue(config.rotation_y, trans_stf_min_, trans_stf_max_), + saturateValue(config.rotation_z, trans_stf_min_, trans_stf_max_), config.nullspace_stiffness); + } + } + + void CartesianImpedanceControllerRos::dynamicDampingCb( + cartesian_impedance_controller::dampingConfig &config, uint32_t level) + { + if (config.update_damping_factors) + { + CartesianImpedanceController::setDampingFactors( + config.translation_x, config.translation_y, config.translation_z, config.rotation_x, config.rotation_y, config.rotation_z, config.nullspace_damping); + } + } + + void CartesianImpedanceControllerRos::dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, + uint32_t level) + { + Eigen::Vector6d F{Eigen::Vector6d::Zero()}; + if (config.apply_wrench) + { + F << config.f_x, config.f_y, config.f_z, config.tau_x, config.tau_y, config.tau_z; + if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) + { + ROS_ERROR("Could not transform wrench. Not applying it."); + return; + } + } + this->applyWrench(F); + } + + void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg) + { + ROS_INFO("Got trajectory msg from trajectory topic."); + if (this->traj_as_->isActive()) + { + this->traj_as_->setPreempted(); + ROS_INFO("Preempted running action server goal."); + } + trajStart(*msg); + } + + void CartesianImpedanceControllerRos::trajGoalCb() + { + this->traj_as_goal_ = this->traj_as_->acceptNewGoal(); + ROS_INFO("Accepted new goal from action server."); + trajStart(this->traj_as_goal_->trajectory); + } + + void CartesianImpedanceControllerRos::trajPreemptCb() + { + ROS_INFO("Actionserver got preempted."); + this->traj_as_->setPreempted(); + } + + void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::JointTrajectory &trajectory) + { + this->traj_duration_ = trajectory.points[trajectory.points.size() - 1].time_from_start; + ROS_INFO_STREAM("Starting a new trajectory with " << trajectory.points.size() << " points that takes " << this->traj_duration_ << "s."); + this->trajectory_ = trajectory; + this->traj_running_ = true; + this->traj_start_ = ros::Time::now(); + this->traj_index_ = 0; + + // Pre-compute Cartesian poses for all trajectory points + this->traj_positions_.clear(); + this->traj_orientations_.clear(); + this->traj_positions_.reserve(trajectory.points.size()); + this->traj_orientations_.reserve(trajectory.points.size()); + + for (size_t i = 0; i < trajectory.points.size(); ++i) + { + // Robustness Check: Joint size + if (trajectory.points.at(i).positions.size() != this->n_joints_) { + ROS_ERROR("Trajectory point %zu has incorrect number of joints (%zu vs %zu). Aborting trajectory.", + i, trajectory.points.at(i).positions.size(), this->n_joints_); + this->traj_positions_.clear(); + this->traj_orientations_.clear(); + this->traj_running_ = false; + if (this->traj_as_->isActive()) { this->traj_as_->setAborted(); } + return; // Exit trajStart + } + + Eigen::VectorXd q = Eigen::VectorXd::Map(trajectory.points.at(i).positions.data(), + trajectory.points.at(i).positions.size()); + Eigen::Vector3d position; + Eigen::Quaterniond orientation; + getFk(q, &position, &orientation); + + // Robustness Check: Ensure quaternion hemisphere consistency for Slerp + if (i > 0 && this->traj_orientations_.back().dot(orientation) < 0.0) { + orientation.coeffs() *= -1.0; + } + + this->traj_positions_.push_back(position); + this->traj_orientations_.push_back(orientation); + + if (this->verbose_print_) + { + ROS_INFO_STREAM("Trajectory point " << i << " position: " << position.transpose() + << " orientation: " << orientation.coeffs().transpose()); + } + } + + // Call trajUpdate once to set initial targets + trajUpdate(); + if (this->nullspace_stiffness_ < 5.) + { + ROS_WARN("Nullspace stiffness is low. The joints might not follow the planned path."); + } + } + + void CartesianImpedanceControllerRos::trajUpdate() + { + // Check if trajectory is finished + ros::Time current_time = ros::Time::now(); + if (current_time > (this->traj_start_ + this->traj_duration_)) + { + // Ensure final pose is set + if (!this->traj_positions_.empty()) { + this->position_d_target_ = this->traj_positions_.back(); + this->orientation_d_target_ = this->traj_orientations_.back(); + Eigen::VectorXd q_last = Eigen::VectorXd::Map(trajectory_.points.back().positions.data(), + trajectory_.points.back().positions.size()); + this->setNullspaceConfig(q_last); + } + + ROS_INFO_STREAM("Finished executing trajectory."); + if (this->traj_as_->isActive()) + { + this->traj_as_->setSucceeded(); + } + this->traj_running_ = false; + return; + } + + // Calculate current time within trajectory + ros::Duration time_from_start = current_time - this->traj_start_; + + // Find the two waypoints that bracket the current time + // traj_index_ is updated here to avoid re-searching from start every time + while (this->traj_index_ < trajectory_.points.size() && + time_from_start >= trajectory_.points.at(this->traj_index_).time_from_start) + { + this->traj_index_++; + } + + size_t next_index = this->traj_index_; + + // If we're past the last point (should be handled by finish check, but safety), use last point + if (next_index >= trajectory_.points.size()) + { + this->position_d_target_ = this->traj_positions_.back(); + this->orientation_d_target_ = this->traj_orientations_.back(); + Eigen::VectorXd q_last = Eigen::VectorXd::Map(trajectory_.points.back().positions.data(), + trajectory_.points.back().positions.size()); + this->setNullspaceConfig(q_last); + return; + } + + // If we're before the first point (or exactly at it), use the first point + if (next_index == 0) + { + this->position_d_target_ = this->traj_positions_[0]; + this->orientation_d_target_ = this->traj_orientations_[0]; + Eigen::VectorXd q_first = Eigen::VectorXd::Map(trajectory_.points[0].positions.data(), + trajectory_.points[0].positions.size()); + this->setNullspaceConfig(q_first); + return; + } + + // Otherwise, interpolate between the two points: prev_index and next_index + size_t prev_index = next_index - 1; + + // Calculate interpolation factor (alpha) between 0 and 1 + double t_prev = trajectory_.points.at(prev_index).time_from_start.toSec(); + double t_next = trajectory_.points.at(next_index).time_from_start.toSec(); + double t_curr = time_from_start.toSec(); + + double dt = t_next - t_prev; + double alpha = 1.0; // Default alpha if segment duration is invalid + if (dt > 1e-6) // Use a small epsilon to avoid floating point issues near zero + { + alpha = (t_curr - t_prev) / dt; + } else { + ROS_WARN_THROTTLE(1.0, "Trajectory segment duration is near-zero or negative. Clamping alpha=1."); + } + // Clamp alpha strictly between 0 and 1 + alpha = std::max(0.0, std::min(1.0, alpha)); + + if (this->verbose_print_) + { + ROS_INFO_STREAM_THROTTLE(0.5, "Interpolating between points " << prev_index << " and " << next_index + << " with alpha=" << alpha << " (t=" << t_curr << ")"); + } + + // Linear interpolation for position + this->position_d_target_ = (1.0 - alpha) * this->traj_positions_[prev_index] + + alpha * this->traj_positions_[next_index]; + + // SLERP for orientation (using cached, hemisphere-consistent quaternions) + this->orientation_d_target_ = this->traj_orientations_[prev_index].slerp( + alpha, this->traj_orientations_[next_index]); + + // Linear interpolation for nullspace configuration + Eigen::VectorXd q_prev = Eigen::VectorXd::Map(trajectory_.points.at(prev_index).positions.data(), + trajectory_.points.at(prev_index).positions.size()); + Eigen::VectorXd q_next = Eigen::VectorXd::Map(trajectory_.points.at(next_index).positions.data(), + trajectory_.points.at(next_index).positions.size()); + Eigen::VectorXd q_interp = (1.0 - alpha) * q_prev + alpha * q_next; + + this->setNullspaceConfig(q_interp); + } +} // namespace cartesian_impedance_controller From c61f3c43e112df948406f9f3dfccfe155f2955b4 Mon Sep 17 00:00:00 2001 From: Guma <44319454+Cody-Vu@users.noreply.github.com> Date: Mon, 5 May 2025 11:58:40 +0200 Subject: [PATCH 5/6] Add files via upload --- cartesian_impedance_moveit_plugin.xml | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 cartesian_impedance_moveit_plugin.xml diff --git a/cartesian_impedance_moveit_plugin.xml b/cartesian_impedance_moveit_plugin.xml new file mode 100644 index 0000000..5cf45ef --- /dev/null +++ b/cartesian_impedance_moveit_plugin.xml @@ -0,0 +1,11 @@ + + + + Controller description + + + From fee82b189b36ef0baf34094d1ef17c0f056f137d Mon Sep 17 00:00:00 2001 From: Guma <44319454+Cody-Vu@users.noreply.github.com> Date: Mon, 5 May 2025 12:03:12 +0200 Subject: [PATCH 6/6] Update package.xml --- package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/package.xml b/package.xml index ceff515..baaff33 100644 --- a/package.xml +++ b/package.xml @@ -52,10 +52,11 @@ tf_conversions trajectory_msgs yaml-cpp - + moveit_ros_control_interface + gtest