From 54f3fafad08a56964d90c43b9c25ad45583e730d Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Thu, 8 Feb 2024 20:21:41 +0900 Subject: [PATCH 1/4] Add EnableObject API --- .../mujincontrollerclient/binpickingtask.h | 13 +++++++ src/binpickingtask.cpp | 37 +++++++++++++++---- 2 files changed, 43 insertions(+), 7 deletions(-) diff --git a/include/mujincontrollerclient/binpickingtask.h b/include/mujincontrollerclient/binpickingtask.h index 4eba5142..fc2fea62 100644 --- a/include/mujincontrollerclient/binpickingtask.h +++ b/include/mujincontrollerclient/binpickingtask.h @@ -564,6 +564,19 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource /// \param timeout timeout of communication virtual void Release(const std::string& targetname, const std::string& robotname = "", const std::string& toolname = "", const double timeout = 1); + /// \brief enables object + /// \param objectName name of the target to enable + /// \param state whether to enable + /// \param timeout timeout of communication + virtual void EnableObject(const std::string& objectName, bool state, const double timeout = 1); + + /// \brief enables object link + /// \param objectName name of the target to enable + /// \param linkName link name of the target to enable + /// \param state whether to enable + /// \param timeout timeout of communication + virtual void EnableLink(const std::string& objectName, const std::string& linkName, bool state, const double timeout = 1); + /// \brief calls planning GetRobotBridgeIOVariableString and returns the contents of the signal in a string with correct endianness virtual void GetRobotBridgeIOVariableString(const std::vector& ionames, std::vector& iovalues, const double timeout=10); diff --git a/src/binpickingtask.cpp b/src/binpickingtask.cpp index 7a55c127..85ac64e8 100644 --- a/src/binpickingtask.cpp +++ b/src/binpickingtask.cpp @@ -1757,12 +1757,12 @@ void BinPickingTaskResource::Grab(const std::string& targetname, const std::stri { SetMapTaskParameters(_ss, _mapTaskParameters); _ss << GetJsonString("command", "Grab") << ", "; - _ss << GetJsonString("targetname", targetname) << ", "; + _ss << ", " << GetJsonString("targetname", targetname); if (!robotname.empty()) { - _ss << GetJsonString("robotname", robotname) << ", "; + _ss << ", " << GetJsonString("robotname", robotname); } if (!toolname.empty()) { - _ss << GetJsonString("toolname", toolname) << ", "; + _ss << ", " << GetJsonString("toolname", toolname); } _ss << "}"; rapidjson::Document d; @@ -1772,19 +1772,42 @@ void BinPickingTaskResource::Grab(const std::string& targetname, const std::stri void BinPickingTaskResource::Release(const std::string& targetname, const std::string& robotname, const std::string& toolname, const double timeout) { SetMapTaskParameters(_ss, _mapTaskParameters); - _ss << GetJsonString("command", "Release") << ", "; - _ss << GetJsonString("targetname", targetname) << ", "; + _ss << GetJsonString("command", "Release"); + _ss << ", " << GetJsonString("targetname", targetname); if (!robotname.empty()) { - _ss << GetJsonString("robotname", robotname) << ", "; + _ss << ", " << GetJsonString("robotname", robotname); } if (!toolname.empty()) { - _ss << GetJsonString("toolname", toolname) << ", "; + _ss << ", " << GetJsonString("toolname", toolname); } _ss << "}"; rapidjson::Document d; ExecuteCommand(_ss.str(), d, timeout); } +void BinPickingTaskResource::EnableObject(const std::string& objectName, bool state, const double timeout) +{ + SetMapTaskParameters(_ss, _mapTaskParameters); + _ss << GetJsonString("command", "EnableObject"); + _ss << ", " << GetJsonString("objectName", objectName); + _ss << ", " << GetJsonString("state", state); + _ss << "}"; + rapidjson::Document d; + ExecuteCommand(_ss.str(), d, timeout); +} + +void BinPickingTaskResource::EnableLink(const std::string& objectName, const std::string& linkName, bool state, const double timeout) +{ + SetMapTaskParameters(_ss, _mapTaskParameters); + _ss << GetJsonString("command", "EnableLink"); + _ss << ", " << GetJsonString("objectName", objectName); + _ss << ", " << GetJsonString("linkName", linkName); + _ss << ", " << GetJsonString("state", state); + _ss << "}"; + rapidjson::Document d; + ExecuteCommand(_ss.str(), d, timeout); +} + void BinPickingTaskResource::GetRobotBridgeIOVariableString(const std::vector& ionames, std::vector& iovalues, const double timeout) { SetMapTaskParameters(_ss, _mapTaskParameters); From 62dfb43fdb2ffd088f2c210767ee6cd03bf34a68 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Thu, 8 Feb 2024 20:27:57 +0900 Subject: [PATCH 2/4] incr timeout --- include/mujincontrollerclient/binpickingtask.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/mujincontrollerclient/binpickingtask.h b/include/mujincontrollerclient/binpickingtask.h index fc2fea62..d1491ef8 100644 --- a/include/mujincontrollerclient/binpickingtask.h +++ b/include/mujincontrollerclient/binpickingtask.h @@ -555,27 +555,27 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource /// \param robotname name of the robot to grab with /// \param toolname name of the tool to grab with /// \param timeout timeout of communication - virtual void Grab(const std::string& targetname, const std::string& robotname = "", const std::string& toolname = "", const double timeout = 1); + virtual void Grab(const std::string& targetname, const std::string& robotname = "", const std::string& toolname = "", const double timeout = 5); /// \brief releases object /// \param targetname name of the target to release /// \param robotname name of the robot to release from /// \param toolname name of the tool to release from /// \param timeout timeout of communication - virtual void Release(const std::string& targetname, const std::string& robotname = "", const std::string& toolname = "", const double timeout = 1); + virtual void Release(const std::string& targetname, const std::string& robotname = "", const std::string& toolname = "", const double timeout = 5); /// \brief enables object /// \param objectName name of the target to enable /// \param state whether to enable /// \param timeout timeout of communication - virtual void EnableObject(const std::string& objectName, bool state, const double timeout = 1); + virtual void EnableObject(const std::string& objectName, bool state, const double timeout = 5); /// \brief enables object link /// \param objectName name of the target to enable /// \param linkName link name of the target to enable /// \param state whether to enable /// \param timeout timeout of communication - virtual void EnableLink(const std::string& objectName, const std::string& linkName, bool state, const double timeout = 1); + virtual void EnableLink(const std::string& objectName, const std::string& linkName, bool state, const double timeout = 5); /// \brief calls planning GetRobotBridgeIOVariableString and returns the contents of the signal in a string with correct endianness virtual void GetRobotBridgeIOVariableString(const std::vector& ionames, std::vector& iovalues, const double timeout=10); From c5624d81ae70e140f900ca1e60b8e4f2c190031f Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Thu, 8 Feb 2024 20:30:25 +0900 Subject: [PATCH 3/4] timeout --- include/mujincontrollerclient/binpickingtask.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/include/mujincontrollerclient/binpickingtask.h b/include/mujincontrollerclient/binpickingtask.h index d1491ef8..b22cf9fb 100644 --- a/include/mujincontrollerclient/binpickingtask.h +++ b/include/mujincontrollerclient/binpickingtask.h @@ -564,16 +564,16 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource /// \param timeout timeout of communication virtual void Release(const std::string& targetname, const std::string& robotname = "", const std::string& toolname = "", const double timeout = 5); - /// \brief enables object - /// \param objectName name of the target to enable - /// \param state whether to enable + /// \brief manipulates enabled state of object + /// \param objectName name of the object to modify enabled state of. + /// \param state whether to enable (true) or disable (false) link /// \param timeout timeout of communication virtual void EnableObject(const std::string& objectName, bool state, const double timeout = 5); - /// \brief enables object link - /// \param objectName name of the target to enable - /// \param linkName link name of the target to enable - /// \param state whether to enable + /// \brief manipulates enabled state of link + /// \param objectName name of the object link belongs to + /// \param linkName name of the link to modify enabled state of. + /// \param state whether to enable (true) or disable (false) link. /// \param timeout timeout of communication virtual void EnableLink(const std::string& objectName, const std::string& linkName, bool state, const double timeout = 5); From b8449f4fee8defefd0549f272609e162c44cba0d Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Tue, 12 Mar 2024 10:09:22 +0900 Subject: [PATCH 4/4] fix typo --- src/binpickingtask.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/binpickingtask.cpp b/src/binpickingtask.cpp index 85ac64e8..94e0a8de 100644 --- a/src/binpickingtask.cpp +++ b/src/binpickingtask.cpp @@ -1756,7 +1756,7 @@ void BinPickingTaskResource::ExecuteSingleXMLTrajectory(const std::string& traje void BinPickingTaskResource::Grab(const std::string& targetname, const std::string& robotname, const std::string& toolname, const double timeout) { SetMapTaskParameters(_ss, _mapTaskParameters); - _ss << GetJsonString("command", "Grab") << ", "; + _ss << GetJsonString("command", "Grab"); _ss << ", " << GetJsonString("targetname", targetname); if (!robotname.empty()) { _ss << ", " << GetJsonString("robotname", robotname);