Impedance ref link and pose reference with custom reference frame

README.md

@@ -96,6 +96,7 @@ CartesianImpedance_trajectory_controller:
   dynamic_reconfigure: true             # Starts dynamic reconfigure server
   handle_trajectories: true             # Accept traj., e.g. from MoveIt
   robot_description: /robot_description # In case of a varying name
+  control_frame: iiwa_link_ee           # The frame to which Stiffness and damping refer to (must belong to the robot body)
   wrench_ee_frame: iiwa_link_ee         # Default frame for wrench commands
   delta_tau_max: 1.0                    # Max. commanded torque diff between steps in Nm
   filtering:                            # Update existing values (0.0 1.0] per s

include/cartesian_impedance_controller/cartesian_impedance_controller.h

@@ -142,7 +142,7 @@ namespace cartesian_impedance_controller
     */
     Eigen::VectorXd calculateCommandedTorques(const Eigen::VectorXd &q, const Eigen::VectorXd &dq,
                                               const Eigen::Vector3d &position, Eigen::Quaterniond orientation,
-                                              const Eigen::MatrixXd &jacobian);
+                                              const Eigen::MatrixXd &jacobian, const Eigen::Matrix3d &R_control_root = Eigen::Matrix3d::Identity());
 
     /*! \brief Get the state of the controller. Updates when "calculateCommandedTorques" is called
     * 
@@ -215,6 +215,8 @@ namespace cartesian_impedance_controller
 
     Eigen::MatrixXd jacobian_; //!< Jacobian. Row format: 3 translations, 3 rotation
 
+    Eigen::Matrix<double, 6,6> T_control_w_adj_{Eigen::Matrix<double, 6,6>::Identity()};
+
     // End Effector
     Eigen::Matrix<double, 6, 1> error_; //!< Calculate pose error
     Eigen::Vector3d position_{Eigen::Vector3d::Zero()};           //!< Current end-effector position

include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h

@@ -216,6 +216,21 @@ namespace cartesian_impedance_controller
     */
     bool transformWrench(Eigen::Matrix<double, 6, 1> *cartesian_wrench, const std::string &from_frame, const std::string &to_frame) const;
 
+    /*! \brief Transforms the pose (3D vector and quaternion) in a target frame.
+    *
+    * Takes a vector and a quaternion and transforms it to a given coordinate frame. 
+    * E.g. from_frame= "world" , to_frame = "bh_link_ee"
+    * 
+    * @sa referencePoseCb
+    * \param[in] pos Vector representing the linear position
+    * \param[in] quat Quaternion representing the angular position
+    * \param[in] from_frame       Source frame
+    * \param[in] to_frame         Target frame
+    * \return True on success, false on failure.
+    */
+    bool transformPose(Eigen::Vector3d *pos, Eigen::Quaterniond *quat,
+                      const std::string &from_frame, const std::string &to_frame) const;
+
     /*! \brief Verbose printing; publishes ROS messages and tf frames.
      *
      * Always publishes commanded torques.
@@ -284,6 +299,7 @@ namespace cartesian_impedance_controller
     std::string end_effector_;      //!< End-effector link name
     std::string robot_description_; //!< URDF of the robot
     std::string root_frame_;        //!< Base frame obtained from URDF
+    std::string control_frame_;     //!< Frame wrt impedance is referred
 
     Eigen::VectorXd tau_m_;         //!< Measured joint torques
 

include/cartesian_impedance_controller/rbdyn_wrapper.h

@@ -39,6 +39,9 @@ class rbdyn_wrapper
         return;
       }
     }
+
+    _R_control_root.setIdentity();
+
     throw std::runtime_error("Index for end effector link " + end_effector + " not found in URDF. Aborting.");
   }
 
@@ -57,6 +60,12 @@ class rbdyn_wrapper
     rbd::forwardKinematics(rbdyn_urdf.mb, rbdyn_urdf.mbc);
     rbd::forwardVelocity(rbdyn_urdf.mb, rbdyn_urdf.mbc);
 
+    if (_cf_index != 0) { //0 is root link
+
+      sva::PTransformd tf_control_frame = rbdyn_urdf.mbc.bodyPosW.at(_cf_index);
+      _R_control_root = sva::conversions::toHomogeneous(tf_control_frame).topLeftCorner<3,3>();
+    }
+
     return jac.jacobian(rbdyn_urdf.mb, rbdyn_urdf.mbc);
   }
 
@@ -113,6 +122,23 @@ class rbdyn_wrapper
     return _rbdyn_urdf.mb.body(0).name();
   }
 
+  Eigen::Matrix3d get_R_control_frame() const {
+    return _R_control_root;
+  }
+
+  void set_control_frame(const std::string& control_frame) {
+
+    for (size_t i = 0; i < _rbdyn_urdf.mb.nrBodies(); i++)
+    {
+      if (_rbdyn_urdf.mb.body(i).name() == control_frame)
+      {
+        _cf_index = i;
+        return;
+      }
+    }
+    throw std::runtime_error("Index for control frame link " + control_frame + " not found in URDF. Aborting.");
+  }
+
 private:
   void _update_urdf_state(mc_rbdyn_urdf::URDFParserResult &rbdyn_urdf, const Eigen::VectorXd &q,
                           const Eigen::VectorXd &dq)
@@ -149,4 +175,6 @@ class rbdyn_wrapper
   mc_rbdyn_urdf::URDFParserResult _rbdyn_urdf;
   std::vector<size_t> _rbd_indices;
   size_t _ef_index;
+  size_t _cf_index;
+  Eigen::Matrix3d _R_control_root;
 };

src/cartesian_impedance_controller.cpp

@@ -234,14 +234,17 @@ namespace cartesian_impedance_controller
                                                                           const Eigen::VectorXd &dq,
                                                                           const Eigen::Vector3d &position,
                                                                           Eigen::Quaterniond orientation,
-                                                                          const Eigen::MatrixXd &jacobian)
+                                                                          const Eigen::MatrixXd &jacobian,
+                                                                          const Eigen::Matrix3d &R_control_root)
   {
     // Update controller to the current robot state
     this->q_ = q;
     this->dq_ = dq;
     this->position_ << position;
     this->orientation_.coeffs() << orientation.coeffs();
     this->jacobian_ << jacobian;
+    T_control_w_adj_ << R_control_root, Eigen::Matrix3d::Zero(), 
+                        Eigen::Matrix3d::Zero(), R_control_root;
 
     return this->calculateCommandedTorques();
   }
@@ -265,7 +268,8 @@ namespace cartesian_impedance_controller
     Eigen::VectorXd tau_task(this->n_joints_), tau_nullspace(this->n_joints_), tau_ext(this->n_joints_);
 
     // Torque calculated for Cartesian impedance control with respect to a Cartesian pose reference in the end, in the frame of the EE of the robot.
-    tau_task << this->jacobian_.transpose() * (-this->cartesian_stiffness_ * this->error_ - this->cartesian_damping_ * (this->jacobian_ * this->dq_));
+    tau_task << this->jacobian_.transpose() * (T_control_w_adj_ * -this->cartesian_stiffness_ * T_control_w_adj_.transpose() * this->error_ 
+                                              - T_control_w_adj_ * this->cartesian_damping_ * T_control_w_adj_.transpose() * (this->jacobian_ * this->dq_));
     // Torque for joint impedance control with respect to a desired configuration and projected in the null-space of the robot's Jacobian, so it should not affect the Cartesian motion of the robot's end-effector.
     tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - this->jacobian_.transpose() * jacobian_transpose_pinv) *
                          (this->nullspace_stiffness_ * (this->q_d_nullspace_ - this->q_) - this->nullspace_damping_ * this->dq_);

src/cartesian_impedance_controller_ros.cpp

@@ -99,6 +99,7 @@ namespace cartesian_impedance_controller
     nh->getParam("joints", joint_names);
     this->pub_state_.init(*nh, "controller_state", 10);
     this->pub_state_.msg_.header.seq = 0;
+    this->pub_state_.msg_.header.frame_id = this->root_frame_;
     for (size_t i = 0; i < this->n_joints_; i++)
     {
       this->pub_state_.msg_.joint_state.name.push_back(joint_names.at(i));
@@ -162,9 +163,10 @@ namespace cartesian_impedance_controller
 
     // Fetch parameters
     node_handle.param<std::string>("end_effector", this->end_effector_, "iiwa_link_ee");
-    ROS_INFO_STREAM("End effektor link is: " << this->end_effector_);
+    ROS_INFO_STREAM("End effector link is: " << this->end_effector_);
     // Frame for applying commanded Cartesian wrenches
     node_handle.param<std::string>("wrench_ee_frame", this->wrench_ee_frame_, this->end_effector_);
+    ROS_INFO_STREAM("Wrench end effector frame is: " << this->wrench_ee_frame_);
     bool dynamic_reconfigure{true};
     node_handle.param<bool>("dynamic_reconfigure", dynamic_reconfigure, true);
     bool enable_trajectories{true};
@@ -189,6 +191,10 @@ namespace cartesian_impedance_controller
     }
     this->root_frame_ = this->rbdyn_wrapper_.root_link();
     node_handle.setParam("root_frame", this->root_frame_);
+    // The reference frame the stiffness and damping refer to
+    node_handle.param<std::string>("control_frame", this->control_frame_, this->rbdyn_wrapper_.root_link());
+    this->rbdyn_wrapper_.set_control_frame(this->control_frame_);
+    ROS_INFO_STREAM("Control frame is: " << this->control_frame_);
 
     // Initialize base_tools and member variables
     this->setNumberOfJoints(this->joint_handles_.size());
@@ -293,6 +299,14 @@ namespace cartesian_impedance_controller
     }
     getJacobian(this->q_, this->dq_, &this->jacobian_);
     getFk(this->q_, &this->position_, &this->orientation_);
+
+    if (control_frame_.compare(root_frame_) != 0 ) {
+
+      Eigen::Matrix3d R = this->rbdyn_wrapper_.get_R_control_frame();
+      T_control_w_adj_ << R, Eigen::Matrix3d::Zero(), 
+                          Eigen::Matrix3d::Zero(), R;
+
+    }
   }
 
   void CartesianImpedanceControllerRos::controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg)
@@ -322,17 +336,24 @@ namespace cartesian_impedance_controller
 
   void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg)
   {
-    if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_)
-    {
-      ROS_WARN_STREAM("Reference poses need to be in the root frame '" << this->root_frame_ << "'. Ignoring.");
-      return;
-    }
+
     Eigen::Vector3d position_d;
     position_d << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
     const Eigen::Quaterniond last_orientation_d_target(this->orientation_d_);
     Eigen::Quaterniond orientation_d;
     orientation_d.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z,
         msg->pose.orientation.w;
+
+    //if empty, consider as wrt root link
+    if  (!msg->header.frame_id.empty() && msg->header.frame_id.compare(this->root_frame_) != 0)
+    {
+      if (!transformPose(&position_d, &orientation_d, msg->header.frame_id, this->root_frame_))
+      {
+        ROS_ERROR("Could not transform Pose. Not applying it.");
+        return;
+      }
+    }
+
     if (last_orientation_d_target.coeffs().dot(this->orientation_d_.coeffs()) < 0.0)
     {
       this->orientation_d_.coeffs() << -this->orientation_d_.coeffs();
@@ -413,6 +434,33 @@ namespace cartesian_impedance_controller
     }
   }
 
+  bool CartesianImpedanceControllerRos::transformPose(Eigen::Vector3d *pos, Eigen::Quaterniond *quat,
+                                                        const std::string &from_frame, const std::string &to_frame) const
+  {
+    try
+    {
+      tf::StampedTransform transform;
+      tf_listener_.lookupTransform(to_frame, from_frame, ros::Time(0), transform);
+      tf::Vector3 v_pos(pos->operator()(0), pos->operator()(1), pos->operator()(2));
+      tf::Vector3 v_pos_rot = tf::quatRotate(transform.getRotation(), v_pos);
+
+      *pos << v_pos_rot[0]+transform.getOrigin().getX(), 
+              v_pos_rot[1]+transform.getOrigin().getY(), 
+              v_pos_rot[2]+transform.getOrigin().getZ();
+
+      Eigen::Quaterniond eig_transform_quat(transform.getRotation().getW(), transform.getRotation().getX(), transform.getRotation().getY(), transform.getRotation().getZ());
+      *quat = quat->inverse() * eig_transform_quat;
+      quat->normalize();
+
+      return true;
+    }
+    catch (const tf::TransformException &ex)
+    {
+      ROS_ERROR_THROTTLE(1, "%s", ex.what());
+      return false;
+    }
+  }
+
   void CartesianImpedanceControllerRos::publishMsgsAndTf()
   {
     // publish commanded torques
@@ -520,7 +568,7 @@ namespace cartesian_impedance_controller
     if (config.apply_wrench)
     {
       F << config.f_x, config.f_y, config.f_z, config.tau_x, config.tau_y, config.tau_z;
-      if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_))
+      if (this->wrench_ee_frame_.compare( this->root_frame_) != 0 && !transformWrench(&F, this->wrench_ee_frame_, this->root_frame_))
       {
         ROS_ERROR("Could not transform wrench. Not applying it.");
         return;