├── .gitignore ├── src ├── rpcIdl │ ├── IRpcServer_index.txt │ ├── include │ │ ├── RpcServerImpl.h │ │ └── IRpcServer.h │ ├── src │ │ ├── main.cpp │ │ └── RpcServerImpl.cpp │ ├── CMakeLists.txt │ ├── IRpcServer.thrift │ └── rpcIdl.xml ├── iKin │ ├── genericChainController │ │ ├── config.ini │ │ └── CMakeLists.txt │ ├── CMakeLists.txt │ ├── iCubLimbsFwKin │ │ ├── CMakeLists.txt │ │ └── main.cpp │ ├── onlineSolver │ │ ├── CMakeLists.txt │ │ └── main.cpp │ └── fwInvKinematics │ │ ├── CMakeLists.txt │ │ └── main.cpp ├── optimization │ ├── CMakeLists.txt │ └── calibReference │ │ ├── CMakeLists.txt │ │ └── main.cpp ├── ctrlLib │ ├── CMakeLists.txt │ └── onlinePTuner │ │ └── CMakeLists.txt ├── anyRobotCartesianInterface │ ├── app │ │ ├── scripts │ │ │ ├── client.xml │ │ │ └── robot_server_solver.xml │ │ ├── conf │ │ │ ├── solver.ini │ │ │ ├── kinematics.ini │ │ │ └── server.ini │ │ └── CMakeLists.txt │ ├── src │ │ ├── CMakeLists.txt │ │ ├── client │ │ │ ├── CMakeLists.txt │ │ │ └── main.cpp │ │ ├── server │ │ │ ├── CMakeLists.txt │ │ │ └── main.cpp │ │ ├── fakeRobot │ │ │ ├── CMakeLists.txt │ │ │ └── main.cpp │ │ ├── solver │ │ │ ├── CMakeLists.txt │ │ │ └── main.cpp │ │ └── fakeMotorDevice │ │ │ ├── CMakeLists.txt │ │ │ ├── include │ │ │ ├── fakeMotorDevice.h │ │ │ └── private │ │ │ │ └── fakeMotorDeviceComponents.h │ │ │ └── src │ │ │ ├── fakeMotorDevice.cpp │ │ │ ├── fakeMotorDeviceClient.cpp │ │ │ └── fakeMotorDeviceServer.cpp │ └── CMakeLists.txt ├── iDyn │ ├── CMakeLists.txt │ ├── oneChainDynamics │ │ ├── CMakeLists.txt │ │ └── main.cpp │ ├── oneChainWithSensor │ │ ├── CMakeLists.txt │ │ └── main.cpp │ └── multiLimbJacobian │ │ └── CMakeLists.txt ├── motorControlBasic │ ├── CMakeLists.txt │ └── tutorial_arm.cpp ├── periodicThread │ ├── CMakeLists.txt │ └── tutorial_periodic_thread.cpp ├── motorControlImpedance │ ├── CMakeLists.txt │ └── tutorial_arm_joint_impedance.cpp ├── perceptiveModels │ ├── CMakeLists.txt │ └── tutorial_perceptiveModels.cpp ├── CTestConfig.cmake ├── imageProcessing │ ├── CMakeLists.txt │ ├── lookAtLocation.cpp │ └── findLocation.cpp ├── actionPrimitives │ ├── app │ │ └── conf │ │ │ ├── grasp_model.ini │ │ │ ├── config.ini │ │ │ └── hand_sequences.ini │ └── CMakeLists.txt ├── learningMachines │ ├── CMakeLists.txt │ ├── lmlibdirect.cpp │ ├── lmlibindirect.cpp │ └── lmlibportable.cpp ├── misc │ └── relay.cpp ├── smoke-tests │ └── ipopt.cpp └── CMakeLists.txt ├── doc ├── images │ ├── see_ball.jpg │ ├── fixate_ball.jpg │ ├── cartIFArchitecture.jpg │ └── cartIFPlantResponse.jpg ├── hello_icub.dox ├── python-motor-control.dox ├── tutorial_module.dox ├── doc.dox ├── motor_control_tutorial.dox ├── periodic_thread.dox ├── iDyn_oneChain.dox ├── tutorial_module_documentation.dox └── impedance_control_tutorial.dox ├── .github └── workflows │ └── trigger-documentation.yml ├── README.md └── python ├── python-motor-control.py ├── python_imaging.py └── python_simworld_control.py /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | /src/build*/ 3 | *kdev* 4 | *.orig 5 | -------------------------------------------------------------------------------- /src/rpcIdl/IRpcServer_index.txt: -------------------------------------------------------------------------------- 1 | include/IRpcServer.h 2 | src/IRpcServer.cpp 3 | -------------------------------------------------------------------------------- /doc/images/see_ball.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/icub-tutorials/HEAD/doc/images/see_ball.jpg -------------------------------------------------------------------------------- /doc/images/fixate_ball.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/icub-tutorials/HEAD/doc/images/fixate_ball.jpg -------------------------------------------------------------------------------- /doc/images/cartIFArchitecture.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/icub-tutorials/HEAD/doc/images/cartIFArchitecture.jpg -------------------------------------------------------------------------------- /doc/images/cartIFPlantResponse.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/icub-tutorials/HEAD/doc/images/cartIFPlantResponse.jpg -------------------------------------------------------------------------------- /src/iKin/genericChainController/config.ini: -------------------------------------------------------------------------------- 1 | // 2-links planar manipulator 2 | 3 | numLinks 2 4 | 5 | link_0 (A 1.0) (D 0.0) (alpha 0.0) (offset 0.0) (min -180.0) (max 180.0) 6 | link_1 (A 1.0) (D 0.0) (alpha 0.0) (offset 0.0) (min -180.0) (max 180.0) 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/optimization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2012 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(optimization_tutorials) 7 | add_subdirectory(calibReference) 8 | -------------------------------------------------------------------------------- /src/ctrlLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2012 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(ctrlLib_tutorials) 7 | add_subdirectory(onlinePTuner) 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/app/scripts/client.xml: -------------------------------------------------------------------------------- 1 | 2 | Client Launcher 3 | 4 | 5 | /server/state:o 6 | 7 | 8 | 9 | client 10 | node 11 | node 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/iDyn/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2012 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(iDyn_tutorials) 7 | add_subdirectory(multiLimbJacobian) 8 | add_subdirectory(oneChainDynamics) 9 | add_subdirectory(oneChainWithSensor) 10 | 11 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/app/conf/solver.ini: -------------------------------------------------------------------------------- 1 | 2 | robot fake_robot 3 | name solver 4 | period 30 5 | dof (1 1 1) 6 | rest_pos (0.0 0.0 0.0) 7 | rest_weights (0.0 0.0 0.0) 8 | pose full 9 | mode shot 10 | verbosity off 11 | maxIter 200 12 | tol 0.0001 13 | constr_tol 0.000001 14 | interPoints off 15 | ping_robot_tmo 10.0 16 | 17 | -------------------------------------------------------------------------------- /src/rpcIdl/include/RpcServerImpl.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | class RpcServerImpl : public IRpcServer { 5 | int answer; 6 | bool running; 7 | 8 | public: 9 | RpcServerImpl(); 10 | 11 | virtual int32_t get_answer(); 12 | virtual bool set_answer(const int32_t rightAnswer); 13 | virtual int32_t add_int(const int32_t x); 14 | virtual bool start(); 15 | virtual bool stop(); 16 | virtual bool is_running(); 17 | }; 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/iKin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2012 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(iKin_tutorials) 7 | add_subdirectory(iCubLimbsFwKin) 8 | add_subdirectory(fwInvKinematics) 9 | add_subdirectory(genericChainController) 10 | add_subdirectory(onlineSolver) 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/motorControlBasic/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2012 iCub Facility, Istituto Italiano di Tecnologia 2 | # Author: Lorenzo Natale 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | 6 | cmake_minimum_required(VERSION 3.5) 7 | project(motorControlBasic) 8 | 9 | find_package(YARP) 10 | 11 | add_executable(tutorial_arm tutorial_arm.cpp) 12 | target_link_libraries(tutorial_arm ${YARP_LIBRARIES}) 13 | install(TARGETS tutorial_arm DESTINATION bin) 14 | -------------------------------------------------------------------------------- /src/periodicThread/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2012 iCub Facility, Istituto Italiano di Tecnologia 2 | # Author: Lorenzo Natale 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | 6 | cmake_minimum_required(VERSION 3.5) 7 | project(periodicThread) 8 | 9 | find_package(YARP) 10 | 11 | add_executable(${PROJECT_NAME} tutorial_periodic_thread.cpp) 12 | target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}) 13 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 14 | 15 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | file(GLOB conf ${CMAKE_CURRENT_SOURCE_DIR}/conf/*.ini) 6 | file(GLOB scripts ${CMAKE_CURRENT_SOURCE_DIR}/scripts/*.xml) 7 | 8 | yarp_install(FILES ${conf} DESTINATION bin) 9 | yarp_install(FILES ${scripts} DESTINATION bin) 10 | 11 | -------------------------------------------------------------------------------- /src/iDyn/oneChainDynamics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2010-2011 RobotCub Consortium 2 | # Author: Serena Ivaldi 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | CMAKE_MINIMUM_REQUIRED(VERSION 3.5) 6 | 7 | PROJECT(oneChainDynamics) 8 | 9 | FIND_PACKAGE(YARP) 10 | FIND_PACKAGE(ICUB) 11 | 12 | 13 | SET(folder_source main.cpp) 14 | ADD_EXECUTABLE(${PROJECT_NAME} ${folder_source}) 15 | TARGET_LINK_LIBRARIES(${PROJECT_NAME} iDyn ${YARP_LIBRARIES}) 16 | INSTALL(TARGETS ${PROJECT_NAME} DESTINATION bin) 17 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/app/conf/kinematics.ini: -------------------------------------------------------------------------------- 1 | // 3-links planar manipulator 2 | // note that it is not important to set the joints bound coherently with the hardware, 3 | // since they will be aligned automatically at run-time through a call to getLimits() 4 | 5 | numLinks 3 6 | 7 | link_0 (A 1.0) (D 0.0) (alpha 0.0) (offset 0.0) (min -180.0) (max 180.0) 8 | link_1 (A 1.0) (D 0.0) (alpha 0.0) (offset 0.0) (min -180.0) (max 180.0) 9 | link_2 (A 1.0) (D 0.0) (alpha 0.0) (offset 0.0) (min -180.0) (max 180.0) 10 | 11 | 12 | -------------------------------------------------------------------------------- /src/iDyn/oneChainWithSensor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2010-2011 RobotCub Consortium 2 | # Author: Serena Ivaldi 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | CMAKE_MINIMUM_REQUIRED(VERSION 3.5) 6 | 7 | PROJECT(oneChainWithSensor) 8 | 9 | FIND_PACKAGE(YARP) 10 | FIND_PACKAGE(ICUB) 11 | 12 | SET(folder_source main.cpp) 13 | ADD_EXECUTABLE(${PROJECT_NAME} ${folder_source}) 14 | TARGET_LINK_LIBRARIES(${PROJECT_NAME} skinDynLib iDyn ${YARP_LIBRARIES}) 15 | INSTALL(TARGETS ${PROJECT_NAME} DESTINATION bin) -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | 7 | add_subdirectory(fakeMotorDevice) 8 | 9 | set(fakeMotorDevice_INCLUDE_DIRS ../fakeMotorDevice/include) 10 | add_subdirectory(fakeRobot) 11 | add_subdirectory(solver) 12 | add_subdirectory(server) 13 | add_subdirectory(client) 14 | 15 | -------------------------------------------------------------------------------- /src/motorControlImpedance/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2012 iCub Facility, Istituto Italiano di Tecnologia 2 | # Author: Lorenzo Natale 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | 6 | cmake_minimum_required(VERSION 3.5) 7 | project(tutorial_arm_joint_impedance) 8 | 9 | find_package(YARP) 10 | 11 | add_executable(tutorial_arm_joint_impedance tutorial_arm_joint_impedance.cpp) 12 | target_link_libraries(tutorial_arm_joint_impedance ${YARP_LIBRARIES}) 13 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 14 | -------------------------------------------------------------------------------- /src/iKin/iCubLimbsFwKin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT) 2 | # All Rights Reserved. 3 | # Authors: Ugo Pattacini 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(iCubLimbsFwKin) 7 | 8 | find_package(YARP) 9 | find_package(ICUB) 10 | 11 | add_executable(${PROJECT_NAME} main.cpp) 12 | target_compile_definitions(${PROJECT_NAME} PRIVATE _USE_MATH_DEFINES) 13 | target_link_libraries(${PROJECT_NAME} iKin ${YARP_LIBRARIES}) 14 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) -------------------------------------------------------------------------------- /src/perceptiveModels/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2015 iCub Facility - Istituto Italiano di Tecnologia 2 | # Author: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(tutorial_perceptiveModels) 7 | 8 | find_package(YARP) 9 | find_package(ICUB) 10 | 11 | set(sources tutorial_perceptiveModels.cpp) 12 | add_executable(${PROJECT_NAME} ${sources}) 13 | target_link_libraries(${PROJECT_NAME} perceptiveModels ${YARP_LIBRARIES}) 14 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 15 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(anyRobotCartesianInterface) 7 | 8 | find_package(YARP 3.5.1 REQUIRED) 9 | find_package(ICUB) 10 | 11 | set(CMAKE_INSTALL_PREFIX "${CMAKE_CURRENT_SOURCE_DIR}" CACHE PATH "Installation directory" FORCE) 12 | 13 | add_subdirectory(src) 14 | add_subdirectory(app) 15 | 16 | -------------------------------------------------------------------------------- /src/ctrlLib/onlinePTuner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2012 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(onlinePTuner) 7 | 8 | find_package(YARP) 9 | find_package(ICUB) 10 | 11 | set(folder_source main.cpp) 12 | add_executable(${PROJECT_NAME} ${folder_source}) 13 | target_link_libraries(${PROJECT_NAME} ctrlLib ${YARP_LIBRARIES}) 14 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 15 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/app/scripts/robot_server_solver.xml: -------------------------------------------------------------------------------- 1 | 2 | Services Launcher 3 | 4 | 5 | 6 | 7 | 8 | fakeRobot 9 | node 10 | 11 | 12 | 13 | server 14 | node 15 | 16 | 17 | 18 | solver 19 | node 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/iKin/onlineSolver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2010 RobotCub Consortium 2 | # Author: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | 6 | cmake_minimum_required(VERSION 3.5) 7 | project(onlineSolver) 8 | 9 | find_package(YARP) 10 | find_package(ICUB) 11 | 12 | if(NOT ICUB_USE_IPOPT) 13 | message(FATAL_ERROR "IPOPT is required") 14 | endif() 15 | 16 | set(folder_source main.cpp) 17 | add_executable(${PROJECT_NAME} ${folder_source}) 18 | target_link_libraries(${PROJECT_NAME} iKin ${YARP_LIBRARIES}) 19 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 20 | -------------------------------------------------------------------------------- /src/CTestConfig.cmake: -------------------------------------------------------------------------------- 1 | ## This file should be placed in the root directory of your project. 2 | ## Then modify the CMakeLists.txt file in the root directory of your 3 | ## project to incorporate the testing dashboard. 4 | ## # The following are required to uses Dart and the Cdash dashboard 5 | ## ENABLE_TESTING() 6 | ## INCLUDE(CTest) 7 | set(CTEST_PROJECT_NAME "iCub-smoke-tests") 8 | set(CTEST_NIGHTLY_START_TIME "00:00:00 CEST") 9 | 10 | set(CTEST_DROP_METHOD "http") 11 | set(CTEST_DROP_SITE "dashboard.icub.org") 12 | set(CTEST_DROP_LOCATION "/submit.php?project=iCub") 13 | set(CTEST_DROP_SITE_CDASH TRUE) 14 | -------------------------------------------------------------------------------- /src/iDyn/multiLimbJacobian/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2010-2011 RobotCub Consortium 2 | # Author: Serena Ivaldi 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | cmake_minimum_required(VERSION 3.5) 6 | 7 | project(multiLimbJacobian) 8 | 9 | find_package(YARP) 10 | find_package(ICUB) 11 | 12 | set(folder_source main.cpp) 13 | add_executable(${PROJECT_NAME} ${folder_source}) 14 | target_compile_definitions(${PROJECT_NAME} PRIVATE _USE_MATH_DEFINES) 15 | target_link_libraries(${PROJECT_NAME} ctrlLib iKin skinDynLib iDyn ${YARP_LIBRARIES}) 16 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/client/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(client) 7 | 8 | find_package(YARP) 9 | 10 | set(folder_source main.cpp) 11 | source_group("Source Files" FILES ${folder_source}) 12 | 13 | add_executable(${PROJECT_NAME} ${folder_source}) 14 | target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}) 15 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 16 | -------------------------------------------------------------------------------- /.github/workflows/trigger-documentation.yml: -------------------------------------------------------------------------------- 1 | name: Trigger Documentation 2 | 3 | on: 4 | push: 5 | branches: 6 | - 'master' 7 | 8 | jobs: 9 | docs: 10 | name: "Trigger" 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - name: Repository Dispatch 15 | uses: peter-evans/repository-dispatch@v3 16 | with: 17 | token: ${{ secrets.BOT_TRIGGER_ROBOTOLOGY_DOCUMENTATION }} 18 | repository: robotology/robotology-documentation 19 | event-type: build_documentation 20 | client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}' 21 | 22 | -------------------------------------------------------------------------------- /src/iKin/genericChainController/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2010 RobotCub Consortium 2 | # Author: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | 6 | cmake_minimum_required(VERSION 3.5) 7 | project(genericChainController) 8 | 9 | find_package(YARP) 10 | find_package(ICUB) 11 | 12 | if(NOT ICUB_USE_IPOPT) 13 | message(FATAL_ERROR "IPOPT is required") 14 | endif() 15 | 16 | set(folder_source main.cpp) 17 | add_executable(${PROJECT_NAME} ${folder_source}) 18 | target_link_libraries(${PROJECT_NAME} iKin ${YARP_LIBRARIES}) 19 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 20 | -------------------------------------------------------------------------------- /src/imageProcessing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2012 iCub Facility, Istituto Italiano di Tecnologia 2 | # Author: Lorenzo Natale 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | 6 | CMAKE_MINIMUM_REQUIRED(VERSION 3.5) 7 | project(imageProcessing) 8 | 9 | find_package(YARP) 10 | 11 | add_executable(findLocation findLocation.cpp) 12 | target_link_libraries(findLocation ${YARP_LIBRARIES}) 13 | install(TARGETS findLocation DESTINATION bin) 14 | 15 | add_executable(lookAtLocation lookAtLocation.cpp) 16 | target_link_libraries(lookAtLocation ${YARP_LIBRARIES}) 17 | install(TARGETS lookAtLocation DESTINATION bin) 18 | -------------------------------------------------------------------------------- /src/actionPrimitives/app/conf/grasp_model.ini: -------------------------------------------------------------------------------- 1 | 2 | name tactile 3 | type left 4 | compensation false 5 | verbosity 1 6 | 7 | [thumb] 8 | name thumb 9 | logic inverse 10 | calibrated true 11 | 12 | [index] 13 | name index 14 | logic inverse 15 | calibrated true 16 | 17 | [middle] 18 | name middle 19 | logic inverse 20 | calibrated true 21 | 22 | [ring] 23 | name ring 24 | logic inverse 25 | calibrated true 26 | 27 | [little] 28 | name little 29 | logic inverse 30 | calibrated true 31 | 32 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | iCub Tutorials 2 | ============== 3 | 4 | [![ZenHub](https://img.shields.io/badge/Shipping_faster_with-ZenHub-435198.svg)](https://zenhub.com) 5 | [![Community](https://img.shields.io/badge/Join-Robotology_Community-blue?style=plastic&logo=github)](https://github.com/robotology/community) 6 | ## 👨🏻‍💻 Maintainers 7 | This repository is maintained by: 8 | 9 | | | | 10 | |:---:|:---:| 11 | | [](https://github.com/niNicogenecogene) | [@Nicogene](https://github.com/Nicogene) | 12 | 13 | 14 | Documentation of tutorials is available [here](http://www.icub.org/software_documentation/icub_tutorials.html) 🌐 15 | -------------------------------------------------------------------------------- /src/iKin/fwInvKinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2010 RobotCub Consortium 2 | # Author: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | 6 | cmake_minimum_required(VERSION 3.5) 7 | project(fwInvKinematics) 8 | 9 | find_package(YARP) 10 | find_package(ICUB) 11 | 12 | if(NOT ICUB_USE_IPOPT) 13 | message(FATAL_ERROR "IPOPT is required") 14 | endif() 15 | 16 | set(folder_source main.cpp) 17 | add_executable(${PROJECT_NAME} ${folder_source}) 18 | target_compile_definitions(${PROJECT_NAME} PRIVATE _USE_MATH_DEFINES) 19 | target_link_libraries(${PROJECT_NAME} iKin ${YARP_LIBRARIES}) 20 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 21 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(server) 7 | 8 | find_package(YARP) 9 | 10 | set(folder_source main.cpp) 11 | source_group("Source Files" FILES ${folder_source}) 12 | 13 | include_directories(${fakeMotorDevice_INCLUDE_DIRS}) 14 | add_executable(${PROJECT_NAME} ${folder_source}) 15 | target_link_libraries(${PROJECT_NAME} fakeMotorDevice ${YARP_LIBRARIES}) 16 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 17 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/fakeRobot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(fakeRobot) 7 | 8 | find_package(YARP) 9 | 10 | set(folder_source main.cpp) 11 | source_group("Source Files" FILES ${folder_source}) 12 | 13 | include_directories(${fakeMotorDevice_INCLUDE_DIRS}) 14 | add_executable(${PROJECT_NAME} ${folder_source}) 15 | target_link_libraries(${PROJECT_NAME} fakeMotorDevice ${YARP_LIBRARIES}) 16 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 17 | 18 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/solver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(solver) 7 | 8 | find_package(YARP) 9 | find_package(ICUB) 10 | find_package(IPOPT REQUIRED) 11 | 12 | set(folder_source main.cpp) 13 | source_group("Source Files" FILES ${folder_source}) 14 | 15 | include_directories(${fakeMotorDevice_INCLUDE_DIRS}) 16 | add_executable(${PROJECT_NAME} ${folder_source}) 17 | target_link_libraries(${PROJECT_NAME} fakeMotorDevice iKin ${YARP_LIBRARIES}) 18 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 19 | -------------------------------------------------------------------------------- /src/optimization/calibReference/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2012 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(calibReference) 7 | 8 | find_package(YARP) 9 | find_package(ICUB) 10 | 11 | if(NOT ICUB_USE_IPOPT) 12 | message(FATAL_ERROR "IPOPT is required") 13 | endif() 14 | 15 | set(folder_source main.cpp) 16 | add_executable(${PROJECT_NAME} ${folder_source}) 17 | target_compile_definitions(${PROJECT_NAME} PRIVATE _USE_MATH_DEFINES) 18 | target_link_libraries(${PROJECT_NAME} optimization ${YARP_LIBRARIES}) 19 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 20 | -------------------------------------------------------------------------------- /src/actionPrimitives/app/conf/config.ini: -------------------------------------------------------------------------------- 1 | 2 | [general] 3 | robot icub 4 | thread_period 50 5 | default_exec_time 2.0 6 | reach_tol 0.01 7 | torso_pitch on 8 | torso_roll off 9 | torso_yaw on 10 | torso_pitch_max 30.0 11 | tracking_mode off 12 | verbosity on 13 | ext_force_thres 5.0 14 | 15 | [arm_dependent] 16 | grasp_orientation (-0.171542 0.124396 -0.977292 3.058211) 17 | grasp_displacement (0.0 0.0 0.05) 18 | systematic_error_displacement (-0.03 -0.00 -0.02) 19 | lifting_displacement (0.0 0.0 0.2) 20 | home_position (-0.30 -0.20 0.10) 21 | 22 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/app/conf/server.ini: -------------------------------------------------------------------------------- 1 | [GENERAL] 2 | ControllerName server 3 | // the robot is simulated @ 100Hz, hence let's give it some margin to respond 4 | // we'll lower a bit the controller's speed 5 | ControllerPeriod 20 6 | SolverNameToConnect solver 7 | KinematicPart custom 8 | CustomKinFile kinematics.ini 9 | NumberOfDrivers 1 10 | 11 | [DRIVER_0] 12 | Key fake_part 13 | JointsOrder direct 14 | 15 | [PLANT_MODEL] 16 | // P(s)=(Kp/s)*((1+Tz*s)/(1+2*Zeta*Tw*s+(Tw*s)^2))*exp(-Td*s) 17 | plant_compensator off 18 | smith_predictor off 19 | 20 | joint_0 ((Kp 1.0) (Tz 0.0) (Tw 0.0) (Zeta 0.0) (Td 0.0)) 21 | joint_1 ((Kp 1.0) (Tz 0.0) (Tw 0.0) (Zeta 0.0) (Td 0.0)) 22 | joint_2 ((Kp 1.0) (Tz 0.0) (Tw 0.0) (Zeta 0.0) (Td 0.0)) 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/actionPrimitives/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2015 iCub Facility - Istituto Italiano di Tecnologia 2 | # Author: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(tutorial_actionPrimitives) 7 | 8 | find_package(YARP) 9 | find_package(ICUB) 10 | 11 | set(sources tutorial_actionPrimitives.cpp) 12 | source_group("Source Files" FILES ${sources}) 13 | 14 | add_executable(${PROJECT_NAME} ${sources}) 15 | target_link_libraries(${PROJECT_NAME} perceptiveModels 16 | actionPrimitives 17 | ${YARP_LIBRARIES}) 18 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 19 | 20 | file(GLOB conf ${CMAKE_CURRENT_SOURCE_DIR}/app/conf/*.ini) 21 | yarp_install(FILES ${conf} DESTINATION actionPrimitives) 22 | -------------------------------------------------------------------------------- /src/rpcIdl/src/main.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | /** 4 | * 5 | * Copyright (C) 2013 Istituto Italiano di Tecnologia, iCub Facility 6 | * 7 | * Authors: Lorenzo Natale 8 | * 9 | * CopyPolicy: Released under the terms of the GNU GPL v2.0. 10 | * 11 | */ 12 | 13 | #include "RpcServerImpl.h" 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | int main(int argc, char *argv[]) { 20 | yarp::os::Network yarp; 21 | 22 | RpcServerImpl server; 23 | yarp::os::Port port; 24 | server.yarp().attachAsServer(port); 25 | if (!port.open("/server")) { return 1; } 26 | 27 | while (true) { 28 | printf("Server is %s\n",server.is_running()?"running":"stopped"); 29 | yarp::os::Time::delay(10); 30 | } 31 | port.close(); 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/fakeMotorDevice/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 2 | # Authors: Ugo Pattacini 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(fakeMotorDevice) 7 | 8 | find_package(YARP) 9 | find_package(ICUB) 10 | 11 | set(folder_header include/fakeMotorDevice.h include/private/fakeMotorDeviceComponents.h) 12 | set(folder_source src/fakeMotorDevice.cpp src/fakeMotorDeviceServer.cpp src/fakeMotorDeviceClient.cpp) 13 | 14 | source_group("Header Files" FILES ${folder_header}) 15 | source_group("Source Files" FILES ${folder_source}) 16 | 17 | include_directories(${PROJECT_SOURCE_DIR}/include) 18 | add_library(${PROJECT_NAME} ${folder_source} ${folder_header}) 19 | target_link_libraries(${PROJECT_NAME} ctrlLib ${YARP_LIBRARIES}) 20 | -------------------------------------------------------------------------------- /src/learningMachines/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2010 RobotCub Consortium 2 | # Authors: Arjan Gijsberts 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | SET(PROJECTNAME lmExample) 6 | SET(LMLIB_DIRECT lmlibdirect) 7 | SET(LMLIB_INDIRECT lmlibindirect) 8 | SET(LMLIB_PORTABLE lmlibportable) 9 | 10 | PROJECT(${PROJECTNAME}) 11 | 12 | FIND_PACKAGE(YARP) 13 | FIND_PACKAGE(ICUB) 14 | 15 | ADD_EXECUTABLE(${LMLIB_DIRECT} ${LMLIB_DIRECT}.cpp) 16 | ADD_EXECUTABLE(${LMLIB_INDIRECT} ${LMLIB_INDIRECT}.cpp) 17 | ADD_EXECUTABLE(${LMLIB_PORTABLE} ${LMLIB_PORTABLE}.cpp) 18 | 19 | TARGET_LINK_LIBRARIES(${LMLIB_DIRECT} learningMachine ${YARP_LIBRARIES}) 20 | TARGET_LINK_LIBRARIES(${LMLIB_INDIRECT} learningMachine ${YARP_LIBRARIES}) 21 | TARGET_LINK_LIBRARIES(${LMLIB_PORTABLE} learningMachine ${YARP_LIBRARIES}) 22 | 23 | INSTALL(TARGETS ${LMLIB_DIRECT} DESTINATION bin) 24 | INSTALL(TARGETS ${LMLIB_INDIRECT} DESTINATION bin) 25 | INSTALL(TARGETS ${LMLIB_PORTABLE} DESTINATION bin) 26 | 27 | -------------------------------------------------------------------------------- /src/rpcIdl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright: 2015 iCub Facility - Istituto Italiano di Tecnologia 2 | # Author: Lorenzo Natale 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | 5 | cmake_minimum_required(VERSION 3.5) 6 | project(rpcIdl) 7 | 8 | find_package(YARP) 9 | 10 | yarp_idl_to_dir(INPUT_FILES IRpcServer.thrift OUTPUT_DIR ${CMAKE_CURRENT_SOURCE_DIR}) 11 | 12 | file(GLOB header_files include/*.h) 13 | file(GLOB source_files src/*.cpp) 14 | file(GLOB idl_files *.thrift) 15 | set(doc_files ${PROJECT_NAME}.xml) 16 | 17 | source_group("Header Files" FILES ${header_files}) 18 | source_group("Source Files" FILES ${source_files}) 19 | source_group("IDL Files" FILES ${idl_files}) 20 | source_group("DOC Files" FILES ${doc_files}) 21 | 22 | include_directories(${PROJECT_SOURCE_DIR}/include) 23 | add_executable(${PROJECT_NAME} ${header_files} ${source_files} ${idl_files} ${doc_files}) 24 | target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}) 25 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 26 | -------------------------------------------------------------------------------- /src/rpcIdl/src/RpcServerImpl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | RpcServerImpl::RpcServerImpl() 7 | { 8 | answer=-1; 9 | running=true; 10 | } 11 | 12 | int32_t RpcServerImpl::get_answer() 13 | { 14 | std::cout<<"The answer is "< 17 | #include 18 | 19 | using namespace std; 20 | using namespace yarp::os; 21 | 22 | int main(int argc, char **argv) { 23 | Network yarp; 24 | BufferedPort inPort; 25 | inPort.open("/relay/in"); 26 | 27 | BufferedPort outPort; 28 | outPort.open("/relay/out"); 29 | 30 | while (true) { 31 | cout << "waiting for input" << endl; 32 | Bottle *input = inPort.read(); 33 | if (input!=NULL) 34 | { 35 | Bottle& output = outPort.prepare(); 36 | output=*input; 37 | 38 | cout << "writing " << output.toString() << endl; 39 | outPort.write(); 40 | } 41 | } 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /src/rpcIdl/IRpcServer.thrift: -------------------------------------------------------------------------------- 1 | # Copyright: (C) 2013 iCub Facility - Istituto Italiano di Tecnologia 2 | # Authors: Lorenzo Natale 3 | # CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | # 5 | # IRpcServer.thrift 6 | 7 | /** 8 | * IRpcServer 9 | * 10 | * Interface for an example module. 11 | */ 12 | 13 | service IRpcServer { 14 | 15 | /** 16 | * Get answer from server 17 | * @return the answer 18 | */ 19 | i32 get_answer(); 20 | 21 | /** 22 | * Set value for future answers. 23 | * @param rightAnswer new answer 24 | * @return true if connection was successful 25 | */ 26 | bool set_answer(1:i32 rightAnswer) 27 | 28 | /** 29 | * Add one integer to future answers. 30 | * @param x value to add 31 | * @return new value 32 | */ 33 | i32 add_int(1:i32 x); 34 | 35 | /** 36 | * Start service 37 | * @return true if service started correctly 38 | */ 39 | bool start(); 40 | 41 | /** 42 | * Stop service 43 | * @return true if service stopped correctly 44 | */ 45 | bool stop(); 46 | 47 | /** 48 | * Check is service is running 49 | * @return true/false if service is/is not running 50 | */ 51 | bool is_running(); 52 | } 53 | -------------------------------------------------------------------------------- /python/python-motor-control.py: -------------------------------------------------------------------------------- 1 | /** 2 | * @ingroup icub_tutorials 3 | * 4 | * \defgroup icub_python_motor_interfaces Using motor interfaces with Python 5 | * 6 | * Shows how to control the robot using motor interfaces within Python. 7 | * 8 | * \author Lorenzo Natale 9 | * 10 | * CopyPolicy: Released under the terms of GPL 2.0 or later 11 | */ 12 | 13 | import yarp; 14 | yarp.Network.init(); 15 | 16 | # prepare a property object 17 | props = yarp.Property() 18 | props.put("device","remote_controlboard") 19 | props.put("local","/client/right_arm") 20 | props.put("remote","/icubSim/right_arm") 21 | 22 | # create remote driver 23 | armDriver = yarp.PolyDriver(props) 24 | 25 | #query motor control interfaces 26 | iPos = armDriver.viewIPositionControl() 27 | iVel = armDriver.viewIVelocityControl() 28 | iEnc = armDriver.viewIEncoders() 29 | 30 | #retrieve number of joints 31 | jnts=iPos.getAxes() 32 | 33 | print 'Controlling', jnts, 'joints' 34 | 35 | # read encoders 36 | encs=yarp.Vector(jnts) 37 | iEnc.getEncoders(encs.data()) 38 | 39 | ## store as home position 40 | home=yarp.Vector(jnts, encs.data()) 41 | 42 | #initialize a new tmp vector identical to encs 43 | tmp=yarp.Vector(jnts) 44 | 45 | 46 | tmp.set(0, tmp.get(0)+10) 47 | tmp.set(1, tmp.get(1)+10) 48 | tmp.set(2, tmp.get(2)+10) 49 | tmp.set(3, tmp.get(3)+10) 50 | 51 | iPos.positionMove(tmp.data()) 52 | 53 | ## wait some time then go back to initial position 54 | iPos.positionMove(encs.data()) 55 | -------------------------------------------------------------------------------- /src/actionPrimitives/app/conf/hand_sequences.ini: -------------------------------------------------------------------------------- 1 | 2 | [GENERAL] 3 | numSequences 3 4 | 5 | [SEQ_0] 6 | key open_hand 7 | numWayPoints 1 8 | wp_0 (poss (0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (vels (60.0 60.0 60.0 60.0 60.0 60.0 60.0 60.0 60.0)) (tols (1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0)) (thres (1000.0 1000.0 1000.0 1000.0 1000.0)) (tmo 10.0) 9 | 10 | [SEQ_1] 11 | key karate_hand 12 | numWayPoints 1 13 | wp_0 (poss (35.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (vels (60.0 60.0 60.0 60.0 60.0 60.0 60.0 60.0 60.0)) (tols (1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0)) (thres (1000.0 1000.0 1000.0 1000.0 1000.0)) (tmo 10.0) 14 | 15 | [SEQ_2] 16 | key close_hand 17 | numWayPoints 4 18 | wp_0 (poss (15.0 80.0 0.0 0.0 70.0 0.0 70.0 0.0 60.0)) (vels (10.0 40.0 30.0 30.0 30.0 30.0 30.0 30.0 30.0)) (tols (1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0)) (thres (30.0 30.0 30.0 90.0 90.0)) (tmo 10.0) 19 | wp_1 (poss (15.0 80.0 0.0 0.0 70.0 20.0 70.0 20.0 100.0)) (vels (10.0 40.0 30.0 30.0 30.0 30.0 30.0 30.0 30.0)) (tols (1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0)) (thres (30.0 30.0 30.0 90.0 90.0)) (tmo 10.0) 20 | wp_2 (poss (15.0 80.0 20.0 0.0 70.0 40.0 70.0 40.0 120.0)) (vels (10.0 40.0 30.0 30.0 30.0 30.0 30.0 30.0 30.0)) (tols (1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0)) (thres (30.0 30.0 30.0 90.0 90.0)) (tmo 10.0) 21 | wp_3 (poss (15.0 80.0 20.0 10.0 70.0 50.0 70.0 50.0 130.0)) (vels (10.0 40.0 30.0 30.0 30.0 30.0 30.0 30.0 30.0)) (tols (1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0)) (thres (30.0 30.0 30.0 90.0 90.0)) (tmo 10.0) 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/smoke-tests/ipopt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Department of Robotics Brain and Cognitive Sciences, Istituto Italiano di Tecnologie 3 | * Author: Ugo Pattacini 4 | * CopyPolicy: Released under the terms of the GNU GPL v2.0. 5 | * 6 | */ 7 | 8 | // This code is just a white test to verify Ipopt installation 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | using namespace yarp::sig; 21 | using namespace yarp::math; 22 | using namespace iCub::iKin; 23 | 24 | 25 | int main() 26 | { 27 | printf("Ipopt: testing correct installation...\n"); 28 | 29 | iCubArm arm("right"); 30 | iKinChain *chain=arm.asChain(); 31 | 32 | iKinIpOptMin slv(*chain,IKINCTRL_POSE_XYZ,1e-3,1e-6,100); 33 | slv.setUserScaling(true,100.0,100.0,100.0); 34 | 35 | Vector xf(7,0.0); 36 | xf[0]=-0.3; 37 | xf[1]=+0.1; 38 | xf[2]=+0.1; 39 | 40 | slv.solve(chain->getAng(),xf); 41 | Vector xhat=chain->EndEffPose().subVector(0,2); 42 | xf=xf.subVector(0,2); 43 | double dist=norm(xf-xhat); 44 | bool ok=dist<1e-3; 45 | 46 | printf("target position = (%s) [m]\n",xf.toString(5,5).c_str()); 47 | printf("solved position = (%s) [m]\n",xhat.toString(5,5).c_str()); 48 | printf("distance to target = %g [m] ... ",dist); 49 | if (ok) 50 | { 51 | printf("test successful!\n"); 52 | return 0; 53 | } 54 | else 55 | { 56 | printf("test failed!\n"); 57 | return 1; 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/fakeMotorDevice/src/fakeMotorDevice.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 3 | * Author: Ugo Pattacini 4 | * email: ugo.pattacini@iit.it 5 | * Permission is granted to copy, distribute, and/or modify this program 6 | * under the terms of the GNU General Public License, version 2 or any 7 | * later version published by the Free Software Foundation. 8 | * 9 | * A copy of the license can be found at 10 | * http://www.robotcub.org/icub/license/gpl.txt 11 | * 12 | * This program is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 15 | * Public License for more details 16 | */ 17 | 18 | #include 19 | #include 20 | 21 | using namespace yarp::dev; 22 | 23 | /**********************************************************/ 24 | void registerFakeMotorDevices() 25 | { 26 | DriverCreator *factoryServer=new DriverCreatorOf("fakeyServer", 27 | "fakeMotorDeviceClient", 28 | "fakeMotorDeviceServer"); 29 | 30 | DriverCreator *factoryClient=new DriverCreatorOf("fakeyClient", 31 | "fakeMotorDeviceClient", 32 | "fakeMotorDeviceClient"); 33 | 34 | Drivers::factory().add(factoryServer); 35 | Drivers::factory().add(factoryClient); 36 | } 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | set(PROJECTNAME icub-tutorials) 4 | 5 | project(${PROJECTNAME}) 6 | 7 | find_package(YARP 3.5.1 REQUIRED COMPONENTS os sig dev math gsl idl_tools) 8 | find_package(ICUB REQUIRED) 9 | list(APPEND CMAKE_MODULE_PATH ${ICUB_MODULE_PATH}) 10 | 11 | include_directories(${ICUB_INCLUDE_DIRS}) 12 | 13 | ## this is for testing only, please ignore if you are reading tutorials 14 | include(CTest) 15 | ### end test specific code 16 | 17 | # go by subdirectories 18 | add_subdirectory(imageProcessing) 19 | add_subdirectory(motorControlBasic) 20 | add_subdirectory(motorControlImpedance) 21 | add_subdirectory(periodicThread) 22 | add_subdirectory(ctrlLib) 23 | add_subdirectory(iDyn) 24 | add_subdirectory(rpcIdl) 25 | 26 | if(TARGET learningMachine) 27 | add_subdirectory(learningMachines) 28 | else() 29 | message(STATUS "Unmet dependencies, skipping learningMachines") 30 | endif() 31 | 32 | if(TARGET perceptiveModels) 33 | add_subdirectory(perceptiveModels) 34 | else() 35 | message(STATUS "Unmet dependencies, skipping perceptiveModels") 36 | endif() 37 | 38 | if(TARGET actionPrimitives) 39 | add_subdirectory(actionPrimitives) 40 | else() 41 | message(STATUS "Unmet dependencies, skipping actionPrimitives") 42 | endif() 43 | 44 | if(ICUB_USE_IPOPT) 45 | find_package(IPOPT QUIET) 46 | message(STATUS "Testing IPOPT dependent code") 47 | 48 | add_executable(test_ipopt smoke-tests/ipopt.cpp) 49 | target_link_libraries(test_ipopt ${YARP_LIBRARIES} iKin) 50 | install(TARGETS test_ipopt DESTINATION bin) 51 | 52 | ### execute tests 53 | add_test(NAME test_ipopt COMMAND test_ipopt) 54 | add_subdirectory(iKin) 55 | add_subdirectory(optimization) 56 | else() 57 | message(STATUS "IPOPT not found") 58 | endif() 59 | 60 | ###################### end of test specific code 61 | 62 | -------------------------------------------------------------------------------- /src/rpcIdl/include/IRpcServer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | // Autogenerated by Thrift Compiler (0.12.0-yarped) 10 | // 11 | // This is an automatically generated file. 12 | // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. 13 | 14 | #ifndef YARP_THRIFT_GENERATOR_SERVICE_IRPCSERVER_H 15 | #define YARP_THRIFT_GENERATOR_SERVICE_IRPCSERVER_H 16 | 17 | #include 18 | #include 19 | 20 | /** 21 | * IRpcServer 22 | * Interface for an example module. 23 | */ 24 | class IRpcServer : 25 | public yarp::os::Wire 26 | { 27 | public: 28 | // Constructor 29 | IRpcServer(); 30 | 31 | /** 32 | * Get answer from server 33 | * @return the answer 34 | */ 35 | virtual std::int32_t get_answer(); 36 | 37 | /** 38 | * Set value for future answers. 39 | * @param rightAnswer new answer 40 | * @return true if connection was successful 41 | */ 42 | virtual bool set_answer(const std::int32_t rightAnswer); 43 | 44 | /** 45 | * Add one integer to future answers. 46 | * @param x value to add 47 | * @return new value 48 | */ 49 | virtual std::int32_t add_int(const std::int32_t x); 50 | 51 | /** 52 | * Start service 53 | * @return true if service started correctly 54 | */ 55 | virtual bool start(); 56 | 57 | /** 58 | * Stop service 59 | * @return true if service stopped correctly 60 | */ 61 | virtual bool stop(); 62 | 63 | /** 64 | * Check is service is running 65 | * @return true/false if service is/is not running 66 | */ 67 | virtual bool is_running(); 68 | 69 | // help method 70 | virtual std::vector help(const std::string& functionName = "--all"); 71 | 72 | // read from ConnectionReader 73 | bool read(yarp::os::ConnectionReader& connection) override; 74 | }; 75 | 76 | #endif // YARP_THRIFT_GENERATOR_SERVICE_IRPCSERVER_H 77 | -------------------------------------------------------------------------------- /doc/hello_icub.dox: -------------------------------------------------------------------------------- 1 | /** 2 | @page icub_hello_icub The classic hello world 3 | 4 | \section sec_goal Goal 5 | In this short tutorial we learn how to use CMake. We compile a simple test program that links 6 | all YARP libraries. This program is trivial and does not use the robot yet. 7 | 8 | We assume you have: 9 | - installed and compiled YARP 10 | - correctly set YARP_DIR 11 | 12 | First we need to prepare the main.cpp. 13 | 14 | Open a text editor and type the following: 15 | 16 | \code 17 | #include 18 | #include 19 | 20 | using namespace std; 21 | using namespace yarp::os; 22 | 23 | int main() 24 | { 25 | printf("Starting the application\n"); 26 | int times=10; 27 | 28 | while(times--) 29 | { 30 | printf("Hello iCub\n"); 31 | SystemClock::delaySystem(0.5); //wait 0.5 seconds 32 | } 33 | printf("Goodbye!\n"); 34 | } 35 | \endcode 36 | 37 | Save it to main.cpp. 38 | 39 | This program will print the "Hello iCub" message 40 | for some times, waiting 0.5 seconds at every iteration of the while loop. 41 | 42 | Let's see now how to compile this program. 43 | 44 | Go back to your text editor and write the following file: 45 | 46 | \code 47 | project(HELLO_ICUB) 48 | 49 | find_package(YARP REQUIRED) 50 | 51 | add_executable(hello main.cpp) 52 | 53 | # we now add the YARP libraries to our project. 54 | target_link_libraries(hello ${YARP_LIBRARIES}) 55 | \endcode 56 | 57 | Now save this file as CMakeLists.txt, in the same directory as your main.cpp file. 58 | 59 | This code tells cmake to produce appropriate project files to compile an executable 60 | named hello, using the file main.cpp. It also links in yarp libraries. 61 | 62 | Now run cmake: 63 | \verbatim 64 | cmake ./ 65 | \endverbatim 66 | 67 | If cmake complains about not being able to localize YARP, it means that you have 68 | not followed correctly the procedure in the manual about the compilation of YARP. 69 | In particular, check that you have correctly set the environment veriables YARP_DIR. 70 | It is also possible to manually set these values, but we do not recommend it. 71 | 72 | Now you are ready to compile this example using your favorite compiler, and execute it. 73 | 74 | In linux: 75 | \code 76 | make 77 | ./hello 78 | \endcode 79 | 80 | **/ 81 | -------------------------------------------------------------------------------- /src/imageProcessing/lookAtLocation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | using namespace std; 7 | using namespace yarp::os; 8 | using namespace yarp::sig; 9 | using namespace yarp::dev; 10 | int main() { 11 | Network yarp; // set up yarp 12 | BufferedPort targetPort; 13 | targetPort.open("/tutorial/target/in"); 14 | Network::connect("/tutorial/target/out","/tutorial/target/in"); 15 | 16 | Property options; 17 | options.put("device", "remote_controlboard"); 18 | options.put("local", "/tutorial/motor/client"); 19 | options.put("remote", "/icubSim/head"); 20 | PolyDriver robotHead(options); 21 | if (!robotHead.isValid()) { 22 | printf("Cannot connect to robot head\n"); 23 | return 1; 24 | } 25 | IControlMode *mod; 26 | IPositionControl *pos; 27 | IVelocityControl *vel; 28 | IEncoders *enc; 29 | robotHead.view(mod); 30 | robotHead.view(pos); 31 | robotHead.view(vel); 32 | robotHead.view(enc); 33 | if (mod==NULL || pos==NULL || vel==NULL || enc==NULL) { 34 | printf("Cannot get interface to robot head\n"); 35 | robotHead.close(); 36 | return 1; 37 | } 38 | int jnts = 0; 39 | pos->getAxes(&jnts); 40 | Vector setpoints; 41 | setpoints.resize(jnts); 42 | 43 | // enable velocity control mode 44 | vector modes(jnts,VOCAB_CM_VELOCITY); 45 | mod->setControlModes(modes.data()); 46 | 47 | while (1) { // repeat forever 48 | Vector *target = targetPort.read(); // read a target 49 | if (target!=NULL) { // check we actually got something 50 | printf("We got a vector containing"); 51 | for (size_t i=0; isize(); i++) { 52 | printf(" %g", (*target)[i]); 53 | } 54 | printf("\n"); 55 | 56 | double x = (*target)[0]; 57 | double y = (*target)[1]; 58 | double conf = (*target)[2]; 59 | 60 | x -= 320/2; 61 | y -= 240/2; 62 | 63 | double vx = x*0.1; 64 | double vy = -y*0.1; 65 | 66 | // prepare command 67 | for (int i=0; i0.5) { 72 | setpoints[3] = vy; 73 | setpoints[4] = vx; 74 | } else { 75 | setpoints[3] = 0; 76 | setpoints[4] = 0; 77 | } 78 | vel->velocityMove(setpoints.data()); 79 | } 80 | } 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /src/imageProcessing/findLocation.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | /** 4 | * @ingroup icub_tutorials 5 | * \defgroup imageProc imageProc 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | using namespace yarp::sig; 18 | using namespace yarp::os; 19 | 20 | int main(int argc, char *argv[]) 21 | { 22 | Network yarp; 23 | 24 | BufferedPort > imagePort; // make a port for reading images 25 | BufferedPort > outPort; 26 | 27 | imagePort.open("/imageProc/image/in"); // give the port a name 28 | outPort.open("/imageProc/image/out"); 29 | 30 | while (1) { // repeat forever 31 | ImageOf *image = imagePort.read(); // read an image 32 | ImageOf &outImage = outPort.prepare(); //get an output image 33 | 34 | outImage=*image; 35 | 36 | if (image!=NULL) { // check we actually got something 37 | //printf("We got an image of size %dx%d\n", image->width(), image->height()); 38 | double xMean = 0; 39 | double yMean = 0; 40 | int ct = 0; 41 | for (int x=0; xwidth(); x++) { 42 | for (int y=0; yheight(); y++) { 43 | PixelRgb& pixel = image->pixel(x,y); 44 | // very simple test for blueishness 45 | // make sure blue level exceeds red and green by a factor of 2 46 | if (pixel.b>pixel.r*1.2+10 && pixel.b>pixel.g*1.2+10) { 47 | // there's a blueish pixel at (x,y)! 48 | // let's find the average location of these pixels 49 | xMean += x; 50 | yMean += y; 51 | ct++; 52 | 53 | outImage(x,y).r=255; 54 | } 55 | } 56 | } 57 | if (ct>0) { 58 | xMean /= ct; 59 | yMean /= ct; 60 | } 61 | if (ct>(image->width()/20)*(image->height()/20)) { 62 | printf("Best guess at blue target: %g %g\n", xMean, yMean); 63 | } 64 | 65 | outPort.write(); 66 | } 67 | } 68 | return 0; 69 | } 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /doc/python-motor-control.dox: -------------------------------------------------------------------------------- 1 | /** 2 | @page icub_python_basic_motor Using Motor Control Interfaces in Python 3 | 4 | \section sec_goal Goal 5 | This tutorial shows how to use Python to interface with the iCub. In particular we focus on controlling the motors using the motor control interfaces. We assume that you have already compiled or installed the Python bingings. This tutorial assume basic knowledge of how YARP works in C++. We just show how the robot C++ interface can be used in Python. 6 | 7 | Dislaimer: this code does not do anything fancy with the robot. We let you have fun with the robot and Python once you know how the interface works. 8 | 9 | \section sec_python-ll Joint level motor control 10 | 11 | For this tutorial we only need the yarp python bindings. 12 | 13 | First initialize yarp. 14 | 15 | \code 16 | import yarp; 17 | yarp.Network.init(); 18 | \endcode 19 | 20 | We know initialize a remote_controlboard object connected to the right_arm. 21 | 22 | \code 23 | # prepare a property object 24 | props = yarp.Property() 25 | props.put("device","remote_controlboard") 26 | props.put("local","/client/right_arm") 27 | props.put("remote","/icubSim/right_arm") 28 | 29 | # create remote driver 30 | armDriver = yarp.PolyDriver(props) 31 | \endcode 32 | 33 | Now we can query the motor interfaces. 34 | 35 | \code 36 | #query motor control interfaces 37 | iPos = armDriver.viewIPositionControl() 38 | iEnc = armDriver.viewIEncoders() 39 | \endcode 40 | 41 | We are now ready to retrive the number of joints, create vectors to store the position commands and move the arm. 42 | 43 | \code 44 | #retrieve number of joints 45 | jnts=iPos.getAxes() 46 | 47 | print 'Controlling', jnts, 'joints' 48 | 49 | # read encoders 50 | encs=yarp.Vector(jnts) 51 | iEnc.getEncoders(encs.data()) 52 | 53 | # store as home position 54 | home=yarp.Vector(jnts, encs.data()) 55 | 56 | #initialize a new tmp vector identical to encs 57 | tmp=yarp.Vector(jnts) 58 | 59 | # add 10 degrees to the first four joints 60 | tmp.set(0, tmp.get(0)+10) 61 | tmp.set(1, tmp.get(1)+10) 62 | tmp.set(2, tmp.get(2)+10) 63 | tmp.set(3, tmp.get(3)+10) 64 | 65 | # move to new position 66 | iPos.positionMove(tmp.data()) 67 | 68 | \endcode 69 | 70 | Now wait some time and go back to home position: 71 | 72 | \code 73 | iPos.positionMove(home.data()) 74 | \endcode 75 | 76 | Code can be found at python/python-motor-control.py 77 | 78 | This file can be edited at doc/python-motor-control.dox 79 | 80 | **/ -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/fakeRobot/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 3 | * Author: Ugo Pattacini 4 | * email: ugo.pattacini@iit.it 5 | * Permission is granted to copy, distribute, and/or modify this program 6 | * under the terms of the GNU General Public License, version 2 or any 7 | * later version published by the Free Software Foundation. 8 | * 9 | * A copy of the license can be found at 10 | * http://www.robotcub.org/icub/license/gpl.txt 11 | * 12 | * This program is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 15 | * Public License for more details 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | using namespace std; 26 | using namespace yarp::os; 27 | using namespace yarp::dev; 28 | 29 | /** 30 | * This container class launches the server part of the 31 | * fake motor device in order to simulate a robot called 32 | * "fake_robot" wiht the part "fake_part" composed of three 33 | * actuated rotational joints. 34 | */ 35 | class Launcher: public RFModule 36 | { 37 | PolyDriver driver; 38 | 39 | public: 40 | /**********************************************************/ 41 | bool configure(ResourceFinder &rf) 42 | { 43 | Property options; 44 | options.put("device","fakeyServer"); 45 | options.put("local","/fake_robot/fake_part"); 46 | 47 | driver.open(options); 48 | return driver.isValid(); 49 | } 50 | 51 | /**********************************************************/ 52 | bool close() 53 | { 54 | if (driver.isValid()) 55 | driver.close(); 56 | 57 | return true; 58 | } 59 | 60 | /**********************************************************/ 61 | double getPeriod() { return 1.0; } 62 | bool updateModule() { return true; } 63 | }; 64 | 65 | 66 | /**********************************************************/ 67 | int main() 68 | { 69 | Network yarp; 70 | if (!yarp.checkNetwork()) 71 | { 72 | cout<<"Error: yarp server does not seem available"< 2 | 3 | 4 | 5 | rpcIdl 6 | 7 | Example to show idl usage. 8 | Released under the terms of the GNU GPL v2.0 9 | 1.0 10 | 11 | 12 | Implements a trivial server that provides access to an integer. 13 | 14 | @subsection sec-intro Introduction 15 | This is the introduction. 123 16 | You can use html, for example to add a link to 17 | the yarp page and Doxygen tags. 18 | 19 | @subsection sec-details More stuff 20 | This is a detailed description, etc. 21 | 22 | 23 | 24 | name 25 | from 26 | context 27 | 28 | 29 | 30 | Lorenzo Natale 31 | 32 | 33 | 34 | 35 | Image 36 | /rpcIdl/img:i 37 | no 38 | no 39 | Feed images to rpcIdl using this port. 40 | 41 | 42 | 43 | Image 44 | /rpcIdl/img:o 45 | Output image. 46 | 47 | 48 | 49 | yarp::os::Bottle 50 | /rpcIdl/target:o 51 | 52 | Outputs the x-y location of whatever is the output of the module: 53 | - int i value of y 54 | - int j value of x 55 | 56 | 57 | 58 | 59 | 60 | IRpcServer 61 | IRpcServer.thrift 62 | /rpcIdl/server 63 | Command port 64 | 65 | 66 | 67 | 68 | IRpcServer 69 | IRpcServer.thrift 70 | /rpcIdl/client 71 | Control remote object 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /src/learningMachines/lmlibdirect.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2007-2009 Arjan Gijsberts 3 | * CopyPolicy: Released under the terms of the GNU GPL v2.0. 4 | * 5 | * Example code how to use the learningMachine library in a direct manner. 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #define INPUT_MIN -10 16 | #define INPUT_MAX 10 17 | #define NO_TRAIN 400 18 | #define NO_TEST 400 19 | 20 | using namespace iCub::learningmachine; 21 | using namespace yarp::sig; 22 | 23 | std::pair createSample(double min_in, double max_in) { 24 | std::pair sample; 25 | sample.first.resize(1); 26 | sample.second.resize(1); 27 | double input = yarp::math::Rand::scalar(min_in, max_in); 28 | sample.first[0] = input; 29 | sample.second[0] = std::sin(input); 30 | return sample; 31 | } 32 | 33 | /* 34 | * This example shows how LearningMachine classes can be used in a direct manner 35 | * in your code. Please see all direct/indirect/portable examples to have an 36 | * idea which method suits your application best. 37 | * 38 | * Please keep in mind that the purpose is to demonstrate how to interface with 39 | * the learningMachine library. The synthetic data used in this example is 40 | * utterly useless. 41 | */ 42 | 43 | int main(int argc, char** argv) { 44 | std::cout << "LearningMachine library example (direct)" << std::endl; 45 | 46 | // one dimensional input, one dimensional output, c = 1.0 47 | LSSVMLearner lssvm = LSSVMLearner(1, 1, 2.); 48 | lssvm.getKernel()->setGamma(32.); 49 | 50 | // normalizer that scales [-10,10] -> [-1,1] 51 | // IScalers only do R^1 -> R^1, for higher dimensions use ScaleTransformer 52 | FixedRangeScaler scaler = FixedRangeScaler(INPUT_MIN, INPUT_MAX); 53 | 54 | // create and feed training samples 55 | for(int i = 0; i < NO_TRAIN; i++) { 56 | std::pair sample = createSample(INPUT_MIN, INPUT_MAX); 57 | sample.first[0] = scaler.transform(sample.first[0]); 58 | lssvm.feedSample(sample.first, sample.second); 59 | } 60 | 61 | // train the machine on the data (it's a batch machine!) 62 | lssvm.train(); 63 | 64 | // feed test samples 65 | double MSE = 0.; 66 | for(int i = 0; i < NO_TEST; i++) { 67 | std::pair sample = createSample(INPUT_MIN, INPUT_MAX); 68 | sample.first[0] = scaler.transform(sample.first[0]); 69 | Prediction prediction = lssvm.predict(sample.first); 70 | double diff = sample.second[0] - prediction.getPrediction()[0]; 71 | MSE += diff * diff; 72 | } 73 | MSE /= NO_TEST; 74 | std::cout << "MSE on test data after " << NO_TEST << " samples: " << MSE << std::endl; 75 | } 76 | 77 | 78 | -------------------------------------------------------------------------------- /src/periodicThread/tutorial_periodic_thread.cpp: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | using namespace yarp::os; 15 | using namespace yarp::dev; 16 | using namespace yarp::sig; 17 | 18 | using namespace std; 19 | 20 | class ControlThread: public PeriodicThread 21 | { 22 | PolyDriver dd; 23 | IVelocityControl *ivel; 24 | IEncoders *iencs; 25 | Vector encoders; 26 | Vector commands; 27 | int count; 28 | public: 29 | ControlThread(double period):PeriodicThread(period){} 30 | 31 | bool threadInit() 32 | { 33 | //initialize here variables 34 | printf("ControlThread:starting\n"); 35 | 36 | Property options; 37 | options.put("device", "remote_controlboard"); 38 | options.put("local", "/local/head"); 39 | 40 | //substitute icubSim with icub for use with the real robot 41 | options.put("remote", "/icubSim/head"); 42 | 43 | dd.open(options); 44 | 45 | dd.view(iencs); 46 | dd.view(ivel); 47 | 48 | if ( (!iencs) || (!ivel) ) 49 | return false; 50 | 51 | int joints; 52 | 53 | iencs->getAxes(&joints); 54 | 55 | encoders.resize(joints); 56 | commands.resize(joints); 57 | 58 | commands=10000; 59 | ivel->setRefAccelerations(commands.data()); 60 | 61 | count=0; 62 | return true; 63 | } 64 | 65 | void threadRelease() 66 | { 67 | printf("ControlThread:stopping the robot\n"); 68 | 69 | ivel->stop(); 70 | 71 | dd.close(); 72 | 73 | printf("Done, goodbye from ControlThread\n"); 74 | } 75 | 76 | void run() 77 | { 78 | //do the work 79 | iencs->getEncoders(encoders.data()); 80 | 81 | count++; 82 | 83 | if (count%2) 84 | commands=5; 85 | else 86 | commands=-5; 87 | 88 | ivel->velocityMove(commands.data()); 89 | 90 | printf("."); 91 | } 92 | }; 93 | 94 | int main(int argc, char *argv[]) 95 | { 96 | Network yarp; 97 | 98 | if (!yarp.checkNetwork()) 99 | { 100 | printf("No yarp network, quitting\n"); 101 | return 1; 102 | } 103 | 104 | 105 | ControlThread myThread(4.0); //period is 4s 106 | 107 | myThread.start(); 108 | 109 | bool done=false; 110 | double startTime=Time::now(); 111 | while(!done) 112 | { 113 | if ((Time::now()-startTime)>5) 114 | done=true; 115 | } 116 | 117 | myThread.stop(); 118 | 119 | return 0; 120 | } 121 | -------------------------------------------------------------------------------- /src/iKin/iCubLimbsFwKin/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @ingroup icub_tutorials 3 | * 4 | * \defgroup icub_iCubLimbsFwKin Example on Forward Kinematics of iCub limbs 5 | * 6 | * A tutorial on how to use iKin library for forward kinematics of iCub limbs. 7 | * 8 | * \author Ugo Pattacini 9 | * 10 | * CopyPolicy: Released under the terms of GPL 2.0 or later 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | using namespace std; 25 | using namespace yarp::os; 26 | using namespace yarp::sig; 27 | using namespace iCub::iKin; 28 | 29 | /****************************************************************/ 30 | int main(int argc, char *argv[]) 31 | { 32 | ResourceFinder rf; 33 | rf.configure(argc, argv); 34 | if (rf.check("help")) 35 | { 36 | cout << "Options:" << endl; 37 | cout << "--kinematics eye|arm|leg" << endl; 38 | cout << "--type left|right|left_v2|..." << endl; 39 | cout << "--q \"(1.0 ... n)\"" << endl; 40 | return EXIT_SUCCESS; 41 | } 42 | 43 | string kinematics = rf.check("kinematics", Value("eye")).asString(); 44 | string type = rf.check("type", Value("left")).asString(); 45 | 46 | transform(kinematics.begin(), kinematics.end(), kinematics.begin(), ::tolower); 47 | transform(type.begin(), type.end(), type.begin(), ::tolower); 48 | 49 | if ((kinematics != "eye") && (kinematics != "arm") && (kinematics != "leg")) 50 | { 51 | cerr << "unrecognized kinematics \"" << kinematics << "\"" << endl; 52 | return EXIT_FAILURE; 53 | } 54 | 55 | unique_ptr limb; 56 | if (kinematics == "eye") 57 | { 58 | limb = unique_ptr(new iCubEye(type)); 59 | } 60 | else if (kinematics == "arm") 61 | { 62 | limb = unique_ptr(new iCubArm(type)); 63 | } 64 | else 65 | { 66 | limb = unique_ptr(new iCubLeg(type)); 67 | } 68 | 69 | cout << "Asked for type \"" << type << "\"" << endl; 70 | cout << "Type used \"" << limb->getType() << "\"" << endl; 71 | 72 | iKinChain* chain = limb->asChain(); 73 | chain->setAllConstraints(false); 74 | for (size_t i = 0; i < chain->getN(); i++) 75 | { 76 | chain->releaseLink(i); 77 | } 78 | 79 | Vector q(chain->getN(), 0.0); 80 | if (Bottle *b = rf.find("q").asList()) 81 | { 82 | size_t len = std::min(q.length(), (size_t)b->size()); 83 | for (size_t i = 0; i < len; i++) 84 | { 85 | q[i] = b->get(i).asFloat64(); 86 | } 87 | } 88 | 89 | Matrix H = chain->getH((M_PI / 180.0) * q); 90 | 91 | cout << "kinematics=\"" << kinematics << "/" << type << "\"" << endl; 92 | cout << "q=(" << ((180.0 / M_PI) * chain->getAng()).toString(5, 5) << ")" << endl; 93 | cout << "H=" << endl << H.toString(5, 5) << endl; 94 | cout << endl; 95 | 96 | return EXIT_SUCCESS; 97 | } 98 | -------------------------------------------------------------------------------- /src/optimization/calibReference/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @ingroup icub_tutorials 3 | * 4 | * \defgroup icub_calibReference Calibration of Reference 5 | * Frames 6 | * 7 | * A tutorial on how to use the calibration procedure to find 8 | * out the roto-translation matrix of two sets of matching 3D 9 | * points. 10 | * 11 | * \author Ugo Pattacini 12 | * 13 | * CopyPolicy: Released under the terms of GPL 2.0 or later 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | using namespace std; 29 | using namespace yarp::os; 30 | using namespace yarp::sig; 31 | using namespace yarp::math; 32 | using namespace iCub::ctrl; 33 | using namespace iCub::optimization; 34 | 35 | 36 | int main() 37 | { 38 | // define our working bounding box 39 | Vector min(3),max(3); 40 | min[0]=-5.0; max[0]=5.0; 41 | min[1]=-5.0; max[1]=5.0; 42 | min[2]=-5.0; max[2]=5.0; 43 | 44 | // define the "unknown" transformation: 45 | // translation 46 | Vector p(3); 47 | p[0]=0.3; p[1]=-2.0; p[2]=3.1; 48 | // rotation 49 | Vector euler(3); 50 | euler[0]=M_PI/4.0; euler[1]=M_PI/6.0; euler[2]=M_PI/3.0; 51 | 52 | Matrix H=euler2dcm(euler); 53 | H(0,3)=p[0]; H(1,3)=p[1]; H(2,3)=p[2]; 54 | 55 | // ansisotropic scale 56 | Vector scale(3); 57 | scale[0]=0.9; scale[1]=0.8; scale[2]=1.1; 58 | Vector h_scale=scale; 59 | h_scale.push_back(1.0); 60 | 61 | // create the calibrator 62 | CalibReferenceWithMatchedPoints calibrator; 63 | 64 | // generate randomly the two clouds of matching 3D points 65 | for (int i=0; i<10; i++) 66 | { 67 | Vector p0=Rand::vector(min,max); 68 | p0.push_back(1.0); 69 | 70 | // make data a bit dirty (add up noise in the range (-1,1) [cm]) 71 | Vector eps=Rand::vector(Vector(3,-0.01),Vector(3,0.01)); 72 | eps.push_back(0.0); 73 | Vector p1=h_scale*(H*p0)+eps; 74 | 75 | // feed the calibrator with the matching pair 76 | calibrator.addPoints(p0,p1); 77 | } 78 | 79 | // set the working bounding box where the solution is seeked 80 | calibrator.setBounds(min,max); 81 | 82 | // carry out the calibration 83 | Matrix Hcap; 84 | Vector scalecap; 85 | double error; 86 | double t0=SystemClock::nowSystem(); 87 | calibrator.calibrate(Hcap,scalecap,error); 88 | double dt=SystemClock::nowSystem()-t0; 89 | 90 | // the final report 91 | cout<tutorial 10 | we showed how to use the ResourceFinder to organize modules parameters in $YARP_DATA_HOME/contexts. 11 | 12 | The RFModule helper class simplify writing an iCub module that uses the ResourceFinder class. 13 | 14 | This is how a module will look like: 15 | 16 | \code 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | using namespace std; 24 | using namespace yarp::os; 25 | 26 | class MyModule:public RFModule 27 | { 28 | Port handlerPort; // a port to handle messages 29 | int count; 30 | public: 31 | 32 | double getPeriod() 33 | { 34 | /* module periodicity (seconds), called implicitly by the module. */ 35 | return 1.0; 36 | } 37 | 38 | /* This is our main function. Will be called periodically every getPeriod() seconds */ 39 | bool updateModule() 40 | { 41 | count++; 42 | cout<<"["< 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | using namespace yarp::dev; 14 | using namespace yarp::sig; 15 | using namespace yarp::os; 16 | 17 | int main(int argc, char *argv[]) 18 | { 19 | Network yarp; 20 | 21 | Property params; 22 | params.fromCommand(argc, argv); 23 | 24 | if (!params.check("robot")) 25 | { 26 | fprintf(stderr, "Please specify the name of the robot\n"); 27 | fprintf(stderr, "--robot name (e.g. icub)\n"); 28 | return 1; 29 | } 30 | std::string robotName=params.find("robot").asString(); 31 | std::string remotePorts="/"; 32 | remotePorts+=robotName; 33 | remotePorts+="/right_arm"; 34 | 35 | std::string localPorts="/test/client"; 36 | 37 | Property options; 38 | options.put("device", "remote_controlboard"); 39 | options.put("local", localPorts); //local port names 40 | options.put("remote", remotePorts); //where we connect to 41 | 42 | // create a device 43 | PolyDriver robotDevice(options); 44 | if (!robotDevice.isValid()) { 45 | printf("Device not available. Here are the known devices:\n"); 46 | printf("%s", Drivers::factory().toString().c_str()); 47 | return 0; 48 | } 49 | 50 | IPositionControl *pos; 51 | IEncoders *encs; 52 | 53 | bool ok; 54 | ok = robotDevice.view(pos); 55 | ok = ok && robotDevice.view(encs); 56 | 57 | if (!ok) { 58 | printf("Problems acquiring interfaces\n"); 59 | return 0; 60 | } 61 | 62 | int nj=0; 63 | pos->getAxes(&nj); 64 | Vector encoders; 65 | Vector command; 66 | Vector tmp; 67 | encoders.resize(nj); 68 | tmp.resize(nj); 69 | command.resize(nj); 70 | 71 | int i; 72 | for (i = 0; i < nj; i++) { 73 | tmp[i] = 50.0; 74 | } 75 | pos->setRefAccelerations(tmp.data()); 76 | 77 | for (i = 0; i < nj; i++) { 78 | tmp[i] = 10.0; 79 | pos->setRefSpeed(i, tmp[i]); 80 | } 81 | 82 | //pos->setRefSpeeds(tmp.data())) 83 | 84 | //fisrst read all encoders 85 | // 86 | printf("waiting for encoders"); 87 | while(!encs->getEncoders(encoders.data())) 88 | { 89 | Time::delay(0.1); 90 | printf("."); 91 | } 92 | printf("\n;"); 93 | 94 | command=encoders; 95 | //now set the shoulder to some value 96 | command[0]=-50; 97 | command[1]=20; 98 | command[2]=-10; 99 | command[3]=50; 100 | pos->positionMove(command.data()); 101 | 102 | bool done=false; 103 | 104 | while(!done) 105 | { 106 | pos->checkMotionDone(&done); 107 | Time::delay(0.1); 108 | } 109 | 110 | int times=0; 111 | while(true) 112 | { 113 | times++; 114 | if (times%2) 115 | { 116 | command[0]=-50; 117 | command[1]=20; 118 | command[2]=-10; 119 | command[3]=50; 120 | } 121 | else 122 | { 123 | command[0]=-20; 124 | command[1]=40; 125 | command[2]=-10; 126 | command[3]=30; 127 | } 128 | 129 | pos->positionMove(command.data()); 130 | 131 | int count=50; 132 | while(count--) 133 | { 134 | Time::delay(0.1); 135 | bool ret=encs->getEncoders(encoders.data()); 136 | 137 | if (!ret) 138 | { 139 | fprintf(stderr, "Error receiving encoders, check connectivity with the robot\n"); 140 | } 141 | else 142 | { 143 | printf("%.1lf %.1lf %.1lf %.1lf\n", encoders[0], encoders[1], encoders[2], encoders[3]); 144 | } 145 | } 146 | } 147 | 148 | robotDevice.close(); 149 | 150 | return 0; 151 | } 152 | -------------------------------------------------------------------------------- /src/iKin/onlineSolver/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @ingroup icub_tutorials 3 | * 4 | * \defgroup icub_onlineSolver Example for iKin online Solver 5 | * 6 | * A tutorial on how to use the iKin online solver. 7 | * 8 | * \author Ugo Pattacini 9 | * 10 | * CopyPolicy: Released under the terms of GPL 2.0 or later 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | using namespace yarp::os; 29 | using namespace yarp::sig; 30 | using namespace yarp::math; 31 | using namespace iCub::iKin; 32 | 33 | 34 | int main() 35 | { 36 | Bottle cmd, reply; 37 | Network yarp; 38 | 39 | if (!yarp.checkNetwork()) 40 | return 1; 41 | 42 | // declare the on-line arm solver called "solver" 43 | iCubArmCartesianSolver onlineSolver("solver"); 44 | 45 | Property options; 46 | // it will operate on the simulator (which is supposed to be already running) 47 | options.put("robot","icubSim"); 48 | // it will work with the right arm 49 | options.put("type","right"); 50 | // it will achieve just the positional pose 51 | options.put("pose","xyz"); 52 | // switch off verbosity 53 | options.put("verbosity","off"); 54 | 55 | // launch the solver and let it connect to the simulator 56 | if (!onlineSolver.open(options)) 57 | return 1; 58 | 59 | // prepare ports 60 | Port in, out, rpc; 61 | in.open("/in"); out.open("/out"); rpc.open("/rpc"); 62 | Network::connect("/solver/out",in.getName()); 63 | Network::connect(out.getName(),"/solver/in"); 64 | Network::connect(rpc.getName(),"/solver/rpc"); 65 | 66 | // print status 67 | cmd.clear(); 68 | cmd.addVocab32(IKINSLV_VOCAB_CMD_GET); 69 | cmd.addVocab32(IKINSLV_VOCAB_OPT_DOF); 70 | rpc.write(cmd,reply); 71 | cout<<"got dof: "<attachAll(list)) 83 | { 84 | cout<<"Error: Unable to attach device drivers!"< 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace std; 29 | using namespace yarp::os; 30 | using namespace yarp::dev; 31 | using namespace yarp::sig; 32 | using namespace yarp::math; 33 | 34 | /** 35 | * This class implements the client. 36 | */ 37 | class ClientModule: public RFModule 38 | { 39 | protected: 40 | PolyDriver client; 41 | ICartesianControl *arm; 42 | Vector xdhat; 43 | 44 | public: 45 | /**********************************************************/ 46 | bool configure(ResourceFinder &rf) 47 | { 48 | string remote=rf.find("remote").asString(); 49 | string local=rf.find("local").asString(); 50 | 51 | // Just usual things... 52 | Property option("(device cartesiancontrollerclient)"); 53 | option.put("remote","/"+remote); 54 | option.put("local","/"+local); 55 | 56 | if (!client.open(option)) 57 | return false; 58 | 59 | // open the view 60 | client.view(arm); 61 | arm->setTrajTime(2.0); 62 | arm->setInTargetTol(1e-3); 63 | 64 | Vector dof; 65 | arm->getDOF(dof); 66 | for (size_t i=0; igetLimits(i,&min,&max); 70 | 71 | // these margins are just to prevent the 72 | // solver from solving for exactly the 73 | // joints bounds 74 | min+=1.0; max-=1.0; // [deg] 75 | arm->setLimits(i,min,max); 76 | } 77 | 78 | Rand::init(); 79 | 80 | return true; 81 | } 82 | 83 | /**********************************************************/ 84 | bool close() 85 | { 86 | if (client.isValid()) 87 | client.close(); 88 | 89 | return true; 90 | } 91 | 92 | /**********************************************************/ 93 | bool updateModule() 94 | { 95 | bool done=false; 96 | arm->checkMotionDone(&done); 97 | if (done) 98 | { 99 | Vector xd(3); 100 | xd[0]=Rand::scalar(1.0,2.5); 101 | xd[1]=Rand::scalar(0.0,2.0); 102 | xd[2]=0.0; 103 | 104 | cout<goToPositionSync(xd); 107 | Vector odhat,qdhat; 108 | arm->getDesired(xdhat,odhat,qdhat); 109 | cout<<"Going to: ("<getPose(x,o); 116 | cout<<"Running: ("< 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #define MIN(a, b) ((a < b) ? a : b) 18 | 19 | #define NO_TRAIN 1000 20 | #define NO_TEST 1000 21 | #define NOISE_MIN -0.05 22 | #define NOISE_MAX 0.05 23 | 24 | 25 | using namespace iCub::learningmachine; 26 | using namespace yarp::os; 27 | using namespace yarp::sig; 28 | using namespace yarp::math; 29 | 30 | // taken from LWPR example code 31 | double cross(double x1, double x2) { 32 | x1 *= x1; 33 | x2 *= x2; 34 | double a = std::exp(-10 * x1); 35 | double b = std::exp(-50 * x2); 36 | double c = 1.25 * std::exp(-5 * (x1 + x2)); 37 | return (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c); 38 | } 39 | 40 | double sin2d(double x1, double x2) { 41 | return std::sin(x1 + x2); 42 | } 43 | 44 | void elementProd(const Vector& v1, Vector& v2) { 45 | for(size_t i = 0; i < MIN(v1.size(), v2.size()); i++) { 46 | v2[i] = v1[i] * v2[i]; 47 | } 48 | } 49 | 50 | Vector elementDiv(const Vector& v, double d) { 51 | Vector ret(v.size()); 52 | for(size_t i = 0; i < v.size(); i++) { 53 | ret[i] = (d == 0.) ? v[i] : v[i] / d; 54 | } 55 | return ret; 56 | } 57 | 58 | 59 | std::pair createSample() { 60 | std::pair sample; 61 | sample.first.resize(2); 62 | sample.second.resize(2); 63 | sample.first[0] = Rand::scalar(-1, +1); 64 | sample.first[1] = Rand::scalar(-1, +1); 65 | sample.second[0] = sin2d(sample.first[0], sample.first[1]); 66 | sample.second[1] = cross(sample.first[0], sample.first[1]); 67 | return sample; 68 | } 69 | 70 | /* 71 | * This example shows how LearningMachine classes can be used in a indirect 72 | * manner in your code. In this context, this means that the YARP configuration 73 | * mechanism is used and instances are of the abstract base type. This 74 | * facilitates easy migration to other learning methods. Please see all 75 | * direct/indirect/portable examples to have an idea which method suits your 76 | * application best. 77 | * 78 | * Please keep in mind that the purpose is to demonstrate how to interface with 79 | * the learningMachine library. The synthetic data used in this example is 80 | * utterly useless. 81 | */ 82 | 83 | int main(int argc, char** argv) { 84 | Vector trainMSE(2); 85 | Vector testMSE(2); 86 | Vector noise_min(2); 87 | Vector noise_max(2); 88 | 89 | std::cout << "LearningMachine library example (indirect)" << std::endl; 90 | 91 | // create Regularized Least Squares learner 92 | // we need pointers here! 93 | IMachineLearner* rls = new RLSLearner(); 94 | Property p("(dom 250) (cod 2) (lambda 0.5)"); 95 | rls->configure(p); 96 | std::cout << "Learner:" << std::endl << rls->getInfo() << std::endl; 97 | 98 | // create Random Feature transformer 99 | ITransformer* rf = new RandomFeature(); 100 | p.fromString("(dom 2) (cod 250) (gamma 16.0)", true); 101 | rf->configure(p); 102 | std::cout << "Transformer:" << std::endl << rf->getInfo() << std::endl; 103 | 104 | 105 | // create and feed training samples 106 | noise_min = NOISE_MIN; 107 | noise_max = NOISE_MAX; 108 | 109 | trainMSE = 0.0; 110 | for(int i = 0; i < NO_TRAIN; i++) { 111 | // create a new training sample 112 | std::pair sample = createSample(); 113 | 114 | // add some noise to output for training 115 | Vector noisyOutput = sample.second + Rand::vector(noise_min, noise_max); 116 | 117 | // transform input using RF 118 | Vector transInput = rf->transform(sample.first); 119 | 120 | // make prediction before feeding full sample 121 | Prediction prediction = rls->predict(transInput); 122 | //std::cout << "Predict: " << prediction.toString() << std::endl; 123 | Vector diff = prediction.getPrediction() - sample.second; 124 | elementProd(diff, diff); 125 | trainMSE = trainMSE + diff; 126 | 127 | // train on complete sample with noisy output 128 | rls->feedSample(transInput, noisyOutput); 129 | } 130 | trainMSE = elementDiv(trainMSE, NO_TRAIN); 131 | std::cout << "Train MSE: " << trainMSE.toString() << std::endl; 132 | 133 | // predict test samples 134 | testMSE = 0.; 135 | for(int i = 0; i < NO_TEST; i++) { 136 | // create a new testing sample 137 | std::pair sample = createSample(); 138 | 139 | // transform input using RF 140 | Vector transInput = rf->transform(sample.first); 141 | 142 | // make prediction 143 | Prediction prediction = rls->predict(transInput); 144 | Vector diff = prediction.getPrediction() - sample.second; 145 | elementProd(diff, diff); 146 | //std::cout << "Sample: " << sample.input << 147 | testMSE = testMSE + diff; 148 | } 149 | testMSE = elementDiv(testMSE, NO_TEST); 150 | std::cout << "Test MSE: " << testMSE.toString() << std::endl; 151 | 152 | delete rls; 153 | delete rf; 154 | } 155 | 156 | 157 | -------------------------------------------------------------------------------- /src/iDyn/oneChainDynamics/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright: 2010-2011 RobotCub Consortium 3 | * Author: Serena Ivaldi 4 | * CopyPolicy: Released under the terms of the GNU GPL v2.0. 5 | **/ 6 | 7 | // 8 | // The most basic example on the use of iDyn: the computation of forces and torques in a single chain. 9 | // 10 | // Author: Serena Ivaldi - 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | using namespace yarp::sig; 20 | using namespace iCub::iDyn; 21 | 22 | 23 | 24 | // useful print functions 25 | // print a matrix nicely 26 | void printMatrix(const string &s, const Matrix &m) 27 | { 28 | cout< default 103 | armTorsoDyn.prepareNewtonEuler(DYNAMIC); 104 | 105 | // then we perform the computations 106 | // 1) Kinematic Phase 107 | // 2) Wrench Phase 108 | // the computeNewtonEuler() methoddo everything autonomously! 109 | armTorsoDyn.computeNewtonEuler(w0,dw0,ddp0,Fend,Muend); 110 | 111 | // then we can retrieve our results... 112 | // forces moments and torques 113 | Matrix F = armTorsoDyn.getForces(); 114 | Matrix Mu = armTorsoDyn.getMoments(); 115 | Vector Tau = armTorsoDyn.getTorques(); 116 | 117 | // now print the information 118 | 119 | cout<<"iCubArmDyn has "< 19 | 20 | #include 21 | #include 22 | 23 | using namespace std; 24 | using namespace yarp::os; 25 | using namespace yarp::dev; 26 | using namespace yarp::sig; 27 | 28 | /**********************************************************/ 29 | fakeMotorDeviceClient::fakeMotorDeviceClient() 30 | { 31 | configured=false; 32 | statePort.setOwner(this); 33 | } 34 | 35 | /**********************************************************/ 36 | bool fakeMotorDeviceClient::open(Searchable &config) 37 | { 38 | printf("Opening Fake Motor Device Client ...\n"); 39 | 40 | string remote=config.check("remote",Value("/fakeyServer")).asString(); 41 | string local=config.check("local",Value("/fakeyClient")).asString(); 42 | 43 | statePort.open(local+"/state:i"); 44 | cmdPort.open(local+"/cmd:o"); 45 | rpcPort.open(local+"/rpc"); 46 | 47 | bool ok=true; 48 | ok&=Network::connect(remote+"/state:o",statePort.getName(),"udp"); 49 | ok&=Network::connect(cmdPort.getName(),remote+"/cmd:i","udp"); 50 | ok&=Network::connect(rpcPort.getName(),remote+"/rpc","tcp"); 51 | 52 | if (ok) 53 | { 54 | configured=true; 55 | 56 | printf("Fake Motor Device Client successfully open\n"); 57 | return true; 58 | } 59 | else 60 | { 61 | statePort.close(); 62 | cmdPort.close(); 63 | rpcPort.close(); 64 | 65 | printf("Fake Motor Device Client failed to open\n"); 66 | return false; 67 | } 68 | } 69 | 70 | /**********************************************************/ 71 | bool fakeMotorDeviceClient::close() 72 | { 73 | printf("Closing Fake Motor Device Client ...\n"); 74 | 75 | statePort.interrupt(); 76 | cmdPort.interrupt(); 77 | rpcPort.interrupt(); 78 | 79 | statePort.close(); 80 | cmdPort.close(); 81 | rpcPort.close(); 82 | 83 | configured=false; 84 | 85 | printf("Fake Motor Device Client successfully closed\n"); 86 | return true; 87 | } 88 | 89 | /**********************************************************/ 90 | bool fakeMotorDeviceClient::getLimits(int axis, double *min, double *max) 91 | { 92 | if (!configured || (min==NULL) || (max==NULL)) 93 | return false; 94 | 95 | Bottle cmd,reply; 96 | cmd.addVocab32("lim"); 97 | cmd.addVocab32("get"); 98 | cmd.addInt32(axis); 99 | if (rpcPort.write(cmd,reply)) 100 | { 101 | *min=reply.get(1).asFloat64(); 102 | *max=reply.get(2).asFloat64(); 103 | 104 | return true; 105 | } 106 | else 107 | return false; 108 | } 109 | 110 | /**********************************************************/ 111 | bool fakeMotorDeviceClient::getAxes(int *ax) 112 | { 113 | if (!configured || (ax==NULL)) 114 | return false; 115 | 116 | Bottle cmd,reply; 117 | cmd.addVocab32("enc"); 118 | cmd.addVocab32("axes"); 119 | if (rpcPort.write(cmd,reply)) 120 | { 121 | *ax=reply.get(1).asInt32(); 122 | return true; 123 | } 124 | else 125 | return false; 126 | } 127 | 128 | /**********************************************************/ 129 | bool fakeMotorDeviceClient::getEncoders(double *encs) 130 | { 131 | if (!configured || (encs==NULL)) 132 | return false; 133 | 134 | lock_guard lg(mtx); 135 | for (size_t i=0; iencs.length(); i++) 136 | encs[i]=this->encs[i]; 137 | 138 | return true; 139 | } 140 | 141 | /**********************************************************/ 142 | bool fakeMotorDeviceClient::velocityMove(int j, double sp) 143 | { 144 | if (!configured) 145 | return false; 146 | 147 | Bottle cmd,reply; 148 | cmd.addVocab32("vel"); 149 | cmd.addVocab32("move"); 150 | cmd.addInt32(j); 151 | cmd.addFloat64(sp); 152 | if (rpcPort.write(cmd,reply)) 153 | return true; 154 | else 155 | return false; 156 | } 157 | 158 | /**********************************************************/ 159 | bool fakeMotorDeviceClient::setRefAcceleration(int j, double acc) 160 | { 161 | if (!configured) 162 | return false; 163 | 164 | Bottle cmd,reply; 165 | cmd.addVocab32("vel"); 166 | cmd.addVocab32("acc"); 167 | cmd.addInt32(j); 168 | cmd.addFloat64(acc); 169 | if (rpcPort.write(cmd,reply)) 170 | return true; 171 | else 172 | return false; 173 | } 174 | 175 | /**********************************************************/ 176 | bool fakeMotorDeviceClient::stop(int j) 177 | { 178 | if (!configured) 179 | return false; 180 | 181 | Bottle cmd,reply; 182 | cmd.addVocab32("vel"); 183 | cmd.addVocab32("stop"); 184 | cmd.addInt32(j); 185 | if (rpcPort.write(cmd,reply)) 186 | return true; 187 | else 188 | return false; 189 | } 190 | 191 | 192 | 193 | -------------------------------------------------------------------------------- /python/python_simworld_control.py: -------------------------------------------------------------------------------- 1 | import collections 2 | import yarp 3 | 4 | yarp.Network.init() # Initialise YARP 5 | 6 | 7 | class WorldController: 8 | """Class for controlling iCub simulator via its RPC world port.""" 9 | 10 | def __init__(self): 11 | self._rpc_client = yarp.RpcClient() 12 | self._port_name = "/WorldController-" + str(id(self)) + "/commands" 13 | self._rpc_client.open(self._port_name) 14 | self._rpc_client.addOutput("/icubSim/world") 15 | 16 | # A dictionary to track simulator object IDs for all types of objects 17 | self._sim_ids_counters = collections.defaultdict(lambda:0) 18 | 19 | # A sequence to track internal object IDs. This list stores tuples (object type, simulator id) 20 | # so that outside one does not have to remember the type of object. 21 | self._objects = [ ] 22 | 23 | def _execute(self, cmd): 24 | """Execute an RPC command, returning obtained answer bottle.""" 25 | ans = yarp.Bottle() 26 | self._rpc_client.write(cmd, ans) 27 | return ans 28 | 29 | def _is_success(self, ans): 30 | """Check if RPC call answer Bottle indicates successfull execution.""" 31 | return ans.size() == 1 and ans.get(0).asVocab() == 27503 # Vocab for '[ok]' 32 | 33 | def _prepare_del_all_command(self): 34 | """Prepare the "world del all" command bottle.""" 35 | result = yarp.Bottle() 36 | result.clear() 37 | map(result.addString, [ "world", "del", "all" ]) 38 | return result 39 | 40 | def _prepare_create_command(self, obj, size, location, colour): 41 | """Prepare an RPC command for creating an object in the simulator environment. 42 | 43 | See Simulator Readme section 'Object Creation' 44 | 45 | Parameters: 46 | obj - object type string. 'sph', 'box', 'cyl' 'ssph', 'sbox' or 'scyl'. 47 | size - list of values specifying the size of an object. Parameters depend on object type: 48 | (s)box: [ x, y, z ] 49 | (s)sph: [ radius ] 50 | (s)cyl: [ radius, length ] 51 | location - coordinates of the object location, [ x, y, z ] 52 | colour - object colour in RGB (normalised), [ r, g, b ] 53 | Returns: 54 | yarp.Bottle with the command, ready to be sent to the rpc port of the simulator 55 | 56 | """ 57 | result = yarp.Bottle() 58 | result.clear() 59 | 60 | map(result.addString, ["world", "mk", obj]) 61 | map(result.addDouble, size) 62 | map(result.addDouble, location) 63 | map(result.addDouble, colour) 64 | 65 | return result 66 | 67 | def _prepare_move_command(self, obj, obj_id, location): 68 | """Prepare the "world set " command bottle.""" 69 | result = yarp.Bottle() 70 | result.clear() 71 | 72 | map(result.addString, ["world", "set", obj]) 73 | result.addInt(obj_id) 74 | map(result.addDouble, location) 75 | 76 | return result 77 | 78 | def _prepare_rot_command(self, obj, obj_id, rotation): 79 | """Prepare the "world rot " command bottle.""" 80 | result = yarp.Bottle() 81 | result.clear() 82 | 83 | map(result.addString, ["world", "rot", obj]) 84 | result.addInt(obj_id) 85 | map(result.addDouble, rotation) 86 | 87 | return result 88 | 89 | def _prepare_get_command(self, obj, obj_id): 90 | """Prepare the "world get " command bottle.""" 91 | result = yarp.Bottle() 92 | result.clear() 93 | 94 | map(result.addString, ["world", "get", obj]) 95 | result.addInt(obj_id) 96 | 97 | return result 98 | 99 | def create_object(self, obj, size, location, colour): 100 | """Create an object of a specified type, size, location and colour, returning internal object ID or -1 on error.""" 101 | cmd = self._prepare_create_command(obj, size, location, colour) 102 | 103 | if self._is_success(self._execute(cmd)): 104 | obj_sim_id = self._sim_ids_counters[obj] + 1 # iCub simulator IDs start from 1 105 | 106 | # Update the counters 107 | self._sim_ids_counters[obj] += 1 108 | self._objects.append((obj, obj_sim_id)) 109 | 110 | # Internal object IDs are shared among all types of objects and start from 0; 111 | # they are essentially indices of the self._objects sequence 112 | return len(self._objects) - 1 113 | else: 114 | return -1 # error 115 | 116 | def move_object(self, obj_id, location): 117 | """Move an object specified by the internal id to another location.""" 118 | obj_desc = self._objects[obj_id] 119 | return self._is_success(self._execute(self._prepare_move_command(obj_desc[0], obj_desc[1], location))) 120 | 121 | def rotate_object(self, obj_id, rotation): 122 | """Rotate an object specified by the internal id giving its absolute rotation.""" 123 | obj_desc = self._objects[obj_id] 124 | return self._is_success(self._execute(self._prepare_rot_command(obj_desc[0], obj_desc[1], rotation))) 125 | 126 | def get_object_location(self, obj_id): 127 | """Obtain the object location from the simulator. Returns None on failure.""" 128 | obj_desc = self._objects[obj_id] 129 | result = self._execute(self._prepare_get_command(obj_desc[0], obj_desc[1])) 130 | if result.size() == 3: 131 | return [ result.get(i).asDouble() for i in xrange(3) ] # 3-element list with xyz coordinates 132 | else: 133 | return None # An error occured 134 | 135 | def del_all(self): 136 | """Delete all objects from the simultor""" 137 | result = self._is_success(self._execute(self._prepare_del_all_command())) 138 | 139 | if result: 140 | # Clear the counters 141 | self._sim_ids_counters.clear() 142 | del self._objects[:] 143 | 144 | return result 145 | 146 | def __del__(self): 147 | try: 148 | if self._rpc_client <> None: 149 | self.del_all() 150 | self._rpc_client.close() 151 | del self._rpc_client 152 | except AttributeError: 153 | pass 154 | -------------------------------------------------------------------------------- /doc/doc.dox: -------------------------------------------------------------------------------- 1 | // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-/** 2 | 3 | /** 4 | @page icub_tutorials Tutorials 5 | 6 | \section sec_cpp C++ Tutorials 7 | 8 | We recommend you first get accustomed with YARP. There are a list of tutorials available in this page: YARP Tutorials. In particular you should at least follow 9 | 10 | - Getting started with YARP ports 11 | - Specialized RPC ports 12 | - Writing modules, the RFModule 13 | 14 | The source code of the following tutorials is hosted in the icub-tutorials repository. 15 | 16 | A list of tutorials on various topics from basic compilation, visual processing, to control in the operational space. 17 | 18 | - \ref icub_hello_icub - how to write the very first program 19 | - \ref icub_motor_control_tutorial - a tutorial on how to use the motor interfaces 20 | - \ref icub_impedance_control_tutorial - a tutorial on how to use the joint level torque/impedance interface 21 | - \ref icub_basic_image_processing - a tutorial on a basic image processing 22 | - \ref icub_periodic_thread - a tutorial on how to write a control loop using threads 23 | - \ref icub_cartesian_interface - a tutorial on how to control a robot's limb in the operational space 24 | - \ref icub_gaze_interface - a tutorial on how to control the robot gaze through a YARP interface 25 | - \ref icub_anyrobot_cartesian_interface - how to control a generic robot through the cartesian interface 26 | - iCub Basic Demos - you might also examine some of our demos built on the basic components above. 27 | 28 | Advanced integration with the YARP framework require that you get used to the ResourceFinder: 29 | - Tutorials on the ResourceFinder 30 | 31 | 32 | \section sec_lib_cpp C++ tutorials related to specific libraries 33 | 34 | Online documentation for the libraries is available here: 35 | iCub libraries. 36 | 37 | The following are some specific tutorials. 38 | 39 | \subsection sec_ikin_lib_cpp iKin library 40 | 41 | - src/iKin/iCubLimbsFwKin/main.cpp - a tutorial on how to use \ref iKin for forward kinematics of iCub limbs 42 | - src/iKin/fwInvKinematics/main.cpp - a tutorial on how to directly use \ref iKin to cope with forward/inverse kinematics problems 43 | - src/iKin/onlineSolver/main.cpp - a tutorial on how to solve online inverse kinematics of a generic robot limb 44 | - src/iKin/genericChainController/main.cpp - a tutorial on how to control a generic kinematic chain 45 | 46 | Online documentation is available here: 47 | iKin online documentation. 48 | 49 | \subsection sec_idyn_lib_cpp iDyn library 50 | 51 | - \ref idyn_introduction - a short introduction to the \ref iDyn library 52 | - \ref idyn_one_chain_tutorial - how to compute torques in a single chain, using \ref iDyn library 53 | 54 | Online documentation is available here: 55 | iDyn online documentation. 56 | 57 | \subsection sec_ml_lib_cpp Learning Machine Library 58 | 59 | These tutorials demonstrate how to interface with the learningMachine library. The easiest 60 | way is to use machine learning classes directly. The library also provides C++ abstract interfaces 61 | that facilitate migration to their learning methods; their use is illustrated in the intermediate and 62 | advanced tutorials. You should check all examples and choose the method that better suits your application. 63 | 64 | - Basic tutorial. Use learning machines directly: src/learningMachines/lmlibdirect.cpp 65 | - Intermediate tutotial. Use learning machines thourgh abstract interfaces: src/learningMachines/lmlibindirect.cpp 66 | - Advanced tutorial. Use learning machines through abstract interfaces and factory: src/learningMachines/lmlibportable.cpp 67 | 68 | Online documentation is available here: 69 | learningMachines online documentation. 70 | 71 | \subsection sec_other_lis_cpp Other libraries 72 | 73 | - src/ctrlLib/onlinePTuner/main.cpp - a tutorial on online tuning of a P controller 74 | - src/optimization/calibReference/main.cpp - a tutorial on how to calibrate two reference frames from a cloud of matching points 75 | - src/perceptiveModels/tutorial_perceptiveModels.cpp - a tutorial on how to detect contacts of the fingers with external object 76 | - src/actionPrimitives/tutorial_actionPrimitives.cpp - a tutorial on how to sequence simple actions in fingers so as Cartesian space. 77 | 78 | \section sec_python Python Tutorials 79 | 80 | These tutorials assume you installed the YARP and iCub bindings for Python. Follow installation instructions reported in the manual at Interoperability with other languages and middleware. 81 | 82 | - \ref icub_python_basic_motor - a tutorial on how to use the motor interfaces in Python 83 | - \ref icub_python_simworld_control - a tutorial on how to control the iCub simulator from Python via its RPC interface 84 | - \ref icub_python_imaging - a tutorial on processing YARP images in Python 85 | 86 | \section sec_module_doc Module Documentation Tutorials 87 | 88 | - \ref xml_module_documentation - tutorial on how to write the XML documentation of a module 89 | 90 | * This file can be edited at \link doc/doc.dox 91 | * 92 | **/ 93 | -------------------------------------------------------------------------------- /doc/motor_control_tutorial.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | @page icub_motor_control_tutorial Getting accustomed with motor interfaces 4 | 5 | This tutorial shows how to get interfaces to control the arm. 6 | 7 | \section sec_intro Introduction 8 | 9 | Motor control in YARP is done through a motor control device. 10 | 11 | The code in this tutorial is a good example to start playing with motor control. This example uses a remoted device driver which exports 12 | exactly the same interfaces of the actual device driver (only initialization is different). The 13 | main difference between the two is in terms of efficiency, if you're developing a low-level 14 | control loop you might want to be local, on the other hand a simple sporadic position control 15 | can be effected through the remote device driver. Let's assume we have started the server side 16 | already (e.g. the simulator). 17 | 18 | \section sec_initialization Initialization 19 | 20 | Let us assume the simulator is up and running. 21 | 22 | In practice, we start by preparing a set of configuration parameters: 23 | 24 | \code 25 | Property options; 26 | options.put("device", "remote_controlboard"); 27 | options.put("local", "/test/client"); //local port names 28 | options.put("remote", "/icubSim/right_arm"); //where we connect to 29 | \endcode 30 | 31 | where the ''local'' and ''remote'' parameters are used to set the port names for the 32 | connection to the server side. In our example this means that the process will create the 33 | following (local) ports: 34 | 35 | \code 36 | /test/client/state:i 37 | /test/client/command:o 38 | /test/client/rpc:o 39 | \endcode 40 | 41 | And will automatically connect them to the server, on the following (remote) ports 42 | (we assume here that the server created those ports correctly): 43 | 44 | \code 45 | /icubSim/right_arm/state:o 46 | /icubSim/right_arm/command:i 47 | /icubSim/right_arm/rpc:i 48 | \endcode 49 | 50 | To create the driver we use the PolyDriver: 51 | 52 | \code 53 | PolyDriver robotDevice(options); 54 | if (!robotDevice.isValid()) { 55 | printf("Device not available. Here are the known devices:\n"); 56 | printf("%s", Drivers::factory().toString().c_str()); 57 | return 1; 58 | } 59 | \endcode 60 | 61 | which instantiates the driver. If the parameters are correct and the server side has been prepared the local driver also connects to the remote one. The next step is to get a set of interfaces (pointers to) to work with, for example: 62 | 63 | \code 64 | IPositionControl *pos; 65 | IVelocityControl *vel; 66 | IEncoders *enc; 67 | \endcode 68 | 69 | and: 70 | 71 | \code 72 | robotDevice.view(pos); 73 | robotDevice.view(vel); 74 | robotDevice.view(enc); 75 | \endcode 76 | 77 | if everything went ok the pointers are valid pointers (check it!). For example: 78 | \code 79 | if (pos==0) { 80 | printf("Error getting IPositionControl interface.\n"); 81 | return 1; 82 | } 83 | \endcode 84 | 85 | We can thus start with checking how many axes we can control by doing: 86 | 87 | \code 88 | int jnts = 0; 89 | pos->getAxes(&jnts); 90 | \endcode 91 | 92 | Let's create some vectors for later use: 93 | \code 94 | Vector tmp; 95 | Vector encoders; 96 | Vector command_position; 97 | Vector command_velocity; 98 | 99 | tmp.resize(jnts); 100 | encoders.resize(jnts); 101 | ... 102 | \endcode 103 | 104 | Let's now initialize the controllers and start up the amplifiers: 105 | 106 | \code 107 | /* we need to set reference accelerations used to generate the velocity */ 108 | /* profile, here 50 degrees/sec^2 */ 109 | int i; 110 | for (i = 0; i < jnts; i++) { 111 | tmp[i] = 50.0; 112 | } 113 | pos->setRefAccelerations(tmp.data()); 114 | \endcode 115 | 116 | if needed we can check the position of our axes by doing: 117 | 118 | \code 119 | enc->getEncoders(encoders.data()); 120 | \endcode 121 | 122 | which reads the values of the motor encoders into a vector of doubles of size ''jnts''. 123 | 124 | Important: check the return value of this function to ensure that communication 125 | with the robot is sane and that the function can return updated values. 126 | 127 | \code 128 | bool ret=enc->getEncoders(encoders.data()); 129 | if (!ret) 130 | { 131 | fprintf(stderr, "Error reading encoders, check connectivity with the robot\n"); 132 | } 133 | else 134 | { 135 | /* use encoders */ 136 | } 137 | \endcode 138 | 139 | \section sec_position Position control 140 | 141 | First do: 142 | \code 143 | int i; 144 | for (i = 0; i < jnts; i++) { 145 | tmp[i] = 40.0; 146 | } 147 | pos->setRefSpeeds(tmp.data()); 148 | \endcode 149 | 150 | which sets the reference speed for all axes to 40 degrees per second. The reference speed is used 151 | to interpolate the trajectory between the current and the desired position. 152 | 153 | Now you can do: 154 | \code 155 | bool ok = pos->positionMove(command_position.data()); 156 | \endcode 157 | 158 | where ''command_position'' is the desired position. This starts a movement of all axes toward 159 | the desired position. 160 | 161 | \section sec_velocity Velocity control 162 | 163 | To send velocity commands do: 164 | \code 165 | bool ok = vel->velocityMove(command_velocity.data()); 166 | \endcode 167 | 168 | which accelerates all axes to the velocity described by the vector ''command_velocity'' (in degrees per second). 169 | 170 | \section sec_closing Closing the device 171 | 172 | When you are done with controlling the robot don't forget to close the device: 173 | \code 174 | robotDevice.close(); 175 | \endcode 176 | 177 | \section sec_more_interfaces More interfaces 178 | 179 | To read more about the interfaces that are available in YARP read the online documentation: 180 | http://eris.liralab.it/yarpdoc/namespaceyarp_1_1dev.html 181 | 182 | \section sec_code Code 183 | 184 | See code in: src/motorControlBasic/tutorial_arm.cpp 185 | 186 | **/ 187 | -------------------------------------------------------------------------------- /doc/periodic_thread.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | @page icub_periodic_thread The PeriodicThread Class 4 | 5 | In this tutorial we show how to write a control loop using threads. Before you read this 6 | tutorial you should at least get accustomed with motor interfaces as described in 7 | \ref icub_motor_control_tutorial (material in \ref icub_basic_image_processing might 8 | also be useful). 9 | 10 | \section sec_intro Introduction 11 | 12 | Often, you want to perform different tasks in parallel or with a given periodicity. Modern 13 | operating system provides support for writing threads. Threads are functions that are 14 | executed by the CPU in parallel. The use of threads is recommended in machines 15 | with multiple cores. 16 | 17 | YARP supports threads with two main classes: yarp::os::Thread and yarp::os::PeriodicThread. 18 | 19 | In this tutorial we show how to use the yarp::os::PeriodicThread class to write a control loop 20 | with a certain periodicity. 21 | 22 | \section sec_yourownthread Writing your own thread 23 | 24 | To write a thread you have to derive a new class from yarp::os::PeriodicThread. In doing so 25 | you can to explicitly call the PeriodicThread constructor and pass to it the periodicity of 26 | the thread (in seconds). 27 | 28 | \code 29 | class ControlThread: public yarp::os::PeriodicThread 30 | { 31 | public: 32 | ControlThread(double period):PeriodicThread(period) 33 | {...} 34 | }; 35 | \endcode 36 | 37 | We add three methods to the class: run(), threadInit() and threadRelease(). The run() 38 | function will be called periodically every "period" seconds. threadInit() is called once 39 | when the thread starts (before run is executed). Finally the thread executes threadRelease() 40 | when closing. 41 | 42 | We would like to write a thread that periodically check the encoders and alternates two velocity 43 | commands. This thread will control for example the head of the robot. We need a PolyDriver to store 44 | the robot device (see \ref icub_motor_control_tutorial), and an instance of IEncoders and 45 | IVelocityControl. 46 | 47 | \code 48 | PolyDriver dd; 49 | IVelocityControl *ivel; 50 | IEncoders *iencs; 51 | \endcode 52 | 53 | We also prepare two vectors of to store the encoders and one to store the commands we will send 54 | to the controller. A counter will allow us to alternate between two commands. 55 | 56 | \code 57 | Vector encoders; 58 | Vector commands; 59 | int count; 60 | \endcode 61 | 62 | Now we can implement the bool threadInit() function. First we will create an instance of the 63 | polydriver and configure it to connect to the head. 64 | 65 | \code 66 | bool threadInit() 67 | { 68 | /* initialize here variables */ 69 | printf("ControlThread:starting\n"); 70 | 71 | Property options; 72 | options.put("device", "remote_controlboard"); 73 | options.put("local", "/local/head"); 74 | 75 | /* substitute icubSim with icub for use with the real robot */ 76 | options.put("remote", "/icubSim/head"); 77 | 78 | dd.open(options); 79 | \endcode 80 | 81 | at this point we have to check that the device is valid and really offers the interfaces we 82 | need: 83 | 84 | \code 85 | dd.view(iencs); 86 | dd.view(ivel); 87 | 88 | if ( (!iencs) || (!ivel) ) 89 | return false; 90 | \endcode 91 | 92 | Note: it is important to return false if something does not go as expected in the initialization. This will prevent the thread from running. 93 | 94 | Now we can get the number of joints, resize the vectors and initialize the acceleration of the 95 | motors: 96 | 97 | \code 98 | int joints; 99 | 100 | iencs->getAxes(&joints); 101 | 102 | encoders.resize(joints); 103 | commands.resize(joints); 104 | 105 | commands=10000; 106 | 107 | ivel->setRefAccelerations(commands.data()); 108 | count=0; 109 | return true; 110 | } /* the function threadInit() finishes here */ 111 | \endcode 112 | 113 | It is now time to implement the run function. This function is the thread's body, and perform 114 | the real control task: 115 | 116 | \code 117 | void run() 118 | { 119 | /* reads encoders */ 120 | iencs->getEncoders(encoders.data()); 121 | 122 | count++; 123 | 124 | /* alternates two commands */ 125 | if (count%2) 126 | commands=5; 127 | else 128 | commands=-5; 129 | 130 | ivel->velocityMove(commands.data()); 131 | 132 | /* print something at each iteration so we see when the thread runs */ 133 | printf("."); 134 | } 135 | \endcode 136 | 137 | When the thread will quit, we have to make sure that we stop the head and release used resources. 138 | We put this code in threadRelease(): 139 | 140 | \code 141 | void threadRelease() 142 | { 143 | printf("ControlThread:stopping the robot\n"); 144 | 145 | ivel->stop(); 146 | 147 | dd.close(); 148 | 149 | printf("Done, goodbye from ControlThread\n"); 150 | } 151 | \endcode 152 | 153 | \section sec_runningthethread Running the thread 154 | Now that we have implemented our thread we can run it. This can be done easily in a few steps: 155 | 156 | Create an instance of the thread, we use here 4s of period: 157 | \code 158 | ControlThread myThread(4.0); 159 | \endcode 160 | 161 | Start the thread: 162 | \code 163 | myThread.start(); 164 | \endcode 165 | 166 | The thread is created and start to execute initThread(); if the latter returns true, the 167 | thread executes run() periodically. In the meanwhile the main thread can perform other 168 | operations. In this example we do not do anything smarter than just sitting idle for 169 | 10 seconds: 170 | 171 | \code 172 | bool done=false; 173 | double startTime=Time::now(); 174 | const int SOME_TIME=10; 175 | while(!done) 176 | { 177 | if ((Time::now()-startTime)>SOME_TIME) 178 | done=true; 179 | } 180 | \endcode 181 | 182 | 183 | To stop the thread we call: 184 | 185 | \code 186 | myThread.stop(); 187 | \endcode 188 | 189 | stop() blocks and waits that the thread performs the last run() function, and threadRelease(), 190 | after which we can safely return. 191 | 192 | Full working code of this tutorial is available here: src/periodicThread/tutorial_periodic_thread.cpp 193 | * 194 | **/ 195 | -------------------------------------------------------------------------------- /doc/iDyn_oneChain.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | @page idyn_one_chain_tutorial Computation of torques in a single chain, using iDyn 4 | 5 | In this tutorial we show how to compute the joint torques in a single kinematic chain, using the iDyn library. 6 | Before you read this tutorial you should at least get accustomed with the iDyn library, and the basic of Newton-Euler algorithm (reading the \ref idyn_introduction "short introduction to iDyn" might be useful). 7 | 8 | Remember to include YARP and iCub as packages, and to link iDyn library in CMakeLists.txt: 9 | \code 10 | FIND_PACKAGE(YARP) 11 | FIND_PACKAGE(ICUB) 12 | ... 13 | TARGET_LINK_LIBRARIES(${PROJECTNAME} iDyn ${YARP_LIBRARIES}) 14 | \endcode 15 | note that in this case we need to look for the packages because we are developing a tutorial which is outside iCub project. 16 | 17 | Using iDyn classes, vectors and matrices are used all the times. 18 | \code 19 | #include 20 | #include 21 | #include 22 | ... 23 | using namespace yarp::sig; 24 | using namespace iCub::iDyn; 25 | \endcode 26 | 27 | We start creating a iDyn::iCubArmDyn, that is a chain with arm and torso links. 28 | If you are familiar with iKin, it is exactly the same limb type, with the same kinematic properties of a iCubArm. 29 | Of course, iCubArmDyn is more complete, since it basically inherits the kinematics and provides the dynamics. 30 | \code 31 | iCubArmDyn armTorsoDyn("right"); 32 | \endcode 33 | 34 | By default in iCubArmDyn the torso is blocked, as it is in its corresponding iKin limb, iCubArm; 35 | we unblock it, so we can also use the torso links. Note that releasing the links we have N==DOF (i.e. the number of links equal to the number of degrees of freedom). 36 | \code 37 | armTorsoDyn.releaseLink(0); 38 | armTorsoDyn.releaseLink(1); 39 | armTorsoDyn.releaseLink(2); 40 | \endcode 41 | 42 | Before any computation, we must prepare the necessary variables and put them in the chain model. 43 | First of all, we must set the joint angles in the limb model: in dynamics, we need joint positions, velocities and accelerations. 44 | 45 | \code 46 | Vector q(armTorsoDyn.getN()); q.zero(); 47 | Vector dq(q); Vector ddq(q); 48 | \endcode 49 | 50 | In iCub we only have position measurements from the encoders, so in order to avoid noisy values on velocity and acceleration, we suggest to apply at least a linear filter on the velocity data computed by differentiation, and a quadratic filter on the acceleration data. 51 | As a suggestion, look for adaptWinPolyEstimator in the ctrlLib, or have a look at the module WholeBodyTorqueObserver. 52 | Note that if the joint angles exceed the joint limits, their value is saturated to their min/max value automatically (as in iKin). At the moment there's not such a limitation for velocities and accelerations. It must be reminded that the joint limits could be different in the real robot, so it is always a good practice, when using the robot, to call the alignJointsBound method, which queries the robot for the real joints limits. 53 | 54 | \code 55 | armTorsoDyn.setAng(q); 56 | armTorsoDyn.setDAng(dq); 57 | armTorsoDyn.setD2Ang(ddq); 58 | \endcode 59 | 60 | In order to initialize the kinematic phase of Newton-Euler algorithm, we need the kinematic informations to be set in the base of the chain, where the base is intended to be the first link (first w.r.t the Denavit Hartenberg convention for describing the links, so the torso base in iCubArmDyn). If the robot is fixed and not moving, everything can be simply set to zero: but remember to provide the gravity information! 61 | 62 | \code 63 | Vector w0(3); Vector dw0(3); Vector ddp0(3); 64 | w0=dw0=ddp0=0.0; ddp0[2]=9.81; 65 | \endcode 66 | 67 | The end-effector external wrench (force/moment), is necessary to initialize the wrench phase of the Newton-Euler algorithm. 68 | If the robot is not touching anything, the external wrench is zero by default. If there's contact... look at the \ref WrenchObserver module. 69 | 70 | \code 71 | Vector Fend(3); Fend.zero(); 72 | Vector Muend(Fend); 73 | \endcode 74 | 75 | 76 | We can now start using iDyn. 77 | In order to perform the kinematic and wrench computation in a chain, we must first tell the limb that we want to use the iDyn::RecursiveNewtonEuler library, by calling a "prepare" method. 78 | In fact, before using our method, a new chain, parallel to the iDynChain, is built: it is a OneChainNewtonEuler, and it is made of OneLinkNewtonEuler virtual links, each having its "real" corresponding iDynLink; in addition, a BaseLink and a FinalLink elements are added to the chain, defining the entry points for the kinematic and wrench phases of Newton-Euler computations. 79 | 80 | The reason of the existence of the prepareNewtonEuler method is twofold: 81 | -# one can use a iDynLimb without dynamics, since it includes the kinematics of iKin 82 | for example one may only want to set joint angles and compute the jacobian 83 | -# one may prefer to use its own dynamic library to compute internal forces/torques, and not 84 | necessarily the Newton-Euler one we provide. 85 | 86 | When calling prepareNewtonEuler(), we must specify the type of computations we want to perform, which are basically: 87 | 88 | - STATIC: we only need q (joints positions) 89 | - DYNAMIC: we need q,dq,ddq (joints position, velocity, acceleration) --> default 90 | 91 | \code 92 | armTorsoDyn.prepareNewtonEuler(DYNAMIC); 93 | \endcode 94 | 95 | Finally we perform the computations, by calling a single function which performs autonomously: 96 | -# Initialization of both phases 97 | -# Kinematic Phase 98 | -# Wrench Phase 99 | 100 | \code 101 | armTorsoDyn.computeNewtonEuler(w0,dw0,ddp0,Fend,Muend); 102 | \endcode 103 | 104 | We can now retrieve the results: forces, moments and joint torques. In particular, F and M are 3xN matrices, where the i-th column is the i-th force/moment (each having 3 components), whereas Tau is a 1xN vector, since the joint torque is a scalar number. 105 | 106 | \code 107 | Matrix F = armTorsoDyn.getForces(); 108 | Matrix Mu = armTorsoDyn.getMoments(); 109 | Vector Tau = armTorsoDyn.getTorques(); 110 | \endcode 111 | 112 | 113 | Full working code of this tutorial is available here: 114 | \code 115 | icub-tutorials/src/iDyn/oneChainDynamics/main.cpp 116 | \endcode 117 | 118 | \author Serena Ivaldi 119 | * 120 | **/ 121 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/solver/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 3 | * Author: Ugo Pattacini 4 | * email: ugo.pattacini@iit.it 5 | * Permission is granted to copy, distribute, and/or modify this program 6 | * under the terms of the GNU General Public License, version 2 or any 7 | * later version published by the Free Software Foundation. 8 | * 9 | * A copy of the license can be found at 10 | * http://www.robotcub.org/icub/license/gpl.txt 11 | * 12 | * This program is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 15 | * Public License for more details 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace std; 27 | using namespace yarp::os; 28 | using namespace iCub::iKin; 29 | 30 | /** 31 | * This class inherits from the CartesianSolver super-class 32 | * implementing the solver 33 | */ 34 | class fakeRobotCartesianSolver : public CartesianSolver 35 | { 36 | protected: 37 | /** 38 | * This particular method serves to describe all the device 39 | * drivers used by the solver to access the robot, along with 40 | * the kinematic structure of the links. 41 | * 42 | * @param options The parameters required by the super-class to 43 | * get configured. 44 | * @return A pointer to the descriptor or NULL if something wrong happens. 45 | */ 46 | PartDescriptor *getPartDesc(Searchable &options) 47 | { 48 | if (!options.check("CustomKinFile")) 49 | { 50 | cout<<"Error: \"CustomKinFile\" option is missing!"<isValid()) 70 | { 71 | cout<<"Error: invalid links parameters!"<lmb=limb; // a pointer to the iKinLimb 79 | p->chn=limb->asChain(); // the associated iKinChain object 80 | p->cns=NULL; // any further (linear) constraints on the joints other than the bounds? This requires some more effort 81 | p->prp.push_back(optPart); // attach the options to open the device driver of the fake part 82 | p->rvs.push_back(false); // it may happen that the motor commands to be sent are in reversed order wrt the order of kinematics links (e.g. the iCub torso); if so put here "true" 83 | p->num=1; // only one device driver for the whole limb (see below) 84 | 85 | // whenever a limb is actuated resorting to more than one device 86 | // (e.g. for iCub: torso+arm), the following applies: 87 | // 88 | // p->prp.push_back(optDevice_1); 89 | // p->prp.push_back(optDevice_2); 90 | // p->rvs.push_back(true); 91 | // p->rvs.push_back(false); 92 | // p->num=2; 93 | 94 | return p; 95 | } 96 | 97 | public: 98 | /**********************************************************/ 99 | fakeRobotCartesianSolver(const string &name) : CartesianSolver(name) { } 100 | }; 101 | 102 | class SolverModule: public RFModule 103 | { 104 | protected: 105 | CartesianSolver *solver; 106 | 107 | public: 108 | /**********************************************************/ 109 | SolverModule() : solver(NULL) { } 110 | 111 | /**********************************************************/ 112 | bool configure(ResourceFinder &rf) 113 | { 114 | if (!rf.check("name")) 115 | { 116 | cout<<"Error: \"name\" option is missing!"<open(config)) 129 | { 130 | delete solver; 131 | return false; 132 | } 133 | 134 | return true; 135 | } 136 | 137 | /************************************************************************/ 138 | bool interruptModule() 139 | { 140 | if (solver!=NULL) 141 | solver->interrupt(); 142 | 143 | return true; 144 | } 145 | 146 | /**********************************************************/ 147 | bool close() 148 | { 149 | delete solver; 150 | return true; 151 | } 152 | 153 | /**********************************************************/ 154 | double getPeriod() 155 | { 156 | return 1.0; 157 | } 158 | 159 | /**********************************************************/ 160 | bool updateModule() 161 | { 162 | if (solver->isClosed() || solver->getTimeoutFlag()) 163 | return false; 164 | else 165 | return true; 166 | } 167 | }; 168 | 169 | /**********************************************************/ 170 | int main(int argc, char *argv[]) 171 | { 172 | Network yarp; 173 | if (!yarp.checkNetwork()) 174 | { 175 | cout<<"Error: yarp server does not seem available"< 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | using namespace yarp::dev; 13 | using namespace yarp::sig; 14 | using namespace yarp::os; 15 | 16 | int main(int argc, char *argv[]) 17 | { 18 | Network yarp; 19 | 20 | Property params; 21 | params.fromCommand(argc, argv); 22 | 23 | if (!params.check("robot")) 24 | { 25 | fprintf(stderr, "Please specify the name of the robot\n"); 26 | fprintf(stderr, "--robot name (e.g. icub)\n"); 27 | return 1; 28 | } 29 | std::string robotName=params.find("robot").asString(); 30 | std::string remotePorts="/"; 31 | remotePorts+=robotName; 32 | remotePorts+="/right_arm"; 33 | 34 | std::string localPorts="/test/client"; 35 | 36 | Property options; 37 | options.put("device", "remote_controlboard"); 38 | options.put("local", localPorts); //local port names 39 | options.put("remote", remotePorts); //where we connect to 40 | 41 | // create a device 42 | PolyDriver robotDevice(options); 43 | if (!robotDevice.isValid()) { 44 | printf("Device not available. Here are the known devices:\n"); 45 | printf("%s", Drivers::factory().toString().c_str()); 46 | return 1; 47 | } 48 | 49 | IPositionControl *pos; 50 | IEncoders *encs; 51 | IControlMode *ictrl; 52 | IInteractionMode *iint; 53 | IImpedanceControl *iimp; 54 | ITorqueControl *itrq; 55 | 56 | 57 | bool ok; 58 | ok = robotDevice.view(pos); 59 | ok = ok && robotDevice.view(encs); 60 | ok = ok && robotDevice.view(ictrl); 61 | ok = ok && robotDevice.view(iimp); 62 | ok = ok && robotDevice.view(itrq); 63 | ok = ok && robotDevice.view(iint); 64 | 65 | if (!ok) { 66 | printf("Problems acquiring interfaces\n"); 67 | return 1; 68 | } 69 | 70 | int nj=0; 71 | pos->getAxes(&nj); 72 | Vector encoders; 73 | Vector torques; 74 | Vector command; 75 | Vector tmp; 76 | int control_mode; 77 | yarp::dev::InteractionModeEnum interaction_mode; 78 | encoders.resize(nj); 79 | torques.resize(nj); 80 | tmp.resize(nj); 81 | command.resize(nj); 82 | 83 | int i; 84 | for (i = 0; i < nj; i++) { 85 | tmp[i] = 50.0; 86 | } 87 | pos->setRefAccelerations(tmp.data()); 88 | 89 | for (i = 0; i < nj; i++) { 90 | tmp[i] = 10.0; 91 | pos->setRefSpeed(i, tmp[i]); 92 | //SET THE IMPEDANCE: 93 | //0.111 is the stiffness coefficient. units: Nm/deg 94 | //0.014 is the damping coefficient. units: Nm/(deg/s) 95 | //0 is the additional torque offset 96 | //WARNING: playing with this value may lead to undamped oscillatory behaviours. 97 | //when you raise the stiffness, you should increase the damping coefficient accordingly. 98 | iimp->setImpedance(i, 0.111, 0.014); 99 | } 100 | 101 | //pos->setRefSpeeds(tmp.data())) 102 | 103 | //fisrst zero all joints 104 | // 105 | command=0; 106 | //now set the shoulder to some value 107 | command[0]=-50; 108 | command[1]=20; 109 | command[2]=-10; 110 | command[3]=50; 111 | pos->positionMove(command.data()); 112 | 113 | /* 114 | bool done=false; 115 | 116 | while(!done) 117 | { 118 | pos->checkMotionDone(&done); 119 | Time::delay(0.1); 120 | } 121 | */ 122 | 123 | int times=0; 124 | while(true) 125 | { 126 | times++; 127 | if (times%2) 128 | { 129 | // set the elbow joint only in compliant mode 130 | ictrl->setControlMode(3,VOCAB_CM_POSITION); 131 | iint->setInteractionMode(3,VOCAB_IM_COMPLIANT); 132 | // set new reference positions 133 | command[0]=-50; 134 | command[1]=20; 135 | command[2]=-10; 136 | command[3]=60; 137 | } 138 | else 139 | { 140 | // set the elbow joint in stiff mode 141 | ictrl->setControlMode(3,VOCAB_CM_POSITION); 142 | iint->setInteractionMode(3,VOCAB_IM_STIFF); 143 | // set new reference positions 144 | command[0]=-20; 145 | command[1]=40; 146 | command[2]=-10; 147 | command[3]=30; 148 | } 149 | 150 | pos->positionMove(command.data()); 151 | 152 | int count=50; 153 | while(count--) 154 | { 155 | Time::delay(0.1); 156 | encs->getEncoders(encoders.data()); 157 | itrq->getTorques(torques.data()); 158 | printf("Encoders: %+5.1lf %+5.1lf %+5.1lf %+5.1lf ", encoders[0], encoders[1], encoders[2], encoders[3]); 159 | printf("Torques: %+5.1lfNm %+5.1lfNm %+5.1lfNm %+5.1lfNm ", torques[0], torques[1], torques[2], torques[3]); 160 | printf("Control: "); 161 | for (i = 0; i < 4; i++) 162 | { 163 | ictrl->getControlMode(i, &control_mode); 164 | iint->getInteractionMode(i, &interaction_mode); 165 | switch (control_mode) 166 | { 167 | case VOCAB_CM_IDLE: printf("IDLE "); break; 168 | case VOCAB_CM_POSITION: printf("POSITION "); break; 169 | case VOCAB_CM_POSITION_DIRECT: printf("POSITION DIRECT "); break; 170 | case VOCAB_CM_VELOCITY: printf("VELOCITY "); break; 171 | case VOCAB_CM_MIXED: printf("MIXED POS/VEL"); break; 172 | case VOCAB_CM_TORQUE: printf("TORQUE "); break; 173 | default: 174 | case VOCAB_CM_UNKNOWN: printf("UNKNOWN "); break; 175 | } 176 | } 177 | printf("\n"); 178 | printf("Interaction: "); 179 | for (i = 0; i < 4; i++) 180 | { 181 | switch (interaction_mode) 182 | { 183 | case VOCAB_IM_COMPLIANT: printf("(COMPLIANT MODE)"); break; 184 | case VOCAB_IM_STIFF: printf("(STIFF MODE)"); break; 185 | default: 186 | case VOCAB_CM_UNKNOWN: printf("(UNKNOWN) "); break; 187 | } 188 | } 189 | printf("\n"); 190 | } 191 | } 192 | 193 | robotDevice.close(); 194 | 195 | return 0; 196 | } 197 | -------------------------------------------------------------------------------- /src/iDyn/oneChainWithSensor/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright: 2010-2011 RobotCub Consortium 3 | * Author: Serena Ivaldi 4 | * CopyPolicy: Released under the terms of the GNU GPL v2.0. 5 | **/ 6 | 7 | // 8 | // The most basic example on the use of iDyn when the chain is provided with 9 | // a force/torque sensor, and the sensor is placed inside the chain. 10 | // 11 | // Author: Serena Ivaldi - 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | using namespace yarp::sig; 23 | using namespace iCub::iDyn; 24 | using namespace iCub::skinDynLib; 25 | 26 | // useful print functions 27 | // print a matrix nicely 28 | void printMatrix(const string &s, const Matrix &m) 29 | { 30 | cout< default 85 | arm->prepareNewtonEuler(STATIC); 86 | 87 | // now the chain must be updated with all the necessary informations!!!! 88 | 89 | // 1) the joint angles (pos, vel, acc) of the limbs: we can take this values from the encoders.. 90 | Vector q(arm->getN()); q.zero(); 91 | Vector dq(q); Vector ddq(q); 92 | // now we set the joints values 93 | // note: if the joint angles exceed the joint limits, their value is saturated to the min/max automatically 94 | // note: STATIC computations only use q, while DYNAMIC computations use q,dq,ddq.. but we set everything 95 | arm->setAng(q); 96 | arm->setDAng(dq); 97 | arm->setD2Ang(ddq); 98 | 99 | // 2) the inertial information to initialize the kinematic phase 100 | // this information must be set at the base of the chain, where the base is intended to be the first link (first.. in the Denavit 101 | // Hartenberg convention) 102 | // note: if nothing is moving, it can be simply zero, but ddp0 must provide the gravity information :) 103 | Vector w0(3); Vector dw0(3); Vector ddp0(3); 104 | w0=dw0=ddp0=0.0; ddp0[2]=9.81; 105 | // here we set the kinematic information on the arm chain 106 | arm->initKinematicNewtonEuler(w0,dw0,ddp0); 107 | 108 | // 3) the wrench (force/moment) measured by the FT sensor placed inside the chain 109 | Vector Fsens(3); Fsens.zero(); 110 | Fsens=2.0; // or whatever number you like... here its is 2 (Newton) per force component.. 111 | Vector Musens(3); Musens.zero(); 112 | Musens=0.1; //or whatever number you like... here its is 0.1 (Newton per meter) per moment component.. 113 | 114 | // finally we set the measurements on the sensor, and we start the computation... 115 | // this method autonomously perform a kinematic phase (to this we set the kinematics information before..) 116 | // then the wrench phase is "split": from the sensor back to the base, the computations are performed in the 117 | // backward sense, while from sensor to the end-effector in the forward sense. 118 | armWSensorSolver->computeFromSensorNewtonEuler(Fsens,Musens); 119 | 120 | // then we can retrieve our results... 121 | // forces moments and torques 122 | Matrix F = arm->getForces(); 123 | Matrix Mu = arm->getMoments(); 124 | Vector Tau = arm->getTorques(); 125 | 126 | // now print the information 127 | 128 | cout<<"iCubArmNoTorsoDyn has "<getN()<<" links."<getAng()); 134 | 135 | cout<<"\n\n"; 136 | 137 | printVector("Measured Force in the FT sensor ",Fsens); 138 | printVector("Measured Moment in the FT sensor ",Musens); 139 | 140 | cout<<"\n\n"; 141 | 142 | printMatrix("Forces ",F); 143 | printMatrix("Moments ",Mu); 144 | printVector("Torques ",Tau); 145 | 146 | // remember to delete everything before exiting :) 147 | cout<<"\nDestroying sensor..."<yarpmanager. 14 | 15 | \section sec_xml_head Heading 16 | 17 | The XML file should start with the following lines that contain the instructions 18 | on how to produce the Doxygen documentation from the XML itself: 19 | 20 | \code{.xml} 21 | 22 | 23 | \endcode 24 | 25 | The description is all contained within the module section: 26 | 27 | \code{.xml} 28 | 29 | ... 30 | 31 | \endcode 32 | 33 | \section sec_xml_general General Information on the Module 34 | 35 | A bunch of dedicated key account for describing general information on the module 36 | as in the following: 37 | 38 | \code{.xml} 39 | 40 | rpcIdl 41 | 42 | 44 | icub-module 45 | 46 | Example to show idl usage. 47 | 48 | Released under the terms of the GNU GPL v2.0 49 | 50 | 1.0 51 | 52 | 53 | Implements a trivial server that provides access to an integer. 54 | You can use html, for example to add a link to the yarp page and Doxygen tags. 55 | 56 | @subsection sec-intro Introduction 57 | This is the introduction. 123 58 | 59 | @subsection sec-details More stuff 60 | This is a detailed description, etc. 61 | 62 | 63 | 64 | 65 | Lorenzo Natale 66 | 67 | \endcode 68 | 69 | \section sec_xml_parameters Arguments 70 | 71 | Here comes the arguments section that lists down all the command-line parameters 72 | of the module: 73 | 74 | \code{.xml} 75 | 76 | 77 | name 78 | from 79 | context 80 | 81 | \endcode 82 | 83 | \section sec_xml_data Data 84 | 85 | The data section contains information on the input and output ports of the 86 | module: 87 | 88 | \code{.xml} 89 | 90 | 91 | 92 | 93 | 94 | Image 95 | 96 | 97 | /rpcIdl/img:i 98 | 99 | 103 | no 104 | 105 | 107 | no 108 | Feed images to rpcIdl using this port. 109 | 110 | 111 | 112 | Image 113 | /rpcIdl/img:o 114 | Output image. 115 | 116 | 117 | 118 | yarp::os::Bottle 119 | /rpcIdl/target:o 120 | 121 | Outputs the x-y location of whatever is the output of the module: 122 | - int i value of y 123 | - int j value of x 124 | 125 | 126 | 127 | \endcode 128 | 129 | \section sec_xml_services Services 130 | 131 | The services section describes the Thrift services exposed by the module 132 | according to the 133 | Interface Description Language rules contained in the rpcIdl.thrift file: 134 | 135 | \code{.xml} 136 | 137 | 138 | IRpcServer 139 | IRpcServer.thrift 140 | /rpcIdl/server 141 | Command port 142 | 143 | 144 | 145 | IRpcServer 146 | IRpcServer.thrift 147 | /rpcIdl/client 148 | Control remote object 149 | 150 | 151 | \endcode 152 | 153 | \section sec_xml_dependencies Dependencies 154 | 155 | In the final sections dependencies and development we find further 156 | lists containing useful information to run the module. 157 | 158 | \code{.xml} 159 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 |
178 | 179 | 180 | \endcode 181 | 182 | \section sec_xml_conclusion Conclusion 183 | 184 | Concluding, once processed the final Doxygen documentation will look like as in 185 | \ref rpcIdl. 186 | 187 | To install the description file in the location where all the xml are supposed to 188 | be just rely on the following cmake directive: 189 | 190 | \code 191 | yarp_install(FILES rpcIdl.thrift DESTINATION ${ICUB_MODULES_INSTALL_DIR}) 192 | \endcode 193 | 194 | * 195 | **/ 196 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/fakeMotorDevice/src/fakeMotorDeviceServer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 3 | * Author: Ugo Pattacini 4 | * email: ugo.pattacini@iit.it 5 | * Permission is granted to copy, distribute, and/or modify this program 6 | * under the terms of the GNU General Public License, version 2 or any 7 | * later version published by the Free Software Foundation. 8 | * 9 | * A copy of the license can be found at 10 | * http://www.robotcub.org/icub/license/gpl.txt 11 | * 12 | * This program is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 15 | * Public License for more details 16 | */ 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | using namespace std; 24 | using namespace yarp::os; 25 | using namespace yarp::dev; 26 | using namespace yarp::sig; 27 | using namespace iCub::ctrl; 28 | 29 | /**********************************************************/ 30 | fakeMotorDeviceServer::fakeMotorDeviceServer() : PeriodicThread(0.01) 31 | { 32 | motors=NULL; 33 | configured=false; 34 | } 35 | 36 | /**********************************************************/ 37 | bool fakeMotorDeviceServer::open(Searchable &config) 38 | { 39 | printf("Opening Fake Motor Device Server ...\n"); 40 | 41 | string local=config.check("local",Value("/fakeyServer")).asString(); 42 | int Ts=config.check("Ts",Value(10)).asInt32(); 43 | 44 | statePort.open(local+"/state:o"); 45 | cmdPort.open(local+"/cmd:i"); 46 | rpcPort.open(local+"/rpc"); 47 | rpcPort.setReader(*this); 48 | 49 | // the part is composed of three rotational joints 50 | // whose bounds are given in degrees just below 51 | Matrix lim(3,2); 52 | lim(0,0)=-180.0; lim(0,1)=180.0; // joint 0 53 | lim(1,0)=-90.0; lim(1,1)=90.0; // joint 1 54 | lim(2,0)=-45.0; lim(2,1)=45.0; // joint 2 55 | 56 | Vector q0; 57 | for (int i=0; iget().length(),0.0); 65 | 66 | setPeriod((double)Ts/1000.0); 67 | start(); 68 | 69 | configured=true; 70 | 71 | printf("Fake Motor Device Server successfully open\n"); 72 | return true; 73 | } 74 | 75 | /**********************************************************/ 76 | bool fakeMotorDeviceServer::close() 77 | { 78 | printf("Closing Fake Motor Device Server ...\n"); 79 | 80 | if (isRunning()) 81 | stop(); 82 | 83 | mtx.lock(); 84 | PeriodicThread::stop(); 85 | mtx.unlock(); 86 | 87 | statePort.interrupt(); 88 | cmdPort.interrupt(); 89 | rpcPort.interrupt(); 90 | 91 | statePort.close(); 92 | cmdPort.close(); 93 | rpcPort.close(); 94 | 95 | if (motors!=NULL) 96 | { 97 | delete motors; 98 | motors=NULL; 99 | } 100 | 101 | configured=false; 102 | 103 | printf("Fake Motor Device Server successfully closed\n"); 104 | return true; 105 | } 106 | 107 | /**********************************************************/ 108 | void fakeMotorDeviceServer::run() 109 | { 110 | lock_guard lg(mtx); 111 | if (Bottle *cmd=cmdPort.read(false)) 112 | { 113 | if ((size_t)cmd->size()>=vel.length()) 114 | for (size_t i=0; iget(i).asFloat64(); 116 | } 117 | 118 | statePort.prepare()=motors->integrate(vel); 119 | statePort.write(); 120 | } 121 | 122 | /**********************************************************/ 123 | bool fakeMotorDeviceServer::read(ConnectionReader &connection) 124 | { 125 | Bottle cmd,reply; 126 | cmd.read(connection); 127 | 128 | mtx.lock(); 129 | 130 | int codeIF=cmd.get(0).asVocab32(); 131 | int codeMethod=cmd.get(1).asVocab32(); 132 | 133 | if (codeIF==Vocab32::encode("lim")) 134 | { 135 | if (codeMethod==Vocab32::encode("get")) 136 | { 137 | int axis=cmd.get(2).asInt32(); 138 | double min,max; 139 | if (getLimits(axis,&min,&max)) 140 | { 141 | reply.addVocab32("ack"); 142 | reply.addFloat64(min); 143 | reply.addFloat64(max); 144 | } 145 | } 146 | } 147 | else if (codeIF==Vocab32::encode("enc")) 148 | { 149 | if (codeMethod==Vocab32::encode("axes")) 150 | { 151 | int ax; 152 | if (getAxes(&ax)) 153 | { 154 | reply.addVocab32("ack"); 155 | reply.addInt32(ax); 156 | } 157 | } 158 | } 159 | else if (codeIF==Vocab32::encode("vel")) 160 | { 161 | if (codeMethod==Vocab32::encode("move")) 162 | { 163 | int axis=cmd.get(2).asInt32(); 164 | double sp=cmd.get(3).asFloat64(); 165 | if (velocityMove(axis,sp)) 166 | reply.addVocab32("ack"); 167 | } 168 | else if (codeMethod==Vocab32::encode("acc")) 169 | { 170 | int axis=cmd.get(2).asInt32(); 171 | double acc=cmd.get(3).asFloat64(); 172 | if (setRefAcceleration(axis,acc)) 173 | reply.addVocab32("ack"); 174 | } 175 | else if (codeMethod==Vocab32::encode("stop")) 176 | { 177 | int axis=cmd.get(2).asInt32(); 178 | if (stop(axis)) 179 | reply.addVocab32("ack"); 180 | } 181 | } 182 | 183 | if (reply.size()==0) 184 | reply.addVocab32("nack"); 185 | 186 | mtx.unlock(); 187 | 188 | if (ConnectionWriter *returnToSender=connection.getWriter()) 189 | reply.write(*returnToSender); 190 | 191 | return true; 192 | } 193 | 194 | /**********************************************************/ 195 | bool fakeMotorDeviceServer::getLimits(int axis, double *min, double *max) 196 | { 197 | if (!configured) 198 | return false; 199 | 200 | if ((axis<(int)motors->get().length()) && (min!=NULL) && (max!=NULL)) 201 | { 202 | Matrix lim=motors->getLim(); 203 | *min=lim(axis,0); *max=lim(axis,1); 204 | return true; 205 | } 206 | else 207 | return false; 208 | } 209 | 210 | /**********************************************************/ 211 | bool fakeMotorDeviceServer::getAxes(int *ax) 212 | { 213 | if (!configured) 214 | return false; 215 | 216 | if (ax!=NULL) 217 | { 218 | *ax=(int)motors->get().length(); 219 | return true; 220 | } 221 | else 222 | return false; 223 | } 224 | 225 | /**********************************************************/ 226 | bool fakeMotorDeviceServer::velocityMove(int j, double sp) 227 | { 228 | if (!configured) 229 | return false; 230 | 231 | if ((size_t)j 9 | #include 10 | #include 11 | #include "iCub/learningMachine/MachinePortable.h" 12 | #include "iCub/learningMachine/MachineCatalogue.h" 13 | #include "iCub/learningMachine/TransformerPortable.h" 14 | #include "iCub/learningMachine/TransformerCatalogue.h" 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #define MIN(a, b) ((a < b) ? a : b) 23 | 24 | #define NO_TRAIN 4000 25 | #define NO_TEST 8000 26 | #define NOISE_MIN -0.05 27 | #define NOISE_MAX 0.05 28 | 29 | 30 | using namespace iCub::learningmachine; 31 | using namespace yarp::os; 32 | using namespace yarp::sig; 33 | using namespace yarp::math; 34 | 35 | // taken from LWPR example code 36 | double cross(double x1, double x2) { 37 | x1 *= x1; 38 | x2 *= x2; 39 | double a = std::exp(-10 * x1); 40 | double b = std::exp(-50 * x2); 41 | double c = 1.25 * std::exp(-5 * (x1 + x2)); 42 | return (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c); 43 | } 44 | 45 | double sin2d(double x1, double x2) { 46 | return std::sin(x1 + x2); 47 | } 48 | 49 | void elementProd(const Vector& v1, Vector& v2) { 50 | for(size_t i = 0; i < MIN(v1.size(), v2.size()); i++) { 51 | v2[i] = v1[i] * v2[i]; 52 | } 53 | } 54 | 55 | Vector elementDiv(const Vector& v, double d) { 56 | Vector ret(v.size()); 57 | for(size_t i = 0; i < v.size(); i++) { 58 | ret[i] = (d == 0.) ? v[i] : v[i] / d; 59 | } 60 | return ret; 61 | } 62 | 63 | 64 | std::pair createSample() { 65 | std::pair sample; 66 | sample.first.resize(2); 67 | sample.second.resize(2); 68 | sample.first[0] = Rand::scalar(-1, +1); 69 | sample.first[1] = Rand::scalar(-1, +1); 70 | sample.second[0] = sin2d(sample.first[0], sample.first[1]); 71 | sample.second[1] = cross(sample.first[0], sample.first[1]); 72 | return sample; 73 | } 74 | 75 | /* 76 | * This example shows how LearningMachine classes can be used in a indirect 77 | * manner in your code. In this context, this means that the YARP configuration 78 | * mechanism is used and instances are of the abstract base type. This 79 | * facilitates easy migration to other learning methods. Please see all 80 | * direct/indirect/portable examples to have an idea which method suits your 81 | * application best. 82 | * 83 | * Please keep in mind that the purpose is to demonstrate how to interface with 84 | * the learningMachine library. The synthetic data used in this example is 85 | * utterly useless. 86 | */ 87 | 88 | int main(int argc, char** argv) { 89 | int nrf = 250; 90 | Vector trainMSE(2); 91 | Vector testMSE(2); 92 | Vector noise_min(2); 93 | Vector noise_max(2); 94 | // train + test time for transformer and machine 95 | double tic; 96 | double trtrtime = 0.0; 97 | double trtetime = 0.0; 98 | double mctrtime = 0.0; 99 | double mctetime = 0.0; 100 | 101 | if(argc > 1) sscanf(argv[1], "%d", &nrf); 102 | 103 | // initialize catalogue of machine factory 104 | registerMachines(); 105 | // initialize catalogue of transformers 106 | registerTransformers(); 107 | 108 | std::cout << "LearningMachine library example (portable)" << std::endl; 109 | 110 | // create Regularized Least Squares learner 111 | std::string name("RLS"); 112 | MachinePortable mp = MachinePortable("RLS"); 113 | Property p; 114 | p.put("dom", Value(nrf)); 115 | p.put("cod", Value(2)); 116 | p.put("lambda", Value(0.5)); 117 | mp.getWrapped().configure(p); 118 | std::cout << "Learner:" << std::endl << mp.getWrapped().getInfo() << std::endl; 119 | 120 | // create Random Feature transformer 121 | TransformerPortable tp = TransformerPortable("RandomFeature"); 122 | p.clear(); 123 | p.put("dom", Value(2)); 124 | p.put("cod", Value(nrf)); 125 | p.put("gamma", Value(16.0)); 126 | tp.getWrapped().configure(p); 127 | std::cout << "Transformer:" << std::endl << tp.getWrapped().getInfo() << std::endl; 128 | 129 | 130 | // create and feed training samples 131 | noise_min = NOISE_MIN; 132 | noise_max = NOISE_MAX; 133 | 134 | 135 | trainMSE = 0.0; 136 | for(int i = 0; i < NO_TRAIN; i++) { 137 | // create a new training sample 138 | std::pair sample = createSample(); 139 | 140 | // add some noise to output for training 141 | Vector noisyOutput = sample.second + Rand::vector(noise_min, noise_max); 142 | 143 | // transform input using RF 144 | tic = yarp::os::SystemClock::nowSystem(); 145 | Vector transInput = tp.getWrapped().transform(sample.first); 146 | trtrtime += (yarp::os::SystemClock::nowSystem() - tic); 147 | 148 | // make prediction before feeding full sample 149 | tic = yarp::os::SystemClock::nowSystem(); 150 | Prediction prediction = mp.getWrapped().predict(transInput); 151 | 152 | // train on complete sample with noisy output 153 | mp.getWrapped().feedSample(transInput, noisyOutput); 154 | mctrtime += (yarp::os::SystemClock::nowSystem() - tic); 155 | 156 | Vector diff = prediction.getPrediction() - sample.second; 157 | elementProd(diff, diff); 158 | trainMSE = trainMSE + diff; 159 | } 160 | trainMSE = elementDiv(trainMSE, NO_TRAIN); 161 | std::cout << "Train MSE: " << trainMSE.toString() << std::endl; 162 | std::cout << "Train Transformer Time per Sample: " << (trtrtime / NO_TRAIN) << std::endl; 163 | std::cout << "Train Machine Time per Sample: " << (mctrtime / NO_TRAIN) << std::endl; 164 | std::cout << "Combined Time per Sample: " << ((trtrtime + mctrtime) / NO_TRAIN) << std::endl; 165 | 166 | std::cout << std::endl; 167 | std::cout << "Saving machine portable to file 'mp.txt'..."; 168 | bool ok = mp.writeToFile("mp.txt"); 169 | std::cout << ((ok) ? "ok!" : "failed :(") << std::endl; 170 | 171 | std::cout << "Saving transformer portable to file 'tp.txt'..."; 172 | ok = tp.writeToFile("tp.txt"); 173 | std::cout << ((ok) ? "ok!" : "failed :(") << std::endl; 174 | 175 | std::cout << "Loading machine portable from file 'mp.txt'..."; 176 | ok = mp.readFromFile("mp.txt"); 177 | std::cout << ((ok) ? "ok!" : "failed :(") << std::endl; 178 | 179 | std::cout << "Loading transformer portable from file 'tp.txt'..."; 180 | ok = tp.readFromFile("tp.txt"); 181 | std::cout << ((ok) ? "ok!" : "failed :(") << std::endl; 182 | std::cout << std::endl; 183 | 184 | // predict test samples 185 | testMSE = 0.; 186 | for(int i = 0; i < NO_TEST; i++) { 187 | // create a new testing sample 188 | std::pair sample = createSample(); 189 | 190 | // transform input using RF 191 | tic = yarp::os::SystemClock::nowSystem(); 192 | Vector transInput = tp.getWrapped().transform(sample.first); 193 | trtetime += (yarp::os::SystemClock::nowSystem() - tic); 194 | 195 | // make prediction 196 | tic = yarp::os::SystemClock::nowSystem(); 197 | Prediction prediction = mp.getWrapped().predict(transInput); 198 | mctetime += (yarp::os::SystemClock::nowSystem() - tic); 199 | Vector diff = prediction.getPrediction() - sample.second; 200 | elementProd(diff, diff); 201 | //std::cout << "Sample: " << sample.input << 202 | testMSE = testMSE + diff; 203 | } 204 | testMSE = elementDiv(testMSE, NO_TEST); 205 | std::cout << "Test MSE: " << testMSE.toString() << std::endl; 206 | std::cout << "Test Transformer Time per Sample: " << (trtetime / NO_TEST) << std::endl; 207 | std::cout << "Test Machine Time per Sample: " << (mctetime / NO_TEST) << std::endl; 208 | std::cout << "Combined Time per Sample: " << ((trtetime + mctetime) / NO_TEST) << std::endl; 209 | 210 | return 0; 211 | } 212 | 213 | 214 | -------------------------------------------------------------------------------- /doc/impedance_control_tutorial.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | @page icub_impedance_control_tutorial Getting accustomed with torque/impedance interfaces 4 | 5 | This tutorial shows how to use the following interfaces: 6 | 7 | yarp::dev::IControlMode 8 | 9 | yarp::dev::ITorqueControl 10 | 11 | yarp::dev::IImpedanceControl 12 | 13 | yarp::dev::IInteractionControl 14 | 15 | 16 | \section sec_intro Introduction 17 | Prerequisites, see \ref icub_motor_control_tutorial 18 | 19 | The code described in this tutorial can be found at src/motorControlImpedance/tutorial_arm_joint_impedance.cpp \n 20 | The example is based on the previous tutorial src/motorControlBasic/tutorial_arm.cpp 21 | 22 | The example shows how to open the ITorqueControl to show the arm joint torques and how to change the control mode of the elbow joint from position to impedance control. 23 | 24 | A detailed reference of the yarp interfaces used in this tutorial (IControlMode, ITorqueControl, IImpedanceControl, IInteractionControl) can be found in the YARP doxygen documentation and in the YARP Control Modes documentation. 25 | 26 | \section sec_initialization Initialization 27 | 28 | Let us assume the module wholeBodyDynamics is up and running. The wholeBodyDynamics module, based on the \ref iDyn library acts as a server and is responsibile for the calculation of the joint torques. 29 | In order to access to the torque values, our program has to declare and open the yarp ITorqueControl interface, in the same manner as you 30 | open the IPositionControl interface (see previous tutorial: \ref icub_motor_control_tutorial). 31 | We can also open the IControlMode interface (resposibile to set the control mode of each joint (position/velocity/impedance etc.), the IImpedanceControl interface, used to set the impedance parameters (joint stiffness/ joint damping etc) and the IInteractionControl interface, used to set a joint in stiff or compliant interaction mode. 32 | 33 | \code 34 | IPositionControl *pos; 35 | IEncoders *encs; 36 | IControlMode *ictrl; 37 | IInteractionMode *iint; 38 | IImpedanceControl *iimp; 39 | ITorqueControl *itrq; 40 | \endcode 41 | 42 | and: 43 | 44 | \code 45 | bool ok; 46 | ok = robotDevice.view(pos); 47 | ok = ok && robotDevice.view(encs); 48 | ok = ok && robotDevice.view(ictrl); 49 | ok = ok && robotDevice.view(iint); 50 | ok = ok && robotDevice.view(iimp); 51 | ok = ok && robotDevice.view(itrq); 52 | 53 | if (!ok) { 54 | printf("Problems acquiring interfaces\n"); 55 | return 0; 56 | } 57 | \endcode 58 | 59 | We can thus start with checking how many axes we can control by doing: 60 | 61 | \code 62 | int jnts = 0; 63 | pos->getAxes(&jnts); 64 | \endcode 65 | 66 | Let's create some vectors for later use: 67 | \code 68 | Vector tmp; 69 | Vector encoders; 70 | Vector command_position; 71 | Vector command_velocity; 72 | 73 | tmp.resize(jnts); 74 | encoders.resize(jnts); 75 | ... 76 | \endcode 77 | 78 | Let's now initialize the controllers and start up the amplifiers: 79 | 80 | \code 81 | // we need to set reference accelerations used to generate the velocity 82 | // profile, here 50 degrees/sec^2 83 | int i; 84 | for (i = 0; i < jnts; i++) { 85 | tmp[i] = 50.0; 86 | } 87 | pos->setRefAccelerations(tmp.data()); 88 | for (i = 0; i < jnts; i++) { 89 | tmp[i] = 10.0; 90 | pos->setRefSpeed(i, tmp[i]); 91 | } 92 | \endcode 93 | 94 | if needed we can check the position of our axes by doing: 95 | 96 | \code 97 | enc->getEncoders(encoders.data()); 98 | \endcode 99 | 100 | which reads the values of the motor encoders into a vector of doubles of size ''jnts''. 101 | 102 | \section sec_position Position control 103 | 104 | First do: 105 | \code 106 | int i; 107 | for (i = 0; i < jnts; i++) { 108 | tmp[i] = 40.0; 109 | } 110 | pos->setRefSpeeds(tmp.data()); 111 | \endcode 112 | 113 | which sets the reference speed for all axes to 40 degrees per second. The reference speed is used 114 | to interpolate the trajectory between the current and the desired position. 115 | 116 | Now you can do: 117 | \code 118 | bool ok = pos->positionMove(command_position.data()); 119 | \endcode 120 | 121 | where ''command_position'' is the desired position. This starts a movement of all axes toward 122 | the desired position. 123 | 124 | \section sec_torque_reading Reading the torques of a joint 125 | You can use the ITorqueControl interface to read the torque at a specific joint. 126 | \code 127 | Vector torques; 128 | torques.resize(nj); 129 | itrq->getTorques(torques.data()); 130 | \endcode 131 | 132 | \section sec_impedance Using different control modes: Position Control 133 | 134 | You can change the impedence parameters of a joint at any time: 135 | \code 136 | double stiffness = 0.111; // stiffness coefficient, units are Nm/deg 137 | double damping = 0.014; // damping coefficient, units are Nm/(deg/s) 138 | bool ok = iimp->setImpedance(joint, stiffness, damping); 139 | \endcode 140 | 141 | You can use IInteractionMode interface to change the interaction mode of a specific joint to compliant interaction mode: 142 | 143 | \code 144 | ictrl->setPositionMode(3); 145 | iint->setInteractionMode(3,VOCAB_IM_COMPLIANT); 146 | \endcode 147 | 148 | When in position control mode with compliant interaction mode, the low level controller (DSP) computes a refence torque Td as: 149 | 150 | \f[ T_d = K_{spring} * (q_d-q) - K_{damp} * \dot{q} + F_{offset} \f] 151 | 152 | which is tracked by the internal PID regulator. 153 | 154 | You can specify a new equilibrium position using the usual position interface, because the control mode of the 155 | joint is still VOCAB_CM_POSITION even if the interaction mode has changed: 156 | 157 | \code 158 | bool ok = pos->positionMove(command_position.data()); 159 | \endcode 160 | 161 | If you need to change the interaction mode back to stiff mode: 162 | 163 | \code 164 | iint->setInteractionMode(3,VOCAB_IM_COMPLIANT); 165 | \endcode 166 | 167 | An ImpedanceVelocity control mode is also available. 168 | In this control modality you can use vel->VelocityMove() command in order to command a velocity while mantaining the impedance specified with the iimp->setImpedance(joint, stiffness, damping, dorce_offset) command. 169 | 170 | \section sec_torque_control Using different control modes: Torque Control 171 | 172 | Torque control mode can be used to directly specify a reference torque at a joint. 173 | 174 | \code 175 | ictrl->setTorqueMode(jnt); 176 | 177 | double jnt_torque= 0.0; //expressed in Nm 178 | itrq->setRefTorque(jnt_torque); 179 | \endcode 180 | 181 | The gains of the PID controller resposibile of tracking the torque reference can be obtained using ITorqueControl::getTorquePid() method. 182 | 183 | NOTE: the reference torque that you can specify on the icub joints is limited (clamped) to a given value. You can check this value in the yarprobotinterface configuration files for your robot. 184 | 185 | NOTE2: near the hardware limits of the robot, the torque PID controlled is disabled, in order to prevent to command a torque againts the hardware limits. The PID controller is automatically re-enabled as soon as the joint is moved far the limits (You can view the value of the limites using the IControlLimits interface). 186 | 187 | NOTE3: during calibration/parking of the robot, yarprobotinterface automatically sets the control mode of all the joints to position control mode with stiff interaction mode. 188 | 189 | NOTE4: If your application changes the control mode/interaction mode of one joint, it's good practice to reset it to the previous modality when your application quits. 190 | 191 | \section sec_closing Closing the device 192 | 193 | When you are done with controlling the robot don't forget to close the device: 194 | \code 195 | robotDevice.close(); 196 | \endcode 197 | 198 | \section sec_more_interfaces More interfaces 199 | 200 | To read more about the interfaces that are available in YARP read the doxygen online documentation and in the YARP Control Modes documentation. 201 | 202 | \section sec_code Code 203 | 204 | See code in: src/motorControlImpedance/tutorial_arm_joint_impedance.cpp 205 | **/ 206 | -------------------------------------------------------------------------------- /src/iKin/fwInvKinematics/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @ingroup icub_tutorials 3 | * 4 | * \defgroup icub_fwInvKinematics Forward/Inverse Kinematics 5 | * Example 6 | * 7 | * A tutorial on how to use iKin library for forward/inverse 8 | * kinematics. 9 | * 10 | * \author Ugo Pattacini 11 | * 12 | * CopyPolicy: Released under the terms of GPL 2.0 or later 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | using namespace std; 27 | using namespace yarp::os; 28 | using namespace yarp::sig; 29 | using namespace yarp::math; 30 | using namespace iCub::ctrl; 31 | using namespace iCub::iKin; 32 | 33 | 34 | // this inherited class (re-)implements the iCub right arm 35 | // but shows at the same time how to handle any generic serial 36 | // link chain 37 | class genericRightArm : public iKinLimb 38 | { 39 | public: 40 | genericRightArm() : iKinLimb() 41 | { 42 | allocate("don't care"); 43 | } 44 | 45 | protected: 46 | virtual void allocate(const string &_type) 47 | { 48 | // the type is used to discriminate between left and right limb 49 | 50 | // you have to specify the rototranslational matrix H0 from the origin 51 | // to the root reference so as from iCub specs. 52 | Matrix H0(4,4); 53 | H0.zero(); 54 | H0(0,1)=-1.0; 55 | H0(1,2)=-1.0; 56 | H0(2,0)=1.0; 57 | H0(3,3)=1.0; 58 | setH0(H0); 59 | 60 | // define the links in standard D-H convention 61 | // A, D, alpha, offset(*), min theta, max theta 62 | pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD)); 63 | pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD)); 64 | pushLink(new iKinLink(-0.0233647, -0.1433, M_PI/2.0, -105.0*CTRL_DEG2RAD, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD)); 65 | pushLink(new iKinLink( 0.0, -0.10774, M_PI/2.0, -M_PI/2.0, -95.5*CTRL_DEG2RAD, 5.0*CTRL_DEG2RAD)); 66 | pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, 0.0, 160.8*CTRL_DEG2RAD)); 67 | pushLink(new iKinLink( 0.0, -0.15228, -M_PI/2.0, -105.0*CTRL_DEG2RAD, -37.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD)); 68 | pushLink(new iKinLink( 0.015, 0.0, M_PI/2.0, 0.0, 5.5*CTRL_DEG2RAD, 106.0*CTRL_DEG2RAD)); 69 | pushLink(new iKinLink( 0.0, -0.1373, M_PI/2.0, -M_PI/2.0, -90.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD)); 70 | pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -90.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD)); 71 | pushLink(new iKinLink( 0.0625, 0.016, 0.0, M_PI, -20.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD)); 72 | // (*) remind that offset is added to theta before computing the rototranslational matrix 73 | 74 | // usually the first three links which describes the torso kinematic come 75 | // as blocked, i.e. they do not belong to the set of arm's dof. 76 | blockLink(0,0.0); 77 | blockLink(1,0.0); 78 | blockLink(2,0.0); 79 | } 80 | }; 81 | 82 | 83 | int main() 84 | { 85 | // some useful variables 86 | Vector q0,qf,qhat,xf,xhat; 87 | 88 | // declare the limb 89 | genericRightArm genArm; 90 | 91 | // you can get the same result by creating an iCubArm object; 92 | // iKin already provides internally coded limbs for iCub, such as 93 | // iCubArm, iCubLeg, iCubEye, ..., along with the proper H0 matrix 94 | iCubArm libArm("right"); 95 | 96 | // get a chain on the arm; you can use arm object directly but then some 97 | // methods will not be available, such as the access to links through 98 | // [] or () operators. This prevent the user from adding/removing links 99 | // to iCub limbs as well as changing their properties too easily. 100 | // Anyway, arm object is affected by modifications on the chain. 101 | iKinChain *chain; 102 | if (true) // selector 103 | chain=genArm.asChain(); 104 | else 105 | chain=libArm.asChain(); 106 | 107 | // get initial joints configuration 108 | q0=chain->getAng(); 109 | 110 | // dump DOF bounds using () operators and set 111 | // a second joints configuration in the middle of the compact set. 112 | // Remind that angles are expressed in radians 113 | qf.resize(chain->getDOF()); 114 | for (unsigned int i=0; igetDOF(); i++) 115 | { 116 | double min=(*chain)(i).getMin(); 117 | double max=(*chain)(i).getMax(); 118 | qf[i]=(min+max)/2.0; 119 | 120 | // last joint set to 1 deg higher than the bound 121 | if (i==chain->getDOF()-1) 122 | qf[i]=max+1.0*CTRL_DEG2RAD; 123 | 124 | cout << "joint " << i << " in [" << CTRL_RAD2DEG*min << "," << CTRL_RAD2DEG*max 125 | << "] set to " << CTRL_RAD2DEG*qf[i] << endl; 126 | } 127 | 128 | // it is not allowed to overcome the bounds... 129 | // ...see the result 130 | qf=chain->setAng(qf); 131 | cout << "Actual joints set to " << (CTRL_RAD2DEG*qf).toString() << endl; 132 | // anyway user can disable the constraints checking by calling 133 | // the chain method setAllConstraints(false) 134 | 135 | // there are three links for the torso which do not belong to the 136 | // DOF set since they are blocked. User can access them through [] operators 137 | cout << "Torso blocked links at:" << endl; 138 | for (unsigned int i=0; igetN()-chain->getDOF(); i++) 139 | cout << CTRL_RAD2DEG*(*chain)[i].getAng() << " "; 140 | cout << endl; 141 | 142 | // user can unblock blocked links augumenting the number of DOF 143 | cout << "Unblocking the first torso joint... "; 144 | chain->releaseLink(0); 145 | cout << chain->getDOF() << " DOFs available" << endl; 146 | cout << "Blocking the first torso joint again... "; 147 | chain->blockLink(0); 148 | cout << chain->getDOF() << " DOFs available" << endl; 149 | 150 | // retrieve the end-effector pose. 151 | // Translational part is in meters. 152 | // Rotational part is in axis-angle representation 153 | xf=chain->EndEffPose(); 154 | cout << "Current arm end-effector pose: " << xf.toString() << endl; 155 | 156 | // go back to the starting joints configuration 157 | chain->setAng(q0); 158 | 159 | // instantiate a IPOPT solver for inverse kinematic 160 | // for both translational and rotational part 161 | iKinIpOptMin slv(*chain,IKINCTRL_POSE_FULL,1e-3,1e-6,100); 162 | 163 | // In order to speed up the process, a scaling for the problem 164 | // is usually required (a good scaling holds each element of the jacobian 165 | // of constraints and the hessian of lagrangian in norm between 0.1 and 10.0). 166 | slv.setUserScaling(true,100.0,100.0,100.0); 167 | 168 | // note how the solver called internally the chain->setAllConstraints(false) 169 | // method in order to relax constraints 170 | for (unsigned int i=0; igetN(); i++) 171 | { 172 | cout << "link " << i << ": " << 173 | (chain->getConstraint(i)?"constrained":"not-constrained") << endl; 174 | } 175 | 176 | // solve for xf starting from current configuration q0 177 | double t=SystemClock::nowSystem(); 178 | qhat=slv.solve(chain->getAng(),xf); 179 | double dt=SystemClock::nowSystem()-t; 180 | 181 | // in general the solved qf is different from the initial qf 182 | // due to the redundancy 183 | cout << "qhat: " << (CTRL_RAD2DEG*qhat).toString() << endl; 184 | 185 | // check how much we achieve our goal 186 | // note that the chain has been manipulated by the solver, 187 | // so it's already in the final configuration 188 | xhat=chain->EndEffPose(); 189 | cout << "Desired arm end-effector pose xf= " << xf.toString() << endl; 190 | cout << "Achieved arm end-effector pose K(qhat)= " << xhat.toString() << endl; 191 | cout << "||xf-K(qhat)||=" << norm(xf-xhat) << endl; 192 | cout << "Solved in " << dt << " [s]" << endl; 193 | 194 | return 0; 195 | } 196 | 197 | 198 | -------------------------------------------------------------------------------- /src/perceptiveModels/tutorial_perceptiveModels.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 3 | * Author: Ugo Pattacini 4 | * email: ugo.pattacini@iit.it 5 | * Permission is granted to copy, distribute, and/or modify this program 6 | * under the terms of the GNU General Public License, version 2 or any 7 | * later version published by the Free Software Foundation. 8 | * 9 | * A copy of the license can be found at 10 | * http://www.robotcub.org/icub/license/gpl.txt 11 | * 12 | * This program is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 15 | * Public License for more details 16 | */ 17 | 18 | /** 19 | \defgroup tutorial_perceptiveModels Tutorial on the perceptiveModels library 20 | 21 | Example module for the use of \ref PerceptiveModels library. 22 | 23 | Author: Ugo Pattacini 24 | 25 | CopyPolicy: Released under the terms of the GNU GPL v2.0. 26 | 27 | \section intro_sec Description 28 | This simple module gives a very brief introduction to the use 29 | of \ref PerceptiveModels framework for the case of detection of 30 | contacts of the fingers with external objects. 31 | 32 | Two types of models are envisaged: \n 33 | -# the springy approach that learns the relations between the motor 34 | joints and the distal fingers joints to detect discrepancies caused 35 | by external contacts; 36 | -# a direct approach that relies on the output of tactile 37 | sensors. 38 | 39 | The output of this module is printed out on the screen reporting the 40 | data as gathered from the sensors as well as a synthetic number which 41 | accounts for the contact detection: the larger it becomes, the stronger 42 | should be the force excerted by the external object. 43 | 44 | \section lib_sec Libraries 45 | - YARP libraries. 46 | - \ref PerceptiveModels library. 47 | 48 | \section parameters_sec Parameters 49 | --name \e name 50 | - specify the module name, which is \e percex by default. 51 | 52 | --robot \e robot 53 | - specify the robot name (e.g. \e icub or \e icubSim). 54 | 55 | --hand \e hand 56 | - specify which hand has to be used: \e hand can be \e left or 57 | \e right (by default). 58 | 59 | --model \e type 60 | - specify the type of perceptive model to be employed for 61 | sensing external contacts; possible choices are: \e springy 62 | and \e tactile. In case the \e springy model is used, a 63 | preliminary calibration phase is carried out. 64 | 65 | --finger \e finger 66 | - specify the finger to be used; possible choices are: \e thumb, 67 | \e index, \e middle, \e ring and \e little. 68 | 69 | \section tested_os_sec Tested OS 70 | Windows, Linux 71 | 72 | \author Ugo Pattacini 73 | */ 74 | 75 | #include 76 | #include 77 | #include 78 | 79 | #include 80 | #include 81 | #include 82 | #include 83 | #include 84 | #include 85 | 86 | #include 87 | #include 88 | 89 | using namespace std; 90 | using namespace yarp::os; 91 | using namespace yarp::dev; 92 | using namespace iCub::perception; 93 | 94 | 95 | /************************************************************************/ 96 | class ExampleModule: public RFModule 97 | { 98 | Model *model; 99 | PolyDriver driver; 100 | bool calibrate; 101 | string fingerName; 102 | 103 | IPositionControl *ipos; 104 | IEncoders *ienc; 105 | 106 | double min,max,*val; 107 | int joint; 108 | 109 | public: 110 | /************************************************************************/ 111 | ExampleModule() : calibrate(true) { } 112 | 113 | /************************************************************************/ 114 | bool configure(ResourceFinder &rf) 115 | { 116 | string name=rf.find("name").asString(); 117 | string robot=rf.find("robot").asString(); 118 | string hand=rf.find("hand").asString(); 119 | string modelType=rf.find("modelType").asString(); 120 | fingerName=rf.find("finger").asString(); 121 | 122 | if (fingerName=="thumb") 123 | joint=10; 124 | else if (fingerName=="index") 125 | joint=12; 126 | else if (fingerName=="middle") 127 | joint=14; 128 | else if (fingerName=="ring") 129 | joint=15; 130 | else if (fingerName=="little") 131 | joint=15; 132 | else 133 | { 134 | printf("unknown finger!\n"); 135 | return false; 136 | } 137 | 138 | Property driverOpt("(device remote_controlboard)"); 139 | driverOpt.put("remote","/"+robot+"/"+hand+"_arm"); 140 | driverOpt.put("local","/"+name); 141 | if (!driver.open(driverOpt)) 142 | return false; 143 | 144 | driver.view(ipos); 145 | driver.view(ienc); 146 | 147 | IControlLimits *ilim; 148 | driver.view(ilim); 149 | 150 | ilim->getLimits(joint,&min,&max); 151 | double margin=0.1*(max-min); 152 | min=min+margin; 153 | max=max-margin; 154 | val=&min; 155 | 156 | Property genOpt; 157 | genOpt.put("name",name+"/"+modelType); 158 | genOpt.put("robot",robot); 159 | genOpt.put("type",hand); 160 | genOpt.put("verbose",1); 161 | string general(genOpt.toString()); 162 | string thumb( "(thumb (name thumb))"); 163 | string index( "(index (name index))"); 164 | string middle("(middle (name middle))"); 165 | string ring( "(ring (name ring))"); 166 | string little("(little (name little))"); 167 | 168 | Property options((general+" "+thumb+" "+index+" "+middle+" "+ring+" "+little).c_str()); 169 | printf("configuring options: %s\n",options.toString().c_str()); 170 | 171 | if (modelType=="springy") 172 | model=new SpringyFingersModel; 173 | else if (modelType=="tactile") 174 | model=new TactileFingersModel; 175 | else 176 | { 177 | printf("unknown model type!\n"); 178 | return false; 179 | } 180 | 181 | if (model->fromProperty(options)) 182 | return true; 183 | else 184 | { 185 | delete model; 186 | return false; 187 | } 188 | } 189 | 190 | /************************************************************************/ 191 | bool close() 192 | { 193 | driver.close(); 194 | 195 | Property options; 196 | model->toProperty(options); 197 | printf("model options: %s\n",options.toString().c_str()); 198 | 199 | return true; 200 | } 201 | 202 | /************************************************************************/ 203 | double getPeriod() 204 | { 205 | return 0.1; 206 | } 207 | 208 | /************************************************************************/ 209 | bool updateModule() 210 | { 211 | if (calibrate) 212 | { 213 | Property options; 214 | options.put("finger",fingerName); 215 | model->calibrate(options); 216 | calibrate=false; 217 | 218 | ipos->setRefAcceleration(joint,1e9); 219 | if ((fingerName=="ring")||(fingerName=="little")) 220 | ipos->setRefSpeed(joint,60.0); 221 | else 222 | ipos->setRefSpeed(joint,30.0); 223 | 224 | ipos->positionMove(joint,*val); 225 | } 226 | else 227 | { 228 | if (iCub::perception::Node *finger=model->getNode(fingerName)) 229 | { 230 | Value data; finger->getSensorsData(data); 231 | Value out; finger->getOutput(out); 232 | printf("%s sensors data = %s; output = %s\n", 233 | finger->getName().c_str(),data.toString().c_str(),out.toString().c_str()); 234 | } 235 | 236 | double fb; ienc->getEncoder(joint,&fb); 237 | if (fabs(*val-fb)<5.0) 238 | { 239 | val==&min?val=&max:val=&min; 240 | ipos->positionMove(joint,*val); 241 | } 242 | } 243 | 244 | return true; 245 | } 246 | }; 247 | 248 | 249 | /************************************************************************/ 250 | int main(int argc, char *argv[]) 251 | { 252 | Network yarp; 253 | if (!yarp.checkNetwork()) 254 | { 255 | printf("YARP server not available!\n"); 256 | return 1; 257 | } 258 | 259 | ResourceFinder rf; 260 | rf.setDefault("name","percex"); 261 | rf.setDefault("robot","icub"); 262 | rf.setDefault("hand","right"); 263 | rf.setDefault("modelType","springy"); 264 | rf.setDefault("finger","index"); 265 | rf.configure(argc,argv); 266 | 267 | ExampleModule mod; 268 | return mod.runModule(rf); 269 | } 270 | -------------------------------------------------------------------------------- /src/anyRobotCartesianInterface/src/fakeMotorDevice/include/private/fakeMotorDeviceComponents.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia 3 | * Author: Ugo Pattacini 4 | * email: ugo.pattacini@iit.it 5 | * Permission is granted to copy, distribute, and/or modify this program 6 | * under the terms of the GNU General Public License, version 2 or any 7 | * later version published by the Free Software Foundation. 8 | * 9 | * A copy of the license can be found at 10 | * http://www.robotcub.org/icub/license/gpl.txt 11 | * 12 | * This program is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 15 | * Public License for more details 16 | */ 17 | 18 | #ifndef __FAKEMOTORDEVICECOMPONENTS_H__ 19 | #define __FAKEMOTORDEVICECOMPONENTS_H__ 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | /** 30 | * This class implements the server part of the fake motor device driver. 31 | * Only the used interface methods are actually implemented. 32 | */ 33 | class fakeMotorDeviceServer : public yarp::dev::DeviceDriver, 34 | public yarp::os::PeriodicThread, 35 | public yarp::os::PortReader, 36 | public yarp::dev::IControlLimits, 37 | public yarp::dev::IEncoders, 38 | public yarp::dev::IVelocityControl 39 | { 40 | protected: 41 | yarp::os::BufferedPort statePort; 42 | yarp::os::BufferedPort cmdPort; 43 | yarp::os::Port rpcPort; 44 | 45 | std::mutex mtx; 46 | 47 | iCub::ctrl::Integrator *motors; 48 | yarp::sig::Vector vel; 49 | bool configured; 50 | 51 | void run(); 52 | /** 53 | * This method decodes the requests forwarded by the client and 54 | * responds with corresponding replies. 55 | */ 56 | bool read(yarp::os::ConnectionReader &connection); 57 | 58 | public: 59 | fakeMotorDeviceServer(); 60 | bool open(yarp::os::Searchable &config); 61 | bool close(); 62 | 63 | //////////////////////////////////////////////////////////// 64 | //// 65 | //// IControlLimits Interface 66 | //// 67 | /**********************************************************/ 68 | bool getLimits(int axis, double *min, double *max); 69 | 70 | // not implemented 71 | /**********************************************************/ 72 | bool setLimits(int,double,double) { return false; } 73 | bool setVelLimits(int,double,double) { return false; } 74 | bool getVelLimits(int,double*,double*) { return false; } 75 | 76 | //////////////////////////////////////////////////////////// 77 | //// 78 | //// IEncoders Interface 79 | //// 80 | /**********************************************************/ 81 | bool getAxes(int *ax); 82 | 83 | // not implemented 84 | /**********************************************************/ 85 | bool getEncoder(int,double*) { return false; } 86 | bool getEncoders(double*) { return false; } 87 | bool resetEncoder(int) { return false; } 88 | bool resetEncoders() { return false; } 89 | bool setEncoder(int,double) { return false; } 90 | bool setEncoders(const double*) { return false; } 91 | bool getEncoderSpeed(int,double*) { return false; } 92 | bool getEncoderSpeeds(double*) { return false; } 93 | bool getEncoderAcceleration(int,double*) { return false; } 94 | bool getEncoderAccelerations(double*) { return false; } 95 | 96 | //////////////////////////////////////////////////////////// 97 | //// 98 | //// IVelocityControl Interface 99 | //// 100 | /**********************************************************/ 101 | bool velocityMove(int j, double sp); 102 | bool setRefAcceleration(int j, double acc); 103 | bool stop(int j); 104 | 105 | // not implemented 106 | /**********************************************************/ 107 | bool velocityMove(const int,const int*,const double*) { return false; } 108 | bool velocityMove(const double*) { return false; } 109 | bool setRefAccelerations(const int,const int*,const double*) { return false; } 110 | bool setRefAccelerations(const double*) { return false; } 111 | bool getRefAccelerations(const int,const int*,double*) { return false; } 112 | bool getRefAcceleration(int,double*) { return false; } 113 | bool getRefAccelerations(double*) { return false; } 114 | bool stop(const int,const int*) { return false; } 115 | bool stop() { return false; } 116 | }; 117 | 118 | /** 119 | * This class implements the client part of the fake motor device driver. 120 | * Only the used interface methods are actually implemented. 121 | */ 122 | class fakeMotorDeviceClient : public yarp::dev::DeviceDriver, 123 | public yarp::dev::IControlLimits, 124 | public yarp::dev::IEncoders, 125 | public yarp::dev::IVelocityControl 126 | { 127 | protected: 128 | class StatePort : public yarp::os::BufferedPort 129 | { 130 | fakeMotorDeviceClient *owner; 131 | void onRead(yarp::sig::Vector &encs) 132 | { 133 | if (owner!=NULL) 134 | { 135 | std::lock_guard lg(owner->mtx); 136 | owner->encs=encs; 137 | } 138 | } 139 | public: 140 | StatePort() : owner(NULL) { useCallback(); } 141 | void setOwner(fakeMotorDeviceClient *owner) { this->owner=owner; } 142 | }; 143 | 144 | StatePort statePort; 145 | yarp::os::BufferedPort cmdPort; 146 | yarp::os::RpcClient rpcPort; 147 | 148 | std::mutex mtx; 149 | 150 | yarp::sig::Vector encs; 151 | bool configured; 152 | 153 | friend class StatePort; 154 | 155 | public: 156 | fakeMotorDeviceClient(); 157 | bool open(yarp::os::Searchable &config); 158 | bool close(); 159 | 160 | //////////////////////////////////////////////////////////// 161 | //// 162 | //// IControlLimits Interface 163 | //// 164 | /**********************************************************/ 165 | bool getLimits(int axis, double *min, double *max); 166 | 167 | // not implemented 168 | /**********************************************************/ 169 | bool setLimits(int,double,double) { return false; } 170 | bool setVelLimits(int,double,double) { return false; } 171 | bool getVelLimits(int,double*,double*) { return false; } 172 | 173 | //////////////////////////////////////////////////////////// 174 | //// 175 | //// IEncoders Interface 176 | //// 177 | /**********************************************************/ 178 | bool getAxes(int *ax); 179 | bool getEncoders(double *encs); 180 | 181 | // not implemented 182 | /**********************************************************/ 183 | bool getEncoder(int,double*) { return false; } 184 | bool resetEncoder(int) { return false; } 185 | bool resetEncoders() { return false; } 186 | bool setEncoder(int,double) { return false; } 187 | bool setEncoders(const double*) { return false; } 188 | bool getEncoderSpeed(int,double*) { return false; } 189 | bool getEncoderSpeeds(double*) { return false; } 190 | bool getEncoderAcceleration(int,double*) { return false; } 191 | bool getEncoderAccelerations(double*) { return false; } 192 | 193 | //////////////////////////////////////////////////////////// 194 | //// 195 | //// IVelocityControl Interface 196 | //// 197 | /**********************************************************/ 198 | bool velocityMove(int j, double sp); 199 | bool setRefAcceleration(int j, double acc); 200 | bool stop(int j); 201 | 202 | // not implemented 203 | /**********************************************************/ 204 | bool velocityMove(const int,const int*,const double*) { return false; } 205 | bool velocityMove(const double*) { return false; } 206 | bool setRefAccelerations(const int,const int*,const double*) { return false; } 207 | bool setRefAccelerations(const double*) { return false; } 208 | bool getRefAccelerations(const int,const int*,double*) { return false; } 209 | bool getRefAcceleration(int,double*) { return false; } 210 | bool getRefAccelerations(double*) { return false; } 211 | bool stop(const int,const int*) { return false; } 212 | bool stop() { return false; } 213 | }; 214 | 215 | #endif 216 | 217 | 218 | --------------------------------------------------------------------------------