Local api fix

bindings/python/pyRosImpl.cpp

@@ -91,6 +91,8 @@ PYBIND11_MODULE(pyci, m) {
             .def("setVelocityLimits", &CartesianTask::setVelocityLimits)
             .def("setAccelerationLimits", &CartesianTask::setAccelerationLimits)
             .def("getPoseReference", py_task_get_pose_reference)
+            .def("setIsVelocityLocal", &CartesianTask::setIsVelocityLocal)
+            .def("isVelocityLocal", &CartesianTask::isVelocityLocal)
             .def("abort", &CartesianTask::abort);
 
     py::class_<ComTask,

include/cartesian_interface/problem/Cartesian.h

@@ -136,6 +136,17 @@ class CartesianTask : public virtual TaskDescription
      */
     virtual bool setVelocityReference(const Eigen::Vector6d& base_vel_ref) = 0;
 
+    /**
+     * @brief isVelocityLocal returns true if velocities are expressed in local (distal_link) frame
+     */
+    virtual bool isVelocityLocal() const = 0;
+
+    /**
+     * @brief setIsVelocityLocal sets the velocity reference to be local
+     * @param is_velocity_local if true, the velocity reference is local
+     */
+    virtual void setIsVelocityLocal(const bool is_velocity_local) = 0;
+
     /**
      * @brief setAccelerationReference sets a new desired acceleration for the task.
      * @return

include/cartesian_interface/sdk/problem/Cartesian.h

@@ -46,6 +46,8 @@ class CartesianTaskImpl :
 
     /* Parameters */
     bool isSubtaskLocal() const override;
+    bool isVelocityLocal() const override;
+    void setIsVelocityLocal(const bool is_velocity_local) override;
 
     /* Safety limits */
     virtual void getVelocityLimits(double& max_vel_lin,
@@ -131,6 +133,8 @@ class CartesianTaskImpl :
          */
     bool _is_body_jacobian;
 
+    bool _is_velocity_local;
+
     /**
          * @brief Parameter that weights orientation errors w.r.t. position errors.
          * For example, setting it to orientation_gain = 2.0 means that an error of

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

@@ -34,6 +34,8 @@ class ClientApi::CartesianRos : virtual public CartesianTask,
     bool validate() override;
     void enableOnlineTrajectoryGeneration() override;
     bool isSubtaskLocal() const override;
+    bool isVelocityLocal() const override;
+    void setIsVelocityLocal(const bool is_velocity_local) override;
     void getVelocityLimits(double & max_vel_lin, double & max_vel_ang) const override;
     void getAccelerationLimits(double & max_acc_lin, double & max_acc_ang) const override;
     void setVelocityLimits(double max_vel_lin, double max_vel_ang) override;
@@ -84,6 +86,7 @@ class ClientApi::CartesianRos : virtual public CartesianTask,
     ros::ServiceClient _set_safety_lims_cli;
     ros::ServiceClient _set_base_link_cli;
     ros::ServiceClient _set_ctrl_mode_cli;
+    ros::ServiceClient _set_is_velocity_local_cli;
     mutable ros::ServiceClient _cart_info_cli;
 
     bool _Tref_recv, _vref_recv;

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

@@ -16,6 +16,8 @@
 #include <cartesian_interface/SetSafetyLimits.h>
 #include <cartesian_interface/CartesianTaskInfo.h>
 
+#include <std_srvs/SetBool.h>
+
 
 namespace XBot { namespace Cartesian {
 
@@ -94,11 +96,14 @@ class ServerApi::CartesianRos : public ServerApi::TaskRos,
     bool set_safety_lims_cb(cartesian_interface::SetSafetyLimitsRequest& req,
                              cartesian_interface::SetSafetyLimitsResponse& res);
 
+    bool use_local_velocity_reference_cb(std_srvs::SetBoolRequest& req,
+                                         std_srvs::SetBoolResponse& res);
+
 
 
     ros::Publisher _pose_ref_pub, _vel_ref_pub, _acc_ref_pub, _task_info_pub;
     ros::Subscriber _pose_ref_sub, _vel_ref_sub;
-    ros::ServiceServer _get_info_srv, _set_base_link_srv, _set_ctrl_srv, _set_safety_srv;
+    ros::ServiceServer _get_info_srv, _set_base_link_srv, _set_ctrl_srv, _set_safety_srv, _use_local_velocity_reference_srv;
 
     CartesianTask::Ptr _cart;
 

include/cartesian_interface/sdk/rt/CartesianRt.h

@@ -26,6 +26,8 @@ class CartesianRt : public virtual CartesianTask,
 
     void enableOnlineTrajectoryGeneration() override;
     bool isSubtaskLocal() const override;
+    bool isVelocityLocal() const override;
+    void setIsVelocityLocal(const bool is_velocity_local) override;
     void getVelocityLimits(double & max_vel_lin, double & max_vel_ang) const override;
     void getAccelerationLimits(double & max_acc_lin, double & max_acc_ang) const override;
     void setVelocityLimits(double max_vel_lin, double max_vel_ang) override;
@@ -58,7 +60,7 @@ class CartesianRt : public virtual CartesianTask,
     {
         ControlType _ctrl_mode;
         std::string _base_link, _distal_link;
-        bool _is_body_jacobian;
+        bool _is_body_jacobian, _is_velocity_local;
         double _orientation_gain;
         Eigen::Affine3d _T;
         Eigen::Affine3d _Tcurr;

msg/CartesianTaskInfo.msg

@@ -3,6 +3,7 @@ string distal_link
 string control_mode
 string state
 bool use_local_subtasks
+bool use_local_velocity
 float64 max_vel_lin
 float64 max_vel_ang
 float64 max_acc_lin

src/opensot/task_adapters/OpenSotCartesian.cpp

@@ -35,7 +35,7 @@ bool OpenSotCartesianAdapter::initialize(const OpenSoT::OptvarHelper& vars)
 
 
     /* Cartesian task specific parameters */
-    _opensot_cart->setIsBodyJacobian(_ci_cart->isSubtaskLocal());
+    _opensot_cart->rotateToLocal(_ci_cart->isSubtaskLocal());
 
     /* Register observer */
     auto this_shared_ptr = std::dynamic_pointer_cast<OpenSotCartesianAdapter>(shared_from_this());
@@ -61,7 +61,13 @@ void OpenSotCartesianAdapter::update(double time, double period)
     Eigen::Affine3d Tref;
     Eigen::Vector6d vref;
     _ci_cart->getPoseReference(Tref, &vref);
-    _opensot_cart->setReference(Tref, vref*_ctx->params()->getControlPeriod());
+    if(_ci_cart->isVelocityLocal())
+    {
+        _opensot_cart->setReference(Tref);
+        _opensot_cart->setVelocityLocalReference(vref*_ctx->params()->getControlPeriod());
+    }
+    else
+        _opensot_cart->setReference(Tref, vref*_ctx->params()->getControlPeriod());
 }
 
 bool OpenSotCartesianAdapter::onBaseLinkChanged()

src/problem/impl/Cartesian.cpp

@@ -86,7 +86,8 @@ CartesianTaskImpl::CartesianTaskImpl(YAML::Node task_node, Context::ConstPtr con
     _state(State::Online),
     _vref_time_to_live(-1.0),
     _orientation_gain(1.0),
-    _is_body_jacobian(false)
+    _is_body_jacobian(false),
+    _is_velocity_local(false)
 {
     bool is_com = task_node["type"].as<std::string>() == "Com";
 
@@ -108,6 +109,11 @@ CartesianTaskImpl::CartesianTaskImpl(YAML::Node task_node, Context::ConstPtr con
         _is_body_jacobian = true;
     }
 
+    if(task_node["use_local_velocity"] && task_node["use_local_velocity"].as<bool>())
+    {
+        _is_velocity_local = true;
+    }
+
     _otg_maxvel.setConstant(1.0);
     _otg_maxacc.setConstant(10.0);
     _trajectory = std::make_shared<Trajectory>();
@@ -343,6 +349,17 @@ bool CartesianTaskImpl::setVelocityReference(const Eigen::Vector6d & base_vel_re
     return true;
 }
 
+bool CartesianTaskImpl::isVelocityLocal() const
+{
+    return _is_velocity_local;
+}
+
+void CartesianTaskImpl::setIsVelocityLocal(const bool is_velocity_local)
+{
+    _is_velocity_local = is_velocity_local;
+}
+
+
 bool CartesianTaskImpl::setAccelerationReference(const Eigen::Vector6d &base_acc_ref)
 {
     if(getActivationState() == ActivationState::Disabled)

src/ros/client_api/CartesianRos.cpp

@@ -7,6 +7,7 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <eigen_conversions/eigen_msg.h>
+#include <std_srvs/SetBool.h>
 
 using namespace XBot::Cartesian;
 using namespace XBot::Cartesian::ClientApi;
@@ -59,6 +60,8 @@ CartesianRos::CartesianRos(std::string name,
 
     _set_safety_lims_cli = _nh.serviceClient<SetSafetyLimits>(name + "/set_safety_limits");
 
+    _set_is_velocity_local_cli = _nh.serviceClient<std_srvs::SetBool>(name + "/use_local_velocity_reference");
+
     _task_info_sub = _nh.subscribe(name + "/cartesian_task_properties", 10,
                                  &CartesianRos::on_task_info_recv, this);
 }
@@ -80,6 +83,24 @@ bool CartesianRos::isSubtaskLocal() const
     return get_task_info().use_local_subtasks;
 }
 
+void CartesianRos::setIsVelocityLocal(const bool is_velocity_local)
+{
+    std_srvs::SetBool srv;
+    srv.request.data = is_velocity_local;
+    if(!_set_is_velocity_local_cli.call(srv))
+    {
+        throw std::runtime_error(fmt::format("Unable to call service '{}'",
+                                             _set_is_velocity_local_cli.getService()));
+    }
+
+    ROS_INFO("%s", srv.response.message.c_str());
+}
+
+bool CartesianRos::isVelocityLocal() const
+{
+    return get_task_info().use_local_velocity;
+}
+
 void CartesianRos::getVelocityLimits(double & max_vel_lin,
                                      double & max_vel_ang) const
 {
@@ -316,6 +337,7 @@ GetCartesianTaskInfoResponse CartesianRos::get_task_info() const
         res.control_mode      = _info.control_mode     ;
         res.state             = _info.state            ;
         res.use_local_subtasks = _info.use_local_subtasks;
+        res.use_local_velocity = _info.use_local_velocity;
         res.max_vel_lin       = _info.max_vel_lin      ;
         res.max_vel_ang       = _info.max_vel_ang      ;
         res.max_acc_lin       = _info.max_acc_lin      ;

src/ros/server_api/CartesianRos.cpp

@@ -87,6 +87,17 @@ CartesianRos::CartesianRos(CartesianTask::Ptr cart_task,
     _get_info_srv = _ctx->nh().advertiseService(_task->getName() + "/get_cartesian_task_properties",
                                                 &CartesianRos::get_task_info_cb, this);
 
+    _use_local_velocity_reference_srv = _ctx->nh().advertiseService(_task->getName() + "/use_local_velocity_reference",
+                                                                     &CartesianRos::use_local_velocity_reference_cb, this);
+
+}
+
+bool CartesianRos::use_local_velocity_reference_cb(std_srvs::SetBoolRequest& req,
+                                     std_srvs::SetBoolResponse& res)
+{
+    _cart->setIsVelocityLocal(req.data);
+    res.success = true;
+    return true;
 }
 
 void CartesianRos::run(ros::Time time)
@@ -150,6 +161,7 @@ void CartesianRos::publish_task_info()
     msg.max_vel_lin = srv.response.max_vel_lin;
     msg.state = srv.response.state;
     msg.use_local_subtasks = srv.response.use_local_subtasks;
+    msg.use_local_velocity = srv.response.use_local_velocity;
 
     _task_info_pub.publish(msg);
 
@@ -170,14 +182,17 @@ void CartesianRos::online_velocity_reference_cb(geometry_msgs::TwistStampedConst
     Eigen::Matrix3d b_R_f;
     b_R_f.setIdentity();
 
-    if(msg->header.frame_id == "world")
+    if(!_cart->isVelocityLocal())
     {
-        b_R_f = _model->getPose(_cart->getBaseLink()).linear().transpose();
-    }
-    else if(msg->header.frame_id != "")
-    {
-        // throws if frame_id does not exist
-        b_R_f = _model->getPose(msg->header.frame_id, _cart->getBaseLink()).linear();
+        if(msg->header.frame_id == "world")
+        {
+            b_R_f = _model->getPose(_cart->getBaseLink()).linear().transpose();
+        }
+        else if(msg->header.frame_id != "")
+        {
+            // throws if frame_id does not exist
+            b_R_f = _model->getPose(msg->header.frame_id, _cart->getBaseLink()).linear();
+        }
     }
 
     vel.head<3>() = b_R_f * vel.head<3>();
@@ -193,6 +208,7 @@ bool CartesianRos::get_task_info_cb(cartesian_interface::GetCartesianTaskInfoReq
     res.distal_link = _cart->getDistalLink();
     res.control_mode = EnumToString(_cart->getControlMode());
     res.use_local_subtasks = _cart->isSubtaskLocal();
+    res.use_local_velocity = _cart->isVelocityLocal();
 
     _cart->getVelocityLimits(res.max_vel_lin,
                              res.max_vel_ang);

src/rt/CartesianRt.cpp

@@ -50,6 +50,8 @@ void CartesianRt::sendState(bool send)
 
     _rt_data._is_body_jacobian = _task_impl->isSubtaskLocal();
 
+    _rt_data._is_velocity_local = _task_impl->isVelocityLocal();
+
     _to_cli_queue.push(_rt_data);
 }
 
@@ -62,6 +64,16 @@ bool CartesianRt::isSubtaskLocal() const
     return _cli_data._is_body_jacobian;
 }
 
+bool CartesianRt::isVelocityLocal() const
+{
+    return _cli_data._is_velocity_local;
+}
+
+void CartesianRt::setIsVelocityLocal(const bool is_velocity_local)
+{
+    _cli_data._is_velocity_local = is_velocity_local;
+}
+
 void CartesianRt::getVelocityLimits(double & max_vel_lin, double & max_vel_ang) const
 {
     max_vel_lin = _cli_data._max_vel.first;

srv/GetCartesianTaskInfo.srv

@@ -4,6 +4,7 @@ string  distal_link
 string  control_mode
 string  state
 bool    use_local_subtasks
+bool    use_local_velocity
 float64 max_vel_lin
 float64 max_vel_ang
 float64 max_acc_lin