From aa2886e2ddaade10298beff6212ad0c3b5b31942 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 27 Sep 2024 12:54:46 +0200 Subject: [PATCH 1/5] updated to new API name --- src/opensot/task_adapters/OpenSotCartesian.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/opensot/task_adapters/OpenSotCartesian.cpp b/src/opensot/task_adapters/OpenSotCartesian.cpp index b51a593c..6676812a 100644 --- a/src/opensot/task_adapters/OpenSotCartesian.cpp +++ b/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(shared_from_this()); From 7375d6a0c97f1998782bf54f0b9e8199b6a79589 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 27 Sep 2024 14:12:00 +0200 Subject: [PATCH 2/5] Implemented local velocity CartesI/O API --- include/cartesian_interface/problem/Cartesian.h | 5 +++++ include/cartesian_interface/sdk/problem/Cartesian.h | 3 +++ .../sdk/ros/client_api/CartesianRos.h | 1 + include/cartesian_interface/sdk/rt/CartesianRt.h | 3 ++- src/opensot/task_adapters/OpenSotCartesian.cpp | 8 +++++++- src/problem/impl/Cartesian.cpp | 13 ++++++++++++- src/ros/client_api/CartesianRos.cpp | 6 ++++++ src/ros/server_api/CartesianRos.cpp | 2 ++ src/rt/CartesianRt.cpp | 7 +++++++ srv/GetCartesianTaskInfo.srv | 1 + 10 files changed, 46 insertions(+), 3 deletions(-) diff --git a/include/cartesian_interface/problem/Cartesian.h b/include/cartesian_interface/problem/Cartesian.h index 4a053142..b503a3b5 100644 --- a/include/cartesian_interface/problem/Cartesian.h +++ b/include/cartesian_interface/problem/Cartesian.h @@ -136,6 +136,11 @@ 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 setAccelerationReference sets a new desired acceleration for the task. * @return diff --git a/include/cartesian_interface/sdk/problem/Cartesian.h b/include/cartesian_interface/sdk/problem/Cartesian.h index 1837699d..da3a59b5 100644 --- a/include/cartesian_interface/sdk/problem/Cartesian.h +++ b/include/cartesian_interface/sdk/problem/Cartesian.h @@ -46,6 +46,7 @@ class CartesianTaskImpl : /* Parameters */ bool isSubtaskLocal() const override; + bool isVelocityLocal() const override; /* Safety limits */ virtual void getVelocityLimits(double& max_vel_lin, @@ -131,6 +132,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 diff --git a/include/cartesian_interface/sdk/ros/client_api/CartesianRos.h b/include/cartesian_interface/sdk/ros/client_api/CartesianRos.h index 36d6ef36..a8b8adae 100644 --- a/include/cartesian_interface/sdk/ros/client_api/CartesianRos.h +++ b/include/cartesian_interface/sdk/ros/client_api/CartesianRos.h @@ -34,6 +34,7 @@ class ClientApi::CartesianRos : virtual public CartesianTask, bool validate() override; void enableOnlineTrajectoryGeneration() override; bool isSubtaskLocal() const override; + bool isVelocityLocal() const 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; diff --git a/include/cartesian_interface/sdk/rt/CartesianRt.h b/include/cartesian_interface/sdk/rt/CartesianRt.h index d3d21f86..78ab89c4 100644 --- a/include/cartesian_interface/sdk/rt/CartesianRt.h +++ b/include/cartesian_interface/sdk/rt/CartesianRt.h @@ -26,6 +26,7 @@ class CartesianRt : public virtual CartesianTask, void enableOnlineTrajectoryGeneration() override; bool isSubtaskLocal() const override; + bool isVelocityLocal() const 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 +59,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; diff --git a/src/opensot/task_adapters/OpenSotCartesian.cpp b/src/opensot/task_adapters/OpenSotCartesian.cpp index 6676812a..5568c1f1 100644 --- a/src/opensot/task_adapters/OpenSotCartesian.cpp +++ b/src/opensot/task_adapters/OpenSotCartesian.cpp @@ -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() diff --git a/src/problem/impl/Cartesian.cpp b/src/problem/impl/Cartesian.cpp index 259a99af..54f3ffeb 100644 --- a/src/problem/impl/Cartesian.cpp +++ b/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() == "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()) + { + _is_velocity_local = true; + } + _otg_maxvel.setConstant(1.0); _otg_maxacc.setConstant(10.0); _trajectory = std::make_shared(); @@ -343,6 +349,11 @@ bool CartesianTaskImpl::setVelocityReference(const Eigen::Vector6d & base_vel_re return true; } +bool CartesianTaskImpl::isVelocityLocal() const +{ + return _is_velocity_local; +} + bool CartesianTaskImpl::setAccelerationReference(const Eigen::Vector6d &base_acc_ref) { if(getActivationState() == ActivationState::Disabled) diff --git a/src/ros/client_api/CartesianRos.cpp b/src/ros/client_api/CartesianRos.cpp index 6f950099..92dd1fa5 100644 --- a/src/ros/client_api/CartesianRos.cpp +++ b/src/ros/client_api/CartesianRos.cpp @@ -80,6 +80,11 @@ bool CartesianRos::isSubtaskLocal() const return get_task_info().use_local_subtasks; } +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 +321,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 ; diff --git a/src/ros/server_api/CartesianRos.cpp b/src/ros/server_api/CartesianRos.cpp index 45b3c7fa..8ce7ea3a 100644 --- a/src/ros/server_api/CartesianRos.cpp +++ b/src/ros/server_api/CartesianRos.cpp @@ -150,6 +150,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); @@ -193,6 +194,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); diff --git a/src/rt/CartesianRt.cpp b/src/rt/CartesianRt.cpp index 6f5888a1..dd9a6ac8 100644 --- a/src/rt/CartesianRt.cpp +++ b/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,11 @@ bool CartesianRt::isSubtaskLocal() const return _cli_data._is_body_jacobian; } +bool CartesianRt::isVelocityLocal() const +{ + return _cli_data._is_velocity_local; +} + void CartesianRt::getVelocityLimits(double & max_vel_lin, double & max_vel_ang) const { max_vel_lin = _cli_data._max_vel.first; diff --git a/srv/GetCartesianTaskInfo.srv b/srv/GetCartesianTaskInfo.srv index 13ae01c4..59778dba 100644 --- a/srv/GetCartesianTaskInfo.srv +++ b/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 From 22c472512bfe9b0ae98cd8bd4ec69dac99d76439 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 27 Sep 2024 16:04:56 +0200 Subject: [PATCH 3/5] added pythong binsgined and services --- bindings/python/pyRosImpl.cpp | 2 ++ .../cartesian_interface/problem/Cartesian.h | 6 ++++ .../sdk/problem/Cartesian.h | 1 + .../sdk/ros/client_api/CartesianRos.h | 2 ++ .../sdk/ros/server_api/CartesianRos.h | 7 ++++- .../cartesian_interface/sdk/rt/CartesianRt.h | 1 + src/problem/impl/Cartesian.cpp | 6 ++++ src/ros/client_api/CartesianRos.cpp | 16 +++++++++++ src/ros/server_api/CartesianRos.cpp | 28 ++++++++++++++----- src/rt/CartesianRt.cpp | 5 ++++ 10 files changed, 66 insertions(+), 8 deletions(-) diff --git a/bindings/python/pyRosImpl.cpp b/bindings/python/pyRosImpl.cpp index 35ca1fac..b660e76e 100644 --- a/bindings/python/pyRosImpl.cpp +++ b/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_ #include +#include + 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; diff --git a/include/cartesian_interface/sdk/rt/CartesianRt.h b/include/cartesian_interface/sdk/rt/CartesianRt.h index 78ab89c4..f0d5388d 100644 --- a/include/cartesian_interface/sdk/rt/CartesianRt.h +++ b/include/cartesian_interface/sdk/rt/CartesianRt.h @@ -27,6 +27,7 @@ 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; diff --git a/src/problem/impl/Cartesian.cpp b/src/problem/impl/Cartesian.cpp index 54f3ffeb..cfc0648f 100644 --- a/src/problem/impl/Cartesian.cpp +++ b/src/problem/impl/Cartesian.cpp @@ -354,6 +354,12 @@ 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) diff --git a/src/ros/client_api/CartesianRos.cpp b/src/ros/client_api/CartesianRos.cpp index 92dd1fa5..403e51c6 100644 --- a/src/ros/client_api/CartesianRos.cpp +++ b/src/ros/client_api/CartesianRos.cpp @@ -7,6 +7,7 @@ #include #include #include +#include using namespace XBot::Cartesian; using namespace XBot::Cartesian::ClientApi; @@ -59,6 +60,8 @@ CartesianRos::CartesianRos(std::string name, _set_safety_lims_cli = _nh.serviceClient(name + "/set_safety_limits"); + _set_is_velocity_local_cli = _nh.serviceClient(name + "/use_local_velocity_reference"); + _task_info_sub = _nh.subscribe(name + "/cartesian_task_properties", 10, &CartesianRos::on_task_info_recv, this); } @@ -80,6 +83,19 @@ 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; diff --git a/src/ros/server_api/CartesianRos.cpp b/src/ros/server_api/CartesianRos.cpp index 8ce7ea3a..29e13ac1 100644 --- a/src/ros/server_api/CartesianRos.cpp +++ b/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) @@ -171,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>(); diff --git a/src/rt/CartesianRt.cpp b/src/rt/CartesianRt.cpp index dd9a6ac8..b9845ad7 100644 --- a/src/rt/CartesianRt.cpp +++ b/src/rt/CartesianRt.cpp @@ -69,6 +69,11 @@ 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; From eaf2441d667049b345c5f93eb6e4bdb69bd918c0 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Tue, 1 Oct 2024 13:18:28 +0200 Subject: [PATCH 4/5] missing file from previous commit --- msg/CartesianTaskInfo.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/CartesianTaskInfo.msg b/msg/CartesianTaskInfo.msg index 44272da0..4ced7cb0 100644 --- a/msg/CartesianTaskInfo.msg +++ b/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 From 08e6f01fb6af9b366fc5cc52e67567aaa7aa321b Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Wed, 28 May 2025 16:52:03 +0200 Subject: [PATCH 5/5] added active joint mask in constraints --- src/opensot/task_adapters/OpenSotTask.cpp | 24 +++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/opensot/task_adapters/OpenSotTask.cpp b/src/opensot/task_adapters/OpenSotTask.cpp index add35103..2b5e734a 100644 --- a/src/opensot/task_adapters/OpenSotTask.cpp +++ b/src/opensot/task_adapters/OpenSotTask.cpp @@ -247,6 +247,30 @@ bool OpenSotConstraintAdapter::initialize(const OpenSoT::OptvarHelper& vars) _ci_constr->getName())); } + // active joint mask + if(_ci_constr->getDisabledJoints().size() > 0) + { + std::vector active_joints_mask(_model->getNv(), true); + + for(auto jstr : _ci_constr->getDisabledJoints()) + { + try{ + auto jinfo = _model->getJointInfo(jstr); + std::fill_n(active_joints_mask.begin() + jinfo.iv, + jinfo.nv, + false); + } + catch(...) + { + auto id = _model->getVIndexFromVName(jstr); + active_joints_mask[id] = false; + } + + } + + _opensot_constr->setActiveJointsMask(active_joints_mask); + } + // indices if(_ci_constr->getIndices().size() != _ci_constr->getSize()) {