Add EnableObject

include/mujincontrollerclient/binpickingtask.h

@@ -556,14 +556,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 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 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);
 
     /// \brief calls planning GetRobotBridgeIOVariableString and returns the contents of the signal in a string with correct endianness
     virtual void GetRobotBridgeIOVariableString(const std::vector<std::string>& ionames, std::vector<std::string>& iovalues, const double timeout=10);

src/binpickingtask.cpp

@@ -1786,6 +1786,29 @@ void BinPickingTaskResource::Release(const std::string& targetname, const std::s
     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<std::string>& ionames, std::vector<std::string>& iovalues, const double timeout)
 {
     SetMapTaskParameters(_ss, _mapTaskParameters);