Skip to content

GitLab

Commit 629a6767 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

update for dynamic-graph-python v4 

- removed custom DYNAMIC_GRAPH_PYTHON_MODULE
- moved python lib to dynamic_graph/
- added required ${plugin}-python.hh
- use np.array everywhere
- updated tests
- created delay.hh
- removed main plugin
- switched to example-robot-data instead of talos-data
parent 6b496c9d
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 629a6767
......@@ -23,7 +23,6 @@ ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
SET(CUSTOM_HEADER_DIR "sot/talos_balance")
SET(CXX_DISABLE_WERROR TRUE)
SET(DOXYGEN_USE_MATHJAX YES)
SET(CMAKE_CXX_STANDARD 11)
# JRL-cmakemodule setup
INCLUDE(cmake/base.cmake)
......@@ -33,6 +32,7 @@ INCLUDE(cmake/python.cmake)
# Project definition
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX C)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
CHECK_MINIMAL_CXX_STANDARD(11 REQUIRED)
IF(INITIALIZE_WITH_NAN)
MESSAGE(STATUS "Initialize with NaN all the Eigen entries.")
......@@ -40,112 +40,23 @@ IF(INITIALIZE_WITH_NAN)
ENDIF(INITIALIZE_WITH_NAN)
# Project dependencies
SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework system regex)
#SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework system regex)
IF(BUILD_TESTING)
FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
ENDIF(BUILD_TESTING)
IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON()
STRING(REGEX REPLACE "-" "_" PROJECT_NAME_UNDERSCORE ${PROJECT_NAME})
SEARCH_FOR_BOOST_PYTHON(REQUIRED)
STRING(REGEX REPLACE "-" "_" PROJECT_NAME_UNDERSCORE ${PROJECT_NAME})
SET(PY_NAME ${PROJECT_NAME_UNDERSCORE})
ADD_PROJECT_DEPENDENCY(dynamic-graph-python REQUIRED
PKG_CONFIG_REQUIRES dynamic-graph-python)
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
ENDIF(BUILD_PYTHON_INTERFACE)
SEARCH_FOR_BOOST()
ADD_PROJECT_DEPENDENCY(sot-core REQUIRED PKG_CONFIG_REQUIRES sot-core)
ADD_PROJECT_DEPENDENCY(eiquadprog REQUIRED PKG_CONFIG_REQUIRES eiquadprog)
ADD_PROJECT_DEPENDENCY(talos_data)
ADD_REQUIRED_DEPENDENCY("parametric-curves")
MACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE MODULENAME SUBMODULENAME LIBRARYNAME TARGETNAME)
# By default the __init__.py file is installed.
SET(INSTALL_INIT_PY 1)
SET(SOURCE_PYTHON_MODULE "cmake/dynamic_graph/python-module-py.cc")
IF("${SUBMODULENAME}" STREQUAL "")
SET(FULLMODULENAME "${MODULENAME}")
ELSE("${SUBMODULENAME}" STREQUAL "")
SET(FULLMODULENAME "${MODULENAME}/${SUBMODULENAME}")
ENDIF("${SUBMODULENAME}" STREQUAL "")
# Check if there is optional parameters.
set(extra_macro_args ${ARGN})
list(LENGTH extra_macro_args num_extra_args)
if( ${num_extra_args} GREATER 0)
list(GET extra_macro_args 0 INSTALL_INIT_PY)
if( ${num_extra_args} GREATER 1)
list(GET extra_macro_args 1 SOURCE_PYTHON_MODULE)
endif(${num_extra_args} GREATER 1)
endif(${num_extra_args} GREATER 0)
IF(NOT DEFINED PYTHONLIBS_FOUND)
FINDPYTHON()
ELSEIF(NOT ${PYTHONLIBS_FOUND} STREQUAL "TRUE")
MESSAGE(FATAL_ERROR "Python has not been found.")
ENDIF()
SET(PYTHON_MODULE ${TARGETNAME})
# We need to set this policy to old to accept wrap target.
CMAKE_POLICY(PUSH)
IF(POLICY CMP0037)
CMAKE_POLICY(SET CMP0037 OLD)
ENDIF()
ADD_LIBRARY(${PYTHON_MODULE}
MODULE
${PROJECT_SOURCE_DIR}/${SOURCE_PYTHON_MODULE})
FILE(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/src/${FULLMODULENAME})
SET_TARGET_PROPERTIES(${PYTHON_MODULE}
PROPERTIES PREFIX ""
OUTPUT_NAME ${FULLMODULENAME}/wrap
)
CMAKE_POLICY(POP)
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} "-Wl,--no-as-needed")
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} ${LIBRARYNAME} ${PYTHON_LIBRARY})
TARGET_INCLUDE_DIRECTORIES(${PYTHON_MODULE} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS})
#
# Installation
#
SET(PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/${FULLMODULENAME})
INSTALL(TARGETS ${PYTHON_MODULE}
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION
${PYTHON_INSTALL_DIR})
SET(ENTITY_CLASS_LIST "")
FOREACH (ENTITY ${NEW_ENTITY_CLASS})
SET(ENTITY_CLASS_LIST "${ENTITY_CLASS_LIST}${ENTITY}('')\n")
ENDFOREACH(ENTITY ${NEW_ENTITY_CLASS})
# Install if INSTALL_INIT_PY is set to 1
IF (${INSTALL_INIT_PY} EQUAL 1)
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/cmake/dynamic_graph/submodule/__init__.py.cmake
${PROJECT_BINARY_DIR}/src/${FULLMODULENAME}/__init__.py
)
INSTALL(
FILES ${PROJECT_BINARY_DIR}/src/${FULLMODULENAME}/__init__.py
DESTINATION ${PYTHON_INSTALL_DIR}
)
ENDIF(${INSTALL_INIT_PY} EQUAL 1)
ENDMACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE)
MACRO(SOT_TALOS_BALANCE_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME)
DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE("${PY_NAME}"
"${SUBMODULENAME}" "${LIBRARYNAME}" "${TARGETNAME}")
ENDMACRO(SOT_TALOS_BALANCE_PYTHON_MODULE)
ADD_PROJECT_DEPENDENCY(parametric-curves REQUIRED)
# Verbosity level
IF (NOT (\"${CMAKE_VERBOSITY_LEVEL}\" STREQUAL \"\"))
......@@ -176,9 +87,8 @@ SET(${PROJECT_NAME}_SOURCES
ADD_LIBRARY(${PROJECT_NAME} SHARED
${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES}
sot-core::sot-core)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} parametric-curves)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC
sot-core::sot-core parametric-curves::parametric-curves)
IF(SUFFIX_SO_VERSION)
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
......
cmake @ 9dcde03a
Compare fb4c22c3...9dcde03a
Subproject commit fb4c22c319ec5320f9a85527eb1a4130954846f5
Subproject commit 9dcde03a880cccc86531019a6845708f5c54c35d
include/sot/talos_balance/delay.hh 0 → 100644
View file @ 629a6767
// Copyright 2018 CNRS - Airbus SAS
// Author: Joseph Mirabel
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef __sot_talos_balance_delay_H__
#define __sot_talos_balance_delay_H__
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/command-bind.h>
#if defined(WIN32)
#if defined(delay_EXPORTS)
#define DELAY_EXPORT __declspec(dllexport)
#else
#define DELAY_EXPORT __declspec(dllimport)
#endif
#else
#define DELAY_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/// Delay
template <typename Value, typename Time = int>
class DELAY_EXPORT Delay : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
Delay(const std::string& name)
: Entity(name),
sin(NULL, "Delay(" + name + ")::input(unspecified)::sin"),
currentOUT("Delay(" + name + ")::output(unspecified)::current"),
previousOUT("Delay(" + name + ")::output(unspecified)::previous") {
currentOUT.setFunction(boost::bind(&Delay::current, this, _1, _2));
previousOUT.setFunction(boost::bind(&Delay::previous, this, _1, _2));
signalRegistration(sin << currentOUT << previousOUT);
using command::makeCommandVoid1;
std::string docstring =
"\n"
" Set current value in memory\n";
addCommand("setMemory", makeCommandVoid1(*this, &Delay::setMemory, docstring));
}
~Delay() {}
/// Header documentation of the python class
virtual std::string getDocString() const {
return "Enable delayed signal in SoT.\n"
"Signal previous at time t+1 will return the value of signal current "
" at time <= t (the last time signal current was called).";
}
void setMemory(const Value& val) { mem = val; }
private:
Value& current(Value& res, const Time& time) {
previousOUT.access(time);
res = sin.access(time);
mem = res;
return res;
}
Value& previous(Value& res, const Time&) {
res = mem;
return res;
}
Value mem;
SignalPtr<Value, Time> sin;
Signal<Value, Time> currentOUT, previousOUT;
};
typedef Delay<Vector, int> DelayVector;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_simple_pid_H__
src/CMakeLists.txt
View file @ 629a6767
......@@ -53,7 +53,7 @@ FOREACH(plugin ${plugins})
SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
ENDIF(SUFFIX_SO_VERSION)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${PROJECT_NAME} ${${LIBRARY_NAME}_deps})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC ${PROJECT_NAME} ${${LIBRARY_NAME}_deps})
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(TARGETS ${LIBRARY_NAME} EXPORT ${TARGETS_EXPORT_NAME}
......@@ -62,21 +62,13 @@ FOREACH(plugin ${plugins})
IF(BUILD_PYTHON_INTERFACE)
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
SOT_TALOS_BALANCE_PYTHON_MODULE(${PYTHON_LIBRARY_NAME}
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap)
DYNAMIC_GRAPH_PYTHON_MODULE("${PY_NAME}/${PYTHON_LIBRARY_NAME}"
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
MODULE_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
ENDIF(BUILD_PYTHON_INTERFACE)
ENDFOREACH(plugin)
TARGET_LINK_LIBRARIES(distribute-wrench eiquadprog::eiquadprog)
IF(BUILD_PYTHON_INTERFACE)
SOT_TALOS_BALANCE_PYTHON_MODULE("" ${PROJECT_NAME} wrap)
INSTALL(FILES
${CMAKE_CURRENT_BINARY_DIR}/${PY_NAME}/__init__.py
DESTINATION ${PYTHON_SITELIB}/${PY_NAME}
)
ENDIF(BUILD_PYTHON_INTERFACE)
TARGET_LINK_LIBRARIES(distribute-wrench PUBLIC eiquadprog::eiquadprog)
# main python module
SET(${PROJECT_NAME}_PYTHON_FILES
......@@ -174,24 +166,24 @@ SET(${PROJECT_NAME}_ROS_EXEC_FILES
IF(BUILD_PYTHON_INTERFACE)
FOREACH(python ${${PROJECT_NAME}_PYTHON_FILES})
PYTHON_INSTALL_ON_SITE(${PY_NAME} ${python})
PYTHON_INSTALL_ON_SITE(dynamic_graph/${PY_NAME} ${python})
ENDFOREACH(python ${${PROJECT_NAME}_PYTHON_FILES})
FOREACH(python ${${PROJECT_NAME}_TALOS_PYTHON_FILES})
PYTHON_INSTALL_ON_SITE(${PY_NAME}/talos ${python})
PYTHON_INSTALL_ON_SITE(dynamic_graph/${PY_NAME}/talos ${python})
ENDFOREACH(python ${${PROJECT_NAME}_TALOS_PYTHON_FILES})
FOREACH(python ${${PROJECT_NAME}_UTILS_PYTHON_FILES})
PYTHON_INSTALL_ON_SITE(${PY_NAME}/utils ${python})
PYTHON_INSTALL_ON_SITE(dynamic_graph/${PY_NAME}/utils ${python})
ENDFOREACH(python ${${PROJECT_NAME}_UTILS_PYTHON_FILES})
FOREACH(python ${${PROJECT_NAME}_SIMULATION_FILES})
PYTHON_INSTALL_ON_SITE(${PY_NAME}/test ${python})
PYTHON_INSTALL_ON_SITE(dynamic_graph/${PY_NAME}/test ${python})
ENDFOREACH(python ${${PROJECT_NAME}_SIMULATION_FILES})
FOREACH(python ${${PROJECT_NAME}_ROS_EXEC_FILES})
INSTALL(PROGRAMS ${PY_NAME}/test/${python}
DESTINATION ${CMAKE_BINARY_DIR}/lib/${PROJECT_NAME})
DESTINATION lib/${PROJECT_NAME})
ENDFOREACH(python ${${PROJECT_NAME}_ROS_EXEC_FILES})
ENDIF(BUILD_PYTHON_INTERFACE)
src/admittance-controller-end-effector-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/admittance-controller-end-effector.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::AdmittanceControllerEndEffector> entities_t;
src/ankle-admittance-controller-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/ankle-admittance-controller.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::AnkleAdmittanceController> entities_t;
src/ankle-joint-selector-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/ankle-joint-selector.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::AnkleJointSelector> entities_t;
src/boolean-identity-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/boolean-identity.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::BooleanIdentity> entities_t;
src/com-admittance-controller-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/com-admittance-controller.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::ComAdmittanceController> entities_t;
src/coupled-admittance-controller-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/coupled-admittance-controller.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::CoupledAdmittanceController> entities_t;
src/dcm-com-controller-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/dcm-com-controller.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::DcmComController> entities_t;
src/dcm-controller-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/dcm-controller.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::DcmController> entities_t;
src/dcm-estimator-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/dcm-estimator.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::DcmEstimator> entities_t;
src/delay-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/delay.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::DelayVector> entities_t;
src/delay.cpp
View file @ 629a6767
......@@ -24,76 +24,12 @@
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/command-bind.h>
#if defined(WIN32)
#if defined(delay_EXPORTS)
#define DELAY_EXPORT __declspec(dllexport)
#else
#define DELAY_EXPORT __declspec(dllimport)
#endif
#else
#define DELAY_EXPORT
#endif
#include "sot/talos_balance/delay.hh"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/// Delay
template <typename Value, typename Time = int>
class DELAY_EXPORT Delay : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
Delay(const std::string& name)
: Entity(name),
sin(NULL, "Delay(" + name + ")::input(unspecified)::sin"),
currentOUT("Delay(" + name + ")::output(unspecified)::current"),
previousOUT("Delay(" + name + ")::output(unspecified)::previous") {
currentOUT.setFunction(boost::bind(&Delay::current, this, _1, _2));
previousOUT.setFunction(boost::bind(&Delay::previous, this, _1, _2));
signalRegistration(sin << currentOUT << previousOUT);
using command::makeCommandVoid1;
std::string docstring =
"\n"
" Set current value in memory\n";
addCommand("setMemory", makeCommandVoid1(*this, &Delay::setMemory, docstring));
}
~Delay() {}
/// Header documentation of the python class
virtual std::string getDocString() const {
return "Enable delayed signal in SoT.\n"
"Signal previous at time t+1 will return the value of signal current "
" at time <= t (the last time signal current was called).";
}
void setMemory(const Value& val) { mem = val; }
private:
Value& current(Value& res, const Time& time) {
previousOUT.access(time);
res = sin.access(time);
mem = res;
return res;
}
Value& previous(Value& res, const Time&) {
res = mem;
return res;
}
Value mem;
SignalPtr<Value, Time> sin;
Signal<Value, Time> currentOUT, previousOUT;
};
typedef Delay<Vector, int> DelayVector;
template <>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DelayVector, "DelayVector");
} // namespace talos_balance
......
src/distribute-wrench-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/distribute-wrench.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::DistributeWrench> entities_t;
src/dummy-dcm-estimator-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/dummy-dcm-estimator.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::DummyDcmEstimator> entities_t;
src/dummy-walking-pattern-generator-python.hh 0 → 100644
View file @ 629a6767
#include <sot/talos_balance/dummy-walking-pattern-generator.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::DummyWalkingPatternGenerator> entities_t;
src/sot_talos_balance/create_entities_utils.py → src/dynamic_graph/sot_talos_balance/create_entities_utils.py
View file @ 629a6767
from sot_talos_balance.talos_control_manager import TalosControlManager
from sot_talos_balance.example import Example
from dynamic_graph.sot.core.parameter_server import ParameterServer
from dynamic_graph.tracer_real_time import TracerRealTime
from time import sleep
from sot_talos_balance.talos_base_estimator import TalosBaseEstimator
from dynamic_graph.sot.core.madgwickahrs import MadgwickAHRS
from sot_talos_balance.dcm_estimator import DcmEstimator
from sot_talos_balance.ft_calibration import FtCalibration
from sot_talos_balance.ft_wrist_calibration import FtWristCalibration
from sot_talos_balance.euler_to_quat import EulerToQuat
from sot_talos_balance.quat_to_euler import QuatToEuler
from sot_talos_balance.pose_quaternion_to_matrix_homo import PoseQuaternionToMatrixHomo
from dynamic_graph.sot.core.operator import Mix_of_vector
from dynamic_graph.sot.core.operator import Selec_of_vector
from dynamic_graph.sot.core.operator import Component_of_vector
from dynamic_graph.sot.core.operator import MatrixHomoToPoseQuaternion
from dynamic_graph.sot.core.operator import PoseRollPitchYawToMatrixHomo
from dynamic_graph.sot.core.operator import MatrixHomoToPoseRollPitchYaw
from sot_talos_balance.boolean_identity import BooleanIdentity
from sot_talos_balance.int_identity import IntIdentity
from sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
from sot_talos_balance.simple_pid import SimplePID
from sot_talos_balance.simple_pidd import SimplePIDD
from sot_talos_balance.joint_position_controller import JointPositionController
from sot_talos_balance.simple_admittance_controller import SimpleAdmittanceController
from sot_talos_balance.admittance_controller_end_effector import AdmittanceControllerEndEffector
from sot_talos_balance.ankle_admittance_controller import AnkleAdmittanceController
from sot_talos_balance.foot_force_difference_controller import FootForceDifferenceController
from sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
from sot_talos_balance.com_admittance_controller import ComAdmittanceController
from sot_talos_balance.dcm_controller import DcmController
from sot_talos_balance.dcm_com_controller import DcmComController
from sot_talos_balance.simple_zmp_estimator import SimpleZmpEstimator
from sot_talos_balance.simple_distribute_wrench import SimpleDistributeWrench
from sot_talos_balance.distribute_wrench import DistributeWrench
from sot_talos_balance.simple_reference_frame import SimpleReferenceFrame
from sot_talos_balance.state_transformation import StateTransformation
from sot_talos_balance.dummy_walking_pattern_generator import DummyWalkingPatternGenerator
from sot_talos_balance.ankle_joint_selector import AnkleJointSelector
from sot_talos_balance.qualisys_client import QualisysClient
from sot_talos_balance.hip_flexibility_compensation import HipFlexibilityCompensation
from sot_talos_balance.simple_state_integrator import SimpleStateIntegrator
from sot_talos_balance.delay import DelayVector
from sot_talos_balance.round_double_to_int import RoundDoubleToInt
# python
from sot_talos_balance.utils import filter_utils
from sot_talos_balance.utils.sot_utils import Bunch
from dynamic_graph import plug
import numpy as np
from dynamic_graph import plug
from dynamic_graph.sot.core.madgwickahrs import MadgwickAHRS
from dynamic_graph.sot.core.operator import (Component_of_vector, MatrixHomoToPoseQuaternion,
MatrixHomoToPoseRollPitchYaw, Mix_of_vector, PoseRollPitchYawToMatrixHomo,
Selec_of_vector)
from dynamic_graph.sot.core.parameter_server import ParameterServer
from dynamic_graph.sot_talos_balance.admittance_controller_end_effector import AdmittanceControllerEndEffector
from dynamic_graph.sot_talos_balance.ankle_admittance_controller import AnkleAdmittanceController
from dynamic_graph.sot_talos_balance.ankle_joint_selector import AnkleJointSelector
from dynamic_graph.sot_talos_balance.boolean_identity import BooleanIdentity
from dynamic_graph.sot_talos_balance.com_admittance_controller import ComAdmittanceController
from dynamic_graph.sot_talos_balance.dcm_com_controller import DcmComController
from dynamic_graph.sot_talos_balance.dcm_controller import DcmController
from dynamic_graph.sot_talos_balance.dcm_estimator import DcmEstimator
from dynamic_graph.sot_talos_balance.delay import DelayVector
from dynamic_graph.sot_talos_balance.distribute_wrench import DistributeWrench
from dynamic_graph.sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
from dynamic_graph.sot_talos_balance.dummy_walking_pattern_generator import DummyWalkingPatternGenerator
from dynamic_graph.sot_talos_balance.euler_to_quat import EulerToQuat
from dynamic_graph.sot_talos_balance.example import Example
from dynamic_graph.sot_talos_balance.foot_force_difference_controller import FootForceDifferenceController
from dynamic_graph.sot_talos_balance.ft_calibration import FtCalibration
from dynamic_graph.sot_talos_balance.ft_wrist_calibration import FtWristCalibration
from dynamic_graph.sot_talos_balance.hip_flexibility_compensation import HipFlexibilityCompensation
from dynamic_graph.sot_talos_balance.int_identity import IntIdentity
from dynamic_graph.sot_talos_balance.joint_position_controller import JointPositionController
from dynamic_graph.sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
from dynamic_graph.sot_talos_balance.pose_quaternion_to_matrix_homo import PoseQuaternionToMatrixHomo
from dynamic_graph.sot_talos_balance.qualisys_client import QualisysClient
from dynamic_graph.sot_talos_balance.quat_to_euler import QuatToEuler
from dynamic_graph.sot_talos_balance.round_double_to_int import RoundDoubleToInt
from dynamic_graph.sot_talos_balance.simple_admittance_controller import SimpleAdmittanceController
from dynamic_graph.sot_talos_balance.simple_distribute_wrench import SimpleDistributeWrench
from dynamic_graph.sot_talos_balance.simple_pid import SimplePID
from dynamic_graph.sot_talos_balance.simple_pidd import SimplePIDD
from dynamic_graph.sot_talos_balance.simple_reference_frame import SimpleReferenceFrame
from dynamic_graph.sot_talos_balance.simple_state_integrator import SimpleStateIntegrator
from dynamic_graph.sot_talos_balance.simple_zmp_estimator import SimpleZmpEstimator
from dynamic_graph.sot_talos_balance.state_transformation import StateTransformation
from dynamic_graph.sot_talos_balance.talos_base_estimator import TalosBaseEstimator
from dynamic_graph.sot_talos_balance.talos_control_manager import TalosControlManager
# python
from dynamic_graph.sot_talos_balance.utils import filter_utils
from dynamic_graph.sot_talos_balance.utils.sot_utils import Bunch
from dynamic_graph.tracer_real_time import TracerRealTime
N_JOINTS = 32
def create_qualisys_client(address):
mocap = QualisysClient('mocap')
mocap.setMocapIPAdress(address)
......@@ -113,6 +107,7 @@ def create_config_trajectory_generator(dt, robot):
jtg.init(dt, N_CONFIG)
return jtg
def create_torque_trajectory_generator(dt, robot):
N_CONFIG = N_JOINTS + 6
jtg = NdTrajectoryGenerator("torqueTrajGen")
......@@ -121,6 +116,7 @@ def create_torque_trajectory_generator(dt, robot):
jtg.init(dt, N_CONFIG)
return jtg
def create_com_trajectory_generator(dt, robot):
comTrajGen = NdTrajectoryGenerator("comTrajGen")
comTrajGen.initial_value.value = robot.dynamic.com.value
......@@ -536,7 +532,7 @@ def create_dcm_com_controller(Kp, Ki, dt, robot, dcmSignal):
return controller
def fill_parameter_server(param_server,conf, dt, robot_name='robot'):
def fill_parameter_server(param_server, conf, dt, robot_name='robot'):
# Init should be called before addCtrlMode
# because the size of state vector must be known.
param_server.init(dt, conf.urdfFileName, robot_name)
......@@ -551,15 +547,15 @@ def fill_parameter_server(param_server,conf, dt, robot_name='robot'):
# Set the force limits for each id
for key in conf.mapForceIdToForceLimits:
param_server.setForceLimitsFromId(key, tuple(conf.mapForceIdToForceLimits[key][0]),
tuple(conf.mapForceIdToForceLimits[key][1]))
param_server.setForceLimitsFromId(key, np.array(conf.mapForceIdToForceLimits[key][0]),
np.array(conf.mapForceIdToForceLimits[key][1]))
# Set the force sensor id for each sensor name
for key in conf.mapNameToForceId:
param_server.setForceNameToForceId(key, conf.mapNameToForceId[key])
# Set the map from the urdf joint list to the sot joint list
param_server.setJointsUrdfToSot(conf.urdftosot)