From 896b942eaa37179e0c5fe5576b0e9a37a2c186d7 Mon Sep 17 00:00:00 2001 From: stanley Date: Sun, 30 Oct 2022 16:11:48 -0400 Subject: [PATCH 01/11] Add impedance control method computed in local frame for c++ --- include/mim_control/impedance_controller.hpp | 41 +++++++++ src/impedance_controller.cpp | 95 ++++++++++++++++++++ 2 files changed, 136 insertions(+) diff --git a/include/mim_control/impedance_controller.hpp b/include/mim_control/impedance_controller.hpp index bb9f13d..0b9fdd2 100644 --- a/include/mim_control/impedance_controller.hpp +++ b/include/mim_control/impedance_controller.hpp @@ -143,6 +143,47 @@ class ImpedanceController */ const pinocchio::FrameIndex& get_endframe_index(); + /** + * @brief Similar to run(), but desired_end_frame_placement and + * desired_end_frame_velocity are in the root joint frame + * + * @param robot_configuration robot generalized coordinates configuration. + * @param robot_velocity robot generalized coordinates velocity. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run_local(Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force); + + /** + * @brief computes the velocity of the end_frame with respect to a frame + whose origin aligns with the root frame but is oriented as the world frame. + Note: To use it, you must call a pinocchio forward kinematics function before. + Used in the run_local() method. + * + * @param pinocchio_data The data object to use for the computation. + * @param robot_configuration robot generalized coordinates configuration. + * @param robot_velocity robot generalized coordinates velocity. + */ + void compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, + Eigen::Ref &robot_configration, + Eigen::Ref &robot_velocity); + private: // attributes /** @brief Rigid body dynamics model. */ pinocchio::Model pinocchio_model_; diff --git a/src/impedance_controller.cpp b/src/impedance_controller.cpp index e1bba52..cf6403c 100644 --- a/src/impedance_controller.cpp +++ b/src/impedance_controller.cpp @@ -165,6 +165,85 @@ void ImpedanceController::run_precomputed_data( return; } +void ImpedanceController::run_local(Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force) +{ + assert(robot_configuration.size() == pinocchio_model_.nq && + "robot_configuration is not of the good size."); + assert(robot_velocity.size() == pinocchio_model_.nv && + "robot_velocity is not of the good size."); + // Get the current frame placements and velocity. + // Compute the jacobians + pinocchio::computeJointJacobians( + pinocchio_model_, pinocchio_data_, robot_configuration); + pinocchio::updateFramePlacement( + pinocchio_model_, pinocchio_data_, root_frame_index_); + pinocchio::updateFramePlacement( + pinocchio_model_, pinocchio_data_, end_frame_index_); + // Compute relative velocity between frames + compute_relative_velocity_between_frames(pinocchio_data_, robot_configuration, robot_velocity); + root_placement_ = pinocchio_data_.oMf[root_frame_index_]; + end_placement_ = pinocchio_data_.oMf[end_frame_index_]; + + // Orientations + root_orientation_.rotation() = root_placement_.rotation(); + end_orientation_.rotation() = end_placement_.rotation(); + + // Actual end frame placement in root frame. + actual_end_frame_placement_ = pinocchio::SE3 (Eigen::Matrix3d::Identity(), end_placement_.translation() - root_placement_.translation()); + + // Placement error. + err_se3_.head<3>() = root_orientation_.rotation() * + (desired_end_frame_placement.translation() - + actual_end_frame_placement_.translation()); + + err_se3_.head<3>() = (desired_end_frame_placement.translation() - + actual_end_frame_placement_.translation()); + + err_se3_.tail<3>() = + pinocchio::log3(desired_end_frame_placement.rotation().transpose() * + actual_end_frame_placement_.rotation()); + + // Actual end frame velocity in root frame. + actual_end_frame_velocity_ = pinocchio::Motion(relative_vel_between_frames.segment(0, 3), Eigen::Vector3d({0, 0, 0})); + + // Velocity error + err_vel_ = (desired_end_frame_velocity - actual_end_frame_velocity_); + + // Compute the force to be applied to the environment. + impedance_force_ = gain_proportional * err_se3_.array(); + impedance_force_ += + (gain_derivative * err_vel_.toVector().array()).matrix(); + impedance_force_ -= + (gain_feed_forward_force * feed_forward_force.toVector().array()) + .matrix(); + + // Get the jacobian. + pinocchio::getFrameJacobian(pinocchio_model_, + pinocchio_data_, + end_frame_index_, + pinocchio::LOCAL_WORLD_ALIGNED, + end_jacobian_); + + impedance_jacobian_ = end_jacobian_; + + // compute the output torques + torques_ = (impedance_jacobian_.transpose() * impedance_force_); + + if (pinocchio_model_has_free_flyer_) + joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); + else + { + joint_torques_ = torques_; + } +} + const Eigen::VectorXd& ImpedanceController::get_torques() { return torques_; @@ -185,4 +264,20 @@ const pinocchio::FrameIndex& ImpedanceController::get_endframe_index() return end_frame_index_; } +void ImpedanceController::compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, + Eigen::Ref &robot_configration, + Eigen::Ref &robot_velocity) +{ + Eigen::Matrix3d root_frame_rotation = pinocchio_data_.oMf[root_frame_index_].rotation(); + Eigen::Matrix3d end_frame_rotation = pinocchio_data_.oMf[end_frame_index_].rotation(); + pinocchio::SE3 frame_config_root = pinocchio::SE3(root_frame_rotation, Eigen::Vector3d::Zero()); + pinocchio::SE3 frame_config_end = pinocchio::SE3(end_frame_rotation, Eigen::Vector3d::Zero()); + // root frame jocobian + root_jacobian_.resize(6, pinocchio_model_.nv); + root_jacobian_.fill(0.); + Eigen::VectorXd vel_root_in_world_frame = frame_config_root.toActionMatrix() * root_jacobian_ * robot_velocity; + Eigen::VectorXd vel_end_in_world_frame = frame_config_end.toActionMatrix() * end_jacobian_ * robot_velocity; + relative_vel_between_frames = vel_end_in_world_frame - vel_root_in_world_frame; +} + } // namespace mim_control \ No newline at end of file From 40c75eaf65e8d92c75aaea1bb6a182a1f0de1b38 Mon Sep 17 00:00:00 2001 From: stanley Date: Fri, 27 Jan 2023 14:50:26 -0500 Subject: [PATCH 02/11] Fix quaternion difference bug for centroidal controller in python, modify c++ impedance algo in world frame, and modify cmake & package.xml so other ROS packages can use this --- CMakeLists.txt | 17 + include/mim_control/impedance_controller.hpp | 479 ++++++++-------- package.xml | 17 + .../robot_centroidal_controller.py | 44 +- src/impedance_controller.cpp | 515 +++++++++--------- 5 files changed, 554 insertions(+), 518 deletions(-) create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 6a028db..9fc9200 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,6 +32,7 @@ search_for_boost_python() # Optionnal find_package(dynamic-graph QUIET) find_package(dynamic-graph-python QUIET) +find_package(ament_cmake QUIET) set(build_dynamic_graph_plugins FALSE) if(${dynamic-graph_FOUND} AND ${dynamic-graph-python_FOUND}) @@ -97,6 +98,14 @@ if(${build_dynamic_graph_plugins}) DESTINATION include) endif() +# +# Allow other ROS packages to link to this package during compilation +# +if (ament_cmake_FOUND) + ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) + ament_export_dependencies(yaml_utils) +endif() + # # Install the package # @@ -154,3 +163,11 @@ add_documentation() # create the cmake package # generate_cmake_package(INSTALL_EXPORT) + +# +# Allow other ROS 2 packages to find the header files +# +if (ament_cmake_FOUND) + ament_export_include_directories(include) + ament_package() +endif() diff --git a/include/mim_control/impedance_controller.hpp b/include/mim_control/impedance_controller.hpp index 0b9fdd2..d546e32 100644 --- a/include/mim_control/impedance_controller.hpp +++ b/include/mim_control/impedance_controller.hpp @@ -4,7 +4,7 @@ * @copyright Copyright (c) 2020, New York University and Max Planck * Gesellschaft * - * @brief This is the implementation for impedance controller between any two + * @brief This is the implementation for mim_control_ controller between any two * frames of the robot. * */ @@ -19,240 +19,243 @@ namespace mim_control /** * @brief Impedance controller between any two frames of the robot. */ -class ImpedanceController -{ -public: - typedef Eigen::Array Array6d; - typedef Eigen::Matrix Vector6d; - - /** - * @brief Construct a new ImpedanceController object. - */ - ImpedanceController(); - - /** - * @brief Initialize the internal data. None real-time safe method. - * - * @param pinocchio_model rigid body model of the robot - * @param root_frame_name root frame name where the spring starts(Ex. Hip) - * @param end_frame_name frame name where the spring ends(Ex. end effector) - */ - void initialize(const pinocchio::Model& pinocchio_model, - const std::string& root_frame_name, - const std::string& end_frame_name); - - /** - * @brief Computes the desired joint torques - * - * \f$ - * \tau = J^T (k_p (x^{des} - x) + - * k_d (\dot{x}^{des} - \dot{x}) -k_f f) - * \f$ - * - * with: - * - \f$ \tau \f$ the joint torques, - * - \f$ J \f$ the Jacobian of the sub-kinematic-tree between the root and - * the end frame, - * - \f$ k_p \f$ the gain proportional to the frame placement error, - * - \f$ x_{des} \f$ desired end frame placement with respect to the - * desired root frame. - * - \f$ x \f$ measured end frame placement with respect to the - * measured root frame. - * - \f$ k_d \f$ is the derivative gain applied to the time derivative of - * the error. - * - \f$ \dot{x}^{des} \f$ desired end frame velocity with respect to the - * desired root frame. - * - \f$ \dot{x} \f$ measured end frame velocity with respect to the - * measured root frame. - * - \f$ k_f \f$ the gain over the feed forward force, - * - \f$ f \f$ the feed forward force, - * - * @param robot_configuration robot generalized coordinates configuration. - * @param robot_velocity robot generalized coordinates velocity. - * @param gain_proportional 6d vector for the proportional gains on {x, y, - * z, roll, pitch, yaw}. - * @param gain_derivative 6d vector for the proportional gains on {x, y, z, - * roll, pitch, yaw}. - * @param gain_feed_forward_force gain multiplying the feed forward force. - * @param desired_end_frame_placement desired end frame placement relative - * to the desired root joint. - * @param desired_end_frame_velocity desired end frame velocity relative to - * the desired root joint. - * @param feed_forward_force feed forward force applied to the foot by the - * environment. - */ - void run(Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force); - - - /** - * @brief Similar to `run()` but with the data already precomputed. - * - * @param pinocchio_data The data object to use for the computation. - * @param gain_proportional 6d vector for the proportional gains on {x, y, - * z, roll, pitch, yaw}. - * @param gain_derivative 6d vector for the proportional gains on {x, y, z, - * roll, pitch, yaw}. - * @param gain_feed_forward_force gain multiplying the feed forward force. - * @param desired_end_frame_placement desired end frame placement relative - * to the desired root joint. - * @param desired_end_frame_velocity desired end frame velocity relative to - * the desired root joint. - * @param feed_forward_force feed forward force applied to the foot by the - * environment. - */ - void run_precomputed_data(pinocchio::Data& pinocchio_data, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force); - - /** - * @brief Get the computed torques from the impedance controller. - * - * @return Eigen::VectorXd& - */ - const Eigen::VectorXd& get_torques(); - - /** - * @brief Get the computed joint torques from the impedance controller. - * - * @return Eigen::VectorXd& - */ - const Eigen::VectorXd& get_joint_torques(); - - /** - * @brief Get the impedance force \f$ f_i \f$ with \f$ \tau = J^T f_i \f$. - * - * @return Vector6d& - */ - const Vector6d& get_impedance_force(); - - /** - * @brief Returns the index of the endeffector frame. - * - * @return pinocchio::FrameIndex - */ - const pinocchio::FrameIndex& get_endframe_index(); - - /** - * @brief Similar to run(), but desired_end_frame_placement and - * desired_end_frame_velocity are in the root joint frame - * - * @param robot_configuration robot generalized coordinates configuration. - * @param robot_velocity robot generalized coordinates velocity. - * @param gain_proportional 6d vector for the proportional gains on {x, y, - * z, roll, pitch, yaw}. - * @param gain_derivative 6d vector for the proportional gains on {x, y, z, - * roll, pitch, yaw}. - * @param gain_feed_forward_force gain multiplying the feed forward force. - * @param desired_end_frame_placement desired end frame placement relative - * to the desired root joint. - * @param desired_end_frame_velocity desired end frame velocity relative to - * the desired root joint. - * @param feed_forward_force feed forward force applied to the foot by the - * environment. - */ - void run_local(Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force); - - /** - * @brief computes the velocity of the end_frame with respect to a frame - whose origin aligns with the root frame but is oriented as the world frame. - Note: To use it, you must call a pinocchio forward kinematics function before. - Used in the run_local() method. - * - * @param pinocchio_data The data object to use for the computation. - * @param robot_configuration robot generalized coordinates configuration. - * @param robot_velocity robot generalized coordinates velocity. - */ - void compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, - Eigen::Ref &robot_configration, - Eigen::Ref &robot_velocity); - -private: // attributes - /** @brief Rigid body dynamics model. */ - pinocchio::Model pinocchio_model_; - - /** @brief Cache of the rigid body dynamics algorithms. */ - pinocchio::Data pinocchio_data_; - - /** @brief (urdf) name of the root frame. The impedance controller is - * computed with respect to this frame. */ - std::string root_frame_name_; - - /** @brief Index of the root frame in the pinocchio model. */ - pinocchio::FrameIndex root_frame_index_; - - /** @brief Measured root frame placement. */ - pinocchio::SE3 root_placement_; - - /** @brief Measured root frame orientation. */ - pinocchio::SE3 root_orientation_; - - /** @brief Measured root frame velocity. */ - pinocchio::Motion root_velocity_; - - /** @brief (urdf) name of the end frame. This is the controlled frame. */ - std::string end_frame_name_; - - /** @brief Index of the end frame in the pinocchio model. */ - pinocchio::FrameIndex end_frame_index_; - - /** @brief Jacobian of the end frame. */ - pinocchio::Data::Matrix6x end_jacobian_; - - /** @brief Measured end frame placement. */ - pinocchio::SE3 end_placement_; - - /** @brief Measured end frame orientation. */ - pinocchio::SE3 end_orientation_; - - /** @brief Measured end frame velocity. */ - pinocchio::Motion end_velocity_; - - /** @brief Impedance force. Accessible for machine learning purposes. */ - Vector6d impedance_force_; - - /** @brief Measured end frame placement in root frame. */ - pinocchio::SE3 actual_end_frame_placement_; - - /** @brief Measured end frame placement in root frame. */ - pinocchio::Motion actual_end_frame_velocity_; - - /** @brief SE3 placement error. */ - Eigen::Matrix err_se3_; - - /** @brief SE3 velocity error. */ - pinocchio::Motion err_vel_; - - /** @brief Jacobian used in the computation of the impedance. */ - pinocchio::Data::Matrix6x impedance_jacobian_; - - /** @brief Output torques. */ - Eigen::VectorXd torques_; - - /** @brief Output torques. */ - Eigen::VectorXd joint_torques_; - - /** @brief Checks out if the Pinocchio rigid body model of the robot - * contains a free-flyer. This is used to return the command i.e. the - * torques to be applied to the joints only. */ - bool pinocchio_model_has_free_flyer_; -}; - -} // namespace mim_control + class ImpedanceController + { + public: + typedef Eigen::Array Array6d; + typedef Eigen::Matrix Vector6d; + + /** + * @brief Construct a new ImpedanceController object. + */ + ImpedanceController(); + + /** + * @brief Initialize the internal data. None real-time safe method. + * + * @param pinocchio_model rigid body model of the robot + * @param root_frame_name root frame name where the spring starts(Ex. Hip) + * @param end_frame_name frame name where the spring ends(Ex. end effector) + */ + void initialize(const pinocchio::Model& pinocchio_model, + const std::string& root_frame_name, + const std::string& end_frame_name); + + /** + * @brief Computes the desired joint torques + * + * \f$ + * \tau = J^T (k_p (x^{des} - x) + + * k_d (\dot{x}^{des} - \dot{x}) -k_f f) + * \f$ + * + * with: + * - \f$ \tau \f$ the joint torques, + * - \f$ J \f$ the Jacobian of the sub-kinematic-tree between the root and + * the end frame, + * - \f$ k_p \f$ the gain proportional to the frame placement error, + * - \f$ x_{des} \f$ desired end frame placement with respect to the + * desired root frame. + * - \f$ x \f$ measured end frame placement with respect to the + * measured root frame. + * - \f$ k_d \f$ is the derivative gain applied to the time derivative of + * the error. + * - \f$ \dot{x}^{des} \f$ desired end frame velocity with respect to the + * desired root frame. + * - \f$ \dot{x} \f$ measured end frame velocity with respect to the + * measured root frame. + * - \f$ k_f \f$ the gain over the feed forward force, + * - \f$ f \f$ the feed forward force, + * + * @param robot_configuration robot generalized coordinates configuration. + * @param robot_velocity robot generalized coordinates velocity. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run(Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force); + + + /** + * @brief Similar to `run()` but with the data already precomputed. + * + * @param pinocchio_data The data object to use for the computation. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run_precomputed_data(pinocchio::Data& pinocchio_data, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force); + + /** + * @brief Get the computed torques from the mim_control_ controller. + * + * @return Eigen::VectorXd& + */ + const Eigen::VectorXd& get_torques(); + + /** + * @brief Get the computed joint torques from the mim_control_ controller. + * + * @return Eigen::VectorXd& + */ + const Eigen::VectorXd& get_joint_torques(); + + /** + * @brief Get the mim_control_ force \f$ f_i \f$ with \f$ \tau = J^T f_i \f$. + * + * @return Vector6d& + */ + const Vector6d& get_impedance_force(); + + /** + * @brief Returns the index of the endeffector frame. + * + * @return pinocchio::FrameIndex + */ + const pinocchio::FrameIndex& get_endframe_index(); + + const pinocchio::FrameIndex& get_rootframe_index(); + + /** + * @biref Similar to run(), but in the root joint frame + * + * @param robot_configuration robot generalized coordinates configuration. + * @param robot_velocity robot generalized coordinates velocity. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run_local(Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force); + + /** + * @brief computes the velocity of the end_frame with respect to a frame + whose origin aligns with the root frame but is oriented as the world frame + * + */ + void compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, + Eigen::Ref &robot_configration, + Eigen::Ref &robot_velocity); + + private: // attributes + /** @brief Rigid body dynamics model. */ + pinocchio::Model pinocchio_model_; + + /** @brief Cache of the rigid body dynamics algorithms. */ + pinocchio::Data pinocchio_data_; + + /** @brief (urdf) name of the root frame. The mim_control_ controller is + * computed with respect to this frame. */ + std::string root_frame_name_; + + /** @brief Index of the root frame in the pinocchio model. */ + pinocchio::FrameIndex root_frame_index_; + + /** @brief Measured root frame placement. */ + pinocchio::SE3 root_placement_; + + /** @brief Measured root frame orientation. */ + pinocchio::SE3 root_orientation_; + + /** @brief Measured root frame velocity. */ + pinocchio::Motion root_velocity_; + + /** @brief (urdf) name of the end frame. This is the controlled frame. */ + std::string end_frame_name_; + + /** @brief Index of the end frame in the pinocchio model. */ + pinocchio::FrameIndex end_frame_index_; + + /** @brief Jacobian of the end frame. */ + pinocchio::Data::Matrix6x end_jacobian_; + + /** @brief Measured end frame placement. */ + pinocchio::SE3 end_placement_; + + /** @brief Measured end frame orientation. */ + pinocchio::SE3 end_orientation_; + + /** @brief Measured end frame velocity. */ + pinocchio::Motion end_velocity_; + + /** @brief Impedance force. Accessible for machine learning purposes. */ + Vector6d impedance_force_; + + /** @brief Measured end frame placement in root frame. */ + pinocchio::SE3 actual_end_frame_placement_; + + /** @brief Measured end frame placement in root frame. */ + pinocchio::Motion actual_end_frame_velocity_; + + /** @brief SE3 placement error. */ + Eigen::Matrix err_se3_; + + /** @brief SE3 velocity error. */ + pinocchio::Motion err_vel_; + + /** @brief Jacobian used in the computation of the mim_control_. */ + pinocchio::Data::Matrix6x impedance_jacobian_; + + /** @brief Output torques. */ + Eigen::VectorXd torques_; + + /** @brief Output torques. */ + Eigen::VectorXd joint_torques_; + + /** @brief Checks out if the Pinocchio rigid body model of the robot + * contains a free-flyer. This is used to return the command i.e. the + * torques to be applied to the joints only. */ + bool pinocchio_model_has_free_flyer_; + + /** @brief Stores the root jacobian */ + pinocchio::Data::Matrix6x root_jacobian_; + + /** @brief Relative velocity between the end_frame with respect to a frame + * whose origin aligns with the root frame but is oriented as the world frame. */ + Eigen::VectorXd relative_vel_between_frames; + }; + +} // namespace mim_control_ \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..b6121d4 --- /dev/null +++ b/package.xml @@ -0,0 +1,17 @@ + + + + mim_control + 0.0.0 + TODO: Package description + user + TODO: License declaration + + ament_cmake + mpi_cmake_modules + yaml_utils + + + ament_cmake + + \ No newline at end of file diff --git a/python/mim_control/robot_centroidal_controller.py b/python/mim_control/robot_centroidal_controller.py index f6b29b5..15d5de5 100644 --- a/python/mim_control/robot_centroidal_controller.py +++ b/python/mim_control/robot_centroidal_controller.py @@ -1,10 +1,3 @@ -################################################################################################################## -## This file is the centroidal controller -################################################################################################################# -## Author: Avadesh Meduri & Julian Viereck -## Date: 9/12/2020 -################################################################################################################# - import numpy as np import pinocchio as pin @@ -21,15 +14,15 @@ def mat(a): class RobotCentroidalController: def __init__( - self, - robot_config, - mu, - kc, - dc, - kb, - db, - qp_penalty_lin=3 * [1e6], - qp_penalty_ang=3 * [1e6], + self, + robot_config, + mu, + kc, + dc, + kb, + db, + qp_penalty_lin=3 * [1e6], + qp_penalty_ang=3 * [1e6], ): """ Input: @@ -70,7 +63,8 @@ def compute_com_wrench(self, q, dq, des_pos, des_vel, des_ori, des_angvel): vcom = np.reshape(np.array(dq[0:3]), (3,)) Ib = robot.mass(q)[3:6, 3:6] - quat_diff = self.quaternion_difference(arr(q[3:7]), arr(des_ori)) + tmp = pin.Quaternion(des_ori[3], des_ori[0], des_ori[1], des_ori[2]) * pin.Quaternion(q[6], q[3], q[4], q[5]).conjugate() + quat_diff = pin.log3(tmp.toRotationMatrix()) cur_angvel = arr(dq[3:6]) @@ -93,7 +87,7 @@ def compute_com_wrench(self, q, dq, des_pos, des_vel, des_ori, des_angvel): arr( arr(np.multiply(self.kb, quat_diff)) + ( - Ib * mat(np.multiply(self.db, des_angvel - cur_angvel)) + Ib * mat(np.multiply(self.db, des_angvel - cur_angvel)) ).T ), ] @@ -120,7 +114,9 @@ def compute_force_qp(self, q, dq, cnt_array, w_com): robot = self.pin_robot_wrapper com = np.reshape(np.array(q[0:3]), (3,)) robot.framesForwardKinematics(q) + r = [robot.data.oMf[i].translation - com for i in self.eff_ids] + nb_ee = self.robot_config.nb_ee # Use the contact activation from the plan to determine which of the # forces should be active. @@ -142,8 +138,8 @@ def compute_force_qp(self, q, dq, cnt_array, w_com): if cnt_array[i] == 0: continue - A[:3, 3 * j : 3 * (j + 1)] = np.eye(3) - A[3:, 3 * j : 3 * (j + 1)] = pin.skew(r[i]) + A[:3, 3 * j: 3 * (j + 1)] = np.eye(3) + A[3:, 3 * j: 3 * (j + 1)] = pin.skew(r[i]) G[5 * j + 0, 3 * j + 0] = 1 # mu Fz - Fx >= 0 G[5 * j + 0, 3 * j + 2] = -self.mu @@ -166,7 +162,7 @@ def compute_force_qp(self, q, dq, cnt_array, w_com): for i in range(len(cnt_array)): if cnt_array[i] == 0: continue - F[3 * i : 3 * (i + 1)] = solx[3 * j : 3 * (j + 1)] + F[3 * i: 3 * (i + 1)] = solx[3 * j: 3 * (j + 1)] j += 1 return F @@ -182,9 +178,9 @@ def skew(self, v): def quaternion_to_rotation(self, q): """ converts quaternion to rotation matrix """ return ( - (q[3] ** 2 - q[:3].dot(q[:3])) * np.eye(3) - + 2.0 * np.outer(q[:3], q[:3]) - + 2.0 * q[3] * self.skew(q[:3]) + (q[3] ** 2 - q[:3].dot(q[:3])) * np.eye(3) + + 2.0 * np.outer(q[:3], q[:3]) + + 2.0 * q[3] * self.skew(q[:3]) ) def exp_quaternion(self, w): diff --git a/src/impedance_controller.cpp b/src/impedance_controller.cpp index cf6403c..36caeac 100644 --- a/src/impedance_controller.cpp +++ b/src/impedance_controller.cpp @@ -8,276 +8,279 @@ */ #include "mim_control/impedance_controller.hpp" - #include "pinocchio/algorithm/frames.hpp" namespace mim_control { -ImpedanceController::ImpedanceController() -{ -} + ImpedanceController::ImpedanceController() + { + } -void ImpedanceController::initialize(const pinocchio::Model& pinocchio_model, - const std::string& root_frame_name, - const std::string& end_frame_name) -{ - // Copy the arguments internally. - pinocchio_model_ = pinocchio_model; - - // Create the cache of the rigid body dynamics algorithms - pinocchio_data_ = pinocchio::Data(pinocchio_model_); - - // Copy the arguments internally. - root_frame_name_ = root_frame_name; - end_frame_name_ = end_frame_name; - - // Fetch the index of the frame in the robot model. - root_frame_index_ = pinocchio_model_.getFrameId(root_frame_name_); - end_frame_index_ = pinocchio_model_.getFrameId(end_frame_name_); - - // initialize the size of the vectors. - end_jacobian_.resize(6, pinocchio_model_.nv); - end_jacobian_.fill(0.); - impedance_jacobian_.resize(6, pinocchio_model_.nv); - impedance_jacobian_.fill(0.); - - // Defines if the model has a freeflyer. - pinocchio_model_has_free_flyer_ = - pinocchio_model_.joints[1].shortname() == "JointModelFreeFlyer"; - - // output - torques_.resize(pinocchio_model_.nv, 1); - torques_.fill(0.); - if (pinocchio_model_has_free_flyer_) - joint_torques_.resize(pinocchio_model_.nv, 1); - else + void ImpedanceController::initialize(const pinocchio::Model& pinocchio_model, + const std::string& root_frame_name, + const std::string& end_frame_name) { - joint_torques_.resize(pinocchio_model_.nv - 6, 1); + // Copy the arguments internally. + pinocchio_model_ = pinocchio_model; + + // Create the cache of the rigid body dynamics algorithms + pinocchio_data_ = pinocchio::Data(pinocchio_model_); + + // Copy the arguments internally. + root_frame_name_ = root_frame_name; + end_frame_name_ = end_frame_name; + + // Fetch the index of the frame in the robot model. + root_frame_index_ = pinocchio_model_.getFrameId(root_frame_name_); + end_frame_index_ = pinocchio_model_.getFrameId(end_frame_name_); + + // initialize the size of the vectors. + end_jacobian_.resize(6, pinocchio_model_.nv); + end_jacobian_.fill(0.); + impedance_jacobian_.resize(6, pinocchio_model_.nv); + impedance_jacobian_.fill(0.); + + // Defines if the model has a freeflyer. + pinocchio_model_has_free_flyer_ = + pinocchio_model_.joints[1].shortname() == "JointModelFreeFlyer"; + + // output + torques_.resize(pinocchio_model_.nv, 1); + torques_.fill(0.); + if (pinocchio_model_has_free_flyer_) + joint_torques_.resize(pinocchio_model_.nv, 1); + else + { + joint_torques_.resize(pinocchio_model_.nv - 6, 1); + } + + // Intermediate variables. + root_orientation_ = pinocchio::SE3::Identity(); + end_orientation_ = pinocchio::SE3::Identity(); } - // Intermediate variables. - root_orientation_ = pinocchio::SE3::Identity(); - end_orientation_ = pinocchio::SE3::Identity(); -} - -void ImpedanceController::run( - Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force) -{ - assert(robot_configuration.size() == pinocchio_model_.nq && - "robot_configuration is not of the good size."); - assert(robot_velocity.size() == pinocchio_model_.nv && - "robot_velocity is not of the good size."); - - // Get the current frame placements and velocity. - pinocchio::forwardKinematics( - pinocchio_model_, pinocchio_data_, robot_configuration, robot_velocity); - pinocchio::updateFramePlacement( - pinocchio_model_, pinocchio_data_, root_frame_index_); - pinocchio::updateFramePlacement( - pinocchio_model_, pinocchio_data_, end_frame_index_); - - // Compute the jacobians - pinocchio::computeJointJacobians( - pinocchio_model_, pinocchio_data_, robot_configuration); - - run_precomputed_data(pinocchio_data_, - gain_proportional, - gain_derivative, - gain_feed_forward_force, - desired_end_frame_placement, - desired_end_frame_velocity, - feed_forward_force - ); -} - -void ImpedanceController::run_precomputed_data( - pinocchio::Data& pinocchio_data, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force) -{ - root_placement_ = pinocchio_data.oMf[root_frame_index_]; - end_placement_ = pinocchio_data.oMf[end_frame_index_]; - root_velocity_ = pinocchio::getFrameVelocity( - pinocchio_model_, pinocchio_data, root_frame_index_, pinocchio::WORLD); - end_velocity_ = pinocchio::getFrameVelocity( - pinocchio_model_, pinocchio_data, end_frame_index_, pinocchio::WORLD); - - // Orientations - root_orientation_.rotation() = root_placement_.rotation(); - end_orientation_.rotation() = end_placement_.rotation(); - - // Actual end frame placement in root frame. - actual_end_frame_placement_ = root_placement_.actInv(end_placement_); - - // Placement error. - err_se3_.head<3>() = root_orientation_.rotation() * - (desired_end_frame_placement.translation() - - actual_end_frame_placement_.translation()); - err_se3_.tail<3>() = - pinocchio::log3(desired_end_frame_placement.rotation().transpose() * - actual_end_frame_placement_.rotation()); - - // Actual end frame velocity in root frame. - actual_end_frame_velocity_ = - end_placement_.actInv(end_velocity_ - root_velocity_); - - // Velocity error. - err_vel_ = end_orientation_.act(desired_end_frame_velocity - - actual_end_frame_velocity_); - - // Compute the force to be applied to the environment. - impedance_force_ = gain_proportional * err_se3_.array(); - impedance_force_ += - (gain_derivative * err_vel_.toVector().array()).matrix(); - impedance_force_ -= - (gain_feed_forward_force * feed_forward_force.toVector().array()) - .matrix(); - - // Get the jacobian. - pinocchio::getFrameJacobian(pinocchio_model_, - pinocchio_data, - end_frame_index_, - pinocchio::LOCAL_WORLD_ALIGNED, - end_jacobian_); - - impedance_jacobian_ = end_jacobian_; - - // compute the output torques - torques_ = (impedance_jacobian_.transpose() * impedance_force_); - - if (pinocchio_model_has_free_flyer_) - joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); - else + void ImpedanceController::run( + Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force) { - joint_torques_ = torques_; + assert(robot_configuration.size() == pinocchio_model_.nq && + "robot_configuration is not of the good size."); + assert(robot_velocity.size() == pinocchio_model_.nv && + "robot_velocity is not of the good size."); + // Get the current frame placements and velocity. + pinocchio::forwardKinematics( + pinocchio_model_, pinocchio_data_, robot_configuration, robot_velocity); + pinocchio::updateFramePlacement( + pinocchio_model_, pinocchio_data_, root_frame_index_); + pinocchio::updateFramePlacement( + pinocchio_model_, pinocchio_data_, end_frame_index_); + // Compute the jacobians + pinocchio::computeJointJacobians( + pinocchio_model_, pinocchio_data_, robot_configuration); + // Compute relative velocity between frames + compute_relative_velocity_between_frames(pinocchio_data_, robot_configuration, robot_velocity); + run_precomputed_data(pinocchio_data_, + gain_proportional, + gain_derivative, + gain_feed_forward_force, + desired_end_frame_placement, + desired_end_frame_velocity, + feed_forward_force + ); } - return; -} - -void ImpedanceController::run_local(Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force) -{ - assert(robot_configuration.size() == pinocchio_model_.nq && - "robot_configuration is not of the good size."); - assert(robot_velocity.size() == pinocchio_model_.nv && - "robot_velocity is not of the good size."); - // Get the current frame placements and velocity. - // Compute the jacobians - pinocchio::computeJointJacobians( - pinocchio_model_, pinocchio_data_, robot_configuration); - pinocchio::updateFramePlacement( - pinocchio_model_, pinocchio_data_, root_frame_index_); - pinocchio::updateFramePlacement( - pinocchio_model_, pinocchio_data_, end_frame_index_); - // Compute relative velocity between frames - compute_relative_velocity_between_frames(pinocchio_data_, robot_configuration, robot_velocity); - root_placement_ = pinocchio_data_.oMf[root_frame_index_]; - end_placement_ = pinocchio_data_.oMf[end_frame_index_]; - - // Orientations - root_orientation_.rotation() = root_placement_.rotation(); - end_orientation_.rotation() = end_placement_.rotation(); - - // Actual end frame placement in root frame. - actual_end_frame_placement_ = pinocchio::SE3 (Eigen::Matrix3d::Identity(), end_placement_.translation() - root_placement_.translation()); - - // Placement error. - err_se3_.head<3>() = root_orientation_.rotation() * - (desired_end_frame_placement.translation() - - actual_end_frame_placement_.translation()); - - err_se3_.head<3>() = (desired_end_frame_placement.translation() - - actual_end_frame_placement_.translation()); - - err_se3_.tail<3>() = - pinocchio::log3(desired_end_frame_placement.rotation().transpose() * - actual_end_frame_placement_.rotation()); - - // Actual end frame velocity in root frame. - actual_end_frame_velocity_ = pinocchio::Motion(relative_vel_between_frames.segment(0, 3), Eigen::Vector3d({0, 0, 0})); - - // Velocity error - err_vel_ = (desired_end_frame_velocity - actual_end_frame_velocity_); - - // Compute the force to be applied to the environment. - impedance_force_ = gain_proportional * err_se3_.array(); - impedance_force_ += - (gain_derivative * err_vel_.toVector().array()).matrix(); - impedance_force_ -= - (gain_feed_forward_force * feed_forward_force.toVector().array()) - .matrix(); - - // Get the jacobian. - pinocchio::getFrameJacobian(pinocchio_model_, - pinocchio_data_, - end_frame_index_, - pinocchio::LOCAL_WORLD_ALIGNED, - end_jacobian_); - - impedance_jacobian_ = end_jacobian_; - - // compute the output torques - torques_ = (impedance_jacobian_.transpose() * impedance_force_); - - if (pinocchio_model_has_free_flyer_) - joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); - else + + void ImpedanceController::run_precomputed_data( + pinocchio::Data& pinocchio_data, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force) { - joint_torques_ = torques_; + root_placement_ = pinocchio_data.oMf[root_frame_index_]; + end_placement_ = pinocchio_data.oMf[end_frame_index_]; + root_velocity_ = pinocchio::getFrameVelocity( + pinocchio_model_, pinocchio_data, root_frame_index_, pinocchio::WORLD); + end_velocity_ = pinocchio::getFrameVelocity( + pinocchio_model_, pinocchio_data, end_frame_index_, pinocchio::WORLD); + + // Orientations + root_orientation_.rotation() = root_placement_.rotation(); + end_orientation_.rotation() = end_placement_.rotation(); + + // Actual end frame placement in root frame. + actual_end_frame_placement_ = root_placement_.actInv(end_placement_); + + // Placement error. + err_se3_.head<3>() = root_orientation_.rotation() * + (desired_end_frame_placement.translation() - + actual_end_frame_placement_.translation()); + err_se3_.tail<3>() = + pinocchio::log3(desired_end_frame_placement.rotation().transpose() * + actual_end_frame_placement_.rotation()); + + // Actual end frame velocity in root frame. + actual_end_frame_velocity_ = + end_placement_.actInv(end_velocity_ - root_velocity_); + + // Velocity error. + err_vel_ = end_orientation_.act(desired_end_frame_velocity - + actual_end_frame_velocity_); + + // Compute the force to be applied to the environment. + impedance_force_ = gain_proportional * err_se3_.array(); + impedance_force_ += + (gain_derivative * err_vel_.toVector().array()).matrix(); + impedance_force_ -= + (gain_feed_forward_force * feed_forward_force.toVector().array()) + .matrix(); + + // Get the jacobian. + pinocchio::getFrameJacobian(pinocchio_model_, + pinocchio_data, + end_frame_index_, + pinocchio::LOCAL_WORLD_ALIGNED, + end_jacobian_); + + impedance_jacobian_ = end_jacobian_; + + // compute the output torques + torques_ = (impedance_jacobian_.transpose() * impedance_force_); + + if (pinocchio_model_has_free_flyer_) + joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); + else + { + joint_torques_ = torques_; + } + return; } -} -const Eigen::VectorXd& ImpedanceController::get_torques() -{ - return torques_; -} + void ImpedanceController::run_local(Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force) + { + assert(robot_configuration.size() == pinocchio_model_.nq && + "robot_configuration is not of the good size."); + assert(robot_velocity.size() == pinocchio_model_.nv && + "robot_velocity is not of the good size."); + // Get the current frame placements and velocity. + // Compute the jacobians + pinocchio::computeJointJacobians( + pinocchio_model_, pinocchio_data_, robot_configuration); + pinocchio::framesForwardKinematics(pinocchio_model_, pinocchio_data_, robot_configuration); + // Compute relative velocity between frames + compute_relative_velocity_between_frames(pinocchio_data_, robot_configuration, robot_velocity); + root_placement_ = pinocchio_data_.oMf[root_frame_index_]; + end_placement_ = pinocchio_data_.oMf[end_frame_index_]; + + // Orientations + root_orientation_.rotation() = root_placement_.rotation(); + end_orientation_.rotation() = end_placement_.rotation(); + + // Actual end frame placement in root frame. + actual_end_frame_placement_ = pinocchio::SE3 (Eigen::Matrix3d::Identity(), end_placement_.translation() - root_placement_.translation()); + + // Placement error. + err_se3_.head<3>() = (desired_end_frame_placement.translation() - + actual_end_frame_placement_.translation()); + + err_se3_.tail<3>() = + pinocchio::log3(desired_end_frame_placement.rotation().transpose() * + actual_end_frame_placement_.rotation()); + + // Actual end frame velocity in root frame. + actual_end_frame_velocity_ = pinocchio::Motion(relative_vel_between_frames.head( 3), Eigen::Vector3d({0, 0, 0})); + + // Velocity error + err_vel_ = (desired_end_frame_velocity - actual_end_frame_velocity_); + + // Compute the force to be applied to the environment. + impedance_force_ = gain_proportional * err_se3_.array(); + impedance_force_ += + (gain_derivative * err_vel_.toVector().array()).matrix(); + impedance_force_ -= + (gain_feed_forward_force * feed_forward_force.toVector().array()) + .matrix(); + + // Get the jacobian. + pinocchio::getFrameJacobian(pinocchio_model_, + pinocchio_data_, + end_frame_index_, + pinocchio::LOCAL_WORLD_ALIGNED, + end_jacobian_); + + impedance_jacobian_ = end_jacobian_; + + // compute the output torques + torques_ = (impedance_jacobian_.transpose() * impedance_force_); + + if (pinocchio_model_has_free_flyer_) + joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); + else + { + joint_torques_ = torques_; + } + } -const Eigen::VectorXd& ImpedanceController::get_joint_torques() -{ - return joint_torques_; -} + const Eigen::VectorXd& ImpedanceController::get_torques() + { + return torques_; + } -const ImpedanceController::Vector6d& ImpedanceController::get_impedance_force() -{ - return impedance_force_; -} + const Eigen::VectorXd& ImpedanceController::get_joint_torques() + { + return joint_torques_; + } -const pinocchio::FrameIndex& ImpedanceController::get_endframe_index() -{ - return end_frame_index_; -} + const ImpedanceController::Vector6d& ImpedanceController::get_impedance_force() + { + return impedance_force_; + } -void ImpedanceController::compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, - Eigen::Ref &robot_configration, - Eigen::Ref &robot_velocity) -{ - Eigen::Matrix3d root_frame_rotation = pinocchio_data_.oMf[root_frame_index_].rotation(); - Eigen::Matrix3d end_frame_rotation = pinocchio_data_.oMf[end_frame_index_].rotation(); - pinocchio::SE3 frame_config_root = pinocchio::SE3(root_frame_rotation, Eigen::Vector3d::Zero()); - pinocchio::SE3 frame_config_end = pinocchio::SE3(end_frame_rotation, Eigen::Vector3d::Zero()); - // root frame jocobian - root_jacobian_.resize(6, pinocchio_model_.nv); - root_jacobian_.fill(0.); - Eigen::VectorXd vel_root_in_world_frame = frame_config_root.toActionMatrix() * root_jacobian_ * robot_velocity; - Eigen::VectorXd vel_end_in_world_frame = frame_config_end.toActionMatrix() * end_jacobian_ * robot_velocity; - relative_vel_between_frames = vel_end_in_world_frame - vel_root_in_world_frame; -} - -} // namespace mim_control \ No newline at end of file + const pinocchio::FrameIndex& ImpedanceController::get_endframe_index() + { + return end_frame_index_; + } + + const pinocchio::FrameIndex& ImpedanceController::get_rootframe_index() + { + return root_frame_index_; + } + + void ImpedanceController::compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, Eigen::Ref &robot_configration, Eigen::Ref &robot_velocity) { + Eigen::Matrix3d root_frame_rotation = pinocchio_data_.oMf[root_frame_index_].rotation(); + Eigen::Matrix3d end_frame_rotation = pinocchio_data_.oMf[end_frame_index_].rotation(); + pinocchio::SE3 frame_config_root = pinocchio::SE3(root_frame_rotation, Eigen::Vector3d::Zero()); + pinocchio::SE3 frame_config_end = pinocchio::SE3(end_frame_rotation, Eigen::Vector3d::Zero()); + // root frame jocobian + root_jacobian_.resize(6, pinocchio_model_.nv); + root_jacobian_.fill(0.); + pinocchio::getFrameJacobian(pinocchio_model_, + pinocchio_data, + root_frame_index_, + pinocchio::LOCAL, + root_jacobian_); + pinocchio::getFrameJacobian(pinocchio_model_, + pinocchio_data, + end_frame_index_, + pinocchio::LOCAL, + end_jacobian_); + Eigen::VectorXd vel_root_in_world_frame = frame_config_root.toActionMatrix() * root_jacobian_ * robot_velocity; + Eigen::VectorXd vel_end_in_world_frame = frame_config_end.toActionMatrix() * end_jacobian_ * robot_velocity; + relative_vel_between_frames = vel_end_in_world_frame - vel_root_in_world_frame; + } + +} // namespace mim_control_ From f1c66be3c0460daa04ffac9c32b2de90d92922ce Mon Sep 17 00:00:00 2001 From: stanley Date: Tue, 31 Jan 2023 19:55:32 -0500 Subject: [PATCH 03/11] Update package.xml --- package.xml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/package.xml b/package.xml index b6121d4..99dc35a 100644 --- a/package.xml +++ b/package.xml @@ -3,9 +3,8 @@ mim_control 0.0.0 - TODO: Package description - user - TODO: License declaration + Generic controllers for legged robots + Stanley Wu ament_cmake mpi_cmake_modules From 33ef65ed981ee8fc44a0f2e4ac6f4413b50edbf0 Mon Sep 17 00:00:00 2001 From: stanley Date: Wed, 1 Feb 2023 19:36:47 -0500 Subject: [PATCH 04/11] Add back license tag to avoid build error --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 99dc35a..e1b6fda 100644 --- a/package.xml +++ b/package.xml @@ -5,6 +5,7 @@ 0.0.0 Generic controllers for legged robots Stanley Wu + TODO: License declaration ament_cmake mpi_cmake_modules From 6addafde8db29fe797fc0ffc73f80fbddb08d407 Mon Sep 17 00:00:00 2001 From: stanley Date: Tue, 7 Feb 2023 11:08:17 -0500 Subject: [PATCH 05/11] Remove underscore --- src/impedance_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/impedance_controller.cpp b/src/impedance_controller.cpp index 36caeac..a62c0a3 100644 --- a/src/impedance_controller.cpp +++ b/src/impedance_controller.cpp @@ -283,4 +283,4 @@ namespace mim_control relative_vel_between_frames = vel_end_in_world_frame - vel_root_in_world_frame; } -} // namespace mim_control_ +} // namespace mim_control From 8001af6fc903e21acdab15fa5de9c1ac00d0fb9a Mon Sep 17 00:00:00 2001 From: stanley Date: Tue, 7 Feb 2023 17:31:22 -0500 Subject: [PATCH 06/11] Remove ament_cmake --- CMakeLists.txt | 19 +------------------ package.xml | 4 ---- 2 files changed, 1 insertion(+), 22 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9fc9200..da0ff28 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,7 +32,6 @@ search_for_boost_python() # Optionnal find_package(dynamic-graph QUIET) find_package(dynamic-graph-python QUIET) -find_package(ament_cmake QUIET) set(build_dynamic_graph_plugins FALSE) if(${dynamic-graph_FOUND} AND ${dynamic-graph-python_FOUND}) @@ -98,14 +97,6 @@ if(${build_dynamic_graph_plugins}) DESTINATION include) endif() -# -# Allow other ROS packages to link to this package during compilation -# -if (ament_cmake_FOUND) - ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) - ament_export_dependencies(yaml_utils) -endif() - # # Install the package # @@ -162,12 +153,4 @@ add_documentation() # # create the cmake package # -generate_cmake_package(INSTALL_EXPORT) - -# -# Allow other ROS 2 packages to find the header files -# -if (ament_cmake_FOUND) - ament_export_include_directories(include) - ament_package() -endif() +generate_cmake_package(INSTALL_EXPORT) \ No newline at end of file diff --git a/package.xml b/package.xml index e1b6fda..abeef4b 100644 --- a/package.xml +++ b/package.xml @@ -7,11 +7,7 @@ Stanley Wu TODO: License declaration - ament_cmake mpi_cmake_modules yaml_utils - - ament_cmake - \ No newline at end of file From 7b178a39cb19bb11356edc675257d02d3b5b2099 Mon Sep 17 00:00:00 2001 From: stanley Date: Tue, 7 Feb 2023 17:34:32 -0500 Subject: [PATCH 07/11] Add license --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index abeef4b..488c0aa 100644 --- a/package.xml +++ b/package.xml @@ -5,7 +5,7 @@ 0.0.0 Generic controllers for legged robots Stanley Wu - TODO: License declaration + BSD-3-Clause mpi_cmake_modules yaml_utils From ce5e3e16f2528de48f971af070d7308f8426e637 Mon Sep 17 00:00:00 2001 From: stanley Date: Sun, 19 Feb 2023 19:40:49 -0500 Subject: [PATCH 08/11] Replace mim_control_ with impedance --- include/mim_control/impedance_controller.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/include/mim_control/impedance_controller.hpp b/include/mim_control/impedance_controller.hpp index d546e32..829f9b4 100644 --- a/include/mim_control/impedance_controller.hpp +++ b/include/mim_control/impedance_controller.hpp @@ -4,7 +4,7 @@ * @copyright Copyright (c) 2020, New York University and Max Planck * Gesellschaft * - * @brief This is the implementation for mim_control_ controller between any two + * @brief This is the implementation for impedance controller between any two * frames of the robot. * */ @@ -116,21 +116,21 @@ namespace mim_control const pinocchio::Force& feed_forward_force); /** - * @brief Get the computed torques from the mim_control_ controller. + * @brief Get the computed torques from the impedance controller. * * @return Eigen::VectorXd& */ const Eigen::VectorXd& get_torques(); /** - * @brief Get the computed joint torques from the mim_control_ controller. + * @brief Get the computed joint torques from the impedance controller. * * @return Eigen::VectorXd& */ const Eigen::VectorXd& get_joint_torques(); /** - * @brief Get the mim_control_ force \f$ f_i \f$ with \f$ \tau = J^T f_i \f$. + * @brief Get the impedance force \f$ f_i \f$ with \f$ \tau = J^T f_i \f$. * * @return Vector6d& */ @@ -187,7 +187,7 @@ namespace mim_control /** @brief Cache of the rigid body dynamics algorithms. */ pinocchio::Data pinocchio_data_; - /** @brief (urdf) name of the root frame. The mim_control_ controller is + /** @brief (urdf) name of the root frame. The impedance controller is * computed with respect to this frame. */ std::string root_frame_name_; @@ -236,7 +236,7 @@ namespace mim_control /** @brief SE3 velocity error. */ pinocchio::Motion err_vel_; - /** @brief Jacobian used in the computation of the mim_control_. */ + /** @brief Jacobian used in the computation of the impedance. */ pinocchio::Data::Matrix6x impedance_jacobian_; /** @brief Output torques. */ @@ -258,4 +258,4 @@ namespace mim_control Eigen::VectorXd relative_vel_between_frames; }; -} // namespace mim_control_ \ No newline at end of file +} // namespace impedance \ No newline at end of file From 02d70c1d0026bf5b73e8ead5763cb71511d7a220 Mon Sep 17 00:00:00 2001 From: stanley Date: Sun, 19 Feb 2023 20:17:24 -0500 Subject: [PATCH 09/11] Fix quaternion_difference() --- include/mim_control/impedance_controller.hpp | 2 +- .../robot_centroidal_controller.py | 24 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/include/mim_control/impedance_controller.hpp b/include/mim_control/impedance_controller.hpp index 829f9b4..d8116d1 100644 --- a/include/mim_control/impedance_controller.hpp +++ b/include/mim_control/impedance_controller.hpp @@ -258,4 +258,4 @@ namespace mim_control Eigen::VectorXd relative_vel_between_frames; }; -} // namespace impedance \ No newline at end of file +} // namespace mim_control \ No newline at end of file diff --git a/python/mim_control/robot_centroidal_controller.py b/python/mim_control/robot_centroidal_controller.py index 15d5de5..b53df6a 100644 --- a/python/mim_control/robot_centroidal_controller.py +++ b/python/mim_control/robot_centroidal_controller.py @@ -1,3 +1,10 @@ +################################################################################################################## +## This file is the centroidal controller +################################################################################################################# +## Author: Avadesh Meduri & Julian Viereck +## Date: 9/12/2020 +################################################################################################################# + import numpy as np import pinocchio as pin @@ -63,8 +70,7 @@ def compute_com_wrench(self, q, dq, des_pos, des_vel, des_ori, des_angvel): vcom = np.reshape(np.array(dq[0:3]), (3,)) Ib = robot.mass(q)[3:6, 3:6] - tmp = pin.Quaternion(des_ori[3], des_ori[0], des_ori[1], des_ori[2]) * pin.Quaternion(q[6], q[3], q[4], q[5]).conjugate() - quat_diff = pin.log3(tmp.toRotationMatrix()) + quat_diff = self.quaternion_difference(des_ori, q) cur_angvel = arr(dq[3:6]) @@ -221,13 +227,7 @@ def integrate_quaternion(self, q, w): return self.quaternion_product(dq, q) def quaternion_difference(self, q1, q2): - """computes the tangent vector from q1 to q2 at Identity - returns vecotr w - s.t. q2 = exp(.5 * w)*q1 - """ - # first compute dq s.t. q2 = q1*dq - q1conjugate = np.array([-q1[0], -q1[1], -q1[2], q1[3]]) - # order of multiplication is very essential here - dq = self.quaternion_product(q2, q1conjugate) - # increment is log of dq - return self.log_quaternion(dq) + """computes the difference between 2 quaternions, q1 and q2 """ + tmp = pin.Quaternion(q1[3], q1[0], q1[1], q1[2]) * pin.Quaternion(q2[6], q2[3], q2[4], q2[5]).conjugate() + quat_diff = pin.log3(tmp.toRotationMatrix()) + return quat_diff From 0678ac617946f9e92a98a72c8512b523f2fccd2b Mon Sep 17 00:00:00 2001 From: stanley Date: Sun, 19 Feb 2023 20:30:36 -0500 Subject: [PATCH 10/11] Unindent class --- include/mim_control/impedance_controller.hpp | 476 +++++++++---------- 1 file changed, 238 insertions(+), 238 deletions(-) diff --git a/include/mim_control/impedance_controller.hpp b/include/mim_control/impedance_controller.hpp index d8116d1..ad4c9df 100644 --- a/include/mim_control/impedance_controller.hpp +++ b/include/mim_control/impedance_controller.hpp @@ -19,243 +19,243 @@ namespace mim_control /** * @brief Impedance controller between any two frames of the robot. */ - class ImpedanceController - { - public: - typedef Eigen::Array Array6d; - typedef Eigen::Matrix Vector6d; - - /** - * @brief Construct a new ImpedanceController object. - */ - ImpedanceController(); - - /** - * @brief Initialize the internal data. None real-time safe method. - * - * @param pinocchio_model rigid body model of the robot - * @param root_frame_name root frame name where the spring starts(Ex. Hip) - * @param end_frame_name frame name where the spring ends(Ex. end effector) - */ - void initialize(const pinocchio::Model& pinocchio_model, - const std::string& root_frame_name, - const std::string& end_frame_name); - - /** - * @brief Computes the desired joint torques - * - * \f$ - * \tau = J^T (k_p (x^{des} - x) + - * k_d (\dot{x}^{des} - \dot{x}) -k_f f) - * \f$ - * - * with: - * - \f$ \tau \f$ the joint torques, - * - \f$ J \f$ the Jacobian of the sub-kinematic-tree between the root and - * the end frame, - * - \f$ k_p \f$ the gain proportional to the frame placement error, - * - \f$ x_{des} \f$ desired end frame placement with respect to the - * desired root frame. - * - \f$ x \f$ measured end frame placement with respect to the - * measured root frame. - * - \f$ k_d \f$ is the derivative gain applied to the time derivative of - * the error. - * - \f$ \dot{x}^{des} \f$ desired end frame velocity with respect to the - * desired root frame. - * - \f$ \dot{x} \f$ measured end frame velocity with respect to the - * measured root frame. - * - \f$ k_f \f$ the gain over the feed forward force, - * - \f$ f \f$ the feed forward force, - * - * @param robot_configuration robot generalized coordinates configuration. - * @param robot_velocity robot generalized coordinates velocity. - * @param gain_proportional 6d vector for the proportional gains on {x, y, - * z, roll, pitch, yaw}. - * @param gain_derivative 6d vector for the proportional gains on {x, y, z, - * roll, pitch, yaw}. - * @param gain_feed_forward_force gain multiplying the feed forward force. - * @param desired_end_frame_placement desired end frame placement relative - * to the desired root joint. - * @param desired_end_frame_velocity desired end frame velocity relative to - * the desired root joint. - * @param feed_forward_force feed forward force applied to the foot by the - * environment. - */ - void run(Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force); - - - /** - * @brief Similar to `run()` but with the data already precomputed. - * - * @param pinocchio_data The data object to use for the computation. - * @param gain_proportional 6d vector for the proportional gains on {x, y, - * z, roll, pitch, yaw}. - * @param gain_derivative 6d vector for the proportional gains on {x, y, z, - * roll, pitch, yaw}. - * @param gain_feed_forward_force gain multiplying the feed forward force. - * @param desired_end_frame_placement desired end frame placement relative - * to the desired root joint. - * @param desired_end_frame_velocity desired end frame velocity relative to - * the desired root joint. - * @param feed_forward_force feed forward force applied to the foot by the - * environment. - */ - void run_precomputed_data(pinocchio::Data& pinocchio_data, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force); - - /** - * @brief Get the computed torques from the impedance controller. - * - * @return Eigen::VectorXd& - */ - const Eigen::VectorXd& get_torques(); - - /** - * @brief Get the computed joint torques from the impedance controller. - * - * @return Eigen::VectorXd& - */ - const Eigen::VectorXd& get_joint_torques(); - - /** - * @brief Get the impedance force \f$ f_i \f$ with \f$ \tau = J^T f_i \f$. - * - * @return Vector6d& - */ - const Vector6d& get_impedance_force(); - - /** - * @brief Returns the index of the endeffector frame. - * - * @return pinocchio::FrameIndex - */ - const pinocchio::FrameIndex& get_endframe_index(); - - const pinocchio::FrameIndex& get_rootframe_index(); - - /** - * @biref Similar to run(), but in the root joint frame - * - * @param robot_configuration robot generalized coordinates configuration. - * @param robot_velocity robot generalized coordinates velocity. - * @param gain_proportional 6d vector for the proportional gains on {x, y, - * z, roll, pitch, yaw}. - * @param gain_derivative 6d vector for the proportional gains on {x, y, z, - * roll, pitch, yaw}. - * @param gain_feed_forward_force gain multiplying the feed forward force. - * @param desired_end_frame_placement desired end frame placement relative - * to the desired root joint. - * @param desired_end_frame_velocity desired end frame velocity relative to - * the desired root joint. - * @param feed_forward_force feed forward force applied to the foot by the - * environment. - */ - void run_local(Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force); - - /** - * @brief computes the velocity of the end_frame with respect to a frame - whose origin aligns with the root frame but is oriented as the world frame - * - */ - void compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, - Eigen::Ref &robot_configration, - Eigen::Ref &robot_velocity); - - private: // attributes - /** @brief Rigid body dynamics model. */ - pinocchio::Model pinocchio_model_; - - /** @brief Cache of the rigid body dynamics algorithms. */ - pinocchio::Data pinocchio_data_; - - /** @brief (urdf) name of the root frame. The impedance controller is - * computed with respect to this frame. */ - std::string root_frame_name_; - - /** @brief Index of the root frame in the pinocchio model. */ - pinocchio::FrameIndex root_frame_index_; - - /** @brief Measured root frame placement. */ - pinocchio::SE3 root_placement_; - - /** @brief Measured root frame orientation. */ - pinocchio::SE3 root_orientation_; - - /** @brief Measured root frame velocity. */ - pinocchio::Motion root_velocity_; - - /** @brief (urdf) name of the end frame. This is the controlled frame. */ - std::string end_frame_name_; - - /** @brief Index of the end frame in the pinocchio model. */ - pinocchio::FrameIndex end_frame_index_; - - /** @brief Jacobian of the end frame. */ - pinocchio::Data::Matrix6x end_jacobian_; - - /** @brief Measured end frame placement. */ - pinocchio::SE3 end_placement_; - - /** @brief Measured end frame orientation. */ - pinocchio::SE3 end_orientation_; - - /** @brief Measured end frame velocity. */ - pinocchio::Motion end_velocity_; - - /** @brief Impedance force. Accessible for machine learning purposes. */ - Vector6d impedance_force_; - - /** @brief Measured end frame placement in root frame. */ - pinocchio::SE3 actual_end_frame_placement_; - - /** @brief Measured end frame placement in root frame. */ - pinocchio::Motion actual_end_frame_velocity_; - - /** @brief SE3 placement error. */ - Eigen::Matrix err_se3_; - - /** @brief SE3 velocity error. */ - pinocchio::Motion err_vel_; - - /** @brief Jacobian used in the computation of the impedance. */ - pinocchio::Data::Matrix6x impedance_jacobian_; - - /** @brief Output torques. */ - Eigen::VectorXd torques_; - - /** @brief Output torques. */ - Eigen::VectorXd joint_torques_; - - /** @brief Checks out if the Pinocchio rigid body model of the robot - * contains a free-flyer. This is used to return the command i.e. the - * torques to be applied to the joints only. */ - bool pinocchio_model_has_free_flyer_; - - /** @brief Stores the root jacobian */ - pinocchio::Data::Matrix6x root_jacobian_; - - /** @brief Relative velocity between the end_frame with respect to a frame - * whose origin aligns with the root frame but is oriented as the world frame. */ - Eigen::VectorXd relative_vel_between_frames; - }; +class ImpedanceController +{ +public: + typedef Eigen::Array Array6d; + typedef Eigen::Matrix Vector6d; + + /** + * @brief Construct a new ImpedanceController object. + */ + ImpedanceController(); + + /** + * @brief Initialize the internal data. None real-time safe method. + * + * @param pinocchio_model rigid body model of the robot + * @param root_frame_name root frame name where the spring starts(Ex. Hip) + * @param end_frame_name frame name where the spring ends(Ex. end effector) + */ + void initialize(const pinocchio::Model& pinocchio_model, + const std::string& root_frame_name, + const std::string& end_frame_name); + + /** + * @brief Computes the desired joint torques + * + * \f$ + * \tau = J^T (k_p (x^{des} - x) + + * k_d (\dot{x}^{des} - \dot{x}) -k_f f) + * \f$ + * + * with: + * - \f$ \tau \f$ the joint torques, + * - \f$ J \f$ the Jacobian of the sub-kinematic-tree between the root and + * the end frame, + * - \f$ k_p \f$ the gain proportional to the frame placement error, + * - \f$ x_{des} \f$ desired end frame placement with respect to the + * desired root frame. + * - \f$ x \f$ measured end frame placement with respect to the + * measured root frame. + * - \f$ k_d \f$ is the derivative gain applied to the time derivative of + * the error. + * - \f$ \dot{x}^{des} \f$ desired end frame velocity with respect to the + * desired root frame. + * - \f$ \dot{x} \f$ measured end frame velocity with respect to the + * measured root frame. + * - \f$ k_f \f$ the gain over the feed forward force, + * - \f$ f \f$ the feed forward force, + * + * @param robot_configuration robot generalized coordinates configuration. + * @param robot_velocity robot generalized coordinates velocity. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run(Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force); + + + /** + * @brief Similar to `run()` but with the data already precomputed. + * + * @param pinocchio_data The data object to use for the computation. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run_precomputed_data(pinocchio::Data& pinocchio_data, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force); + + /** + * @brief Get the computed torques from the impedance controller. + * + * @return Eigen::VectorXd& + */ + const Eigen::VectorXd& get_torques(); + + /** + * @brief Get the computed joint torques from the impedance controller. + * + * @return Eigen::VectorXd& + */ + const Eigen::VectorXd& get_joint_torques(); + + /** + * @brief Get the impedance force \f$ f_i \f$ with \f$ \tau = J^T f_i \f$. + * + * @return Vector6d& + */ + const Vector6d& get_impedance_force(); + + /** + * @brief Returns the index of the endeffector frame. + * + * @return pinocchio::FrameIndex + */ + const pinocchio::FrameIndex& get_endframe_index(); + + const pinocchio::FrameIndex& get_rootframe_index(); + + /** + * @biref Similar to run(), but in the root joint frame + * + * @param robot_configuration robot generalized coordinates configuration. + * @param robot_velocity robot generalized coordinates velocity. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run_local(Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double& gain_feed_forward_force, + const pinocchio::SE3& desired_end_frame_placement, + const pinocchio::Motion& desired_end_frame_velocity, + const pinocchio::Force& feed_forward_force); + + /** + * @brief computes the velocity of the end_frame with respect to a frame + whose origin aligns with the root frame but is oriented as the world frame + * + */ + void compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, + Eigen::Ref &robot_configration, + Eigen::Ref &robot_velocity); + +private: // attributes + /** @brief Rigid body dynamics model. */ + pinocchio::Model pinocchio_model_; + + /** @brief Cache of the rigid body dynamics algorithms. */ + pinocchio::Data pinocchio_data_; + + /** @brief (urdf) name of the root frame. The impedance controller is + * computed with respect to this frame. */ + std::string root_frame_name_; + + /** @brief Index of the root frame in the pinocchio model. */ + pinocchio::FrameIndex root_frame_index_; + + /** @brief Measured root frame placement. */ + pinocchio::SE3 root_placement_; + + /** @brief Measured root frame orientation. */ + pinocchio::SE3 root_orientation_; + + /** @brief Measured root frame velocity. */ + pinocchio::Motion root_velocity_; + + /** @brief (urdf) name of the end frame. This is the controlled frame. */ + std::string end_frame_name_; + + /** @brief Index of the end frame in the pinocchio model. */ + pinocchio::FrameIndex end_frame_index_; + + /** @brief Jacobian of the end frame. */ + pinocchio::Data::Matrix6x end_jacobian_; + + /** @brief Measured end frame placement. */ + pinocchio::SE3 end_placement_; + + /** @brief Measured end frame orientation. */ + pinocchio::SE3 end_orientation_; + + /** @brief Measured end frame velocity. */ + pinocchio::Motion end_velocity_; + + /** @brief Impedance force. Accessible for machine learning purposes. */ + Vector6d impedance_force_; + + /** @brief Measured end frame placement in root frame. */ + pinocchio::SE3 actual_end_frame_placement_; + + /** @brief Measured end frame placement in root frame. */ + pinocchio::Motion actual_end_frame_velocity_; + + /** @brief SE3 placement error. */ + Eigen::Matrix err_se3_; + + /** @brief SE3 velocity error. */ + pinocchio::Motion err_vel_; + + /** @brief Jacobian used in the computation of the impedance. */ + pinocchio::Data::Matrix6x impedance_jacobian_; + + /** @brief Output torques. */ + Eigen::VectorXd torques_; + + /** @brief Output torques. */ + Eigen::VectorXd joint_torques_; + + /** @brief Checks out if the Pinocchio rigid body model of the robot + * contains a free-flyer. This is used to return the command i.e. the + * torques to be applied to the joints only. */ + bool pinocchio_model_has_free_flyer_; + + /** @brief Stores the root jacobian */ + pinocchio::Data::Matrix6x root_jacobian_; + + /** @brief Relative velocity between the end_frame with respect to a frame + * whose origin aligns with the root frame but is oriented as the world frame. */ + Eigen::VectorXd relative_vel_between_frames; +}; } // namespace mim_control \ No newline at end of file From 42912fa633e80552a769dfaa559e2d1e82660cef Mon Sep 17 00:00:00 2001 From: stanley Date: Thu, 23 Feb 2023 16:32:35 -0500 Subject: [PATCH 11/11] Reformat hpp and cpp files --- include/mim_control/impedance_controller.hpp | 477 +++++++++-------- src/impedance_controller.cpp | 534 +++++++++---------- 2 files changed, 498 insertions(+), 513 deletions(-) diff --git a/include/mim_control/impedance_controller.hpp b/include/mim_control/impedance_controller.hpp index ad4c9df..2bc1e06 100644 --- a/include/mim_control/impedance_controller.hpp +++ b/include/mim_control/impedance_controller.hpp @@ -14,248 +14,247 @@ #include "pinocchio/multibody/data.hpp" #include "pinocchio/multibody/model.hpp" -namespace mim_control -{ +namespace mim_control { /** * @brief Impedance controller between any two frames of the robot. */ -class ImpedanceController -{ +class ImpedanceController { public: - typedef Eigen::Array Array6d; - typedef Eigen::Matrix Vector6d; - - /** - * @brief Construct a new ImpedanceController object. - */ - ImpedanceController(); - - /** - * @brief Initialize the internal data. None real-time safe method. - * - * @param pinocchio_model rigid body model of the robot - * @param root_frame_name root frame name where the spring starts(Ex. Hip) - * @param end_frame_name frame name where the spring ends(Ex. end effector) - */ - void initialize(const pinocchio::Model& pinocchio_model, - const std::string& root_frame_name, - const std::string& end_frame_name); - - /** - * @brief Computes the desired joint torques - * - * \f$ - * \tau = J^T (k_p (x^{des} - x) + - * k_d (\dot{x}^{des} - \dot{x}) -k_f f) - * \f$ - * - * with: - * - \f$ \tau \f$ the joint torques, - * - \f$ J \f$ the Jacobian of the sub-kinematic-tree between the root and - * the end frame, - * - \f$ k_p \f$ the gain proportional to the frame placement error, - * - \f$ x_{des} \f$ desired end frame placement with respect to the - * desired root frame. - * - \f$ x \f$ measured end frame placement with respect to the - * measured root frame. - * - \f$ k_d \f$ is the derivative gain applied to the time derivative of - * the error. - * - \f$ \dot{x}^{des} \f$ desired end frame velocity with respect to the - * desired root frame. - * - \f$ \dot{x} \f$ measured end frame velocity with respect to the - * measured root frame. - * - \f$ k_f \f$ the gain over the feed forward force, - * - \f$ f \f$ the feed forward force, - * - * @param robot_configuration robot generalized coordinates configuration. - * @param robot_velocity robot generalized coordinates velocity. - * @param gain_proportional 6d vector for the proportional gains on {x, y, - * z, roll, pitch, yaw}. - * @param gain_derivative 6d vector for the proportional gains on {x, y, z, - * roll, pitch, yaw}. - * @param gain_feed_forward_force gain multiplying the feed forward force. - * @param desired_end_frame_placement desired end frame placement relative - * to the desired root joint. - * @param desired_end_frame_velocity desired end frame velocity relative to - * the desired root joint. - * @param feed_forward_force feed forward force applied to the foot by the - * environment. - */ - void run(Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force); - - - /** - * @brief Similar to `run()` but with the data already precomputed. - * - * @param pinocchio_data The data object to use for the computation. - * @param gain_proportional 6d vector for the proportional gains on {x, y, - * z, roll, pitch, yaw}. - * @param gain_derivative 6d vector for the proportional gains on {x, y, z, - * roll, pitch, yaw}. - * @param gain_feed_forward_force gain multiplying the feed forward force. - * @param desired_end_frame_placement desired end frame placement relative - * to the desired root joint. - * @param desired_end_frame_velocity desired end frame velocity relative to - * the desired root joint. - * @param feed_forward_force feed forward force applied to the foot by the - * environment. - */ - void run_precomputed_data(pinocchio::Data& pinocchio_data, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force); - - /** - * @brief Get the computed torques from the impedance controller. - * - * @return Eigen::VectorXd& - */ - const Eigen::VectorXd& get_torques(); - - /** - * @brief Get the computed joint torques from the impedance controller. - * - * @return Eigen::VectorXd& - */ - const Eigen::VectorXd& get_joint_torques(); - - /** - * @brief Get the impedance force \f$ f_i \f$ with \f$ \tau = J^T f_i \f$. - * - * @return Vector6d& - */ - const Vector6d& get_impedance_force(); - - /** - * @brief Returns the index of the endeffector frame. - * - * @return pinocchio::FrameIndex - */ - const pinocchio::FrameIndex& get_endframe_index(); - - const pinocchio::FrameIndex& get_rootframe_index(); - - /** - * @biref Similar to run(), but in the root joint frame - * - * @param robot_configuration robot generalized coordinates configuration. - * @param robot_velocity robot generalized coordinates velocity. - * @param gain_proportional 6d vector for the proportional gains on {x, y, - * z, roll, pitch, yaw}. - * @param gain_derivative 6d vector for the proportional gains on {x, y, z, - * roll, pitch, yaw}. - * @param gain_feed_forward_force gain multiplying the feed forward force. - * @param desired_end_frame_placement desired end frame placement relative - * to the desired root joint. - * @param desired_end_frame_velocity desired end frame velocity relative to - * the desired root joint. - * @param feed_forward_force feed forward force applied to the foot by the - * environment. - */ - void run_local(Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force); - - /** - * @brief computes the velocity of the end_frame with respect to a frame - whose origin aligns with the root frame but is oriented as the world frame - * - */ - void compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, - Eigen::Ref &robot_configration, - Eigen::Ref &robot_velocity); - -private: // attributes - /** @brief Rigid body dynamics model. */ - pinocchio::Model pinocchio_model_; - - /** @brief Cache of the rigid body dynamics algorithms. */ - pinocchio::Data pinocchio_data_; - - /** @brief (urdf) name of the root frame. The impedance controller is - * computed with respect to this frame. */ - std::string root_frame_name_; - - /** @brief Index of the root frame in the pinocchio model. */ - pinocchio::FrameIndex root_frame_index_; - - /** @brief Measured root frame placement. */ - pinocchio::SE3 root_placement_; - - /** @brief Measured root frame orientation. */ - pinocchio::SE3 root_orientation_; - - /** @brief Measured root frame velocity. */ - pinocchio::Motion root_velocity_; - - /** @brief (urdf) name of the end frame. This is the controlled frame. */ - std::string end_frame_name_; - - /** @brief Index of the end frame in the pinocchio model. */ - pinocchio::FrameIndex end_frame_index_; - - /** @brief Jacobian of the end frame. */ - pinocchio::Data::Matrix6x end_jacobian_; - - /** @brief Measured end frame placement. */ - pinocchio::SE3 end_placement_; - - /** @brief Measured end frame orientation. */ - pinocchio::SE3 end_orientation_; - - /** @brief Measured end frame velocity. */ - pinocchio::Motion end_velocity_; - - /** @brief Impedance force. Accessible for machine learning purposes. */ - Vector6d impedance_force_; - - /** @brief Measured end frame placement in root frame. */ - pinocchio::SE3 actual_end_frame_placement_; - - /** @brief Measured end frame placement in root frame. */ - pinocchio::Motion actual_end_frame_velocity_; - - /** @brief SE3 placement error. */ - Eigen::Matrix err_se3_; - - /** @brief SE3 velocity error. */ - pinocchio::Motion err_vel_; - - /** @brief Jacobian used in the computation of the impedance. */ - pinocchio::Data::Matrix6x impedance_jacobian_; - - /** @brief Output torques. */ - Eigen::VectorXd torques_; - - /** @brief Output torques. */ - Eigen::VectorXd joint_torques_; - - /** @brief Checks out if the Pinocchio rigid body model of the robot - * contains a free-flyer. This is used to return the command i.e. the - * torques to be applied to the joints only. */ - bool pinocchio_model_has_free_flyer_; - - /** @brief Stores the root jacobian */ - pinocchio::Data::Matrix6x root_jacobian_; - - /** @brief Relative velocity between the end_frame with respect to a frame - * whose origin aligns with the root frame but is oriented as the world frame. */ - Eigen::VectorXd relative_vel_between_frames; + typedef Eigen::Array Array6d; + typedef Eigen::Matrix Vector6d; + + /** + * @brief Construct a new ImpedanceController object. + */ + ImpedanceController(); + + /** + * @brief Initialize the internal data. None real-time safe method. + * + * @param pinocchio_model rigid body model of the robot + * @param root_frame_name root frame name where the spring starts(Ex. Hip) + * @param end_frame_name frame name where the spring ends(Ex. end effector) + */ + void initialize(const pinocchio::Model &pinocchio_model, + const std::string &root_frame_name, + const std::string &end_frame_name); + + /** + * @brief Computes the desired joint torques + * + * \f$ + * \tau = J^T (k_p (x^{des} - x) + + * k_d (\dot{x}^{des} - \dot{x}) -k_f f) + * \f$ + * + * with: + * - \f$ \tau \f$ the joint torques, + * - \f$ J \f$ the Jacobian of the sub-kinematic-tree between the root and + * the end frame, + * - \f$ k_p \f$ the gain proportional to the frame placement error, + * - \f$ x_{des} \f$ desired end frame placement with respect to the + * desired root frame. + * - \f$ x \f$ measured end frame placement with respect to the + * measured root frame. + * - \f$ k_d \f$ is the derivative gain applied to the time derivative of + * the error. + * - \f$ \dot{x}^{des} \f$ desired end frame velocity with respect to the + * desired root frame. + * - \f$ \dot{x} \f$ measured end frame velocity with respect to the + * measured root frame. + * - \f$ k_f \f$ the gain over the feed forward force, + * - \f$ f \f$ the feed forward force, + * + * @param robot_configuration robot generalized coordinates configuration. + * @param robot_velocity robot generalized coordinates velocity. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run(Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double &gain_feed_forward_force, + const pinocchio::SE3 &desired_end_frame_placement, + const pinocchio::Motion &desired_end_frame_velocity, + const pinocchio::Force &feed_forward_force); + + /** + * @brief Similar to `run()` but with the data already precomputed. + * + * @param pinocchio_data The data object to use for the computation. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run_precomputed_data(pinocchio::Data &pinocchio_data, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double &gain_feed_forward_force, + const pinocchio::SE3 &desired_end_frame_placement, + const pinocchio::Motion &desired_end_frame_velocity, + const pinocchio::Force &feed_forward_force); + + /** + * @brief Get the computed torques from the impedance controller. + * + * @return Eigen::VectorXd& + */ + const Eigen::VectorXd &get_torques(); + + /** + * @brief Get the computed joint torques from the impedance controller. + * + * @return Eigen::VectorXd& + */ + const Eigen::VectorXd &get_joint_torques(); + + /** + * @brief Get the impedance force \f$ f_i \f$ with \f$ \tau = J^T f_i \f$. + * + * @return Vector6d& + */ + const Vector6d &get_impedance_force(); + + /** + * @brief Returns the index of the endeffector frame. + * + * @return pinocchio::FrameIndex + */ + const pinocchio::FrameIndex &get_endframe_index(); + + const pinocchio::FrameIndex &get_rootframe_index(); + + /** + * @biref Similar to run(), but in the root joint frame + * + * @param robot_configuration robot generalized coordinates configuration. + * @param robot_velocity robot generalized coordinates velocity. + * @param gain_proportional 6d vector for the proportional gains on {x, y, + * z, roll, pitch, yaw}. + * @param gain_derivative 6d vector for the proportional gains on {x, y, z, + * roll, pitch, yaw}. + * @param gain_feed_forward_force gain multiplying the feed forward force. + * @param desired_end_frame_placement desired end frame placement relative + * to the desired root joint. + * @param desired_end_frame_velocity desired end frame velocity relative to + * the desired root joint. + * @param feed_forward_force feed forward force applied to the foot by the + * environment. + */ + void run_local(Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double &gain_feed_forward_force, + const pinocchio::SE3 &desired_end_frame_placement, + const pinocchio::Motion &desired_end_frame_velocity, + const pinocchio::Force &feed_forward_force); + + /** + * @brief computes the velocity of the end_frame with respect to a frame + whose origin aligns with the root frame but is oriented as the world frame + * + */ + void compute_relative_velocity_between_frames( + pinocchio::Data &pinocchio_data, + Eigen::Ref &robot_configration, + Eigen::Ref &robot_velocity); + +private: // attributes + /** @brief Rigid body dynamics model. */ + pinocchio::Model pinocchio_model_; + + /** @brief Cache of the rigid body dynamics algorithms. */ + pinocchio::Data pinocchio_data_; + + /** @brief (urdf) name of the root frame. The impedance controller is + * computed with respect to this frame. */ + std::string root_frame_name_; + + /** @brief Index of the root frame in the pinocchio model. */ + pinocchio::FrameIndex root_frame_index_; + + /** @brief Measured root frame placement. */ + pinocchio::SE3 root_placement_; + + /** @brief Measured root frame orientation. */ + pinocchio::SE3 root_orientation_; + + /** @brief Measured root frame velocity. */ + pinocchio::Motion root_velocity_; + + /** @brief (urdf) name of the end frame. This is the controlled frame. */ + std::string end_frame_name_; + + /** @brief Index of the end frame in the pinocchio model. */ + pinocchio::FrameIndex end_frame_index_; + + /** @brief Jacobian of the end frame. */ + pinocchio::Data::Matrix6x end_jacobian_; + + /** @brief Measured end frame placement. */ + pinocchio::SE3 end_placement_; + + /** @brief Measured end frame orientation. */ + pinocchio::SE3 end_orientation_; + + /** @brief Measured end frame velocity. */ + pinocchio::Motion end_velocity_; + + /** @brief Impedance force. Accessible for machine learning purposes. */ + Vector6d impedance_force_; + + /** @brief Measured end frame placement in root frame. */ + pinocchio::SE3 actual_end_frame_placement_; + + /** @brief Measured end frame placement in root frame. */ + pinocchio::Motion actual_end_frame_velocity_; + + /** @brief SE3 placement error. */ + Eigen::Matrix err_se3_; + + /** @brief SE3 velocity error. */ + pinocchio::Motion err_vel_; + + /** @brief Jacobian used in the computation of the impedance. */ + pinocchio::Data::Matrix6x impedance_jacobian_; + + /** @brief Output torques. */ + Eigen::VectorXd torques_; + + /** @brief Output torques. */ + Eigen::VectorXd joint_torques_; + + /** @brief Checks out if the Pinocchio rigid body model of the robot + * contains a free-flyer. This is used to return the command i.e. the + * torques to be applied to the joints only. */ + bool pinocchio_model_has_free_flyer_; + + /** @brief Stores the root jacobian */ + pinocchio::Data::Matrix6x root_jacobian_; + + /** @brief Relative velocity between the end_frame with respect to a frame + * whose origin aligns with the root frame but is oriented as the world frame. + */ + Eigen::VectorXd relative_vel_between_frames; }; -} // namespace mim_control \ No newline at end of file +} // namespace mim_control \ No newline at end of file diff --git a/src/impedance_controller.cpp b/src/impedance_controller.cpp index a62c0a3..fce882e 100644 --- a/src/impedance_controller.cpp +++ b/src/impedance_controller.cpp @@ -10,277 +10,263 @@ #include "mim_control/impedance_controller.hpp" #include "pinocchio/algorithm/frames.hpp" -namespace mim_control -{ - ImpedanceController::ImpedanceController() - { - } - - void ImpedanceController::initialize(const pinocchio::Model& pinocchio_model, - const std::string& root_frame_name, - const std::string& end_frame_name) - { - // Copy the arguments internally. - pinocchio_model_ = pinocchio_model; - - // Create the cache of the rigid body dynamics algorithms - pinocchio_data_ = pinocchio::Data(pinocchio_model_); - - // Copy the arguments internally. - root_frame_name_ = root_frame_name; - end_frame_name_ = end_frame_name; - - // Fetch the index of the frame in the robot model. - root_frame_index_ = pinocchio_model_.getFrameId(root_frame_name_); - end_frame_index_ = pinocchio_model_.getFrameId(end_frame_name_); - - // initialize the size of the vectors. - end_jacobian_.resize(6, pinocchio_model_.nv); - end_jacobian_.fill(0.); - impedance_jacobian_.resize(6, pinocchio_model_.nv); - impedance_jacobian_.fill(0.); - - // Defines if the model has a freeflyer. - pinocchio_model_has_free_flyer_ = - pinocchio_model_.joints[1].shortname() == "JointModelFreeFlyer"; - - // output - torques_.resize(pinocchio_model_.nv, 1); - torques_.fill(0.); - if (pinocchio_model_has_free_flyer_) - joint_torques_.resize(pinocchio_model_.nv, 1); - else - { - joint_torques_.resize(pinocchio_model_.nv - 6, 1); - } - - // Intermediate variables. - root_orientation_ = pinocchio::SE3::Identity(); - end_orientation_ = pinocchio::SE3::Identity(); - } - - void ImpedanceController::run( - Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force) - { - assert(robot_configuration.size() == pinocchio_model_.nq && - "robot_configuration is not of the good size."); - assert(robot_velocity.size() == pinocchio_model_.nv && - "robot_velocity is not of the good size."); - // Get the current frame placements and velocity. - pinocchio::forwardKinematics( - pinocchio_model_, pinocchio_data_, robot_configuration, robot_velocity); - pinocchio::updateFramePlacement( - pinocchio_model_, pinocchio_data_, root_frame_index_); - pinocchio::updateFramePlacement( - pinocchio_model_, pinocchio_data_, end_frame_index_); - // Compute the jacobians - pinocchio::computeJointJacobians( - pinocchio_model_, pinocchio_data_, robot_configuration); - // Compute relative velocity between frames - compute_relative_velocity_between_frames(pinocchio_data_, robot_configuration, robot_velocity); - run_precomputed_data(pinocchio_data_, - gain_proportional, - gain_derivative, - gain_feed_forward_force, - desired_end_frame_placement, - desired_end_frame_velocity, - feed_forward_force - ); - } - - void ImpedanceController::run_precomputed_data( - pinocchio::Data& pinocchio_data, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force) - { - root_placement_ = pinocchio_data.oMf[root_frame_index_]; - end_placement_ = pinocchio_data.oMf[end_frame_index_]; - root_velocity_ = pinocchio::getFrameVelocity( - pinocchio_model_, pinocchio_data, root_frame_index_, pinocchio::WORLD); - end_velocity_ = pinocchio::getFrameVelocity( - pinocchio_model_, pinocchio_data, end_frame_index_, pinocchio::WORLD); - - // Orientations - root_orientation_.rotation() = root_placement_.rotation(); - end_orientation_.rotation() = end_placement_.rotation(); - - // Actual end frame placement in root frame. - actual_end_frame_placement_ = root_placement_.actInv(end_placement_); - - // Placement error. - err_se3_.head<3>() = root_orientation_.rotation() * - (desired_end_frame_placement.translation() - - actual_end_frame_placement_.translation()); - err_se3_.tail<3>() = - pinocchio::log3(desired_end_frame_placement.rotation().transpose() * - actual_end_frame_placement_.rotation()); - - // Actual end frame velocity in root frame. - actual_end_frame_velocity_ = - end_placement_.actInv(end_velocity_ - root_velocity_); - - // Velocity error. - err_vel_ = end_orientation_.act(desired_end_frame_velocity - - actual_end_frame_velocity_); - - // Compute the force to be applied to the environment. - impedance_force_ = gain_proportional * err_se3_.array(); - impedance_force_ += - (gain_derivative * err_vel_.toVector().array()).matrix(); - impedance_force_ -= - (gain_feed_forward_force * feed_forward_force.toVector().array()) - .matrix(); - - // Get the jacobian. - pinocchio::getFrameJacobian(pinocchio_model_, - pinocchio_data, - end_frame_index_, - pinocchio::LOCAL_WORLD_ALIGNED, - end_jacobian_); - - impedance_jacobian_ = end_jacobian_; - - // compute the output torques - torques_ = (impedance_jacobian_.transpose() * impedance_force_); - - if (pinocchio_model_has_free_flyer_) - joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); - else - { - joint_torques_ = torques_; - } - return; - } - - void ImpedanceController::run_local(Eigen::Ref robot_configuration, - Eigen::Ref robot_velocity, - Eigen::Ref gain_proportional, - Eigen::Ref gain_derivative, - const double& gain_feed_forward_force, - const pinocchio::SE3& desired_end_frame_placement, - const pinocchio::Motion& desired_end_frame_velocity, - const pinocchio::Force& feed_forward_force) - { - assert(robot_configuration.size() == pinocchio_model_.nq && - "robot_configuration is not of the good size."); - assert(robot_velocity.size() == pinocchio_model_.nv && - "robot_velocity is not of the good size."); - // Get the current frame placements and velocity. - // Compute the jacobians - pinocchio::computeJointJacobians( - pinocchio_model_, pinocchio_data_, robot_configuration); - pinocchio::framesForwardKinematics(pinocchio_model_, pinocchio_data_, robot_configuration); - // Compute relative velocity between frames - compute_relative_velocity_between_frames(pinocchio_data_, robot_configuration, robot_velocity); - root_placement_ = pinocchio_data_.oMf[root_frame_index_]; - end_placement_ = pinocchio_data_.oMf[end_frame_index_]; - - // Orientations - root_orientation_.rotation() = root_placement_.rotation(); - end_orientation_.rotation() = end_placement_.rotation(); - - // Actual end frame placement in root frame. - actual_end_frame_placement_ = pinocchio::SE3 (Eigen::Matrix3d::Identity(), end_placement_.translation() - root_placement_.translation()); - - // Placement error. - err_se3_.head<3>() = (desired_end_frame_placement.translation() - - actual_end_frame_placement_.translation()); - - err_se3_.tail<3>() = - pinocchio::log3(desired_end_frame_placement.rotation().transpose() * - actual_end_frame_placement_.rotation()); - - // Actual end frame velocity in root frame. - actual_end_frame_velocity_ = pinocchio::Motion(relative_vel_between_frames.head( 3), Eigen::Vector3d({0, 0, 0})); - - // Velocity error - err_vel_ = (desired_end_frame_velocity - actual_end_frame_velocity_); - - // Compute the force to be applied to the environment. - impedance_force_ = gain_proportional * err_se3_.array(); - impedance_force_ += - (gain_derivative * err_vel_.toVector().array()).matrix(); - impedance_force_ -= - (gain_feed_forward_force * feed_forward_force.toVector().array()) - .matrix(); - - // Get the jacobian. - pinocchio::getFrameJacobian(pinocchio_model_, - pinocchio_data_, - end_frame_index_, - pinocchio::LOCAL_WORLD_ALIGNED, - end_jacobian_); - - impedance_jacobian_ = end_jacobian_; - - // compute the output torques - torques_ = (impedance_jacobian_.transpose() * impedance_force_); - - if (pinocchio_model_has_free_flyer_) - joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); - else - { - joint_torques_ = torques_; - } - } - - const Eigen::VectorXd& ImpedanceController::get_torques() - { - return torques_; - } - - const Eigen::VectorXd& ImpedanceController::get_joint_torques() - { - return joint_torques_; - } - - const ImpedanceController::Vector6d& ImpedanceController::get_impedance_force() - { - return impedance_force_; - } - - const pinocchio::FrameIndex& ImpedanceController::get_endframe_index() - { - return end_frame_index_; - } - - const pinocchio::FrameIndex& ImpedanceController::get_rootframe_index() - { - return root_frame_index_; - } - - void ImpedanceController::compute_relative_velocity_between_frames(pinocchio::Data& pinocchio_data, Eigen::Ref &robot_configration, Eigen::Ref &robot_velocity) { - Eigen::Matrix3d root_frame_rotation = pinocchio_data_.oMf[root_frame_index_].rotation(); - Eigen::Matrix3d end_frame_rotation = pinocchio_data_.oMf[end_frame_index_].rotation(); - pinocchio::SE3 frame_config_root = pinocchio::SE3(root_frame_rotation, Eigen::Vector3d::Zero()); - pinocchio::SE3 frame_config_end = pinocchio::SE3(end_frame_rotation, Eigen::Vector3d::Zero()); - // root frame jocobian - root_jacobian_.resize(6, pinocchio_model_.nv); - root_jacobian_.fill(0.); - pinocchio::getFrameJacobian(pinocchio_model_, - pinocchio_data, - root_frame_index_, - pinocchio::LOCAL, - root_jacobian_); - pinocchio::getFrameJacobian(pinocchio_model_, - pinocchio_data, - end_frame_index_, - pinocchio::LOCAL, - end_jacobian_); - Eigen::VectorXd vel_root_in_world_frame = frame_config_root.toActionMatrix() * root_jacobian_ * robot_velocity; - Eigen::VectorXd vel_end_in_world_frame = frame_config_end.toActionMatrix() * end_jacobian_ * robot_velocity; - relative_vel_between_frames = vel_end_in_world_frame - vel_root_in_world_frame; - } - -} // namespace mim_control +namespace mim_control { +ImpedanceController::ImpedanceController() {} + +void ImpedanceController::initialize(const pinocchio::Model &pinocchio_model, + const std::string &root_frame_name, + const std::string &end_frame_name) { + // Copy the arguments internally. + pinocchio_model_ = pinocchio_model; + + // Create the cache of the rigid body dynamics algorithms + pinocchio_data_ = pinocchio::Data(pinocchio_model_); + + // Copy the arguments internally. + root_frame_name_ = root_frame_name; + end_frame_name_ = end_frame_name; + + // Fetch the index of the frame in the robot model. + root_frame_index_ = pinocchio_model_.getFrameId(root_frame_name_); + end_frame_index_ = pinocchio_model_.getFrameId(end_frame_name_); + + // initialize the size of the vectors. + end_jacobian_.resize(6, pinocchio_model_.nv); + end_jacobian_.fill(0.); + impedance_jacobian_.resize(6, pinocchio_model_.nv); + impedance_jacobian_.fill(0.); + + // Defines if the model has a freeflyer. + pinocchio_model_has_free_flyer_ = + pinocchio_model_.joints[1].shortname() == "JointModelFreeFlyer"; + + // output + torques_.resize(pinocchio_model_.nv, 1); + torques_.fill(0.); + if (pinocchio_model_has_free_flyer_) + joint_torques_.resize(pinocchio_model_.nv, 1); + else { + joint_torques_.resize(pinocchio_model_.nv - 6, 1); + } + + // Intermediate variables. + root_orientation_ = pinocchio::SE3::Identity(); + end_orientation_ = pinocchio::SE3::Identity(); +} + +void ImpedanceController::run( + Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double &gain_feed_forward_force, + const pinocchio::SE3 &desired_end_frame_placement, + const pinocchio::Motion &desired_end_frame_velocity, + const pinocchio::Force &feed_forward_force) { + assert(robot_configuration.size() == pinocchio_model_.nq && + "robot_configuration is not of the good size."); + assert(robot_velocity.size() == pinocchio_model_.nv && + "robot_velocity is not of the good size."); + // Get the current frame placements and velocity. + pinocchio::forwardKinematics(pinocchio_model_, pinocchio_data_, + robot_configuration, robot_velocity); + pinocchio::updateFramePlacement(pinocchio_model_, pinocchio_data_, + root_frame_index_); + pinocchio::updateFramePlacement(pinocchio_model_, pinocchio_data_, + end_frame_index_); + // Compute the jacobians + pinocchio::computeJointJacobians(pinocchio_model_, pinocchio_data_, + robot_configuration); + // Compute relative velocity between frames + compute_relative_velocity_between_frames(pinocchio_data_, robot_configuration, + robot_velocity); + run_precomputed_data(pinocchio_data_, gain_proportional, gain_derivative, + gain_feed_forward_force, desired_end_frame_placement, + desired_end_frame_velocity, feed_forward_force); +} + +void ImpedanceController::run_precomputed_data( + pinocchio::Data &pinocchio_data, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double &gain_feed_forward_force, + const pinocchio::SE3 &desired_end_frame_placement, + const pinocchio::Motion &desired_end_frame_velocity, + const pinocchio::Force &feed_forward_force) { + root_placement_ = pinocchio_data.oMf[root_frame_index_]; + end_placement_ = pinocchio_data.oMf[end_frame_index_]; + root_velocity_ = pinocchio::getFrameVelocity( + pinocchio_model_, pinocchio_data, root_frame_index_, pinocchio::WORLD); + end_velocity_ = pinocchio::getFrameVelocity( + pinocchio_model_, pinocchio_data, end_frame_index_, pinocchio::WORLD); + + // Orientations + root_orientation_.rotation() = root_placement_.rotation(); + end_orientation_.rotation() = end_placement_.rotation(); + + // Actual end frame placement in root frame. + actual_end_frame_placement_ = root_placement_.actInv(end_placement_); + + // Placement error. + err_se3_.head<3>() = root_orientation_.rotation() * + (desired_end_frame_placement.translation() - + actual_end_frame_placement_.translation()); + err_se3_.tail<3>() = + pinocchio::log3(desired_end_frame_placement.rotation().transpose() * + actual_end_frame_placement_.rotation()); + + // Actual end frame velocity in root frame. + actual_end_frame_velocity_ = + end_placement_.actInv(end_velocity_ - root_velocity_); + + // Velocity error. + err_vel_ = end_orientation_.act(desired_end_frame_velocity - + actual_end_frame_velocity_); + + // Compute the force to be applied to the environment. + impedance_force_ = gain_proportional * err_se3_.array(); + impedance_force_ += (gain_derivative * err_vel_.toVector().array()).matrix(); + impedance_force_ -= + (gain_feed_forward_force * feed_forward_force.toVector().array()) + .matrix(); + + // Get the jacobian. + pinocchio::getFrameJacobian(pinocchio_model_, pinocchio_data, + end_frame_index_, pinocchio::LOCAL_WORLD_ALIGNED, + end_jacobian_); + + impedance_jacobian_ = end_jacobian_; + + // compute the output torques + torques_ = (impedance_jacobian_.transpose() * impedance_force_); + + if (pinocchio_model_has_free_flyer_) + joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); + else { + joint_torques_ = torques_; + } + return; +} + +void ImpedanceController::run_local( + Eigen::Ref robot_configuration, + Eigen::Ref robot_velocity, + Eigen::Ref gain_proportional, + Eigen::Ref gain_derivative, + const double &gain_feed_forward_force, + const pinocchio::SE3 &desired_end_frame_placement, + const pinocchio::Motion &desired_end_frame_velocity, + const pinocchio::Force &feed_forward_force) { + assert(robot_configuration.size() == pinocchio_model_.nq && + "robot_configuration is not of the good size."); + assert(robot_velocity.size() == pinocchio_model_.nv && + "robot_velocity is not of the good size."); + // Get the current frame placements and velocity. + // Compute the jacobians + pinocchio::computeJointJacobians(pinocchio_model_, pinocchio_data_, + robot_configuration); + pinocchio::framesForwardKinematics(pinocchio_model_, pinocchio_data_, + robot_configuration); + // Compute relative velocity between frames + compute_relative_velocity_between_frames(pinocchio_data_, robot_configuration, + robot_velocity); + root_placement_ = pinocchio_data_.oMf[root_frame_index_]; + end_placement_ = pinocchio_data_.oMf[end_frame_index_]; + + // Orientations + root_orientation_.rotation() = root_placement_.rotation(); + end_orientation_.rotation() = end_placement_.rotation(); + + // Actual end frame placement in root frame. + actual_end_frame_placement_ = pinocchio::SE3( + Eigen::Matrix3d::Identity(), + end_placement_.translation() - root_placement_.translation()); + + // Placement error. + err_se3_.head<3>() = (desired_end_frame_placement.translation() - + actual_end_frame_placement_.translation()); + + err_se3_.tail<3>() = + pinocchio::log3(desired_end_frame_placement.rotation().transpose() * + actual_end_frame_placement_.rotation()); + + // Actual end frame velocity in root frame. + actual_end_frame_velocity_ = pinocchio::Motion( + relative_vel_between_frames.head(3), Eigen::Vector3d({0, 0, 0})); + + // Velocity error + err_vel_ = (desired_end_frame_velocity - actual_end_frame_velocity_); + + // Compute the force to be applied to the environment. + impedance_force_ = gain_proportional * err_se3_.array(); + impedance_force_ += (gain_derivative * err_vel_.toVector().array()).matrix(); + impedance_force_ -= + (gain_feed_forward_force * feed_forward_force.toVector().array()) + .matrix(); + + // Get the jacobian. + pinocchio::getFrameJacobian(pinocchio_model_, pinocchio_data_, + end_frame_index_, pinocchio::LOCAL_WORLD_ALIGNED, + end_jacobian_); + + impedance_jacobian_ = end_jacobian_; + + // compute the output torques + torques_ = (impedance_jacobian_.transpose() * impedance_force_); + + if (pinocchio_model_has_free_flyer_) + joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); + else { + joint_torques_ = torques_; + } +} + +const Eigen::VectorXd &ImpedanceController::get_torques() { return torques_; } + +const Eigen::VectorXd &ImpedanceController::get_joint_torques() { + return joint_torques_; +} + +const ImpedanceController::Vector6d & +ImpedanceController::get_impedance_force() { + return impedance_force_; +} + +const pinocchio::FrameIndex &ImpedanceController::get_endframe_index() { + return end_frame_index_; +} + +const pinocchio::FrameIndex &ImpedanceController::get_rootframe_index() { + return root_frame_index_; +} + +void ImpedanceController::compute_relative_velocity_between_frames( + pinocchio::Data &pinocchio_data, + Eigen::Ref &robot_configration, + Eigen::Ref &robot_velocity) { + Eigen::Matrix3d root_frame_rotation = + pinocchio_data_.oMf[root_frame_index_].rotation(); + Eigen::Matrix3d end_frame_rotation = + pinocchio_data_.oMf[end_frame_index_].rotation(); + pinocchio::SE3 frame_config_root = + pinocchio::SE3(root_frame_rotation, Eigen::Vector3d::Zero()); + pinocchio::SE3 frame_config_end = + pinocchio::SE3(end_frame_rotation, Eigen::Vector3d::Zero()); + // root frame jocobian + root_jacobian_.resize(6, pinocchio_model_.nv); + root_jacobian_.fill(0.); + pinocchio::getFrameJacobian(pinocchio_model_, pinocchio_data, + root_frame_index_, pinocchio::LOCAL, + root_jacobian_); + pinocchio::getFrameJacobian(pinocchio_model_, pinocchio_data, + end_frame_index_, pinocchio::LOCAL, + end_jacobian_); + Eigen::VectorXd vel_root_in_world_frame = + frame_config_root.toActionMatrix() * root_jacobian_ * robot_velocity; + Eigen::VectorXd vel_end_in_world_frame = + frame_config_end.toActionMatrix() * end_jacobian_ * robot_velocity; + relative_vel_between_frames = + vel_end_in_world_frame - vel_root_in_world_frame; +} + +} // namespace mim_control