Added ocra-icub-clients to the repo

ocra-icub-clients/CMakeLists.txt

@@ -0,0 +1,25 @@
+# This file is part of ocra-icub.
+# Copyright (C) 2016 Institut des Systèmes Intelligents et de Robotique (ISIR)
+# author(s): Ryan Lober, Antoine Hoarau
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+cmake_minimum_required(VERSION 2.8.11)
+project(ocra-icub-clients CXX)
+
+
+add_subdirectory(example-client)
+add_subdirectory(stepping-demo)
+add_subdirectory(task-operations-demo)
+# add_subdirectory(your-client)

ocra-icub-clients/README.md

@@ -0,0 +1,26 @@
+# ocra-controller-clients
+
+A place to store all of our delicious ocra based controller clients.
+
+### example-client
+
+A simple client to show how to use task connections and trajectories to move the icub's hand around
+
+**Authors:**
+ [Ryan Lober](https://github.com/rlober)
+
+
+### stepping-demo
+
+Icub stepping from side to side - not walking.
+
+**Authors:**
+ [Ryan Lober](https://github.com/rlober)
+
+
+### task-operations-demo
+
+Shows some of the basic functionality of the `TaskConnection` interface.
+
+**Authors:**
+ [Ryan Lober](https://github.com/rlober)

ocra-icub-clients/example-client/CMakeLists.txt

@@ -0,0 +1,43 @@
+# This file is part of ocra-icub.
+# Copyright (C) 2016 Institut des Systèmes Intelligents et de Robotique (ISIR)
+# author(s): Ryan Lober, Antoine Hoarau
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+SET(PROJECTNAME example-client)
+PROJECT(${PROJECTNAME} CXX)
+
+FILE(GLOB folder_source ./src/*.cpp)
+FILE(GLOB folder_header ./include/${PROJECTNAME}/*.h)
+
+SOURCE_GROUP("Source Files" FILES ${folder_source})
+SOURCE_GROUP("Header Files" FILES ${folder_header})
+
+INCLUDE_DIRECTORIES(
+${PROJECT_SOURCE_DIR}/include
+${YARP_INCLUDE_DIRS}
+${OcraIcub_INCLUDE_DIRS}
+${OcraRecipes_INCLUDE_DIRS}
+)
+
+ADD_EXECUTABLE(${PROJECTNAME} ${folder_source} ${folder_header})
+
+LIST(APPEND link_libs   ${YARP_LIBRARIES}
+                        ${OcraRecipes_LIBRARIES}
+                        ocra-icub
+                        )
+
+TARGET_LINK_LIBRARIES(${PROJECTNAME} ${link_libs})
+
+INSTALL(TARGETS ${PROJECTNAME} DESTINATION bin)

ocra-icub-clients/example-client/include/example-client/ExampleClient.h

@@ -0,0 +1,36 @@
+#ifndef EXAMPLE_CLIENT_H
+#define EXAMPLE_CLIENT_H
+
+#include <ocra-icub/IcubClient.h>
+#include <ocra-recipes/TrajectoryThread.h>
+#include <ocra-recipes/ControllerClient.h>
+// #include <ocra/control/Model.h>
+
+class ExampleClient : public ocra_recipes::ControllerClient
+{
+DEFINE_CLASS_POINTER_TYPEDEFS(ExampleClient)
+
+public:
+    ExampleClient (std::shared_ptr<ocra::Model> modelPtr, const int loopPeriod);
+    virtual ~ExampleClient ();
+
+protected:
+    virtual bool initialize();
+    virtual void release();
+    virtual void loop();
+
+private:
+
+    double startTime;
+    double waitTime;
+    bool trigger;
+    bool done;
+    Eigen::MatrixXd waypoints;
+
+    std::shared_ptr<ocra_recipes::TrajectoryThread> leftHandTrajThread;
+
+    bool p1, p2, p3;
+};
+
+
+#endif // EXAMPLE_CLIENT_H

ocra-icub-clients/example-client/src/ExampleClient.cpp

@@ -0,0 +1,89 @@
+#include "example-client/ExampleClient.h"
+
+ExampleClient::ExampleClient(std::shared_ptr<ocra::Model> modelPtr, const int loopPeriod)
+: ocra_recipes::ControllerClient(modelPtr, loopPeriod)
+{
+    // poopoo
+}
+
+ExampleClient::~ExampleClient()
+{
+    //caca
+}
+
+bool ExampleClient::initialize()
+{
+    startTime = yarp::os::Time::now();
+    trigger = true;
+
+    ocra_recipes::TRAJECTORY_TYPE trajType = ocra_recipes::MIN_JERK;
+
+    waypoints.resize(3,3);
+    waypoints <<     0.20, 0.25, 0.15,
+                    -0.06, 0.10, 0.30,
+                     0.55, 0.85, 0.65;
+
+    ocra_recipes::TERMINATION_STRATEGY termStrategy = ocra_recipes::BACK_AND_FORTH;
+
+    leftHandTrajThread = std::make_shared<ocra_recipes::TrajectoryThread>(10, "leftHandCartesian", waypoints, trajType, termStrategy);
+
+    // leftHandTrajThread->setDisplacement(0.2);
+    leftHandTrajThread->setGoalErrorThreshold(0.03);
+    leftHandTrajThread->setMaxVelocity(0.2);
+
+
+    done=false;
+
+    p1 = true;
+    p2 = true;
+    p3 = true;
+
+    waitTime = 1.0;
+
+    std::cout << "Begin loop." << std::endl;
+
+    return true;
+}
+
+void ExampleClient::release()
+{
+    /* Do nothing. */
+}
+
+void ExampleClient::loop()
+{
+
+    while(!done && ((yarp::os::Time::now() - startTime) > waitTime))
+    {
+        if(trigger){
+            leftHandTrajThread->start();
+            std::cout << "Traj thread started." << std::endl;
+            trigger = false;
+        }
+
+        if ((yarp::os::Time::now()-startTime)>15.0){
+            if(p1){
+                p1=false;
+                std::cout << "Changing to CYCLE mode." << std::endl;
+                leftHandTrajThread->setTerminationStrategy(ocra_recipes::CYCLE);
+            }
+            if ((yarp::os::Time::now()-startTime)>30.0){
+                if(p2){
+                    p2=false;
+                    std::cout << "Pausing trajectory." << std::endl;
+                    leftHandTrajThread->pause();
+                }
+                if ((yarp::os::Time::now()-startTime)>40.0){
+                    if(p3){
+                        p3=false;
+                        std::cout << "Un-pausing trajectory." << std::endl;
+                        leftHandTrajThread->unpause();
+                        done=true;
+                    }
+                }
+            }
+        }
+    }
+
+
+}

ocra-icub-clients/example-client/src/example-client.cpp

@@ -0,0 +1,91 @@
+/*! \file       example-client.cpp
+ *  \brief
+ *  \details
+ *  \author     [Ryan Lober](http://www.ryanlober.com)
+ *  \author     [Antoine Hoarau](http://ahoarau.github.io)
+ *  \date       Feb 2016
+ *  \copyright  GNU General Public License.
+ */
+/*
+ *  This file is part of ocra-icub.
+ *  Copyright (C) 2016 Institut des Systèmes Intelligents et de Robotique (ISIR)
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <yarp/os/ResourceFinder.h>
+#include <yarp/os/Network.h>
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/Time.h>
+
+#include "example-client/ExampleClient.h"
+#include <ocra-icub/IcubClient.h>
+#include <ocra-recipes/ControllerClient.h>
+#include <ocra-recipes/ClientManager.h>
+
+
+int main (int argc, char * argv[])
+{
+    yarp::os::Log yLog;
+    yarp::os::Network yarp;
+
+    double network_timeout = 10.0;
+    if (!yarp.checkNetwork(network_timeout))
+    {
+        yLog.fatal() << "YARP network is not available";
+        return -1;
+    }
+
+    std::cout << "Making model initializer" << std::endl;
+    ocra_icub::ModelInitializer modelIni = ocra_icub::ModelInitializer();
+
+    int loopPeriod = 10;
+
+    std::shared_ptr<ocra_recipes::ControllerClient> ctrlClient;
+    std::cout << "Making controller client" << std::endl;
+
+    if(modelIni.getModel())
+    {
+        std::cout << "Model is not empty." << std::endl;
+    }else
+        std::cout << "Model IS empty!" << std::endl;
+
+    ctrlClient = std::make_shared<ExampleClient>(modelIni.getModel(), loopPeriod);
+
+    std::shared_ptr<ocra_recipes::ClientManager> clientManager;
+    std::cout << "Making client manager" << std::endl;
+    clientManager = std::make_shared<ocra_recipes::ClientManager>(ctrlClient);
+
+    std::cout << "Resource finder stuff" << std::endl;
+    yarp::os::ResourceFinder rf;
+    rf.setVerbose(true);
+    rf.setDefaultConfigFile("example-client.ini"); //default config file name.
+    rf.setDefaultContext("example-client"); //when no parameters are given to the module this is the default context
+    rf.configure(argc,argv);
+
+    if (rf.check("help"))
+    {
+        clientManager->printHelp();
+        return 0;
+    }
+
+    std::cout << "Configuring" << std::endl;
+    clientManager->configure(rf);
+
+    std::cout << "Launching client" << std::endl;
+    return clientManager->launchClient();
+
+    // return clientManager->runModule(rf);
+}

ocra-icub-clients/stepping-demo/CMakeLists.txt

@@ -0,0 +1,43 @@
+# This file is part of ocra-icub.
+# Copyright (C) 2016 Institut des Systèmes Intelligents et de Robotique (ISIR)
+# author(s): Ryan Lober, Antoine Hoarau
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+SET(PROJECTNAME stepping-demo)
+PROJECT(${PROJECTNAME} CXX)
+
+FILE(GLOB folder_source ./src/*.cpp)
+FILE(GLOB folder_header ./include/${PROJECTNAME}/*.h)
+
+SOURCE_GROUP("Source Files" FILES ${folder_source})
+SOURCE_GROUP("Header Files" FILES ${folder_header})
+
+INCLUDE_DIRECTORIES(
+${PROJECT_SOURCE_DIR}/include
+${YARP_INCLUDE_DIRS}
+${OcraRecipes_INCLUDE_DIRS}
+${OcraIcub_INCLUDE_DIRS}
+)
+
+ADD_EXECUTABLE(${PROJECTNAME} ${folder_source} ${folder_header})
+
+LIST(APPEND link_libs   ${YARP_LIBRARIES}
+                        ${OcraRecipes_LIBRARIES}
+                        ocra-icub
+                        )
+
+TARGET_LINK_LIBRARIES(${PROJECTNAME} ${link_libs})
+
+INSTALL(TARGETS ${PROJECTNAME} DESTINATION bin)