Update Interaction Task

CMakeLists.txt

@@ -94,6 +94,9 @@ add_service_files(
     SetControlMode.srv
     SetSafetyLimits.srv
     SetTransform.srv
+    SetImpedanceRefLink.srv
+    GetForceLimits.srv
+    SetForceLimits.srv
     )
 
 

bindings/python/pyRosImpl.cpp

@@ -111,7 +111,9 @@ PYBIND11_MODULE(pyci, m) {
             .def("setForceLimits", &InteractionTask::setForceLimits)
             .def("setStiffnessTransition", &InteractionTask::setStiffnessTransition)
             .def("abortStiffnessTransition", &InteractionTask::abortStiffnessTransition)
-            .def("getStiffnessState", &InteractionTask::getStiffnessState);
+            .def("getStiffnessState", &InteractionTask::getStiffnessState)
+            .def("setImpedanceRefLink", &InteractionTask::setImpedanceRefLink)
+            .def("getImpedanceRefLink", &InteractionTask::getImpedanceRefLink);
 
     py::class_<ClientApi::InteractionRos,
             InteractionTask,

bindings/python/pyRosImpl.h

@@ -120,9 +120,9 @@ auto py_task_get_acc_lims(const CartesianTask& t)
 
 auto py_task_get_force_lims(const InteractionTask& t)
 {
-    Eigen::Vector6d l, a;
-    t.getForceLimits(l, a);
-    return std::make_tuple(l, a);
+    Eigen::Vector6d l;
+    t.getForceLimits(l);
+    return l;
 }
 
 auto py_get_pose_reference(const RosClient& r, const std::string& ee)

include/cartesian_interface/problem/Interaction.h

@@ -35,20 +35,22 @@ class InteractionTask : public virtual CartesianTask
     virtual const Impedance & getImpedance () = 0;
 
     virtual const Eigen::Vector6d& getForceReference () const = 0;
-    virtual void getForceLimits (Eigen::Vector6d& fmin,
-                                 Eigen::Vector6d& fmax) const = 0;
+    virtual void getForceLimits (Eigen::Vector6d& fmax) const = 0;
 
     virtual bool setImpedance (const Impedance & impedance) = 0;
 
 
 	virtual void setForceReference (const Eigen::Vector6d& f) = 0;
-    virtual bool setForceLimits (const Eigen::Vector6d& fmin,
-                                 const Eigen::Vector6d& fmax)= 0;
+    virtual bool setForceLimits (const Eigen::Vector6d& fmax)= 0;
 
 	virtual void  abortStiffnessTransition () = 0;
 	virtual bool  setStiffnessTransition (const Interpolator<Eigen::Matrix6d>::WayPointVector & way_points) = 0;
 	virtual State getStiffnessState () const = 0;
 
+    virtual const std::string& getImpedanceRefLink() const = 0;
+    virtual bool setImpedanceRefLink(const std::string& new_impedance_ref_link) = 0;
+
+
 };
 
 class AdmittanceTask : public virtual InteractionTask

include/cartesian_interface/sdk/problem/Interaction.h

@@ -36,33 +36,36 @@ class InteractionTaskImpl : public virtual InteractionTask,
     const Impedance & getImpedance () override;
 
     const Eigen::Vector6d& getForceReference() const override;
-    void getForceLimits(Eigen::Vector6d& fmin,
-                        Eigen::Vector6d& fmax) const override;
+    void getForceLimits(Eigen::Vector6d& fmax) const override;
 
     bool setImpedance (const Impedance & impedance) override;
 
 	void setForceReference(const Eigen::Vector6d& f) override;
-    bool setForceLimits(const Eigen::Vector6d& fmin,
-                        const Eigen::Vector6d& fmax) override;
+    bool setForceLimits(const Eigen::Vector6d& fmax) override;
 
 	/* these methods are used to change the stiffness in a smooth way... */
 
 	void  abortStiffnessTransition();
 	bool  setStiffnessTransition(const Interpolator<Eigen::Matrix6d>::WayPointVector & way_points);
 	State getStiffnessState() const;
 
+    const std::string& getImpedanceRefLink() const;
+    bool setImpedanceRefLink(const std::string& new_impedance_ref_link);
+
 private:
 
     static constexpr double REF_TTL = 0.3;
 
 	Impedance       _impedance;
 
-    Eigen::Vector6d _fref, _fmin, _fmax;
+    Eigen::Vector6d _fref, _fmax;
     double          _ref_timeout;
 
 	State                              _state;
 	Interpolator<Eigen::Matrix6d>::Ptr _interpolator;
 
+    std::string _impedance_ref_link;
+
 };
 
 class AdmittanceTaskImpl : public virtual AdmittanceTask,

include/cartesian_interface/sdk/ros/client_api/InteractionRos.h

@@ -11,6 +11,11 @@
 #include <cartesian_interface/SetImpedance.h>
 #include <cartesian_interface/InteractionTaskInfo.h>
 
+#include <cartesian_interface/SetImpedanceRefLink.h>
+
+#include <cartesian_interface/GetForceLimits.h>
+#include <cartesian_interface/SetForceLimits.h>
+
 
 namespace XBot { namespace Cartesian {
 
@@ -33,18 +38,22 @@ class ClientApi::InteractionRos : virtual public InteractionTask,
     const Impedance & getImpedance ();
 
 	const Eigen::Vector6d& getForceReference () const override;
-    void getForceLimits (Eigen::Vector6d& fmin, Eigen::Vector6d& fmax) const override;
+    void getForceLimits (Eigen::Vector6d& fmax) const override;
 
     bool setImpedance (const Impedance & impedance) override;
 
 	void setForceReference (const Eigen::Vector6d& f) override;
-    bool setForceLimits (const Eigen::Vector6d& fmin, const Eigen::Vector6d& fmax) override;
+    bool setForceLimits (const Eigen::Vector6d& fmax) override;
 
 	bool  waitTransitionCompleted (double timeout);
 	void  abortStiffnessTransition () override;
 	bool  setStiffnessTransition (const Interpolator<Eigen::Matrix6d>::WayPointVector & way_points) override;
 	State getStiffnessState () const override;
 
+    const std::string& getImpedanceRefLink() const;
+    bool setImpedanceRefLink(const std::string& new_impedance_ref_link);
+
+
 private:
 
     typedef cartesian_interface::ReachCartesianImpedanceAction ActionType;
@@ -55,6 +64,13 @@ class ClientApi::InteractionRos : virtual public InteractionTask,
 	mutable ros::ServiceClient _get_impedance_cli;
     mutable ros::ServiceClient _set_impedance_cli;
     mutable ros::ServiceClient _interaction_info_cli;
+    ros::ServiceClient _set_impedance_ref_link_cli;
+    mutable ros::ServiceClient _get_force_limits_cli;
+    mutable ros::ServiceClient _set_force_limits_cli;
+
+    ros::Subscriber _task_info_sub;
+
+    mutable std::string _impedance_ref_link;
 
 	cartesian_interface::InteractionTaskInfo _info;
 
@@ -66,6 +82,8 @@ class ClientApi::InteractionRos : virtual public InteractionTask,
 
     void on_action_done(const actionlib::SimpleClientGoalState& state,
                         const cartesian_interface::ReachCartesianImpedanceResultConstPtr& result);
+
+    void on_task_info_recv(cartesian_interface::InteractionTaskInfoConstPtr msg);
 
 	Impedance _impedance;
 	Eigen::Vector6d _f;

include/cartesian_interface/sdk/ros/server_api/InteractionRos.h

@@ -10,6 +10,11 @@
 #include <cartesian_interface/GetInteractionTaskInfo.h>
 #include <cartesian_interface/GetImpedance.h>
 #include <cartesian_interface/SetImpedance.h>
+#include <cartesian_interface/SetImpedanceRefLink.h>
+#include <cartesian_interface/GetForceLimits.h>
+#include <cartesian_interface/SetForceLimits.h>
+
+#include <cartesian_interface/InteractionTaskInfo.h>
 
 namespace XBot { namespace Cartesian {
 
@@ -79,16 +84,28 @@ class ServerApi::InteractionRos : public ServerApi::CartesianRos
 
     bool set_impedance_cb(cartesian_interface::SetImpedanceRequest& req,
                           cartesian_interface::SetImpedanceResponse& res);
-
+
+    bool get_force_limits_cb(cartesian_interface::GetForceLimitsRequest&  req,
+                             cartesian_interface::GetForceLimitsResponse& res);
+
+    bool set_force_limits_cb(cartesian_interface::SetForceLimitsRequest&  req,
+                             cartesian_interface::SetForceLimitsResponse& res);
+
     ros::Subscriber              _fref_sub;
-    ros::Publisher               _fref_pub, _impd_pub;
+    ros::Publisher               _fref_pub, _impd_pub, _task_info_pub;
 
     ros::ServiceServer _get_info_srv, _get_impedance_srv, _set_impedance_srv;
+    ros::ServiceServer _set_impedance_ref_link_srv, _get_force_limits_srv, _set_force_limits_srv;
 
     InteractionTask::Ptr         _ci_inter;
 
 	std::unique_ptr<RCIAManager> _action  ;
 
+    void publish_task_info();
+
+    bool set_impedance_ref_link_cb(cartesian_interface::SetImpedanceRefLinkRequest& req,
+                          cartesian_interface::SetImpedanceRefLinkResponse& res);
+
 };
 
 } }

include/cartesian_interface/sdk/rt/CInteractionRt.h

@@ -29,19 +29,20 @@ class InteractionRt : public virtual InteractionTask,
     const Impedance& getImpedance() override;
 
     const Eigen::Vector6d& getForceReference() const override;
-    void getForceLimits(Eigen::Vector6d& fmin,
-						Eigen::Vector6d& fmax) const override;
+    void getForceLimits(Eigen::Vector6d& fmax) const override;
 
     bool setImpedance(const Impedance & impedance) override;
 
     void setForceReference(const Eigen::Vector6d& f) override;
-    bool setForceLimits(const Eigen::Vector6d& fmin,
-						const Eigen::Vector6d& fmax) override;
+    bool setForceLimits(const Eigen::Vector6d& fmax) override;
 
 	void  abortStiffnessTransition() override;
 	bool  setStiffnessTransition(const Interpolator<Eigen::Matrix6d>::WayPointVector & way_points) override;
 	State getStiffnessState() const override;
 
+    const std::string& getImpedanceRefLink() const override;
+    bool setImpedanceRefLink(const std::string& new_impedance_ref_link) override;
+
 private:
 
     typedef std::function<void(InteractionTask&)> CallbackType;
@@ -53,9 +54,10 @@ class InteractionRt : public virtual InteractionTask,
 
 		Eigen::Vector6d _force    ;
 		Eigen::Vector6d _force_max;
-		Eigen::Vector6d _force_min;
 
 		State _stiffness_state;
+
+        std::string _impedance_ref_link;
 	};
 
     LockFreeQueue<DataToClient, 1024> _to_cli_queue;

include/cartesian_interface/utils/Manipulability.h

@@ -5,7 +5,6 @@
 #include <cartesian_interface/problem/Cartesian.h>
 #include <cartesian_interface/utils/LoadConfig.h>
 
-#include <XBotInterface/MatLogger.hpp>
 #include <ros/ros.h>
 
 #include <functional>

msg/CartesianImpedance.msg

@@ -1,2 +1,3 @@
+std_msgs/Header header
 cartesian_interface/CartesianImpedanceBase linear
 cartesian_interface/CartesianImpedanceBase angular

msg/InteractionTaskInfo.msg

@@ -1,2 +1,3 @@
 string state
+string impedance_ref_link
 

src/CartesianInterfaceImpl.cpp

@@ -136,7 +136,8 @@ void XBot::Cartesian::CartesianInterfaceImpl::add_task(TaskDescription::Ptr task
 
         Logger::success(Logger::Severity::HIGH) <<  "Successfully added Cartesian Interaction task with\n" <<
                                                     "   BASE LINK:   " << XBot::bold_on << cart_desc->getBaseLink() << XBot::bold_off  << "\n" << XBot::color_yellow <<
-                                                    "   DISTAL LINK: " << XBot::bold_on << cart_desc->getDistalLink() << XBot::bold_off << Logger::endl();
+                                                    "   DISTAL LINK: " << XBot::bold_on << cart_desc->getDistalLink() << XBot::bold_off << "\n" << XBot::color_yellow <<
+                                                    "   IMPEDANCE REF LINK: " << XBot::bold_on << cart_desc->getImpedanceRefLink() << XBot::bold_off << Logger::endl();
     }
 
     if(auto cart_desc = std::dynamic_pointer_cast<CartesianTask>(task_desc))

src/problem/impl/Cartesian.cpp

@@ -445,6 +445,19 @@ void CartesianTaskImpl::update(double time, double period)
     }
     else
     {
+        // in velocity mode, we integrate the pose reference
+        if(_ctrl_mode == ControlType::Velocity)
+        {
+            _T.translation() += _vel.head<3>() * period;
+
+            if(!_vel.tail<3>().isZero())
+            {
+                Eigen::AngleAxisd delta_R(_vel.tail<3>().norm() * period, _vel.tail<3>().normalized());
+                _T.linear() = delta_R * _T.linear();
+            }
+
+        }
+
         if(_vref_time_to_live < 0.0)
         {
             _vel.setZero();